diff --git a/cartographer/io/ply_writing_points_processor.cc b/cartographer/io/ply_writing_points_processor.cc index 0dc4195..b4aa4ad 100644 --- a/cartographer/io/ply_writing_points_processor.cc +++ b/cartographer/io/ply_writing_points_processor.cc @@ -34,9 +34,10 @@ namespace { // 'output_file'. void WriteBinaryPlyHeader(const bool has_color, const int64 num_points, FileWriter* const file_writer) { - string color_header = !has_color ? "" : "property uchar red\n" - "property uchar green\n" - "property uchar blue\n"; + string color_header = !has_color ? "" + : "property uchar red\n" + "property uchar green\n" + "property uchar blue\n"; std::ostringstream stream; stream << "ply\n" << "format binary_little_endian 1.0\n" diff --git a/cartographer/mapping/trajectory_connectivity_test.cc b/cartographer/mapping/trajectory_connectivity_test.cc index 15a7a75..7b2c780 100644 --- a/cartographer/mapping/trajectory_connectivity_test.cc +++ b/cartographer/mapping/trajectory_connectivity_test.cc @@ -110,12 +110,10 @@ TEST_F(TrajectoryConnectivityTest, ConnectedComponents) { five_cluster = &connections[0]; } for (int i = 0; i <= 9; ++i) { - EXPECT_EQ(i <= 4, - std::find(zero_cluster->begin(), zero_cluster->end(), - trajectory(i)) != zero_cluster->end()); - EXPECT_EQ(i > 4, - std::find(five_cluster->begin(), five_cluster->end(), - trajectory(i)) != five_cluster->end()); + EXPECT_EQ(i <= 4, std::find(zero_cluster->begin(), zero_cluster->end(), + trajectory(i)) != zero_cluster->end()); + EXPECT_EQ(i > 4, std::find(five_cluster->begin(), five_cluster->end(), + trajectory(i)) != five_cluster->end()); } } @@ -123,14 +121,12 @@ TEST_F(TrajectoryConnectivityTest, ConnectionCount) { for (int i = 0; i < 10; ++i) { trajectory_connectivity_.Connect(trajectory(0), trajectory(1)); // Permute the arguments to check invariance. - EXPECT_EQ( - i + 1, - trajectory_connectivity_.ConnectionCount(trajectory(1), trajectory(0))); + EXPECT_EQ(i + 1, trajectory_connectivity_.ConnectionCount(trajectory(1), + trajectory(0))); } for (int i = 1; i < 9; ++i) { - EXPECT_EQ(0, - trajectory_connectivity_.ConnectionCount(trajectory(i), - trajectory(i + 1))); + EXPECT_EQ(0, trajectory_connectivity_.ConnectionCount(trajectory(i), + trajectory(i + 1))); } } diff --git a/cartographer/mapping_2d/local_trajectory_builder.cc b/cartographer/mapping_2d/local_trajectory_builder.cc index efee7dd..f282239 100644 --- a/cartographer/mapping_2d/local_trajectory_builder.cc +++ b/cartographer/mapping_2d/local_trajectory_builder.cc @@ -91,9 +91,8 @@ sensor::LaserFan LocalTrajectoryBuilder::TransformAndFilterLaserFan( returns_and_misses.returns.push_back(return_); } else { returns_and_misses.misses.push_back( - laser_fan.origin + - options_.laser_missing_echo_ray_length() * - (return_ - laser_fan.origin).normalized()); + laser_fan.origin + options_.laser_missing_echo_ray_length() * + (return_ - laser_fan.origin).normalized()); } } } @@ -101,8 +100,9 @@ sensor::LaserFan LocalTrajectoryBuilder::TransformAndFilterLaserFan( sensor::TransformLaserFan(returns_and_misses, tracking_to_tracking_2d), options_.laser_min_z(), options_.laser_max_z()); return sensor::LaserFan{ - cropped.origin, sensor::VoxelFiltered(cropped.returns, - options_.laser_voxel_filter_size()), + cropped.origin, + sensor::VoxelFiltered(cropped.returns, + options_.laser_voxel_filter_size()), sensor::VoxelFiltered(cropped.misses, options_.laser_voxel_filter_size())}; } diff --git a/cartographer/mapping_2d/probability_grid.h b/cartographer/mapping_2d/probability_grid.h index c6ebdaf..7ae4cdd 100644 --- a/cartographer/mapping_2d/probability_grid.h +++ b/cartographer/mapping_2d/probability_grid.h @@ -106,9 +106,8 @@ class ProbabilityGrid { // Returns true if the probability at the specified index is known. bool IsKnown(const Eigen::Array2i& xy_index) const { - return limits_.Contains(xy_index) && - cells_[GetIndexOfCell(xy_index)] != - mapping::kUnknownProbabilityValue; + return limits_.Contains(xy_index) && cells_[GetIndexOfCell(xy_index)] != + mapping::kUnknownProbabilityValue; } // Fills in 'offset' and 'limits' to define a subregion of that contains all diff --git a/cartographer/mapping_2d/scan_matching/correlative_scan_matcher.cc b/cartographer/mapping_2d/scan_matching/correlative_scan_matcher.cc index c5b985a..10fa355 100644 --- a/cartographer/mapping_2d/scan_matching/correlative_scan_matcher.cc +++ b/cartographer/mapping_2d/scan_matching/correlative_scan_matcher.cc @@ -38,9 +38,8 @@ SearchParameters::SearchParameters(const double linear_search_window, } const double kSafetyMargin = 1. - 1e-3; angular_perturbation_step_size = - kSafetyMargin * - std::acos(1. - - common::Pow2(resolution) / (2. * common::Pow2(max_scan_range))); + kSafetyMargin * std::acos(1. - common::Pow2(resolution) / + (2. * common::Pow2(max_scan_range))); num_angular_perturbations = std::ceil(angular_search_window / angular_perturbation_step_size); num_scans = 2 * num_angular_perturbations + 1; diff --git a/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.cc b/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.cc index d73dc81..823394b 100644 --- a/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.cc +++ b/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.cc @@ -228,10 +228,9 @@ bool FastCorrelativeScanMatcher::MatchFullSubmap( M_PI, // Angular search window, 180 degrees in both directions. point_cloud, limits_.resolution()); const transform::Rigid2d center = transform::Rigid2d::Translation( - limits_.max() - - 0.5 * limits_.resolution() * - Eigen::Vector2d(limits_.cell_limits().num_y_cells, - limits_.cell_limits().num_x_cells)); + limits_.max() - 0.5 * limits_.resolution() * + Eigen::Vector2d(limits_.cell_limits().num_y_cells, + limits_.cell_limits().num_x_cells)); return MatchWithSearchParameters(search_parameters, center, point_cloud, min_score, score, pose_estimate); } diff --git a/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_test.cc b/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_test.cc index e7ca0e9..cab2044 100644 --- a/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_test.cc +++ b/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_test.cc @@ -63,8 +63,9 @@ TEST(PrecomputationGridTest, CorrectValues) { probability_grid.GetProbability(xy_index + Eigen::Array2i(x, y))); } } - EXPECT_NEAR(max_score, PrecomputationGrid::ToProbability( - precomputation_grid.GetValue(xy_index)), + EXPECT_NEAR(max_score, + PrecomputationGrid::ToProbability( + precomputation_grid.GetValue(xy_index)), 1e-4); } } @@ -97,8 +98,9 @@ TEST(PrecomputationGridTest, TinyProbabilityGrid) { probability_grid.GetProbability(xy_index + Eigen::Array2i(x, y))); } } - EXPECT_NEAR(max_score, PrecomputationGrid::ToProbability( - precomputation_grid.GetValue(xy_index)), + EXPECT_NEAR(max_score, + PrecomputationGrid::ToProbability( + precomputation_grid.GetValue(xy_index)), 1e-4); } } diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index e4440dd..56473b0 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -302,8 +302,9 @@ void SparsePoseGraph::WaitForAllComputations() { common::FromSeconds(1.))) { std::ostringstream progress_info; progress_info << "Optimizing: " << std::fixed << std::setprecision(1) - << 100. * (constraint_builder_.GetNumFinishedScans() - - num_finished_scans_at_start) / + << 100. * + (constraint_builder_.GetNumFinishedScans() - + num_finished_scans_at_start) / (trajectory_nodes_.size() - num_finished_scans_at_start) << "%..."; diff --git a/cartographer/mapping_3d/kalman_local_trajectory_builder.cc b/cartographer/mapping_3d/kalman_local_trajectory_builder.cc index 96fd6c4..8299a2c 100644 --- a/cartographer/mapping_3d/kalman_local_trajectory_builder.cc +++ b/cartographer/mapping_3d/kalman_local_trajectory_builder.cc @@ -193,11 +193,12 @@ void KalmanLocalTrajectoryBuilder::AddOdometerData( kalman_filter::PoseTracker::ModelFunction::k3D, time)); } pose_tracker_->AddOdometerPoseObservation( - time, pose, kalman_filter::BuildPoseCovariance( - options_.kalman_local_trajectory_builder_options() - .odometer_translational_variance(), - options_.kalman_local_trajectory_builder_options() - .odometer_rotational_variance())); + time, pose, + kalman_filter::BuildPoseCovariance( + options_.kalman_local_trajectory_builder_options() + .odometer_translational_variance(), + options_.kalman_local_trajectory_builder_options() + .odometer_rotational_variance())); } const KalmanLocalTrajectoryBuilder::PoseEstimate& diff --git a/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc b/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc index ec705a3..c050851 100644 --- a/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc +++ b/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc @@ -228,9 +228,8 @@ std::vector FastCorrelativeScanMatcher::GenerateDiscreteScans( } const float kSafetyMargin = 1.f - 1e-2f; const float angular_step_size = - kSafetyMargin * std::acos(1.f - - common::Pow2(resolution_) / - (2.f * common::Pow2(max_scan_range))); + kSafetyMargin * std::acos(1.f - common::Pow2(resolution_) / + (2.f * common::Pow2(max_scan_range))); const int angular_window_size = common::RoundToInt( search_parameters.angular_search_window / angular_step_size); // TODO(whess): Should there be a small search window for rotations around diff --git a/cartographer/mapping_3d/scan_matching/interpolated_grid_test.cc b/cartographer/mapping_3d/scan_matching/interpolated_grid_test.cc index 8bd1f63..e7c2478 100644 --- a/cartographer/mapping_3d/scan_matching/interpolated_grid_test.cc +++ b/cartographer/mapping_3d/scan_matching/interpolated_grid_test.cc @@ -75,11 +75,10 @@ TEST_F(InterpolatedGridTest, MonotonicBehaviorBetweenGridPointsInX) { for (double sample = kSampleStep; sample < hybrid_grid_.resolution() - 2 * kSampleStep; sample += kSampleStep) { - EXPECT_LT(0., - grid_difference * - (interpolated_grid_.GetProbability( - x + sample + kSampleStep, y, z) - - interpolated_grid_.GetProbability(x + sample, y, z))); + EXPECT_LT(0., grid_difference * (interpolated_grid_.GetProbability( + x + sample + kSampleStep, y, z) - + interpolated_grid_.GetProbability( + x + sample, y, z))); } } } diff --git a/cartographer/mapping_3d/scan_matching/precomputation_grid_test.cc b/cartographer/mapping_3d/scan_matching/precomputation_grid_test.cc index 894fa2f..1ff78c7 100644 --- a/cartographer/mapping_3d/scan_matching/precomputation_grid_test.cc +++ b/cartographer/mapping_3d/scan_matching/precomputation_grid_test.cc @@ -61,10 +61,9 @@ TEST(PrecomputedGridGeneratorTest, TestAgainstNaiveAlgorithm) { for (int dx = 0; dx < width; ++dx) { for (int dy = 0; dy < width; ++dy) { for (int dz = 0; dz < width; ++dz) { - max_probability = - std::max(max_probability, - hybrid_grid.GetProbability( - Eigen::Array3i(x + dx, y + dy, z + dz))); + max_probability = std::max( + max_probability, hybrid_grid.GetProbability( + Eigen::Array3i(x + dx, y + dy, z + dz))); } } } diff --git a/cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher.cc b/cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher.cc index 9275aa7..3ca643f 100644 --- a/cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher.cc +++ b/cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher.cc @@ -68,9 +68,8 @@ RealTimeCorrelativeScanMatcher::GenerateExhaustiveSearchTransforms( } const float kSafetyMargin = 1.f - 1e-3f; const float angular_step_size = - kSafetyMargin * std::acos(1.f - - common::Pow2(resolution) / - (2.f * common::Pow2(max_scan_range))); + kSafetyMargin * std::acos(1.f - common::Pow2(resolution) / + (2.f * common::Pow2(max_scan_range))); const int angular_window_size = common::RoundToInt(options_.angular_search_window() / angular_step_size); for (int z = -linear_window_size; z <= linear_window_size; ++z) { diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc index 2f2cd19..9270008 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/cartographer/mapping_3d/sparse_pose_graph.cc @@ -215,14 +215,14 @@ void SparsePoseGraph::ComputeConstraintsForScan( // Unchanged covariance as (submap <- map) is a translation. const transform::Rigid3d constraint_transform = submap->local_pose().inverse() * pose; - constraints_.push_back(Constraint{ - submap_index, - scan_index, - {constraint_transform, common::ComputeSpdMatrixSqrtInverse( - covariance, - options_.constraint_builder_options() - .lower_covariance_eigenvalue_bound())}, - Constraint::INTRA_SUBMAP}); + constraints_.push_back( + Constraint{submap_index, + scan_index, + {constraint_transform, + common::ComputeSpdMatrixSqrtInverse( + covariance, options_.constraint_builder_options() + .lower_covariance_eigenvalue_bound())}, + Constraint::INTRA_SUBMAP}); } CHECK_LT(submap_states_.size(), std::numeric_limits::max()); @@ -293,8 +293,9 @@ void SparsePoseGraph::WaitForAllComputations() { common::FromSeconds(1.))) { std::ostringstream progress_info; progress_info << "Optimizing: " << std::fixed << std::setprecision(1) - << 100. * (constraint_builder_.GetNumFinishedScans() - - num_finished_scans_at_start) / + << 100. * + (constraint_builder_.GetNumFinishedScans() - + num_finished_scans_at_start) / (trajectory_nodes_.size() - num_finished_scans_at_start) << "%..."; diff --git a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem_test.cc b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem_test.cc index 2ed6c48..1db9d13 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem_test.cc +++ b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem_test.cc @@ -132,22 +132,25 @@ TEST_F(OptimizationProblemTest, ReducesNoise) { std::vector constraints; for (int j = 0; j != kNumNodes; ++j) { constraints.push_back(OptimizationProblem::Constraint{ - 0, j, OptimizationProblem::Constraint::Pose{ - AddNoise(test_data[j].ground_truth_pose, test_data[j].noise), - Eigen::Matrix::Identity()}}); + 0, j, + OptimizationProblem::Constraint::Pose{ + AddNoise(test_data[j].ground_truth_pose, test_data[j].noise), + Eigen::Matrix::Identity()}}); // We add an additional independent, but equally noisy observation. constraints.push_back(OptimizationProblem::Constraint{ - 1, j, OptimizationProblem::Constraint::Pose{ - AddNoise(test_data[j].ground_truth_pose, - RandomYawOnlyTransform(0.2, 0.3)), - Eigen::Matrix::Identity()}}); + 1, j, + OptimizationProblem::Constraint::Pose{ + AddNoise(test_data[j].ground_truth_pose, + RandomYawOnlyTransform(0.2, 0.3)), + Eigen::Matrix::Identity()}}); // We add very noisy data with high covariance (i.e. small Lambda) to verify // it is mostly ignored. constraints.push_back(OptimizationProblem::Constraint{ - 2, j, OptimizationProblem::Constraint::Pose{ - kSubmap2Transform.inverse() * test_data[j].ground_truth_pose * - RandomTransform(1e3, 3.), - 1e-9 * Eigen::Matrix::Identity()}}); + 2, j, + OptimizationProblem::Constraint::Pose{ + kSubmap2Transform.inverse() * test_data[j].ground_truth_pose * + RandomTransform(1e3, 3.), + 1e-9 * Eigen::Matrix::Identity()}}); } std::vector submap_transforms = { diff --git a/cartographer/mapping_3d/submaps.cc b/cartographer/mapping_3d/submaps.cc index 8b1842b..b1a5d13 100644 --- a/cartographer/mapping_3d/submaps.cc +++ b/cartographer/mapping_3d/submaps.cc @@ -82,11 +82,11 @@ void GenerateSegmentForSlice(const sensor::LaserFan& laser_fan, segments->push_back(LaserSegment{laser_origin_xy, hit.head<2>(), true}); } else { // Laser hit is above. - segments->push_back(LaserSegment{ - laser_origin_xy, - laser_origin_xy + - (kSliceHalfHeight - laser_origin_z) / delta_z * delta_xy, - false}); + segments->push_back( + LaserSegment{laser_origin_xy, + laser_origin_xy + (kSliceHalfHeight - laser_origin_z) / + delta_z * delta_xy, + false}); } } else { // Laser ray originates above the slice. @@ -100,10 +100,10 @@ void GenerateSegmentForSlice(const sensor::LaserFan& laser_fan, false}); } else if (hit.z() < kSliceHalfHeight) { // Laser return is inside the slice. - segments->push_back(LaserSegment{ - laser_origin_xy + - (kSliceHalfHeight - laser_origin_z) / delta_z * delta_xy, - hit.head<2>(), true}); + segments->push_back( + LaserSegment{laser_origin_xy + (kSliceHalfHeight - laser_origin_z) / + delta_z * delta_xy, + hit.head<2>(), true}); } } }