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

Remove covariance from the 3D Ceres scan matcher. (#200) 

Instead of passing around fake covariances generated by the scan matcher,
we introduce appropriate values where they are needed.
master
Wolfgang Hess 2017-02-08 15:22:28 +01:00 committed by GitHub
parent b4a1021538
commit b2175f0d93
13 changed files with 20 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
cartographer/mapping_2d/sparse_pose_graph_test.cc
View File

@ -109,7 +109,6 @@ class SparsePoseGraphTest : public ::testing::Test {
occupied_space_weight_0 = 20., occupied_space_weight_0 = 20.,
translation_weight = 10., translation_weight = 10.,
rotation_weight = 1., rotation_weight = 1.,
covariance_scale = 1.,
only_optimize_yaw = true, only_optimize_yaw = true,
ceres_solver_options = { ceres_solver_options = {
use_nonmonotonic_steps = true, use_nonmonotonic_steps = true,

11
cartographer/mapping_3d/kalman_local_trajectory_builder.cc
View File

@ -154,7 +154,6 @@ KalmanLocalTrajectoryBuilder::AddAccumulatedLaserFan(
} }
transform::Rigid3d pose_observation; transform::Rigid3d pose_observation;
kalman_filter::PoseCovariance covariance_observation;
ceres::Solver::Summary summary; ceres::Solver::Summary summary;
sensor::AdaptiveVoxelFilter low_resolution_adaptive_voxel_filter( sensor::AdaptiveVoxelFilter low_resolution_adaptive_voxel_filter(
@ -166,10 +165,12 @@ KalmanLocalTrajectoryBuilder::AddAccumulatedLaserFan(
&submaps_->high_resolution_matching_grid()}, &submaps_->high_resolution_matching_grid()},
{&low_resolution_point_cloud_in_tracking, {&low_resolution_point_cloud_in_tracking,
&submaps_->low_resolution_matching_grid()}}, &submaps_->low_resolution_matching_grid()}},
&pose_observation, &covariance_observation, &pose_observation, &summary);
&summary); pose_tracker_->AddPoseObservation(
pose_tracker_->AddPoseObservation(time, pose_observation, time, pose_observation,
covariance_observation); options_.kalman_local_trajectory_builder_options()
.scan_matcher_variance() *
kalman_filter::PoseCovariance::Identity());
kalman_filter::PoseCovariance covariance_estimate; kalman_filter::PoseCovariance covariance_estimate;
pose_tracker_->GetPoseEstimateMeanAndCovariance( pose_tracker_->GetPoseEstimateMeanAndCovariance(

2
cartographer/mapping_3d/kalman_local_trajectory_builder_options.cc
View File

@ -36,6 +36,8 @@ CreateKalmanLocalTrajectoryBuilderOptions(
*options.mutable_pose_tracker_options() = *options.mutable_pose_tracker_options() =
kalman_filter::CreatePoseTrackerOptions( kalman_filter::CreatePoseTrackerOptions(
parameter_dictionary->GetDictionary("pose_tracker").get()); parameter_dictionary->GetDictionary("pose_tracker").get());
options.set_scan_matcher_variance(
parameter_dictionary->GetDouble("scan_matcher_variance"));
options.set_odometer_translational_variance( options.set_odometer_translational_variance(
parameter_dictionary->GetDouble("odometer_translational_variance")); parameter_dictionary->GetDouble("odometer_translational_variance"));
options.set_odometer_rotational_variance( options.set_odometer_rotational_variance(

2
cartographer/mapping_3d/kalman_local_trajectory_builder_test.cc
View File

@ -69,7 +69,6 @@ class KalmanLocalTrajectoryBuilderTest : public ::testing::Test {
occupied_space_weight_1 = 20., occupied_space_weight_1 = 20.,
translation_weight = 0.1, translation_weight = 0.1,
rotation_weight = 0.3, rotation_weight = 0.3,
covariance_scale = 1e-1,
only_optimize_yaw = false, only_optimize_yaw = false,
ceres_solver_options = { ceres_solver_options = {
use_nonmonotonic_steps = true, use_nonmonotonic_steps = true,
@ -112,6 +111,7 @@ class KalmanLocalTrajectoryBuilderTest : public ::testing::Test {
num_odometry_states = 1, num_odometry_states = 1,
}, },
scan_matcher_variance = 1e-6,
odometer_translational_variance = 1e-7, odometer_translational_variance = 1e-7,
odometer_rotational_variance = 1e-7, odometer_rotational_variance = 1e-7,
}, },

1
cartographer/mapping_3d/proto/kalman_local_trajectory_builder_options.proto
View File

@ -28,6 +28,7 @@ message KalmanLocalTrajectoryBuilderOptions {
real_time_correlative_scan_matcher_options = 2; real_time_correlative_scan_matcher_options = 2;
optional kalman_filter.proto.PoseTrackerOptions pose_tracker_options = 3; optional kalman_filter.proto.PoseTrackerOptions pose_tracker_options = 3;
optional double scan_matcher_variance = 6;
optional double odometer_translational_variance = 4; optional double odometer_translational_variance = 4;
optional double odometer_rotational_variance = 5; optional double odometer_rotational_variance = 5;
} }

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

@ -67,8 +67,6 @@ proto::CeresScanMatcherOptions CreateCeresScanMatcherOptions(
parameter_dictionary->GetDouble("translation_weight")); parameter_dictionary->GetDouble("translation_weight"));
options.set_rotation_weight( options.set_rotation_weight(
parameter_dictionary->GetDouble("rotation_weight")); parameter_dictionary->GetDouble("rotation_weight"));
options.set_covariance_scale(
parameter_dictionary->GetDouble("covariance_scale"));
options.set_only_optimize_yaw( options.set_only_optimize_yaw(
parameter_dictionary->GetBool("only_optimize_yaw")); parameter_dictionary->GetBool("only_optimize_yaw"));
*options.mutable_ceres_solver_options() = *options.mutable_ceres_solver_options() =
@ -90,7 +88,6 @@ void CeresScanMatcher::Match(const transform::Rigid3d& previous_pose,
const std::vector<PointCloudAndHybridGridPointers>& const std::vector<PointCloudAndHybridGridPointers>&
point_clouds_and_hybrid_grids, point_clouds_and_hybrid_grids,
transform::Rigid3d* const pose_estimate, transform::Rigid3d* const pose_estimate,
kalman_filter::PoseCovariance* const covariance,
ceres::Solver::Summary* const summary) { ceres::Solver::Summary* const summary) {
ceres::Problem problem; ceres::Problem problem;
CeresPose ceres_pose( CeresPose ceres_pose(
@ -136,10 +133,6 @@ void CeresScanMatcher::Match(const transform::Rigid3d& previous_pose,
ceres::Solve(ceres_solver_options_, &problem, summary); ceres::Solve(ceres_solver_options_, &problem, summary);
*pose_estimate = ceres_pose.ToRigid(); *pose_estimate = ceres_pose.ToRigid();
// TODO(whess): Remove once the UKF is gone.
*covariance = 1e-5 * options_.covariance_scale() *
kalman_filter::PoseCovariance::Identity();
} }
} // namespace scan_matching } // namespace scan_matching

4
cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h
View File

@ -22,7 +22,6 @@
#include "Eigen/Core" #include "Eigen/Core"
#include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/kalman_filter/pose_tracker.h"
#include "cartographer/mapping_3d/hybrid_grid.h" #include "cartographer/mapping_3d/hybrid_grid.h"
#include "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.pb.h" #include "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.pb.h"
#include "cartographer/sensor/point_cloud.h" #include "cartographer/sensor/point_cloud.h"
@ -47,14 +46,13 @@ class CeresScanMatcher {
CeresScanMatcher& operator=(const CeresScanMatcher&) = delete; CeresScanMatcher& operator=(const CeresScanMatcher&) = delete;
// Aligns 'point_clouds' within the 'hybrid_grids' given an // Aligns 'point_clouds' within the 'hybrid_grids' given an
// 'initial_pose_estimate' and returns 'pose_estimate', 'covariance', and // 'initial_pose_estimate' and returns 'pose_estimate', and
// the solver 'summary'. // the solver 'summary'.
void Match(const transform::Rigid3d& previous_pose, void Match(const transform::Rigid3d& previous_pose,
const transform::Rigid3d& initial_pose_estimate, const transform::Rigid3d& initial_pose_estimate,
const std::vector<PointCloudAndHybridGridPointers>& const std::vector<PointCloudAndHybridGridPointers>&
point_clouds_and_hybrid_grids, point_clouds_and_hybrid_grids,
transform::Rigid3d* pose_estimate, transform::Rigid3d* pose_estimate,
kalman_filter::PoseCovariance* covariance,
ceres::Solver::Summary* summary); ceres::Solver::Summary* summary);
private: private:

4
cartographer/mapping_3d/scan_matching/ceres_scan_matcher_test.cc
View File

@ -53,7 +53,6 @@ class CeresScanMatcherTest : public ::testing::Test {
occupied_space_weight_0 = 1., occupied_space_weight_0 = 1.,
translation_weight = 0.01, translation_weight = 0.01,
rotation_weight = 0.1, rotation_weight = 0.1,
covariance_scale = 10.,
only_optimize_yaw = false, only_optimize_yaw = false,
ceres_solver_options = { ceres_solver_options = {
use_nonmonotonic_steps = true, use_nonmonotonic_steps = true,
@ -67,12 +66,11 @@ class CeresScanMatcherTest : public ::testing::Test {
void TestFromInitialPose(const transform::Rigid3d& initial_pose) { void TestFromInitialPose(const transform::Rigid3d& initial_pose) {
transform::Rigid3d pose; transform::Rigid3d pose;
kalman_filter::PoseCovariance covariance;
ceres::Solver::Summary summary; ceres::Solver::Summary summary;
ceres_scan_matcher_->Match(initial_pose, initial_pose, ceres_scan_matcher_->Match(initial_pose, initial_pose,
{{&point_cloud_, &hybrid_grid_}}, &pose, {{&point_cloud_, &hybrid_grid_}}, &pose,
&covariance, &summary); &summary);
EXPECT_NEAR(0., summary.final_cost, 1e-2) << summary.FullReport(); EXPECT_NEAR(0., summary.final_cost, 1e-2) << summary.FullReport();
EXPECT_THAT(pose, transform::IsNearly(expected_pose_, 3e-2)); EXPECT_THAT(pose, transform::IsNearly(expected_pose_, 3e-2));
} }

3
cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.proto
View File

@ -25,9 +25,6 @@ message CeresScanMatcherOptions {
optional double translation_weight = 2; optional double translation_weight = 2;
optional double rotation_weight = 3; optional double rotation_weight = 3;
// Scale applied to the covariance estimate from Ceres.
optional double covariance_scale = 4;
// Whether only to allow changes to yaw, keeping roll/pitch constant. // Whether only to allow changes to yaw, keeping roll/pitch constant.
optional bool only_optimize_yaw = 5; optional bool only_optimize_yaw = 5;

12
cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc
View File

@ -250,12 +250,10 @@ void ConstraintBuilder::ComputeConstraint(
// effect that, in the absence of better information, we prefer the original // effect that, in the absence of better information, we prefer the original
// CSM estimate. // CSM estimate.
ceres::Solver::Summary unused_summary; ceres::Solver::Summary unused_summary;
kalman_filter::PoseCovariance covariance;
ceres_scan_matcher_.Match( ceres_scan_matcher_.Match(
pose_estimate, pose_estimate, pose_estimate, pose_estimate,
{{&filtered_point_cloud, submap_scan_matcher->hybrid_grid}}, {{&filtered_point_cloud, submap_scan_matcher->hybrid_grid}},
&pose_estimate, &covariance, &unused_summary); &pose_estimate, &unused_summary);
// 'covariance' is unchanged as (submap <- map) is a translation.
const transform::Rigid3d constraint_transform = const transform::Rigid3d constraint_transform =
submap->local_pose().inverse() * pose_estimate; submap->local_pose().inverse() * pose_estimate;
@ -263,8 +261,8 @@ void ConstraintBuilder::ComputeConstraint(
submap_index, submap_index,
scan_index, scan_index,
{constraint_transform, {constraint_transform,
common::ComputeSpdMatrixSqrtInverse( 1. / std::sqrt(options_.lower_covariance_eigenvalue_bound()) *
covariance, options_.lower_covariance_eigenvalue_bound())}, kalman_filter::PoseCovariance::Identity()},
OptimizationProblem::Constraint::INTER_SUBMAP}); OptimizationProblem::Constraint::INTER_SUBMAP});
if (options_.log_matches()) { if (options_.log_matches()) {
@ -276,9 +274,7 @@ void ConstraintBuilder::ComputeConstraint(
<< " differs by translation " << std::fixed << std::setprecision(2) << " differs by translation " << std::fixed << std::setprecision(2)
<< difference.translation().norm() << " rotation " << difference.translation().norm() << " rotation "
<< std::setprecision(3) << transform::GetAngle(difference) << std::setprecision(3) << transform::GetAngle(difference)
<< " with score " << std::setprecision(1) << 100. * score << " with score " << std::setprecision(1) << 100. * score << "%.";
<< "% covariance trace " << std::scientific << std::setprecision(4)
<< covariance.trace() << ".";
LOG(INFO) << info.str(); LOG(INFO) << info.str();
} }
} }

1
configuration_files/sparse_pose_graph.lua
View File

@ -55,7 +55,6 @@ SPARSE_POSE_GRAPH = {
occupied_space_weight_0 = 20., occupied_space_weight_0 = 20.,
translation_weight = 10., translation_weight = 10.,
rotation_weight = 1., rotation_weight = 1.,
covariance_scale = 1e-6,
only_optimize_yaw = false, only_optimize_yaw = false,
ceres_solver_options = { ceres_solver_options = {
use_nonmonotonic_steps = false, use_nonmonotonic_steps = false,

2
configuration_files/trajectory_builder_3d.lua
View File

@ -37,7 +37,6 @@ TRAJECTORY_BUILDER_3D = {
occupied_space_weight_1 = 30., occupied_space_weight_1 = 30.,
translation_weight = 0.3, translation_weight = 0.3,
rotation_weight = 2e3, rotation_weight = 2e3,
covariance_scale = 2.34e-4,
only_optimize_yaw = false, only_optimize_yaw = false,
ceres_solver_options = { ceres_solver_options = {
use_nonmonotonic_steps = false, use_nonmonotonic_steps = false,
@ -84,6 +83,7 @@ TRAJECTORY_BUILDER_3D = {
rotation_delta_cost_weight = 1e-1, rotation_delta_cost_weight = 1e-1,
}, },
scan_matcher_variance = 2.34e-9,
odometer_translational_variance = 1e-7, odometer_translational_variance = 1e-7,
odometer_rotational_variance = 1e-7, odometer_rotational_variance = 1e-7,
}, },

6
docs/source/configuration.rst
View File

@ -324,6 +324,9 @@ cartographer.mapping_2d.scan_matching.proto.RealTimeCorrelativeScanMatcherOption
cartographer.kalman_filter.proto.PoseTrackerOptions pose_tracker_options cartographer.kalman_filter.proto.PoseTrackerOptions pose_tracker_options
Not yet documented. Not yet documented.
double scan_matcher_variance
Not yet documented.
double odometer_translational_variance double odometer_translational_variance
Not yet documented. Not yet documented.
@ -422,9 +425,6 @@ double translation_weight
double rotation_weight double rotation_weight
Not yet documented. Not yet documented.
double covariance_scale
Scale applied to the covariance estimate from Ceres.
bool only_optimize_yaw bool only_optimize_yaw
Whether only to allow changes to yaw, keeping roll/pitch constant. Whether only to allow changes to yaw, keeping roll/pitch constant.