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

remove point cloud filter

Browse Source
This commit is contained in:
Joe Dong
2023-09-07 16:09:35 +08:00
parent 436bda54e2
commit a5535ee1f0
5 changed files with 221 additions and 98 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/src/utils.cpp
+48
View File
@@ -12,6 +12,7 @@
#include <regex>
#include "orbbec_camera/utils.h"
#include <sensor_msgs/point_cloud2_iterator.hpp>
namespace orbbec_camera {
sensor_msgs::msg::CameraInfo convertToCameraInfo(OBCameraIntrinsic intrinsic,
OBCameraDistortion distortion, int width) {
@@ -82,6 +83,53 @@ void saveRGBPointsToPly(const std::shared_ptr<ob::Frame> &frame, const std::stri
}
}
void soavePointCloudMsgToPly(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");
sensor_msgs::PointCloud2ConstIterator<uint8_t> iter_r(msg, "r");
sensor_msgs::PointCloud2ConstIterator<uint8_t> iter_g(msg, "g");
sensor_msgs::PointCloud2ConstIterator<uint8_t> iter_b(msg, "b");
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, "property uchar red\n");
fprintf(fp, "property uchar green\n");
fprintf(fp, "property uchar blue\n");
fprintf(fp, "end_header\n");
for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z, ++iter_r, ++iter_g, ++iter_b) {
if (!std::isnan(*iter_x) && !std::isnan(*iter_y) && !std::isnan(*iter_z)) {
fprintf(fp, "%.3f %.3f %.3f %d %d %d\n", *iter_x, *iter_y, *iter_z, (int)*iter_r,
(int)*iter_g, (int)*iter_b);
}
}
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+");