From cf8e79559f94811c56d655c7336a5e65bd732646 Mon Sep 17 00:00:00 2001 From: gaschler Date: Tue, 24 Oct 2017 13:39:54 +0200 Subject: [PATCH] Follow TimedPointCloud. (#549) Follows googlecartographer/cartographer#601. (#573) --- .../cartographer_ros/assets_writer_main.cc | 6 ++-- .../cartographer_ros/msg_conversion.cc | 15 ++++----- .../cartographer_ros/msg_conversion.h | 2 +- .../cartographer_ros/msg_conversion_test.cc | 33 ++++++++++++------- cartographer_ros/cartographer_ros/node.cc | 14 ++++++-- .../cartographer_ros/sensor_bridge.cc | 20 +++++------ .../cartographer_ros/sensor_bridge.h | 2 +- 7 files changed, 54 insertions(+), 38 deletions(-) diff --git a/cartographer_ros/cartographer_ros/assets_writer_main.cc b/cartographer_ros/cartographer_ros/assets_writer_main.cc index 180780b..397c31a 100644 --- a/cartographer_ros/cartographer_ros/assets_writer_main.cc +++ b/cartographer_ros/cartographer_ros/assets_writer_main.cc @@ -90,11 +90,10 @@ std::unique_ptr HandleMessage( carto::sensor::PointCloudWithIntensities point_cloud = ToPointCloudWithIntensities(message); CHECK_EQ(point_cloud.intensities.size(), point_cloud.points.size()); - CHECK_EQ(point_cloud.offset_seconds.size(), point_cloud.points.size()); for (size_t i = 0; i < point_cloud.points.size(); ++i) { const carto::common::Time time = - start_time + carto::common::FromSeconds(point_cloud.offset_seconds[i]); + start_time + carto::common::FromSeconds(point_cloud.points[i][3]); if (!transform_interpolation_buffer.Has(time)) { continue; } @@ -105,7 +104,8 @@ std::unique_ptr HandleMessage( tracking_frame, message.header.frame_id, ToRos(time))); const carto::transform::Rigid3f sensor_to_map = (tracking_to_map * sensor_to_tracking).cast(); - points_batch->points.push_back(sensor_to_map * point_cloud.points[i]); + points_batch->points.push_back(sensor_to_map * + point_cloud.points[i].head<3>()); points_batch->intensities.push_back(point_cloud.intensities[i]); // We use the last transform for the origin, which is approximately correct. points_batch->origin = sensor_to_map * Eigen::Vector3f::Zero(); diff --git a/cartographer_ros/cartographer_ros/msg_conversion.cc b/cartographer_ros/cartographer_ros/msg_conversion.cc index 920bf28..4cd2abf 100644 --- a/cartographer_ros/cartographer_ros/msg_conversion.cc +++ b/cartographer_ros/cartographer_ros/msg_conversion.cc @@ -108,9 +108,10 @@ PointCloudWithIntensities LaserScanToPointCloudWithIntensities( const float first_echo = GetFirstEcho(echoes); if (msg.range_min <= first_echo && first_echo <= msg.range_max) { const Eigen::AngleAxisf rotation(angle, Eigen::Vector3f::UnitZ()); - point_cloud.points.push_back(rotation * - (first_echo * Eigen::Vector3f::UnitX())); - point_cloud.offset_seconds.push_back(i * msg.time_increment); + Eigen::Vector4f point; + point << rotation * (first_echo * Eigen::Vector3f::UnitX()), + i * msg.time_increment; + point_cloud.points.push_back(point); if (msg.intensities.size() > 0) { CHECK_EQ(msg.intensities.size(), msg.ranges.size()); const auto& echo_intensities = msg.intensities[i]; @@ -140,7 +141,7 @@ bool PointCloud2HasField(const sensor_msgs::PointCloud2& pc2, sensor_msgs::PointCloud2 ToPointCloud2Message( const int64 timestamp, const string& frame_id, - const ::cartographer::sensor::PointCloud& point_cloud) { + const ::cartographer::sensor::TimedPointCloud& point_cloud) { auto msg = PreparePointCloud2Message(timestamp, frame_id, point_cloud.size()); ::ros::serialization::OStream stream(msg.data.data(), msg.data.size()); for (const auto& point : point_cloud) { @@ -171,9 +172,8 @@ PointCloudWithIntensities ToPointCloudWithIntensities( pcl::PointCloud 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.points.emplace_back(point.x, point.y, point.z, 0.f); point_cloud.intensities.push_back(point.intensity); - point_cloud.offset_seconds.push_back(0.f); } } else { pcl::PointCloud pcl_point_cloud; @@ -182,9 +182,8 @@ PointCloudWithIntensities ToPointCloudWithIntensities( // 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.points.emplace_back(point.x, point.y, point.z, 0.f); point_cloud.intensities.push_back(1.0); - point_cloud.offset_seconds.push_back(0.f); } } return point_cloud; diff --git a/cartographer_ros/cartographer_ros/msg_conversion.h b/cartographer_ros/cartographer_ros/msg_conversion.h index d175815..eea4e21 100644 --- a/cartographer_ros/cartographer_ros/msg_conversion.h +++ b/cartographer_ros/cartographer_ros/msg_conversion.h @@ -35,7 +35,7 @@ namespace cartographer_ros { sensor_msgs::PointCloud2 ToPointCloud2Message( int64 timestamp, const string& frame_id, - const ::cartographer::sensor::PointCloud& point_cloud); + const ::cartographer::sensor::TimedPointCloud& point_cloud); geometry_msgs::Transform ToGeometryMsgTransform( const ::cartographer::transform::Rigid3d& rigid3d); diff --git a/cartographer_ros/cartographer_ros/msg_conversion_test.cc b/cartographer_ros/cartographer_ros/msg_conversion_test.cc index 92b63a0..5239b07 100644 --- a/cartographer_ros/cartographer_ros/msg_conversion_test.cc +++ b/cartographer_ros/cartographer_ros/msg_conversion_test.cc @@ -36,19 +36,26 @@ TEST(MsgConversion, LaserScanToPointCloud) { laser_scan.range_max = 10.f; const auto point_cloud = ToPointCloudWithIntensities(laser_scan).points; - EXPECT_TRUE(point_cloud[0].isApprox(Eigen::Vector3f(1.f, 0.f, 0.f), 1e-6)); + EXPECT_TRUE( + point_cloud[0].isApprox(Eigen::Vector4f(1.f, 0.f, 0.f, 0.f), 1e-6)); EXPECT_TRUE(point_cloud[1].isApprox( - Eigen::Vector3f(1.f / std::sqrt(2.f), 1.f / std::sqrt(2.f), 0.f), 1e-6)); - EXPECT_TRUE(point_cloud[2].isApprox(Eigen::Vector3f(0.f, 1.f, 0.f), 1e-6)); - EXPECT_TRUE(point_cloud[3].isApprox( - Eigen::Vector3f(-1.f / std::sqrt(2.f), 1.f / std::sqrt(2.f), 0.f), 1e-6)); - EXPECT_TRUE(point_cloud[4].isApprox(Eigen::Vector3f(-1.f, 0.f, 0.f), 1e-6)); - EXPECT_TRUE(point_cloud[5].isApprox( - Eigen::Vector3f(-1.f / std::sqrt(2.f), -1.f / std::sqrt(2.f), 0.f), + Eigen::Vector4f(1.f / std::sqrt(2.f), 1.f / std::sqrt(2.f), 0.f, 0.f), 1e-6)); - EXPECT_TRUE(point_cloud[6].isApprox(Eigen::Vector3f(0.f, -1.f, 0.f), 1e-6)); + EXPECT_TRUE( + point_cloud[2].isApprox(Eigen::Vector4f(0.f, 1.f, 0.f, 0.f), 1e-6)); + EXPECT_TRUE(point_cloud[3].isApprox( + Eigen::Vector4f(-1.f / std::sqrt(2.f), 1.f / std::sqrt(2.f), 0.f, 0.f), + 1e-6)); + EXPECT_TRUE( + point_cloud[4].isApprox(Eigen::Vector4f(-1.f, 0.f, 0.f, 0.f), 1e-6)); + EXPECT_TRUE(point_cloud[5].isApprox( + Eigen::Vector4f(-1.f / std::sqrt(2.f), -1.f / std::sqrt(2.f), 0.f, 0.f), + 1e-6)); + EXPECT_TRUE( + point_cloud[6].isApprox(Eigen::Vector4f(0.f, -1.f, 0.f, 0.f), 1e-6)); EXPECT_TRUE(point_cloud[7].isApprox( - Eigen::Vector3f(1.f / std::sqrt(2.f), -1.f / std::sqrt(2.f), 0.f), 1e-6)); + Eigen::Vector4f(1.f / std::sqrt(2.f), -1.f / std::sqrt(2.f), 0.f, 0.f), + 1e-6)); } TEST(MsgConversion, LaserScanToPointCloudWithInfinityAndNaN) { @@ -66,8 +73,10 @@ TEST(MsgConversion, LaserScanToPointCloudWithInfinityAndNaN) { const auto point_cloud = ToPointCloudWithIntensities(laser_scan).points; ASSERT_EQ(2, point_cloud.size()); - EXPECT_TRUE(point_cloud[0].isApprox(Eigen::Vector3f(0.f, 2.f, 0.f), 1e-6)); - EXPECT_TRUE(point_cloud[1].isApprox(Eigen::Vector3f(-3.f, 0.f, 0.f), 1e-6)); + EXPECT_TRUE( + point_cloud[0].isApprox(Eigen::Vector4f(0.f, 2.f, 0.f, 0.f), 1e-6)); + EXPECT_TRUE( + point_cloud[1].isApprox(Eigen::Vector4f(-3.f, 0.f, 0.f, 0.f), 1e-6)); } } // namespace diff --git a/cartographer_ros/cartographer_ros/node.cc b/cartographer_ros/cartographer_ros/node.cc index ad9388d..55f0ed3 100644 --- a/cartographer_ros/cartographer_ros/node.cc +++ b/cartographer_ros/cartographer_ros/node.cc @@ -171,12 +171,20 @@ void Node::PublishTrajectoryStates(const ::ros::WallTimerEvent& timer_event) { // We only publish a point cloud if it has changed. It is not needed at high // frequency, and republishing it would be computationally wasteful. if (trajectory_state.pose_estimate.time != extrapolator.GetLastPoseTime()) { + // TODO(gaschler): Consider using other message without time information. + carto::sensor::TimedPointCloud point_cloud; + point_cloud.reserve(trajectory_state.pose_estimate.point_cloud.size()); + for (const Eigen::Vector3f point : + trajectory_state.pose_estimate.point_cloud) { + Eigen::Vector4f point_time; + point_time << point, 0.f; + point_cloud.push_back(point_time); + } scan_matched_point_cloud_publisher_.publish(ToPointCloud2Message( carto::common::ToUniversal(trajectory_state.pose_estimate.time), node_options_.map_frame, - carto::sensor::TransformPointCloud( - trajectory_state.pose_estimate.point_cloud, - trajectory_state.local_to_map.cast()))); + carto::sensor::TransformTimedPointCloud( + point_cloud, trajectory_state.local_to_map.cast()))); extrapolator.AddPose(trajectory_state.pose_estimate.time, trajectory_state.pose_estimate.pose); } diff --git a/cartographer_ros/cartographer_ros/sensor_bridge.cc b/cartographer_ros/cartographer_ros/sensor_bridge.cc index 1f2abdb..809d961 100644 --- a/cartographer_ros/cartographer_ros/sensor_bridge.cc +++ b/cartographer_ros/cartographer_ros/sensor_bridge.cc @@ -128,9 +128,9 @@ void SensorBridge::HandlePointCloud2Message( const string& sensor_id, const sensor_msgs::PointCloud2::ConstPtr& msg) { pcl::PointCloud pcl_point_cloud; pcl::fromROSMsg(*msg, pcl_point_cloud); - carto::sensor::PointCloud point_cloud; + carto::sensor::TimedPointCloud point_cloud; for (const auto& point : pcl_point_cloud) { - point_cloud.emplace_back(point.x, point.y, point.z); + point_cloud.emplace_back(point.x, point.y, point.z, 0.f); } HandleRangefinder(sensor_id, FromRos(msg->header.stamp), msg->header.frame_id, point_cloud); @@ -142,12 +142,13 @@ void SensorBridge::HandleLaserScan( const string& sensor_id, const carto::common::Time start_time, const string& frame_id, const carto::sensor::PointCloudWithIntensities& points) { + // TODO(gaschler): Use per-point time instead of subdivisions. for (int i = 0; i != num_subdivisions_per_laser_scan_; ++i) { const size_t start_index = points.points.size() * i / num_subdivisions_per_laser_scan_; const size_t end_index = points.points.size() * (i + 1) / num_subdivisions_per_laser_scan_; - const carto::sensor::PointCloud subdivision( + const carto::sensor::TimedPointCloud subdivision( points.points.begin() + start_index, points.points.begin() + end_index); if (start_index == end_index) { continue; @@ -155,22 +156,21 @@ void SensorBridge::HandleLaserScan( const size_t middle_index = (start_index + end_index) / 2; const carto::common::Time subdivision_time = start_time + - carto::common::FromSeconds(points.offset_seconds.at(middle_index)); + carto::common::FromSeconds(points.points.at(middle_index)[3]); HandleRangefinder(sensor_id, subdivision_time, frame_id, subdivision); } } -void SensorBridge::HandleRangefinder(const string& sensor_id, - const carto::common::Time time, - const string& frame_id, - const carto::sensor::PointCloud& ranges) { +void SensorBridge::HandleRangefinder( + const string& sensor_id, const carto::common::Time time, + const string& frame_id, const carto::sensor::TimedPointCloud& ranges) { const auto sensor_to_tracking = tf_bridge_.LookupToTracking(time, CheckNoLeadingSlash(frame_id)); if (sensor_to_tracking != nullptr) { trajectory_builder_->AddRangefinderData( sensor_id, time, sensor_to_tracking->translation().cast(), - carto::sensor::TransformPointCloud(ranges, - sensor_to_tracking->cast())); + carto::sensor::TransformTimedPointCloud( + ranges, sensor_to_tracking->cast())); } } diff --git a/cartographer_ros/cartographer_ros/sensor_bridge.h b/cartographer_ros/cartographer_ros/sensor_bridge.h index 24cc758..f9d8d31 100644 --- a/cartographer_ros/cartographer_ros/sensor_bridge.h +++ b/cartographer_ros/cartographer_ros/sensor_bridge.h @@ -73,7 +73,7 @@ class SensorBridge { void HandleRangefinder(const string& sensor_id, ::cartographer::common::Time time, const string& frame_id, - const ::cartographer::sensor::PointCloud& ranges); + const ::cartographer::sensor::TimedPointCloud& ranges); const int num_subdivisions_per_laser_scan_; const TfBridge tf_bridge_;