diff --git a/cartographer_ros/cartographer_ros/assets_writer_main.cc b/cartographer_ros/cartographer_ros/assets_writer_main.cc index 28798dc..91bf114 100644 --- a/cartographer_ros/cartographer_ros/assets_writer_main.cc +++ b/cartographer_ros/cartographer_ros/assets_writer_main.cc @@ -76,30 +76,37 @@ std::unique_ptr HandleMessage( const tf2_ros::Buffer& tf_buffer, const carto::transform::TransformInterpolationBuffer& transform_interpolation_buffer) { - const carto::common::Time time = FromRos(message.header.stamp); - if (!transform_interpolation_buffer.Has(time)) { - return nullptr; - } - const carto::transform::Rigid3d tracking_to_map = - transform_interpolation_buffer.Lookup(time); - const carto::transform::Rigid3d sensor_to_tracking = - ToRigid3d(tf_buffer.lookupTransform( - tracking_frame, message.header.frame_id, message.header.stamp)); - const carto::transform::Rigid3f sensor_to_map = - (tracking_to_map * sensor_to_tracking).cast(); + const carto::common::Time start_time = FromRos(message.header.stamp); auto points_batch = carto::common::make_unique(); - points_batch->time = time; - points_batch->origin = sensor_to_map * Eigen::Vector3f::Zero(); + points_batch->start_time = start_time; points_batch->frame_id = message.header.frame_id; carto::sensor::PointCloudWithIntensities point_cloud = ToPointCloudWithIntensities(message); - CHECK(point_cloud.intensities.size() == point_cloud.points.size()); + 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]); + if (!transform_interpolation_buffer.Has(time)) { + continue; + } + const carto::transform::Rigid3d tracking_to_map = + transform_interpolation_buffer.Lookup(time); + const carto::transform::Rigid3d sensor_to_tracking = + ToRigid3d(tf_buffer.lookupTransform( + 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->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(); + } + if (points_batch->points.empty()) { + return nullptr; } return points_batch; } @@ -189,9 +196,8 @@ void Run(const string& pose_graph_filename, } } - while (!delayed_messages.empty() && - delayed_messages.front().getTime() < - message.getTime() - kDelay) { + while (!delayed_messages.empty() && delayed_messages.front().getTime() < + message.getTime() - kDelay) { const rosbag::MessageInstance& delayed_message = delayed_messages.front(); diff --git a/cartographer_ros/cartographer_ros/msg_conversion.cc b/cartographer_ros/cartographer_ros/msg_conversion.cc index d540d8b..920bf28 100644 --- a/cartographer_ros/cartographer_ros/msg_conversion.cc +++ b/cartographer_ros/cartographer_ros/msg_conversion.cc @@ -110,6 +110,7 @@ PointCloudWithIntensities LaserScanToPointCloudWithIntensities( 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); if (msg.intensities.size() > 0) { CHECK_EQ(msg.intensities.size(), msg.ranges.size()); const auto& echo_intensities = msg.intensities[i]; @@ -172,6 +173,7 @@ PointCloudWithIntensities ToPointCloudWithIntensities( 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); + point_cloud.offset_seconds.push_back(0.f); } } else { pcl::PointCloud pcl_point_cloud; @@ -182,6 +184,7 @@ PointCloudWithIntensities ToPointCloudWithIntensities( 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); + point_cloud.offset_seconds.push_back(0.f); } } return point_cloud; diff --git a/cartographer_ros/cartographer_ros/sensor_bridge.cc b/cartographer_ros/cartographer_ros/sensor_bridge.cc index e656888..a3b2eab 100644 --- a/cartographer_ros/cartographer_ros/sensor_bridge.cc +++ b/cartographer_ros/cartographer_ros/sensor_bridge.cc @@ -114,16 +114,14 @@ void SensorBridge::HandleImuMessage(const string& sensor_id, void SensorBridge::HandleLaserScanMessage( const string& sensor_id, const sensor_msgs::LaserScan::ConstPtr& msg) { HandleLaserScan(sensor_id, FromRos(msg->header.stamp), msg->header.frame_id, - ToPointCloudWithIntensities(*msg).points, - msg->time_increment); + ToPointCloudWithIntensities(*msg)); } void SensorBridge::HandleMultiEchoLaserScanMessage( const string& sensor_id, const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) { HandleLaserScan(sensor_id, FromRos(msg->header.stamp), msg->header.frame_id, - ToPointCloudWithIntensities(*msg).points, - msg->time_increment); + ToPointCloudWithIntensities(*msg)); } void SensorBridge::HandlePointCloud2Message( @@ -140,21 +138,21 @@ void SensorBridge::HandlePointCloud2Message( const TfBridge& SensorBridge::tf_bridge() const { return tf_bridge_; } -void SensorBridge::HandleLaserScan(const string& sensor_id, - const carto::common::Time start_time, - const string& frame_id, - const carto::sensor::PointCloud& points, - const double seconds_between_points) { +void SensorBridge::HandleLaserScan( + const string& sensor_id, const carto::common::Time start_time, + const string& frame_id, + const carto::sensor::PointCloudWithIntensities& points) { for (int i = 0; i != num_subdivisions_per_laser_scan_; ++i) { const size_t start_index = - points.size() * i / num_subdivisions_per_laser_scan_; + points.points.size() * i / num_subdivisions_per_laser_scan_; const size_t end_index = - points.size() * (i + 1) / num_subdivisions_per_laser_scan_; - const carto::sensor::PointCloud subdivision(points.begin() + start_index, - points.begin() + end_index); + points.points.size() * (i + 1) / num_subdivisions_per_laser_scan_; + const carto::sensor::PointCloud subdivision( + points.points.begin() + start_index, points.points.begin() + end_index); + const size_t middle_index = (start_index + end_index) / 2; const carto::common::Time subdivision_time = - start_time + carto::common::FromSeconds((start_index + end_index) / 2. * - seconds_between_points); + start_time + + carto::common::FromSeconds(points.offset_seconds.at(middle_index)); HandleRangefinder(sensor_id, subdivision_time, frame_id, subdivision); } } diff --git a/cartographer_ros/cartographer_ros/sensor_bridge.h b/cartographer_ros/cartographer_ros/sensor_bridge.h index 1f4c7c7..24cc758 100644 --- a/cartographer_ros/cartographer_ros/sensor_bridge.h +++ b/cartographer_ros/cartographer_ros/sensor_bridge.h @@ -66,11 +66,10 @@ class SensorBridge { const TfBridge& tf_bridge() const; private: - void HandleLaserScan(const string& sensor_id, - ::cartographer::common::Time start_time, - const string& frame_id, - const ::cartographer::sensor::PointCloud& points, - double seconds_between_points); + void HandleLaserScan( + const string& sensor_id, ::cartographer::common::Time start_time, + const string& frame_id, + const ::cartographer::sensor::PointCloudWithIntensities& points); void HandleRangefinder(const string& sensor_id, ::cartographer::common::Time time, const string& frame_id,