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

Remove 2D scan matcher covariances. (#307) 

This replaces the scaled covariances derived from the Ceres
scan matcher by directly configurable weights. Using covariances
did not provide the expected benefit, and replacing the scaling
matrix by two values will allow a faster evaluation of the cost
function in the future.
master
Wolfgang Hess 2017-05-31 11:56:32 +02:00 committed by GitHub
parent 4bb02c7240
commit a2e52f81cf
19 changed files with 86 additions and 101 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
cartographer/mapping/proto/sparse_pose_graph_options.proto
View File

@ -28,6 +28,11 @@ message SparsePoseGraphOptions {
optional mapping.sparse_pose_graph.proto.ConstraintBuilderOptions
constraint_builder_options = 3;
// Weights used in the optimization problem for non-loop-closure scan matcher
// constraints.
optional double matcher_translation_weight = 7;
optional double matcher_rotation_weight = 8;
// Options for the optimization problem.
optional mapping.sparse_pose_graph.proto.OptimizationProblemOptions
optimization_problem_options = 4;

19
cartographer/mapping/sparse_pose_graph.cc
View File

@ -16,7 +16,6 @@
#include "cartographer/mapping/sparse_pose_graph.h"
#include "cartographer/kalman_filter/pose_tracker.h"
#include "cartographer/mapping/sparse_pose_graph/constraint_builder.h"
#include "cartographer/mapping/sparse_pose_graph/optimization_problem_options.h"
#include "cartographer/transform/transform.h"
@ -44,6 +43,10 @@ proto::SparsePoseGraphOptions CreateSparsePoseGraphOptions(
*options.mutable_constraint_builder_options() =
sparse_pose_graph::CreateConstraintBuilderOptions(
parameter_dictionary->GetDictionary("constraint_builder").get());
options.set_matcher_translation_weight(
parameter_dictionary->GetDouble("matcher_translation_weight"));
options.set_matcher_rotation_weight(
parameter_dictionary->GetDouble("matcher_rotation_weight"));
*options.mutable_optimization_problem_options() =
sparse_pose_graph::CreateOptimizationProblemOptions(
parameter_dictionary->GetDictionary("optimization_problem").get());
@ -55,6 +58,20 @@ proto::SparsePoseGraphOptions CreateSparsePoseGraphOptions(
return options;
}
Eigen::Matrix<double, 6, 6> FromTranslationRotationWeights(
const double translation_weight, const double rotation_weight) {
Eigen::Matrix<double, 6, 6> result;
// clang-format off
result << translation_weight, 0., 0., 0., 0., 0.,
0., translation_weight, 0., 0., 0., 0.,
0., 0., translation_weight, 0., 0., 0.,
0., 0., 0., rotation_weight, 0., 0.,
0., 0., 0., 0., rotation_weight, 0.,
0., 0., 0., 0., 0., rotation_weight;
// clang-format on
return result;
}
proto::SparsePoseGraph SparsePoseGraph::ToProto() {
proto::SparsePoseGraph proto;

4
cartographer/mapping/sparse_pose_graph.h
View File

@ -35,6 +35,10 @@ namespace mapping {
proto::SparsePoseGraphOptions CreateSparsePoseGraphOptions(
common::LuaParameterDictionary* const parameter_dictionary);
// TODO(whess): Change to two doubles for performance.
Eigen::Matrix<double, 6, 6> FromTranslationRotationWeights(
double translation_weight, double rotation_weight);
class SparsePoseGraph {
public:
// A "constraint" as in the paper by Konolige, Kurt, et al. "Efficient sparse

4
cartographer/mapping/sparse_pose_graph/constraint_builder.cc
View File

@ -40,6 +40,10 @@ proto::ConstraintBuilderOptions CreateConstraintBuilderOptions(
parameter_dictionary->GetDouble("global_localization_min_score"));
options.set_lower_covariance_eigenvalue_bound(
parameter_dictionary->GetDouble("lower_covariance_eigenvalue_bound"));
options.set_loop_closure_translation_weight(
parameter_dictionary->GetDouble("loop_closure_translation_weight"));
options.set_loop_closure_rotation_weight(
parameter_dictionary->GetDouble("loop_closure_rotation_weight"));
options.set_log_matches(parameter_dictionary->GetBool("log_matches"));
*options.mutable_fast_correlative_scan_matcher_options() =
mapping_2d::scan_matching::CreateFastCorrelativeScanMatcherOptions(

5
cartographer/mapping/sparse_pose_graph/proto/constraint_builder_options.proto
View File

@ -42,8 +42,13 @@ message ConstraintBuilderOptions {
optional double global_localization_min_score = 5;
// Lower bound for covariance eigenvalues to limit the weight of matches.
// TODO(whess): Deprecated and only used in 3D. Replace usage and remove.
optional double lower_covariance_eigenvalue_bound = 7;
// Weights used in the optimization problem for loop closure constraints.
optional double loop_closure_translation_weight = 13;
optional double loop_closure_rotation_weight = 14;
// If enabled, logs information of loop-closing constraints for debugging.
optional bool log_matches = 8;

6
cartographer/mapping_2d/global_trajectory_builder.cc
View File

@ -42,10 +42,8 @@ void GlobalTrajectoryBuilder::AddRangefinderData(
sparse_pose_graph_->AddScan(
insertion_result->time, insertion_result->tracking_to_tracking_2d,
insertion_result->range_data_in_tracking_2d,
insertion_result->pose_estimate_2d,
kalman_filter::Project2D(insertion_result->covariance_estimate),
trajectory_id_, insertion_result->matching_submap,
insertion_result->insertion_submaps);
insertion_result->pose_estimate_2d, trajectory_id_,
insertion_result->matching_submap, insertion_result->insertion_submaps);
}
}

19
cartographer/mapping_2d/local_trajectory_builder.cc
View File

@ -69,8 +69,7 @@ void LocalTrajectoryBuilder::ScanMatch(
common::Time time, const transform::Rigid3d& pose_prediction,
const transform::Rigid3d& tracking_to_tracking_2d,
const sensor::RangeData& range_data_in_tracking_2d,
transform::Rigid3d* pose_observation,
kalman_filter::PoseCovariance* covariance_observation) {
transform::Rigid3d* pose_observation) {
const ProbabilityGrid& probability_grid =
submaps_.Get(submaps_.matching_index())->probability_grid;
transform::Rigid2d pose_prediction_2d =
@ -89,21 +88,13 @@ void LocalTrajectoryBuilder::ScanMatch(
}
transform::Rigid2d tracking_2d_to_map;
kalman_filter::Pose2DCovariance covariance_observation_2d;
ceres::Solver::Summary summary;
ceres_scan_matcher_.Match(pose_prediction_2d, initial_ceres_pose,
filtered_point_cloud_in_tracking_2d,
probability_grid, &tracking_2d_to_map,
&covariance_observation_2d, &summary);
probability_grid, &tracking_2d_to_map, &summary);
*pose_observation =
transform::Embed3D(tracking_2d_to_map) * tracking_to_tracking_2d;
// This covariance is used for non-yaw rotation and the fake height of 0.
constexpr double kFakePositionCovariance = 1.;
constexpr double kFakeOrientationCovariance = 1.;
*covariance_observation =
kalman_filter::Embed3D(covariance_observation_2d, kFakePositionCovariance,
kFakeOrientationCovariance);
}
std::unique_ptr<LocalTrajectoryBuilder::InsertionResult>
@ -144,10 +135,8 @@ LocalTrajectoryBuilder::AddHorizontalRangeData(
return nullptr;
}
kalman_filter::PoseCovariance covariance_observation;
ScanMatch(time, pose_prediction, tracking_to_tracking_2d,
range_data_in_tracking_2d, &pose_estimate_,
&covariance_observation);
range_data_in_tracking_2d, &pose_estimate_);
odometry_correction_ = transform::Rigid3d::Identity();
if (!odometry_state_tracker_.empty()) {
// We add an odometry state, so that the correction from the scan matching
@ -199,7 +188,7 @@ LocalTrajectoryBuilder::AddHorizontalRangeData(
return common::make_unique<InsertionResult>(InsertionResult{
time, matching_submap, insertion_submaps, tracking_to_tracking_2d,
range_data_in_tracking_2d, pose_estimate_2d, covariance_observation});
range_data_in_tracking_2d, pose_estimate_2d});
}
const mapping::GlobalTrajectoryBuilderInterface::PoseEstimate&

8
cartographer/mapping_2d/local_trajectory_builder.h
View File

@ -46,7 +46,6 @@ class LocalTrajectoryBuilder {
transform::Rigid3d tracking_to_tracking_2d;
sensor::RangeData range_data_in_tracking_2d;
transform::Rigid2d pose_estimate_2d;
kalman_filter::PoseCovariance covariance_estimate;
};
explicit LocalTrajectoryBuilder(
@ -71,13 +70,12 @@ class LocalTrajectoryBuilder {
const transform::Rigid3f& tracking_to_tracking_2d,
const sensor::RangeData& range_data) const;
// Scan match 'range_data_in_tracking_2d' and fill in the
// 'pose_observation' and 'covariance_observation' with the result.
// Scan matches 'range_data_in_tracking_2d' and fill in the 'pose_observation'
// with the result.
void ScanMatch(common::Time time, const transform::Rigid3d& pose_prediction,
const transform::Rigid3d& tracking_to_tracking_2d,
const sensor::RangeData& range_data_in_tracking_2d,
transform::Rigid3d* pose_observation,
kalman_filter::PoseCovariance* covariance_observation);
transform::Rigid3d* pose_observation);
// Lazily constructs an ImuTracker.
void InitializeImuTracker(common::Time time);

15
cartographer/mapping_2d/scan_matching/ceres_scan_matcher.cc
View File

@ -22,7 +22,6 @@
#include "Eigen/Core"
#include "cartographer/common/ceres_solver_options.h"
#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/kalman_filter/pose_tracker.h"
#include "cartographer/mapping_2d/probability_grid.h"
#include "cartographer/mapping_2d/scan_matching/occupied_space_cost_functor.h"
#include "cartographer/mapping_2d/scan_matching/rotation_delta_cost_functor.h"
@ -44,8 +43,6 @@ proto::CeresScanMatcherOptions CreateCeresScanMatcherOptions(
parameter_dictionary->GetDouble("translation_weight"));
options.set_rotation_weight(
parameter_dictionary->GetDouble("rotation_weight"));
options.set_covariance_scale(
parameter_dictionary->GetDouble("covariance_scale"));
*options.mutable_ceres_solver_options() =
common::CreateCeresSolverOptionsProto(
parameter_dictionary->GetDictionary("ceres_solver_options").get());
@ -67,7 +64,6 @@ void CeresScanMatcher::Match(const transform::Rigid2d& previous_pose,
const sensor::PointCloud& point_cloud,
const ProbabilityGrid& probability_grid,
transform::Rigid2d* const pose_estimate,
kalman_filter::Pose2DCovariance* const covariance,
ceres::Solver::Summary* const summary) const {
double ceres_pose_estimate[3] = {initial_pose_estimate.translation().x(),
initial_pose_estimate.translation().y(),
@ -100,17 +96,6 @@ void CeresScanMatcher::Match(const transform::Rigid2d& previous_pose,
*pose_estimate = transform::Rigid2d(
{ceres_pose_estimate[0], ceres_pose_estimate[1]}, ceres_pose_estimate[2]);
ceres::Covariance::Options options;
ceres::Covariance covariance_computer(options);
std::vector<std::pair<const double*, const double*>> covariance_blocks;
covariance_blocks.emplace_back(ceres_pose_estimate, ceres_pose_estimate);
CHECK(covariance_computer.Compute(covariance_blocks, &problem));
double ceres_covariance[3 * 3];
covariance_computer.GetCovarianceBlock(ceres_pose_estimate,
ceres_pose_estimate, ceres_covariance);
*covariance = Eigen::Map<kalman_filter::Pose2DCovariance>(ceres_covariance);
*covariance *= options_.covariance_scale();
}
} // namespace scan_matching

6
cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h
View File

@ -22,7 +22,6 @@
#include "Eigen/Core"
#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/kalman_filter/pose_tracker.h"
#include "cartographer/mapping_2d/probability_grid.h"
#include "cartographer/mapping_2d/scan_matching/proto/ceres_scan_matcher_options.pb.h"
#include "cartographer/sensor/point_cloud.h"
@ -45,14 +44,13 @@ class CeresScanMatcher {
CeresScanMatcher& operator=(const CeresScanMatcher&) = delete;
// Aligns 'point_cloud' within the 'probability_grid' given an
// 'initial_pose_estimate' and returns 'pose_estimate', 'covariance', and
// the solver 'summary'.
// 'initial_pose_estimate' and returns a 'pose_estimate' and the solver
// 'summary'.
void Match(const transform::Rigid2d& previous_pose,
const transform::Rigid2d& initial_pose_estimate,
const sensor::PointCloud& point_cloud,
const ProbabilityGrid& probability_grid,
transform::Rigid2d* pose_estimate,
kalman_filter::Pose2DCovariance* covariance,
ceres::Solver::Summary* summary) const;
private:

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

@ -47,7 +47,6 @@ class CeresScanMatcherTest : public ::testing::Test {
occupied_space_weight = 1.,
translation_weight = 0.1,
rotation_weight = 1.5,
covariance_scale = 10.,
ceres_solver_options = {
use_nonmonotonic_steps = true,
max_num_iterations = 50,
@ -61,12 +60,11 @@ class CeresScanMatcherTest : public ::testing::Test {
void TestFromInitialPose(const transform::Rigid2d& initial_pose) {
transform::Rigid2d pose;
kalman_filter::Pose2DCovariance covariance;
const transform::Rigid2d expected_pose =
transform::Rigid2d::Translation({-0.5, 0.5});
ceres::Solver::Summary summary;
ceres_scan_matcher_->Match(initial_pose, initial_pose, point_cloud_,
probability_grid_, &pose, &covariance, &summary);
probability_grid_, &pose, &summary);
EXPECT_NEAR(0., summary.final_cost, 1e-2) << summary.FullReport();
EXPECT_THAT(pose, transform::IsNearly(expected_pose, 1e-2))
<< "Actual: " << transform::ToProto(pose).DebugString()

1
cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_test.cc
View File

@ -22,7 +22,6 @@
#include "Eigen/Geometry"
#include "cartographer/common/lua_parameter_dictionary_test_helpers.h"
#include "cartographer/common/make_unique.h"
#include "cartographer/kalman_filter/pose_tracker.h"
#include "cartographer/mapping_2d/probability_grid.h"
#include "cartographer/mapping_2d/range_data_inserter.h"
#include "cartographer/sensor/point_cloud.h"

29
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -90,8 +90,7 @@ void SparsePoseGraph::GrowSubmapTransformsAsNeeded(
void SparsePoseGraph::AddScan(
common::Time time, const transform::Rigid3d& tracking_to_pose,
const sensor::RangeData& range_data_in_pose, const transform::Rigid2d& pose,
const kalman_filter::Pose2DCovariance& covariance, const int trajectory_id,
const mapping::Submap* const matching_submap,
const int trajectory_id, const mapping::Submap* const matching_submap,
const std::vector<const mapping::Submap*>& insertion_submaps) {
const transform::Rigid3d optimized_pose(
GetLocalToGlobalTransform(trajectory_id) * transform::Embed3D(pose));
@ -128,7 +127,7 @@ void SparsePoseGraph::AddScan(
AddWorkItem([=]() REQUIRES(mutex_) {
ComputeConstraintsForScan(matching_submap, insertion_submaps,
finished_submap, pose, covariance);
finished_submap, pose);
});
}
@ -207,8 +206,7 @@ void SparsePoseGraph::ComputeConstraintsForOldScans(
void SparsePoseGraph::ComputeConstraintsForScan(
const mapping::Submap* matching_submap,
std::vector<const mapping::Submap*> insertion_submaps,
const mapping::Submap* finished_submap, const transform::Rigid2d& pose,
const kalman_filter::Pose2DCovariance& covariance) {
const mapping::Submap* finished_submap, const transform::Rigid2d& pose) {
GrowSubmapTransformsAsNeeded(insertion_submaps);
const mapping::SubmapId matching_id = GetSubmapId(matching_submap);
const transform::Rigid2d optimized_pose =
@ -234,21 +232,16 @@ void SparsePoseGraph::ComputeConstraintsForScan(
const mapping::SubmapId submap_id = GetSubmapId(submap);
CHECK(!submap_states_.at(submap_id).finished);
submap_states_.at(submap_id).node_ids.emplace(node_id);
// Unchanged covariance as (submap <- map) is a translation.
const transform::Rigid2d constraint_transform =
sparse_pose_graph::ComputeSubmapPose(*submap).inverse() * pose;
constexpr double kFakePositionCovariance = 1e-6;
constexpr double kFakeOrientationCovariance = 1e-6;
constraints_.push_back(Constraint{
submap_id,
node_id,
{transform::Embed3D(constraint_transform),
common::ComputeSpdMatrixSqrtInverse(
kalman_filter::Embed3D(covariance, kFakePositionCovariance,
kFakeOrientationCovariance),
options_.constraint_builder_options()
.lower_covariance_eigenvalue_bound())},
Constraint::INTRA_SUBMAP});
constraints_.push_back(
Constraint{submap_id,
node_id,
{transform::Embed3D(constraint_transform),
mapping::FromTranslationRotationWeights(
options_.matcher_translation_weight(),
options_.matcher_rotation_weight())},
Constraint::INTRA_SUBMAP});
}
for (int trajectory_id = 0; trajectory_id < submap_states_.num_trajectories();

10
cartographer/mapping_2d/sparse_pose_graph.h
View File

@ -30,7 +30,6 @@
#include "cartographer/common/mutex.h"
#include "cartographer/common/thread_pool.h"
#include "cartographer/common/time.h"
#include "cartographer/kalman_filter/pose_tracker.h"
#include "cartographer/mapping/sparse_pose_graph.h"
#include "cartographer/mapping/trajectory_connectivity.h"
#include "cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h"
@ -68,9 +67,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
// into the 'insertion_submaps'.
void AddScan(common::Time time, const transform::Rigid3d& tracking_to_pose,
const sensor::RangeData& range_data_in_pose,
const transform::Rigid2d& pose,
const kalman_filter::Pose2DCovariance& pose_covariance,
int trajectory_id, const mapping::Submap* matching_submap,
const transform::Rigid2d& pose, int trajectory_id,
const mapping::Submap* matching_submap,
const std::vector<const mapping::Submap*>& insertion_submaps)
EXCLUDES(mutex_);
@ -125,8 +123,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
void ComputeConstraintsForScan(
const mapping::Submap* matching_submap,
std::vector<const mapping::Submap*> insertion_submaps,
const mapping::Submap* finished_submap, const transform::Rigid2d& pose,
const kalman_filter::Pose2DCovariance& covariance) REQUIRES(mutex_);
const mapping::Submap* finished_submap, const transform::Rigid2d& pose)
REQUIRES(mutex_);
// Computes constraints for a scan and submap pair.
void ComputeConstraint(const mapping::NodeId& node_id,

28
cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc
View File

@ -29,7 +29,6 @@
#include "cartographer/common/make_unique.h"
#include "cartographer/common/math.h"
#include "cartographer/common/thread_pool.h"
#include "cartographer/kalman_filter/pose_tracker.h"
#include "cartographer/mapping_2d/scan_matching/proto/ceres_scan_matcher_options.pb.h"
#include "cartographer/mapping_2d/scan_matching/proto/fast_correlative_scan_matcher_options.pb.h"
#include "cartographer/transform/transform.h"
@ -212,25 +211,20 @@ void ConstraintBuilder::ComputeConstraint(
// effect that, in the absence of better information, we prefer the original
// CSM estimate.
ceres::Solver::Summary unused_summary;
kalman_filter::Pose2DCovariance covariance;
ceres_scan_matcher_.Match(pose_estimate, pose_estimate, filtered_point_cloud,
*submap_scan_matcher->probability_grid,
&pose_estimate, &covariance, &unused_summary);
// 'covariance' is unchanged as (submap <- map) is a translation.
&pose_estimate, &unused_summary);
const transform::Rigid2d constraint_transform =
ComputeSubmapPose(*submap).inverse() * pose_estimate;
constexpr double kFakePositionCovariance = 1e-6;
constexpr double kFakeOrientationCovariance = 1e-6;
constraint->reset(new Constraint{
submap_id,
node_id,
{transform::Embed3D(constraint_transform),
common::ComputeSpdMatrixSqrtInverse(
kalman_filter::Embed3D(covariance, kFakePositionCovariance,
kFakeOrientationCovariance),
options_.lower_covariance_eigenvalue_bound())},
Constraint::INTER_SUBMAP});
constraint->reset(
new Constraint{submap_id,
node_id,
{transform::Embed3D(constraint_transform),
mapping::FromTranslationRotationWeights(
options_.loop_closure_translation_weight(),
options_.loop_closure_rotation_weight())},
Constraint::INTER_SUBMAP});
if (options_.log_matches()) {
std::ostringstream info;
@ -245,9 +239,7 @@ void ConstraintBuilder::ComputeConstraint(
<< difference.translation().norm() << " rotation "
<< std::setprecision(3) << std::abs(difference.normalized_angle());
}
info << " with score " << std::setprecision(1) << 100. * score
<< "% covariance trace " << std::scientific << std::setprecision(4)
<< covariance.trace() << ".";
info << " with score " << std::setprecision(1) << 100. * score << "%.";
LOG(INFO) << info.str();
}
}

14
cartographer/mapping_2d/sparse_pose_graph_test.cc
View File

@ -79,6 +79,8 @@ class SparsePoseGraphTest : public ::testing::Test {
min_score = 0.5,
global_localization_min_score = 0.6,
lower_covariance_eigenvalue_bound = 1e-6,
loop_closure_translation_weight = 1.,
loop_closure_rotation_weight = 1.,
log_matches = true,
fast_correlative_scan_matcher = {
linear_search_window = 3.,
@ -89,7 +91,6 @@ class SparsePoseGraphTest : public ::testing::Test {
occupied_space_weight = 20.,
translation_weight = 10.,
rotation_weight = 1.,
covariance_scale = 1.,
ceres_solver_options = {
use_nonmonotonic_steps = true,
max_num_iterations = 50,
@ -117,6 +118,8 @@ class SparsePoseGraphTest : public ::testing::Test {
},
},
},
matcher_translation_weight = 1.,
matcher_rotation_weight = 1.,
optimization_problem = {
acceleration_weight = 1.,
rotation_weight = 1e2,
@ -147,8 +150,6 @@ class SparsePoseGraphTest : public ::testing::Test {
const sensor::PointCloud new_point_cloud = sensor::TransformPointCloud(
point_cloud_,
transform::Embed3D(current_pose_.inverse().cast<float>()));
kalman_filter::Pose2DCovariance covariance =
kalman_filter::Pose2DCovariance::Identity();
const mapping::Submap* const matching_submap =
submaps_->Get(submaps_->matching_index());
std::vector<const mapping::Submap*> insertion_submaps;
@ -161,10 +162,9 @@ class SparsePoseGraphTest : public ::testing::Test {
constexpr int kTrajectoryId = 0;
submaps_->InsertRangeData(TransformRangeData(
range_data, transform::Embed3D(pose_estimate.cast<float>())));
sparse_pose_graph_->AddScan(common::FromUniversal(0),
transform::Rigid3d::Identity(), range_data,
pose_estimate, covariance, kTrajectoryId,
matching_submap, insertion_submaps);
sparse_pose_graph_->AddScan(
common::FromUniversal(0), transform::Rigid3d::Identity(), range_data,
pose_estimate, kTrajectoryId, matching_submap, insertion_submaps);
}
void MoveRelative(const transform::Rigid2d& movement) {

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

@ -46,8 +46,8 @@ class CeresScanMatcher {
CeresScanMatcher& operator=(const CeresScanMatcher&) = delete;
// Aligns 'point_clouds' within the 'hybrid_grids' given an
// 'initial_pose_estimate' and returns 'pose_estimate', and
// the solver 'summary'.
// 'initial_pose_estimate' and returns a 'pose_estimate' and the solver
// 'summary'.
void Match(const transform::Rigid3d& previous_pose,
const transform::Rigid3d& initial_pose_estimate,
const std::vector<PointCloudAndHybridGridPointers>&

5
configuration_files/sparse_pose_graph.lua
View File

@ -25,6 +25,8 @@ SPARSE_POSE_GRAPH = {
min_score = 0.55,
global_localization_min_score = 0.6,
lower_covariance_eigenvalue_bound = 1e-11,
loop_closure_translation_weight = 1.1e4,
loop_closure_rotation_weight = 1e5,
log_matches = true,
fast_correlative_scan_matcher = {
linear_search_window = 7.,
@ -35,7 +37,6 @@ SPARSE_POSE_GRAPH = {
occupied_space_weight = 20.,
translation_weight = 10.,
rotation_weight = 1.,
covariance_scale = 1e-4,
ceres_solver_options = {
use_nonmonotonic_steps = true,
max_num_iterations = 10,
@ -63,6 +64,8 @@ SPARSE_POSE_GRAPH = {
},
},
},
matcher_translation_weight = 5e2,
matcher_rotation_weight = 1.6e3,
optimization_problem = {
huber_scale = 1e1,
acceleration_weight = 1e4,

1
configuration_files/trajectory_builder_2d.lua
View File

@ -39,7 +39,6 @@ TRAJECTORY_BUILDER_2D = {
occupied_space_weight = 1e1,
translation_weight = 1e1,
rotation_weight = 1e2,
covariance_scale = 1e-2,
ceres_solver_options = {
use_nonmonotonic_steps = false,
max_num_iterations = 20,