Remove covariance computation from branch-and-bound. (#27)
These covariances were only used in 2D and are only an estimate. Following 3D, we change 2D to use the (local) covariance computed using Ceres.master
parent
14355a91a1
commit
e526a7022f
cartographer
mapping_3d/sparse_pose_graph
configuration_files
|
@ -58,7 +58,6 @@ google_library(mapping_2d_scan_matching_fast_correlative_scan_matcher
|
||||||
DEPENDS
|
DEPENDS
|
||||||
common_math
|
common_math
|
||||||
common_port
|
common_port
|
||||||
kalman_filter_pose_tracker
|
|
||||||
mapping_2d_probability_grid
|
mapping_2d_probability_grid
|
||||||
mapping_2d_scan_matching_correlative_scan_matcher
|
mapping_2d_scan_matching_correlative_scan_matcher
|
||||||
mapping_2d_scan_matching_proto_fast_correlative_scan_matcher_options
|
mapping_2d_scan_matching_proto_fast_correlative_scan_matcher_options
|
||||||
|
@ -74,7 +73,6 @@ google_library(mapping_2d_scan_matching_fast_global_localizer
|
||||||
HDRS
|
HDRS
|
||||||
fast_global_localizer.h
|
fast_global_localizer.h
|
||||||
DEPENDS
|
DEPENDS
|
||||||
kalman_filter_pose_tracker
|
|
||||||
mapping_2d_scan_matching_fast_correlative_scan_matcher
|
mapping_2d_scan_matching_fast_correlative_scan_matcher
|
||||||
sensor_voxel_filter
|
sensor_voxel_filter
|
||||||
)
|
)
|
||||||
|
|
|
@ -24,7 +24,6 @@
|
||||||
|
|
||||||
#include "Eigen/Geometry"
|
#include "Eigen/Geometry"
|
||||||
#include "cartographer/common/math.h"
|
#include "cartographer/common/math.h"
|
||||||
#include "cartographer/kalman_filter/pose_tracker.h"
|
|
||||||
#include "cartographer/mapping_2d/probability_grid.h"
|
#include "cartographer/mapping_2d/probability_grid.h"
|
||||||
#include "cartographer/sensor/point_cloud.h"
|
#include "cartographer/sensor/point_cloud.h"
|
||||||
#include "cartographer/transform/transform.h"
|
#include "cartographer/transform/transform.h"
|
||||||
|
@ -86,8 +85,6 @@ CreateFastCorrelativeScanMatcherOptions(
|
||||||
parameter_dictionary->GetDouble("angular_search_window"));
|
parameter_dictionary->GetDouble("angular_search_window"));
|
||||||
options.set_branch_and_bound_depth(
|
options.set_branch_and_bound_depth(
|
||||||
parameter_dictionary->GetInt("branch_and_bound_depth"));
|
parameter_dictionary->GetInt("branch_and_bound_depth"));
|
||||||
options.set_covariance_scale(
|
|
||||||
parameter_dictionary->GetDouble("covariance_scale"));
|
|
||||||
return options;
|
return options;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -212,20 +209,18 @@ FastCorrelativeScanMatcher::~FastCorrelativeScanMatcher() {}
|
||||||
bool FastCorrelativeScanMatcher::Match(
|
bool FastCorrelativeScanMatcher::Match(
|
||||||
const transform::Rigid2d& initial_pose_estimate,
|
const transform::Rigid2d& initial_pose_estimate,
|
||||||
const sensor::PointCloud2D& point_cloud, const float min_score,
|
const sensor::PointCloud2D& point_cloud, const float min_score,
|
||||||
float* score, transform::Rigid2d* pose_estimate,
|
float* score, transform::Rigid2d* pose_estimate) const {
|
||||||
kalman_filter::Pose2DCovariance* covariance) const {
|
|
||||||
const SearchParameters search_parameters(options_.linear_search_window(),
|
const SearchParameters search_parameters(options_.linear_search_window(),
|
||||||
options_.angular_search_window(),
|
options_.angular_search_window(),
|
||||||
point_cloud, limits_.resolution());
|
point_cloud, limits_.resolution());
|
||||||
return MatchWithSearchParameters(search_parameters, initial_pose_estimate,
|
return MatchWithSearchParameters(search_parameters, initial_pose_estimate,
|
||||||
point_cloud, min_score, score, pose_estimate,
|
point_cloud, min_score, score,
|
||||||
covariance);
|
pose_estimate);
|
||||||
}
|
}
|
||||||
|
|
||||||
bool FastCorrelativeScanMatcher::MatchFullSubmap(
|
bool FastCorrelativeScanMatcher::MatchFullSubmap(
|
||||||
const sensor::PointCloud2D& point_cloud, float min_score, float* score,
|
const sensor::PointCloud2D& point_cloud, float min_score, float* score,
|
||||||
transform::Rigid2d* pose_estimate,
|
transform::Rigid2d* pose_estimate) const {
|
||||||
kalman_filter::Pose2DCovariance* covariance) const {
|
|
||||||
// Compute a search window around the center of the submap that includes it
|
// Compute a search window around the center of the submap that includes it
|
||||||
// fully.
|
// fully.
|
||||||
const SearchParameters search_parameters(
|
const SearchParameters search_parameters(
|
||||||
|
@ -238,18 +233,16 @@ bool FastCorrelativeScanMatcher::MatchFullSubmap(
|
||||||
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,
|
return MatchWithSearchParameters(search_parameters, center, point_cloud,
|
||||||
min_score, score, pose_estimate, covariance);
|
min_score, score, pose_estimate);
|
||||||
}
|
}
|
||||||
|
|
||||||
bool FastCorrelativeScanMatcher::MatchWithSearchParameters(
|
bool FastCorrelativeScanMatcher::MatchWithSearchParameters(
|
||||||
SearchParameters search_parameters,
|
SearchParameters search_parameters,
|
||||||
const transform::Rigid2d& initial_pose_estimate,
|
const transform::Rigid2d& initial_pose_estimate,
|
||||||
const sensor::PointCloud2D& point_cloud, float min_score, float* score,
|
const sensor::PointCloud2D& point_cloud, float min_score, float* score,
|
||||||
transform::Rigid2d* pose_estimate,
|
transform::Rigid2d* pose_estimate) const {
|
||||||
kalman_filter::Pose2DCovariance* covariance) const {
|
|
||||||
CHECK_NOTNULL(score);
|
CHECK_NOTNULL(score);
|
||||||
CHECK_NOTNULL(pose_estimate);
|
CHECK_NOTNULL(pose_estimate);
|
||||||
CHECK_NOTNULL(covariance);
|
|
||||||
|
|
||||||
const Eigen::Rotation2Dd initial_rotation = initial_pose_estimate.rotation();
|
const Eigen::Rotation2Dd initial_rotation = initial_pose_estimate.rotation();
|
||||||
const sensor::PointCloud2D rotated_point_cloud =
|
const sensor::PointCloud2D rotated_point_cloud =
|
||||||
|
@ -266,22 +259,15 @@ bool FastCorrelativeScanMatcher::MatchWithSearchParameters(
|
||||||
|
|
||||||
const std::vector<Candidate> lowest_resolution_candidates =
|
const std::vector<Candidate> lowest_resolution_candidates =
|
||||||
ComputeLowestResolutionCandidates(discrete_scans, search_parameters);
|
ComputeLowestResolutionCandidates(discrete_scans, search_parameters);
|
||||||
// The following are intermediate results for computing covariance. See
|
|
||||||
// "Real-Time Correlative Scan Matching" by Olson.
|
|
||||||
Eigen::Matrix3d K = Eigen::Matrix3d::Zero();
|
|
||||||
Eigen::Vector3d u = Eigen::Vector3d::Zero();
|
|
||||||
double s = 0.;
|
|
||||||
const Candidate best_candidate = BranchAndBound(
|
const Candidate best_candidate = BranchAndBound(
|
||||||
discrete_scans, search_parameters, lowest_resolution_candidates,
|
discrete_scans, search_parameters, lowest_resolution_candidates,
|
||||||
precomputation_grid_stack_->max_depth(), min_score, &K, &u, &s);
|
precomputation_grid_stack_->max_depth(), min_score);
|
||||||
if (best_candidate.score > min_score) {
|
if (best_candidate.score > min_score) {
|
||||||
*score = best_candidate.score;
|
*score = best_candidate.score;
|
||||||
*pose_estimate = transform::Rigid2d(
|
*pose_estimate = transform::Rigid2d(
|
||||||
{initial_pose_estimate.translation().x() + best_candidate.x,
|
{initial_pose_estimate.translation().x() + best_candidate.x,
|
||||||
initial_pose_estimate.translation().y() + best_candidate.y},
|
initial_pose_estimate.translation().y() + best_candidate.y},
|
||||||
initial_rotation * Eigen::Rotation2Dd(best_candidate.orientation));
|
initial_rotation * Eigen::Rotation2Dd(best_candidate.orientation));
|
||||||
*covariance = (1. / s) * K - (1. / (s * s)) * (u * u.transpose());
|
|
||||||
*covariance *= options_.covariance_scale();
|
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
return false;
|
return false;
|
||||||
|
@ -361,16 +347,8 @@ Candidate FastCorrelativeScanMatcher::BranchAndBound(
|
||||||
const std::vector<DiscreteScan>& discrete_scans,
|
const std::vector<DiscreteScan>& discrete_scans,
|
||||||
const SearchParameters& search_parameters,
|
const SearchParameters& search_parameters,
|
||||||
const std::vector<Candidate>& candidates, const int candidate_depth,
|
const std::vector<Candidate>& candidates, const int candidate_depth,
|
||||||
float min_score, Eigen::Matrix3d* K, Eigen::Vector3d* u, double* s) const {
|
float min_score) const {
|
||||||
if (candidate_depth == 0) {
|
if (candidate_depth == 0) {
|
||||||
// Update the covariance computation intermediate values.
|
|
||||||
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;
|
|
||||||
}
|
|
||||||
// Return the best candidate.
|
// Return the best candidate.
|
||||||
return *candidates.begin();
|
return *candidates.begin();
|
||||||
}
|
}
|
||||||
|
@ -405,7 +383,7 @@ Candidate FastCorrelativeScanMatcher::BranchAndBound(
|
||||||
best_high_resolution_candidate,
|
best_high_resolution_candidate,
|
||||||
BranchAndBound(discrete_scans, search_parameters,
|
BranchAndBound(discrete_scans, search_parameters,
|
||||||
higher_resolution_candidates, candidate_depth - 1,
|
higher_resolution_candidates, candidate_depth - 1,
|
||||||
best_high_resolution_candidate.score, K, u, s));
|
best_high_resolution_candidate.score));
|
||||||
}
|
}
|
||||||
return best_high_resolution_candidate;
|
return best_high_resolution_candidate;
|
||||||
}
|
}
|
||||||
|
|
|
@ -30,7 +30,6 @@
|
||||||
|
|
||||||
#include "Eigen/Core"
|
#include "Eigen/Core"
|
||||||
#include "cartographer/common/port.h"
|
#include "cartographer/common/port.h"
|
||||||
#include "cartographer/kalman_filter/pose_tracker.h"
|
|
||||||
#include "cartographer/mapping_2d/probability_grid.h"
|
#include "cartographer/mapping_2d/probability_grid.h"
|
||||||
#include "cartographer/mapping_2d/scan_matching/correlative_scan_matcher.h"
|
#include "cartographer/mapping_2d/scan_matching/correlative_scan_matcher.h"
|
||||||
#include "cartographer/mapping_2d/scan_matching/proto/fast_correlative_scan_matcher_options.pb.h"
|
#include "cartographer/mapping_2d/scan_matching/proto/fast_correlative_scan_matcher_options.pb.h"
|
||||||
|
@ -109,20 +108,18 @@ class FastCorrelativeScanMatcher {
|
||||||
|
|
||||||
// Aligns 'point_cloud' within the 'probability_grid' given an
|
// Aligns 'point_cloud' within the 'probability_grid' given an
|
||||||
// 'initial_pose_estimate'. If a score above 'min_score' (excluding equality)
|
// 'initial_pose_estimate'. If a score above 'min_score' (excluding equality)
|
||||||
// is possible, true is returned, and 'score', 'pose_estimate' and
|
// is possible, true is returned, and 'score' and 'pose_estimate' are updated
|
||||||
// 'covariance' are updated with the result.
|
// with the result.
|
||||||
bool Match(const transform::Rigid2d& initial_pose_estimate,
|
bool Match(const transform::Rigid2d& initial_pose_estimate,
|
||||||
const sensor::PointCloud2D& point_cloud, float min_score,
|
const sensor::PointCloud2D& point_cloud, float min_score,
|
||||||
float* score, transform::Rigid2d* pose_estimate,
|
float* score, transform::Rigid2d* pose_estimate) const;
|
||||||
kalman_filter::Pose2DCovariance* covariance) const;
|
|
||||||
|
|
||||||
// Aligns 'point_cloud' within the full 'probability_grid', i.e., not
|
// Aligns 'point_cloud' within the full 'probability_grid', i.e., not
|
||||||
// restricted to the configured search window. If a score above 'min_score'
|
// restricted to the configured search window. If a score above 'min_score'
|
||||||
// (excluding equality) is possible, true is returned, and 'score',
|
// (excluding equality) is possible, true is returned, and 'score' and
|
||||||
// 'pose_estimate' and 'covariance' are updated with the result.
|
// 'pose_estimate' are updated with the result.
|
||||||
bool MatchFullSubmap(const sensor::PointCloud2D& point_cloud, float min_score,
|
bool MatchFullSubmap(const sensor::PointCloud2D& point_cloud, float min_score,
|
||||||
float* score, transform::Rigid2d* pose_estimate,
|
float* score, transform::Rigid2d* pose_estimate) const;
|
||||||
kalman_filter::Pose2DCovariance* covariance) const;
|
|
||||||
|
|
||||||
private:
|
private:
|
||||||
// The actual implementation of the scan matcher, called by Match() and
|
// The actual implementation of the scan matcher, called by Match() and
|
||||||
|
@ -132,8 +129,7 @@ class FastCorrelativeScanMatcher {
|
||||||
SearchParameters search_parameters,
|
SearchParameters search_parameters,
|
||||||
const transform::Rigid2d& initial_pose_estimate,
|
const transform::Rigid2d& initial_pose_estimate,
|
||||||
const sensor::PointCloud2D& point_cloud, float min_score, float* score,
|
const sensor::PointCloud2D& point_cloud, float min_score, float* score,
|
||||||
transform::Rigid2d* pose_estimate,
|
transform::Rigid2d* pose_estimate) const;
|
||||||
kalman_filter::Pose2DCovariance* covariance) const;
|
|
||||||
std::vector<Candidate> ComputeLowestResolutionCandidates(
|
std::vector<Candidate> ComputeLowestResolutionCandidates(
|
||||||
const std::vector<DiscreteScan>& discrete_scans,
|
const std::vector<DiscreteScan>& discrete_scans,
|
||||||
const SearchParameters& search_parameters) const;
|
const SearchParameters& search_parameters) const;
|
||||||
|
@ -146,9 +142,7 @@ class FastCorrelativeScanMatcher {
|
||||||
Candidate BranchAndBound(const std::vector<DiscreteScan>& discrete_scans,
|
Candidate BranchAndBound(const std::vector<DiscreteScan>& discrete_scans,
|
||||||
const SearchParameters& search_parameters,
|
const SearchParameters& search_parameters,
|
||||||
const std::vector<Candidate>& candidates,
|
const std::vector<Candidate>& candidates,
|
||||||
int candidate_depth, float min_score,
|
int candidate_depth, float min_score) const;
|
||||||
Eigen::Matrix3d* K, Eigen::Vector3d* u,
|
|
||||||
double* s) const;
|
|
||||||
|
|
||||||
const proto::FastCorrelativeScanMatcherOptions options_;
|
const proto::FastCorrelativeScanMatcherOptions options_;
|
||||||
MapLimits limits_;
|
MapLimits limits_;
|
||||||
|
|
|
@ -111,7 +111,6 @@ CreateFastCorrelativeScanMatcherTestOptions(const int branch_and_bound_depth) {
|
||||||
return {
|
return {
|
||||||
linear_search_window = 3.,
|
linear_search_window = 3.,
|
||||||
angular_search_window = 1.,
|
angular_search_window = 1.,
|
||||||
covariance_scale = 1.,
|
|
||||||
branch_and_bound_depth = )text" +
|
branch_and_bound_depth = )text" +
|
||||||
std::to_string(branch_and_bound_depth) + "}");
|
std::to_string(branch_and_bound_depth) + "}");
|
||||||
return CreateFastCorrelativeScanMatcherOptions(parameter_dictionary.get());
|
return CreateFastCorrelativeScanMatcherOptions(parameter_dictionary.get());
|
||||||
|
@ -159,11 +158,10 @@ TEST(FastCorrelativeScanMatcherTest, CorrectPose) {
|
||||||
FastCorrelativeScanMatcher fast_correlative_scan_matcher(probability_grid,
|
FastCorrelativeScanMatcher fast_correlative_scan_matcher(probability_grid,
|
||||||
options);
|
options);
|
||||||
transform::Rigid2d pose_estimate;
|
transform::Rigid2d pose_estimate;
|
||||||
kalman_filter::Pose2DCovariance unused_covariance;
|
|
||||||
float score;
|
float score;
|
||||||
EXPECT_TRUE(fast_correlative_scan_matcher.Match(
|
EXPECT_TRUE(fast_correlative_scan_matcher.Match(
|
||||||
transform::Rigid2d::Identity(), point_cloud, kMinScore, &score,
|
transform::Rigid2d::Identity(), point_cloud, kMinScore, &score,
|
||||||
&pose_estimate, &unused_covariance));
|
&pose_estimate));
|
||||||
EXPECT_LT(kMinScore, score);
|
EXPECT_LT(kMinScore, score);
|
||||||
EXPECT_THAT(expected_pose,
|
EXPECT_THAT(expected_pose,
|
||||||
transform::IsNearly(pose_estimate.cast<float>(), 0.03f))
|
transform::IsNearly(pose_estimate.cast<float>(), 0.03f))
|
||||||
|
@ -211,10 +209,9 @@ TEST(FastCorrelativeScanMatcherTest, FullSubmapMatching) {
|
||||||
FastCorrelativeScanMatcher fast_correlative_scan_matcher(probability_grid,
|
FastCorrelativeScanMatcher fast_correlative_scan_matcher(probability_grid,
|
||||||
options);
|
options);
|
||||||
transform::Rigid2d pose_estimate;
|
transform::Rigid2d pose_estimate;
|
||||||
kalman_filter::Pose2DCovariance unused_covariance;
|
|
||||||
float score;
|
float score;
|
||||||
EXPECT_TRUE(fast_correlative_scan_matcher.MatchFullSubmap(
|
EXPECT_TRUE(fast_correlative_scan_matcher.MatchFullSubmap(
|
||||||
point_cloud, kMinScore, &score, &pose_estimate, &unused_covariance));
|
point_cloud, kMinScore, &score, &pose_estimate));
|
||||||
EXPECT_LT(kMinScore, score);
|
EXPECT_LT(kMinScore, score);
|
||||||
EXPECT_THAT(expected_pose,
|
EXPECT_THAT(expected_pose,
|
||||||
transform::IsNearly(pose_estimate.cast<float>(), 0.03f))
|
transform::IsNearly(pose_estimate.cast<float>(), 0.03f))
|
||||||
|
|
|
@ -16,7 +16,6 @@
|
||||||
|
|
||||||
#include "cartographer/mapping_2d/scan_matching/fast_global_localizer.h"
|
#include "cartographer/mapping_2d/scan_matching/fast_global_localizer.h"
|
||||||
|
|
||||||
#include "cartographer/kalman_filter/pose_tracker.h"
|
|
||||||
#include "glog/logging.h"
|
#include "glog/logging.h"
|
||||||
|
|
||||||
namespace cartographer {
|
namespace cartographer {
|
||||||
|
@ -36,7 +35,6 @@ bool PerformGlobalLocalization(
|
||||||
CHECK(best_score != nullptr) << "Need a non-null best_score!";
|
CHECK(best_score != nullptr) << "Need a non-null best_score!";
|
||||||
*best_score = cutoff;
|
*best_score = cutoff;
|
||||||
transform::Rigid2d pose_estimate;
|
transform::Rigid2d pose_estimate;
|
||||||
kalman_filter::Pose2DCovariance best_pose_estimate_covariance;
|
|
||||||
const sensor::PointCloud2D filtered_point_cloud =
|
const sensor::PointCloud2D filtered_point_cloud =
|
||||||
voxel_filter.Filter(point_cloud);
|
voxel_filter.Filter(point_cloud);
|
||||||
bool success = false;
|
bool success = false;
|
||||||
|
@ -47,13 +45,11 @@ bool PerformGlobalLocalization(
|
||||||
for (auto& matcher : matchers) {
|
for (auto& matcher : matchers) {
|
||||||
float score = -1;
|
float score = -1;
|
||||||
transform::Rigid2d pose_estimate;
|
transform::Rigid2d pose_estimate;
|
||||||
kalman_filter::Pose2DCovariance pose_estimate_covariance;
|
|
||||||
if (matcher->MatchFullSubmap(filtered_point_cloud, *best_score, &score,
|
if (matcher->MatchFullSubmap(filtered_point_cloud, *best_score, &score,
|
||||||
&pose_estimate, &pose_estimate_covariance)) {
|
&pose_estimate)) {
|
||||||
CHECK_GT(score, *best_score) << "MatchFullSubmap lied!";
|
CHECK_GT(score, *best_score) << "MatchFullSubmap lied!";
|
||||||
*best_score = score;
|
*best_score = score;
|
||||||
*best_pose_estimate = pose_estimate;
|
*best_pose_estimate = pose_estimate;
|
||||||
best_pose_estimate_covariance = pose_estimate_covariance;
|
|
||||||
success = true;
|
success = true;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
@ -27,7 +27,4 @@ message FastCorrelativeScanMatcherOptions {
|
||||||
|
|
||||||
// Number of precomputed grids to use.
|
// Number of precomputed grids to use.
|
||||||
optional int32 branch_and_bound_depth = 2;
|
optional int32 branch_and_bound_depth = 2;
|
||||||
|
|
||||||
// Covariance estimate is multiplied by this scale.
|
|
||||||
optional double covariance_scale = 5;
|
|
||||||
};
|
};
|
||||||
|
|
|
@ -191,12 +191,11 @@ void ConstraintBuilder::ComputeConstraint(
|
||||||
// - the ComputeSubmapPose() (map <- i)
|
// - the ComputeSubmapPose() (map <- i)
|
||||||
float score = 0.;
|
float score = 0.;
|
||||||
transform::Rigid2d pose_estimate = transform::Rigid2d::Identity();
|
transform::Rigid2d pose_estimate = transform::Rigid2d::Identity();
|
||||||
kalman_filter::Pose2DCovariance covariance;
|
|
||||||
|
|
||||||
if (match_full_submap) {
|
if (match_full_submap) {
|
||||||
if (submap_scan_matcher->fast_correlative_scan_matcher->MatchFullSubmap(
|
if (submap_scan_matcher->fast_correlative_scan_matcher->MatchFullSubmap(
|
||||||
filtered_point_cloud, options_.global_localization_min_score(),
|
filtered_point_cloud, options_.global_localization_min_score(),
|
||||||
&score, &pose_estimate, &covariance)) {
|
&score, &pose_estimate)) {
|
||||||
trajectory_connectivity->Connect(scan_trajectory, submap_trajectory);
|
trajectory_connectivity->Connect(scan_trajectory, submap_trajectory);
|
||||||
} else {
|
} else {
|
||||||
return;
|
return;
|
||||||
|
@ -204,12 +203,11 @@ void ConstraintBuilder::ComputeConstraint(
|
||||||
} else {
|
} else {
|
||||||
if (!submap_scan_matcher->fast_correlative_scan_matcher->Match(
|
if (!submap_scan_matcher->fast_correlative_scan_matcher->Match(
|
||||||
initial_pose, filtered_point_cloud, options_.min_score(), &score,
|
initial_pose, filtered_point_cloud, options_.min_score(), &score,
|
||||||
&pose_estimate, &covariance)) {
|
&pose_estimate)) {
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
// We've reported a successful local match.
|
// We've reported a successful local match.
|
||||||
CHECK_GT(score, options_.min_score());
|
CHECK_GT(score, options_.min_score());
|
||||||
// 'covariance' is unchanged as (submap <- map) is a translation.
|
|
||||||
{
|
{
|
||||||
common::MutexLocker locker(&mutex_);
|
common::MutexLocker locker(&mutex_);
|
||||||
score_histogram_.Add(score);
|
score_histogram_.Add(score);
|
||||||
|
@ -218,15 +216,13 @@ void ConstraintBuilder::ComputeConstraint(
|
||||||
|
|
||||||
// Use the CSM estimate as both the initial and previous pose. This has the
|
// Use the CSM estimate as both the initial and previous pose. This has the
|
||||||
// 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. We also prefer to use the CSM covariance estimate for loop
|
// CSM estimate.
|
||||||
// closure constraints since it is representative of a larger area (as opposed
|
|
||||||
// to the local Ceres estimate of covariance).
|
|
||||||
ceres::Solver::Summary unused_summary;
|
ceres::Solver::Summary unused_summary;
|
||||||
kalman_filter::Pose2DCovariance unused_covariance;
|
kalman_filter::Pose2DCovariance covariance;
|
||||||
ceres_scan_matcher_.Match(pose_estimate, pose_estimate, filtered_point_cloud,
|
ceres_scan_matcher_.Match(pose_estimate, pose_estimate, filtered_point_cloud,
|
||||||
*submap_scan_matcher->probability_grid,
|
*submap_scan_matcher->probability_grid,
|
||||||
&pose_estimate, &unused_covariance,
|
&pose_estimate, &covariance, &unused_summary);
|
||||||
&unused_summary);
|
// 'covariance' is unchanged as (submap <- map) is a translation.
|
||||||
|
|
||||||
const transform::Rigid2d constraint_transform =
|
const transform::Rigid2d constraint_transform =
|
||||||
ComputeSubmapPose(*submap).inverse() * pose_estimate;
|
ComputeSubmapPose(*submap).inverse() * pose_estimate;
|
||||||
|
|
|
@ -85,7 +85,6 @@ class SparsePoseGraphTest : public ::testing::Test {
|
||||||
linear_search_window = 3.,
|
linear_search_window = 3.,
|
||||||
angular_search_window = 0.1,
|
angular_search_window = 0.1,
|
||||||
branch_and_bound_depth = 3,
|
branch_and_bound_depth = 3,
|
||||||
covariance_scale = 1.,
|
|
||||||
},
|
},
|
||||||
ceres_scan_matcher = {
|
ceres_scan_matcher = {
|
||||||
occupied_space_cost_functor_weight = 20.,
|
occupied_space_cost_functor_weight = 20.,
|
||||||
|
|
|
@ -237,9 +237,7 @@ void ConstraintBuilder::ComputeConstraint(
|
||||||
|
|
||||||
// Use the CSM estimate as both the initial and previous pose. This has the
|
// Use the CSM estimate as both the initial and previous pose. This has the
|
||||||
// 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. We also prefer to use the CSM covariance estimate for loop
|
// CSM estimate.
|
||||||
// closure constraints since it is representative of a larger area (as opposed
|
|
||||||
// to the local Ceres estimate of covariance).
|
|
||||||
ceres::Solver::Summary unused_summary;
|
ceres::Solver::Summary unused_summary;
|
||||||
kalman_filter::PoseCovariance covariance;
|
kalman_filter::PoseCovariance covariance;
|
||||||
ceres_scan_matcher_.Match(
|
ceres_scan_matcher_.Match(
|
||||||
|
|
|
@ -31,13 +31,12 @@ SPARSE_POSE_GRAPH = {
|
||||||
linear_search_window = 7.,
|
linear_search_window = 7.,
|
||||||
angular_search_window = math.rad(30.),
|
angular_search_window = math.rad(30.),
|
||||||
branch_and_bound_depth = 7,
|
branch_and_bound_depth = 7,
|
||||||
covariance_scale = 1e-6,
|
|
||||||
},
|
},
|
||||||
ceres_scan_matcher = {
|
ceres_scan_matcher = {
|
||||||
occupied_space_cost_functor_weight = 20.,
|
occupied_space_cost_functor_weight = 20.,
|
||||||
previous_pose_translation_delta_cost_functor_weight = 10.,
|
previous_pose_translation_delta_cost_functor_weight = 10.,
|
||||||
initial_pose_estimate_rotation_delta_cost_functor_weight = 1.,
|
initial_pose_estimate_rotation_delta_cost_functor_weight = 1.,
|
||||||
covariance_scale = 1e-6,
|
covariance_scale = 1e-4,
|
||||||
ceres_solver_options = {
|
ceres_solver_options = {
|
||||||
use_nonmonotonic_steps = true,
|
use_nonmonotonic_steps = true,
|
||||||
max_num_iterations = 10,
|
max_num_iterations = 10,
|
||||||
|
@ -67,7 +66,7 @@ SPARSE_POSE_GRAPH = {
|
||||||
},
|
},
|
||||||
},
|
},
|
||||||
optimization_problem = {
|
optimization_problem = {
|
||||||
huber_scale = 5e2,
|
huber_scale = 1e1,
|
||||||
acceleration_scale = 7e4,
|
acceleration_scale = 7e4,
|
||||||
rotation_scale = 3e6,
|
rotation_scale = 3e6,
|
||||||
consecutive_scan_translation_penalty_factor = 1e5,
|
consecutive_scan_translation_penalty_factor = 1e5,
|
||||||
|
|
Loading…
Reference in New Issue