RTABMapRegistration.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2011-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * 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::RTABMapRegistration
 * @author     Markus Grotz ( markus dot grotz at kit dot edu )
 * @date       2015
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "RTABMapRegistration.h"
 
#include <rtabmap/core/odometry/OdometryF2M.h>
 
 
#include <rtabmap/core/util3d.h>
#include <rtabmap/core/Transform.h>
 
 
 
#include <VisionX/interface/components/PointCloudAndImageAndCalibrationProviderInterface.h>
 
using namespace armarx;
using namespace rtabmap;
 
 
PropertyDefinitionsPtr RTABMapRegistration::createPropertyDefinitions()
{
    return PropertyDefinitionsPtr(new RTABMapRegistrationPropertyDefinitions(
                                      getConfigIdentifier()));
}
 
void RTABMapRegistration::onInitCapturingPointCloudProvider()
{
    agentName = getProperty<std::string>("agentName").getValue();
    provideGlobalPointCloud = getProperty<bool>("provideGlobalPointCloud").getValue();
 
    usingProxy(getProperty<std::string>("RobotStateComponentName").getValue());
    usingProxy(getProperty<std::string>("WorkingMemoryName").getValue());
 
    passThroughFilter.setFilterFieldName("z");
    passThroughFilter.setFilterLimits(getProperty<float>("MinDistance").getValue(), getProperty<float>("MaxDistance").getValue());
 
    updateTask = new PeriodicTask<RTABMapRegistration>(this, &RTABMapRegistration::processData, 1000);
 
    ParametersMap parameters;
    //    parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapTimeThr(), "1000"));
    parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemImageKept(), "true"));
 
    ULogger::setType(ULogger::kTypeConsole);
    //ULogger::setLevel(ULogger::kDebug);
    ULogger::setLevel(ULogger::kWarning);
 
    rtabmap = new Rtabmap();
    rtabmap->init(parameters);
    rtabmapThread = new RtabmapThread(rtabmap);
 
    odomThread = new OdometryThread(new OdometryF2M());
 
    odomThread->registerToEventsManager();
    rtabmapThread->registerToEventsManager();
 
    this->registerToEventsManager();
 
    UEventsManager::createPipe(this, odomThread, "CameraEvent");
}
 
void RTABMapRegistration::onExitCapturingPointCloudProvider()
{
    this->unregisterFromEventsManager();
 
    resultCondition.notify_one();
    dataCondition.notify_one();
 
    rtabmapThread->unregisterFromEventsManager();
    odomThread->unregisterFromEventsManager();
 
    odomThread->join(true);
    rtabmapThread->join(true);
 
    rtabmap->close();
 
    delete odomThread;
    delete rtabmapThread;
    delete rtabmap;
}
 
void RTABMapRegistration::onStartCapture(float framesPerSecond)
{
    robotStateComponent = getProxy<RobotStateComponentInterfacePrx>(getProperty<std::string>("RobotStateComponentName").getValue());
    workingMemory = getProxy<memoryx::WorkingMemoryInterfacePrx>(getProperty<std::string>("WorkingMemoryName").getValue());
    agentInstancesSegment = workingMemory->getAgentInstancesSegment();
 
    SharedRobotInterfacePrx synchronizedRobot = robotStateComponent->getSynchronizedRobot();
 
    /*
    Ice::CommunicatorPtr iceCommunicator = getIceManager()->getCommunicator();
    agentInstance = new memoryx::AgentInstance(agentName);
    agentInstance->setSharedRobot(synchronizedRobot);
    agentInstance->setStringifiedSharedRobotInterfaceProxy(iceCommunicator->proxyToString(synchronizedRobot));
    agentInstance->setAgentFilePath(robotStateComponent->getRobotFilename());
    */
 
    resultPointCloudPtr.reset(new pcl::PointCloud<pcl::PointXYZRGBA>);
    hasNewResultPointCloud = false;
    hasNewData = false;
 
    FramedPoseBasePtr poseInGlobalFrame = synchronizedRobot->getRobotNode(sourceFrameName)->getGlobalPose();
    initialCameraPose = armarx::FramedPosePtr::dynamicCast(poseInGlobalFrame)->toEigen();
 
    rtabmapThread->start();
    odomThread->start();
 
    updateTask->start();
 
 
    while (captureEnabled && (!rtabmapThread->isRunning() || !odomThread->isRunning()))
    {
        ARMARX_WARNING << " waiting for RTABMap";
        sleep(1);
    }
 
    //    post(new OdometryResetEvent());
 
 
    if (provideGlobalPointCloud)
    {
        requestGlobalPointCloud();
    }
}
 
void RTABMapRegistration::onStopCapture()
{
    updateTask->stop();
 
    odomThread->kill();
    rtabmapThread->kill();
}
 
void RTABMapRegistration::requestGlobalPointCloud()
{
    UEventsManager::post(new RtabmapEventCmd(RtabmapEventCmd::kCmdPublish3DMap, true, true, false));
}
 
 
bool RTABMapRegistration::doCapture()
{
    boost::mutex::scoped_lock lock(resultPointCloudMutex);
 
    while (captureEnabled && !hasNewResultPointCloud)
    {
        resultCondition.wait(lock);
    }
 
    if (!captureEnabled)
    {
        return false;
    }
 
    hasNewResultPointCloud = false;
 
    if (resultPointCloudPtr->points.size())
    {
        providePointCloud(resultPointCloudPtr);
 
        return true;
    }
    else
    {
        return false;
    }
 
 
}
 
 
bool RTABMapRegistration::handleEvent(UEvent* event)
{
    if (event->getClassName().compare("RtabmapEvent") == 0)
    {
        RtabmapEvent* rtabmapEvent = static_cast<RtabmapEvent*>(event);
 
        boost::mutex::scoped_lock lock(RTABDataMutex);
 
        /*
        FramedPoseBasePtr poseInRootFrame = robotStateComponent->getSynchronizedRobot()->getRobotNode(sourceFrameName)->getPoseInRootFrame();
        Eigen::Matrix4f cameraToRoot = armarx::FramedPosePtr::dynamicCast(poseInRootFrame)->toEigen();
 
        Signature s = stats.getSignature();
 
        Eigen::Matrix4f transform = s.getPose().toEigen4f() * s.getLocalTransform().toEigen4f();
        transform.block<3, 1>(0, 3) = transform.block<3, 1>(0, 3) * Eigen::Scaling(1000.0f);
        transform = initialCameraPose * transform;
 
        Eigen::Matrix4f robotPose = transform * cameraToRoot.inverse();
        updatePosition(robotPose);
        */
 
        if (!provideGlobalPointCloud)
        {
            signatures = rtabmapEvent->getStats().getSignatures();
 
            hasNewData = true;
            dataCondition.notify_one();
        }
 
    }
    else if (event->getClassName().compare("OdometryEvent") == 0)
    {
        OdometryEvent* rtabmapEvent = static_cast<OdometryEvent*>(event);
 
        if (rtabmapEvent->pose().isNull())
        {
            ARMARX_WARNING << deactivateSpam(1) << "unable to register current view. odometry lost.";
        }
    }
    else if (provideGlobalPointCloud && event->getClassName().compare("RtabmapEvent3DMap") == 0)
    {
        RtabmapEvent3DMap* rtabmapEvent = static_cast<RtabmapEvent3DMap*>(event);
 
        boost::mutex::scoped_lock lock(RTABDataMutex);
 
        signatures = rtabmapEvent->getSignatures();
        poses = rtabmapEvent->getPoses();
 
        hasNewData = true;
        dataCondition.notify_one();
 
    }
 
    return false;
}
 
 
 
 
bool RTABMapRegistration::extractPointCloud(SensorData sensorData, Eigen::Matrix4f transform, pcl::PointCloud<pcl::PointXYZRGBA>::Ptr& tempCloudPtr)
{
    std::vector<CameraModel> cameraModels = sensorData.cameraModels();
 
    if (cameraModels.size() != 1)
    {
        return false;
    }
 
    CameraModel m = cameraModels[0];
 
    transform.block<3, 1>(0, 3) = transform.block<3, 1>(0, 3) * Eigen::Scaling(1000.0f);
    transform = initialCameraPose * transform;
 
    int decimation = (provideGlobalPointCloud) ? 4 : 1;
 
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudPtr = util3d::cloudFromDepthRGB(sensorData.imageRaw(), sensorData.depthOrRightRaw(), m, decimation);
 
    pcl::copyPointCloud(*cloudPtr, *tempCloudPtr);
    passThroughFilter.setInputCloud(tempCloudPtr);
    passThroughFilter.filter(*tempCloudPtr);
 
    Eigen::Affine3f convertUnitToMM = Eigen::Affine3f(Eigen::Scaling(1000.0f));
    pcl::transformPointCloud(*tempCloudPtr, *tempCloudPtr, convertUnitToMM);
 
    pcl::transformPointCloud(*tempCloudPtr, *tempCloudPtr, transform);
 
    return true;
}
 
void RTABMapRegistration::processData()
{
    boost::mutex::scoped_lock lock(RTABDataMutex);
 
    while (captureEnabled && !hasNewData)
    {
        dataCondition.wait(lock);
    }
 
    if (!captureEnabled)
    {
        return;
    }
 
    hasNewData = false;
 
    pcl::PointCloud<pcl::PointXYZRGBA>::Ptr nextCloudPtr(new pcl::PointCloud<pcl::PointXYZRGBA>());
 
    for (auto& items : signatures)
    {
        const int id = items.first;
        const Signature signature = items.second;
 
        if (signature.isBadSignature())
        {
            continue;
        }
 
        pcl::PointCloud<pcl::PointXYZRGBA>::Ptr tempCloudPtr(new pcl::PointCloud<pcl::PointXYZRGBA>());
 
        Eigen::Matrix4f transform;
 
        if (signature.isEnabled())
        {
            transform = signature.getPose().toEigen4f();
        }
        else
        {
            if (poses.count(id))
            {
                transform = poses.at(id).toEigen4f();
            }
            else
            {
                continue;
            }
        }
 
        extractPointCloud(signature.sensorData(), transform, tempCloudPtr);
 
        *nextCloudPtr += *tempCloudPtr;
 
        /*
        IceUtil::Time currentTime = IceUtil::Time::now();
        std::string fileName = "/tmp/cloud" + std::to_string(currentTime.toMilliSeconds())  + ".pcd";
        pcl::io::savePCDFile(fileName, *temp);
        */
    }
 
    if (provideGlobalPointCloud)
    {
        requestGlobalPointCloud();
    }
 
 
    boost::mutex::scoped_lock resultLock(resultPointCloudMutex);
 
    //    resultPointCloudPtr->header.seq = signatures.end()
 
    pcl::copyPointCloud(*nextCloudPtr, *resultPointCloudPtr);
 
    hasNewResultPointCloud = true;
    resultCondition.notify_one();
}
 
void RTABMapRegistration::updatePosition(Eigen::Matrix4f pose)
{
    Eigen::Matrix3f orientation = pose.block<3, 3>(0, 0);
    FramedOrientationPtr orientationPtr = new FramedOrientation(orientation, GlobalFrame, agentName);
    Eigen::Vector3f position = pose.block<3, 1>(0, 3);
    FramedPositionPtr positionPtr = new FramedPosition(position, GlobalFrame, agentName);
    agentInstance->setOrientation(orientationPtr);
    agentInstance->setPosition(positionPtr);
 
    if (agentInstanceId.empty() || !agentInstancesSegment->hasEntityById(agentInstanceId))
    {
        agentInstanceId = agentInstancesSegment->addEntity(agentInstance);
    }
    else
    {
        agentInstancesSegment->updateEntity(agentInstanceId, agentInstance);
    }
}
 
 
void armarx::RTABMapRegistration::onInitImageProcessor()
{
    providerName = getProperty<std::string>("providerName").getValue();
    usingImageProvider(providerName);
}
 
 
void armarx::RTABMapRegistration::onConnectImageProcessor()
{
    visionx::ImageType imageDisplayType = visionx::tools::typeNameToImageType("rgb");
 
    visionx::ImageProviderInfo imageProviderInfo = getImageProvider(providerName, imageDisplayType);
 
    int numImages = imageProviderInfo.numberImages;
 
    if (numImages != 2)
    {
        ARMARX_FATAL << "invalid number of images. aborting";
        return;
    }
 
    images = new CByteImage*[numImages];
 
    for (int i = 0 ; i < numImages ; i++)
    {
        images[i] = visionx::tools::createByteImage(imageProviderInfo);
    }
 
    visionx::StereoCalibrationInterfacePrx calibrationProvider = visionx::StereoCalibrationInterfacePrx::checkedCast(imageProviderInfo.proxy);
    if (!calibrationProvider)
    {
        ARMARX_FATAL << "unable to retrieve calibration information. aborting.";
        return;
    }
 
    sourceFrameName = calibrationProvider->getReferenceFrame();
 
    visionx::StereoCalibration calibration = calibrationProvider->getStereoCalibration();
 
    float fx = calibration.calibrationRight.cameraParam.focalLength[0];
    float fy = calibration.calibrationRight.cameraParam.focalLength[1];
    float cx = calibration.calibrationRight.cameraParam.principalPoint[0];
    float cy = calibration.calibrationRight.cameraParam.principalPoint[1];
 
    // todo test if cameras are rectified
    float tx = 0.075;
    //float tx = calibration.calibrationRight.cameraParam.translation[0];
 
    cameraModel = CameraModel(fx, fy, cx, cy, Transform::getIdentity(), tx);
}
 
void armarx::RTABMapRegistration::onExitImageProcessor()
{
    post(new CameraEvent());
 
    delete images[0];
    delete images[1];
    delete [] images;
}
 
 
void armarx::RTABMapRegistration::process()
{
    if (!waitForImages(1000))
    {
        ARMARX_WARNING << "Timeout or error while waiting for image data";
        return;
    }
    long lastStamp = imageMetaInfo ? imageMetaInfo->timeProvided : 0.0;
    if (getImages(providerName, images, imageMetaInfo) != 2)
    {
        ARMARX_WARNING << "Unable to transfer or read images";
        return;
    }
    if (lastStamp == imageMetaInfo->timeProvided)
    {
        ARMARX_INFO << "Last timestamp as before - skipping image";
        return;
    }
 
    // rgb format is CV_8UC3
    const cv::Mat tempRGBImage = cv::cvarrToMat(IplImageAdaptor::Adapt(images[0]));
    cv::Mat RGBImage;
    cv::cvtColor(tempRGBImage, RGBImage, CV_RGB2BGR);
 
 
    // depth image format is CV_16U1 or CV_32FC1
 
    cv::Mat depthImage = cv::Mat(RGBImage.rows, RGBImage.cols, CV_16UC1);
 
    for (int j = 0; j < depthImage.rows; j++)
    {
        for (int i = 0; i < depthImage.cols; i++)
        {
            unsigned short value = images[1]->pixels[3 * (j * images[1]->width + i) + 0]
                                   + (images[1]->pixels[3 * (j * images[1]->width + i) + 1] << 8);
 
            depthImage.at<unsigned short>(j, i) = value;
        }
    }
 
 
    /*
        cv::Mat disparityImage = cv::Mat(RGBImage.rows, RGBImage.cols, CV_32FC1);
        for (int j = 0; j < disparityImage.rows; j++)
        {
            for (int i = 0; i < disparityImage.cols; i++)
            {
                int value = images[1]->pixels[3 * (j * images[1]->width + i) + 0]
                          + (images[1]->pixels[3 * (j * images[1]->width + i) + 1] << 8);
                          + (images[1]->pixels[3 * (j * images[1]->width + i) + 2] << 16);
 
                disparityImage.at<float>(j, i) = value;
            }
        }
        const cv::Mat depthImage = util3d::depthFromDisparity(disparityImage, cameraModel.cx(), cameraModel.Tx(), CV_32FC1);
      */
 
    double time = imageMetaInfo->timeProvided / 1000.0;
    SensorData sensorData(RGBImage, depthImage, cameraModel, ++imageId, time);
 
    post(new CameraEvent(sensorData, providerName));
}
 
visionx::MetaPointCloudFormatPtr RTABMapRegistration::getDefaultPointCloudFormat()
{
    visionx::MetaPointCloudFormatPtr info = new visionx::MetaPointCloudFormat();
    info->size = 640 * 480 * sizeof(visionx::ColoredPoint3D);
 
    if (provideGlobalPointCloud)
    {
        info->capacity = info->size * 50;
    }
    else
    {
        info->capacity = info->size;
    }
 
    info->type = visionx::PointContentType::eColoredPoints;
    return info;
}