chisel_conversions.h
Go to the documentation of this file.
1 /*
2  * chisel_conversions.h
3  *
4  * Created on: 2018-03-25
5  * Author: mathieu
6  */
7 
8 #ifndef CHISEL_CONVERSIONS_H_
9 #define CHISEL_CONVERSIONS_H_
10 
12 #include <open_chisel/Chisel.h>
13 #include <pcl/PolygonMesh.h>
14 #include <pcl/common/transforms.h>
15 
16 namespace rtabmap
17 {
18 
19 std::shared_ptr<chisel::ColorImage<unsigned char> > colorImageToChisel(const cv::Mat & image)
20 {
21  UASSERT(image.type() == CV_8UC3 || image.type() == CV_8UC4);
22  std::shared_ptr<chisel::ColorImage<unsigned char> > imageChisel(new chisel::ColorImage<unsigned char>(image.cols, image.rows, image.channels()));
23  memcpy(imageChisel->GetMutableData(), image.data, image.total()*sizeof(unsigned char)*image.channels());
24  return imageChisel;
25 }
26 
27 std::shared_ptr<chisel::DepthImage<float> > depthImageToChisel(const cv::Mat & image)
28 {
29  UASSERT(image.type() == CV_32FC1);
30  std::shared_ptr<chisel::DepthImage<float> > imageChisel(new chisel::DepthImage<float>(image.cols, image.rows));
31  memcpy(imageChisel->GetMutableData(), (float*)image.data, image.total()*sizeof(float));
32  return imageChisel;
33 }
34 
35 chisel::PinholeCamera cameraModelToChiselCamera(const CameraModel& camera)
36 {
37  chisel::PinholeCamera cameraToReturn;
38  chisel::Intrinsics intrinsics;
39  intrinsics.SetFx(camera.fx());
40  intrinsics.SetFy(camera.fy());
41  intrinsics.SetCx(camera.cx());
42  intrinsics.SetCy(camera.cy());
43  cameraToReturn.SetIntrinsics(intrinsics);
44  cameraToReturn.SetWidth(camera.imageWidth());
45  cameraToReturn.SetHeight(camera.imageHeight());
46  return cameraToReturn;
47 }
48 
49 template<typename PointRGBT>
50 chisel::PointCloudPtr pointCloudRGBToChisel(const typename pcl::PointCloud<PointRGBT>& cloud, const Transform & transform = Transform::getIdentity())
51 {
52  chisel::PointCloudPtr chiselCloud(new chisel::PointCloud());
53  chiselCloud->GetMutablePoints().resize(cloud.size());
54  chiselCloud->GetMutableColors().resize(cloud.size());
55  float byteToFloat = 1.0f / 255.0f;
56  int oi=0;
57  Eigen::Affine3f transformf = transform.toEigen3f();
58  for(unsigned int i=0; i<cloud.size(); ++i)
59  {
60  const PointRGBT & pt = cloud.at(i);
61  if(pcl::isFinite(pt))
62  {
63  PointRGBT ptt = pcl::transformPoint(pt, transformf);
64 
65  chisel::Vec3& xyz = chiselCloud->GetMutablePoints().at(oi);
66  xyz(0) = ptt.x;
67  xyz(1) = ptt.y;
68  xyz(2) = ptt.z;
69 
70  chisel::Vec3& rgb = chiselCloud->GetMutableColors().at(oi);
71  rgb(0) = ptt.r * byteToFloat;
72  rgb(1) = ptt.g * byteToFloat;
73  rgb(2) = ptt.b * byteToFloat;
74 
75  ++oi;
76  }
77  }
78  chiselCloud->GetMutablePoints().resize(oi);
79  chiselCloud->GetMutableColors().resize(oi);
80  return chiselCloud;
81 }
82 
83 template<typename PointT>
84 chisel::PointCloudPtr pointCloudToChisel(const typename pcl::PointCloud<PointT>& cloud, const Transform & transform = Transform::getIdentity())
85 {
86  chisel::PointCloudPtr chiselCloud(new chisel::PointCloud());
87  chiselCloud->GetMutablePoints().resize(cloud.size());
88  int oi=0;
89  Eigen::Affine3f transformf = transform.toEigen3f();
90  for(unsigned int i=0; i<cloud.size(); ++i)
91  {
92  const PointT & pt = cloud.at(i);
93  if(pcl::isFinite(pt))
94  {
95  PointT ptt = pcl::transformPoint(pt, transformf);
96 
97  chisel::Vec3& xyz = chiselCloud->GetMutablePoints().at(oi);
98  xyz(0) = ptt.x;
99  xyz(1) = ptt.y;
100  xyz(2) = ptt.z;
101 
102  ++oi;
103  }
104  }
105  chiselCloud->GetMutablePoints().resize(oi);
106  return chiselCloud;
107 }
108 
109 pcl::PolygonMesh::Ptr chiselToPolygonMesh(const chisel::MeshMap& meshMap, unsigned char r=100, unsigned char g=100, unsigned char b=100)
110 {
111  pcl::PolygonMesh::Ptr mesh (new pcl::PolygonMesh);
112 
113  if(meshMap.size())
114  {
115  bool hasColor = meshMap.begin()->second->colors.size();
116  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
117  size_t v = 0;
118  for (const std::pair<chisel::ChunkID, chisel::MeshPtr>& it : meshMap)
119  {
120  UASSERT((!hasColor || (it.second->vertices.size() == it.second->colors.size())) &&
121  it.second->vertices.size() == it.second->normals.size());
122  cloud->resize(cloud->size() + it.second->vertices.size());
123 
124  mesh->polygons.resize(mesh->polygons.size()+it.second->vertices.size()/3);
125 
126  for (unsigned int i=0;i<it.second->vertices.size(); ++i)
127  {
128  pcl::PointXYZRGBNormal & pt = cloud->at(v);
129  pt.x = it.second->vertices[i][0];
130  pt.y = it.second->vertices[i][1];
131  pt.z = it.second->vertices[i][2];
132  if(hasColor)
133  {
134  pt.r = it.second->colors[i][0] * 255.0f;
135  pt.g = it.second->colors[i][1] * 255.0f;
136  pt.b = it.second->colors[i][2] * 255.0f;
137  }
138  else
139  {
140  pt.r = r;
141  pt.g = g;
142  pt.b = b;
143  }
144  pt.normal_x = it.second->normals[i][0];
145  pt.normal_y = it.second->normals[i][1];
146  pt.normal_z = it.second->normals[i][2];
147  pcl::Vertices & polygon = mesh->polygons.at(v/3);
148  polygon.vertices.push_back(v++);
149  }
150  }
151  pcl::toPCLPointCloud2(*cloud, mesh->cloud);
152  }
153  return mesh;
154 }
155 
156 }
157 
158 
159 #endif /* CHISEL_CONVERSIONS_H_ */
pcl::PolygonMesh::Ptr chiselToPolygonMesh(const chisel::MeshMap &meshMap, unsigned char r=100, unsigned char g=100, unsigned char b=100)
bool RTABMAP_EXP isFinite(const cv::Point3f &pt)
Definition: util3d.cpp:3108
int imageWidth() const
Definition: CameraModel.h:120
std::shared_ptr< chisel::ColorImage< unsigned char > > colorImageToChisel(const cv::Mat &image)
cv::Point3f RTABMAP_EXP transformPoint(const cv::Point3f &pt, const Transform &transform)
static Transform getIdentity()
Definition: Transform.cpp:380
chisel::PinholeCamera cameraModelToChiselCamera(const CameraModel &camera)
#define UASSERT(condition)
double cx() const
Definition: CameraModel.h:104
chisel::PointCloudPtr pointCloudToChisel(const typename pcl::PointCloud< PointT > &cloud, const Transform &transform=Transform::getIdentity())
double fy() const
Definition: CameraModel.h:103
chisel::PointCloudPtr pointCloudRGBToChisel(const typename pcl::PointCloud< PointRGBT > &cloud, const Transform &transform=Transform::getIdentity())
double cy() const
Definition: CameraModel.h:105
double fx() const
Definition: CameraModel.h:102
int imageHeight() const
Definition: CameraModel.h:121
std::shared_ptr< chisel::DepthImage< float > > depthImageToChisel(const cv::Mat &image)


rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Dec 14 2020 03:34:58