fixed save point cloud

orbbec · Sep 15, 2023 · 4e2e201 · 4e2e201
1 parent e0d8263
commit 4e2e201
Show file tree

Hide file tree

Showing 3 changed files with 45 additions and 4 deletions.
diff --git a/include/orbbec_camera/utils.h b/include/orbbec_camera/utils.h
@@ -38,7 +38,10 @@ void savePointsToPly(std::shared_ptr<ob::Frame> frame, const std::string &fileNa
 
 void saveRGBPointsToPly(std::shared_ptr<ob::Frame> frame, const std::string &fileName);
 
-void soavePointCloudMsgToPly(const sensor_msgs::PointCloud2 &msg, const std::string &fileName);
+void saveRGBPointCloudMsgToPly(const sensor_msgs::PointCloud2 &msg, const std::string &fileName);
+
+void saveDepthPointCloudMsgToPly(const sensor_msgs::PointCloud2 &msg, const std::string &fileName);
+
 
 tf2::Quaternion rotationMatrixToQuaternion(const float rotation[9]);
 

diff --git a/src/ob_camera_node.cpp b/src/ob_camera_node.cpp
@@ -495,7 +495,7 @@ void OBCameraNode::publishDepthPointCloud(const std::shared_ptr<ob::FrameSet>& f
       boost::filesystem::create_directory(current_path + "/point_cloud");
     }
     ROS_INFO_STREAM("Saving point cloud to " << filename);
-    soavePointCloudMsgToPly(cloud_msg_, filename);
+    saveDepthPointCloudMsgToPly(cloud_msg_, filename);
   }
 }
 
@@ -601,7 +601,7 @@ void OBCameraNode::publishColoredPointCloud(const std::shared_ptr<ob::FrameSet>&
       boost::filesystem::create_directory(current_path + "/point_cloud");
     }
     ROS_INFO_STREAM("Saving point cloud to " << filename);
-    soavePointCloudMsgToPly(cloud_msg_, filename);
+    saveRGBPointCloudMsgToPly(cloud_msg_, filename);
   }
 }
 

diff --git a/src/utils.cpp b/src/utils.cpp
@@ -221,7 +221,7 @@ void saveRGBPointsToPly(std::shared_ptr<ob::Frame> frame, const std::string &fil
   fclose(fp);
 }
 
-void soavePointCloudMsgToPly(const sensor_msgs::PointCloud2 &msg, const std::string &fileName) {
+void saveRGBPointCloudMsgToPly(const sensor_msgs::PointCloud2 &msg, const std::string &fileName) {
   FILE *fp = fopen(fileName.c_str(), "wb+");
   CHECK_NOTNULL(fp);
 
@@ -267,6 +267,44 @@ void soavePointCloudMsgToPly(const sensor_msgs::PointCloud2 &msg, const std::str
   fclose(fp);
 }
 
+void saveDepthPointCloudMsgToPly(const sensor_msgs::PointCloud2 &msg, const std::string &fileName) {
+  FILE *fp = fopen(fileName.c_str(), "wb+");
+  CHECK_NOTNULL(fp);
+  sensor_msgs::PointCloud2ConstIterator<float> iter_x(msg, "x");
+  sensor_msgs::PointCloud2ConstIterator<float> iter_y(msg, "y");
+  sensor_msgs::PointCloud2ConstIterator<float> iter_z(msg, "z");
+
+  // First, count the actual number of valid points
+  size_t valid_points = 0;
+  for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) {
+    if (!std::isnan(*iter_x) && !std::isnan(*iter_y) && !std::isnan(*iter_z)) {
+      ++valid_points;
+    }
+  }
+
+  // Reset the iterators
+  iter_x = sensor_msgs::PointCloud2ConstIterator<float>(msg, "x");
+  iter_y = sensor_msgs::PointCloud2ConstIterator<float>(msg, "y");
+  iter_z = sensor_msgs::PointCloud2ConstIterator<float>(msg, "z");
+
+  fprintf(fp, "ply\n");
+  fprintf(fp, "format ascii 1.0\n");
+  fprintf(fp, "element vertex %zu\n", valid_points);
+  fprintf(fp, "property float x\n");
+  fprintf(fp, "property float y\n");
+  fprintf(fp, "property float z\n");
+  fprintf(fp, "end_header\n");
+
+  for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) {
+    if (!std::isnan(*iter_x) && !std::isnan(*iter_y) && !std::isnan(*iter_z)) {
+      fprintf(fp, "%.3f %.3f %.3f\n", *iter_x, *iter_y, *iter_z);
+    }
+  }
+
+  fflush(fp);
+  fclose(fp);
+}
+
 void savePointsToPly(std::shared_ptr<ob::Frame> frame, const std::string &fileName) {
   size_t point_size = frame->dataSize() / sizeof(OBPoint);
   FILE *fp = fopen(fileName.c_str(), "wb+");