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

Follow cartographer#1357 (#964)

master
Juraj Oršulić 2018-08-13 13:38:23 +02:00 committed by Alexander Belyaev
parent 55e83c39a5
commit c12da5e8b3
7 changed files with 68 additions and 56 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
cartographer_ros/cartographer_ros/assets_writer.cc
View File

@ -110,7 +110,8 @@ std::unique_ptr<carto::io::PointsBatch> HandleMessage(
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 =
point_cloud_time + carto::common::FromSeconds(point_cloud.points[i][3]); point_cloud_time +
carto::common::FromSeconds(point_cloud.points[i].time);
if (!transform_interpolation_buffer.Has(time)) { if (!transform_interpolation_buffer.Has(time)) {
continue; continue;
} }
@ -121,8 +122,9 @@ 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 * points_batch->points.push_back(
point_cloud.points[i].head<3>()); sensor_to_map *
carto::sensor::ToRangefinderPoint(point_cloud.points[i]));
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();

43
cartographer_ros/cartographer_ros/msg_conversion.cc
View File

@ -147,9 +147,9 @@ LaserScanToPointCloudWithIntensities(const LaserMessageType& msg) {
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());
Eigen::Vector4f point; const cartographer::sensor::TimedRangefinderPoint point{
point << rotation * (first_echo * Eigen::Vector3f::UnitX()), rotation * (first_echo * Eigen::Vector3f::UnitX()),
i * msg.time_increment; i * msg.time_increment};
point_cloud.points.push_back(point); 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());
@ -165,10 +165,10 @@ LaserScanToPointCloudWithIntensities(const LaserMessageType& msg) {
} }
::cartographer::common::Time timestamp = FromRos(msg.header.stamp); ::cartographer::common::Time timestamp = FromRos(msg.header.stamp);
if (!point_cloud.points.empty()) { if (!point_cloud.points.empty()) {
const double duration = point_cloud.points.back()[3]; const double duration = point_cloud.points.back().time;
timestamp += cartographer::common::FromSeconds(duration); timestamp += cartographer::common::FromSeconds(duration);
for (Eigen::Vector4f& point : point_cloud.points) { for (auto& point : point_cloud.points) {
point[3] -= duration; point.time -= duration;
} }
} }
return std::make_tuple(point_cloud, timestamp); return std::make_tuple(point_cloud, timestamp);
@ -191,10 +191,10 @@ sensor_msgs::PointCloud2 ToPointCloud2Message(
const ::cartographer::sensor::TimedPointCloud& 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 Eigen::Vector4f& point : point_cloud) { for (const cartographer::sensor::TimedRangefinderPoint& point : point_cloud) {
stream.next(point.x()); stream.next(point.position.x());
stream.next(point.y()); stream.next(point.position.y());
stream.next(point.z()); stream.next(point.position.z());
stream.next(kPointCloudComponentFourMagic); stream.next(kPointCloudComponentFourMagic);
} }
return msg; return msg;
@ -225,7 +225,8 @@ ToPointCloudWithIntensities(const sensor_msgs::PointCloud2& msg) {
point_cloud.points.reserve(pcl_point_cloud.size()); point_cloud.points.reserve(pcl_point_cloud.size());
point_cloud.intensities.reserve(pcl_point_cloud.size()); point_cloud.intensities.reserve(pcl_point_cloud.size());
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.time); point_cloud.points.push_back(
{Eigen::Vector3f{point.x, point.y, point.z}, point.time});
point_cloud.intensities.push_back(point.intensity); point_cloud.intensities.push_back(point.intensity);
} }
} else { } else {
@ -234,7 +235,8 @@ ToPointCloudWithIntensities(const sensor_msgs::PointCloud2& msg) {
point_cloud.points.reserve(pcl_point_cloud.size()); point_cloud.points.reserve(pcl_point_cloud.size());
point_cloud.intensities.reserve(pcl_point_cloud.size()); point_cloud.intensities.reserve(pcl_point_cloud.size());
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, 0.f); point_cloud.points.push_back(
{Eigen::Vector3f{point.x, point.y, point.z}, 0.f});
point_cloud.intensities.push_back(point.intensity); point_cloud.intensities.push_back(point.intensity);
} }
} }
@ -246,7 +248,8 @@ ToPointCloudWithIntensities(const sensor_msgs::PointCloud2& msg) {
point_cloud.points.reserve(pcl_point_cloud.size()); point_cloud.points.reserve(pcl_point_cloud.size());
point_cloud.intensities.reserve(pcl_point_cloud.size()); point_cloud.intensities.reserve(pcl_point_cloud.size());
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.time); point_cloud.points.push_back(
{Eigen::Vector3f{point.x, point.y, point.z}, point.time});
point_cloud.intensities.push_back(1.0f); point_cloud.intensities.push_back(1.0f);
} }
} else { } else {
@ -255,19 +258,21 @@ ToPointCloudWithIntensities(const sensor_msgs::PointCloud2& msg) {
point_cloud.points.reserve(pcl_point_cloud.size()); point_cloud.points.reserve(pcl_point_cloud.size());
point_cloud.intensities.reserve(pcl_point_cloud.size()); point_cloud.intensities.reserve(pcl_point_cloud.size());
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, 0.f); point_cloud.points.push_back(
{Eigen::Vector3f{point.x, point.y, point.z}, 0.f});
point_cloud.intensities.push_back(1.0f); point_cloud.intensities.push_back(1.0f);
} }
} }
} }
::cartographer::common::Time timestamp = FromRos(msg.header.stamp); ::cartographer::common::Time timestamp = FromRos(msg.header.stamp);
if (!point_cloud.points.empty()) { if (!point_cloud.points.empty()) {
const double duration = point_cloud.points.back()[3]; const double duration = point_cloud.points.back().time;
timestamp += cartographer::common::FromSeconds(duration); timestamp += cartographer::common::FromSeconds(duration);
for (Eigen::Vector4f& point : point_cloud.points) { for (auto& point : point_cloud.points) {
point[3] -= duration; point.time -= duration;
CHECK_LE(point[3], 0) << "Encountered a point with a larger stamp than " CHECK_LE(point.time, 0.f)
"the last point in the cloud."; << "Encountered a point with a larger stamp than "
"the last point in the cloud.";
} }
} }
return std::make_tuple(point_cloud, timestamp); return std::make_tuple(point_cloud, timestamp);

40
cartographer_ros/cartographer_ros/msg_conversion_test.cc
View File

@ -50,25 +50,25 @@ TEST(MsgConversion, LaserScanToPointCloud) {
const auto point_cloud = const auto point_cloud =
std::get<0>(ToPointCloudWithIntensities(laser_scan)).points; std::get<0>(ToPointCloudWithIntensities(laser_scan)).points;
EXPECT_TRUE( EXPECT_TRUE(
point_cloud[0].isApprox(Eigen::Vector4f(1.f, 0.f, 0.f, 0.f), kEps)); point_cloud[0].position.isApprox(Eigen::Vector3f(1.f, 0.f, 0.f), kEps));
EXPECT_TRUE(point_cloud[1].isApprox( EXPECT_TRUE(point_cloud[1].position.isApprox(
Eigen::Vector4f(1.f / std::sqrt(2.f), 1.f / std::sqrt(2.f), 0.f, 0.f), Eigen::Vector3f(1.f / std::sqrt(2.f), 1.f / std::sqrt(2.f), 0.f), kEps));
EXPECT_TRUE(
point_cloud[2].position.isApprox(Eigen::Vector3f(0.f, 1.f, 0.f), kEps));
EXPECT_TRUE(point_cloud[3].position.isApprox(
Eigen::Vector3f(-1.f / std::sqrt(2.f), 1.f / std::sqrt(2.f), 0.f), kEps));
EXPECT_TRUE(
point_cloud[4].position.isApprox(Eigen::Vector3f(-1.f, 0.f, 0.f), kEps));
EXPECT_TRUE(point_cloud[5].position.isApprox(
Eigen::Vector3f(-1.f / std::sqrt(2.f), -1.f / std::sqrt(2.f), 0.f),
kEps)); kEps));
EXPECT_TRUE( EXPECT_TRUE(
point_cloud[2].isApprox(Eigen::Vector4f(0.f, 1.f, 0.f, 0.f), kEps)); point_cloud[6].position.isApprox(Eigen::Vector3f(0.f, -1.f, 0.f), kEps));
EXPECT_TRUE(point_cloud[3].isApprox( EXPECT_TRUE(point_cloud[7].position.isApprox(
Eigen::Vector4f(-1.f / std::sqrt(2.f), 1.f / std::sqrt(2.f), 0.f, 0.f), Eigen::Vector3f(1.f / std::sqrt(2.f), -1.f / std::sqrt(2.f), 0.f), kEps));
kEps)); for (int i = 0; i < 8; ++i) {
EXPECT_TRUE( EXPECT_NEAR(point_cloud[i].time, 0.f, kEps);
point_cloud[4].isApprox(Eigen::Vector4f(-1.f, 0.f, 0.f, 0.f), kEps)); }
EXPECT_TRUE(point_cloud[5].isApprox(
Eigen::Vector4f(-1.f / std::sqrt(2.f), -1.f / std::sqrt(2.f), 0.f, 0.f),
kEps));
EXPECT_TRUE(
point_cloud[6].isApprox(Eigen::Vector4f(0.f, -1.f, 0.f, 0.f), kEps));
EXPECT_TRUE(point_cloud[7].isApprox(
Eigen::Vector4f(1.f / std::sqrt(2.f), -1.f / std::sqrt(2.f), 0.f, 0.f),
kEps));
} }
TEST(MsgConversion, LaserScanToPointCloudWithInfinityAndNaN) { TEST(MsgConversion, LaserScanToPointCloudWithInfinityAndNaN) {
@ -88,9 +88,11 @@ TEST(MsgConversion, LaserScanToPointCloudWithInfinityAndNaN) {
std::get<0>(ToPointCloudWithIntensities(laser_scan)).points; std::get<0>(ToPointCloudWithIntensities(laser_scan)).points;
ASSERT_EQ(2, point_cloud.size()); ASSERT_EQ(2, point_cloud.size());
EXPECT_TRUE( EXPECT_TRUE(
point_cloud[0].isApprox(Eigen::Vector4f(0.f, 2.f, 0.f, 0.f), kEps)); point_cloud[0].position.isApprox(Eigen::Vector3f(0.f, 2.f, 0.f), kEps));
EXPECT_TRUE( EXPECT_TRUE(
point_cloud[1].isApprox(Eigen::Vector4f(-3.f, 0.f, 0.f, 0.f), kEps)); point_cloud[1].position.isApprox(Eigen::Vector3f(-3.f, 0.f, 0.f), kEps));
EXPECT_NEAR(point_cloud[0].time, 0.f, kEps);
EXPECT_NEAR(point_cloud[1].time, 0.f, kEps);
} }
::testing::Matcher<const LandmarkObservation&> EqualsLandmark( ::testing::Matcher<const LandmarkObservation&> EqualsLandmark(

7
cartographer_ros/cartographer_ros/node.cc
View File

@ -208,11 +208,10 @@ void Node::PublishLocalTrajectoryData(const ::ros::TimerEvent& timer_event) {
carto::sensor::TimedPointCloud point_cloud; carto::sensor::TimedPointCloud point_cloud;
point_cloud.reserve(trajectory_data.local_slam_data->range_data_in_local point_cloud.reserve(trajectory_data.local_slam_data->range_data_in_local
.returns.size()); .returns.size());
for (const Eigen::Vector3f point : for (const cartographer::sensor::RangefinderPoint point :
trajectory_data.local_slam_data->range_data_in_local.returns) { trajectory_data.local_slam_data->range_data_in_local.returns) {
Eigen::Vector4f point_time; point_cloud.push_back(cartographer::sensor::ToTimedRangefinderPoint(
point_time << point, 0.f; point, 0.f /* time */));
point_cloud.push_back(point_time);
} }
scan_matched_point_cloud_publisher_.publish(ToPointCloud2Message( scan_matched_point_cloud_publisher_.publish(ToPointCloud2Message(
carto::common::ToUniversal(trajectory_data.local_slam_data->time), carto::common::ToUniversal(trajectory_data.local_slam_data->time),

11
cartographer_ros/cartographer_ros/rosbag_validate_main.cc
View File

@ -219,14 +219,15 @@ class RangeDataChecker {
const cartographer::sensor::TimedPointCloud& point_cloud = const cartographer::sensor::TimedPointCloud& point_cloud =
std::get<0>(point_cloud_time).points; std::get<0>(point_cloud_time).points;
*to = std::get<1>(point_cloud_time); *to = std::get<1>(point_cloud_time);
*from = *to + cartographer::common::FromSeconds(point_cloud[0][3]); *from = *to + cartographer::common::FromSeconds(point_cloud[0].time);
Eigen::Vector4f points_sum = Eigen::Vector4f::Zero(); Eigen::Vector4f points_sum = Eigen::Vector4f::Zero();
for (const Eigen::Vector4f& point : point_cloud) { for (const auto& point : point_cloud) {
points_sum += point; points_sum.head<3>() += point.position;
points_sum[3] += point.time;
} }
if (point_cloud.size() > 0) { if (point_cloud.size() > 0) {
double first_point_relative_time = point_cloud[0][3]; double first_point_relative_time = point_cloud[0].time;
double last_point_relative_time = (*point_cloud.rbegin())[3]; double last_point_relative_time = (*point_cloud.rbegin()).time;
if (first_point_relative_time > 0) { if (first_point_relative_time > 0) {
LOG_FIRST_N(ERROR, 1) LOG_FIRST_N(ERROR, 1)
<< "Sensor with frame_id \"" << message.header.frame_id << "Sensor with frame_id \"" << message.header.frame_id

12
cartographer_ros/cartographer_ros/sensor_bridge.cc
View File

@ -177,7 +177,7 @@ void SensorBridge::HandleLaserScan(
if (points.points.empty()) { if (points.points.empty()) {
return; return;
} }
CHECK_LE(points.points.back()[3], 0); CHECK_LE(points.points.back().time, 0.f);
// TODO(gaschler): Use per-point time instead of subdivisions. // 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 =
@ -189,7 +189,7 @@ void SensorBridge::HandleLaserScan(
if (start_index == end_index) { if (start_index == end_index) {
continue; continue;
} }
const double time_to_subdivision_end = subdivision.back()[3]; const double time_to_subdivision_end = subdivision.back().time;
// `subdivision_time` is the end of the measurement so sensor::Collator will // `subdivision_time` is the end of the measurement so sensor::Collator will
// send all other sensor data first. // send all other sensor data first.
const carto::common::Time subdivision_time = const carto::common::Time subdivision_time =
@ -204,10 +204,10 @@ void SensorBridge::HandleLaserScan(
continue; continue;
} }
sensor_to_previous_subdivision_time_[sensor_id] = subdivision_time; sensor_to_previous_subdivision_time_[sensor_id] = subdivision_time;
for (Eigen::Vector4f& point : subdivision) { for (auto& point : subdivision) {
point[3] -= time_to_subdivision_end; point.time -= time_to_subdivision_end;
} }
CHECK_EQ(subdivision.back()[3], 0); CHECK_EQ(subdivision.back().time, 0.f);
HandleRangefinder(sensor_id, subdivision_time, frame_id, subdivision); HandleRangefinder(sensor_id, subdivision_time, frame_id, subdivision);
} }
} }
@ -216,7 +216,7 @@ void SensorBridge::HandleRangefinder(
const std::string& sensor_id, const carto::common::Time time, const std::string& sensor_id, const carto::common::Time time,
const std::string& frame_id, const carto::sensor::TimedPointCloud& ranges) { const std::string& frame_id, const carto::sensor::TimedPointCloud& ranges) {
if (!ranges.empty()) { if (!ranges.empty()) {
CHECK_LE(ranges.back()[3], 0); CHECK_LE(ranges.back().time, 0.f);
} }
const auto sensor_to_tracking = const auto sensor_to_tracking =
tf_bridge_.LookupToTracking(time, CheckNoLeadingSlash(frame_id)); tf_bridge_.LookupToTracking(time, CheckNoLeadingSlash(frame_id));

3
scripts/install_debs.sh
View File

@ -43,3 +43,6 @@ apt-get install -y ninja-build
# Install rosdep dependencies. # Install rosdep dependencies.
rosdep update rosdep update
rosdep install --from-paths src --ignore-src --rosdistro=${ROS_DISTRO} -y rosdep install --from-paths src --ignore-src --rosdistro=${ROS_DISTRO} -y
# Update rosconsole-bridge to fix build issue with Docker image for Kinetic
sudo apt-get install ros-${ROS_DISTRO}-rosconsole-bridge -y