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.
master
Wolfgang Hess 2016-10-13 17:16:32 +02:00 committed by GitHub
parent 0bf37d0190
commit 9006fb6fb1
7 changed files with 46 additions and 50 deletions

View File

@ -47,7 +47,7 @@ void GlobalTrajectoryBuilder::AddHorizontalLaserFan(
if (insertion_result != nullptr) { if (insertion_result != nullptr) {
sparse_pose_graph_->AddScan( sparse_pose_graph_->AddScan(
insertion_result->time, insertion_result->tracking_to_tracking_2d, 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, insertion_result->pose_estimate_2d,
kalman_filter::Project2D(insertion_result->covariance_estimate), kalman_filter::Project2D(insertion_result->covariance_estimate),
insertion_result->submaps, insertion_result->matching_submap, insertion_result->submaps, insertion_result->matching_submap,

View File

@ -78,29 +78,24 @@ kalman_filter::PoseTracker* LocalTrajectoryBuilder::pose_tracker() const {
return pose_tracker_.get(); return pose_tracker_.get();
} }
sensor::LaserFan LocalTrajectoryBuilder::BuildProjectedLaserFan( sensor::LaserFan3D LocalTrajectoryBuilder::BuildCroppedLaserFan(
const transform::Rigid3f& tracking_to_tracking_2d, const transform::Rigid3f& tracking_to_tracking_2d,
const sensor::LaserFan3D& laser_fan) const { 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), sensor::TransformLaserFan3D(laser_fan, tracking_to_tracking_2d),
Eigen::Vector3f(-std::numeric_limits<float>::infinity(), options_.horizontal_laser_min_z(), options_.horizontal_laser_max_z());
-std::numeric_limits<float>::infinity(), return sensor::LaserFan3D{
options_.horizontal_laser_min_z()), cropped_fan.origin,
Eigen::Vector3f(std::numeric_limits<float>::infinity(), sensor::VoxelFiltered(cropped_fan.returns,
std::numeric_limits<float>::infinity(),
options_.horizontal_laser_max_z()));
return sensor::LaserFan{
projected_fan.origin,
sensor::VoxelFiltered(projected_fan.point_cloud,
options_.horizontal_laser_voxel_filter_size()), 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())}; options_.horizontal_laser_voxel_filter_size())};
} }
void LocalTrajectoryBuilder::ScanMatch( void LocalTrajectoryBuilder::ScanMatch(
common::Time time, const transform::Rigid3d& pose_prediction, common::Time time, const transform::Rigid3d& pose_prediction,
const transform::Rigid3d& tracking_to_tracking_2d, 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, transform::Rigid3d* pose_observation,
kalman_filter::PoseCovariance* covariance_observation) { kalman_filter::PoseCovariance* covariance_observation) {
const ProbabilityGrid& probability_grid = const ProbabilityGrid& probability_grid =
@ -113,7 +108,8 @@ void LocalTrajectoryBuilder::ScanMatch(
sensor::AdaptiveVoxelFilter adaptive_voxel_filter( sensor::AdaptiveVoxelFilter adaptive_voxel_filter(
options_.adaptive_voxel_filter_options()); options_.adaptive_voxel_filter_options());
const sensor::PointCloud2D filtered_point_cloud_in_tracking_2d = 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()) { if (options_.use_online_correlative_scan_matching()) {
real_time_correlative_scan_matcher_.Match( real_time_correlative_scan_matcher_.Match(
pose_prediction_2d, filtered_point_cloud_in_tracking_2d, pose_prediction_2d, filtered_point_cloud_in_tracking_2d,
@ -170,10 +166,10 @@ LocalTrajectoryBuilder::AddHorizontalLaserFan(
-transform::GetYaw(pose_prediction), Eigen::Vector3d::UnitZ())) * -transform::GetYaw(pose_prediction), Eigen::Vector3d::UnitZ())) *
pose_prediction.rotation()); pose_prediction.rotation());
const sensor::LaserFan laser_fan_in_tracking_2d = const sensor::LaserFan3D laser_fan_in_tracking_2d =
BuildProjectedLaserFan(tracking_to_tracking_2d.cast<float>(), laser_fan); BuildCroppedLaserFan(tracking_to_tracking_2d.cast<float>(), 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."; LOG(WARNING) << "Dropped empty horizontal laser point cloud.";
return nullptr; return nullptr;
} }
@ -202,9 +198,8 @@ LocalTrajectoryBuilder::AddHorizontalLaserFan(
{pose_observation, covariance_observation}, {pose_observation, covariance_observation},
{scan_matcher_pose_estimate_, covariance_estimate}, {scan_matcher_pose_estimate_, covariance_estimate},
scan_matcher_pose_estimate_, scan_matcher_pose_estimate_,
sensor::TransformPointCloud( sensor::TransformPointCloud(laser_fan_in_tracking_2d.returns,
sensor::ToPointCloud(laser_fan_in_tracking_2d.point_cloud), tracking_2d_to_map.cast<float>())};
tracking_2d_to_map.cast<float>())};
const transform::Rigid2d pose_estimate_2d = const transform::Rigid2d pose_estimate_2d =
transform::Project2D(tracking_2d_to_map); transform::Project2D(tracking_2d_to_map);
@ -218,8 +213,8 @@ LocalTrajectoryBuilder::AddHorizontalLaserFan(
for (int insertion_index : submaps_.insertion_indices()) { for (int insertion_index : submaps_.insertion_indices()) {
insertion_submaps.push_back(submaps_.Get(insertion_index)); insertion_submaps.push_back(submaps_.Get(insertion_index));
} }
submaps_.InsertLaserFan(TransformLaserFan(laser_fan_in_tracking_2d, submaps_.InsertLaserFan(sensor::ProjectLaserFan(TransformLaserFan3D(
pose_estimate_2d.cast<float>())); laser_fan_in_tracking_2d, tracking_2d_to_map.cast<float>())));
return common::make_unique<InsertionResult>(InsertionResult{ return common::make_unique<InsertionResult>(InsertionResult{
time, &submaps_, matching_submap, insertion_submaps, time, &submaps_, matching_submap, insertion_submaps,

View File

@ -49,7 +49,7 @@ class LocalTrajectoryBuilder {
std::vector<const mapping::Submap*> insertion_submaps; std::vector<const mapping::Submap*> insertion_submaps;
transform::Rigid3d tracking_to_tracking_2d; transform::Rigid3d tracking_to_tracking_2d;
transform::Rigid3d tracking_2d_to_map; 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; transform::Rigid2d pose_estimate_2d;
kalman_filter::PoseCovariance covariance_estimate; kalman_filter::PoseCovariance covariance_estimate;
}; };
@ -75,9 +75,8 @@ class LocalTrajectoryBuilder {
kalman_filter::PoseTracker* pose_tracker() const; kalman_filter::PoseTracker* pose_tracker() const;
private: private:
// Transforms 'laser_scan', projects it onto the ground plane, // Transforms 'laser_scan', crops and voxel filters.
// crops and voxel filters. sensor::LaserFan3D BuildCroppedLaserFan(
sensor::LaserFan BuildProjectedLaserFan(
const transform::Rigid3f& tracking_to_tracking_2d, const transform::Rigid3f& tracking_to_tracking_2d,
const sensor::LaserFan3D& laser_fan) const; const sensor::LaserFan3D& laser_fan) const;
@ -85,7 +84,7 @@ class LocalTrajectoryBuilder {
// 'pose_observation' and 'covariance_observation' with the result. // 'pose_observation' and 'covariance_observation' with the result.
void ScanMatch(common::Time time, const transform::Rigid3d& pose_prediction, void ScanMatch(common::Time time, const transform::Rigid3d& pose_prediction,
const transform::Rigid3d& tracking_to_tracking_2d, 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, transform::Rigid3d* pose_observation,
kalman_filter::PoseCovariance* covariance_observation); kalman_filter::PoseCovariance* covariance_observation);

View File

@ -113,12 +113,16 @@ LaserFan3D FilterLaserFanByMaxRange(const LaserFan3D& laser_fan,
return result; return result;
} }
LaserFan ProjectCroppedLaserFan(const LaserFan3D& laser_fan, LaserFan3D CropLaserFan(const LaserFan3D& laser_fan, const float min_z,
const Eigen::Vector3f& min, const float max_z) {
const Eigen::Vector3f& max) { 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>(), return LaserFan{laser_fan.origin.head<2>(),
ProjectToPointCloud2D(Crop(laser_fan.returns, min, max)), ProjectToPointCloud2D(laser_fan.returns),
ProjectToPointCloud2D(Crop(laser_fan.misses, min, max))}; ProjectToPointCloud2D(laser_fan.misses)};
} }
CompressedLaserFan3D Compress(const LaserFan3D& laser_fan) { CompressedLaserFan3D Compress(const LaserFan3D& laser_fan) {

View File

@ -25,10 +25,6 @@
namespace cartographer { namespace cartographer {
namespace sensor { 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 { struct LaserFan {
Eigen::Vector2f origin; Eigen::Vector2f origin;
PointCloud2D point_cloud; PointCloud2D point_cloud;
@ -52,6 +48,10 @@ struct LaserFan3D {
std::vector<uint8> reflectivities; std::vector<uint8> 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, LaserFan3D ToLaserFan3D(const proto::LaserScan& proto, float min_range,
float max_range, float missing_echo_ray_length); float max_range, float missing_echo_ray_length);
@ -70,11 +70,11 @@ LaserFan3D TransformLaserFan3D(const LaserFan3D& laser_fan,
LaserFan3D FilterLaserFanByMaxRange(const LaserFan3D& laser_fan, LaserFan3D FilterLaserFanByMaxRange(const LaserFan3D& laser_fan,
float max_range); float max_range);
// Projects 'laser_fan' into 2D and crops it according to the cuboid defined by // Crops 'laser_fan' according to the region defined by 'min_z' and 'max_z'.
// 'min' and 'max'. LaserFan3D CropLaserFan(const LaserFan3D& laser_fan, float min_z, float max_z);
LaserFan ProjectCroppedLaserFan(const LaserFan3D& laser_fan,
const Eigen::Vector3f& min, // Projects 'laser_fan' into 2D.
const Eigen::Vector3f& max); LaserFan ProjectLaserFan(const LaserFan3D& laser_fan);
// Like LaserFan3D but with compressed point clouds. The point order changes // Like LaserFan3D but with compressed point clouds. The point order changes
// when converting from LaserFan3D. // when converting from LaserFan3D.

View File

@ -65,12 +65,11 @@ PointCloud2D ProjectToPointCloud2D(const PointCloud& point_cloud) {
return point_cloud_2d; return point_cloud_2d;
} }
PointCloud Crop(const PointCloud& point_cloud, const Eigen::Vector3f& min, PointCloud Crop(const PointCloud& point_cloud, const float min_z,
const Eigen::Vector3f& max) { const float max_z) {
PointCloud cropped_point_cloud; PointCloud cropped_point_cloud;
for (const auto& point : point_cloud) { for (const auto& point : point_cloud) {
if (min.x() <= point.x() && point.x() <= max.x() && min.y() <= point.y() && if (min_z <= point.z() && point.z() <= max_z) {
point.y() <= max.y() && min.z() <= point.z() && point.z() <= max.z()) {
cropped_point_cloud.push_back(point); cropped_point_cloud.push_back(point);
} }
} }

View File

@ -45,10 +45,9 @@ PointCloud ToPointCloud(const PointCloud2D& point_cloud_2d);
// Converts 'point_cloud' to a 2D point cloud by removing the z component. // Converts 'point_cloud' to a 2D point cloud by removing the z component.
PointCloud2D ProjectToPointCloud2D(const PointCloud& point_cloud); PointCloud2D ProjectToPointCloud2D(const PointCloud& point_cloud);
// Returns a new point cloud without points that fall outside the axis-aligned // Returns a new point cloud without points that fall outside the region defined
// cuboid defined by 'min' and 'max'. // by 'min_z' and 'max_z'.
PointCloud Crop(const PointCloud& point_cloud, const Eigen::Vector3f& min, PointCloud Crop(const PointCloud& point_cloud, float min_z, float max_z);
const Eigen::Vector3f& max);
// Converts 'point_cloud' to a proto::PointCloud. // Converts 'point_cloud' to a proto::PointCloud.
proto::PointCloud ToProto(const PointCloud& point_cloud); proto::PointCloud ToProto(const PointCloud& point_cloud);