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

Formatting changes. (#199)

master
Wolfgang Hess 2017-02-08 14:30:38 +01:00 committed by GitHub
parent ddb3c890a6
commit b4a1021538
16 changed files with 84 additions and 86 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
cartographer/io/ply_writing_points_processor.cc
View File

@ -34,7 +34,8 @@ namespace {
// 'output_file'. // 'output_file'.
void WriteBinaryPlyHeader(const bool has_color, const int64 num_points, void WriteBinaryPlyHeader(const bool has_color, const int64 num_points,
FileWriter* const file_writer) { FileWriter* const file_writer) {
string color_header = !has_color ? "" : "property uchar red\n" string color_header = !has_color ? ""
: "property uchar red\n"
"property uchar green\n" "property uchar green\n"
"property uchar blue\n"; "property uchar blue\n";
std::ostringstream stream; std::ostringstream stream;

14
cartographer/mapping/trajectory_connectivity_test.cc
View File

@ -110,11 +110,9 @@ TEST_F(TrajectoryConnectivityTest, ConnectedComponents) {
five_cluster = &connections[0]; five_cluster = &connections[0];
} }
for (int i = 0; i <= 9; ++i) { for (int i = 0; i <= 9; ++i) {
EXPECT_EQ(i <= 4, EXPECT_EQ(i <= 4, std::find(zero_cluster->begin(), zero_cluster->end(),
std::find(zero_cluster->begin(), zero_cluster->end(),
trajectory(i)) != zero_cluster->end()); trajectory(i)) != zero_cluster->end());
EXPECT_EQ(i > 4, EXPECT_EQ(i > 4, std::find(five_cluster->begin(), five_cluster->end(),
std::find(five_cluster->begin(), five_cluster->end(),
trajectory(i)) != five_cluster->end()); trajectory(i)) != five_cluster->end());
} }
} }
@ -123,13 +121,11 @@ TEST_F(TrajectoryConnectivityTest, ConnectionCount) {
for (int i = 0; i < 10; ++i) { for (int i = 0; i < 10; ++i) {
trajectory_connectivity_.Connect(trajectory(0), trajectory(1)); trajectory_connectivity_.Connect(trajectory(0), trajectory(1));
// Permute the arguments to check invariance. // Permute the arguments to check invariance.
EXPECT_EQ( EXPECT_EQ(i + 1, trajectory_connectivity_.ConnectionCount(trajectory(1),
i + 1, trajectory(0)));
trajectory_connectivity_.ConnectionCount(trajectory(1), trajectory(0)));
} }
for (int i = 1; i < 9; ++i) { for (int i = 1; i < 9; ++i) {
EXPECT_EQ(0, EXPECT_EQ(0, trajectory_connectivity_.ConnectionCount(trajectory(i),
trajectory_connectivity_.ConnectionCount(trajectory(i),
trajectory(i + 1))); trajectory(i + 1)));
} }
} }

6
cartographer/mapping_2d/local_trajectory_builder.cc
View File

@ -91,8 +91,7 @@ sensor::LaserFan LocalTrajectoryBuilder::TransformAndFilterLaserFan(
returns_and_misses.returns.push_back(return_); returns_and_misses.returns.push_back(return_);
} else { } else {
returns_and_misses.misses.push_back( returns_and_misses.misses.push_back(
laser_fan.origin + laser_fan.origin + options_.laser_missing_echo_ray_length() *
options_.laser_missing_echo_ray_length() *
(return_ - laser_fan.origin).normalized()); (return_ - laser_fan.origin).normalized());
} }
} }
@ -101,7 +100,8 @@ sensor::LaserFan LocalTrajectoryBuilder::TransformAndFilterLaserFan(
sensor::TransformLaserFan(returns_and_misses, tracking_to_tracking_2d), sensor::TransformLaserFan(returns_and_misses, tracking_to_tracking_2d),
options_.laser_min_z(), options_.laser_max_z()); options_.laser_min_z(), options_.laser_max_z());
return sensor::LaserFan{ return sensor::LaserFan{
cropped.origin, sensor::VoxelFiltered(cropped.returns, cropped.origin,
sensor::VoxelFiltered(cropped.returns,
options_.laser_voxel_filter_size()), options_.laser_voxel_filter_size()),
sensor::VoxelFiltered(cropped.misses, sensor::VoxelFiltered(cropped.misses,
options_.laser_voxel_filter_size())}; options_.laser_voxel_filter_size())};

3
cartographer/mapping_2d/probability_grid.h
View File

@ -106,8 +106,7 @@ class ProbabilityGrid {
// Returns true if the probability at the specified index is known. // Returns true if the probability at the specified index is known.
bool IsKnown(const Eigen::Array2i& xy_index) const { bool IsKnown(const Eigen::Array2i& xy_index) const {
return limits_.Contains(xy_index) && return limits_.Contains(xy_index) && cells_[GetIndexOfCell(xy_index)] !=
cells_[GetIndexOfCell(xy_index)] !=
mapping::kUnknownProbabilityValue; mapping::kUnknownProbabilityValue;
} }

5
cartographer/mapping_2d/scan_matching/correlative_scan_matcher.cc
View File

@ -38,9 +38,8 @@ SearchParameters::SearchParameters(const double linear_search_window,
} }
const double kSafetyMargin = 1. - 1e-3; const double kSafetyMargin = 1. - 1e-3;
angular_perturbation_step_size = angular_perturbation_step_size =
kSafetyMargin * kSafetyMargin * std::acos(1. - common::Pow2(resolution) /
std::acos(1. - (2. * common::Pow2(max_scan_range)));
common::Pow2(resolution) / (2. * common::Pow2(max_scan_range)));
num_angular_perturbations = num_angular_perturbations =
std::ceil(angular_search_window / angular_perturbation_step_size); std::ceil(angular_search_window / angular_perturbation_step_size);
num_scans = 2 * num_angular_perturbations + 1; num_scans = 2 * num_angular_perturbations + 1;

3
cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.cc
View File

@ -228,8 +228,7 @@ bool FastCorrelativeScanMatcher::MatchFullSubmap(
M_PI, // Angular search window, 180 degrees in both directions. M_PI, // Angular search window, 180 degrees in both directions.
point_cloud, limits_.resolution()); point_cloud, limits_.resolution());
const transform::Rigid2d center = transform::Rigid2d::Translation( const transform::Rigid2d center = transform::Rigid2d::Translation(
limits_.max() - limits_.max() - 0.5 * limits_.resolution() *
0.5 * limits_.resolution() *
Eigen::Vector2d(limits_.cell_limits().num_y_cells, Eigen::Vector2d(limits_.cell_limits().num_y_cells,
limits_.cell_limits().num_x_cells)); limits_.cell_limits().num_x_cells));
return MatchWithSearchParameters(search_parameters, center, point_cloud, return MatchWithSearchParameters(search_parameters, center, point_cloud,

6
cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_test.cc
View File

@ -63,7 +63,8 @@ TEST(PrecomputationGridTest, CorrectValues) {
probability_grid.GetProbability(xy_index + Eigen::Array2i(x, y))); probability_grid.GetProbability(xy_index + Eigen::Array2i(x, y)));
} }
} }
EXPECT_NEAR(max_score, PrecomputationGrid::ToProbability( EXPECT_NEAR(max_score,
PrecomputationGrid::ToProbability(
precomputation_grid.GetValue(xy_index)), precomputation_grid.GetValue(xy_index)),
1e-4); 1e-4);
} }
@ -97,7 +98,8 @@ TEST(PrecomputationGridTest, TinyProbabilityGrid) {
probability_grid.GetProbability(xy_index + Eigen::Array2i(x, y))); probability_grid.GetProbability(xy_index + Eigen::Array2i(x, y)));
} }
} }
EXPECT_NEAR(max_score, PrecomputationGrid::ToProbability( EXPECT_NEAR(max_score,
PrecomputationGrid::ToProbability(
precomputation_grid.GetValue(xy_index)), precomputation_grid.GetValue(xy_index)),
1e-4); 1e-4);
} }

3
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -302,7 +302,8 @@ void SparsePoseGraph::WaitForAllComputations() {
common::FromSeconds(1.))) { common::FromSeconds(1.))) {
std::ostringstream progress_info; std::ostringstream progress_info;
progress_info << "Optimizing: " << std::fixed << std::setprecision(1) progress_info << "Optimizing: " << std::fixed << std::setprecision(1)
<< 100. * (constraint_builder_.GetNumFinishedScans() - << 100. *
(constraint_builder_.GetNumFinishedScans() -
num_finished_scans_at_start) / num_finished_scans_at_start) /
(trajectory_nodes_.size() - (trajectory_nodes_.size() -
num_finished_scans_at_start) num_finished_scans_at_start)

3
cartographer/mapping_3d/kalman_local_trajectory_builder.cc
View File

@ -193,7 +193,8 @@ void KalmanLocalTrajectoryBuilder::AddOdometerData(
kalman_filter::PoseTracker::ModelFunction::k3D, time)); kalman_filter::PoseTracker::ModelFunction::k3D, time));
} }
pose_tracker_->AddOdometerPoseObservation( pose_tracker_->AddOdometerPoseObservation(
time, pose, kalman_filter::BuildPoseCovariance( time, pose,
kalman_filter::BuildPoseCovariance(
options_.kalman_local_trajectory_builder_options() options_.kalman_local_trajectory_builder_options()
.odometer_translational_variance(), .odometer_translational_variance(),
options_.kalman_local_trajectory_builder_options() options_.kalman_local_trajectory_builder_options()

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

@ -228,8 +228,7 @@ std::vector<DiscreteScan> FastCorrelativeScanMatcher::GenerateDiscreteScans(
} }
const float kSafetyMargin = 1.f - 1e-2f; const float kSafetyMargin = 1.f - 1e-2f;
const float angular_step_size = const float angular_step_size =
kSafetyMargin * std::acos(1.f - kSafetyMargin * std::acos(1.f - common::Pow2(resolution_) /
common::Pow2(resolution_) /
(2.f * common::Pow2(max_scan_range))); (2.f * common::Pow2(max_scan_range)));
const int angular_window_size = common::RoundToInt( const int angular_window_size = common::RoundToInt(
search_parameters.angular_search_window / angular_step_size); search_parameters.angular_search_window / angular_step_size);

7
cartographer/mapping_3d/scan_matching/interpolated_grid_test.cc
View File

@ -75,11 +75,10 @@ TEST_F(InterpolatedGridTest, MonotonicBehaviorBetweenGridPointsInX) {
for (double sample = kSampleStep; for (double sample = kSampleStep;
sample < hybrid_grid_.resolution() - 2 * kSampleStep; sample < hybrid_grid_.resolution() - 2 * kSampleStep;
sample += kSampleStep) { sample += kSampleStep) {
EXPECT_LT(0., EXPECT_LT(0., grid_difference * (interpolated_grid_.GetProbability(
grid_difference *
(interpolated_grid_.GetProbability(
x + sample + kSampleStep, y, z) - x + sample + kSampleStep, y, z) -
interpolated_grid_.GetProbability(x + sample, y, z))); interpolated_grid_.GetProbability(
x + sample, y, z)));
} }
} }
} }

5
cartographer/mapping_3d/scan_matching/precomputation_grid_test.cc
View File

@ -61,9 +61,8 @@ TEST(PrecomputedGridGeneratorTest, TestAgainstNaiveAlgorithm) {
for (int dx = 0; dx < width; ++dx) { for (int dx = 0; dx < width; ++dx) {
for (int dy = 0; dy < width; ++dy) { for (int dy = 0; dy < width; ++dy) {
for (int dz = 0; dz < width; ++dz) { for (int dz = 0; dz < width; ++dz) {
max_probability = max_probability = std::max(
std::max(max_probability, max_probability, hybrid_grid.GetProbability(
hybrid_grid.GetProbability(
Eigen::Array3i(x + dx, y + dy, z + dz))); Eigen::Array3i(x + dx, y + dy, z + dz)));
} }
} }

3
cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher.cc
View File

@ -68,8 +68,7 @@ RealTimeCorrelativeScanMatcher::GenerateExhaustiveSearchTransforms(
} }
const float kSafetyMargin = 1.f - 1e-3f; const float kSafetyMargin = 1.f - 1e-3f;
const float angular_step_size = const float angular_step_size =
kSafetyMargin * std::acos(1.f - kSafetyMargin * std::acos(1.f - common::Pow2(resolution) /
common::Pow2(resolution) /
(2.f * common::Pow2(max_scan_range))); (2.f * common::Pow2(max_scan_range)));
const int angular_window_size = const int angular_window_size =
common::RoundToInt(options_.angular_search_window() / angular_step_size); common::RoundToInt(options_.angular_search_window() / angular_step_size);

13
cartographer/mapping_3d/sparse_pose_graph.cc
View File

@ -215,12 +215,12 @@ void SparsePoseGraph::ComputeConstraintsForScan(
// Unchanged covariance as (submap <- map) is a translation. // Unchanged covariance as (submap <- map) is a translation.
const transform::Rigid3d constraint_transform = const transform::Rigid3d constraint_transform =
submap->local_pose().inverse() * pose; submap->local_pose().inverse() * pose;
constraints_.push_back(Constraint{ constraints_.push_back(
submap_index, Constraint{submap_index,
scan_index, scan_index,
{constraint_transform, common::ComputeSpdMatrixSqrtInverse( {constraint_transform,
covariance, common::ComputeSpdMatrixSqrtInverse(
options_.constraint_builder_options() covariance, options_.constraint_builder_options()
.lower_covariance_eigenvalue_bound())}, .lower_covariance_eigenvalue_bound())},
Constraint::INTRA_SUBMAP}); Constraint::INTRA_SUBMAP});
} }
@ -293,7 +293,8 @@ void SparsePoseGraph::WaitForAllComputations() {
common::FromSeconds(1.))) { common::FromSeconds(1.))) {
std::ostringstream progress_info; std::ostringstream progress_info;
progress_info << "Optimizing: " << std::fixed << std::setprecision(1) progress_info << "Optimizing: " << std::fixed << std::setprecision(1)
<< 100. * (constraint_builder_.GetNumFinishedScans() - << 100. *
(constraint_builder_.GetNumFinishedScans() -
num_finished_scans_at_start) / num_finished_scans_at_start) /
(trajectory_nodes_.size() - (trajectory_nodes_.size() -
num_finished_scans_at_start) num_finished_scans_at_start)

9
cartographer/mapping_3d/sparse_pose_graph/optimization_problem_test.cc
View File

@ -132,19 +132,22 @@ TEST_F(OptimizationProblemTest, ReducesNoise) {
std::vector<OptimizationProblem::Constraint> constraints; std::vector<OptimizationProblem::Constraint> constraints;
for (int j = 0; j != kNumNodes; ++j) { for (int j = 0; j != kNumNodes; ++j) {
constraints.push_back(OptimizationProblem::Constraint{ constraints.push_back(OptimizationProblem::Constraint{
0, j, OptimizationProblem::Constraint::Pose{ 0, j,
OptimizationProblem::Constraint::Pose{
AddNoise(test_data[j].ground_truth_pose, test_data[j].noise), AddNoise(test_data[j].ground_truth_pose, test_data[j].noise),
Eigen::Matrix<double, 6, 6>::Identity()}}); Eigen::Matrix<double, 6, 6>::Identity()}});
// We add an additional independent, but equally noisy observation. // We add an additional independent, but equally noisy observation.
constraints.push_back(OptimizationProblem::Constraint{ constraints.push_back(OptimizationProblem::Constraint{
1, j, OptimizationProblem::Constraint::Pose{ 1, j,
OptimizationProblem::Constraint::Pose{
AddNoise(test_data[j].ground_truth_pose, AddNoise(test_data[j].ground_truth_pose,
RandomYawOnlyTransform(0.2, 0.3)), RandomYawOnlyTransform(0.2, 0.3)),
Eigen::Matrix<double, 6, 6>::Identity()}}); Eigen::Matrix<double, 6, 6>::Identity()}});
// We add very noisy data with high covariance (i.e. small Lambda) to verify // We add very noisy data with high covariance (i.e. small Lambda) to verify
// it is mostly ignored. // it is mostly ignored.
constraints.push_back(OptimizationProblem::Constraint{ constraints.push_back(OptimizationProblem::Constraint{
2, j, OptimizationProblem::Constraint::Pose{ 2, j,
OptimizationProblem::Constraint::Pose{
kSubmap2Transform.inverse() * test_data[j].ground_truth_pose * kSubmap2Transform.inverse() * test_data[j].ground_truth_pose *
RandomTransform(1e3, 3.), RandomTransform(1e3, 3.),
1e-9 * Eigen::Matrix<double, 6, 6>::Identity()}}); 1e-9 * Eigen::Matrix<double, 6, 6>::Identity()}});

14
cartographer/mapping_3d/submaps.cc
View File

@ -82,10 +82,10 @@ void GenerateSegmentForSlice(const sensor::LaserFan& laser_fan,
segments->push_back(LaserSegment{laser_origin_xy, hit.head<2>(), true}); segments->push_back(LaserSegment{laser_origin_xy, hit.head<2>(), true});
} else { } else {
// Laser hit is above. // Laser hit is above.
segments->push_back(LaserSegment{ segments->push_back(
laser_origin_xy, LaserSegment{laser_origin_xy,
laser_origin_xy + laser_origin_xy + (kSliceHalfHeight - laser_origin_z) /
(kSliceHalfHeight - laser_origin_z) / delta_z * delta_xy, delta_z * delta_xy,
false}); false});
} }
} else { } else {
@ -100,9 +100,9 @@ void GenerateSegmentForSlice(const sensor::LaserFan& laser_fan,
false}); false});
} else if (hit.z() < kSliceHalfHeight) { } else if (hit.z() < kSliceHalfHeight) {
// Laser return is inside the slice. // Laser return is inside the slice.
segments->push_back(LaserSegment{ segments->push_back(
laser_origin_xy + LaserSegment{laser_origin_xy + (kSliceHalfHeight - laser_origin_z) /
(kSliceHalfHeight - laser_origin_z) / delta_z * delta_xy, delta_z * delta_xy,
hit.head<2>(), true}); hit.head<2>(), true});
} }
} }