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

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
cartographer/mapping_2d/global_trajectory_builder.cc
View File

@ -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,

39
cartographer/mapping_2d/local_trajectory_builder.cc
View File

@ -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<float>::infinity(),
-std::numeric_limits<float>::infinity(),
options_.horizontal_laser_min_z()),
Eigen::Vector3f(std::numeric_limits<float>::infinity(),
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_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<float>(), laser_fan);
const sensor::LaserFan3D laser_fan_in_tracking_2d =
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.";
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<float>())};
sensor::TransformPointCloud(laser_fan_in_tracking_2d.returns,
tracking_2d_to_map.cast<float>())};
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<float>()));
submaps_.InsertLaserFan(sensor::ProjectLaserFan(TransformLaserFan3D(
laser_fan_in_tracking_2d, tracking_2d_to_map.cast<float>())));
return common::make_unique<InsertionResult>(InsertionResult{
time, &submaps_, matching_submap, insertion_submaps,

9
cartographer/mapping_2d/local_trajectory_builder.h
View File

@ -49,7 +49,7 @@ class LocalTrajectoryBuilder {
std::vector<const mapping::Submap*> 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);

14
cartographer/sensor/laser.cc
View File

@ -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) {

18
cartographer/sensor/laser.h
View File

@ -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<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,
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.

7
cartographer/sensor/point_cloud.cc
View File

@ -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);
}
}

7
cartographer/sensor/point_cloud.h
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.
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);