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

Unwarp each (MultiEcho)LaserScan point in the assets_writer_main. (#526) 

Since these contain timing information, we can use the SLAM trajectory
to compensate for ego motion (unwarp) and the TF messages and URDF to
compensate for sensor-to-tracking motion.

This improves quality greatly.
master
Holger Rapp 2017-10-06 14:23:18 +02:00 committed by Wolfgang Hess
parent e79754bf71
commit d5e6647206
4 changed files with 43 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
cartographer_ros/cartographer_ros/assets_writer_main.cc
View File

@ -76,30 +76,37 @@ std::unique_ptr<carto::io::PointsBatch> 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<float>();
const carto::common::Time start_time = FromRos(message.header.stamp);
auto points_batch = carto::common::make_unique<carto::io::PointsBatch>();
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<float>();
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();

3
cartographer_ros/cartographer_ros/msg_conversion.cc
View File

@ -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::PointXYZ> 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;

28
cartographer_ros/cartographer_ros/sensor_bridge.cc
View File

@ -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);
}
}

9
cartographer_ros/cartographer_ros/sensor_bridge.h
View File

@ -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,