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

Remove 'laser' references from parameter names. (#259) 

Related to #250.
master
Wolfgang Hess 2017-05-04 15:38:41 +02:00 committed by GitHub
parent 12b3cc0d7b
commit 79dc1f848f
18 changed files with 93 additions and 102 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
cartographer/io/outlier_removing_points_processor.cc
View File

@ -88,8 +88,8 @@ void OutlierRemovingPointsProcessor::ProcessInPhaseOne(
void OutlierRemovingPointsProcessor::ProcessInPhaseTwo(
const PointsBatch& batch) {
// TODO(whess): This samples every 'voxel_size' distance and could be improved
// by better ray casting, and also by marking the hits of the current laser
// fan to be excluded.
// by better ray casting, and also by marking the hits of the current range
// data to be excluded.
for (size_t i = 0; i < batch.points.size(); ++i) {
const Eigen::Vector3f delta = batch.points[i] - batch.origin;
const float length = delta.norm();

2
cartographer/kalman_filter/pose_tracker.h
View File

@ -65,7 +65,7 @@ proto::PoseTrackerOptions CreatePoseTrackerOptions(
common::LuaParameterDictionary* parameter_dictionary);
// A Kalman filter for a 3D state of position and orientation.
// Includes functions to update from IMU and laser scan matches.
// Includes functions to update from IMU and range data matches.
class PoseTracker {
public:
enum {

6
cartographer/mapping/trajectory_node.h
View File

@ -45,9 +45,9 @@ struct TrajectoryNode {
// doesn't seem like a good name for a Submaps*. Sort this out.
const Submaps* trajectory;
// Transform from the 3D 'tracking' frame to the 'pose' frame of the
// laser, which contains roll, pitch and height for 2D. In 3D this is
// always identity.
// Transform from the 3D 'tracking' frame to the 'pose' frame of the range
// data, which contains roll, pitch and height for 2D. In 3D this is always
// identity.
transform::Rigid3d tracking_to_pose;
};

34
cartographer/mapping_2d/local_trajectory_builder.cc
View File

@ -27,16 +27,14 @@ namespace mapping_2d {
proto::LocalTrajectoryBuilderOptions CreateLocalTrajectoryBuilderOptions(
common::LuaParameterDictionary* const parameter_dictionary) {
proto::LocalTrajectoryBuilderOptions options;
options.set_laser_min_range(
parameter_dictionary->GetDouble("laser_min_range"));
options.set_laser_max_range(
parameter_dictionary->GetDouble("laser_max_range"));
options.set_laser_min_z(parameter_dictionary->GetDouble("laser_min_z"));
options.set_laser_max_z(parameter_dictionary->GetDouble("laser_max_z"));
options.set_laser_missing_echo_ray_length(
parameter_dictionary->GetDouble("laser_missing_echo_ray_length"));
options.set_laser_voxel_filter_size(
parameter_dictionary->GetDouble("laser_voxel_filter_size"));
options.set_min_range(parameter_dictionary->GetDouble("min_range"));
options.set_max_range(parameter_dictionary->GetDouble("max_range"));
options.set_min_z(parameter_dictionary->GetDouble("min_z"));
options.set_max_z(parameter_dictionary->GetDouble("max_z"));
options.set_missing_data_ray_length(
parameter_dictionary->GetDouble("missing_data_ray_length"));
options.set_voxel_filter_size(
parameter_dictionary->GetDouble("voxel_filter_size"));
options.set_use_online_correlative_scan_matching(
parameter_dictionary->GetBool("use_online_correlative_scan_matching"));
*options.mutable_adaptive_voxel_filter_options() =
@ -86,25 +84,23 @@ sensor::RangeData LocalTrajectoryBuilder::TransformAndFilterRangeData(
sensor::RangeData returns_and_misses{range_data.origin, {}, {}};
for (const Eigen::Vector3f& hit : range_data.returns) {
const float range = (hit - range_data.origin).norm();
if (range >= options_.laser_min_range()) {
if (range <= options_.laser_max_range()) {
if (range >= options_.min_range()) {
if (range <= options_.max_range()) {
returns_and_misses.returns.push_back(hit);
} else {
returns_and_misses.misses.push_back(
range_data.origin + options_.laser_missing_echo_ray_length() *
range_data.origin + options_.missing_data_ray_length() *
(hit - range_data.origin).normalized());
}
}
}
const sensor::RangeData cropped = sensor::CropRangeData(
sensor::TransformRangeData(returns_and_misses, tracking_to_tracking_2d),
options_.laser_min_z(), options_.laser_max_z());
options_.min_z(), options_.max_z());
return sensor::RangeData{
cropped.origin,
sensor::VoxelFiltered(cropped.returns,
options_.laser_voxel_filter_size()),
sensor::VoxelFiltered(cropped.misses,
options_.laser_voxel_filter_size())};
sensor::VoxelFiltered(cropped.returns, options_.voxel_filter_size()),
sensor::VoxelFiltered(cropped.misses, options_.voxel_filter_size())};
}
void LocalTrajectoryBuilder::ScanMatch(
@ -158,7 +154,7 @@ LocalTrajectoryBuilder::AddHorizontalRangeData(
if (imu_tracker_ == nullptr) {
// Until we've initialized the IMU tracker with our first IMU message, we
// cannot compute the orientation of the laser scanner.
// cannot compute the orientation of the rangefinder.
LOG(INFO) << "ImuTracker not yet initialized.";
return nullptr;
}

4
cartographer/mapping_2d/map_limits.h
View File

@ -84,8 +84,8 @@ class MapLimits {
.all();
}
// Computes MapLimits that contain the origin, and all laser rays (both
// returns and misses) in the 'trajectory'.
// Computes MapLimits that contain the origin, and all rays (both returns and
// misses) in the 'trajectory'.
static MapLimits ComputeMapLimits(
const double resolution,
const std::vector<mapping::TrajectoryNode>& trajectory_nodes) {

17
cartographer/mapping_2d/proto/local_trajectory_builder_options.proto
View File

@ -23,19 +23,18 @@ import "cartographer/mapping_2d/scan_matching/proto/ceres_scan_matcher_options.p
import "cartographer/mapping_2d/scan_matching/proto/real_time_correlative_scan_matcher_options.proto";
message LocalTrajectoryBuilderOptions {
// Laser returns outside these ranges will be dropped.
optional float laser_min_range = 14;
optional float laser_max_range = 15;
optional float laser_min_z = 1;
optional float laser_max_z = 2;
// Rangefinder points outside these ranges will be dropped.
optional float min_range = 14;
optional float max_range = 15;
optional float min_z = 1;
optional float max_z = 2;
// Laser returns beyond 'laser_max_range' will be inserted with this length as
// empty space.
optional float laser_missing_echo_ray_length = 16;
// Points beyond 'max_range' will be inserted with this length as empty space.
optional float missing_data_ray_length = 16;
// Voxel filter that gets applied to the range data immediately after
// cropping.
optional float laser_voxel_filter_size = 3;
optional float voxel_filter_size = 3;
// Whether to solve the online scan matching first using the correlative scan
// matcher to generate a good starting point for Ceres.

16
cartographer/mapping_3d/kalman_local_trajectory_builder.cc
View File

@ -96,16 +96,16 @@ KalmanLocalTrajectoryBuilder::AddRangefinderData(
for (const Eigen::Vector3f& hit : range_data_in_first_tracking.returns) {
const Eigen::Vector3f delta = hit - range_data_in_first_tracking.origin;
const float range = delta.norm();
if (range >= options_.laser_min_range()) {
if (range <= options_.laser_max_range()) {
if (range >= options_.min_range()) {
if (range <= options_.max_range()) {
accumulated_range_data_.returns.push_back(hit);
} else {
// We insert a ray cropped to 'laser_max_range' as a miss for hits
// beyond the maximum range. This way the free space up to the maximum
// range will be updated.
// We insert a ray cropped to 'max_range' as a miss for hits beyond the
// maximum range. This way the free space up to the maximum range will
// be updated.
accumulated_range_data_.misses.push_back(
range_data_in_first_tracking.origin +
options_.laser_max_range() / range * delta);
options_.max_range() / range * delta);
}
}
}
@ -129,9 +129,9 @@ KalmanLocalTrajectoryBuilder::AddAccumulatedRangeData(
const sensor::RangeData filtered_range_data = {
range_data_in_tracking.origin,
sensor::VoxelFiltered(range_data_in_tracking.returns,
options_.laser_voxel_filter_size()),
options_.voxel_filter_size()),
sensor::VoxelFiltered(range_data_in_tracking.misses,
options_.laser_voxel_filter_size())};
options_.voxel_filter_size())};
if (filtered_range_data.returns.empty()) {
LOG(WARNING) << "Dropped empty range data.";

21
cartographer/mapping_3d/kalman_local_trajectory_builder_test.cc
View File

@ -47,10 +47,10 @@ class KalmanLocalTrajectoryBuilderTest : public ::testing::Test {
proto::LocalTrajectoryBuilderOptions CreateTrajectoryBuilderOptions() {
auto parameter_dictionary = common::MakeDictionary(R"text(
return {
laser_min_range = 0.5,
laser_max_range = 50.,
min_range = 0.5,
max_range = 50.,
scans_per_accumulation = 1,
laser_voxel_filter_size = 0.05,
voxel_filter_size = 0.05,
high_resolution_adaptive_voxel_filter = {
max_length = 0.7,
@ -191,15 +191,15 @@ class KalmanLocalTrajectoryBuilderTest : public ::testing::Test {
sensor::RangeData GenerateRangeData(const transform::Rigid3d& pose) {
// 360 degree rays at 16 angles.
sensor::PointCloud directions_in_laser_frame;
sensor::PointCloud directions_in_rangefinder_frame;
for (int r = -8; r != 8; ++r) {
for (int s = -250; s != 250; ++s) {
directions_in_laser_frame.push_back(
directions_in_rangefinder_frame.push_back(
Eigen::AngleAxisf(M_PI * s / 250., Eigen::Vector3f::UnitZ()) *
Eigen::AngleAxisf(M_PI / 12. * r / 8., Eigen::Vector3f::UnitY()) *
Eigen::Vector3f::UnitX());
// Second orthogonal laser.
directions_in_laser_frame.push_back(
// Second orthogonal rangefinder.
directions_in_rangefinder_frame.push_back(
Eigen::AngleAxisf(M_PI / 2., Eigen::Vector3f::UnitX()) *
Eigen::AngleAxisf(M_PI * s / 250., Eigen::Vector3f::UnitZ()) *
Eigen::AngleAxisf(M_PI / 12. * r / 8., Eigen::Vector3f::UnitY()) *
@ -210,13 +210,12 @@ class KalmanLocalTrajectoryBuilderTest : public ::testing::Test {
// 50 cm radius spheres.
sensor::PointCloud returns_in_world_frame;
for (const Eigen::Vector3f& direction_in_world_frame :
sensor::TransformPointCloud(directions_in_laser_frame,
sensor::TransformPointCloud(directions_in_rangefinder_frame,
pose.cast<float>())) {
const Eigen::Vector3f laser_origin =
const Eigen::Vector3f origin =
pose.cast<float>() * Eigen::Vector3f::Zero();
returns_in_world_frame.push_back(CollideWithBubbles(
laser_origin,
CollideWithBox(laser_origin, direction_in_world_frame)));
origin, CollideWithBox(origin, direction_in_world_frame)));
}
return {Eigen::Vector3f::Zero(),
sensor::TransformPointCloud(returns_in_world_frame,

10
cartographer/mapping_3d/local_trajectory_builder_options.cc
View File

@ -30,14 +30,12 @@ namespace mapping_3d {
proto::LocalTrajectoryBuilderOptions CreateLocalTrajectoryBuilderOptions(
common::LuaParameterDictionary* const parameter_dictionary) {
proto::LocalTrajectoryBuilderOptions options;
options.set_laser_min_range(
parameter_dictionary->GetDouble("laser_min_range"));
options.set_laser_max_range(
parameter_dictionary->GetDouble("laser_max_range"));
options.set_min_range(parameter_dictionary->GetDouble("min_range"));
options.set_max_range(parameter_dictionary->GetDouble("max_range"));
options.set_scans_per_accumulation(
parameter_dictionary->GetInt("scans_per_accumulation"));
options.set_laser_voxel_filter_size(
parameter_dictionary->GetDouble("laser_voxel_filter_size"));
options.set_voxel_filter_size(
parameter_dictionary->GetDouble("voxel_filter_size"));
*options.mutable_high_resolution_adaptive_voxel_filter_options() =
sensor::CreateAdaptiveVoxelFilterOptions(
parameter_dictionary

8
cartographer/mapping_3d/optimizing_local_trajectory_builder.cc
View File

@ -144,8 +144,8 @@ OptimizingLocalTrajectoryBuilder::AddRangefinderData(
for (const Eigen::Vector3f& hit : ranges) {
const Eigen::Vector3f delta = hit - origin;
const float range = delta.norm();
if (range >= options_.laser_min_range()) {
if (range <= options_.laser_max_range()) {
if (range >= options_.min_range()) {
if (range <= options_.max_range()) {
point_cloud.push_back(hit);
}
}
@ -361,9 +361,9 @@ OptimizingLocalTrajectoryBuilder::AddAccumulatedRangeData(
const sensor::RangeData filtered_range_data = {
range_data_in_tracking.origin,
sensor::VoxelFiltered(range_data_in_tracking.returns,
options_.laser_voxel_filter_size()),
options_.voxel_filter_size()),
sensor::VoxelFiltered(range_data_in_tracking.misses,
options_.laser_voxel_filter_size())};
options_.voxel_filter_size())};
if (filtered_range_data.returns.empty()) {
LOG(WARNING) << "Dropped empty range data.";

11
cartographer/mapping_3d/proto/local_trajectory_builder_options.proto
View File

@ -29,16 +29,17 @@ message LocalTrajectoryBuilderOptions {
OPTIMIZING = 1;
}
// Laser limits.
optional float laser_min_range = 1;
optional float laser_max_range = 2;
// Rangefinder points outside these ranges will be dropped.
optional float min_range = 1;
optional float max_range = 2;
// Number of scans to accumulate into one unwarped, combined scan to use for
// scan matching.
optional int32 scans_per_accumulation = 3;
// Voxel filter that gets applied to the laser before doing anything with it.
optional float laser_voxel_filter_size = 4;
// Voxel filter that gets applied to the range data immediately after
// cropping.
optional float voxel_filter_size = 4;
// Voxel filter used to compute a sparser point cloud for matching.
optional sensor.proto.AdaptiveVoxelFilterOptions

2
cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc
View File

@ -247,7 +247,7 @@ std::vector<DiscreteScan> FastCorrelativeScanMatcher::GenerateDiscreteScans(
const Eigen::Vector3f angle_axis(0.f, 0.f, angles[i]);
// It's important to apply the 'angle_axis' rotation between the translation
// and rotation of the 'initial_pose', so that the rotation is around the
// origin of the laser scanner, and yaw is in map frame.
// origin of the range data, and yaw is in map frame.
const transform::Rigid3f pose(
Eigen::Translation3f(initial_pose.translation()) *
transform::AngleAxisVectorToRotationQuaternion(angle_axis) *

13
cartographer/mapping_3d/scan_matching/rotational_scan_matcher.cc
View File

@ -68,11 +68,10 @@ void AddPointCloudSliceToValueVector(
if (slice.empty()) {
return;
}
// We compute the angle of the ray from the centroid to a laser return.
// If it is orthogonal to the angle we compute between laser returns, we will
// add the angle between laser returns to the histogram with the maximum
// weight. This is to reject, e.g., the angles observed on the ceiling and
// floor.
// We compute the angle of the ray from a point to the centroid of the whole
// point cloud. If it is orthogonal to the angle we compute between points, we
// will add the angle between points to the histogram with the maximum weight.
// This is to reject, e.g., the angles observed on the ceiling and floor.
const Eigen::Vector3f centroid = ComputeCentroid(slice);
Eigen::Vector3f last_point = slice.front();
for (const Eigen::Vector3f& point : slice) {
@ -94,8 +93,8 @@ void AddPointCloudSliceToValueVector(
}
// A function to sort the points in each slice by angle around the centroid.
// This is because the returns from different lasers are interleaved in the
// data.
// This is because the returns from different rangefinders are interleaved in
// the data.
sensor::PointCloud SortSlice(const sensor::PointCloud& slice) {
struct SortableAnglePointPair {
bool operator<(const SortableAnglePointPair& rhs) const {

2
cartographer/sensor/collator_test.cc
View File

@ -31,7 +31,7 @@ namespace {
TEST(Collator, Ordering) {
const std::array<string, 4> kSensorId = {
{"horizontal_laser", "vertical_laser", "imu", "odometry"}};
{"horizontal_rangefinder", "vertical_rangefinder", "imu", "odometry"}};
Data zero(common::FromUniversal(0), Data::Rangefinder{});
Data first(common::FromUniversal(100), Data::Rangefinder{});
Data second(common::FromUniversal(200), Data::Rangefinder{});

12
configuration_files/trajectory_builder_2d.lua
View File

@ -14,12 +14,12 @@
TRAJECTORY_BUILDER_2D = {
use_imu_data = true,
laser_min_range = 0.,
laser_max_range = 30.,
laser_min_z = -0.8,
laser_max_z = 2.,
laser_missing_echo_ray_length = 5.,
laser_voxel_filter_size = 0.025,
min_range = 0.,
max_range = 30.,
min_z = -0.8,
max_z = 2.,
missing_data_ray_length = 5.,
voxel_filter_size = 0.025,
use_online_correlative_scan_matching = false,
adaptive_voxel_filter = {

10
configuration_files/trajectory_builder_3d.lua
View File

@ -12,13 +12,13 @@
-- See the License for the specific language governing permissions and
-- limitations under the License.
MAX_3D_LASER_RANGE = 60.
MAX_3D_RANGE = 60.
TRAJECTORY_BUILDER_3D = {
laser_min_range = 1.,
laser_max_range = MAX_3D_LASER_RANGE,
min_range = 1.,
max_range = MAX_3D_RANGE,
scans_per_accumulation = 1,
laser_voxel_filter_size = 0.15,
voxel_filter_size = 0.15,
high_resolution_adaptive_voxel_filter = {
max_length = 2.,
@ -29,7 +29,7 @@ TRAJECTORY_BUILDER_3D = {
low_resolution_adaptive_voxel_filter = {
max_length = 4.,
min_num_points = 200,
max_range = MAX_3D_LASER_RANGE,
max_range = MAX_3D_RANGE,
},
ceres_scan_matcher = {

17
docs/source/configuration.rst
View File

@ -167,23 +167,22 @@ cartographer.common.proto.CeresSolverOptions ceres_solver_options
cartographer.mapping_2d.proto.LocalTrajectoryBuilderOptions
===========================================================
float laser_min_range
Laser returns outside these ranges will be dropped.
float min_range
Rangefinder points outside these ranges will be dropped.
float laser_max_range
float max_range
Not yet documented.
float laser_min_z
float min_z
Not yet documented.
float laser_max_z
float max_z
Not yet documented.
float laser_missing_echo_ray_length
Laser returns beyond 'laser_max_range' will be inserted with this length as
empty space.
float missing_data_ray_length
Points beyond 'max_range' will be inserted with this length as empty space.
float laser_voxel_filter_size
float voxel_filter_size
Voxel filter that gets applied to the range data immediately after
cropping.