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

Follow TimedPointCloud. (#549) 

Follows googlecartographer/cartographer#601. (#573)
master
gaschler 2017-10-24 13:39:54 +02:00 committed by GitHub
parent 137c75633f
commit cf8e79559f
7 changed files with 54 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
cartographer_ros/cartographer_ros/assets_writer_main.cc
View File

@ -90,11 +90,10 @@ std::unique_ptr<carto::io::PointsBatch> HandleMessage(
carto::sensor::PointCloudWithIntensities point_cloud = carto::sensor::PointCloudWithIntensities point_cloud =
ToPointCloudWithIntensities(message); ToPointCloudWithIntensities(message);
CHECK_EQ(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) { for (size_t i = 0; i < point_cloud.points.size(); ++i) {
const carto::common::Time time = 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)) { if (!transform_interpolation_buffer.Has(time)) {
continue; continue;
} }
@ -105,7 +104,8 @@ std::unique_ptr<carto::io::PointsBatch> HandleMessage(
tracking_frame, message.header.frame_id, ToRos(time))); tracking_frame, message.header.frame_id, ToRos(time)));
const carto::transform::Rigid3f sensor_to_map = const carto::transform::Rigid3f sensor_to_map =
(tracking_to_map * sensor_to_tracking).cast<float>(); (tracking_to_map * sensor_to_tracking).cast<float>();
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]); points_batch->intensities.push_back(point_cloud.intensities[i]);
// We use the last transform for the origin, which is approximately correct. // We use the last transform for the origin, which is approximately correct.
points_batch->origin = sensor_to_map * Eigen::Vector3f::Zero(); points_batch->origin = sensor_to_map * Eigen::Vector3f::Zero();

15
cartographer_ros/cartographer_ros/msg_conversion.cc
View File

@ -108,9 +108,10 @@ PointCloudWithIntensities LaserScanToPointCloudWithIntensities(
const float first_echo = GetFirstEcho(echoes); const float first_echo = GetFirstEcho(echoes);
if (msg.range_min <= first_echo && first_echo <= msg.range_max) { if (msg.range_min <= first_echo && first_echo <= msg.range_max) {
const Eigen::AngleAxisf rotation(angle, Eigen::Vector3f::UnitZ()); const Eigen::AngleAxisf rotation(angle, Eigen::Vector3f::UnitZ());
point_cloud.points.push_back(rotation * Eigen::Vector4f point;
(first_echo * Eigen::Vector3f::UnitX())); point << rotation * (first_echo * Eigen::Vector3f::UnitX()),
point_cloud.offset_seconds.push_back(i * msg.time_increment); i * msg.time_increment;
point_cloud.points.push_back(point);
if (msg.intensities.size() > 0) { if (msg.intensities.size() > 0) {
CHECK_EQ(msg.intensities.size(), msg.ranges.size()); CHECK_EQ(msg.intensities.size(), msg.ranges.size());
const auto& echo_intensities = msg.intensities[i]; const auto& echo_intensities = msg.intensities[i];
@ -140,7 +141,7 @@ bool PointCloud2HasField(const sensor_msgs::PointCloud2& pc2,
sensor_msgs::PointCloud2 ToPointCloud2Message( sensor_msgs::PointCloud2 ToPointCloud2Message(
const int64 timestamp, const string& frame_id, 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()); auto msg = PreparePointCloud2Message(timestamp, frame_id, point_cloud.size());
::ros::serialization::OStream stream(msg.data.data(), msg.data.size()); ::ros::serialization::OStream stream(msg.data.data(), msg.data.size());
for (const auto& point : point_cloud) { for (const auto& point : point_cloud) {
@ -171,9 +172,8 @@ PointCloudWithIntensities ToPointCloudWithIntensities(
pcl::PointCloud<pcl::PointXYZI> pcl_point_cloud; pcl::PointCloud<pcl::PointXYZI> pcl_point_cloud;
pcl::fromROSMsg(message, pcl_point_cloud); pcl::fromROSMsg(message, pcl_point_cloud);
for (const auto& point : 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.intensities.push_back(point.intensity);
point_cloud.offset_seconds.push_back(0.f);
} }
} else { } else {
pcl::PointCloud<pcl::PointXYZ> pcl_point_cloud; pcl::PointCloud<pcl::PointXYZ> pcl_point_cloud;
@ -182,9 +182,8 @@ PointCloudWithIntensities ToPointCloudWithIntensities(
// If we don't have an intensity field, just copy XYZ and fill in // If we don't have an intensity field, just copy XYZ and fill in
// 1.0. // 1.0.
for (const auto& point : 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(1.0); point_cloud.intensities.push_back(1.0);
point_cloud.offset_seconds.push_back(0.f);
} }
} }
return point_cloud; return point_cloud;

2
cartographer_ros/cartographer_ros/msg_conversion.h
View File

@ -35,7 +35,7 @@ namespace cartographer_ros {
sensor_msgs::PointCloud2 ToPointCloud2Message( sensor_msgs::PointCloud2 ToPointCloud2Message(
int64 timestamp, const string& frame_id, int64 timestamp, const string& frame_id,
const ::cartographer::sensor::PointCloud& point_cloud); const ::cartographer::sensor::TimedPointCloud& point_cloud);
geometry_msgs::Transform ToGeometryMsgTransform( geometry_msgs::Transform ToGeometryMsgTransform(
const ::cartographer::transform::Rigid3d& rigid3d); const ::cartographer::transform::Rigid3d& rigid3d);

33
cartographer_ros/cartographer_ros/msg_conversion_test.cc
View File

@ -36,19 +36,26 @@ TEST(MsgConversion, LaserScanToPointCloud) {
laser_scan.range_max = 10.f; laser_scan.range_max = 10.f;
const auto point_cloud = ToPointCloudWithIntensities(laser_scan).points; 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( EXPECT_TRUE(point_cloud[1].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),
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),
1e-6)); 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( 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) { TEST(MsgConversion, LaserScanToPointCloudWithInfinityAndNaN) {
@ -66,8 +73,10 @@ TEST(MsgConversion, LaserScanToPointCloudWithInfinityAndNaN) {
const auto point_cloud = ToPointCloudWithIntensities(laser_scan).points; const auto point_cloud = ToPointCloudWithIntensities(laser_scan).points;
ASSERT_EQ(2, point_cloud.size()); ASSERT_EQ(2, point_cloud.size());
EXPECT_TRUE(point_cloud[0].isApprox(Eigen::Vector3f(0.f, 2.f, 0.f), 1e-6)); EXPECT_TRUE(
EXPECT_TRUE(point_cloud[1].isApprox(Eigen::Vector3f(-3.f, 0.f, 0.f), 1e-6)); 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 } // namespace

14
cartographer_ros/cartographer_ros/node.cc
View File

@ -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 // We only publish a point cloud if it has changed. It is not needed at high
// frequency, and republishing it would be computationally wasteful. // frequency, and republishing it would be computationally wasteful.
if (trajectory_state.pose_estimate.time != extrapolator.GetLastPoseTime()) { 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( scan_matched_point_cloud_publisher_.publish(ToPointCloud2Message(
carto::common::ToUniversal(trajectory_state.pose_estimate.time), carto::common::ToUniversal(trajectory_state.pose_estimate.time),
node_options_.map_frame, node_options_.map_frame,
carto::sensor::TransformPointCloud( carto::sensor::TransformTimedPointCloud(
trajectory_state.pose_estimate.point_cloud, point_cloud, trajectory_state.local_to_map.cast<float>())));
trajectory_state.local_to_map.cast<float>())));
extrapolator.AddPose(trajectory_state.pose_estimate.time, extrapolator.AddPose(trajectory_state.pose_estimate.time,
trajectory_state.pose_estimate.pose); trajectory_state.pose_estimate.pose);
} }

20
cartographer_ros/cartographer_ros/sensor_bridge.cc
View File

@ -128,9 +128,9 @@ void SensorBridge::HandlePointCloud2Message(
const string& sensor_id, const sensor_msgs::PointCloud2::ConstPtr& msg) { const string& sensor_id, const sensor_msgs::PointCloud2::ConstPtr& msg) {
pcl::PointCloud<pcl::PointXYZ> pcl_point_cloud; pcl::PointCloud<pcl::PointXYZ> pcl_point_cloud;
pcl::fromROSMsg(*msg, 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) { 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, HandleRangefinder(sensor_id, FromRos(msg->header.stamp), msg->header.frame_id,
point_cloud); point_cloud);
@ -142,12 +142,13 @@ void SensorBridge::HandleLaserScan(
const string& sensor_id, const carto::common::Time start_time, const string& sensor_id, const carto::common::Time start_time,
const string& frame_id, const string& frame_id,
const carto::sensor::PointCloudWithIntensities& points) { 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) { for (int i = 0; i != num_subdivisions_per_laser_scan_; ++i) {
const size_t start_index = const size_t start_index =
points.points.size() * i / num_subdivisions_per_laser_scan_; points.points.size() * i / num_subdivisions_per_laser_scan_;
const size_t end_index = const size_t end_index =
points.points.size() * (i + 1) / num_subdivisions_per_laser_scan_; 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); points.points.begin() + start_index, points.points.begin() + end_index);
if (start_index == end_index) { if (start_index == end_index) {
continue; continue;
@ -155,22 +156,21 @@ void SensorBridge::HandleLaserScan(
const size_t middle_index = (start_index + end_index) / 2; const size_t middle_index = (start_index + end_index) / 2;
const carto::common::Time subdivision_time = const carto::common::Time subdivision_time =
start_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); HandleRangefinder(sensor_id, subdivision_time, frame_id, subdivision);
} }
} }
void SensorBridge::HandleRangefinder(const string& sensor_id, void SensorBridge::HandleRangefinder(
const carto::common::Time time, const string& sensor_id, const carto::common::Time time,
const string& frame_id, const string& frame_id, const carto::sensor::TimedPointCloud& ranges) {
const carto::sensor::PointCloud& ranges) {
const auto sensor_to_tracking = const auto sensor_to_tracking =
tf_bridge_.LookupToTracking(time, CheckNoLeadingSlash(frame_id)); tf_bridge_.LookupToTracking(time, CheckNoLeadingSlash(frame_id));
if (sensor_to_tracking != nullptr) { if (sensor_to_tracking != nullptr) {
trajectory_builder_->AddRangefinderData( trajectory_builder_->AddRangefinderData(
sensor_id, time, sensor_to_tracking->translation().cast<float>(), sensor_id, time, sensor_to_tracking->translation().cast<float>(),
carto::sensor::TransformPointCloud(ranges, carto::sensor::TransformTimedPointCloud(
sensor_to_tracking->cast<float>())); ranges, sensor_to_tracking->cast<float>()));
} }
} }

2
cartographer_ros/cartographer_ros/sensor_bridge.h
View File

@ -73,7 +73,7 @@ class SensorBridge {
void HandleRangefinder(const string& sensor_id, void HandleRangefinder(const string& sensor_id,
::cartographer::common::Time time, ::cartographer::common::Time time,
const string& frame_id, const string& frame_id,
const ::cartographer::sensor::PointCloud& ranges); const ::cartographer::sensor::TimedPointCloud& ranges);
const int num_subdivisions_per_laser_scan_; const int num_subdivisions_per_laser_scan_;
const TfBridge tf_bridge_; const TfBridge tf_bridge_;