From 14355a91a16fa7f428efbade988a9e8e61a99dd3 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Thu, 22 Sep 2016 17:53:32 +0200 Subject: [PATCH] Remove dead code. (#26) --- cartographer/mapping/map_builder.cc | 1 - .../mapping_2d/local_trajectory_builder.cc | 3 +-- .../mapping_2d/scan_matching/CMakeLists.txt | 1 - ...ime_correlative_scan_matcher_options.proto | 3 --- .../real_time_correlative_scan_matcher.cc | 22 ++----------------- .../real_time_correlative_scan_matcher.h | 8 +++---- ...real_time_correlative_scan_matcher_test.cc | 1 - .../kalman_local_trajectory_builder_test.cc | 1 - ...real_time_correlative_scan_matcher_test.cc | 1 - configuration_files/trajectory_builder_2d.lua | 1 - configuration_files/trajectory_builder_3d.lua | 1 - scripts/update_cmakelists.py | 9 ++++---- 12 files changed, 10 insertions(+), 42 deletions(-) diff --git a/cartographer/mapping/map_builder.cc b/cartographer/mapping/map_builder.cc index c46a566..1b73f73 100644 --- a/cartographer/mapping/map_builder.cc +++ b/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 { diff --git a/cartographer/mapping_2d/local_trajectory_builder.cc b/cartographer/mapping_2d/local_trajectory_builder.cc index 6d6fa50..39d0cb2 100644 --- a/cartographer/mapping_2d/local_trajectory_builder.cc +++ b/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; diff --git a/cartographer/mapping_2d/scan_matching/CMakeLists.txt b/cartographer/mapping_2d/scan_matching/CMakeLists.txt index deadbc6..5cd313e 100644 --- a/cartographer/mapping_2d/scan_matching/CMakeLists.txt +++ b/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 diff --git a/cartographer/mapping_2d/scan_matching/proto/real_time_correlative_scan_matcher_options.proto b/cartographer/mapping_2d/scan_matching/proto/real_time_correlative_scan_matcher_options.proto index 8ea52bc..29c4d15 100644 --- a/cartographer/mapping_2d/scan_matching/proto/real_time_correlative_scan_matcher_options.proto +++ b/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; } diff --git a/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.cc b/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.cc index a0930a0..9dce002 100644 --- a/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.cc +++ b/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, - kalman_filter::Pose2DCovariance* covariance) const { + const ProbabilityGrid& probability_grid, + 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( diff --git a/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.h b/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.h index b7b8321..6dcc653 100644 --- a/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.h +++ b/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.h @@ -41,7 +41,6 @@ #include #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' - // with the result and returns the score. + // 'initial_pose_estimate' then updates 'pose_estimate' with the result and + // returns the score. double Match(const transform::Rigid2d& initial_pose_estimate, const sensor::PointCloud2D& point_cloud, const ProbabilityGrid& probability_grid, - transform::Rigid2d* pose_estimate, - kalman_filter::Pose2DCovariance* covariance) const; + transform::Rigid2d* pose_estimate) const; // Computes the score for each Candidate in a collection. The cost is computed // as the sum of probabilities, different from the Ceres CostFunctions: diff --git a/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_test.cc b/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_test.cc index aae6659..da765cb 100644 --- a/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_test.cc +++ b/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( diff --git a/cartographer/mapping_3d/kalman_local_trajectory_builder_test.cc b/cartographer/mapping_3d/kalman_local_trajectory_builder_test.cc index 5f8641e..505b868 100644 --- a/cartographer/mapping_3d/kalman_local_trajectory_builder_test.cc +++ b/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, diff --git a/cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher_test.cc b/cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher_test.cc index 3c0aa22..a7763ba 100644 --- a/cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher_test.cc +++ b/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( diff --git a/configuration_files/trajectory_builder_2d.lua b/configuration_files/trajectory_builder_2d.lua index e101b03..c04f537 100644 --- a/configuration_files/trajectory_builder_2d.lua +++ b/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 = { diff --git a/configuration_files/trajectory_builder_3d.lua b/configuration_files/trajectory_builder_3d.lua index 2b5e4e8..c4ebb9a 100644 --- a/configuration_files/trajectory_builder_3d.lua +++ b/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., }, }, diff --git a/scripts/update_cmakelists.py b/scripts/update_cmakelists.py index 283820c..6eeacc0 100755 --- a/scripts/update_cmakelists.py +++ b/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( - os.path.dirname(__file__), os.path.pardir, "cartographer")) + root_directory = os.path.realpath( + 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 - targets_in_directory)) + outfile.write("\n\n".join( + t.Format(directory) for t in targets_in_directory)) outfile.write("\n")