From 9006fb6fb12d71a0f51b6549eff717589264d546 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Thu, 13 Oct 2016 17:16:32 +0200 Subject: [PATCH] Show the unprojected point cloud in 2D SLAM. (#59) We now project to 2D later in 2D SLAM, so that the roll and pitch applied to the laser fan in visible in the visualization. --- .../mapping_2d/global_trajectory_builder.cc | 2 +- .../mapping_2d/local_trajectory_builder.cc | 39 ++++++++----------- .../mapping_2d/local_trajectory_builder.h | 9 ++--- cartographer/sensor/laser.cc | 14 ++++--- cartographer/sensor/laser.h | 18 ++++----- cartographer/sensor/point_cloud.cc | 7 ++-- cartographer/sensor/point_cloud.h | 7 ++-- 7 files changed, 46 insertions(+), 50 deletions(-) diff --git a/cartographer/mapping_2d/global_trajectory_builder.cc b/cartographer/mapping_2d/global_trajectory_builder.cc index a413335..5df7a25 100644 --- a/cartographer/mapping_2d/global_trajectory_builder.cc +++ b/cartographer/mapping_2d/global_trajectory_builder.cc @@ -47,7 +47,7 @@ void GlobalTrajectoryBuilder::AddHorizontalLaserFan( if (insertion_result != nullptr) { sparse_pose_graph_->AddScan( insertion_result->time, insertion_result->tracking_to_tracking_2d, - insertion_result->laser_fan_in_tracking_2d, + sensor::ProjectLaserFan(insertion_result->laser_fan_in_tracking_2d), insertion_result->pose_estimate_2d, kalman_filter::Project2D(insertion_result->covariance_estimate), insertion_result->submaps, insertion_result->matching_submap, diff --git a/cartographer/mapping_2d/local_trajectory_builder.cc b/cartographer/mapping_2d/local_trajectory_builder.cc index 39d0cb2..c1c34da 100644 --- a/cartographer/mapping_2d/local_trajectory_builder.cc +++ b/cartographer/mapping_2d/local_trajectory_builder.cc @@ -78,29 +78,24 @@ kalman_filter::PoseTracker* LocalTrajectoryBuilder::pose_tracker() const { return pose_tracker_.get(); } -sensor::LaserFan LocalTrajectoryBuilder::BuildProjectedLaserFan( +sensor::LaserFan3D LocalTrajectoryBuilder::BuildCroppedLaserFan( const transform::Rigid3f& tracking_to_tracking_2d, const sensor::LaserFan3D& laser_fan) const { - const sensor::LaserFan projected_fan = sensor::ProjectCroppedLaserFan( + const sensor::LaserFan3D cropped_fan = sensor::CropLaserFan( sensor::TransformLaserFan3D(laser_fan, tracking_to_tracking_2d), - Eigen::Vector3f(-std::numeric_limits::infinity(), - -std::numeric_limits::infinity(), - options_.horizontal_laser_min_z()), - Eigen::Vector3f(std::numeric_limits::infinity(), - std::numeric_limits::infinity(), - options_.horizontal_laser_max_z())); - return sensor::LaserFan{ - projected_fan.origin, - sensor::VoxelFiltered(projected_fan.point_cloud, + options_.horizontal_laser_min_z(), options_.horizontal_laser_max_z()); + return sensor::LaserFan3D{ + cropped_fan.origin, + sensor::VoxelFiltered(cropped_fan.returns, options_.horizontal_laser_voxel_filter_size()), - sensor::VoxelFiltered(projected_fan.missing_echo_point_cloud, + sensor::VoxelFiltered(cropped_fan.misses, options_.horizontal_laser_voxel_filter_size())}; } void LocalTrajectoryBuilder::ScanMatch( common::Time time, const transform::Rigid3d& pose_prediction, const transform::Rigid3d& tracking_to_tracking_2d, - const sensor::LaserFan& laser_fan_in_tracking_2d, + const sensor::LaserFan3D& laser_fan_in_tracking_2d, transform::Rigid3d* pose_observation, kalman_filter::PoseCovariance* covariance_observation) { const ProbabilityGrid& probability_grid = @@ -113,7 +108,8 @@ void LocalTrajectoryBuilder::ScanMatch( sensor::AdaptiveVoxelFilter adaptive_voxel_filter( options_.adaptive_voxel_filter_options()); const sensor::PointCloud2D filtered_point_cloud_in_tracking_2d = - adaptive_voxel_filter.Filter(laser_fan_in_tracking_2d.point_cloud); + sensor::ProjectToPointCloud2D( + adaptive_voxel_filter.Filter(laser_fan_in_tracking_2d.returns)); if (options_.use_online_correlative_scan_matching()) { real_time_correlative_scan_matcher_.Match( pose_prediction_2d, filtered_point_cloud_in_tracking_2d, @@ -170,10 +166,10 @@ LocalTrajectoryBuilder::AddHorizontalLaserFan( -transform::GetYaw(pose_prediction), Eigen::Vector3d::UnitZ())) * pose_prediction.rotation()); - const sensor::LaserFan laser_fan_in_tracking_2d = - BuildProjectedLaserFan(tracking_to_tracking_2d.cast(), laser_fan); + const sensor::LaserFan3D laser_fan_in_tracking_2d = + BuildCroppedLaserFan(tracking_to_tracking_2d.cast(), laser_fan); - if (laser_fan_in_tracking_2d.point_cloud.empty()) { + if (laser_fan_in_tracking_2d.returns.empty()) { LOG(WARNING) << "Dropped empty horizontal laser point cloud."; return nullptr; } @@ -202,9 +198,8 @@ LocalTrajectoryBuilder::AddHorizontalLaserFan( {pose_observation, covariance_observation}, {scan_matcher_pose_estimate_, covariance_estimate}, scan_matcher_pose_estimate_, - sensor::TransformPointCloud( - sensor::ToPointCloud(laser_fan_in_tracking_2d.point_cloud), - tracking_2d_to_map.cast())}; + sensor::TransformPointCloud(laser_fan_in_tracking_2d.returns, + tracking_2d_to_map.cast())}; const transform::Rigid2d pose_estimate_2d = transform::Project2D(tracking_2d_to_map); @@ -218,8 +213,8 @@ LocalTrajectoryBuilder::AddHorizontalLaserFan( for (int insertion_index : submaps_.insertion_indices()) { insertion_submaps.push_back(submaps_.Get(insertion_index)); } - submaps_.InsertLaserFan(TransformLaserFan(laser_fan_in_tracking_2d, - pose_estimate_2d.cast())); + submaps_.InsertLaserFan(sensor::ProjectLaserFan(TransformLaserFan3D( + laser_fan_in_tracking_2d, tracking_2d_to_map.cast()))); return common::make_unique(InsertionResult{ time, &submaps_, matching_submap, insertion_submaps, diff --git a/cartographer/mapping_2d/local_trajectory_builder.h b/cartographer/mapping_2d/local_trajectory_builder.h index d7786b5..11e42bf 100644 --- a/cartographer/mapping_2d/local_trajectory_builder.h +++ b/cartographer/mapping_2d/local_trajectory_builder.h @@ -49,7 +49,7 @@ class LocalTrajectoryBuilder { std::vector insertion_submaps; transform::Rigid3d tracking_to_tracking_2d; transform::Rigid3d tracking_2d_to_map; - sensor::LaserFan laser_fan_in_tracking_2d; + sensor::LaserFan3D laser_fan_in_tracking_2d; transform::Rigid2d pose_estimate_2d; kalman_filter::PoseCovariance covariance_estimate; }; @@ -75,9 +75,8 @@ class LocalTrajectoryBuilder { kalman_filter::PoseTracker* pose_tracker() const; private: - // Transforms 'laser_scan', projects it onto the ground plane, - // crops and voxel filters. - sensor::LaserFan BuildProjectedLaserFan( + // Transforms 'laser_scan', crops and voxel filters. + sensor::LaserFan3D BuildCroppedLaserFan( const transform::Rigid3f& tracking_to_tracking_2d, const sensor::LaserFan3D& laser_fan) const; @@ -85,7 +84,7 @@ class LocalTrajectoryBuilder { // 'pose_observation' and 'covariance_observation' with the result. void ScanMatch(common::Time time, const transform::Rigid3d& pose_prediction, const transform::Rigid3d& tracking_to_tracking_2d, - const sensor::LaserFan& laser_fan_in_tracking_2d, + const sensor::LaserFan3D& laser_fan_in_tracking_2d, transform::Rigid3d* pose_observation, kalman_filter::PoseCovariance* covariance_observation); diff --git a/cartographer/sensor/laser.cc b/cartographer/sensor/laser.cc index 24ced7e..0348b8a 100644 --- a/cartographer/sensor/laser.cc +++ b/cartographer/sensor/laser.cc @@ -113,12 +113,16 @@ LaserFan3D FilterLaserFanByMaxRange(const LaserFan3D& laser_fan, return result; } -LaserFan ProjectCroppedLaserFan(const LaserFan3D& laser_fan, - const Eigen::Vector3f& min, - const Eigen::Vector3f& max) { +LaserFan3D CropLaserFan(const LaserFan3D& laser_fan, const float min_z, + const float max_z) { + return LaserFan3D{laser_fan.origin, Crop(laser_fan.returns, min_z, max_z), + Crop(laser_fan.misses, min_z, max_z)}; +} + +LaserFan ProjectLaserFan(const LaserFan3D& laser_fan) { return LaserFan{laser_fan.origin.head<2>(), - ProjectToPointCloud2D(Crop(laser_fan.returns, min, max)), - ProjectToPointCloud2D(Crop(laser_fan.misses, min, max))}; + ProjectToPointCloud2D(laser_fan.returns), + ProjectToPointCloud2D(laser_fan.misses)}; } CompressedLaserFan3D Compress(const LaserFan3D& laser_fan) { diff --git a/cartographer/sensor/laser.h b/cartographer/sensor/laser.h index 40efd69..a67f46b 100644 --- a/cartographer/sensor/laser.h +++ b/cartographer/sensor/laser.h @@ -25,10 +25,6 @@ namespace cartographer { namespace sensor { -// Builds a LaserFan from 'proto' and separates any beams with ranges outside -// the range ['min_range', 'max_range']. Beams beyond 'max_range' inserted into -// the 'missing_echo_point_cloud' with length 'missing_echo_ray_length'. The -// points in both clouds are stored in scan order. struct LaserFan { Eigen::Vector2f origin; PointCloud2D point_cloud; @@ -52,6 +48,10 @@ struct LaserFan3D { std::vector reflectivities; }; +// Builds a LaserFan from 'proto' and separates any beams with ranges outside +// the range ['min_range', 'max_range']. Beams beyond 'max_range' are inserted +// into the 'misses' point cloud with length 'missing_echo_ray_length'. The +// points in both clouds are stored in scan order. LaserFan3D ToLaserFan3D(const proto::LaserScan& proto, float min_range, float max_range, float missing_echo_ray_length); @@ -70,11 +70,11 @@ LaserFan3D TransformLaserFan3D(const LaserFan3D& laser_fan, LaserFan3D FilterLaserFanByMaxRange(const LaserFan3D& laser_fan, float max_range); -// Projects 'laser_fan' into 2D and crops it according to the cuboid defined by -// 'min' and 'max'. -LaserFan ProjectCroppedLaserFan(const LaserFan3D& laser_fan, - const Eigen::Vector3f& min, - const Eigen::Vector3f& max); +// Crops 'laser_fan' according to the region defined by 'min_z' and 'max_z'. +LaserFan3D CropLaserFan(const LaserFan3D& laser_fan, float min_z, float max_z); + +// Projects 'laser_fan' into 2D. +LaserFan ProjectLaserFan(const LaserFan3D& laser_fan); // Like LaserFan3D but with compressed point clouds. The point order changes // when converting from LaserFan3D. diff --git a/cartographer/sensor/point_cloud.cc b/cartographer/sensor/point_cloud.cc index b937dac..dc3fbd8 100644 --- a/cartographer/sensor/point_cloud.cc +++ b/cartographer/sensor/point_cloud.cc @@ -65,12 +65,11 @@ PointCloud2D ProjectToPointCloud2D(const PointCloud& point_cloud) { return point_cloud_2d; } -PointCloud Crop(const PointCloud& point_cloud, const Eigen::Vector3f& min, - const Eigen::Vector3f& max) { +PointCloud Crop(const PointCloud& point_cloud, const float min_z, + const float max_z) { PointCloud cropped_point_cloud; for (const auto& point : point_cloud) { - if (min.x() <= point.x() && point.x() <= max.x() && min.y() <= point.y() && - point.y() <= max.y() && min.z() <= point.z() && point.z() <= max.z()) { + if (min_z <= point.z() && point.z() <= max_z) { cropped_point_cloud.push_back(point); } } diff --git a/cartographer/sensor/point_cloud.h b/cartographer/sensor/point_cloud.h index 8b8705b..6879f84 100644 --- a/cartographer/sensor/point_cloud.h +++ b/cartographer/sensor/point_cloud.h @@ -45,10 +45,9 @@ PointCloud ToPointCloud(const PointCloud2D& point_cloud_2d); // Converts 'point_cloud' to a 2D point cloud by removing the z component. PointCloud2D ProjectToPointCloud2D(const PointCloud& point_cloud); -// Returns a new point cloud without points that fall outside the axis-aligned -// cuboid defined by 'min' and 'max'. -PointCloud Crop(const PointCloud& point_cloud, const Eigen::Vector3f& min, - const Eigen::Vector3f& max); +// Returns a new point cloud without points that fall outside the region defined +// by 'min_z' and 'max_z'. +PointCloud Crop(const PointCloud& point_cloud, float min_z, float max_z); // Converts 'point_cloud' to a proto::PointCloud. proto::PointCloud ToProto(const PointCloud& point_cloud);