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) {
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)) {
continue;
}
@ -121,8 +122,9 @@ std::unique_ptr<carto::io::PointsBatch> HandleMessage(
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].head<3>());
points_batch->points.push_back(
sensor_to_map *
carto::sensor::ToRangefinderPoint(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();

43
cartographer_ros/cartographer_ros/msg_conversion.cc
View File

@ -147,9 +147,9 @@ LaserScanToPointCloudWithIntensities(const LaserMessageType& msg) {
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());
Eigen::Vector4f point;
point << rotation * (first_echo * Eigen::Vector3f::UnitX()),
i * msg.time_increment;
const cartographer::sensor::TimedRangefinderPoint 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());
@ -165,10 +165,10 @@ LaserScanToPointCloudWithIntensities(const LaserMessageType& msg) {
}
::cartographer::common::Time timestamp = FromRos(msg.header.stamp);
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);
for (Eigen::Vector4f& point : point_cloud.points) {
point[3] -= duration;
for (auto& point : point_cloud.points) {
point.time -= duration;
}
}
return std::make_tuple(point_cloud, timestamp);
@ -191,10 +191,10 @@ sensor_msgs::PointCloud2 ToPointCloud2Message(
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 Eigen::Vector4f& point : point_cloud) {
stream.next(point.x());
stream.next(point.y());
stream.next(point.z());
for (const cartographer::sensor::TimedRangefinderPoint& point : point_cloud) {
stream.next(point.position.x());
stream.next(point.position.y());
stream.next(point.position.z());
stream.next(kPointCloudComponentFourMagic);
}
return msg;
@ -225,7 +225,8 @@ ToPointCloudWithIntensities(const sensor_msgs::PointCloud2& msg) {
point_cloud.points.reserve(pcl_point_cloud.size());
point_cloud.intensities.reserve(pcl_point_cloud.size());
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);
}
} else {
@ -234,7 +235,8 @@ ToPointCloudWithIntensities(const sensor_msgs::PointCloud2& msg) {
point_cloud.points.reserve(pcl_point_cloud.size());
point_cloud.intensities.reserve(pcl_point_cloud.size());
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);
}
}
@ -246,7 +248,8 @@ ToPointCloudWithIntensities(const sensor_msgs::PointCloud2& msg) {
point_cloud.points.reserve(pcl_point_cloud.size());
point_cloud.intensities.reserve(pcl_point_cloud.size());
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);
}
} else {
@ -255,19 +258,21 @@ ToPointCloudWithIntensities(const sensor_msgs::PointCloud2& msg) {
point_cloud.points.reserve(pcl_point_cloud.size());
point_cloud.intensities.reserve(pcl_point_cloud.size());
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);
}
}
}
::cartographer::common::Time timestamp = FromRos(msg.header.stamp);
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);
for (Eigen::Vector4f& point : point_cloud.points) {
point[3] -= duration;
CHECK_LE(point[3], 0) << "Encountered a point with a larger stamp than "
"the last point in the cloud.";
for (auto& point : point_cloud.points) {
point.time -= duration;
CHECK_LE(point.time, 0.f)
<< "Encountered a point with a larger stamp than "
"the last point in the cloud.";
}
}
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 =
std::get<0>(ToPointCloudWithIntensities(laser_scan)).points;
EXPECT_TRUE(
point_cloud[0].isApprox(Eigen::Vector4f(1.f, 0.f, 0.f, 0.f), kEps));
EXPECT_TRUE(point_cloud[1].isApprox(
Eigen::Vector4f(1.f / std::sqrt(2.f), 1.f / std::sqrt(2.f), 0.f, 0.f),
point_cloud[0].position.isApprox(Eigen::Vector3f(1.f, 0.f, 0.f), kEps));
EXPECT_TRUE(point_cloud[1].position.isApprox(
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));
EXPECT_TRUE(
point_cloud[2].isApprox(Eigen::Vector4f(0.f, 1.f, 0.f, 0.f), kEps));
EXPECT_TRUE(point_cloud[3].isApprox(
Eigen::Vector4f(-1.f / std::sqrt(2.f), 1.f / std::sqrt(2.f), 0.f, 0.f),
kEps));
EXPECT_TRUE(
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));
point_cloud[6].position.isApprox(Eigen::Vector3f(0.f, -1.f, 0.f), kEps));
EXPECT_TRUE(point_cloud[7].position.isApprox(
Eigen::Vector3f(1.f / std::sqrt(2.f), -1.f / std::sqrt(2.f), 0.f), kEps));
for (int i = 0; i < 8; ++i) {
EXPECT_NEAR(point_cloud[i].time, 0.f, kEps);
}
}
TEST(MsgConversion, LaserScanToPointCloudWithInfinityAndNaN) {
@ -88,9 +88,11 @@ TEST(MsgConversion, LaserScanToPointCloudWithInfinityAndNaN) {
std::get<0>(ToPointCloudWithIntensities(laser_scan)).points;
ASSERT_EQ(2, point_cloud.size());
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(
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(

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;
point_cloud.reserve(trajectory_data.local_slam_data->range_data_in_local
.returns.size());
for (const Eigen::Vector3f point :
for (const cartographer::sensor::RangefinderPoint point :
trajectory_data.local_slam_data->range_data_in_local.returns) {
Eigen::Vector4f point_time;
point_time << point, 0.f;
point_cloud.push_back(point_time);
point_cloud.push_back(cartographer::sensor::ToTimedRangefinderPoint(
point, 0.f /* time */));
}
scan_matched_point_cloud_publisher_.publish(ToPointCloud2Message(
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 =
std::get<0>(point_cloud_time).points;
*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();
for (const Eigen::Vector4f& point : point_cloud) {
points_sum += point;
for (const auto& point : point_cloud) {
points_sum.head<3>() += point.position;
points_sum[3] += point.time;
}
if (point_cloud.size() > 0) {
double first_point_relative_time = point_cloud[0][3];
double last_point_relative_time = (*point_cloud.rbegin())[3];
double first_point_relative_time = point_cloud[0].time;
double last_point_relative_time = (*point_cloud.rbegin()).time;
if (first_point_relative_time > 0) {
LOG_FIRST_N(ERROR, 1)
<< "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()) {
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.
for (int i = 0; i != num_subdivisions_per_laser_scan_; ++i) {
const size_t start_index =
@ -189,7 +189,7 @@ void SensorBridge::HandleLaserScan(
if (start_index == end_index) {
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
// send all other sensor data first.
const carto::common::Time subdivision_time =
@ -204,10 +204,10 @@ void SensorBridge::HandleLaserScan(
continue;
}
sensor_to_previous_subdivision_time_[sensor_id] = subdivision_time;
for (Eigen::Vector4f& point : subdivision) {
point[3] -= time_to_subdivision_end;
for (auto& point : subdivision) {
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);
}
}
@ -216,7 +216,7 @@ void SensorBridge::HandleRangefinder(
const std::string& sensor_id, const carto::common::Time time,
const std::string& frame_id, const carto::sensor::TimedPointCloud& ranges) {
if (!ranges.empty()) {
CHECK_LE(ranges.back()[3], 0);
CHECK_LE(ranges.back().time, 0.f);
}
const auto sensor_to_tracking =
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.
rosdep update
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