chisel_conversions.h

Go to the documentation of this file.

/*
 * chisel_conversions.h
 *
 *  Created on: 2018-03-25
 *      Author: mathieu
 */

#ifndef CHISEL_CONVERSIONS_H_
#define CHISEL_CONVERSIONS_H_

#include <rtabmap/core/CameraModel.h>
#include <open_chisel/Chisel.h>
#include <pcl/PolygonMesh.h>
#include <pcl/common/transforms.h>

namespace rtabmap
{

std::shared_ptr<chisel::ColorImage<unsigned char> > colorImageToChisel(const cv::Mat & image)
{
        UASSERT(image.type() == CV_8UC3 || image.type() == CV_8UC4);
        std::shared_ptr<chisel::ColorImage<unsigned char> > imageChisel(new chisel::ColorImage<unsigned char>(image.cols, image.rows, image.channels()));
        memcpy(imageChisel->GetMutableData(), image.data, image.total()*sizeof(unsigned char)*image.channels());
        return imageChisel;
}

std::shared_ptr<chisel::DepthImage<float> > depthImageToChisel(const cv::Mat & image)
{
        UASSERT(image.type() == CV_32FC1);
        std::shared_ptr<chisel::DepthImage<float> > imageChisel(new chisel::DepthImage<float>(image.cols, image.rows));
        memcpy(imageChisel->GetMutableData(), (float*)image.data, image.total()*sizeof(float));
        return imageChisel;
}

chisel::PinholeCamera cameraModelToChiselCamera(const CameraModel& camera)
{
   chisel::PinholeCamera cameraToReturn;
   chisel::Intrinsics intrinsics;
   intrinsics.SetFx(camera.fx());
   intrinsics.SetFy(camera.fy());
   intrinsics.SetCx(camera.cx());
   intrinsics.SetCy(camera.cy());
   cameraToReturn.SetIntrinsics(intrinsics);
   cameraToReturn.SetWidth(camera.imageWidth());
   cameraToReturn.SetHeight(camera.imageHeight());
   return cameraToReturn;
}

template<typename PointRGBT>
chisel::PointCloudPtr pointCloudRGBToChisel(const typename pcl::PointCloud<PointRGBT>& cloud, const Transform & transform = Transform::getIdentity())
{
        chisel::PointCloudPtr chiselCloud(new chisel::PointCloud());
        chiselCloud->GetMutablePoints().resize(cloud.size());
        chiselCloud->GetMutableColors().resize(cloud.size());
        float byteToFloat = 1.0f / 255.0f;
        int oi=0;
        Eigen::Affine3f transformf = transform.toEigen3f();
        for(unsigned int i=0; i<cloud.size(); ++i)
        {
                const PointRGBT & pt = cloud.at(i);
                if(pcl::isFinite(pt))
                {
                        PointRGBT ptt = pcl::transformPoint(pt, transformf);

                        chisel::Vec3& xyz =  chiselCloud->GetMutablePoints().at(oi);
                        xyz(0) = ptt.x;
                        xyz(1) = ptt.y;
                        xyz(2) = ptt.z;

                        chisel::Vec3& rgb = chiselCloud->GetMutableColors().at(oi);
                        rgb(0) = ptt.r * byteToFloat;
                        rgb(1) = ptt.g * byteToFloat;
                        rgb(2) = ptt.b * byteToFloat;

                        ++oi;
                }
        }
        chiselCloud->GetMutablePoints().resize(oi);
        chiselCloud->GetMutableColors().resize(oi);
        return chiselCloud;
}

template<typename PointT>
chisel::PointCloudPtr pointCloudToChisel(const typename pcl::PointCloud<PointT>& cloud, const Transform & transform = Transform::getIdentity())
{
        chisel::PointCloudPtr chiselCloud(new chisel::PointCloud());
        chiselCloud->GetMutablePoints().resize(cloud.size());
        int oi=0;
        Eigen::Affine3f transformf = transform.toEigen3f();
        for(unsigned int i=0; i<cloud.size(); ++i)
        {
                const PointT & pt = cloud.at(i);
                if(pcl::isFinite(pt))
                {
                        PointT ptt = pcl::transformPoint(pt, transformf);

                        chisel::Vec3& xyz =  chiselCloud->GetMutablePoints().at(oi);
                        xyz(0) = ptt.x;
                        xyz(1) = ptt.y;
                        xyz(2) = ptt.z;

                        ++oi;
                }
        }
        chiselCloud->GetMutablePoints().resize(oi);
        return chiselCloud;
}

pcl::PolygonMesh::Ptr chiselToPolygonMesh(const chisel::MeshMap& meshMap, unsigned char r=100, unsigned char g=100, unsigned char b=100)
{
        pcl::PolygonMesh::Ptr mesh (new pcl::PolygonMesh);

        if(meshMap.size())
        {
                bool hasColor = meshMap.begin()->second->colors.size();
                pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
                size_t v = 0;
                for (const std::pair<chisel::ChunkID, chisel::MeshPtr>& it : meshMap)
                {
                        UASSERT((!hasColor || (it.second->vertices.size() == it.second->colors.size())) &&
                                        it.second->vertices.size() == it.second->normals.size());
                        cloud->resize(cloud->size() + it.second->vertices.size());

                        mesh->polygons.resize(mesh->polygons.size()+it.second->vertices.size()/3);

                        for (unsigned int i=0;i<it.second->vertices.size(); ++i)
                        {
                                pcl::PointXYZRGBNormal & pt = cloud->at(v);
                                pt.x = it.second->vertices[i][0];
                                pt.y = it.second->vertices[i][1];
                                pt.z = it.second->vertices[i][2];
                                if(hasColor)
                                {
                                        pt.r = it.second->colors[i][0] * 255.0f;
                                        pt.g = it.second->colors[i][1] * 255.0f;
                                        pt.b = it.second->colors[i][2] * 255.0f;
                                }
                                else
                                {
                                        pt.r = r;
                                        pt.g = g;
                                        pt.b = b;
                                }
                                pt.normal_x = it.second->normals[i][0];
                                pt.normal_y = it.second->normals[i][1];
                                pt.normal_z = it.second->normals[i][2];
                                pcl::Vertices & polygon = mesh->polygons.at(v/3);
                                polygon.vertices.push_back(v++);
                        }
                }
                pcl::toPCLPointCloud2(*cloud, mesh->cloud);
        }
        return mesh;
}

}


#endif /* CHISEL_CONVERSIONS_H_ */