12345qiupeng
/
cartographer_ros
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Added reading intensity values from point clouds (#321)

master
Jeremie Papon 2017-05-09 01:26:05 -07:00 committed by Holger Rapp
parent 5294050f51
commit f6fc7ac6b2
1 changed files with 28 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
cartographer_ros/cartographer_ros/msg_conversion.cc
View File

@ -126,6 +126,16 @@ PointCloudWithIntensities LaserScanToPointCloudWithIntensities(
return point_cloud;
}
bool PointCloud2HasField(const sensor_msgs::PointCloud2& pc2,
const std::string& field_name) {
for (const auto& field : pc2.fields) {
if (field.name == field_name) {
return true;
}
}
return false;
}
} // namespace
sensor_msgs::PointCloud2 ToPointCloud2Message(
@ -154,14 +164,26 @@ PointCloudWithIntensities ToPointCloudWithIntensities(
PointCloudWithIntensities ToPointCloudWithIntensities(
const sensor_msgs::PointCloud2& message) {
pcl::PointCloud<pcl::PointXYZ> pcl_point_cloud;
pcl::fromROSMsg(message, pcl_point_cloud);
PointCloudWithIntensities point_cloud;
// We check for intensity field here to avoid run-time warnings if we pass in
// a PointCloud2 without intensity.
if (PointCloud2HasField(message, "intensity")) {
pcl::PointCloud<pcl::PointXYZI> pcl_point_cloud;
pcl::fromROSMsg(message, pcl_point_cloud);
for (const auto& point : pcl_point_cloud) {
point_cloud.points.emplace_back(point.x, point.y, point.z);
point_cloud.intensities.push_back(point.intensity);
}
} else {
pcl::PointCloud<pcl::PointXYZ> pcl_point_cloud;
pcl::fromROSMsg(message, pcl_point_cloud);
// TODO(hrapp): How to get reflectivities from PCL?
for (const auto& point : pcl_point_cloud) {
point_cloud.points.emplace_back(point.x, point.y, point.z);
point_cloud.intensities.push_back(1.);
// If we don't have an intensity field, just copy XYZ and fill in
// 1.0.
for (const auto& point : pcl_point_cloud) {
point_cloud.points.emplace_back(point.x, point.y, point.z);
point_cloud.intensities.push_back(1.0);
}
}
return point_cloud;
}