FakePointCloudProvider.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::FakePointCloudProvider
 * @author     Markus Grotz ( markus-grotz at web dot de )
 * @date       2015
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "FakePointCloudProvider.h"
 
// IVT
#include <Image/ByteImage.h>
#include <Image/ImageProcessor.h>
#include <Calibration/Calibration.h>
 
#include <opencv2/opencv.hpp>
 
 
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <ArmarXCore/core/time/TimeUtil.h>
 
#include <ArmarXGui/libraries/RemoteGui/RemoteGui.h>
 
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
 
#include <VisionX/tools/TypeMapping.h>
#include <VisionX/tools/ImageUtil.h>
 
#include <VisionX/libraries/PointCloudTools/FramedPointCloud.h>
 
#include <SimoxUtility/algorithm/string/string_tools.h>
 
 
using namespace armarx;
using namespace pcl;
 
namespace visionx
{
    FakePointCloudProviderPropertyDefinitions::FakePointCloudProviderPropertyDefinitions(std::string prefix) :
        CapturingPointCloudProviderPropertyDefinitions(prefix)
    {
        defineOptionalProperty<std::string>("pointCloudFileName", "VisionX/examples/point_clouds/",
                                            "The point cloud to read. If directory, all *.pcd files of this and all subdirectories are read. \n"
                                            "All point clouds are read in before the pointcloud providing starts.");
 
        defineOptionalProperty<bool>("rewind", true, "loop through the point clouds");
        defineOptionalProperty<float>("scaleFactor", -1.0f, "scale point cloud. only applied if value is larger than zero");
        defineOptionalProperty<Eigen::Vector2i>("dimensions", Eigen::Vector2i(640, 480), "")
        .map("320x240", Eigen::Vector2i(320, 240))
        .map("640x480", Eigen::Vector2i(640, 480))
        .map("320x240",     Eigen::Vector2i(320,  240))
        .map("640x480",     Eigen::Vector2i(640,  480))
        .map("800x600",     Eigen::Vector2i(800,  600))
        .map("768x576",     Eigen::Vector2i(768,  576))
        .map("1024x768",    Eigen::Vector2i(1024, 768))
        .map("1024x1024",   Eigen::Vector2i(1024, 1024))
        .map("1280x960",    Eigen::Vector2i(1280, 960))
        .map("1600x1200",   Eigen::Vector2i(1600, 1200));
 
        defineOptionalProperty<std::string>("depthCameraCalibrationFile", "", "Camera depth calibration file (YAML)");
        defineOptionalProperty<std::string>("RGBCameraCalibrationFile", "", "Camera RGB calibration file (YAML)");
 
        defineOptionalProperty<std::string>("sourceFrameName", "",
                                            "Assume point cloud was stored in this frame.");
        defineOptionalProperty<std::string>("TargetFrameName", armarx::GlobalFrame,
                                            "Coordinate frame in which point cloud will be transformed and provided.\n"
                                            "If left empty, no transformation will be applied and source frame name\n"
                                            "will be returned as reference frame.");
 
        defineOptionalProperty<std::string>("RobotStateComponentName", "RobotStateComponent",
                                            "Name of the robot state component to use.");
        defineOptionalProperty<bool>("RemoveNAN", true, "Remove NAN from point cloud");
        defineOptionalProperty<std::string>("ProvidedPointCloudFormat", "XYZRGBA", "Format of the provided point cloud (XYZRGBA, XYZL, XYZRGBL)");
    }
 
    std::string FakePointCloudProvider::getDefaultName() const
    {
        return "FakePointCloudProvider";
    }
 
    void FakePointCloudProvider::onInitComponent()
    {
        ImageProvider::onInitComponent();
        CapturingPointCloudProvider::onInitComponent();
    }
 
    void FakePointCloudProvider::onConnectComponent()
    {
        ImageProvider::onConnectComponent();
        CapturingPointCloudProvider::onConnectComponent();
        //gui
        createOrUpdateRemoteGuiTab(buildGui(), [this](RemoteGui::TabProxy & prx)
        {
            ARMARX_TRACE;
            processGui(prx);
        });
    }
 
    void FakePointCloudProvider::onDisconnectComponent()
    {
        ImageProvider::onDisconnectComponent();
        CapturingPointCloudProvider::onDisconnectComponent();
    }
 
    void FakePointCloudProvider::onExitComponent()
    {
        ImageProvider::onExitComponent();
        CapturingPointCloudProvider::onExitComponent();
    }
 
    void FakePointCloudProvider::onInitCapturingPointCloudProvider()
    {
        getProperty(rewind, "rewind");
        getProperty(pointCloudFileName, "pointCloudFileName");
        getProperty(scaleFactor, "scaleFactor");
        getProperty(sourceFrameName, "sourceFrameName");
        getProperty(targetFrameName, "TargetFrameName");
        getProperty(removeNAN, "RemoveNAN");
 
        getProperty(providedPointCloudFormat, "ProvidedPointCloudFormat");
        ARMARX_INFO << "Providing point clouds in format " << providedPointCloudFormat << ".";
 
        getProperty(robotStateComponentName, "RobotStateComponentName");
        if (!robotStateComponentName.empty())
        {
            usingProxy(robotStateComponentName);
        }
 
        // Check invalid or dangerous configurations and warn early.
        if (!(sourceFrameName.empty() && targetFrameName.empty()))
        {
            if (!sourceFrameName.empty() && !targetFrameName.empty() && robotStateComponentName.empty())
            {
                ARMARX_ERROR << "Source and target frames specified, but no RobotStateComponent is specified.\n"
                             "No transformation will be applied.";
            }
            else if (sourceFrameName.empty() && !targetFrameName.empty())
            {
                ARMARX_WARNING << "Target frame was specified (" << targetFrameName << "), but no source frame is specified.\n"
                               << "Did you set the property `sourceFrameName`? (No transformation will be applied.)";
            }
            else if (!sourceFrameName.empty() && targetFrameName.empty())
            {
                ARMARX_INFO << "Source frame was specified (" << sourceFrameName << "), but no target frame is specified.\n"
                            << "No transformation will be applied and source frame is returned as reference frame.";
            }
        }
 
 
        if (!ArmarXDataPath::getAbsolutePath(pointCloudFileName, pointCloudFileName))
        {
            ARMARX_ERROR << "Could not find point cloud file in ArmarXDataPath: " << pointCloudFileName;
        }
 
        // Loading each file
        if (std::filesystem::is_directory(pointCloudFileName))
        {
            loadPointCloudDirectory(pointCloudFileName);
        }
        else
        {
            loadPointCloud(pointCloudFileName);
        }
 
        if (pointClouds.empty())
        {
            ARMARX_FATAL << "Unable to load point cloud: " << pointCloudFileName;
            throw LocalException("Unable to load point cloud");
        }
 
        ARMARX_INFO << "Loaded " << pointClouds.size() << " point clouds.";
 
 
        visionx::CameraParameters RGBCameraIntrinsics;
        RGBCameraIntrinsics.focalLength.resize(2);
        RGBCameraIntrinsics.principalPoint.resize(2);
 
        if (!getProperty<std::string>("RGBCameraCalibrationFile").getValue().empty() && !loadCalibrationFile(getProperty<std::string>("RGBCameraCalibrationFile").getValue(), RGBCameraIntrinsics))
        {
            ARMARX_WARNING << "Could not load rgb camera parameter file " << getProperty<std::string>("RGBCameraCalibrationFile").getValue() << " - using camera uncalibrated";
        }
 
        visionx::CameraParameters depthCameraIntrinsics;
        depthCameraIntrinsics.focalLength.resize(2);
        depthCameraIntrinsics.principalPoint.resize(2);
 
        if (!getProperty<std::string>("depthCameraCalibrationFile").getValue().empty() && !loadCalibrationFile(getProperty<std::string>("depthCameraCalibrationFile").getValue(), depthCameraIntrinsics))
        {
            ARMARX_WARNING << "Could not load depth camera parameter file " << getProperty<std::string>("depthCameraCalibrationFile").getValue() << " - using camera uncalibrated";
        }
 
        calibration.calibrationLeft = visionx::tools::createDefaultMonocularCalibration();
        calibration.calibrationRight = visionx::tools::createDefaultMonocularCalibration();
        calibration.calibrationLeft.cameraParam = RGBCameraIntrinsics;
        calibration.calibrationRight.cameraParam = depthCameraIntrinsics;
    }
 
    bool FakePointCloudProvider::loadPointCloud(const std::string& fileName)
    {
        if (simox::alg::ends_with(fileName, ".pcd") && std::filesystem::is_regular_file(fileName))
        {
            pcl::PCLPointCloud2Ptr cloudptr(new pcl::PCLPointCloud2());
            ARMARX_INFO << "Loading: " << fileName;
 
            if (io::loadPCDFile(fileName.c_str(), *cloudptr) == -1)
            {
                ARMARX_WARNING << "Unable to load point cloud from file " << fileName;
                return false;
            }
            else
            {
                pointClouds.push_back(cloudptr);
                return true;
            }
        }
 
        return false;
    }
 
    bool FakePointCloudProvider::loadPointCloudDirectory(const std::string& directoryName)
    {
        // reading file names
        std::vector<std::string> fileNames;
 
        for (std::filesystem::recursive_directory_iterator dir(directoryName), end;
             dir != end && getState() < eManagedIceObjectExiting; ++dir)
        {
            // Search for all *.pcd files
            if (dir->path().extension() == ".pcd")
            {
                fileNames.push_back(dir->path().string());
            }
        }
 
        ARMARX_INFO << "In total " << fileNames.size() << " point clouds found.";
 
        // Sorting file names and loading them.
        std::sort(fileNames.begin(),  fileNames.end());
 
        for (std::size_t f = 0; f < fileNames.size(); f++)
        {
            ARMARX_INFO << " File " << f << " / " << fileNames.size() << " is being loaded... " << fileNames[f];
            try
            {
                loadPointCloud(fileNames[f]);
            }
            catch (...)
            {
                ARMARX_WARNING << "Loading pointcloud '" << fileNames[f] << "' failed: " << armarx::GetHandledExceptionString();
            }
        }
 
        return true;
    }
 
    void FakePointCloudProvider::onExitCapturingPointCloudProvider()
    {
        pointClouds.clear();
    }
 
    MetaPointCloudFormatPtr FakePointCloudProvider::getDefaultPointCloudFormat()
    {
        MetaPointCloudFormatPtr format = new MetaPointCloudFormat();
 
        if (providedPointCloudFormat == "XYZL")
        {
            format->type = PointContentType::eLabeledPoints;
        }
        else if (providedPointCloudFormat == "XYZRGBL")
        {
            format->type = PointContentType::eColoredLabeledPoints;
        }
        else
        {
            format->type = PointContentType::eColoredPoints;
        }
 
        format->capacity = static_cast<Ice::Int>(
                               640 * 480 * visionx::tools::getBytesPerPoint(format->type) * 50);
        format->size = format->capacity;
        return format;
    }
 
    void FakePointCloudProvider::onInitImageProvider()
    {
        Eigen::Vector2i dimensions = getProperty<Eigen::Vector2i>("dimensions");
        setImageFormat(visionx::ImageDimension(dimensions(0), dimensions(1)), visionx::eRgb);
        setNumberImages(1);
        rgbImages = new CByteImage*[1];
        rgbImages[0] = new CByteImage(dimensions(0), dimensions(1), CByteImage::eRGB24);
        ::ImageProcessor::Zero(rgbImages[0]);
 
        counter = 0;
    }
 
    void FakePointCloudProvider::onStartCapture(float _frameRate)
    {
        (void) _frameRate; // Unused.
        if (!robotStateComponentName.empty())
        {
            getProxy(robotStateComponent, robotStateComponentName);
            if (!robotStateComponent)
            {
                ARMARX_ERROR << "Failed to obtain robot state component.";
                return;
            }
 
            localRobot = armarx::RemoteRobot::createLocalClone(robotStateComponent);
        }
    }
 
    void FakePointCloudProvider::onStopCapture()
    {
    }
 
    bool FakePointCloudProvider::doCapture()
    {
        pcl::PCLPointCloud2Ptr cloudptr;
 
        {
            std::scoped_lock lock(pointCloudMutex);
 
            if (rewind && static_cast<size_t>(counter.load()) >= pointClouds.size())
            {
                counter = 0;
            }
 
            cloudptr = pointClouds.at(counter);
        }
 
        // Provide point cloud in configured format
        if (providedPointCloudFormat == "XYZRGBA")
        {
            return processAndProvide<pcl::PointXYZRGBA>(cloudptr);
        }
        else if (providedPointCloudFormat == "XYZL")
        {
            return processAndProvide<pcl::PointXYZL>(cloudptr);
        }
        else if (providedPointCloudFormat == "XYZRGBL")
        {
            return processAndProvide<pcl::PointXYZRGBL>(cloudptr);
        }
        else
        {
            ARMARX_ERROR << "Could not provide point cloud, because format '" << providedPointCloudFormat << "' is unknown";
            return false;
        }
    }
 
    bool FakePointCloudProvider::loadCalibrationFile(std::string fileName, visionx::CameraParameters& calibration)
    {
        ArmarXDataPath::getAbsolutePath(fileName, fileName, {}, false);
        cv::FileStorage fs(fileName, cv::FileStorage::READ);
 
        if (!fs.isOpened())
        {
            return false;
        }
 
        std::string cameraName = fs["camera_name"];
        ARMARX_LOG << "loading calibration file for " << cameraName;
 
 
        Eigen::Vector2i dimensions = getProperty<Eigen::Vector2i>("dimensions").getValue();
        int imageWidth = fs["image_width"];
        int imageHeight = fs["image_height"];
 
        if (imageWidth != dimensions(0) || imageHeight != dimensions(1))
        {
            ARMARX_ERROR << "invalid camera size";
            return false;
        }
 
        cv::Mat cameraMatrix, distCoeffs;
 
        fs["camera_matrix"] >> cameraMatrix;
        fs["distortion_coefficients:"] >> distCoeffs;
 
        for (int i = 0; i < 5; i++)
        {
            calibration.distortion[static_cast<std::size_t>(i)] = distCoeffs.at<float>(0, i);
        }
 
        calibration.focalLength[0] = cameraMatrix.at<float>(0, 0);
        calibration.focalLength[1] = cameraMatrix.at<float>(1, 1);
 
        calibration.principalPoint[0] = cameraMatrix.at<float>(2, 1);
        calibration.principalPoint[1] = cameraMatrix.at<float>(2, 2);
 
        return true;
    }
 
    PropertyDefinitionsPtr FakePointCloudProvider::createPropertyDefinitions()
    {
        return PropertyDefinitionsPtr(new FakePointCloudProviderPropertyDefinitions(getConfigIdentifier()));
    }
 
    void FakePointCloudProvider::setPointCloudFilename(const std::string& filename, const Ice::Current&)
    {
        std::scoped_lock lock(pointCloudMutex);
 
        ARMARX_INFO << "Changing point cloud filename to: " << filename;
 
        pointClouds.clear();
        loadPointCloud(filename);
    }
 
    bool FakePointCloudProvider::hasSharedMemorySupport(const Ice::Current&)
    {
        return true;
    }
 
    template<typename PointT>
    bool FakePointCloudProvider::processAndProvide(const PCLPointCloud2Ptr& pointCloud)
    {
        using PointCloudPtrT = typename pcl::PointCloud<PointT>::Ptr;
 
        PointCloudPtrT cloudptr(new pcl::PointCloud<PointT>());
        pcl::fromPCLPointCloud2(*pointCloud, *cloudptr);
 
        if (cloudptr->isOrganized())
        {
            processRGBImage(pointCloud);
        }
 
        PointCloudPtrT resultCloudPtr(new pcl::PointCloud<PointT>());
 
        if (scaleFactor > 0)
        {
            Eigen::Affine3f transform(Eigen::Scaling(scaleFactor));
            pcl::transformPointCloud(*cloudptr, *resultCloudPtr, transform);
        }
        else
        {
            // Copy initial point cloud for possibly applying a second transformation
            pcl::copyPointCloud(*cloudptr, *resultCloudPtr);
        }
 
        const IceUtil::Time time = TimeUtil::GetTime();
        resultCloudPtr->header.stamp = static_cast<uint64_t>(time.toMicroSeconds());
 
        if (!sourceFrameName.empty() || !targetFrameName.empty())
        {
            if (!sourceFrameName.empty() && !targetFrameName.empty())
            {
                // Assume point cloud was stored in source frame and transform to target frame.
                if (localRobot)
                {
                    armarx::RemoteRobot::synchronizeLocalCloneToTimestamp(
                        localRobot, robotStateComponent, time.toMicroSeconds());
                    FramedPointCloud framedPointCloud(resultCloudPtr, sourceFrameName);
                    framedPointCloud.changeFrame(targetFrameName, localRobot);
                }
                else
                {
                    // This was already logged in the onInit().
                    // ARMARX_ERROR << "Source and target frames specified, but no RobotStateComponent available.\n"
                    //              "No transformation is applied.";
                }
            }
            else if (sourceFrameName.empty())
            {
                // This was already logged in the onInit().
                // ARMARX_WARNING << "Target frame was specified (" << targetFrameName << "), but no source frame is specified.\n"
                //                << "Did you set the property `sourceFrameName`? (No transformation is applied.)";
            }
            else if (targetFrameName.empty())
            {
                // This was already logged in the onInit().
                // ARMARX_INFO << "Source frame was specified (" << sourceFrameName << "), but no target frame is specified.\n"
                //             << "No transformation is applied and source frame is returned as reference frame.";
            }
        }
 
 
        if (removeNAN)
        {
            std::vector<int> indices;
            pcl::removeNaNFromPointCloud(*resultCloudPtr, *resultCloudPtr, indices);
        }
 
        ARMARX_VERBOSE << deactivateSpam() << "Providing point cloud of size " << resultCloudPtr->points.size();
        providePointCloud(resultCloudPtr);
        provideImages(rgbImages);
 
        if (freezeImage)
        {
            counter = freezeImageIdx.load();
        }
        else
        {
            ++counter;
        }
        return true;
    }
 
    bool FakePointCloudProvider::processRGBImage(const PCLPointCloud2Ptr& pointCloud)
    {
        pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudptr(new pcl::PointCloud<pcl::PointXYZRGB>());
        pcl::fromPCLPointCloud2(*pointCloud, *cloudptr);
 
        if (!cloudptr->isOrganized())
        {
            ARMARX_WARNING << deactivateSpam(10) << "The point cloud is not organized, so no RGB image is generated";
            return false;
        }
 
        if (static_cast<int>(cloudptr->width) != rgbImages[0]->width
            || static_cast<int>(cloudptr->height) != rgbImages[0]->height)
        {
            ARMARX_WARNING << "Dimensions of point cloud and image do not match, so no RGB image is generated";
            return false;
        }
 
        ::ImageProcessor::Zero(rgbImages[0]);
        const int width = static_cast<int>(cloudptr->width);
        const int height = static_cast<int>(cloudptr->height);
 
        for (int i = 0; i < height; i++)
        {
            for (int j = 0; j < width; j++)
            {
                pcl::PointXYZRGB& point = cloudptr->at(static_cast<std::size_t>(i * width + j));
                rgbImages[0]->pixels[3 * (i * width + j) + 0] = point.r;
                rgbImages[0]->pixels[3 * (i * width + j) + 1] = point.g;
                rgbImages[0]->pixels[3 * (i * width + j) + 2] = point.b;
            }
        }
 
        return true;
    }
 
    void FakePointCloudProvider::onExitImageProvider()
    {
        for (std::size_t i = 0; i < 1; i++)
        {
            delete rgbImages[i];
        }
 
        delete [] rgbImages;
    }
 
    StereoCalibration FakePointCloudProvider::getStereoCalibration(const Ice::Current&)
    {
        return calibration;
    }
 
    bool FakePointCloudProvider::getImagesAreUndistorted(const Ice::Current&)
    {
        return true;
    }
 
    std::string FakePointCloudProvider::getReferenceFrame(const Ice::Current&)
    {
        return !targetFrameName.empty() ? targetFrameName : sourceFrameName;
    }
 
    RemoteGui::WidgetPtr FakePointCloudProvider::buildGui()
    {
        ARMARX_TRACE;
        return RemoteGui::makeSimpleGridLayout().cols(3)
               .addChild(RemoteGui::makeCheckBox("rewind").value(rewind).label("rewind"), 3)
 
               .addChild(RemoteGui::makeCheckBox("removeNAN").value(removeNAN).label("removeNAN"), 3)
 
               .addTextLabel("scaleFactor")
               .addChild(RemoteGui::makeFloatSpinBox("scaleFactor").minmax(0, 10000).steps(10000).decimals(3), 2)
 
               .addChild(RemoteGui::makeCheckBox("freezeImage").value(freezeImage).label("freezeImage"))
               .addChild(RemoteGui::makeIntSpinBox("freezeImageIdx").minmax(0, pointClouds.size() - 1))
               .addTextLabel(" / " + std::to_string(pointClouds.size() - 1));
    }
 
    void FakePointCloudProvider::processGui(armarx::RemoteGui::TabProxy& prx)
    {
        ARMARX_TRACE;
        prx.receiveUpdates();
        prx.getValue(rewind, "rewind");
        prx.getValue(removeNAN, "removeNAN");
        prx.getValue(scaleFactor, "scaleFactor");
        prx.getValue(freezeImage, "freezeImage");
        if (freezeImage)
        {
            prx.getValue(freezeImageIdx, "freezeImageIdx");
        }
        else
        {
            prx.setValue(counter, "freezeImageIdx");
        }
        prx.sendUpdates();
    }
}