MultiSensePointCloudProvider.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::MultiSensePointCloudProvider
 * @author     Markus Grotz ( markus dot grotz at kit dot edu )
 * @date       2016
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "MultiSensePointCloudProvider.h"
 
#include <ArmarXCore/core/system/ArmarXDataPath.h>
 
#include <VisionX/tools/TypeMapping.h>
 
// IVT
#include <Calibration/StereoCalibration.h>
 
namespace armarx
{
 
    void
    disparityCallback(const crl::multisense::image::Header& header, void* userDataP)
    {
        reinterpret_cast<MultiSensePointCloudProvider*>(userDataP)->disparityImageCallback(header);
    }
 
    void
    chromaCallback(const crl::multisense::image::Header& header, void* userDataP)
    {
        reinterpret_cast<MultiSensePointCloudProvider*>(userDataP)->chromaImageCallback(header);
    }
 
    void
    lumaCallback(const crl::multisense::image::Header& header, void* userDataP)
    {
        reinterpret_cast<MultiSensePointCloudProvider*>(userDataP)->lumaImageCallback(header);
    }
 
    void
    lidarCallback(const crl::multisense::lidar::Header& header, void* userDataP)
    {
        reinterpret_cast<MultiSensePointCloudProvider*>(userDataP)->lidarScanCallback(header);
    }
 
    void
    MultiSensePointCloudProvider::onInitCapturingPointCloudProvider()
    {
        crl::multisense::image::Calibration c;
        crl::multisense::lidar::Calibration l;
        crl::multisense::Status status;
 
        try
        {
            driver =
                crl::multisense::Channel::Create(getProperty<std::string>("ipAddress").getValue());
 
            if (!driver)
            {
                throw std::runtime_error("Unable to connect to multisense device");
            }
 
            crl::multisense::Status status = driver->getImageCalibration(c);
            if (status != crl::multisense::Status_Ok)
            {
                throw std::runtime_error("Unable to retrieve image calibration.");
            }
 
 
            if (getProperty<bool>("useLidar").getValue())
            {
                driver->getLidarCalibration(l);
                if (status != crl::multisense::Status_Ok)
                {
                    throw std::runtime_error("Unable to retrieve liadr calibration.");
                }
 
                //cameraToSpindle = Eigen::Matrix4f(l.cameraToSpindleFixed);
                //laserToSpindl = Eigen::Matrix4f(l.laserToSpindle);
            }
        }
        catch (std::exception& e)
        {
            ARMARX_FATAL << "Multisense error: " << e.what()
                         << " Try entering ' roslaunch multisense_bringup multisense.launch "
                            "mtu:=1500 ' in your console";
            crl::multisense::Channel::Destroy(driver);
        }
 
 
        // see http://carnegierobotics.com/support-updates/2015/10/27/multisense-q-reprojection-matrix
 
        q_matrix = cv::Mat::zeros(4, 4, cv::DataType<double>::type);
 
        const int width = 1024;
        const int height = 544;
 
        rgbImage = cv::Mat::zeros(height, width, CV_8UC3);
        disparityImage = cv::Mat::zeros(height, width, CV_16SC1);
 
 
        rgbImages = new CByteImage*[2];
 
        rgbImages[0] = new CByteImage(width, height, CByteImage::eRGB24);
        rgbImages[1] = new CByteImage(width, height, CByteImage::eRGB24);
 
        /*
        Q = [ FyTx,        0,     0,    -FyCxTx]
            [    0,     FxTx,     0,    -FxCyTx]
            [    0,        0,     0,     FxFyTx]
            [    0,        0,   -Fy,          0]
 
        P_right = [Fx,     0,    cx,   FxTx]
                  [ 0,    Fy,    cy,      0]
                  [ 0,     0,     1,      0]
        */
 
        double xScale = 2048 / width;
        double yScale = 1088 / height;
        double Fx = c.right.P[0][0] / xScale;
        double Cx = c.right.P[0][2] / xScale;
        double Fy = c.right.P[1][1] / yScale;
        double Cy = c.right.P[1][2] / yScale;
        double Tx = (c.right.P[0][3] / xScale) / Fx;
 
        q_matrix.at<double>(0, 0) = Fy * Tx;
        q_matrix.at<double>(0, 3) = -Fy * Cx * Tx;
        q_matrix.at<double>(1, 1) = Fx * Tx;
        q_matrix.at<double>(1, 3) = -Fx * Cy * Tx;
        q_matrix.at<double>(2, 3) = Fx * Fy * Tx;
        q_matrix.at<double>(3, 2) = -Fy;
 
 
        std::string calibrationFileName =
            getProperty<std::string>("CalibrationFileName").getValue();
        if (armarx::ArmarXDataPath::getAbsolutePath(calibrationFileName, calibrationFileName))
        {
            CStereoCalibration ivtStereoCalibration;
            if (ivtStereoCalibration.LoadCameraParameters(calibrationFileName.c_str(), true))
            {
                calibration = visionx::tools::convert(ivtStereoCalibration);
            }
            else
            {
                ARMARX_WARNING << "Failed loading calibration file " << calibrationFileName;
            }
        }
        else
        {
            ARMARX_WARNING << "Calibration file with file name " << calibrationFileName
                           << " not found";
 
            calibration.calibrationLeft.cameraParam.width = width;
            calibration.calibrationLeft.cameraParam.height = height;
            calibration.calibrationLeft.cameraParam.principalPoint.push_back(Cx);
            calibration.calibrationLeft.cameraParam.principalPoint.push_back(Cy);
            calibration.calibrationLeft.cameraParam.focalLength.push_back(Fx);
            calibration.calibrationLeft.cameraParam.focalLength.push_back(Fy);
            //::visionx::types::Vec distortion;
            //::visionx::types::Mat rotation;
            //::visionx::types::Vec translation;
            //calibration.calibrationLeft.cameraParam.distortion;
            //calibration.calibrationLeft.cameraParam.rotation;
            //calibration.calibrationLeft.cameraParam.translation;
        }
 
        Eigen::Vector4f minPoint = getProperty<Eigen::Vector4f>("minPoint").getValue();
        Eigen::Vector4f maxPoint = getProperty<Eigen::Vector4f>("maxPoint").getValue();
        ;
 
        cropBoxFilter.setMin(minPoint);
        cropBoxFilter.setMax(maxPoint);
 
        mask = crl::multisense::Source_Unknown;
 
 
        if (getProperty<bool>("useLidar").getValue())
        {
            status = driver->addIsolatedCallback(lidarCallback, this);
            mask |= crl::multisense::Source_Lidar_Scan;
        }
        else
        {
            status = driver->addIsolatedCallback(
                disparityCallback, crl::multisense::Source_Disparity, this);
            mask |= crl::multisense::Source_Disparity;
        }
 
        if (status != crl::multisense::Status_Ok)
        {
            throw std::runtime_error("Unable to attach isolated callback");
        }
 
 
        status = driver->addIsolatedCallback(lumaCallback,
                                             crl::multisense::Source_Luma_Left |
                                                 crl::multisense::Source_Luma_Right,
                                             this);
        mask |= crl::multisense::Source_Luma_Left | crl::multisense::Source_Luma_Right;
 
        if (status != crl::multisense::Status_Ok)
        {
            throw std::runtime_error("Unable to attach isolated callback");
        }
 
 
        status =
            driver->addIsolatedCallback(chromaCallback, crl::multisense::Source_Chroma_Left, this);
        mask |= crl::multisense::Source_Chroma_Left;
 
        if (status != crl::multisense::Status_Ok)
        {
            throw std::runtime_error("Unable to attach isolated callback");
        }
 
        if (getProperty<bool>("enableLight").getValue())
        {
            crl::multisense::lighting::Config lightingConfig;
            lightingConfig.setDutyCycle(crl::multisense::lighting::MAX_DUTY_CYCLE);
 
            if (driver->setLightingConfig(lightingConfig) != crl::multisense::Status_Ok)
            {
                ARMARX_WARNING << "Failed to switch on LED light";
            }
            else
            {
                ARMARX_INFO << "Switched on LED light";
            }
        }
    }
 
    void
    MultiSensePointCloudProvider::onExitCapturingPointCloudProvider()
    {
        crl::multisense::Status status;
 
        if (getProperty<bool>("useLidar").getValue())
        {
            status = driver->removeIsolatedCallback(lidarCallback);
        }
        else
        {
            status = driver->removeIsolatedCallback(disparityCallback);
        }
 
 
        if (status != crl::multisense::Status_Ok)
        {
            ARMARX_FATAL << "Unable to remove isolated callback";
        }
 
        status = driver->removeIsolatedCallback(chromaCallback);
 
        if (status != crl::multisense::Status_Ok)
        {
            ARMARX_FATAL << "Unable to remove isolated callback";
        }
 
        status = driver->removeIsolatedCallback(lumaCallback);
 
        if (status != crl::multisense::Status_Ok)
        {
            ARMARX_FATAL << "Unable to remove isolated callback";
        }
 
        crl::multisense::Channel::Destroy(driver);
 
        delete rgbImages[0];
        delete rgbImages[1];
 
        delete[] rgbImages;
    }
 
    void
    MultiSensePointCloudProvider::onStartCapture(float frameRate)
    {
        crl::multisense::Status status;
        crl::multisense::image::Config config;
 
        try
        {
            status = driver->getImageConfig(config);
            if (status != crl::multisense::Status_Ok)
            {
                throw std::runtime_error("Unable read image config");
            }
 
            config.setFps(frameRate);
            status = driver->setImageConfig(config);
            if (status != crl::multisense::Status_Ok)
            {
                throw std::runtime_error("Unable to set image config");
            }
 
            pointCloudData.reset(new pcl::PointCloud<pcl::PointXYZRGBA>());
 
            pointCloudData->width = config.width();
            pointCloudData->height = config.height();
            pointCloudData->resize(config.width() * config.height());
 
            status = driver->startStreams(mask);
 
            if (status != crl::multisense::Status_Ok)
            {
                throw std::runtime_error("Unable to start image streams");
            }
        }
        catch (std::exception& e)
        {
            ARMARX_FATAL << "multisense error " << e.what();
            crl::multisense::Channel::Destroy(driver);
        }
 
        hasNewColorData = false;
        hasNewLumaData = false;
        hasNewDisparityData = false;
    }
 
    void
    MultiSensePointCloudProvider::onStopCapture()
    {
        crl::multisense::Status status = driver->stopStreams(mask);
 
        if (status != crl::multisense::Status_Ok)
        {
            ARMARX_FATAL << "Unable to stop streams. Status was "
                         << crl::multisense::Channel::statusString(status);
            crl::multisense::Channel::Destroy(driver);
        }
    }
 
    bool
    MultiSensePointCloudProvider::doCapture()
    {
        if (hasNewDisparityData && hasNewColorData)
        {
            pcl::PointCloud<pcl::PointXYZRGBA>::Ptr pointCloudTemp(
                new pcl::PointCloud<pcl::PointXYZRGBA>());
 
            std::unique_lock lock(dataMutex);
 
            cropBoxFilter.setInputCloud(pointCloudData);
            cropBoxFilter.filter(*pointCloudTemp);
 
 
            hasNewColorData = false;
            hasNewDisparityData = false;
 
            lock.unlock();
 
            std::vector<int> index;
            pcl::removeNaNFromPointCloud(*pointCloudTemp, *pointCloudTemp, index);
 
#if 0
            static int x = 0;
            x++;
            std::string outputPCDName = "/home/peter/output_" + boost::lexical_cast<std::string>(x) + ".pcd";
            pcl::io::savePCDFileASCII(outputPCDName.data(), *pointCloudTemp);
#endif
 
 
            ARMARX_INFO << "Providing point cloud of size " << pointCloudTemp->points.size();
            providePointCloud(pointCloudTemp);
 
            provideImages(rgbImages);
            return true;
        }
 
        return false;
    }
 
    void
    MultiSensePointCloudProvider::lumaImageCallback(const crl::multisense::image::Header& header)
    {
        const uint32_t imageSize = header.width * header.height;
 
        std::unique_lock lock(dataMutex);
 
        if (header.source == crl::multisense::Source_Luma_Right)
        {
            uint32_t height = header.height;
            uint32_t width = header.width;
 
            const uint8_t* rightLumaData = reinterpret_cast<const uint8_t*>(header.imageDataP);
 
            for (size_t j = 0; j < height; j++)
            {
                for (size_t i = 0; i < width; i++)
                {
                    const uint32_t index = (j * width) + i;
                    rgbImages[1]->pixels[3 * index + 0] = rightLumaData[index];
                    rgbImages[1]->pixels[3 * index + 1] = rightLumaData[index];
                    rgbImages[1]->pixels[3 * index + 2] = rightLumaData[index];
                }
            }
        }
        else
        {
 
            lumaData.resize(imageSize);
 
            memcpy((void*)&lumaData.data()[0], header.imageDataP, imageSize);
 
            hasNewLumaData = true;
        }
    }
 
    void
    MultiSensePointCloudProvider::chromaImageCallback(const crl::multisense::image::Header& header)
    {
        std::unique_lock lock(dataMutex);
 
        if (!hasNewLumaData)
        {
            return;
        }
 
        const uint8_t* chromaP = reinterpret_cast<const uint8_t*>(header.imageDataP);
 
        uint32_t height = header.height * 2;
        uint32_t width = header.width * 2;
 
        for (size_t j = 0; j < height; j++)
        {
            for (size_t i = 0; i < width; i++)
            {
                const uint32_t lumaOffset = (j * width) + i;
                const uint32_t chromaOffset = 2 * (((j / 2) * (width / 2)) + (i / 2));
 
                const float px_y = static_cast<float>(lumaData[lumaOffset]);
                const float px_cb = static_cast<float>(chromaP[chromaOffset + 0]) - 128.0f;
                const float px_cr = static_cast<float>(chromaP[chromaOffset + 1]) - 128.0f;
 
                float px_r = px_y + 1.402f * px_cr;
                float px_g = px_y - 0.34414f * px_cb - 0.71414f * px_cr;
                float px_b = px_y + 1.772f * px_cb;
 
                px_r = std::max(0.0f, std::min(255.0f, px_r));
                px_g = std::max(0.0f, std::min(255.0f, px_g));
                px_b = std::max(0.0f, std::min(255.0f, px_b));
 
                int index = j * width + i;
 
 
                pcl::PointXYZRGBA& targetPoint = pointCloudData->points[index];
 
                targetPoint.r = static_cast<uint8_t>(px_r);
                targetPoint.g = static_cast<uint8_t>(px_g);
                targetPoint.b = static_cast<uint8_t>(px_b);
 
                rgbImages[0]->pixels[3 * index + 0] = static_cast<uint8_t>(px_r);
                rgbImages[0]->pixels[3 * index + 1] = static_cast<uint8_t>(px_g);
                rgbImages[0]->pixels[3 * index + 2] = static_cast<uint8_t>(px_b);
 
                rgbImage.at<cv::Vec3b>(j, i)[0] = static_cast<uint8_t>(px_r);
                rgbImage.at<cv::Vec3b>(j, i)[1] = static_cast<uint8_t>(px_g);
                rgbImage.at<cv::Vec3b>(j, i)[2] = static_cast<uint8_t>(px_b);
            }
        }
 
        hasNewLumaData = false;
        hasNewColorData = true;
    }
 
    void
    MultiSensePointCloudProvider::lidarScanCallback(const crl::multisense::lidar::Header& header)
    {
 
        std::unique_lock lock(dataMutex);
 
        if (!hasNewColorData)
        {
            return;
        }
 
 
        for (size_t i = 0; i < header.pointCount; i++)
        {
            const uint32_t range = header.rangesP[i];
            const double percent =
                static_cast<double>(i) / static_cast<double>(header.pointCount - 1);
            const double mirrorTheta = -header.scanArc / 2000.0 + percent * header.scanArc / 1000.0;
            const double spindleTheta =
                header.spindleAngleStart / 1000.0 +
                percent * (header.spindleAngleEnd - header.spindleAngleStart) / 1000.0;
 
            Eigen::Matrix4f spindleToMotor = Eigen::Matrix4f::Identity();
            spindleToMotor.block<3, 3>(0, 0) =
                Eigen::AngleAxisf(spindleTheta, Eigen::Vector3f::UnitZ()).matrix();
 
 
            Eigen::Vector4f point(
                range * std::sin(mirrorTheta), 0.0, range * std::cos(mirrorTheta), 1.0);
            Eigen::Vector4f pointCamera = laserToSpindle * point;
            pointCamera = spindleToMotor * pointCamera;
            pointCamera = cameraToSpindle * pointCamera;
 
 
            pcl::PointXYZRGBA& targetPoint = pointCloudData->points[i];
 
            if (std::isfinite(pointCamera[2]))
            {
 
                visionx::CameraParameters p = calibration.calibrationLeft.cameraParam;
 
                int u = (p.focalLength[0] * pointCamera[0] + p.principalPoint[0] * pointCamera[2]) /
                        pointCamera[2];
                int v = (p.focalLength[1] * pointCamera[1] + p.principalPoint[1] * pointCamera[2]) /
                        pointCamera[2];
 
                if (u < p.width && v < p.height && u >= 0 && v >= 0)
                {
                    int index = v * p.width + u;
 
 
                    targetPoint.getArray4fMap() = pointCamera;
 
                    targetPoint.r = static_cast<uint8_t>(rgbImages[0]->pixels[3 * index + 0]);
                    targetPoint.g = static_cast<uint8_t>(rgbImages[0]->pixels[3 * index + 1]);
                    targetPoint.b = static_cast<uint8_t>(rgbImages[0]->pixels[3 * index + 2]);
                }
            }
            else
            {
                targetPoint.x = std::numeric_limits<float>::quiet_NaN();
                targetPoint.y = std::numeric_limits<float>::quiet_NaN();
                targetPoint.z = std::numeric_limits<float>::quiet_NaN();
            }
        }
 
 
        //    pointCloudData->header.frame_id = header.frameId;
        pointCloudData->header.stamp =
            header.timeStartMicroSeconds + header.timeStartSeconds / (1000 * 1000);
 
        hasNewDisparityData = true;
    }
 
    void
    MultiSensePointCloudProvider::disparityImageCallback(
        const crl::multisense::image::Header& header)
    {
        if (header.bitsPerPixel != 16)
        {
            ARMARX_ERROR << "invalid data";
            return;
        }
 
        cv::Mat_<uint16_t> disparityOrigP(
            header.height,
            header.width,
            const_cast<uint16_t*>(reinterpret_cast<const uint16_t*>(header.imageDataP)));
 
        std::unique_lock lock(dataMutex);
 
        cv::Mat_<float> disparity(header.height, header.width);
        disparity = disparityOrigP / 16.0f;
        disparity.copyTo(disparityImage);
 
        cv::Mat_<cv::Vec3f> points(disparityImage.rows, disparityImage.cols);
 
        cv::reprojectImageTo3D(disparity, points, q_matrix, false);
 
        for (size_t j = 0; j < header.height; j++)
        {
            for (size_t i = 0; i < header.width; i++)
            {
                int index = j * header.width + i;
                cv::Vec3f sourcePoint = points.at<cv::Vec3f>(j, i);
                pcl::PointXYZRGBA& targetPoint = pointCloudData->points[index];
 
                //uint16_t value = disparityOrigP.at<uint16_t>(j, i);
 
                //rgbImages[1]->pixels[3 * index + 0] = value & 0xFF;
                //rgbImages[1]->pixels[3 * index + 1] = (value >> 8) & 0xFF;
 
                if (std::isfinite(sourcePoint[2]))
                {
                    targetPoint.x = sourcePoint[0] * 1000.0;
                    targetPoint.y = sourcePoint[1] * 1000.0;
                    targetPoint.z = sourcePoint[2] * 1000.0;
                }
                else
                {
                    targetPoint.x = std::numeric_limits<float>::quiet_NaN();
                    targetPoint.y = std::numeric_limits<float>::quiet_NaN();
                    targetPoint.z = std::numeric_limits<float>::quiet_NaN();
                }
 
                //point.getVector3fMap() =
            }
        }
 
 
        pointCloudData->header.frame_id = header.frameId;
        pointCloudData->header.stamp = header.timeMicroSeconds + header.timeSeconds / (1000 * 1000);
 
 
        hasNewDisparityData = true;
    }
 
    void
    MultiSensePointCloudProvider::onInitImageProvider()
    {
        setImageFormat(visionx::ImageDimension(1024, 544), visionx::eRgb);
        setNumberImages(2);
    }
 
    visionx::StereoCalibration
    MultiSensePointCloudProvider::getStereoCalibration(const Ice::Current&)
    {
        std::unique_lock lock(captureMutex);
 
        return calibration;
    }
 
    visionx::MonocularCalibration
    MultiSensePointCloudProvider::getMonocularCalibration(const Ice::Current& c)
    {
 
        std::unique_lock lock(captureMutex);
 
        return calibration.calibrationLeft;
    }
 
    visionx::MetaPointCloudFormatPtr
    MultiSensePointCloudProvider::getDefaultPointCloudFormat()
    {
        visionx::MetaPointCloudFormatPtr info = new visionx::MetaPointCloudFormat();
        info->capacity = 1024 * 544 * sizeof(visionx::ColoredPoint3D);
        info->size = info->capacity;
        info->type = visionx::PointContentType::eColoredPoints;
        return info;
    }
 
    armarx::PropertyDefinitionsPtr
    MultiSensePointCloudProvider::createPropertyDefinitions()
    {
        return armarx::PropertyDefinitionsPtr(
            new MultiSensePointCloudProviderPropertyDefinitions(getConfigIdentifier()));
    }
} // namespace armarx