i/OrbbecSDK_ROS2
1
0
Fork 0
mirror of https://github.com/orbbec/OrbbecSDK_ROS2.git synced 2026-10-04 12:07:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

fixed save point cloud

Browse Source
This commit is contained in:
Joe Dong
2023-09-15 16:54:30 +08:00
parent 9ef1150a2a
commit d6bf7d014f
5 changed files with 53 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/src/ob_camera_node.cpp
+2 -2
View File
@@ -623,7 +623,7 @@ void OBCameraNode::publishDepthPointCloud(const std::shared_ptr<ob::FrameSet> &f
std::filesystem::create_directory(current_path + "/point_cloud");
}
RCLCPP_INFO_STREAM(logger_, "Saving point cloud to " << filename);
soavePointCloudMsgToPly(point_cloud_msg_, filename);
saveDepthPointsToPly(point_cloud_msg_, filename);
}
}
@@ -733,7 +733,7 @@ void OBCameraNode::publishColoredPointCloud(const std::shared_ptr<ob::FrameSet>
std::filesystem::create_directory(current_path + "/point_cloud");
}
RCLCPP_INFO_STREAM(logger_, "Saving point cloud to " << filename);
soavePointCloudMsgToPly(point_cloud_msg_, filename);
saveRGBPointCloudMsgToPly(point_cloud_msg_, filename);
}
}
orbbec_camera/src/utils.cpp
+41 -2
View File
@@ -87,8 +87,8 @@ void saveRGBPointsToPly(const std::shared_ptr<ob::Frame> &frame, const std::stri
}
}
void soavePointCloudMsgToPly(const sensor_msgs::msg::PointCloud2 &msg,
const std::string &fileName) {
void saveRGBPointCloudMsgToPly(const sensor_msgs::msg::PointCloud2 &msg,
const std::string &fileName) {
FILE *fp = fopen(fileName.c_str(), "wb+");
CHECK_NOTNULL(fp);
@@ -134,6 +134,45 @@ void soavePointCloudMsgToPly(const sensor_msgs::msg::PointCloud2 &msg,
fclose(fp);
}
void saveDepthPointsToPly(const sensor_msgs::msg::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(const 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+");