CoFusionProcessor.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @package    VisionX::ArmarXObjects::CoFusionProcessor
 * @author     Rainer Kartmann ( rainer dot kartmann at student dot kit dot edu )
 * @date       2018
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "CoFusionProcessor.h"
 
#include <pcl/filters/approximate_voxel_grid.h>
 
#include <GL/glew.h>
#include <pangolin/pangolin.h>
 
#include <Image/IplImageAdaptor.h>
 
#include <VisionX/tools/ImageUtil.h>
 
#include <VisionX/interface/components/PointCloudAndImageAndCalibrationProviderInterface.h>
#include <VisionX/interface/components/RGBDImageProvider.h>
#include <unistd.h>
#include <Eigen/Geometry>
 
#include <Core/Model/Model.h>
 
 
using namespace armarx;
 
 
void CoFusionProcessor::onInitImageProcessor()
{
    imageProviderName = getProperty<std::string>("ImageProviderName").getValue();
    usingImageProvider(imageProviderName);
    //usingProxy(getProperty<std::string>("RobotStateComponentName").getValue());
 
}
 
 
void CoFusionProcessor::onConnectImageProcessor()
{
    visionx::ImageProviderInfo imageProviderInfo = getImageProvider(imageProviderName);
    imageProvider = getProxy<visionx::ImageProviderInterfacePrx>(imageProviderName);
 
    int numImages = 2;
    if (imageProviderInfo.numberImages != numImages)
    {
        ARMARX_ERROR << "Number of images provided by " << imageProviderName << " (" << imageProviderInfo.numberImages
                     << ") does not match the expected number of images (" << numImages << ")";
    }
 
    {
        std::lock_guard<std::mutex> lock(imagesMutex);
 
        imageRgb.reset(visionx::tools::createByteImage(imageProviderInfo));
        imageDepth.reset(visionx::tools::createByteImage(imageProviderInfo));
        images[0] = imageRgb.get();
        images[1] = imageDepth.get();
    }
}
 
void CoFusionProcessor::onDisconnectImageProcessor()
{
    std::lock_guard<std::mutex> lock(imagesMutex);
 
    images[0] = nullptr;
    images[1] = nullptr;
    imageRgb.reset(nullptr);
    imageDepth.reset(nullptr);
 
    imageProvider = nullptr;
}
 
void CoFusionProcessor::onExitImageProcessor()
{
 
}
 
 
void CoFusionProcessor::onInitCapturingPointCloudProvider()
{
    setPointCloudSyncMode(getProperty<visionx::ImageSyncMode>("PointCloudSyncMode").getValue());
}
 
void CoFusionProcessor::onExitCapturingPointCloudProvider()
{
 
}
 
 
void CoFusionProcessor::onInitComponent()
{
    ImageProcessor::onInitComponent();
    CapturingPointCloudProvider::onInitComponent();
 
    framesBetweenUpdates = getProperty<int>("FramesBetweenUpdates").getValue();
    downsamplingLeafSize = getProperty<float>("DownsamplingLeafSize").getValue();
 
    framesSinceLastUpdate = framesBetweenUpdates; // trigger first update right away
 
    resultPointCloud.reset(new pcl::PointCloud<OutPointT>());
 
 
    // COFUSION PARAMS
 
    cofusionParams.timeDelta = getProperty<int>("cofusion.TimeDelta").getValue();
    cofusionParams.countThresh = getProperty<int>("cofusion.CountThresh").getValue();
    cofusionParams.errThresh = getProperty<float>("cofusion.ErrThresh").getValue();
    cofusionParams.covThresh = getProperty<float>("cofusion.CovThresh").getValue();
    cofusionParams.closeLoops = getProperty<bool>("cofusion.CloseLoops").getValue();
    cofusionParams.iclnuim = getProperty<bool>("cofusion.Iclnuim").getValue();
    cofusionParams.reloc = getProperty<bool>("cofusion.Reloc").getValue();
    cofusionParams.photoThresh = getProperty<float>("cofusion.PhotoThresh").getValue();
    cofusionParams.initConfidenceGlobal = getProperty<float>("cofusion.InitConfidenceGlobal").getValue();
    cofusionParams.initConfidenceObject = getProperty<float>("cofusion.InitConfidenceObject").getValue();
    cofusionParams.depthCut = getProperty<float>("cofusion.DepthCut").getValue();
    cofusionParams.icpThresh = getProperty<float>("cofusion.IcpThresh").getValue();
    cofusionParams.fastOdom = getProperty<bool>("cofusion.FastOdom").getValue();
    cofusionParams.fernThresh = getProperty<float>("cofusion.FernThresh").getValue();
    cofusionParams.so3 = getProperty<bool>("cofusion.So3").getValue();
    cofusionParams.frameToFrameRGB = getProperty<bool>("cofusion.FrameToFrameRGB").getValue();
    cofusionParams.modelSpawnOffset = getProperty<unsigned>("cofusion.ModelSpawnOffset").getValue();
    cofusionParams.matchingType = getProperty<Model::MatchingType>("cofusion.MatchingType").getValue();
    cofusionParams.exportDirectory = getProperty<std::string>("cofusion.ExportDirectory").getValue();
    cofusionParams.exportSegmentationResults = getProperty<bool>("cofusion.ExportSegmentationResults").getValue();
 
#if USE_MASKFUSION == 1
    cofusionParams.usePrecomputedMasksOnly = getProperty<bool>("cofusion.UsePrecomputedMasksOnly").getValue();
    cofusionParams.frameQueueSize = getProperty<int>("cofusion.FrameQueueSize").getValue();
#endif
}
 
void CoFusionProcessor::onConnectComponent()
{
 
 
    ImageProcessor::onConnectComponent();
    CapturingPointCloudProvider::onConnectComponent();
 
    //robotStateComponent = getProxy<RobotStateComponentInterfacePrx>(getProperty<std::string>("RobotStateComponentName").getValue());
 
 
    visionx::StereoCalibrationProviderInterfacePrx calibrationProvider = visionx::StereoCalibrationProviderInterfacePrx::checkedCast(imageProvider);
 
    if (calibrationProvider)
    {
        ARMARX_LOG << "reading intrinsics from calibration provider";
        visionx::StereoCalibration calibration = calibrationProvider->getStereoCalibration();
 
        cofusionParams.fx = calibration.calibrationRight.cameraParam.focalLength[0];
        cofusionParams.fy = calibration.calibrationRight.cameraParam.focalLength[1];
        cofusionParams.cx = calibration.calibrationRight.cameraParam.principalPoint[0];
        cofusionParams.cy = calibration.calibrationRight.cameraParam.principalPoint[1];
 
        referenceFrameName = calibrationProvider->getReferenceFrame();
    }
    else
    {
        cofusionParams.fx = 525.0;
        cofusionParams.fy = 525.0;
        cofusionParams.cx = 320;
        cofusionParams.cy = 240;
        referenceFrameName = "unkown";
        ARMARX_WARNING << "unable to get camera intrinsics. using dummy values";
    }
 
 
    visionx::ImageFormatInfo imageFormat = imageProvider->getImageFormat();
 
    cofusionParams.width = imageFormat.dimension.width;
    cofusionParams.height = imageFormat.dimension.height;
 
 
    if (getProperty<bool>("ShowTimeStats").getValue())
    {
        if (stopwatchReportTask)
        {
            stopwatchReportTask->stop();
        }
        stopwatch.reset();
        stopwatchReportTask = new PeriodicTask<CoFusionProcessor>(this, &CoFusionProcessor::reportStopwatchStats, 10000);
        stopwatchReportTask->start();
    }
 
    providePointCloud<OutPointT>(resultPointCloud);
 
}
 
void CoFusionProcessor::onDisconnectComponent()
{
    ImageProcessor::onDisconnectComponent();
    CapturingPointCloudProvider::onDisconnectComponent();
}
 
void CoFusionProcessor::onExitComponent()
{
    ImageProcessor::onExitComponent();
    CapturingPointCloudProvider::onExitComponent();
}
 
void CoFusionProcessor::onNewModel(std::shared_ptr<Model> model)
{
 
    ARMARX_INFO << "new model";
}
 
void CoFusionProcessor::onInactiveModel(std::shared_ptr<Model> model)
{
    ARMARX_INFO << "inactive model";
}
 
 
 
void CoFusionProcessor::process()
{
    if (!waitForImages(imageProviderName))
    {
        ARMARX_WARNING << "Timeout or error in waitForImages(" << imageProviderName << ")";
        return;
    }
 
    ARMARX_INFO << deactivateSpam() << "Processing input image ...";
 
    std::lock_guard<std::mutex> lock(imagesMutex);
 
    armarx::MetaInfoSizeBasePtr metaInfo;
    // retrieve images
    int numImages = getImages(imageProviderName, images, metaInfo);
    if (numImages != 2)
    {
        ARMARX_WARNING << "Did not receive correct number of images! (Expected 2, but was "
                       << numImages << ".) Aborting!";
        return;
    }
 
    if (!cofusion)
    {
        initializeCoFusion();
    }
 
    // create FrameData (CoFusion)
    stopwatch.start("ImagePreprocessing");
 
    FrameData frame = constructFrameData();
 
    stopwatch.stop("ImagePreprocessing");
 
    // FramedPoseBasePtr globalPose = robotStateComponent->getRobotSnapshotAtTimestamp(metaInfo->timeProvided)->getRobotNode(referenceFrameName)->getGlobalPose();
 
    // do this for each incoming frame
    stopwatch.start("CoFusion::ProcessFrame");
 
#if USE_MASKFUSION == 1
    std::shared_ptr<FrameData> framePtr = std::make_shared<FrameData>(frame);
 
    cofusion->processFrame(framePtr);
#else
    cofusion->processFrame(frame);
#endif
 
    stopwatch.stop("CoFusion::ProcessFrame");
 
 
    framesSinceLastUpdate++;
    if (framesSinceLastUpdate >= framesBetweenUpdates)
    {
        updateSurfelMaps();
        framesSinceLastUpdate = 0;
 
        ARMARX_VERBOSE << "Stopwatch stats: \n" << stopwatch;
    }
 
 
}
 
 
void CoFusionProcessor::initializeCoFusion()
{
    ARMARX_INFO << "Initializing CoFusion (resolution: " << cofusionParams.width << "x" << cofusionParams.height << ")";
 
    pangolin::Params windowParams;
    windowParams.Set("SAMPLE_BUFFERS", 0);
    windowParams.Set("SAMPLES", 0);
 
    pangolin::CreateWindowAndBind("Main", cofusionParams.width, cofusionParams.height, windowParams);
 
 
    GLenum glewInitError = glewInit();
    if (glewInitError)
    {
        ARMARX_ERROR << "glewInit() failed with error: " << glewInitError;
    }
 
    int prog = glCreateProgram();
    ARMARX_INFO << "Created gl program (" << prog << ")";
 
 
    ARMARX_INFO << "Constructing CoFusion";
    cofusion = cofusionParams.makeCoFusion();
 
    ARMARX_INFO << "registering callbacks";
    cofusion->addNewModelListener([this](std::shared_ptr<Model> m)
    {
        onNewModel(m);
    });
    cofusion->addInactiveModelListener([this](std::shared_ptr<Model> m)
    {
        onInactiveModel(m);
    });
 
#if USE_MASKFUSION == 1
    cofusion->preallocateModels(getProperty<int>("cofusion.PreallocatedModelsCount").getValue());
#endif
}
 
 
FrameData CoFusionProcessor::constructFrameData()
{
    bool copyData = false;
    cv::Mat matRgb = cv::cvarrToMat(IplImageAdaptor::Adapt(imageRgb.get()), copyData);
    cv::Mat matDepth = cv::cvarrToMat(IplImageAdaptor::Adapt(imageDepth.get()), copyData);
 
 
    FrameData frame;
 
    // color image is easy
    frame.rgb = matRgb;
 
    // depth image must be converted
    int rows = matDepth.rows;
    int cols = matDepth.cols;
 
 
    // type is 32 bit flaot
    cv::Mat matDepthCofusion(rows, cols, CV_32FC1);
 
    for (int x = 0; x < rows; ++x)
    {
        for (int y = 0; y < cols; ++y)
        {
            // extract float from source image
 
            /* Constructing code from from DepthImageProviderDynamicSimulation (ArmarXSimulation):
             *
             *  float* depthBufferEndOfRow = depthBuffer.data() + width - 1;
             *  // ...
             *
             *      std::int32_t value = boost::algorithm::clamp(0.f, depthBufferEndOfRow[-x], static_cast<float>(0xffffff));
             *      dImageRow[x * 3]     = value & 0xff;
             *      dImageRow[x * 3 + 1] = (value >> 8) & 0xff;
             *      dImageRow[x * 3 + 2] = (value >> 16) & 0xff;
             */
 
            /*
             * My reasoning:
             * int32_t 'value' above should be casted semantically correctly from float to int
             * (i.e. should contain the correct/rounded value, not the bit pattern).
             * Therefore, we extract the values into a int32_t first, then cast it back to float.
             */
 
            cv::Vec3b depthPixel = matDepth.at<cv::Vec3b>(x, y);
 
            int32_t value = 0;
            value += depthPixel[0];
            value += depthPixel[1] << 8;
            value += depthPixel[2] << 16;
 
            /*
            // For use with row pointer
            value += rowPtr[y * 3];
            value += rowPtr[y * 3 + 1] << 8;
            value += rowPtr[y * 3 + 2] << 16;
            */
 
            float depth = value;
 
            // map millimeters to meters
            depth /= 1000;
 
            // set in new matrix
            matDepthCofusion.at<float>(x, y) = depth;
        }
    }
 
    frame.depth = matDepthCofusion; // in meters
 
    return frame;
}
 
 
 
void CoFusionProcessor::updateSurfelMaps()
{
    std::list<std::shared_ptr<Model>>& models = cofusion->getModels();
 
    if (models.empty())
    {
        ARMARX_WARNING << "Got no models from CoFusion.";
        return;
    }
 
    std::lock_guard<std::mutex> lock(surfelMapMutex);
 
    ARMARX_INFO << "Downloading surfel maps of " << models.size() << " models ...";
    surfelMaps.clear();
    for (std::shared_ptr<Model> const& model : models)
    {
        unsigned int id = model->getID();
 
        ARMARX_VERBOSE << "Downloading surfel map of model " << id;
        // SurfelMap contains unique_ptr's and therefore must be inserted directly
 
#if 1
        stopwatch.start("Model::downloadMap()");
        surfelMaps[id] = model->downloadMap();
 
 
 
        Model::SurfelMap& surfelMap = surfelMaps[id];
        ARMARX_VERBOSE << "SurfelMap contains " << surfelMap.numPoints << " points "
                       << "(container size: " << surfelMap.data->size() << ")";
 
        stopwatch.stop("Model::downloadMap()");
#endif
    }
 
    newFrameAvailable = true;
}
 
 
void CoFusionProcessor::onStartCapture(float framesPerSecond)
{
 
}
 
void CoFusionProcessor::startCaptureForNumFrames(Ice::Int numFrames, const Ice::Current& c)
{
    ARMARX_WARNING << "CoFusionProcessor does currently not support startCaptureForNumFrames(numFrames)";
}
 
void CoFusionProcessor::onStopCapture()
{
}
 
bool CoFusionProcessor::doCapture()
{
    if (newFrameAvailable)
    {
        newFrameAvailable = false;
        resultPointCloud.reset();
        reconstructResultPointCloud();
        providePointCloud<OutPointT>(resultPointCloud);
    }
    else
    {
        usleep(1000);
    }
    return true;
}
 
void CoFusionProcessor::reconstructResultPointCloud()
{
    std::lock_guard<std::mutex> lock(surfelMapMutex);
 
    resultPointCloud.reset(new pcl::PointCloud<OutPointT>());
 
    // Create point cloud
    ARMARX_INFO << "Reconstructing point cloud from " << surfelMaps.size() << " SurfelMaps.";
    stopwatch.start("PointCloud construction");
 
    for (auto& it : surfelMaps)
    {
        unsigned int id = it.first;
        Model::SurfelMap& surfelMap = it.second;
 
 
        ARMARX_INFO << "SurfelMap of model " << id
                    << " contains " << surfelMap.numPoints << " points "
                    << "(container size: " << surfelMap.data->size() << ")";
 
        /* From Model.h @ struct SurfelMap:
         * "Returns a vector of 4-float tuples: position0, color0, normal0, ..., positionN, colorN, normalN
         * Where
         * position is (x,y,z,conf),
         * color is (color encoded as a 24-bit integer, <unused>, initTime, timestamp),
         * and normal is (x,y,z,radius)"
         */
 
        // try to reduce number of reallocations
        resultPointCloud->reserve(resultPointCloud->size() + surfelMap.numPoints);
 
        for (unsigned int i = 0; i < surfelMap.numPoints; ++i)
        {
            Eigen::Vector4f& position4f = (*surfelMap.data)[i * 3 + 0];
            position4f.head<3>() *= 1000; // meters to millimeters
 
            Eigen::Vector4f& color4f = (*surfelMap.data)[i * 3 + 1];
            //Eigen::Vector4f& normal4f = (*surfelMap.data)[i * 3 + 2];
 
            uint32_t color24b = static_cast<uint32_t>(color4f(0));
            uchar r = (color24b >> 16) & 0xFF;
            uchar g = (color24b >> 8) & 0xFF;
            uchar b = (color24b >> 0) & 0xFF;
 
            //Eigen::Vector3f normal = normal4f.head(3);
            //float radius = normal4f(3);
 
 
            //OutPointT point;
            pcl::PointXYZRGBL point;
            point.x = position4f(0);
            point.y = position4f(1);
            point.z = position4f(2);
            point.r = r;
            point.g = g;
            point.b = b;
            point.label = id; // model's id as label
            resultPointCloud->push_back(point);
        }
 
        // todo connect to prior memory and add object
 
    }
 
    if (downsamplingLeafSize > 0.f)
    {
        pcl::PointCloud<OutPointT>::Ptr filtered(new pcl::PointCloud<OutPointT>());
        std::size_t sizeBefore = resultPointCloud->size();
 
        pcl::ApproximateVoxelGrid<OutPointT> grid;
        grid.setLeafSize(downsamplingLeafSize, downsamplingLeafSize, downsamplingLeafSize);
        grid.setInputCloud(resultPointCloud);
        grid.filter(*filtered);
        resultPointCloud.swap(filtered);
 
        std::size_t sizeAfter = resultPointCloud->size();
 
        ARMARX_INFO << "Downsampled to " << sizeAfter << " points (before: " << sizeBefore << ")";
    }
 
    stopwatch.stop("PointCloud construction");
}
 
visionx::MetaPointCloudFormatPtr CoFusionProcessor::getDefaultPointCloudFormat()
{
    visionx::MetaPointCloudFormatPtr info = new visionx::MetaPointCloudFormat();
    info->capacity = 50 * 640 * 480 * sizeof(visionx::ColoredLabeledPoint3D);
    info->size = info->capacity;
    info->type = visionx::eColoredLabeledPoints;
    return info;
}
 
void CoFusionProcessor::reportStopwatchStats()
{
    ARMARX_INFO << "Stopwatch stats:\n " << stopwatch;
}
 
 
armarx::PropertyDefinitionsPtr CoFusionProcessor::createPropertyDefinitions()
{
    return armarx::PropertyDefinitionsPtr(new CoFusionProcessorPropertyDefinitions(
            getConfigIdentifier()));
}