Remove dead code. (#26)

cartographer/mapping/map_builder.cc

@@ -30,7 +30,6 @@
 #include "cartographer/sensor/voxel_filter.h"
 #include "cartographer/transform/rigid_transform.h"
 #include "cartographer/transform/transform.h"
-#include "glog/log_severity.h"
 #include "glog/logging.h"
 
 namespace cartographer {

cartographer/mapping_2d/local_trajectory_builder.cc

@@ -115,10 +115,9 @@ void LocalTrajectoryBuilder::ScanMatch(
   const sensor::PointCloud2D filtered_point_cloud_in_tracking_2d =
       adaptive_voxel_filter.Filter(laser_fan_in_tracking_2d.point_cloud);
   if (options_.use_online_correlative_scan_matching()) {
-    kalman_filter::Pose2DCovariance unused_covariance_observation;
     real_time_correlative_scan_matcher_.Match(
         pose_prediction_2d, filtered_point_cloud_in_tracking_2d,
-        probability_grid, &initial_ceres_pose, &unused_covariance_observation);
+        probability_grid, &initial_ceres_pose);
   }
 
   transform::Rigid2d tracking_2d_to_map;

cartographer/mapping_2d/scan_matching/CMakeLists.txt

@@ -99,7 +99,6 @@ google_library(mapping_2d_scan_matching_real_time_correlative_scan_matcher
   DEPENDS
     common_lua_parameter_dictionary
     common_math
-    kalman_filter_pose_tracker
     mapping_2d_probability_grid
     mapping_2d_scan_matching_correlative_scan_matcher
     mapping_2d_scan_matching_proto_real_time_correlative_scan_matcher_options

cartographer/mapping_2d/scan_matching/proto/real_time_correlative_scan_matcher_options.proto

@@ -28,7 +28,4 @@ message RealTimeCorrelativeScanMatcherOptions {
   // Weights applied to each part of the score.
   optional double translation_delta_cost_weight = 3;
   optional double rotation_delta_cost_weight = 4;
-
-  // Covariance estimate is multiplied by this scale.
-  optional double covariance_scale = 5;
 }

cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.cc

@@ -24,7 +24,6 @@
 #include "Eigen/Geometry"
 #include "cartographer/common/lua_parameter_dictionary.h"
 #include "cartographer/common/math.h"
-#include "cartographer/kalman_filter/pose_tracker.h"
 #include "cartographer/mapping_2d/probability_grid.h"
 #include "cartographer/sensor/point_cloud.h"
 #include "cartographer/transform/transform.h"
@@ -46,8 +45,6 @@ CreateRealTimeCorrelativeScanMatcherOptions(
       parameter_dictionary->GetDouble("translation_delta_cost_weight"));
   options.set_rotation_delta_cost_weight(
       parameter_dictionary->GetDouble("rotation_delta_cost_weight"));
-  options.set_covariance_scale(
-      parameter_dictionary->GetDouble("covariance_scale"));
   CHECK_GE(options.translation_delta_cost_weight(), 0.);
   CHECK_GE(options.rotation_delta_cost_weight(), 0.);
   return options;
@@ -94,10 +91,9 @@ RealTimeCorrelativeScanMatcher::GenerateExhaustiveSearchCandidates(
 double RealTimeCorrelativeScanMatcher::Match(
     const transform::Rigid2d& initial_pose_estimate,
     const sensor::PointCloud2D& point_cloud,
-    const ProbabilityGrid& probability_grid, transform::Rigid2d* pose_estimate,
+    const ProbabilityGrid& probability_grid,
-    kalman_filter::Pose2DCovariance* covariance) const {
+    transform::Rigid2d* pose_estimate) const {
   CHECK_NOTNULL(pose_estimate);
-  CHECK_NOTNULL(covariance);
 
   const Eigen::Rotation2Dd initial_rotation = initial_pose_estimate.rotation();
   const sensor::PointCloud2D rotated_point_cloud =
@@ -119,20 +115,6 @@ double RealTimeCorrelativeScanMatcher::Match(
   ScoreCandidates(probability_grid, discrete_scans, search_parameters,
                   &candidates);
 
-  // Covariance computed as in "Real-Time Correlative Scan Matching" by Olson.
-  Eigen::Matrix3d K = Eigen::Matrix3d::Zero();
-  Eigen::Vector3d u = Eigen::Vector3d::Zero();
-  double s = 0.f;
-  for (const Candidate& candidate : candidates) {
-    const double p = candidate.score;
-    const Eigen::Vector3d xi(candidate.x, candidate.y, candidate.orientation);
-    K += (xi * xi.transpose()) * p;
-    u += xi * p;
-    s += p;
-  }
-  *covariance = (1. / s) * K - (1. / (s * s)) * (u * u.transpose());
-  *covariance *= options_.covariance_scale();
-
   const Candidate& best_candidate =
       *std::max_element(candidates.begin(), candidates.end());
   *pose_estimate = transform::Rigid2d(

cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.h

@@ -41,7 +41,6 @@
 #include <vector>
 
 #include "Eigen/Core"
-#include "cartographer/kalman_filter/pose_tracker.h"
 #include "cartographer/mapping_2d/probability_grid.h"
 #include "cartographer/mapping_2d/scan_matching/correlative_scan_matcher.h"
 #include "cartographer/mapping_2d/scan_matching/proto/real_time_correlative_scan_matcher_options.pb.h"
@@ -66,13 +65,12 @@ class RealTimeCorrelativeScanMatcher {
       const RealTimeCorrelativeScanMatcher&) = delete;
 
   // Aligns 'point_cloud' within the 'probability_grid' given an
-  // 'initial_pose_estimate' then updates 'pose_estimate' and 'covariance'
+  // 'initial_pose_estimate' then updates 'pose_estimate' with the result and
-  // with the result and returns the score.
+  // returns the score.
   double Match(const transform::Rigid2d& initial_pose_estimate,
                const sensor::PointCloud2D& point_cloud,
                const ProbabilityGrid& probability_grid,
-               transform::Rigid2d* pose_estimate,
+               transform::Rigid2d* pose_estimate) const;
-               kalman_filter::Pose2DCovariance* covariance) const;
 
   // Computes the score for each Candidate in a collection. The cost is computed
   // as the sum of probabilities, different from the Ceres CostFunctions:

cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_test.cc

@@ -67,7 +67,6 @@ class RealTimeCorrelativeScanMatcherTest : public ::testing::Test {
           "angular_search_window = 0.16, "
           "translation_delta_cost_weight = 0., "
           "rotation_delta_cost_weight = 0., "
-          "covariance_scale = 1., "
           "}");
       real_time_correlative_scan_matcher_ =
           common::make_unique<RealTimeCorrelativeScanMatcher>(

cartographer/mapping_3d/kalman_local_trajectory_builder_test.cc

@@ -102,7 +102,6 @@ class KalmanLocalTrajectoryBuilderTest : public ::testing::Test {
               angular_search_window = math.rad(1.),
               translation_delta_cost_weight = 1e-1,
               rotation_delta_cost_weight = 1.,
-              covariance_scale = 1.,
             },
             pose_tracker = {
               orientation_model_variance = 5e-4,

cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher_test.cc

@@ -56,7 +56,6 @@ class RealTimeCorrelativeScanMatcherTest : public ::testing::Test {
           angular_search_window = math.rad(1.),
           translation_delta_cost_weight = 1e-1,
           rotation_delta_cost_weight = 1.,
-          covariance_scale = 1.,
         })text");
     real_time_correlative_scan_matcher_.reset(
         new RealTimeCorrelativeScanMatcher(

configuration_files/trajectory_builder_2d.lua

@@ -30,7 +30,6 @@ TRAJECTORY_BUILDER_2D = {
     angular_search_window = math.rad(20.),
     translation_delta_cost_weight = 1e-1,
     rotation_delta_cost_weight = 1e-1,
-    covariance_scale = 1.,
   },
 
   ceres_scan_matcher = {

configuration_files/trajectory_builder_3d.lua

@@ -82,7 +82,6 @@ TRAJECTORY_BUILDER_3D = {
       angular_search_window = math.rad(1.),
       translation_delta_cost_weight = 1e-1,
       rotation_delta_cost_weight = 1e-1,
-      covariance_scale = 1.,
     },
   },
 

scripts/update_cmakelists.py

@@ -14,7 +14,6 @@
 # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 # See the License for the specific language governing permissions and
 # limitations under the License.
-
 """A dumb CMakeLists.txt generator that relies on source name conventions."""
 
 import os
@@ -131,8 +130,8 @@ def ReadFileWithoutGoogleTargets(filename):
 
 
 def main():
-  root_directory = os.path.realpath(os.path.join(
+  root_directory = os.path.realpath(
-      os.path.dirname(__file__), os.path.pardir, "cartographer"))
+      os.path.join(os.path.dirname(__file__), os.path.pardir, "cartographer"))
   targets_by_src = {}
   targets = []
 
@@ -198,8 +197,8 @@ def main():
       if lines:
         outfile.write("\n".join(lines))
         outfile.write("\n")
-      outfile.write("\n\n".join(t.Format(directory) for t in
+      outfile.write("\n\n".join(
-                                targets_in_directory))
+          t.Format(directory) for t in targets_in_directory))
       outfile.write("\n")