Formatting changes. (#199)

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"
+  string color_header = !has_color ? ""
-                                          "property uchar green\n"
+                                   : "property uchar red\n"
-                                          "property uchar blue\n";
+                                     "property uchar green\n"
+                                     "property uchar blue\n";
   std::ostringstream stream;
   stream << "ply\n"
          << "format binary_little_endian 1.0\n"

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,
+    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, std::find(five_cluster->begin(), five_cluster->end(),
-    EXPECT_EQ(i > 4,
+                               trajectory(i)) != five_cluster->end());
-              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(
+    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) {
-    EXPECT_EQ(0,
+    EXPECT_EQ(0, trajectory_connectivity_.ConnectionCount(trajectory(i),
-              trajectory_connectivity_.ConnectionCount(trajectory(i),
+                                                          trajectory(i + 1)));
-                                                       trajectory(i + 1)));
   }
 }
 

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 +
+            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,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,
+      cropped.origin,
-                                            options_.laser_voxel_filter_size()),
+      sensor::VoxelFiltered(cropped.returns,
+                            options_.laser_voxel_filter_size()),
       sensor::VoxelFiltered(cropped.misses,
                             options_.laser_voxel_filter_size())};
 }

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) &&
+    return limits_.Contains(xy_index) && cells_[GetIndexOfCell(xy_index)] !=
-           cells_[GetIndexOfCell(xy_index)] !=
+                                             mapping::kUnknownProbabilityValue;
-               mapping::kUnknownProbabilityValue;
   }
 
   // Fills in 'offset' and 'limits' to define a subregion of that contains all

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 *
+      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 =
       std::ceil(angular_search_window / angular_perturbation_step_size);
   num_scans = 2 * num_angular_perturbations + 1;

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() -
+      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,
                                    min_score, score, pose_estimate);
 }

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(
+      EXPECT_NEAR(max_score,
-                                 precomputation_grid.GetValue(xy_index)),
+                  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(
+      EXPECT_NEAR(max_score,
-                                 precomputation_grid.GetValue(xy_index)),
+                  PrecomputationGrid::ToProbability(
+                      precomputation_grid.GetValue(xy_index)),
                   1e-4);
     }
   }

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() -
+                  << 100. *
-                             num_finished_scans_at_start) /
+                         (constraint_builder_.GetNumFinishedScans() -
+                          num_finished_scans_at_start) /
                          (trajectory_nodes_.size() -
                           num_finished_scans_at_start)
                   << "%...";

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(
+      time, pose,
-                      options_.kalman_local_trajectory_builder_options()
+      kalman_filter::BuildPoseCovariance(
-                          .odometer_translational_variance(),
+          options_.kalman_local_trajectory_builder_options()
-                      options_.kalman_local_trajectory_builder_options()
+              .odometer_translational_variance(),
-                          .odometer_rotational_variance()));
+          options_.kalman_local_trajectory_builder_options()
+              .odometer_rotational_variance()));
 }
 
 const KalmanLocalTrajectoryBuilder::PoseEstimate&

cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc

@@ -228,9 +228,8 @@ std::vector<DiscreteScan> FastCorrelativeScanMatcher::GenerateDiscreteScans(
   }
   const float kSafetyMargin = 1.f - 1e-2f;
   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(
       search_parameters.angular_search_window / angular_step_size);
   // TODO(whess): Should there be a small search window for rotations around

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.,
+          EXPECT_LT(0., grid_difference * (interpolated_grid_.GetProbability(
-                    grid_difference *
+                                               x + sample + kSampleStep, y, z) -
-                        (interpolated_grid_.GetProbability(
+                                           interpolated_grid_.GetProbability(
-                             x + sample + kSampleStep, y, z) -
+                                               x + sample, y, z)));
-                         interpolated_grid_.GetProbability(x + sample, y, z)));
         }
       }
     }

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 =
+            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)));
           }
         }
       }

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 -
+      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(options_.angular_search_window() / angular_step_size);
   for (int z = -linear_window_size; z <= linear_window_size; ++z) {

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{
+    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});
   }
 
   CHECK_LT(submap_states_.size(), std::numeric_limits<int>::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() -
+                  << 100. *
-                             num_finished_scans_at_start) /
+                         (constraint_builder_.GetNumFinishedScans() -
+                          num_finished_scans_at_start) /
                          (trajectory_nodes_.size() -
                           num_finished_scans_at_start)
                   << "%...";

cartographer/mapping_3d/sparse_pose_graph/optimization_problem_test.cc

@@ -132,22 +132,25 @@ TEST_F(OptimizationProblemTest, ReducesNoise) {
   std::vector<OptimizationProblem::Constraint> constraints;
   for (int j = 0; j != kNumNodes; ++j) {
     constraints.push_back(OptimizationProblem::Constraint{
-        0, j, OptimizationProblem::Constraint::Pose{
+        0, j,
-                  AddNoise(test_data[j].ground_truth_pose, test_data[j].noise),
+        OptimizationProblem::Constraint::Pose{
-                  Eigen::Matrix<double, 6, 6>::Identity()}});
+            AddNoise(test_data[j].ground_truth_pose, test_data[j].noise),
+            Eigen::Matrix<double, 6, 6>::Identity()}});
     // We add an additional independent, but equally noisy observation.
     constraints.push_back(OptimizationProblem::Constraint{
-        1, j, OptimizationProblem::Constraint::Pose{
+        1, j,
-                  AddNoise(test_data[j].ground_truth_pose,
+        OptimizationProblem::Constraint::Pose{
-                           RandomYawOnlyTransform(0.2, 0.3)),
+            AddNoise(test_data[j].ground_truth_pose,
-                  Eigen::Matrix<double, 6, 6>::Identity()}});
+                     RandomYawOnlyTransform(0.2, 0.3)),
+            Eigen::Matrix<double, 6, 6>::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{
+        2, j,
-                  kSubmap2Transform.inverse() * test_data[j].ground_truth_pose *
+        OptimizationProblem::Constraint::Pose{
-                      RandomTransform(1e3, 3.),
+            kSubmap2Transform.inverse() * test_data[j].ground_truth_pose *
-                  1e-9 * Eigen::Matrix<double, 6, 6>::Identity()}});
+                RandomTransform(1e3, 3.),
+            1e-9 * Eigen::Matrix<double, 6, 6>::Identity()}});
   }
 
   std::vector<transform::Rigid3d> submap_transforms = {

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{
+        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 {
       // 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{
+        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});
       }
     }
   }