parent
eabcab26ed
commit
94fce13f62
|
@ -128,13 +128,12 @@ class GlobalTrajectoryBuilder : public mapping::TrajectoryBuilderInterface {
|
||||||
} // namespace
|
} // namespace
|
||||||
|
|
||||||
std::unique_ptr<TrajectoryBuilderInterface> CreateGlobalTrajectoryBuilder2D(
|
std::unique_ptr<TrajectoryBuilderInterface> CreateGlobalTrajectoryBuilder2D(
|
||||||
std::unique_ptr<mapping_2d::LocalTrajectoryBuilder>
|
std::unique_ptr<LocalTrajectoryBuilder2D> local_trajectory_builder,
|
||||||
local_trajectory_builder,
|
|
||||||
const int trajectory_id, mapping::PoseGraph2D* const pose_graph,
|
const int trajectory_id, mapping::PoseGraph2D* const pose_graph,
|
||||||
const TrajectoryBuilderInterface::LocalSlamResultCallback&
|
const TrajectoryBuilderInterface::LocalSlamResultCallback&
|
||||||
local_slam_result_callback) {
|
local_slam_result_callback) {
|
||||||
return common::make_unique<GlobalTrajectoryBuilder<
|
return common::make_unique<
|
||||||
mapping_2d::LocalTrajectoryBuilder, mapping::PoseGraph2D>>(
|
GlobalTrajectoryBuilder<LocalTrajectoryBuilder2D, mapping::PoseGraph2D>>(
|
||||||
std::move(local_trajectory_builder), trajectory_id, pose_graph,
|
std::move(local_trajectory_builder), trajectory_id, pose_graph,
|
||||||
local_slam_result_callback);
|
local_slam_result_callback);
|
||||||
}
|
}
|
||||||
|
|
|
@ -19,7 +19,7 @@
|
||||||
|
|
||||||
#include <memory>
|
#include <memory>
|
||||||
|
|
||||||
#include "cartographer/internal/mapping_2d/local_trajectory_builder.h"
|
#include "cartographer/internal/mapping_2d/local_trajectory_builder_2d.h"
|
||||||
#include "cartographer/internal/mapping_3d/local_trajectory_builder.h"
|
#include "cartographer/internal/mapping_3d/local_trajectory_builder.h"
|
||||||
#include "cartographer/mapping/local_slam_result_data.h"
|
#include "cartographer/mapping/local_slam_result_data.h"
|
||||||
#include "cartographer/mapping/trajectory_builder_interface.h"
|
#include "cartographer/mapping/trajectory_builder_interface.h"
|
||||||
|
@ -30,8 +30,7 @@ namespace cartographer {
|
||||||
namespace mapping {
|
namespace mapping {
|
||||||
|
|
||||||
std::unique_ptr<TrajectoryBuilderInterface> CreateGlobalTrajectoryBuilder2D(
|
std::unique_ptr<TrajectoryBuilderInterface> CreateGlobalTrajectoryBuilder2D(
|
||||||
std::unique_ptr<mapping_2d::LocalTrajectoryBuilder>
|
std::unique_ptr<LocalTrajectoryBuilder2D> local_trajectory_builder,
|
||||||
local_trajectory_builder,
|
|
||||||
const int trajectory_id, mapping::PoseGraph2D* const pose_graph,
|
const int trajectory_id, mapping::PoseGraph2D* const pose_graph,
|
||||||
const TrajectoryBuilderInterface::LocalSlamResultCallback&
|
const TrajectoryBuilderInterface::LocalSlamResultCallback&
|
||||||
local_slam_result_callback);
|
local_slam_result_callback);
|
||||||
|
|
|
@ -14,7 +14,7 @@
|
||||||
* limitations under the License.
|
* limitations under the License.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#include "cartographer/internal/mapping_2d/local_trajectory_builder.h"
|
#include "cartographer/internal/mapping_2d/local_trajectory_builder_2d.h"
|
||||||
|
|
||||||
#include <limits>
|
#include <limits>
|
||||||
#include <memory>
|
#include <memory>
|
||||||
|
@ -23,10 +23,10 @@
|
||||||
#include "cartographer/sensor/range_data.h"
|
#include "cartographer/sensor/range_data.h"
|
||||||
|
|
||||||
namespace cartographer {
|
namespace cartographer {
|
||||||
namespace mapping_2d {
|
namespace mapping {
|
||||||
|
|
||||||
LocalTrajectoryBuilder::LocalTrajectoryBuilder(
|
LocalTrajectoryBuilder2D::LocalTrajectoryBuilder2D(
|
||||||
const mapping::proto::LocalTrajectoryBuilderOptions2D& options)
|
const proto::LocalTrajectoryBuilderOptions2D& options)
|
||||||
: options_(options),
|
: options_(options),
|
||||||
active_submaps_(options.submaps_options()),
|
active_submaps_(options.submaps_options()),
|
||||||
motion_filter_(options_.motion_filter_options()),
|
motion_filter_(options_.motion_filter_options()),
|
||||||
|
@ -34,10 +34,10 @@ LocalTrajectoryBuilder::LocalTrajectoryBuilder(
|
||||||
options_.real_time_correlative_scan_matcher_options()),
|
options_.real_time_correlative_scan_matcher_options()),
|
||||||
ceres_scan_matcher_(options_.ceres_scan_matcher_options()) {}
|
ceres_scan_matcher_(options_.ceres_scan_matcher_options()) {}
|
||||||
|
|
||||||
LocalTrajectoryBuilder::~LocalTrajectoryBuilder() {}
|
LocalTrajectoryBuilder2D::~LocalTrajectoryBuilder2D() {}
|
||||||
|
|
||||||
sensor::RangeData
|
sensor::RangeData
|
||||||
LocalTrajectoryBuilder::TransformToGravityAlignedFrameAndFilter(
|
LocalTrajectoryBuilder2D::TransformToGravityAlignedFrameAndFilter(
|
||||||
const transform::Rigid3f& transform_to_gravity_aligned_frame,
|
const transform::Rigid3f& transform_to_gravity_aligned_frame,
|
||||||
const sensor::RangeData& range_data) const {
|
const sensor::RangeData& range_data) const {
|
||||||
const sensor::RangeData cropped =
|
const sensor::RangeData cropped =
|
||||||
|
@ -50,10 +50,10 @@ LocalTrajectoryBuilder::TransformToGravityAlignedFrameAndFilter(
|
||||||
sensor::VoxelFilter(options_.voxel_filter_size()).Filter(cropped.misses)};
|
sensor::VoxelFilter(options_.voxel_filter_size()).Filter(cropped.misses)};
|
||||||
}
|
}
|
||||||
|
|
||||||
std::unique_ptr<transform::Rigid2d> LocalTrajectoryBuilder::ScanMatch(
|
std::unique_ptr<transform::Rigid2d> LocalTrajectoryBuilder2D::ScanMatch(
|
||||||
const common::Time time, const transform::Rigid2d& pose_prediction,
|
const common::Time time, const transform::Rigid2d& pose_prediction,
|
||||||
const sensor::RangeData& gravity_aligned_range_data) {
|
const sensor::RangeData& gravity_aligned_range_data) {
|
||||||
std::shared_ptr<const mapping::Submap2D> matching_submap =
|
std::shared_ptr<const Submap2D> matching_submap =
|
||||||
active_submaps_.submaps().front();
|
active_submaps_.submaps().front();
|
||||||
// The online correlative scan matcher will refine the initial estimate for
|
// The online correlative scan matcher will refine the initial estimate for
|
||||||
// the Ceres scan matcher.
|
// the Ceres scan matcher.
|
||||||
|
@ -80,9 +80,9 @@ std::unique_ptr<transform::Rigid2d> LocalTrajectoryBuilder::ScanMatch(
|
||||||
return pose_observation;
|
return pose_observation;
|
||||||
}
|
}
|
||||||
|
|
||||||
std::unique_ptr<LocalTrajectoryBuilder::MatchingResult>
|
std::unique_ptr<LocalTrajectoryBuilder2D::MatchingResult>
|
||||||
LocalTrajectoryBuilder::AddRangeData(const common::Time time,
|
LocalTrajectoryBuilder2D::AddRangeData(
|
||||||
const sensor::TimedRangeData& range_data) {
|
const common::Time time, const sensor::TimedRangeData& range_data) {
|
||||||
// Initialize extrapolator now if we do not ever use an IMU.
|
// Initialize extrapolator now if we do not ever use an IMU.
|
||||||
if (!options_.use_imu_data()) {
|
if (!options_.use_imu_data()) {
|
||||||
InitializeExtrapolator(time);
|
InitializeExtrapolator(time);
|
||||||
|
@ -155,8 +155,8 @@ LocalTrajectoryBuilder::AddRangeData(const common::Time time,
|
||||||
return nullptr;
|
return nullptr;
|
||||||
}
|
}
|
||||||
|
|
||||||
std::unique_ptr<LocalTrajectoryBuilder::MatchingResult>
|
std::unique_ptr<LocalTrajectoryBuilder2D::MatchingResult>
|
||||||
LocalTrajectoryBuilder::AddAccumulatedRangeData(
|
LocalTrajectoryBuilder2D::AddAccumulatedRangeData(
|
||||||
const common::Time time,
|
const common::Time time,
|
||||||
const sensor::RangeData& gravity_aligned_range_data,
|
const sensor::RangeData& gravity_aligned_range_data,
|
||||||
const transform::Rigid3d& gravity_alignment) {
|
const transform::Rigid3d& gravity_alignment) {
|
||||||
|
@ -193,8 +193,8 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData(
|
||||||
std::move(insertion_result)});
|
std::move(insertion_result)});
|
||||||
}
|
}
|
||||||
|
|
||||||
std::unique_ptr<LocalTrajectoryBuilder::InsertionResult>
|
std::unique_ptr<LocalTrajectoryBuilder2D::InsertionResult>
|
||||||
LocalTrajectoryBuilder::InsertIntoSubmap(
|
LocalTrajectoryBuilder2D::InsertIntoSubmap(
|
||||||
const common::Time time, const sensor::RangeData& range_data_in_local,
|
const common::Time time, const sensor::RangeData& range_data_in_local,
|
||||||
const sensor::RangeData& gravity_aligned_range_data,
|
const sensor::RangeData& gravity_aligned_range_data,
|
||||||
const transform::Rigid3d& pose_estimate,
|
const transform::Rigid3d& pose_estimate,
|
||||||
|
@ -205,9 +205,8 @@ LocalTrajectoryBuilder::InsertIntoSubmap(
|
||||||
|
|
||||||
// Querying the active submaps must be done here before calling
|
// Querying the active submaps must be done here before calling
|
||||||
// InsertRangeData() since the queried values are valid for next insertion.
|
// InsertRangeData() since the queried values are valid for next insertion.
|
||||||
std::vector<std::shared_ptr<const mapping::Submap2D>> insertion_submaps;
|
std::vector<std::shared_ptr<const Submap2D>> insertion_submaps;
|
||||||
for (const std::shared_ptr<mapping::Submap2D>& submap :
|
for (const std::shared_ptr<Submap2D>& submap : active_submaps_.submaps()) {
|
||||||
active_submaps_.submaps()) {
|
|
||||||
insertion_submaps.push_back(submap);
|
insertion_submaps.push_back(submap);
|
||||||
}
|
}
|
||||||
active_submaps_.InsertRangeData(range_data_in_local);
|
active_submaps_.InsertRangeData(range_data_in_local);
|
||||||
|
@ -218,8 +217,7 @@ LocalTrajectoryBuilder::InsertIntoSubmap(
|
||||||
adaptive_voxel_filter.Filter(gravity_aligned_range_data.returns);
|
adaptive_voxel_filter.Filter(gravity_aligned_range_data.returns);
|
||||||
|
|
||||||
return common::make_unique<InsertionResult>(InsertionResult{
|
return common::make_unique<InsertionResult>(InsertionResult{
|
||||||
std::make_shared<const mapping::TrajectoryNode::Data>(
|
std::make_shared<const TrajectoryNode::Data>(TrajectoryNode::Data{
|
||||||
mapping::TrajectoryNode::Data{
|
|
||||||
time,
|
time,
|
||||||
gravity_alignment,
|
gravity_alignment,
|
||||||
filtered_gravity_aligned_point_cloud,
|
filtered_gravity_aligned_point_cloud,
|
||||||
|
@ -230,13 +228,13 @@ LocalTrajectoryBuilder::InsertIntoSubmap(
|
||||||
std::move(insertion_submaps)});
|
std::move(insertion_submaps)});
|
||||||
}
|
}
|
||||||
|
|
||||||
void LocalTrajectoryBuilder::AddImuData(const sensor::ImuData& imu_data) {
|
void LocalTrajectoryBuilder2D::AddImuData(const sensor::ImuData& imu_data) {
|
||||||
CHECK(options_.use_imu_data()) << "An unexpected IMU packet was added.";
|
CHECK(options_.use_imu_data()) << "An unexpected IMU packet was added.";
|
||||||
InitializeExtrapolator(imu_data.time);
|
InitializeExtrapolator(imu_data.time);
|
||||||
extrapolator_->AddImuData(imu_data);
|
extrapolator_->AddImuData(imu_data);
|
||||||
}
|
}
|
||||||
|
|
||||||
void LocalTrajectoryBuilder::AddOdometryData(
|
void LocalTrajectoryBuilder2D::AddOdometryData(
|
||||||
const sensor::OdometryData& odometry_data) {
|
const sensor::OdometryData& odometry_data) {
|
||||||
if (extrapolator_ == nullptr) {
|
if (extrapolator_ == nullptr) {
|
||||||
// Until we've initialized the extrapolator we cannot add odometry data.
|
// Until we've initialized the extrapolator we cannot add odometry data.
|
||||||
|
@ -246,7 +244,7 @@ void LocalTrajectoryBuilder::AddOdometryData(
|
||||||
extrapolator_->AddOdometryData(odometry_data);
|
extrapolator_->AddOdometryData(odometry_data);
|
||||||
}
|
}
|
||||||
|
|
||||||
void LocalTrajectoryBuilder::InitializeExtrapolator(const common::Time time) {
|
void LocalTrajectoryBuilder2D::InitializeExtrapolator(const common::Time time) {
|
||||||
if (extrapolator_ != nullptr) {
|
if (extrapolator_ != nullptr) {
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
@ -255,11 +253,11 @@ void LocalTrajectoryBuilder::InitializeExtrapolator(const common::Time time) {
|
||||||
// in time and thus the last two are used.
|
// in time and thus the last two are used.
|
||||||
constexpr double kExtrapolationEstimationTimeSec = 0.001;
|
constexpr double kExtrapolationEstimationTimeSec = 0.001;
|
||||||
// TODO(gaschler): Consider using InitializeWithImu as 3D does.
|
// TODO(gaschler): Consider using InitializeWithImu as 3D does.
|
||||||
extrapolator_ = common::make_unique<mapping::PoseExtrapolator>(
|
extrapolator_ = common::make_unique<PoseExtrapolator>(
|
||||||
::cartographer::common::FromSeconds(kExtrapolationEstimationTimeSec),
|
::cartographer::common::FromSeconds(kExtrapolationEstimationTimeSec),
|
||||||
options_.imu_gravity_time_constant());
|
options_.imu_gravity_time_constant());
|
||||||
extrapolator_->AddPose(time, transform::Rigid3d::Identity());
|
extrapolator_->AddPose(time, transform::Rigid3d::Identity());
|
||||||
}
|
}
|
||||||
|
|
||||||
} // namespace mapping_2d
|
} // namespace mapping
|
||||||
} // namespace cartographer
|
} // namespace cartographer
|
|
@ -23,8 +23,8 @@
|
||||||
#include "cartographer/internal/mapping/motion_filter.h"
|
#include "cartographer/internal/mapping/motion_filter.h"
|
||||||
#include "cartographer/mapping/pose_extrapolator.h"
|
#include "cartographer/mapping/pose_extrapolator.h"
|
||||||
#include "cartographer/mapping_2d/proto/local_trajectory_builder_options_2d.pb.h"
|
#include "cartographer/mapping_2d/proto/local_trajectory_builder_options_2d.pb.h"
|
||||||
#include "cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h"
|
#include "cartographer/mapping_2d/scan_matching/ceres_scan_matcher_2d.h"
|
||||||
#include "cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.h"
|
#include "cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_2d.h"
|
||||||
#include "cartographer/mapping_2d/submap_2d.h"
|
#include "cartographer/mapping_2d/submap_2d.h"
|
||||||
#include "cartographer/sensor/imu_data.h"
|
#include "cartographer/sensor/imu_data.h"
|
||||||
#include "cartographer/sensor/odometry_data.h"
|
#include "cartographer/sensor/odometry_data.h"
|
||||||
|
@ -33,16 +33,16 @@
|
||||||
#include "cartographer/transform/rigid_transform.h"
|
#include "cartographer/transform/rigid_transform.h"
|
||||||
|
|
||||||
namespace cartographer {
|
namespace cartographer {
|
||||||
namespace mapping_2d {
|
namespace mapping {
|
||||||
|
|
||||||
// Wires up the local SLAM stack (i.e. pose extrapolator, scan matching, etc.)
|
// Wires up the local SLAM stack (i.e. pose extrapolator, scan matching, etc.)
|
||||||
// without loop closure.
|
// without loop closure.
|
||||||
// TODO(gaschler): Add test for this class similar to the 3D test.
|
// TODO(gaschler): Add test for this class similar to the 3D test.
|
||||||
class LocalTrajectoryBuilder {
|
class LocalTrajectoryBuilder2D {
|
||||||
public:
|
public:
|
||||||
struct InsertionResult {
|
struct InsertionResult {
|
||||||
std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data;
|
std::shared_ptr<const TrajectoryNode::Data> constant_data;
|
||||||
std::vector<std::shared_ptr<const mapping::Submap2D>> insertion_submaps;
|
std::vector<std::shared_ptr<const Submap2D>> insertion_submaps;
|
||||||
};
|
};
|
||||||
struct MatchingResult {
|
struct MatchingResult {
|
||||||
common::Time time;
|
common::Time time;
|
||||||
|
@ -52,12 +52,12 @@ class LocalTrajectoryBuilder {
|
||||||
std::unique_ptr<const InsertionResult> insertion_result;
|
std::unique_ptr<const InsertionResult> insertion_result;
|
||||||
};
|
};
|
||||||
|
|
||||||
explicit LocalTrajectoryBuilder(
|
explicit LocalTrajectoryBuilder2D(
|
||||||
const mapping::proto::LocalTrajectoryBuilderOptions2D& options);
|
const proto::LocalTrajectoryBuilderOptions2D& options);
|
||||||
~LocalTrajectoryBuilder();
|
~LocalTrajectoryBuilder2D();
|
||||||
|
|
||||||
LocalTrajectoryBuilder(const LocalTrajectoryBuilder&) = delete;
|
LocalTrajectoryBuilder2D(const LocalTrajectoryBuilder2D&) = delete;
|
||||||
LocalTrajectoryBuilder& operator=(const LocalTrajectoryBuilder&) = delete;
|
LocalTrajectoryBuilder2D& operator=(const LocalTrajectoryBuilder2D&) = delete;
|
||||||
|
|
||||||
// Returns 'MatchingResult' when range data accumulation completed,
|
// Returns 'MatchingResult' when range data accumulation completed,
|
||||||
// otherwise 'nullptr'. Range data must be approximately horizontal
|
// otherwise 'nullptr'. Range data must be approximately horizontal
|
||||||
|
@ -92,21 +92,21 @@ class LocalTrajectoryBuilder {
|
||||||
// Lazily constructs a PoseExtrapolator.
|
// Lazily constructs a PoseExtrapolator.
|
||||||
void InitializeExtrapolator(common::Time time);
|
void InitializeExtrapolator(common::Time time);
|
||||||
|
|
||||||
const mapping::proto::LocalTrajectoryBuilderOptions2D options_;
|
const proto::LocalTrajectoryBuilderOptions2D options_;
|
||||||
mapping::ActiveSubmaps2D active_submaps_;
|
ActiveSubmaps2D active_submaps_;
|
||||||
|
|
||||||
mapping::MotionFilter motion_filter_;
|
MotionFilter motion_filter_;
|
||||||
scan_matching::RealTimeCorrelativeScanMatcher
|
scan_matching::RealTimeCorrelativeScanMatcher2D
|
||||||
real_time_correlative_scan_matcher_;
|
real_time_correlative_scan_matcher_;
|
||||||
scan_matching::CeresScanMatcher ceres_scan_matcher_;
|
scan_matching::CeresScanMatcher2D ceres_scan_matcher_;
|
||||||
|
|
||||||
std::unique_ptr<mapping::PoseExtrapolator> extrapolator_;
|
std::unique_ptr<PoseExtrapolator> extrapolator_;
|
||||||
|
|
||||||
int num_accumulated_ = 0;
|
int num_accumulated_ = 0;
|
||||||
sensor::RangeData accumulated_range_data_;
|
sensor::RangeData accumulated_range_data_;
|
||||||
};
|
};
|
||||||
|
|
||||||
} // namespace mapping_2d
|
} // namespace mapping
|
||||||
} // namespace cartographer
|
} // namespace cartographer
|
||||||
|
|
||||||
#endif // CARTOGRAPHER_INTERNAL_MAPPING_2D_LOCAL_TRAJECTORY_BUILDER_H_
|
#endif // CARTOGRAPHER_INTERNAL_MAPPING_2D_LOCAL_TRAJECTORY_BUILDER_H_
|
|
@ -26,21 +26,21 @@
|
||||||
#include "ceres/cubic_interpolation.h"
|
#include "ceres/cubic_interpolation.h"
|
||||||
|
|
||||||
namespace cartographer {
|
namespace cartographer {
|
||||||
namespace mapping_2d {
|
namespace mapping {
|
||||||
namespace scan_matching {
|
namespace scan_matching {
|
||||||
|
|
||||||
// Computes a cost for matching the 'point_cloud' to the 'probability_grid' with
|
// Computes a cost for matching the 'point_cloud' to the 'probability_grid' with
|
||||||
// a 'pose'. The cost increases when points fall into less occupied space, i.e.
|
// a 'pose'. The cost increases when points fall into less occupied space, i.e.
|
||||||
// at pixels with lower values.
|
// at pixels with lower values.
|
||||||
class OccupiedSpaceCostFunction {
|
class OccupiedSpaceCostFunction2D {
|
||||||
public:
|
public:
|
||||||
static ceres::CostFunction* CreateAutoDiffCostFunction(
|
static ceres::CostFunction* CreateAutoDiffCostFunction(
|
||||||
const double scaling_factor, const sensor::PointCloud& point_cloud,
|
const double scaling_factor, const sensor::PointCloud& point_cloud,
|
||||||
const mapping::ProbabilityGrid& probability_grid) {
|
const ProbabilityGrid& probability_grid) {
|
||||||
return new ceres::AutoDiffCostFunction<OccupiedSpaceCostFunction,
|
return new ceres::AutoDiffCostFunction<OccupiedSpaceCostFunction2D,
|
||||||
ceres::DYNAMIC /* residuals */,
|
ceres::DYNAMIC /* residuals */,
|
||||||
3 /* pose variables */>(
|
3 /* pose variables */>(
|
||||||
new OccupiedSpaceCostFunction(scaling_factor, point_cloud,
|
new OccupiedSpaceCostFunction2D(scaling_factor, point_cloud,
|
||||||
probability_grid),
|
probability_grid),
|
||||||
point_cloud.size());
|
point_cloud.size());
|
||||||
}
|
}
|
||||||
|
@ -55,7 +55,7 @@ class OccupiedSpaceCostFunction {
|
||||||
|
|
||||||
const GridArrayAdapter adapter(probability_grid_);
|
const GridArrayAdapter adapter(probability_grid_);
|
||||||
ceres::BiCubicInterpolator<GridArrayAdapter> interpolator(adapter);
|
ceres::BiCubicInterpolator<GridArrayAdapter> interpolator(adapter);
|
||||||
const mapping::MapLimits& limits = probability_grid_.limits();
|
const MapLimits& limits = probability_grid_.limits();
|
||||||
|
|
||||||
for (size_t i = 0; i < point_cloud_.size(); ++i) {
|
for (size_t i = 0; i < point_cloud_.size(); ++i) {
|
||||||
// Note that this is a 2D point. The third component is a scaling factor.
|
// Note that this is a 2D point. The third component is a scaling factor.
|
||||||
|
@ -79,13 +79,13 @@ class OccupiedSpaceCostFunction {
|
||||||
public:
|
public:
|
||||||
enum { DATA_DIMENSION = 1 };
|
enum { DATA_DIMENSION = 1 };
|
||||||
|
|
||||||
explicit GridArrayAdapter(const mapping::ProbabilityGrid& probability_grid)
|
explicit GridArrayAdapter(const ProbabilityGrid& probability_grid)
|
||||||
: probability_grid_(probability_grid) {}
|
: probability_grid_(probability_grid) {}
|
||||||
|
|
||||||
void GetValue(const int row, const int column, double* const value) const {
|
void GetValue(const int row, const int column, double* const value) const {
|
||||||
if (row < kPadding || column < kPadding || row >= NumRows() - kPadding ||
|
if (row < kPadding || column < kPadding || row >= NumRows() - kPadding ||
|
||||||
column >= NumCols() - kPadding) {
|
column >= NumCols() - kPadding) {
|
||||||
*value = mapping::kMinProbability;
|
*value = kMinProbability;
|
||||||
} else {
|
} else {
|
||||||
*value = static_cast<double>(probability_grid_.GetProbability(
|
*value = static_cast<double>(probability_grid_.GetProbability(
|
||||||
Eigen::Array2i(column - kPadding, row - kPadding)));
|
Eigen::Array2i(column - kPadding, row - kPadding)));
|
||||||
|
@ -103,27 +103,27 @@ class OccupiedSpaceCostFunction {
|
||||||
}
|
}
|
||||||
|
|
||||||
private:
|
private:
|
||||||
const mapping::ProbabilityGrid& probability_grid_;
|
const ProbabilityGrid& probability_grid_;
|
||||||
};
|
};
|
||||||
|
|
||||||
OccupiedSpaceCostFunction(const double scaling_factor,
|
OccupiedSpaceCostFunction2D(const double scaling_factor,
|
||||||
const sensor::PointCloud& point_cloud,
|
const sensor::PointCloud& point_cloud,
|
||||||
const mapping::ProbabilityGrid& probability_grid)
|
const ProbabilityGrid& probability_grid)
|
||||||
: scaling_factor_(scaling_factor),
|
: scaling_factor_(scaling_factor),
|
||||||
point_cloud_(point_cloud),
|
point_cloud_(point_cloud),
|
||||||
probability_grid_(probability_grid) {}
|
probability_grid_(probability_grid) {}
|
||||||
|
|
||||||
OccupiedSpaceCostFunction(const OccupiedSpaceCostFunction&) = delete;
|
OccupiedSpaceCostFunction2D(const OccupiedSpaceCostFunction2D&) = delete;
|
||||||
OccupiedSpaceCostFunction& operator=(const OccupiedSpaceCostFunction&) =
|
OccupiedSpaceCostFunction2D& operator=(const OccupiedSpaceCostFunction2D&) =
|
||||||
delete;
|
delete;
|
||||||
|
|
||||||
const double scaling_factor_;
|
const double scaling_factor_;
|
||||||
const sensor::PointCloud& point_cloud_;
|
const sensor::PointCloud& point_cloud_;
|
||||||
const mapping::ProbabilityGrid& probability_grid_;
|
const ProbabilityGrid& probability_grid_;
|
||||||
};
|
};
|
||||||
|
|
||||||
} // namespace scan_matching
|
} // namespace scan_matching
|
||||||
} // namespace mapping_2d
|
} // namespace mapping
|
||||||
} // namespace cartographer
|
} // namespace cartographer
|
||||||
|
|
||||||
#endif // CARTOGRAPHER_INTERNAL_MAPPING_2D_SCAN_MATCHING_OCCUPIED_SPACE_COST_FUNCTION_H_
|
#endif // CARTOGRAPHER_INTERNAL_MAPPING_2D_SCAN_MATCHING_OCCUPIED_SPACE_COST_FUNCTION_H_
|
|
@ -20,7 +20,7 @@
|
||||||
|
|
||||||
#include "cartographer/common/make_unique.h"
|
#include "cartographer/common/make_unique.h"
|
||||||
#include "cartographer/common/time.h"
|
#include "cartographer/common/time.h"
|
||||||
#include "cartographer/mapping_2d/scan_matching/proto/real_time_correlative_scan_matcher_options.pb.h"
|
#include "cartographer/mapping/scan_matching/proto/real_time_correlative_scan_matcher_options.pb.h"
|
||||||
#include "cartographer/mapping_3d/proto/local_trajectory_builder_options_3d.pb.h"
|
#include "cartographer/mapping_3d/proto/local_trajectory_builder_options_3d.pb.h"
|
||||||
#include "cartographer/mapping_3d/proto/submaps_options_3d.pb.h"
|
#include "cartographer/mapping_3d/proto/submaps_options_3d.pb.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"
|
||||||
|
|
|
@ -25,7 +25,7 @@
|
||||||
#include "cartographer/common/make_unique.h"
|
#include "cartographer/common/make_unique.h"
|
||||||
#include "cartographer/common/time.h"
|
#include "cartographer/common/time.h"
|
||||||
#include "cartographer/internal/mapping/global_trajectory_builder.h"
|
#include "cartographer/internal/mapping/global_trajectory_builder.h"
|
||||||
#include "cartographer/internal/mapping_2d/local_trajectory_builder.h"
|
#include "cartographer/internal/mapping_2d/local_trajectory_builder_2d.h"
|
||||||
#include "cartographer/internal/mapping_3d/local_trajectory_builder.h"
|
#include "cartographer/internal/mapping_3d/local_trajectory_builder.h"
|
||||||
#include "cartographer/mapping/collated_trajectory_builder.h"
|
#include "cartographer/mapping/collated_trajectory_builder.h"
|
||||||
#include "cartographer/sensor/collator.h"
|
#include "cartographer/sensor/collator.h"
|
||||||
|
@ -94,11 +94,9 @@ int MapBuilder::AddTrajectoryBuilder(
|
||||||
trajectory_id, pose_graph_3d_.get(),
|
trajectory_id, pose_graph_3d_.get(),
|
||||||
local_slam_result_callback)));
|
local_slam_result_callback)));
|
||||||
} else {
|
} else {
|
||||||
std::unique_ptr<mapping_2d::LocalTrajectoryBuilder>
|
std::unique_ptr<LocalTrajectoryBuilder2D> local_trajectory_builder;
|
||||||
local_trajectory_builder;
|
|
||||||
if (trajectory_options.has_trajectory_builder_2d_options()) {
|
if (trajectory_options.has_trajectory_builder_2d_options()) {
|
||||||
local_trajectory_builder =
|
local_trajectory_builder = common::make_unique<LocalTrajectoryBuilder2D>(
|
||||||
common::make_unique<mapping_2d::LocalTrajectoryBuilder>(
|
|
||||||
trajectory_options.trajectory_builder_2d_options());
|
trajectory_options.trajectory_builder_2d_options());
|
||||||
}
|
}
|
||||||
trajectory_builders_.push_back(
|
trajectory_builders_.push_back(
|
||||||
|
|
|
@ -16,8 +16,8 @@
|
||||||
|
|
||||||
#include "cartographer/mapping/pose_graph/constraint_builder.h"
|
#include "cartographer/mapping/pose_graph/constraint_builder.h"
|
||||||
|
|
||||||
#include "cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h"
|
#include "cartographer/mapping_2d/scan_matching/ceres_scan_matcher_2d.h"
|
||||||
#include "cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h"
|
#include "cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_2d.h"
|
||||||
#include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h"
|
#include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h"
|
||||||
#include "cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h"
|
#include "cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h"
|
||||||
#include "cartographer/sensor/voxel_filter.h"
|
#include "cartographer/sensor/voxel_filter.h"
|
||||||
|
@ -41,11 +41,11 @@ proto::ConstraintBuilderOptions CreateConstraintBuilderOptions(
|
||||||
parameter_dictionary->GetDouble("loop_closure_rotation_weight"));
|
parameter_dictionary->GetDouble("loop_closure_rotation_weight"));
|
||||||
options.set_log_matches(parameter_dictionary->GetBool("log_matches"));
|
options.set_log_matches(parameter_dictionary->GetBool("log_matches"));
|
||||||
*options.mutable_fast_correlative_scan_matcher_options() =
|
*options.mutable_fast_correlative_scan_matcher_options() =
|
||||||
mapping_2d::scan_matching::CreateFastCorrelativeScanMatcherOptions(
|
scan_matching::CreateFastCorrelativeScanMatcherOptions2D(
|
||||||
parameter_dictionary->GetDictionary("fast_correlative_scan_matcher")
|
parameter_dictionary->GetDictionary("fast_correlative_scan_matcher")
|
||||||
.get());
|
.get());
|
||||||
*options.mutable_ceres_scan_matcher_options() =
|
*options.mutable_ceres_scan_matcher_options() =
|
||||||
mapping_2d::scan_matching::CreateCeresScanMatcherOptions(
|
scan_matching::CreateCeresScanMatcherOptions2D(
|
||||||
parameter_dictionary->GetDictionary("ceres_scan_matcher").get());
|
parameter_dictionary->GetDictionary("ceres_scan_matcher").get());
|
||||||
*options.mutable_fast_correlative_scan_matcher_options_3d() =
|
*options.mutable_fast_correlative_scan_matcher_options_3d() =
|
||||||
mapping_3d::scan_matching::CreateFastCorrelativeScanMatcherOptions(
|
mapping_3d::scan_matching::CreateFastCorrelativeScanMatcherOptions(
|
||||||
|
|
|
@ -16,8 +16,8 @@ syntax = "proto3";
|
||||||
|
|
||||||
package cartographer.mapping.pose_graph.proto;
|
package cartographer.mapping.pose_graph.proto;
|
||||||
|
|
||||||
import "cartographer/mapping_2d/scan_matching/proto/ceres_scan_matcher_options.proto";
|
import "cartographer/mapping_2d/scan_matching/proto/ceres_scan_matcher_options_2d.proto";
|
||||||
import "cartographer/mapping_2d/scan_matching/proto/fast_correlative_scan_matcher_options.proto";
|
import "cartographer/mapping_2d/scan_matching/proto/fast_correlative_scan_matcher_options_2d.proto";
|
||||||
import "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.proto";
|
import "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.proto";
|
||||||
import "cartographer/mapping_3d/scan_matching/proto/fast_correlative_scan_matcher_options.proto";
|
import "cartographer/mapping_3d/scan_matching/proto/fast_correlative_scan_matcher_options.proto";
|
||||||
|
|
||||||
|
@ -48,13 +48,12 @@ message ConstraintBuilderOptions {
|
||||||
bool log_matches = 8;
|
bool log_matches = 8;
|
||||||
|
|
||||||
// Options for the internally used scan matchers.
|
// Options for the internally used scan matchers.
|
||||||
mapping_2d.scan_matching.proto.FastCorrelativeScanMatcherOptions
|
mapping.scan_matching.proto.FastCorrelativeScanMatcherOptions2D
|
||||||
fast_correlative_scan_matcher_options = 9;
|
fast_correlative_scan_matcher_options = 9;
|
||||||
mapping_2d.scan_matching.proto.CeresScanMatcherOptions
|
mapping.scan_matching.proto.CeresScanMatcherOptions2D
|
||||||
ceres_scan_matcher_options = 11;
|
ceres_scan_matcher_options = 11;
|
||||||
mapping_3d.scan_matching.proto.FastCorrelativeScanMatcherOptions
|
mapping_3d.scan_matching.proto.FastCorrelativeScanMatcherOptions
|
||||||
fast_correlative_scan_matcher_options_3d = 10;
|
fast_correlative_scan_matcher_options_3d = 10;
|
||||||
|
|
||||||
mapping_3d.scan_matching.proto.CeresScanMatcherOptions
|
mapping_3d.scan_matching.proto.CeresScanMatcherOptions
|
||||||
ceres_scan_matcher_options_3d = 12;
|
ceres_scan_matcher_options_3d = 12;
|
||||||
}
|
}
|
||||||
|
|
|
@ -14,7 +14,7 @@
|
||||||
|
|
||||||
syntax = "proto3";
|
syntax = "proto3";
|
||||||
|
|
||||||
package cartographer.mapping_2d.scan_matching.proto;
|
package cartographer.mapping.scan_matching.proto;
|
||||||
|
|
||||||
message RealTimeCorrelativeScanMatcherOptions {
|
message RealTimeCorrelativeScanMatcherOptions {
|
||||||
// Minimum linear search window in which the best possible scan alignment
|
// Minimum linear search window in which the best possible scan alignment
|
|
@ -0,0 +1,26 @@
|
||||||
|
#include "cartographer/mapping/scan_matching/real_time_correlative_scan_matcher.h"
|
||||||
|
|
||||||
|
namespace cartographer {
|
||||||
|
namespace mapping {
|
||||||
|
namespace scan_matching {
|
||||||
|
|
||||||
|
proto::RealTimeCorrelativeScanMatcherOptions
|
||||||
|
CreateRealTimeCorrelativeScanMatcherOptions(
|
||||||
|
common::LuaParameterDictionary* const parameter_dictionary) {
|
||||||
|
proto::RealTimeCorrelativeScanMatcherOptions options;
|
||||||
|
options.set_linear_search_window(
|
||||||
|
parameter_dictionary->GetDouble("linear_search_window"));
|
||||||
|
options.set_angular_search_window(
|
||||||
|
parameter_dictionary->GetDouble("angular_search_window"));
|
||||||
|
options.set_translation_delta_cost_weight(
|
||||||
|
parameter_dictionary->GetDouble("translation_delta_cost_weight"));
|
||||||
|
options.set_rotation_delta_cost_weight(
|
||||||
|
parameter_dictionary->GetDouble("rotation_delta_cost_weight"));
|
||||||
|
CHECK_GE(options.translation_delta_cost_weight(), 0.);
|
||||||
|
CHECK_GE(options.rotation_delta_cost_weight(), 0.);
|
||||||
|
return options;
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace scan_matching
|
||||||
|
} // namespace mapping
|
||||||
|
} // namespace cartographer
|
|
@ -0,0 +1,36 @@
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Copyright 2018 The Cartographer Authors
|
||||||
|
*
|
||||||
|
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||||
|
* you may not use this file except in compliance with the License.
|
||||||
|
* You may obtain a copy of the License at
|
||||||
|
*
|
||||||
|
* http://www.apache.org/licenses/LICENSE-2.0
|
||||||
|
*
|
||||||
|
* Unless required by applicable law or agreed to in writing, software
|
||||||
|
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||||
|
* 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.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef CARTOGRAPHER_MAPPING_SCAN_MATCHING_REAL_TIME_CORRELATIVE_SCAN_MATCHER_H_
|
||||||
|
#define CARTOGRAPHER_MAPPING_SCAN_MATCHING_REAL_TIME_CORRELATIVE_SCAN_MATCHER_H_
|
||||||
|
|
||||||
|
#include "cartographer/common/lua_parameter_dictionary.h"
|
||||||
|
#include "cartographer/mapping/scan_matching/proto/real_time_correlative_scan_matcher_options.pb.h"
|
||||||
|
|
||||||
|
namespace cartographer {
|
||||||
|
namespace mapping {
|
||||||
|
namespace scan_matching {
|
||||||
|
|
||||||
|
proto::RealTimeCorrelativeScanMatcherOptions
|
||||||
|
CreateRealTimeCorrelativeScanMatcherOptions(
|
||||||
|
common::LuaParameterDictionary* const parameter_dictionary);
|
||||||
|
|
||||||
|
} // namespace scan_matching
|
||||||
|
} // namespace mapping
|
||||||
|
} // namespace cartographer
|
||||||
|
|
||||||
|
#endif // CARTOGRAPHER_MAPPING_SCAN_MATCHING_REAL_TIME_CORRELATIVE_SCAN_MATCHER_H_
|
|
@ -107,7 +107,7 @@ class TrajectoryBuilderInterface {
|
||||||
const sensor::LandmarkData& landmark_data) = 0;
|
const sensor::LandmarkData& landmark_data) = 0;
|
||||||
// Allows to directly add local SLAM results to the 'PoseGraph'. Note that it
|
// Allows to directly add local SLAM results to the 'PoseGraph'. Note that it
|
||||||
// is invalid to add local SLAM results for a trajectory that has a
|
// is invalid to add local SLAM results for a trajectory that has a
|
||||||
// 'LocalTrajectoryBuilder'.
|
// 'LocalTrajectoryBuilder2D/3D'.
|
||||||
virtual void AddLocalSlamResultData(
|
virtual void AddLocalSlamResultData(
|
||||||
std::unique_ptr<mapping::LocalSlamResultData> local_slam_result_data) = 0;
|
std::unique_ptr<mapping::LocalSlamResultData> local_slam_result_data) = 0;
|
||||||
};
|
};
|
||||||
|
|
|
@ -17,8 +17,8 @@
|
||||||
#include "cartographer/mapping_2d/local_trajectory_builder_options_2d.h"
|
#include "cartographer/mapping_2d/local_trajectory_builder_options_2d.h"
|
||||||
|
|
||||||
#include "cartographer/internal/mapping/motion_filter.h"
|
#include "cartographer/internal/mapping/motion_filter.h"
|
||||||
#include "cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h"
|
#include "cartographer/mapping_2d/scan_matching/ceres_scan_matcher_2d.h"
|
||||||
#include "cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.h"
|
#include "cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_2d.h"
|
||||||
#include "cartographer/mapping_2d/submap_2d.h"
|
#include "cartographer/mapping_2d/submap_2d.h"
|
||||||
#include "cartographer/sensor/voxel_filter.h"
|
#include "cartographer/sensor/voxel_filter.h"
|
||||||
|
|
||||||
|
@ -49,12 +49,12 @@ proto::LocalTrajectoryBuilderOptions2D CreateLocalTrajectoryBuilderOptions2D(
|
||||||
->GetDictionary("loop_closure_adaptive_voxel_filter")
|
->GetDictionary("loop_closure_adaptive_voxel_filter")
|
||||||
.get());
|
.get());
|
||||||
*options.mutable_real_time_correlative_scan_matcher_options() =
|
*options.mutable_real_time_correlative_scan_matcher_options() =
|
||||||
mapping_2d::scan_matching::CreateRealTimeCorrelativeScanMatcherOptions(
|
mapping::scan_matching::CreateRealTimeCorrelativeScanMatcherOptions(
|
||||||
parameter_dictionary
|
parameter_dictionary
|
||||||
->GetDictionary("real_time_correlative_scan_matcher")
|
->GetDictionary("real_time_correlative_scan_matcher")
|
||||||
.get());
|
.get());
|
||||||
*options.mutable_ceres_scan_matcher_options() =
|
*options.mutable_ceres_scan_matcher_options() =
|
||||||
mapping_2d::scan_matching::CreateCeresScanMatcherOptions(
|
mapping::scan_matching::CreateCeresScanMatcherOptions2D(
|
||||||
parameter_dictionary->GetDictionary("ceres_scan_matcher").get());
|
parameter_dictionary->GetDictionary("ceres_scan_matcher").get());
|
||||||
*options.mutable_motion_filter_options() = mapping::CreateMotionFilterOptions(
|
*options.mutable_motion_filter_options() = mapping::CreateMotionFilterOptions(
|
||||||
parameter_dictionary->GetDictionary("motion_filter").get());
|
parameter_dictionary->GetDictionary("motion_filter").get());
|
||||||
|
|
|
@ -14,7 +14,7 @@
|
||||||
* limitations under the License.
|
* limitations under the License.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#include "cartographer/mapping_2d/pose_graph/constraint_builder.h"
|
#include "cartographer/mapping_2d/pose_graph/constraint_builder_2d.h"
|
||||||
|
|
||||||
#include <cmath>
|
#include <cmath>
|
||||||
#include <functional>
|
#include <functional>
|
||||||
|
@ -29,8 +29,8 @@
|
||||||
#include "cartographer/common/make_unique.h"
|
#include "cartographer/common/make_unique.h"
|
||||||
#include "cartographer/common/math.h"
|
#include "cartographer/common/math.h"
|
||||||
#include "cartographer/common/thread_pool.h"
|
#include "cartographer/common/thread_pool.h"
|
||||||
#include "cartographer/mapping_2d/scan_matching/proto/ceres_scan_matcher_options.pb.h"
|
#include "cartographer/mapping_2d/scan_matching/proto/ceres_scan_matcher_options_2d.pb.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_2d.pb.h"
|
||||||
#include "cartographer/metrics/counter.h"
|
#include "cartographer/metrics/counter.h"
|
||||||
#include "cartographer/metrics/gauge.h"
|
#include "cartographer/metrics/gauge.h"
|
||||||
#include "cartographer/metrics/histogram.h"
|
#include "cartographer/metrics/histogram.h"
|
||||||
|
@ -38,7 +38,7 @@
|
||||||
#include "glog/logging.h"
|
#include "glog/logging.h"
|
||||||
|
|
||||||
namespace cartographer {
|
namespace cartographer {
|
||||||
namespace mapping_2d {
|
namespace mapping {
|
||||||
namespace pose_graph {
|
namespace pose_graph {
|
||||||
|
|
||||||
static auto* kConstraintsSearchedMetric = metrics::Counter::Null();
|
static auto* kConstraintsSearchedMetric = metrics::Counter::Null();
|
||||||
|
@ -49,19 +49,19 @@ static auto* kQueueLengthMetric = metrics::Gauge::Null();
|
||||||
static auto* kConstraintScoresMetric = metrics::Histogram::Null();
|
static auto* kConstraintScoresMetric = metrics::Histogram::Null();
|
||||||
static auto* kGlobalConstraintScoresMetric = metrics::Histogram::Null();
|
static auto* kGlobalConstraintScoresMetric = metrics::Histogram::Null();
|
||||||
|
|
||||||
transform::Rigid2d ComputeSubmapPose(const mapping::Submap2D& submap) {
|
transform::Rigid2d ComputeSubmapPose(const Submap2D& submap) {
|
||||||
return transform::Project2D(submap.local_pose());
|
return transform::Project2D(submap.local_pose());
|
||||||
}
|
}
|
||||||
|
|
||||||
ConstraintBuilder::ConstraintBuilder(
|
ConstraintBuilder2D::ConstraintBuilder2D(
|
||||||
const mapping::pose_graph::proto::ConstraintBuilderOptions& options,
|
const pose_graph::proto::ConstraintBuilderOptions& options,
|
||||||
common::ThreadPool* const thread_pool)
|
common::ThreadPool* const thread_pool)
|
||||||
: options_(options),
|
: options_(options),
|
||||||
thread_pool_(thread_pool),
|
thread_pool_(thread_pool),
|
||||||
sampler_(options.sampling_ratio()),
|
sampler_(options.sampling_ratio()),
|
||||||
ceres_scan_matcher_(options.ceres_scan_matcher_options()) {}
|
ceres_scan_matcher_(options.ceres_scan_matcher_options()) {}
|
||||||
|
|
||||||
ConstraintBuilder::~ConstraintBuilder() {
|
ConstraintBuilder2D::~ConstraintBuilder2D() {
|
||||||
common::MutexLocker locker(&mutex_);
|
common::MutexLocker locker(&mutex_);
|
||||||
CHECK_EQ(constraints_.size(), 0) << "WhenDone() was not called";
|
CHECK_EQ(constraints_.size(), 0) << "WhenDone() was not called";
|
||||||
CHECK_EQ(pending_computations_.size(), 0);
|
CHECK_EQ(pending_computations_.size(), 0);
|
||||||
|
@ -69,10 +69,9 @@ ConstraintBuilder::~ConstraintBuilder() {
|
||||||
CHECK(when_done_ == nullptr);
|
CHECK(when_done_ == nullptr);
|
||||||
}
|
}
|
||||||
|
|
||||||
void ConstraintBuilder::MaybeAddConstraint(
|
void ConstraintBuilder2D::MaybeAddConstraint(
|
||||||
const mapping::SubmapId& submap_id, const mapping::Submap2D* const submap,
|
const SubmapId& submap_id, const Submap2D* const submap,
|
||||||
const mapping::NodeId& node_id,
|
const NodeId& node_id, const TrajectoryNode::Data* const constant_data,
|
||||||
const mapping::TrajectoryNode::Data* const constant_data,
|
|
||||||
const transform::Rigid2d& initial_relative_pose) {
|
const transform::Rigid2d& initial_relative_pose) {
|
||||||
if (initial_relative_pose.translation().norm() >
|
if (initial_relative_pose.translation().norm() >
|
||||||
options_.max_constraint_distance()) {
|
options_.max_constraint_distance()) {
|
||||||
|
@ -95,10 +94,9 @@ void ConstraintBuilder::MaybeAddConstraint(
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void ConstraintBuilder::MaybeAddGlobalConstraint(
|
void ConstraintBuilder2D::MaybeAddGlobalConstraint(
|
||||||
const mapping::SubmapId& submap_id, const mapping::Submap2D* const submap,
|
const SubmapId& submap_id, const Submap2D* const submap,
|
||||||
const mapping::NodeId& node_id,
|
const NodeId& node_id, const TrajectoryNode::Data* const constant_data) {
|
||||||
const mapping::TrajectoryNode::Data* const constant_data) {
|
|
||||||
common::MutexLocker locker(&mutex_);
|
common::MutexLocker locker(&mutex_);
|
||||||
constraints_.emplace_back();
|
constraints_.emplace_back();
|
||||||
kQueueLengthMetric->Set(constraints_.size());
|
kQueueLengthMetric->Set(constraints_.size());
|
||||||
|
@ -114,13 +112,13 @@ void ConstraintBuilder::MaybeAddGlobalConstraint(
|
||||||
});
|
});
|
||||||
}
|
}
|
||||||
|
|
||||||
void ConstraintBuilder::NotifyEndOfNode() {
|
void ConstraintBuilder2D::NotifyEndOfNode() {
|
||||||
common::MutexLocker locker(&mutex_);
|
common::MutexLocker locker(&mutex_);
|
||||||
++current_computation_;
|
++current_computation_;
|
||||||
}
|
}
|
||||||
|
|
||||||
void ConstraintBuilder::WhenDone(
|
void ConstraintBuilder2D::WhenDone(
|
||||||
const std::function<void(const ConstraintBuilder::Result&)>& callback) {
|
const std::function<void(const ConstraintBuilder2D::Result&)>& callback) {
|
||||||
common::MutexLocker locker(&mutex_);
|
common::MutexLocker locker(&mutex_);
|
||||||
CHECK(when_done_ == nullptr);
|
CHECK(when_done_ == nullptr);
|
||||||
when_done_ =
|
when_done_ =
|
||||||
|
@ -131,9 +129,8 @@ void ConstraintBuilder::WhenDone(
|
||||||
[this, current_computation] { FinishComputation(current_computation); });
|
[this, current_computation] { FinishComputation(current_computation); });
|
||||||
}
|
}
|
||||||
|
|
||||||
void ConstraintBuilder::ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
|
void ConstraintBuilder2D::ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
|
||||||
const mapping::SubmapId& submap_id,
|
const SubmapId& submap_id, const ProbabilityGrid* const submap,
|
||||||
const mapping::ProbabilityGrid* const submap,
|
|
||||||
const std::function<void()>& work_item) {
|
const std::function<void()>& work_item) {
|
||||||
if (submap_scan_matchers_[submap_id].fast_correlative_scan_matcher !=
|
if (submap_scan_matchers_[submap_id].fast_correlative_scan_matcher !=
|
||||||
nullptr) {
|
nullptr) {
|
||||||
|
@ -147,11 +144,10 @@ void ConstraintBuilder::ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void ConstraintBuilder::ConstructSubmapScanMatcher(
|
void ConstraintBuilder2D::ConstructSubmapScanMatcher(
|
||||||
const mapping::SubmapId& submap_id,
|
const SubmapId& submap_id, const ProbabilityGrid* const submap) {
|
||||||
const mapping::ProbabilityGrid* const submap) {
|
|
||||||
auto submap_scan_matcher =
|
auto submap_scan_matcher =
|
||||||
common::make_unique<scan_matching::FastCorrelativeScanMatcher>(
|
common::make_unique<scan_matching::FastCorrelativeScanMatcher2D>(
|
||||||
*submap, options_.fast_correlative_scan_matcher_options());
|
*submap, options_.fast_correlative_scan_matcher_options());
|
||||||
common::MutexLocker locker(&mutex_);
|
common::MutexLocker locker(&mutex_);
|
||||||
submap_scan_matchers_[submap_id] = {submap, std::move(submap_scan_matcher)};
|
submap_scan_matchers_[submap_id] = {submap, std::move(submap_scan_matcher)};
|
||||||
|
@ -162,8 +158,8 @@ void ConstraintBuilder::ConstructSubmapScanMatcher(
|
||||||
submap_queued_work_items_.erase(submap_id);
|
submap_queued_work_items_.erase(submap_id);
|
||||||
}
|
}
|
||||||
|
|
||||||
const ConstraintBuilder::SubmapScanMatcher*
|
const ConstraintBuilder2D::SubmapScanMatcher*
|
||||||
ConstraintBuilder::GetSubmapScanMatcher(const mapping::SubmapId& submap_id) {
|
ConstraintBuilder2D::GetSubmapScanMatcher(const SubmapId& submap_id) {
|
||||||
common::MutexLocker locker(&mutex_);
|
common::MutexLocker locker(&mutex_);
|
||||||
const SubmapScanMatcher* submap_scan_matcher =
|
const SubmapScanMatcher* submap_scan_matcher =
|
||||||
&submap_scan_matchers_[submap_id];
|
&submap_scan_matchers_[submap_id];
|
||||||
|
@ -171,12 +167,12 @@ ConstraintBuilder::GetSubmapScanMatcher(const mapping::SubmapId& submap_id) {
|
||||||
return submap_scan_matcher;
|
return submap_scan_matcher;
|
||||||
}
|
}
|
||||||
|
|
||||||
void ConstraintBuilder::ComputeConstraint(
|
void ConstraintBuilder2D::ComputeConstraint(
|
||||||
const mapping::SubmapId& submap_id, const mapping::Submap2D* const submap,
|
const SubmapId& submap_id, const Submap2D* const submap,
|
||||||
const mapping::NodeId& node_id, bool match_full_submap,
|
const NodeId& node_id, bool match_full_submap,
|
||||||
const mapping::TrajectoryNode::Data* const constant_data,
|
const TrajectoryNode::Data* const constant_data,
|
||||||
const transform::Rigid2d& initial_relative_pose,
|
const transform::Rigid2d& initial_relative_pose,
|
||||||
std::unique_ptr<ConstraintBuilder::Constraint>* constraint) {
|
std::unique_ptr<ConstraintBuilder2D::Constraint>* constraint) {
|
||||||
const transform::Rigid2d initial_pose =
|
const transform::Rigid2d initial_pose =
|
||||||
ComputeSubmapPose(*submap) * initial_relative_pose;
|
ComputeSubmapPose(*submap) * initial_relative_pose;
|
||||||
const SubmapScanMatcher* const submap_scan_matcher =
|
const SubmapScanMatcher* const submap_scan_matcher =
|
||||||
|
@ -262,7 +258,7 @@ void ConstraintBuilder::ComputeConstraint(
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void ConstraintBuilder::FinishComputation(const int computation_index) {
|
void ConstraintBuilder2D::FinishComputation(const int computation_index) {
|
||||||
Result result;
|
Result result;
|
||||||
std::unique_ptr<std::function<void(const Result&)>> callback;
|
std::unique_ptr<std::function<void(const Result&)>> callback;
|
||||||
{
|
{
|
||||||
|
@ -295,7 +291,7 @@ void ConstraintBuilder::FinishComputation(const int computation_index) {
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
int ConstraintBuilder::GetNumFinishedNodes() {
|
int ConstraintBuilder2D::GetNumFinishedNodes() {
|
||||||
common::MutexLocker locker(&mutex_);
|
common::MutexLocker locker(&mutex_);
|
||||||
if (pending_computations_.empty()) {
|
if (pending_computations_.empty()) {
|
||||||
return current_computation_;
|
return current_computation_;
|
||||||
|
@ -303,13 +299,13 @@ int ConstraintBuilder::GetNumFinishedNodes() {
|
||||||
return pending_computations_.begin()->first;
|
return pending_computations_.begin()->first;
|
||||||
}
|
}
|
||||||
|
|
||||||
void ConstraintBuilder::DeleteScanMatcher(const mapping::SubmapId& submap_id) {
|
void ConstraintBuilder2D::DeleteScanMatcher(const SubmapId& submap_id) {
|
||||||
common::MutexLocker locker(&mutex_);
|
common::MutexLocker locker(&mutex_);
|
||||||
CHECK(pending_computations_.empty());
|
CHECK(pending_computations_.empty());
|
||||||
submap_scan_matchers_.erase(submap_id);
|
submap_scan_matchers_.erase(submap_id);
|
||||||
}
|
}
|
||||||
|
|
||||||
void ConstraintBuilder::RegisterMetrics(metrics::FamilyFactory* factory) {
|
void ConstraintBuilder2D::RegisterMetrics(metrics::FamilyFactory* factory) {
|
||||||
auto* counts = factory->NewCounterFamily(
|
auto* counts = factory->NewCounterFamily(
|
||||||
"/mapping_2d/pose_graph/constraint_builder/constraints",
|
"/mapping_2d/pose_graph/constraint_builder/constraints",
|
||||||
"Constraints computed");
|
"Constraints computed");
|
||||||
|
@ -333,5 +329,5 @@ void ConstraintBuilder::RegisterMetrics(metrics::FamilyFactory* factory) {
|
||||||
}
|
}
|
||||||
|
|
||||||
} // namespace pose_graph
|
} // namespace pose_graph
|
||||||
} // namespace mapping_2d
|
} // namespace mapping
|
||||||
} // namespace cartographer
|
} // namespace cartographer
|
|
@ -14,8 +14,8 @@
|
||||||
* limitations under the License.
|
* limitations under the License.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#ifndef CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_CONSTRAINT_BUILDER_H_
|
#ifndef CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_CONSTRAINT_BUILDER_2D_H_
|
||||||
#define CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_CONSTRAINT_BUILDER_H_
|
#define CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_CONSTRAINT_BUILDER_2D_H_
|
||||||
|
|
||||||
#include <array>
|
#include <array>
|
||||||
#include <deque>
|
#include <deque>
|
||||||
|
@ -32,20 +32,20 @@
|
||||||
#include "cartographer/common/thread_pool.h"
|
#include "cartographer/common/thread_pool.h"
|
||||||
#include "cartographer/mapping/pose_graph/proto/constraint_builder_options.pb.h"
|
#include "cartographer/mapping/pose_graph/proto/constraint_builder_options.pb.h"
|
||||||
#include "cartographer/mapping/pose_graph_interface.h"
|
#include "cartographer/mapping/pose_graph_interface.h"
|
||||||
#include "cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h"
|
#include "cartographer/mapping_2d/scan_matching/ceres_scan_matcher_2d.h"
|
||||||
#include "cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h"
|
#include "cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_2d.h"
|
||||||
#include "cartographer/mapping_2d/submap_2d.h"
|
#include "cartographer/mapping_2d/submap_2d.h"
|
||||||
#include "cartographer/metrics/family_factory.h"
|
#include "cartographer/metrics/family_factory.h"
|
||||||
#include "cartographer/sensor/point_cloud.h"
|
#include "cartographer/sensor/point_cloud.h"
|
||||||
#include "cartographer/sensor/voxel_filter.h"
|
#include "cartographer/sensor/voxel_filter.h"
|
||||||
|
|
||||||
namespace cartographer {
|
namespace cartographer {
|
||||||
namespace mapping_2d {
|
namespace mapping {
|
||||||
namespace pose_graph {
|
namespace pose_graph {
|
||||||
|
|
||||||
// Returns (map <- submap) where 'submap' is a coordinate system at the origin
|
// Returns (map <- submap) where 'submap' is a coordinate system at the origin
|
||||||
// of the Submap.
|
// of the Submap.
|
||||||
transform::Rigid2d ComputeSubmapPose(const mapping::Submap2D& submap);
|
transform::Rigid2d ComputeSubmapPose(const Submap2D& submap);
|
||||||
|
|
||||||
// Asynchronously computes constraints.
|
// Asynchronously computes constraints.
|
||||||
//
|
//
|
||||||
|
@ -55,18 +55,18 @@ transform::Rigid2d ComputeSubmapPose(const mapping::Submap2D& submap);
|
||||||
// MaybeAdd(Global)Constraint()/WhenDone() cycle can follow.
|
// MaybeAdd(Global)Constraint()/WhenDone() cycle can follow.
|
||||||
//
|
//
|
||||||
// This class is thread-safe.
|
// This class is thread-safe.
|
||||||
class ConstraintBuilder {
|
class ConstraintBuilder2D {
|
||||||
public:
|
public:
|
||||||
using Constraint = mapping::PoseGraphInterface::Constraint;
|
using Constraint = PoseGraphInterface::Constraint;
|
||||||
using Result = std::vector<Constraint>;
|
using Result = std::vector<Constraint>;
|
||||||
|
|
||||||
ConstraintBuilder(
|
ConstraintBuilder2D(
|
||||||
const mapping::pose_graph::proto::ConstraintBuilderOptions& options,
|
const pose_graph::proto::ConstraintBuilderOptions& options,
|
||||||
common::ThreadPool* thread_pool);
|
common::ThreadPool* thread_pool);
|
||||||
~ConstraintBuilder();
|
~ConstraintBuilder2D();
|
||||||
|
|
||||||
ConstraintBuilder(const ConstraintBuilder&) = delete;
|
ConstraintBuilder2D(const ConstraintBuilder2D&) = delete;
|
||||||
ConstraintBuilder& operator=(const ConstraintBuilder&) = delete;
|
ConstraintBuilder2D& operator=(const ConstraintBuilder2D&) = delete;
|
||||||
|
|
||||||
// Schedules exploring a new constraint between 'submap' identified by
|
// Schedules exploring a new constraint between 'submap' identified by
|
||||||
// 'submap_id', and the 'compressed_point_cloud' for 'node_id'. The
|
// 'submap_id', and the 'compressed_point_cloud' for 'node_id'. The
|
||||||
|
@ -74,10 +74,9 @@ class ConstraintBuilder {
|
||||||
//
|
//
|
||||||
// The pointees of 'submap' and 'compressed_point_cloud' must stay valid until
|
// The pointees of 'submap' and 'compressed_point_cloud' must stay valid until
|
||||||
// all computations are finished.
|
// all computations are finished.
|
||||||
void MaybeAddConstraint(
|
void MaybeAddConstraint(const SubmapId& submap_id, const Submap2D* submap,
|
||||||
const mapping::SubmapId& submap_id, const mapping::Submap2D* submap,
|
const NodeId& node_id,
|
||||||
const mapping::NodeId& node_id,
|
const TrajectoryNode::Data* const constant_data,
|
||||||
const mapping::TrajectoryNode::Data* const constant_data,
|
|
||||||
const transform::Rigid2d& initial_relative_pose);
|
const transform::Rigid2d& initial_relative_pose);
|
||||||
|
|
||||||
// Schedules exploring a new constraint between 'submap' identified by
|
// Schedules exploring a new constraint between 'submap' identified by
|
||||||
|
@ -87,9 +86,8 @@ class ConstraintBuilder {
|
||||||
// The pointees of 'submap' and 'compressed_point_cloud' must stay valid until
|
// The pointees of 'submap' and 'compressed_point_cloud' must stay valid until
|
||||||
// all computations are finished.
|
// all computations are finished.
|
||||||
void MaybeAddGlobalConstraint(
|
void MaybeAddGlobalConstraint(
|
||||||
const mapping::SubmapId& submap_id, const mapping::Submap2D* submap,
|
const SubmapId& submap_id, const Submap2D* submap, const NodeId& node_id,
|
||||||
const mapping::NodeId& node_id,
|
const TrajectoryNode::Data* const constant_data);
|
||||||
const mapping::TrajectoryNode::Data* const constant_data);
|
|
||||||
|
|
||||||
// Must be called after all computations related to one node have been added.
|
// Must be called after all computations related to one node have been added.
|
||||||
void NotifyEndOfNode();
|
void NotifyEndOfNode();
|
||||||
|
@ -102,48 +100,47 @@ class ConstraintBuilder {
|
||||||
int GetNumFinishedNodes();
|
int GetNumFinishedNodes();
|
||||||
|
|
||||||
// Delete data related to 'submap_id'.
|
// Delete data related to 'submap_id'.
|
||||||
void DeleteScanMatcher(const mapping::SubmapId& submap_id);
|
void DeleteScanMatcher(const SubmapId& submap_id);
|
||||||
|
|
||||||
static void RegisterMetrics(metrics::FamilyFactory* family_factory);
|
static void RegisterMetrics(metrics::FamilyFactory* family_factory);
|
||||||
|
|
||||||
private:
|
private:
|
||||||
struct SubmapScanMatcher {
|
struct SubmapScanMatcher {
|
||||||
const mapping::ProbabilityGrid* probability_grid;
|
const ProbabilityGrid* probability_grid;
|
||||||
std::unique_ptr<scan_matching::FastCorrelativeScanMatcher>
|
std::unique_ptr<scan_matching::FastCorrelativeScanMatcher2D>
|
||||||
fast_correlative_scan_matcher;
|
fast_correlative_scan_matcher;
|
||||||
};
|
};
|
||||||
|
|
||||||
// Either schedules the 'work_item', or if needed, schedules the scan matcher
|
// Either schedules the 'work_item', or if needed, schedules the scan matcher
|
||||||
// construction and queues the 'work_item'.
|
// construction and queues the 'work_item'.
|
||||||
void ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
|
void ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
|
||||||
const mapping::SubmapId& submap_id,
|
const SubmapId& submap_id, const ProbabilityGrid* submap,
|
||||||
const mapping::ProbabilityGrid* submap,
|
|
||||||
const std::function<void()>& work_item) REQUIRES(mutex_);
|
const std::function<void()>& work_item) REQUIRES(mutex_);
|
||||||
|
|
||||||
// Constructs the scan matcher for a 'submap', then schedules its work items.
|
// Constructs the scan matcher for a 'submap', then schedules its work items.
|
||||||
void ConstructSubmapScanMatcher(const mapping::SubmapId& submap_id,
|
void ConstructSubmapScanMatcher(const SubmapId& submap_id,
|
||||||
const mapping::ProbabilityGrid* submap)
|
const ProbabilityGrid* submap)
|
||||||
EXCLUDES(mutex_);
|
EXCLUDES(mutex_);
|
||||||
|
|
||||||
// Returns the scan matcher for a submap, which has to exist.
|
// Returns the scan matcher for a submap, which has to exist.
|
||||||
const SubmapScanMatcher* GetSubmapScanMatcher(
|
const SubmapScanMatcher* GetSubmapScanMatcher(const SubmapId& submap_id)
|
||||||
const mapping::SubmapId& submap_id) EXCLUDES(mutex_);
|
EXCLUDES(mutex_);
|
||||||
|
|
||||||
// Runs in a background thread and does computations for an additional
|
// Runs in a background thread and does computations for an additional
|
||||||
// constraint, assuming 'submap' and 'compressed_point_cloud' do not change
|
// constraint, assuming 'submap' and 'compressed_point_cloud' do not change
|
||||||
// anymore. As output, it may create a new Constraint in 'constraint'.
|
// anymore. As output, it may create a new Constraint in 'constraint'.
|
||||||
void ComputeConstraint(
|
void ComputeConstraint(const SubmapId& submap_id, const Submap2D* submap,
|
||||||
const mapping::SubmapId& submap_id, const mapping::Submap2D* submap,
|
const NodeId& node_id, bool match_full_submap,
|
||||||
const mapping::NodeId& node_id, bool match_full_submap,
|
const TrajectoryNode::Data* const constant_data,
|
||||||
const mapping::TrajectoryNode::Data* const constant_data,
|
|
||||||
const transform::Rigid2d& initial_relative_pose,
|
const transform::Rigid2d& initial_relative_pose,
|
||||||
std::unique_ptr<Constraint>* constraint) EXCLUDES(mutex_);
|
std::unique_ptr<Constraint>* constraint)
|
||||||
|
EXCLUDES(mutex_);
|
||||||
|
|
||||||
// Decrements the 'pending_computations_' count. If all computations are done,
|
// Decrements the 'pending_computations_' count. If all computations are done,
|
||||||
// runs the 'when_done_' callback and resets the state.
|
// runs the 'when_done_' callback and resets the state.
|
||||||
void FinishComputation(int computation_index) EXCLUDES(mutex_);
|
void FinishComputation(int computation_index) EXCLUDES(mutex_);
|
||||||
|
|
||||||
const mapping::pose_graph::proto::ConstraintBuilderOptions options_;
|
const pose_graph::proto::ConstraintBuilderOptions options_;
|
||||||
common::ThreadPool* thread_pool_;
|
common::ThreadPool* thread_pool_;
|
||||||
common::Mutex mutex_;
|
common::Mutex mutex_;
|
||||||
|
|
||||||
|
@ -165,23 +162,23 @@ class ConstraintBuilder {
|
||||||
std::deque<std::unique_ptr<Constraint>> constraints_ GUARDED_BY(mutex_);
|
std::deque<std::unique_ptr<Constraint>> constraints_ GUARDED_BY(mutex_);
|
||||||
|
|
||||||
// Map of already constructed scan matchers by 'submap_id'.
|
// Map of already constructed scan matchers by 'submap_id'.
|
||||||
std::map<mapping::SubmapId, SubmapScanMatcher> submap_scan_matchers_
|
std::map<SubmapId, SubmapScanMatcher> submap_scan_matchers_
|
||||||
GUARDED_BY(mutex_);
|
GUARDED_BY(mutex_);
|
||||||
|
|
||||||
// Map by 'submap_id' of scan matchers under construction, and the work
|
// Map by 'submap_id' of scan matchers under construction, and the work
|
||||||
// to do once construction is done.
|
// to do once construction is done.
|
||||||
std::map<mapping::SubmapId, std::vector<std::function<void()>>>
|
std::map<SubmapId, std::vector<std::function<void()>>>
|
||||||
submap_queued_work_items_ GUARDED_BY(mutex_);
|
submap_queued_work_items_ GUARDED_BY(mutex_);
|
||||||
|
|
||||||
common::FixedRatioSampler sampler_;
|
common::FixedRatioSampler sampler_;
|
||||||
scan_matching::CeresScanMatcher ceres_scan_matcher_;
|
scan_matching::CeresScanMatcher2D ceres_scan_matcher_;
|
||||||
|
|
||||||
// Histogram of scan matcher scores.
|
// Histogram of scan matcher scores.
|
||||||
common::Histogram score_histogram_ GUARDED_BY(mutex_);
|
common::Histogram score_histogram_ GUARDED_BY(mutex_);
|
||||||
};
|
};
|
||||||
|
|
||||||
} // namespace pose_graph
|
} // namespace pose_graph
|
||||||
} // namespace mapping_2d
|
} // namespace mapping
|
||||||
} // namespace cartographer
|
} // namespace cartographer
|
||||||
|
|
||||||
#endif // CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_CONSTRAINT_BUILDER_H_
|
#endif // CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_CONSTRAINT_BUILDER_2D_H_
|
|
@ -14,49 +14,50 @@
|
||||||
* limitations under the License.
|
* limitations under the License.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#ifndef CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_LANDMARK_COST_FUNCTION_H_
|
#ifndef CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_LANDMARK_COST_FUNCTION_2D_H_
|
||||||
#define CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_LANDMARK_COST_FUNCTION_H_
|
#define CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_LANDMARK_COST_FUNCTION_2D_H_
|
||||||
|
|
||||||
#include "Eigen/Core"
|
#include "Eigen/Core"
|
||||||
#include "Eigen/Geometry"
|
#include "Eigen/Geometry"
|
||||||
#include "cartographer/mapping/pose_graph/cost_helpers.h"
|
#include "cartographer/mapping/pose_graph/cost_helpers.h"
|
||||||
#include "cartographer/mapping/pose_graph_interface.h"
|
#include "cartographer/mapping/pose_graph_interface.h"
|
||||||
#include "cartographer/mapping_2d/pose_graph/optimization_problem.h"
|
#include "cartographer/mapping_2d/pose_graph/optimization_problem_2d.h"
|
||||||
#include "cartographer/transform/rigid_transform.h"
|
#include "cartographer/transform/rigid_transform.h"
|
||||||
#include "cartographer/transform/transform.h"
|
#include "cartographer/transform/transform.h"
|
||||||
#include "ceres/ceres.h"
|
#include "ceres/ceres.h"
|
||||||
#include "ceres/jet.h"
|
#include "ceres/jet.h"
|
||||||
|
|
||||||
namespace cartographer {
|
namespace cartographer {
|
||||||
namespace mapping_2d {
|
namespace mapping {
|
||||||
namespace pose_graph {
|
namespace pose_graph {
|
||||||
|
|
||||||
// Cost function measuring the weighted error between the observed pose given by
|
// Cost function measuring the weighted error between the observed pose given by
|
||||||
// the landmark measurement and the linearly interpolated pose of embedded in 3D
|
// the landmark measurement and the linearly interpolated pose of embedded in 3D
|
||||||
// space node poses.
|
// space node poses.
|
||||||
class LandmarkCostFunction {
|
class LandmarkCostFunction2D {
|
||||||
public:
|
public:
|
||||||
using LandmarkObservation =
|
using LandmarkObservation =
|
||||||
mapping::PoseGraphInterface::LandmarkNode::LandmarkObservation;
|
PoseGraphInterface::LandmarkNode::LandmarkObservation;
|
||||||
|
|
||||||
static ceres::CostFunction* CreateAutoDiffCostFunction(
|
static ceres::CostFunction* CreateAutoDiffCostFunction(
|
||||||
const LandmarkObservation& observation, const NodeData& prev_node,
|
const LandmarkObservation& observation,
|
||||||
const NodeData& next_node) {
|
const OptimizationProblem2D::NodeData& prev_node,
|
||||||
|
const OptimizationProblem2D::NodeData& next_node) {
|
||||||
return new ceres::AutoDiffCostFunction<
|
return new ceres::AutoDiffCostFunction<
|
||||||
LandmarkCostFunction, 6 /* residuals */,
|
LandmarkCostFunction2D, 6 /* residuals */,
|
||||||
3 /* previous node pose variables */, 3 /* next node pose variables */,
|
3 /* previous node pose variables */, 3 /* next node pose variables */,
|
||||||
4 /* landmark rotation variables */,
|
4 /* landmark rotation variables */,
|
||||||
3 /* landmark translation variables */>(
|
3 /* landmark translation variables */>(
|
||||||
new LandmarkCostFunction(observation, prev_node, next_node));
|
new LandmarkCostFunction2D(observation, prev_node, next_node));
|
||||||
}
|
}
|
||||||
|
|
||||||
template <typename T>
|
template <typename T>
|
||||||
bool operator()(const T* const prev_node_pose, const T* const next_node_pose,
|
bool operator()(const T* const prev_node_pose, const T* const next_node_pose,
|
||||||
const T* const landmark_rotation,
|
const T* const landmark_rotation,
|
||||||
const T* const landmark_translation, T* const e) const {
|
const T* const landmark_translation, T* const e) const {
|
||||||
using mapping::pose_graph::ComputeUnscaledError;
|
using pose_graph::ComputeUnscaledError;
|
||||||
using mapping::pose_graph::ScaleError;
|
using pose_graph::ScaleError;
|
||||||
using mapping::pose_graph::SlerpQuaternions;
|
using pose_graph::SlerpQuaternions;
|
||||||
|
|
||||||
const std::array<T, 3> interpolated_pose_translation{
|
const std::array<T, 3> interpolated_pose_translation{
|
||||||
{prev_node_pose[0] +
|
{prev_node_pose[0] +
|
||||||
|
@ -102,8 +103,9 @@ class LandmarkCostFunction {
|
||||||
}
|
}
|
||||||
|
|
||||||
private:
|
private:
|
||||||
LandmarkCostFunction(const LandmarkObservation& observation,
|
LandmarkCostFunction2D(const LandmarkObservation& observation,
|
||||||
const NodeData& prev_node, const NodeData& next_node)
|
const OptimizationProblem2D::NodeData& prev_node,
|
||||||
|
const OptimizationProblem2D::NodeData& next_node)
|
||||||
: landmark_to_tracking_transform_(
|
: landmark_to_tracking_transform_(
|
||||||
observation.landmark_to_tracking_transform),
|
observation.landmark_to_tracking_transform),
|
||||||
prev_node_gravity_alignment_(prev_node.gravity_alignment),
|
prev_node_gravity_alignment_(prev_node.gravity_alignment),
|
||||||
|
@ -123,7 +125,7 @@ class LandmarkCostFunction {
|
||||||
};
|
};
|
||||||
|
|
||||||
} // namespace pose_graph
|
} // namespace pose_graph
|
||||||
} // namespace mapping_2d
|
} // namespace mapping
|
||||||
} // namespace cartographer
|
} // namespace cartographer
|
||||||
|
|
||||||
#endif // CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_LANDMARK_COST_FUNCTION_H_
|
#endif // CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_LANDMARK_COST_FUNCTION_2D_H_
|
|
@ -14,7 +14,7 @@
|
||||||
* limitations under the License.
|
* limitations under the License.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#include "cartographer/mapping_2d/pose_graph/landmark_cost_function.h"
|
#include "cartographer/mapping_2d/pose_graph/landmark_cost_function_2d.h"
|
||||||
|
|
||||||
#include <memory>
|
#include <memory>
|
||||||
|
|
||||||
|
@ -23,7 +23,7 @@
|
||||||
#include "gtest/gtest.h"
|
#include "gtest/gtest.h"
|
||||||
|
|
||||||
namespace cartographer {
|
namespace cartographer {
|
||||||
namespace mapping_2d {
|
namespace mapping {
|
||||||
namespace pose_graph {
|
namespace pose_graph {
|
||||||
namespace {
|
namespace {
|
||||||
|
|
||||||
|
@ -31,18 +31,18 @@ using ::testing::DoubleEq;
|
||||||
using ::testing::ElementsAre;
|
using ::testing::ElementsAre;
|
||||||
|
|
||||||
using LandmarkObservation =
|
using LandmarkObservation =
|
||||||
mapping::PoseGraphInterface::LandmarkNode::LandmarkObservation;
|
PoseGraphInterface::LandmarkNode::LandmarkObservation;
|
||||||
|
|
||||||
TEST(LandmarkCostFunctionTest, SmokeTest) {
|
TEST(LandmarkCostFunctionTest, SmokeTest) {
|
||||||
NodeData prev_node;
|
OptimizationProblem2D::NodeData prev_node;
|
||||||
prev_node.time = common::FromUniversal(0);
|
prev_node.time = common::FromUniversal(0);
|
||||||
prev_node.gravity_alignment = Eigen::Quaterniond::Identity();
|
prev_node.gravity_alignment = Eigen::Quaterniond::Identity();
|
||||||
NodeData next_node;
|
OptimizationProblem2D::NodeData next_node;
|
||||||
next_node.time = common::FromUniversal(10);
|
next_node.time = common::FromUniversal(10);
|
||||||
next_node.gravity_alignment = Eigen::Quaterniond::Identity();
|
next_node.gravity_alignment = Eigen::Quaterniond::Identity();
|
||||||
|
|
||||||
std::unique_ptr<ceres::CostFunction> cost_function(
|
std::unique_ptr<ceres::CostFunction> cost_function(
|
||||||
LandmarkCostFunction::CreateAutoDiffCostFunction(
|
LandmarkCostFunction2D::CreateAutoDiffCostFunction(
|
||||||
LandmarkObservation{
|
LandmarkObservation{
|
||||||
0 /* trajectory ID */,
|
0 /* trajectory ID */,
|
||||||
common::FromUniversal(5) /* time */,
|
common::FromUniversal(5) /* time */,
|
||||||
|
@ -73,5 +73,5 @@ TEST(LandmarkCostFunctionTest, SmokeTest) {
|
||||||
|
|
||||||
} // namespace
|
} // namespace
|
||||||
} // namespace pose_graph
|
} // namespace pose_graph
|
||||||
} // namespace mapping_2d
|
} // namespace mapping
|
||||||
} // namespace cartographer
|
} // namespace cartographer
|
|
@ -14,7 +14,7 @@
|
||||||
* limitations under the License.
|
* limitations under the License.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#include "cartographer/mapping_2d/pose_graph/optimization_problem.h"
|
#include "cartographer/mapping_2d/pose_graph/optimization_problem_2d.h"
|
||||||
|
|
||||||
#include <algorithm>
|
#include <algorithm>
|
||||||
#include <array>
|
#include <array>
|
||||||
|
@ -28,21 +28,22 @@
|
||||||
#include "cartographer/common/histogram.h"
|
#include "cartographer/common/histogram.h"
|
||||||
#include "cartographer/common/math.h"
|
#include "cartographer/common/math.h"
|
||||||
#include "cartographer/mapping/pose_graph/ceres_pose.h"
|
#include "cartographer/mapping/pose_graph/ceres_pose.h"
|
||||||
#include "cartographer/mapping_2d/pose_graph/landmark_cost_function.h"
|
#include "cartographer/mapping_2d/pose_graph/landmark_cost_function_2d.h"
|
||||||
#include "cartographer/mapping_2d/pose_graph/spa_cost_function.h"
|
#include "cartographer/mapping_2d/pose_graph/spa_cost_function_2d.h"
|
||||||
#include "cartographer/sensor/odometry_data.h"
|
#include "cartographer/sensor/odometry_data.h"
|
||||||
#include "cartographer/transform/transform.h"
|
#include "cartographer/transform/transform.h"
|
||||||
#include "ceres/ceres.h"
|
#include "ceres/ceres.h"
|
||||||
#include "glog/logging.h"
|
#include "glog/logging.h"
|
||||||
|
|
||||||
namespace cartographer {
|
namespace cartographer {
|
||||||
namespace mapping_2d {
|
namespace mapping {
|
||||||
namespace pose_graph {
|
namespace pose_graph {
|
||||||
|
|
||||||
namespace {
|
namespace {
|
||||||
|
|
||||||
using ::cartographer::mapping::pose_graph::CeresPose;
|
using ::cartographer::mapping::pose_graph::CeresPose;
|
||||||
using LandmarkNode = ::cartographer::mapping::PoseGraphInterface::LandmarkNode;
|
using LandmarkNode = ::cartographer::mapping::PoseGraphInterface::LandmarkNode;
|
||||||
|
using NodeData = OptimizationProblem2D::NodeData;
|
||||||
|
using SubmapData = OptimizationProblem2D::SubmapData;
|
||||||
|
|
||||||
// Converts a pose into the 3 optimization variable format used for Ceres:
|
// Converts a pose into the 3 optimization variable format used for Ceres:
|
||||||
// translation in x and y, followed by the rotation angle representing the
|
// translation in x and y, followed by the rotation angle representing the
|
||||||
|
@ -73,8 +74,8 @@ transform::Rigid3d GetInitialLandmarkPose(
|
||||||
|
|
||||||
void AddLandmarkCostFunctions(
|
void AddLandmarkCostFunctions(
|
||||||
const std::map<std::string, LandmarkNode>& landmark_nodes,
|
const std::map<std::string, LandmarkNode>& landmark_nodes,
|
||||||
const mapping::MapById<mapping::NodeId, NodeData>& node_data,
|
const MapById<NodeId, NodeData>& node_data,
|
||||||
mapping::MapById<mapping::NodeId, std::array<double, 3>>* C_nodes,
|
MapById<NodeId, std::array<double, 3>>* C_nodes,
|
||||||
std::map<std::string, CeresPose>* C_landmarks, ceres::Problem* problem) {
|
std::map<std::string, CeresPose>* C_landmarks, ceres::Problem* problem) {
|
||||||
for (const auto& landmark_node : landmark_nodes) {
|
for (const auto& landmark_node : landmark_nodes) {
|
||||||
for (const auto& observation : landmark_node.second.landmark_observations) {
|
for (const auto& observation : landmark_node.second.landmark_observations) {
|
||||||
|
@ -110,7 +111,7 @@ void AddLandmarkCostFunctions(
|
||||||
problem));
|
problem));
|
||||||
}
|
}
|
||||||
problem->AddResidualBlock(
|
problem->AddResidualBlock(
|
||||||
LandmarkCostFunction::CreateAutoDiffCostFunction(
|
LandmarkCostFunction2D::CreateAutoDiffCostFunction(
|
||||||
observation, prev->data, next->data),
|
observation, prev->data, next->data),
|
||||||
nullptr /* loss function */, C_nodes->at(prev->id).data(),
|
nullptr /* loss function */, C_nodes->at(prev->id).data(),
|
||||||
C_nodes->at(next->id).data(), C_landmarks->at(landmark_id).rotation(),
|
C_nodes->at(next->id).data(), C_landmarks->at(landmark_id).rotation(),
|
||||||
|
@ -121,23 +122,23 @@ void AddLandmarkCostFunctions(
|
||||||
|
|
||||||
} // namespace
|
} // namespace
|
||||||
|
|
||||||
OptimizationProblem::OptimizationProblem(
|
OptimizationProblem2D::OptimizationProblem2D(
|
||||||
const mapping::pose_graph::proto::OptimizationProblemOptions& options)
|
const pose_graph::proto::OptimizationProblemOptions& options)
|
||||||
: options_(options) {}
|
: options_(options) {}
|
||||||
|
|
||||||
OptimizationProblem::~OptimizationProblem() {}
|
OptimizationProblem2D::~OptimizationProblem2D() {}
|
||||||
|
|
||||||
void OptimizationProblem::AddImuData(const int trajectory_id,
|
void OptimizationProblem2D::AddImuData(const int trajectory_id,
|
||||||
const sensor::ImuData& imu_data) {
|
const sensor::ImuData& imu_data) {
|
||||||
imu_data_.Append(trajectory_id, imu_data);
|
imu_data_.Append(trajectory_id, imu_data);
|
||||||
}
|
}
|
||||||
|
|
||||||
void OptimizationProblem::AddOdometryData(
|
void OptimizationProblem2D::AddOdometryData(
|
||||||
const int trajectory_id, const sensor::OdometryData& odometry_data) {
|
const int trajectory_id, const sensor::OdometryData& odometry_data) {
|
||||||
odometry_data_.Append(trajectory_id, odometry_data);
|
odometry_data_.Append(trajectory_id, odometry_data);
|
||||||
}
|
}
|
||||||
|
|
||||||
void OptimizationProblem::AddTrajectoryNode(
|
void OptimizationProblem2D::AddTrajectoryNode(
|
||||||
const int trajectory_id, const common::Time time,
|
const int trajectory_id, const common::Time time,
|
||||||
const transform::Rigid2d& initial_pose, const transform::Rigid2d& pose,
|
const transform::Rigid2d& initial_pose, const transform::Rigid2d& pose,
|
||||||
const Eigen::Quaterniond& gravity_alignment) {
|
const Eigen::Quaterniond& gravity_alignment) {
|
||||||
|
@ -145,41 +146,41 @@ void OptimizationProblem::AddTrajectoryNode(
|
||||||
NodeData{time, initial_pose, pose, gravity_alignment});
|
NodeData{time, initial_pose, pose, gravity_alignment});
|
||||||
}
|
}
|
||||||
|
|
||||||
void OptimizationProblem::InsertTrajectoryNode(
|
void OptimizationProblem2D::InsertTrajectoryNode(
|
||||||
const mapping::NodeId& node_id, const common::Time time,
|
const NodeId& node_id, const common::Time time,
|
||||||
const transform::Rigid2d& initial_pose, const transform::Rigid2d& pose,
|
const transform::Rigid2d& initial_pose, const transform::Rigid2d& pose,
|
||||||
const Eigen::Quaterniond& gravity_alignment) {
|
const Eigen::Quaterniond& gravity_alignment) {
|
||||||
node_data_.Insert(node_id,
|
node_data_.Insert(node_id,
|
||||||
NodeData{time, initial_pose, pose, gravity_alignment});
|
NodeData{time, initial_pose, pose, gravity_alignment});
|
||||||
}
|
}
|
||||||
|
|
||||||
void OptimizationProblem::TrimTrajectoryNode(const mapping::NodeId& node_id) {
|
void OptimizationProblem2D::TrimTrajectoryNode(const NodeId& node_id) {
|
||||||
imu_data_.Trim(node_data_, node_id);
|
imu_data_.Trim(node_data_, node_id);
|
||||||
odometry_data_.Trim(node_data_, node_id);
|
odometry_data_.Trim(node_data_, node_id);
|
||||||
node_data_.Trim(node_id);
|
node_data_.Trim(node_id);
|
||||||
}
|
}
|
||||||
|
|
||||||
void OptimizationProblem::AddSubmap(
|
void OptimizationProblem2D::AddSubmap(
|
||||||
const int trajectory_id, const transform::Rigid2d& global_submap_pose) {
|
const int trajectory_id, const transform::Rigid2d& global_submap_pose) {
|
||||||
submap_data_.Append(trajectory_id, SubmapData{global_submap_pose});
|
submap_data_.Append(trajectory_id, SubmapData{global_submap_pose});
|
||||||
}
|
}
|
||||||
|
|
||||||
void OptimizationProblem::InsertSubmap(
|
void OptimizationProblem2D::InsertSubmap(
|
||||||
const mapping::SubmapId& submap_id,
|
const SubmapId& submap_id, const transform::Rigid2d& global_submap_pose) {
|
||||||
const transform::Rigid2d& global_submap_pose) {
|
|
||||||
submap_data_.Insert(submap_id, SubmapData{global_submap_pose});
|
submap_data_.Insert(submap_id, SubmapData{global_submap_pose});
|
||||||
}
|
}
|
||||||
|
|
||||||
void OptimizationProblem::TrimSubmap(const mapping::SubmapId& submap_id) {
|
void OptimizationProblem2D::TrimSubmap(const SubmapId& submap_id) {
|
||||||
submap_data_.Trim(submap_id);
|
submap_data_.Trim(submap_id);
|
||||||
}
|
}
|
||||||
|
|
||||||
void OptimizationProblem::SetMaxNumIterations(const int32 max_num_iterations) {
|
void OptimizationProblem2D::SetMaxNumIterations(
|
||||||
|
const int32 max_num_iterations) {
|
||||||
options_.mutable_ceres_solver_options()->set_max_num_iterations(
|
options_.mutable_ceres_solver_options()->set_max_num_iterations(
|
||||||
max_num_iterations);
|
max_num_iterations);
|
||||||
}
|
}
|
||||||
|
|
||||||
void OptimizationProblem::Solve(
|
void OptimizationProblem2D::Solve(
|
||||||
const std::vector<Constraint>& constraints,
|
const std::vector<Constraint>& constraints,
|
||||||
const std::set<int>& frozen_trajectories,
|
const std::set<int>& frozen_trajectories,
|
||||||
const std::map<std::string, LandmarkNode>& landmark_nodes) {
|
const std::map<std::string, LandmarkNode>& landmark_nodes) {
|
||||||
|
@ -193,8 +194,8 @@ void OptimizationProblem::Solve(
|
||||||
|
|
||||||
// Set the starting point.
|
// Set the starting point.
|
||||||
// TODO(hrapp): Move ceres data into SubmapData.
|
// TODO(hrapp): Move ceres data into SubmapData.
|
||||||
mapping::MapById<mapping::SubmapId, std::array<double, 3>> C_submaps;
|
MapById<SubmapId, std::array<double, 3>> C_submaps;
|
||||||
mapping::MapById<mapping::NodeId, std::array<double, 3>> C_nodes;
|
MapById<NodeId, std::array<double, 3>> C_nodes;
|
||||||
std::map<std::string, CeresPose> C_landmarks;
|
std::map<std::string, CeresPose> C_landmarks;
|
||||||
bool first_submap = true;
|
bool first_submap = true;
|
||||||
for (const auto& submap_id_data : submap_data_) {
|
for (const auto& submap_id_data : submap_data_) {
|
||||||
|
@ -222,7 +223,7 @@ void OptimizationProblem::Solve(
|
||||||
// Add cost functions for intra- and inter-submap constraints.
|
// Add cost functions for intra- and inter-submap constraints.
|
||||||
for (const Constraint& constraint : constraints) {
|
for (const Constraint& constraint : constraints) {
|
||||||
problem.AddResidualBlock(
|
problem.AddResidualBlock(
|
||||||
SpaCostFunction::CreateAutoDiffCostFunction(constraint.pose),
|
SpaCostFunction2D::CreateAutoDiffCostFunction(constraint.pose),
|
||||||
// Only loop closure constraints should have a loss function.
|
// Only loop closure constraints should have a loss function.
|
||||||
constraint.tag == Constraint::INTER_SUBMAP
|
constraint.tag == Constraint::INTER_SUBMAP
|
||||||
? new ceres::HuberLoss(options_.huber_scale())
|
? new ceres::HuberLoss(options_.huber_scale())
|
||||||
|
@ -245,10 +246,10 @@ void OptimizationProblem::Solve(
|
||||||
|
|
||||||
auto prev_node_it = node_it;
|
auto prev_node_it = node_it;
|
||||||
for (++node_it; node_it != trajectory_end; ++node_it) {
|
for (++node_it; node_it != trajectory_end; ++node_it) {
|
||||||
const mapping::NodeId first_node_id = prev_node_it->id;
|
const NodeId first_node_id = prev_node_it->id;
|
||||||
const NodeData& first_node_data = prev_node_it->data;
|
const NodeData& first_node_data = prev_node_it->data;
|
||||||
prev_node_it = node_it;
|
prev_node_it = node_it;
|
||||||
const mapping::NodeId second_node_id = node_it->id;
|
const NodeId second_node_id = node_it->id;
|
||||||
const NodeData& second_node_data = node_it->data;
|
const NodeData& second_node_data = node_it->data;
|
||||||
|
|
||||||
if (second_node_id.node_index != first_node_id.node_index + 1) {
|
if (second_node_id.node_index != first_node_id.node_index + 1) {
|
||||||
|
@ -258,7 +259,7 @@ void OptimizationProblem::Solve(
|
||||||
const transform::Rigid3d relative_pose =
|
const transform::Rigid3d relative_pose =
|
||||||
ComputeRelativePose(trajectory_id, first_node_data, second_node_data);
|
ComputeRelativePose(trajectory_id, first_node_data, second_node_data);
|
||||||
problem.AddResidualBlock(
|
problem.AddResidualBlock(
|
||||||
SpaCostFunction::CreateAutoDiffCostFunction(Constraint::Pose{
|
SpaCostFunction2D::CreateAutoDiffCostFunction(Constraint::Pose{
|
||||||
relative_pose, options_.consecutive_node_translation_weight(),
|
relative_pose, options_.consecutive_node_translation_weight(),
|
||||||
options_.consecutive_node_rotation_weight()}),
|
options_.consecutive_node_rotation_weight()}),
|
||||||
nullptr /* loss function */, C_nodes.at(first_node_id).data(),
|
nullptr /* loss function */, C_nodes.at(first_node_id).data(),
|
||||||
|
@ -288,32 +289,31 @@ void OptimizationProblem::Solve(
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
const mapping::MapById<mapping::NodeId, NodeData>&
|
const MapById<NodeId, NodeData>& OptimizationProblem2D::node_data() const {
|
||||||
OptimizationProblem::node_data() const {
|
|
||||||
return node_data_;
|
return node_data_;
|
||||||
}
|
}
|
||||||
|
|
||||||
const mapping::MapById<mapping::SubmapId, SubmapData>&
|
const MapById<SubmapId, SubmapData>& OptimizationProblem2D::submap_data()
|
||||||
OptimizationProblem::submap_data() const {
|
const {
|
||||||
return submap_data_;
|
return submap_data_;
|
||||||
}
|
}
|
||||||
|
|
||||||
const std::map<std::string, transform::Rigid3d>&
|
const std::map<std::string, transform::Rigid3d>&
|
||||||
OptimizationProblem::landmark_data() const {
|
OptimizationProblem2D::landmark_data() const {
|
||||||
return landmark_data_;
|
return landmark_data_;
|
||||||
}
|
}
|
||||||
|
|
||||||
const sensor::MapByTime<sensor::ImuData>& OptimizationProblem::imu_data()
|
const sensor::MapByTime<sensor::ImuData>& OptimizationProblem2D::imu_data()
|
||||||
const {
|
const {
|
||||||
return imu_data_;
|
return imu_data_;
|
||||||
}
|
}
|
||||||
|
|
||||||
const sensor::MapByTime<sensor::OdometryData>&
|
const sensor::MapByTime<sensor::OdometryData>&
|
||||||
OptimizationProblem::odometry_data() const {
|
OptimizationProblem2D::odometry_data() const {
|
||||||
return odometry_data_;
|
return odometry_data_;
|
||||||
}
|
}
|
||||||
|
|
||||||
std::unique_ptr<transform::Rigid3d> OptimizationProblem::InterpolateOdometry(
|
std::unique_ptr<transform::Rigid3d> OptimizationProblem2D::InterpolateOdometry(
|
||||||
const int trajectory_id, const common::Time time) const {
|
const int trajectory_id, const common::Time time) const {
|
||||||
const auto it = odometry_data_.lower_bound(trajectory_id, time);
|
const auto it = odometry_data_.lower_bound(trajectory_id, time);
|
||||||
if (it == odometry_data_.EndOfTrajectory(trajectory_id)) {
|
if (it == odometry_data_.EndOfTrajectory(trajectory_id)) {
|
||||||
|
@ -332,7 +332,7 @@ std::unique_ptr<transform::Rigid3d> OptimizationProblem::InterpolateOdometry(
|
||||||
.transform);
|
.transform);
|
||||||
}
|
}
|
||||||
|
|
||||||
transform::Rigid3d OptimizationProblem::ComputeRelativePose(
|
transform::Rigid3d OptimizationProblem2D::ComputeRelativePose(
|
||||||
const int trajectory_id, const NodeData& first_node_data,
|
const int trajectory_id, const NodeData& first_node_data,
|
||||||
const NodeData& second_node_data) const {
|
const NodeData& second_node_data) const {
|
||||||
if (odometry_data_.HasTrajectory(trajectory_id)) {
|
if (odometry_data_.HasTrajectory(trajectory_id)) {
|
||||||
|
@ -352,5 +352,5 @@ transform::Rigid3d OptimizationProblem::ComputeRelativePose(
|
||||||
}
|
}
|
||||||
|
|
||||||
} // namespace pose_graph
|
} // namespace pose_graph
|
||||||
} // namespace mapping_2d
|
} // namespace mapping
|
||||||
} // namespace cartographer
|
} // namespace cartographer
|
|
@ -14,8 +14,8 @@
|
||||||
* limitations under the License.
|
* limitations under the License.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#ifndef CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_OPTIMIZATION_PROBLEM_H_
|
#ifndef CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_OPTIMIZATION_PROBLEM_2D_H_
|
||||||
#define CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_OPTIMIZATION_PROBLEM_H_
|
#define CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_OPTIMIZATION_PROBLEM_2D_H_
|
||||||
|
|
||||||
#include <array>
|
#include <array>
|
||||||
#include <deque>
|
#include <deque>
|
||||||
|
@ -36,9 +36,15 @@
|
||||||
#include "cartographer/transform/timestamped_transform.h"
|
#include "cartographer/transform/timestamped_transform.h"
|
||||||
|
|
||||||
namespace cartographer {
|
namespace cartographer {
|
||||||
namespace mapping_2d {
|
namespace mapping {
|
||||||
namespace pose_graph {
|
namespace pose_graph {
|
||||||
|
|
||||||
|
// Implements the SPA loop closure method.
|
||||||
|
class OptimizationProblem2D {
|
||||||
|
public:
|
||||||
|
using Constraint = PoseGraphInterface::Constraint;
|
||||||
|
using LandmarkNode = PoseGraphInterface::LandmarkNode;
|
||||||
|
|
||||||
struct NodeData {
|
struct NodeData {
|
||||||
common::Time time;
|
common::Time time;
|
||||||
transform::Rigid2d initial_pose;
|
transform::Rigid2d initial_pose;
|
||||||
|
@ -50,18 +56,12 @@ struct SubmapData {
|
||||||
transform::Rigid2d global_pose;
|
transform::Rigid2d global_pose;
|
||||||
};
|
};
|
||||||
|
|
||||||
// Implements the SPA loop closure method.
|
explicit OptimizationProblem2D(
|
||||||
class OptimizationProblem {
|
const pose_graph::proto::OptimizationProblemOptions& options);
|
||||||
public:
|
~OptimizationProblem2D();
|
||||||
using Constraint = mapping::PoseGraphInterface::Constraint;
|
|
||||||
using LandmarkNode = mapping::PoseGraphInterface::LandmarkNode;
|
|
||||||
|
|
||||||
explicit OptimizationProblem(
|
OptimizationProblem2D(const OptimizationProblem2D&) = delete;
|
||||||
const mapping::pose_graph::proto::OptimizationProblemOptions& options);
|
OptimizationProblem2D& operator=(const OptimizationProblem2D&) = delete;
|
||||||
~OptimizationProblem();
|
|
||||||
|
|
||||||
OptimizationProblem(const OptimizationProblem&) = delete;
|
|
||||||
OptimizationProblem& operator=(const OptimizationProblem&) = delete;
|
|
||||||
|
|
||||||
void AddImuData(int trajectory_id, const sensor::ImuData& imu_data);
|
void AddImuData(int trajectory_id, const sensor::ImuData& imu_data);
|
||||||
void AddOdometryData(int trajectory_id,
|
void AddOdometryData(int trajectory_id,
|
||||||
|
@ -70,16 +70,16 @@ class OptimizationProblem {
|
||||||
const transform::Rigid2d& initial_pose,
|
const transform::Rigid2d& initial_pose,
|
||||||
const transform::Rigid2d& pose,
|
const transform::Rigid2d& pose,
|
||||||
const Eigen::Quaterniond& gravity_alignment);
|
const Eigen::Quaterniond& gravity_alignment);
|
||||||
void InsertTrajectoryNode(const mapping::NodeId& node_id, common::Time time,
|
void InsertTrajectoryNode(const NodeId& node_id, common::Time time,
|
||||||
const transform::Rigid2d& initial_pose,
|
const transform::Rigid2d& initial_pose,
|
||||||
const transform::Rigid2d& pose,
|
const transform::Rigid2d& pose,
|
||||||
const Eigen::Quaterniond& gravity_alignment);
|
const Eigen::Quaterniond& gravity_alignment);
|
||||||
void TrimTrajectoryNode(const mapping::NodeId& node_id);
|
void TrimTrajectoryNode(const NodeId& node_id);
|
||||||
void AddSubmap(int trajectory_id,
|
void AddSubmap(int trajectory_id,
|
||||||
const transform::Rigid2d& global_submap_pose);
|
const transform::Rigid2d& global_submap_pose);
|
||||||
void InsertSubmap(const mapping::SubmapId& submap_id,
|
void InsertSubmap(const SubmapId& submap_id,
|
||||||
const transform::Rigid2d& global_submap_pose);
|
const transform::Rigid2d& global_submap_pose);
|
||||||
void TrimSubmap(const mapping::SubmapId& submap_id);
|
void TrimSubmap(const SubmapId& submap_id);
|
||||||
|
|
||||||
void SetMaxNumIterations(int32 max_num_iterations);
|
void SetMaxNumIterations(int32 max_num_iterations);
|
||||||
|
|
||||||
|
@ -88,8 +88,8 @@ class OptimizationProblem {
|
||||||
const std::set<int>& frozen_trajectories,
|
const std::set<int>& frozen_trajectories,
|
||||||
const std::map<std::string, LandmarkNode>& landmark_nodes);
|
const std::map<std::string, LandmarkNode>& landmark_nodes);
|
||||||
|
|
||||||
const mapping::MapById<mapping::NodeId, NodeData>& node_data() const;
|
const MapById<NodeId, NodeData>& node_data() const;
|
||||||
const mapping::MapById<mapping::SubmapId, SubmapData>& submap_data() const;
|
const MapById<SubmapId, SubmapData>& submap_data() const;
|
||||||
const std::map<std::string, transform::Rigid3d>& landmark_data() const;
|
const std::map<std::string, transform::Rigid3d>& landmark_data() const;
|
||||||
const sensor::MapByTime<sensor::ImuData>& imu_data() const;
|
const sensor::MapByTime<sensor::ImuData>& imu_data() const;
|
||||||
const sensor::MapByTime<sensor::OdometryData>& odometry_data() const;
|
const sensor::MapByTime<sensor::OdometryData>& odometry_data() const;
|
||||||
|
@ -102,16 +102,16 @@ class OptimizationProblem {
|
||||||
int trajectory_id, const NodeData& first_node_data,
|
int trajectory_id, const NodeData& first_node_data,
|
||||||
const NodeData& second_node_data) const;
|
const NodeData& second_node_data) const;
|
||||||
|
|
||||||
mapping::pose_graph::proto::OptimizationProblemOptions options_;
|
pose_graph::proto::OptimizationProblemOptions options_;
|
||||||
mapping::MapById<mapping::NodeId, NodeData> node_data_;
|
MapById<NodeId, NodeData> node_data_;
|
||||||
mapping::MapById<mapping::SubmapId, SubmapData> submap_data_;
|
MapById<SubmapId, SubmapData> submap_data_;
|
||||||
std::map<std::string, transform::Rigid3d> landmark_data_;
|
std::map<std::string, transform::Rigid3d> landmark_data_;
|
||||||
sensor::MapByTime<sensor::ImuData> imu_data_;
|
sensor::MapByTime<sensor::ImuData> imu_data_;
|
||||||
sensor::MapByTime<sensor::OdometryData> odometry_data_;
|
sensor::MapByTime<sensor::OdometryData> odometry_data_;
|
||||||
};
|
};
|
||||||
|
|
||||||
} // namespace pose_graph
|
} // namespace pose_graph
|
||||||
} // namespace mapping_2d
|
} // namespace mapping
|
||||||
} // namespace cartographer
|
} // namespace cartographer
|
||||||
|
|
||||||
#endif // CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_OPTIMIZATION_PROBLEM_H_
|
#endif // CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_OPTIMIZATION_PROBLEM_2D_H_
|
|
@ -14,8 +14,8 @@
|
||||||
* limitations under the License.
|
* limitations under the License.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#ifndef CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_SPA_COST_FUNCTION_H_
|
#ifndef CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_SPA_COST_FUNCTION_2D_H_
|
||||||
#define CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_SPA_COST_FUNCTION_H_
|
#define CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_SPA_COST_FUNCTION_2D_H_
|
||||||
|
|
||||||
#include <array>
|
#include <array>
|
||||||
|
|
||||||
|
@ -30,25 +30,23 @@
|
||||||
#include "ceres/jet.h"
|
#include "ceres/jet.h"
|
||||||
|
|
||||||
namespace cartographer {
|
namespace cartographer {
|
||||||
namespace mapping_2d {
|
namespace mapping {
|
||||||
namespace pose_graph {
|
namespace pose_graph {
|
||||||
|
|
||||||
class SpaCostFunction {
|
class SpaCostFunction2D {
|
||||||
public:
|
public:
|
||||||
using Constraint = mapping::PoseGraph::Constraint;
|
|
||||||
|
|
||||||
static ceres::CostFunction* CreateAutoDiffCostFunction(
|
static ceres::CostFunction* CreateAutoDiffCostFunction(
|
||||||
const Constraint::Pose& pose) {
|
const PoseGraph::Constraint::Pose& pose) {
|
||||||
return new ceres::AutoDiffCostFunction<SpaCostFunction, 3 /* residuals */,
|
return new ceres::AutoDiffCostFunction<SpaCostFunction2D, 3 /* residuals */,
|
||||||
3 /* pose variables */,
|
3 /* pose variables */,
|
||||||
3 /* pose variables */>(
|
3 /* pose variables */>(
|
||||||
new SpaCostFunction(pose));
|
new SpaCostFunction2D(pose));
|
||||||
}
|
}
|
||||||
|
|
||||||
template <typename T>
|
template <typename T>
|
||||||
bool operator()(const T* const c_i, const T* const c_j, T* e) const {
|
bool operator()(const T* const c_i, const T* const c_j, T* e) const {
|
||||||
using mapping::pose_graph::ComputeUnscaledError;
|
using pose_graph::ComputeUnscaledError;
|
||||||
using mapping::pose_graph::ScaleError;
|
using pose_graph::ScaleError;
|
||||||
|
|
||||||
const std::array<T, 3> error = ScaleError(
|
const std::array<T, 3> error = ScaleError(
|
||||||
ComputeUnscaledError(transform::Project2D(pose_.zbar_ij), c_i, c_j),
|
ComputeUnscaledError(transform::Project2D(pose_.zbar_ij), c_i, c_j),
|
||||||
|
@ -58,13 +56,14 @@ class SpaCostFunction {
|
||||||
}
|
}
|
||||||
|
|
||||||
private:
|
private:
|
||||||
explicit SpaCostFunction(const Constraint::Pose& pose) : pose_(pose) {}
|
explicit SpaCostFunction2D(const PoseGraph::Constraint::Pose& pose)
|
||||||
|
: pose_(pose) {}
|
||||||
|
|
||||||
const Constraint::Pose pose_;
|
const PoseGraph::Constraint::Pose pose_;
|
||||||
};
|
};
|
||||||
|
|
||||||
} // namespace pose_graph
|
} // namespace pose_graph
|
||||||
} // namespace mapping_2d
|
} // namespace mapping
|
||||||
} // namespace cartographer
|
} // namespace cartographer
|
||||||
|
|
||||||
#endif // CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_SPA_COST_FUNCTION_H_
|
#endif // CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_SPA_COST_FUNCTION_2D_H_
|
|
@ -86,9 +86,8 @@ std::vector<SubmapId> PoseGraph2D::InitializeGlobalSubmapPoses(
|
||||||
optimization_problem_.AddSubmap(
|
optimization_problem_.AddSubmap(
|
||||||
trajectory_id,
|
trajectory_id,
|
||||||
first_submap_pose *
|
first_submap_pose *
|
||||||
mapping_2d::pose_graph::ComputeSubmapPose(*insertion_submaps[0])
|
pose_graph::ComputeSubmapPose(*insertion_submaps[0]).inverse() *
|
||||||
.inverse() *
|
pose_graph::ComputeSubmapPose(*insertion_submaps[1]));
|
||||||
mapping_2d::pose_graph::ComputeSubmapPose(*insertion_submaps[1]));
|
|
||||||
return {last_submap_id,
|
return {last_submap_id,
|
||||||
SubmapId{trajectory_id, last_submap_id.submap_index + 1}};
|
SubmapId{trajectory_id, last_submap_id.submap_index + 1}};
|
||||||
}
|
}
|
||||||
|
@ -244,8 +243,7 @@ void PoseGraph2D::ComputeConstraintsForNode(
|
||||||
transform::Rigid3d::Rotation(constant_data->gravity_alignment.inverse()));
|
transform::Rigid3d::Rotation(constant_data->gravity_alignment.inverse()));
|
||||||
const transform::Rigid2d optimized_pose =
|
const transform::Rigid2d optimized_pose =
|
||||||
optimization_problem_.submap_data().at(matching_id).global_pose *
|
optimization_problem_.submap_data().at(matching_id).global_pose *
|
||||||
mapping_2d::pose_graph::ComputeSubmapPose(*insertion_submaps.front())
|
pose_graph::ComputeSubmapPose(*insertion_submaps.front()).inverse() *
|
||||||
.inverse() *
|
|
||||||
pose;
|
pose;
|
||||||
optimization_problem_.AddTrajectoryNode(
|
optimization_problem_.AddTrajectoryNode(
|
||||||
matching_id.trajectory_id, constant_data->time, pose, optimized_pose,
|
matching_id.trajectory_id, constant_data->time, pose, optimized_pose,
|
||||||
|
@ -257,9 +255,7 @@ void PoseGraph2D::ComputeConstraintsForNode(
|
||||||
CHECK(submap_data_.at(submap_id).state == SubmapState::kActive);
|
CHECK(submap_data_.at(submap_id).state == SubmapState::kActive);
|
||||||
submap_data_.at(submap_id).node_ids.emplace(node_id);
|
submap_data_.at(submap_id).node_ids.emplace(node_id);
|
||||||
const transform::Rigid2d constraint_transform =
|
const transform::Rigid2d constraint_transform =
|
||||||
mapping_2d::pose_graph::ComputeSubmapPose(*insertion_submaps[i])
|
pose_graph::ComputeSubmapPose(*insertion_submaps[i]).inverse() * pose;
|
||||||
.inverse() *
|
|
||||||
pose;
|
|
||||||
constraints_.push_back(Constraint{submap_id,
|
constraints_.push_back(Constraint{submap_id,
|
||||||
node_id,
|
node_id,
|
||||||
{transform::Embed3D(constraint_transform),
|
{transform::Embed3D(constraint_transform),
|
||||||
|
@ -325,7 +321,7 @@ void PoseGraph2D::UpdateTrajectoryConnectivity(const Constraint& constraint) {
|
||||||
|
|
||||||
void PoseGraph2D::HandleWorkQueue() {
|
void PoseGraph2D::HandleWorkQueue() {
|
||||||
constraint_builder_.WhenDone(
|
constraint_builder_.WhenDone(
|
||||||
[this](const mapping_2d::pose_graph::ConstraintBuilder::Result& result) {
|
[this](const pose_graph::ConstraintBuilder2D::Result& result) {
|
||||||
{
|
{
|
||||||
common::MutexLocker locker(&mutex_);
|
common::MutexLocker locker(&mutex_);
|
||||||
constraints_.insert(constraints_.end(), result.begin(), result.end());
|
constraints_.insert(constraints_.end(), result.begin(), result.end());
|
||||||
|
@ -386,8 +382,8 @@ void PoseGraph2D::WaitForAllComputations() {
|
||||||
}
|
}
|
||||||
std::cout << "\r\x1b[KOptimizing: Done. " << std::endl;
|
std::cout << "\r\x1b[KOptimizing: Done. " << std::endl;
|
||||||
constraint_builder_.WhenDone(
|
constraint_builder_.WhenDone(
|
||||||
[this, ¬ification](
|
[this,
|
||||||
const mapping_2d::pose_graph::ConstraintBuilder::Result& result) {
|
¬ification](const pose_graph::ConstraintBuilder2D::Result& result) {
|
||||||
common::MutexLocker locker(&mutex_);
|
common::MutexLocker locker(&mutex_);
|
||||||
constraints_.insert(constraints_.end(), result.begin(), result.end());
|
constraints_.insert(constraints_.end(), result.begin(), result.end());
|
||||||
notification = true;
|
notification = true;
|
||||||
|
@ -440,7 +436,8 @@ void PoseGraph2D::AddSubmapFromProto(
|
||||||
submap_data_.at(submap_id).submap = submap_ptr;
|
submap_data_.at(submap_id).submap = submap_ptr;
|
||||||
// Immediately show the submap at the 'global_submap_pose'.
|
// Immediately show the submap at the 'global_submap_pose'.
|
||||||
global_submap_poses_.Insert(
|
global_submap_poses_.Insert(
|
||||||
submap_id, mapping_2d::pose_graph::SubmapData{global_submap_pose_2d});
|
submap_id,
|
||||||
|
pose_graph::OptimizationProblem2D::SubmapData{global_submap_pose_2d});
|
||||||
AddWorkItem([this, submap_id, global_submap_pose_2d]() REQUIRES(mutex_) {
|
AddWorkItem([this, submap_id, global_submap_pose_2d]() REQUIRES(mutex_) {
|
||||||
CHECK_EQ(frozen_trajectories_.count(submap_id.trajectory_id), 1);
|
CHECK_EQ(frozen_trajectories_.count(submap_id.trajectory_id), 1);
|
||||||
submap_data_.at(submap_id).state = SubmapState::kFinished;
|
submap_data_.at(submap_id).state = SubmapState::kFinished;
|
||||||
|
@ -728,7 +725,7 @@ PoseGraph2D::GetAllSubmapPoses() {
|
||||||
}
|
}
|
||||||
|
|
||||||
transform::Rigid3d PoseGraph2D::ComputeLocalToGlobalTransform(
|
transform::Rigid3d PoseGraph2D::ComputeLocalToGlobalTransform(
|
||||||
const MapById<SubmapId, mapping_2d::pose_graph::SubmapData>&
|
const MapById<SubmapId, pose_graph::OptimizationProblem2D::SubmapData>&
|
||||||
global_submap_poses,
|
global_submap_poses,
|
||||||
const int trajectory_id) const {
|
const int trajectory_id) const {
|
||||||
auto begin_it = global_submap_poses.BeginOfTrajectory(trajectory_id);
|
auto begin_it = global_submap_poses.BeginOfTrajectory(trajectory_id);
|
||||||
|
|
|
@ -35,8 +35,8 @@
|
||||||
#include "cartographer/mapping/pose_graph.h"
|
#include "cartographer/mapping/pose_graph.h"
|
||||||
#include "cartographer/mapping/pose_graph_trimmer.h"
|
#include "cartographer/mapping/pose_graph_trimmer.h"
|
||||||
#include "cartographer/mapping/trajectory_connectivity_state.h"
|
#include "cartographer/mapping/trajectory_connectivity_state.h"
|
||||||
#include "cartographer/mapping_2d/pose_graph/constraint_builder.h"
|
#include "cartographer/mapping_2d/pose_graph/constraint_builder_2d.h"
|
||||||
#include "cartographer/mapping_2d/pose_graph/optimization_problem.h"
|
#include "cartographer/mapping_2d/pose_graph/optimization_problem_2d.h"
|
||||||
#include "cartographer/mapping_2d/submap_2d.h"
|
#include "cartographer/mapping_2d/submap_2d.h"
|
||||||
#include "cartographer/sensor/fixed_frame_pose_data.h"
|
#include "cartographer/sensor/fixed_frame_pose_data.h"
|
||||||
#include "cartographer/sensor/landmark_data.h"
|
#include "cartographer/sensor/landmark_data.h"
|
||||||
|
@ -190,7 +190,7 @@ class PoseGraph2D : public PoseGraph {
|
||||||
// Computes the local to global map frame transform based on the given
|
// Computes the local to global map frame transform based on the given
|
||||||
// 'global_submap_poses'.
|
// 'global_submap_poses'.
|
||||||
transform::Rigid3d ComputeLocalToGlobalTransform(
|
transform::Rigid3d ComputeLocalToGlobalTransform(
|
||||||
const MapById<SubmapId, mapping_2d::pose_graph::SubmapData>&
|
const MapById<SubmapId, pose_graph::OptimizationProblem2D::SubmapData>&
|
||||||
global_submap_poses,
|
global_submap_poses,
|
||||||
int trajectory_id) const REQUIRES(mutex_);
|
int trajectory_id) const REQUIRES(mutex_);
|
||||||
|
|
||||||
|
@ -230,9 +230,8 @@ class PoseGraph2D : public PoseGraph {
|
||||||
void DispatchOptimization() REQUIRES(mutex_);
|
void DispatchOptimization() REQUIRES(mutex_);
|
||||||
|
|
||||||
// Current optimization problem.
|
// Current optimization problem.
|
||||||
mapping_2d::pose_graph::OptimizationProblem optimization_problem_;
|
pose_graph::OptimizationProblem2D optimization_problem_;
|
||||||
mapping_2d::pose_graph::ConstraintBuilder constraint_builder_
|
pose_graph::ConstraintBuilder2D constraint_builder_ GUARDED_BY(mutex_);
|
||||||
GUARDED_BY(mutex_);
|
|
||||||
std::vector<Constraint> constraints_ GUARDED_BY(mutex_);
|
std::vector<Constraint> constraints_ GUARDED_BY(mutex_);
|
||||||
|
|
||||||
// Submaps get assigned an ID and state as soon as they are seen, even
|
// Submaps get assigned an ID and state as soon as they are seen, even
|
||||||
|
@ -244,8 +243,8 @@ class PoseGraph2D : public PoseGraph {
|
||||||
int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0;
|
int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0;
|
||||||
|
|
||||||
// Global submap poses currently used for displaying data.
|
// Global submap poses currently used for displaying data.
|
||||||
MapById<SubmapId, mapping_2d::pose_graph::SubmapData> global_submap_poses_
|
MapById<SubmapId, pose_graph::OptimizationProblem2D::SubmapData>
|
||||||
GUARDED_BY(mutex_);
|
global_submap_poses_ GUARDED_BY(mutex_);
|
||||||
|
|
||||||
// Global landmark poses with all observations.
|
// Global landmark poses with all observations.
|
||||||
std::map<std::string /* landmark ID */, PoseGraph::LandmarkNode>
|
std::map<std::string /* landmark ID */, PoseGraph::LandmarkNode>
|
||||||
|
|
|
@ -19,8 +19,8 @@ package cartographer.mapping.proto;
|
||||||
import "cartographer/mapping/proto/motion_filter_options.proto";
|
import "cartographer/mapping/proto/motion_filter_options.proto";
|
||||||
import "cartographer/sensor/proto/adaptive_voxel_filter_options.proto";
|
import "cartographer/sensor/proto/adaptive_voxel_filter_options.proto";
|
||||||
import "cartographer/mapping_2d/proto/submaps_options_2d.proto";
|
import "cartographer/mapping_2d/proto/submaps_options_2d.proto";
|
||||||
import "cartographer/mapping_2d/scan_matching/proto/ceres_scan_matcher_options.proto";
|
import "cartographer/mapping_2d/scan_matching/proto/ceres_scan_matcher_options_2d.proto";
|
||||||
import "cartographer/mapping_2d/scan_matching/proto/real_time_correlative_scan_matcher_options.proto";
|
import "cartographer/mapping/scan_matching/proto/real_time_correlative_scan_matcher_options.proto";
|
||||||
|
|
||||||
message LocalTrajectoryBuilderOptions2D {
|
message LocalTrajectoryBuilderOptions2D {
|
||||||
// Rangefinder points outside these ranges will be dropped.
|
// Rangefinder points outside these ranges will be dropped.
|
||||||
|
@ -51,10 +51,9 @@ message LocalTrajectoryBuilderOptions2D {
|
||||||
// Whether to solve the online scan matching first using the correlative scan
|
// Whether to solve the online scan matching first using the correlative scan
|
||||||
// matcher to generate a good starting point for Ceres.
|
// matcher to generate a good starting point for Ceres.
|
||||||
bool use_online_correlative_scan_matching = 5;
|
bool use_online_correlative_scan_matching = 5;
|
||||||
cartographer.mapping_2d.scan_matching.proto
|
cartographer.mapping.scan_matching.proto.RealTimeCorrelativeScanMatcherOptions
|
||||||
.RealTimeCorrelativeScanMatcherOptions
|
|
||||||
real_time_correlative_scan_matcher_options = 7;
|
real_time_correlative_scan_matcher_options = 7;
|
||||||
cartographer.mapping_2d.scan_matching.proto.CeresScanMatcherOptions
|
cartographer.mapping.scan_matching.proto.CeresScanMatcherOptions2D
|
||||||
ceres_scan_matcher_options = 8;
|
ceres_scan_matcher_options = 8;
|
||||||
MotionFilterOptions motion_filter_options = 13;
|
MotionFilterOptions motion_filter_options = 13;
|
||||||
|
|
||||||
|
|
|
@ -14,7 +14,7 @@
|
||||||
* limitations under the License.
|
* limitations under the License.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#include "cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h"
|
#include "cartographer/mapping_2d/scan_matching/ceres_scan_matcher_2d.h"
|
||||||
|
|
||||||
#include <utility>
|
#include <utility>
|
||||||
#include <vector>
|
#include <vector>
|
||||||
|
@ -22,21 +22,21 @@
|
||||||
#include "Eigen/Core"
|
#include "Eigen/Core"
|
||||||
#include "cartographer/common/ceres_solver_options.h"
|
#include "cartographer/common/ceres_solver_options.h"
|
||||||
#include "cartographer/common/lua_parameter_dictionary.h"
|
#include "cartographer/common/lua_parameter_dictionary.h"
|
||||||
#include "cartographer/internal/mapping_2d/scan_matching/occupied_space_cost_function.h"
|
#include "cartographer/internal/mapping_2d/scan_matching/occupied_space_cost_function_2d.h"
|
||||||
#include "cartographer/mapping_2d/probability_grid.h"
|
#include "cartographer/mapping_2d/probability_grid.h"
|
||||||
#include "cartographer/mapping_2d/scan_matching/rotation_delta_cost_functor.h"
|
#include "cartographer/mapping_2d/scan_matching/rotation_delta_cost_functor_2d.h"
|
||||||
#include "cartographer/mapping_2d/scan_matching/translation_delta_cost_functor.h"
|
#include "cartographer/mapping_2d/scan_matching/translation_delta_cost_functor_2d.h"
|
||||||
#include "cartographer/transform/transform.h"
|
#include "cartographer/transform/transform.h"
|
||||||
#include "ceres/ceres.h"
|
#include "ceres/ceres.h"
|
||||||
#include "glog/logging.h"
|
#include "glog/logging.h"
|
||||||
|
|
||||||
namespace cartographer {
|
namespace cartographer {
|
||||||
namespace mapping_2d {
|
namespace mapping {
|
||||||
namespace scan_matching {
|
namespace scan_matching {
|
||||||
|
|
||||||
proto::CeresScanMatcherOptions CreateCeresScanMatcherOptions(
|
proto::CeresScanMatcherOptions2D CreateCeresScanMatcherOptions2D(
|
||||||
common::LuaParameterDictionary* const parameter_dictionary) {
|
common::LuaParameterDictionary* const parameter_dictionary) {
|
||||||
proto::CeresScanMatcherOptions options;
|
proto::CeresScanMatcherOptions2D options;
|
||||||
options.set_occupied_space_weight(
|
options.set_occupied_space_weight(
|
||||||
parameter_dictionary->GetDouble("occupied_space_weight"));
|
parameter_dictionary->GetDouble("occupied_space_weight"));
|
||||||
options.set_translation_weight(
|
options.set_translation_weight(
|
||||||
|
@ -49,20 +49,20 @@ proto::CeresScanMatcherOptions CreateCeresScanMatcherOptions(
|
||||||
return options;
|
return options;
|
||||||
}
|
}
|
||||||
|
|
||||||
CeresScanMatcher::CeresScanMatcher(
|
CeresScanMatcher2D::CeresScanMatcher2D(
|
||||||
const proto::CeresScanMatcherOptions& options)
|
const proto::CeresScanMatcherOptions2D& options)
|
||||||
: options_(options),
|
: options_(options),
|
||||||
ceres_solver_options_(
|
ceres_solver_options_(
|
||||||
common::CreateCeresSolverOptions(options.ceres_solver_options())) {
|
common::CreateCeresSolverOptions(options.ceres_solver_options())) {
|
||||||
ceres_solver_options_.linear_solver_type = ceres::DENSE_QR;
|
ceres_solver_options_.linear_solver_type = ceres::DENSE_QR;
|
||||||
}
|
}
|
||||||
|
|
||||||
CeresScanMatcher::~CeresScanMatcher() {}
|
CeresScanMatcher2D::~CeresScanMatcher2D() {}
|
||||||
|
|
||||||
void CeresScanMatcher::Match(const Eigen::Vector2d& target_translation,
|
void CeresScanMatcher2D::Match(const Eigen::Vector2d& target_translation,
|
||||||
const transform::Rigid2d& initial_pose_estimate,
|
const transform::Rigid2d& initial_pose_estimate,
|
||||||
const sensor::PointCloud& point_cloud,
|
const sensor::PointCloud& point_cloud,
|
||||||
const mapping::ProbabilityGrid& probability_grid,
|
const ProbabilityGrid& probability_grid,
|
||||||
transform::Rigid2d* const pose_estimate,
|
transform::Rigid2d* const pose_estimate,
|
||||||
ceres::Solver::Summary* const summary) const {
|
ceres::Solver::Summary* const summary) const {
|
||||||
double ceres_pose_estimate[3] = {initial_pose_estimate.translation().x(),
|
double ceres_pose_estimate[3] = {initial_pose_estimate.translation().x(),
|
||||||
|
@ -71,19 +71,19 @@ void CeresScanMatcher::Match(const Eigen::Vector2d& target_translation,
|
||||||
ceres::Problem problem;
|
ceres::Problem problem;
|
||||||
CHECK_GT(options_.occupied_space_weight(), 0.);
|
CHECK_GT(options_.occupied_space_weight(), 0.);
|
||||||
problem.AddResidualBlock(
|
problem.AddResidualBlock(
|
||||||
OccupiedSpaceCostFunction::CreateAutoDiffCostFunction(
|
OccupiedSpaceCostFunction2D::CreateAutoDiffCostFunction(
|
||||||
options_.occupied_space_weight() /
|
options_.occupied_space_weight() /
|
||||||
std::sqrt(static_cast<double>(point_cloud.size())),
|
std::sqrt(static_cast<double>(point_cloud.size())),
|
||||||
point_cloud, probability_grid),
|
point_cloud, probability_grid),
|
||||||
nullptr /* loss function */, ceres_pose_estimate);
|
nullptr /* loss function */, ceres_pose_estimate);
|
||||||
CHECK_GT(options_.translation_weight(), 0.);
|
CHECK_GT(options_.translation_weight(), 0.);
|
||||||
problem.AddResidualBlock(
|
problem.AddResidualBlock(
|
||||||
TranslationDeltaCostFunctor::CreateAutoDiffCostFunction(
|
TranslationDeltaCostFunctor2D::CreateAutoDiffCostFunction(
|
||||||
options_.translation_weight(), target_translation),
|
options_.translation_weight(), target_translation),
|
||||||
nullptr /* loss function */, ceres_pose_estimate);
|
nullptr /* loss function */, ceres_pose_estimate);
|
||||||
CHECK_GT(options_.rotation_weight(), 0.);
|
CHECK_GT(options_.rotation_weight(), 0.);
|
||||||
problem.AddResidualBlock(
|
problem.AddResidualBlock(
|
||||||
RotationDeltaCostFunctor::CreateAutoDiffCostFunction(
|
RotationDeltaCostFunctor2D::CreateAutoDiffCostFunction(
|
||||||
options_.rotation_weight(), ceres_pose_estimate[2]),
|
options_.rotation_weight(), ceres_pose_estimate[2]),
|
||||||
nullptr /* loss function */, ceres_pose_estimate);
|
nullptr /* loss function */, ceres_pose_estimate);
|
||||||
|
|
||||||
|
@ -94,5 +94,5 @@ void CeresScanMatcher::Match(const Eigen::Vector2d& target_translation,
|
||||||
}
|
}
|
||||||
|
|
||||||
} // namespace scan_matching
|
} // namespace scan_matching
|
||||||
} // namespace mapping_2d
|
} // namespace mapping
|
||||||
} // namespace cartographer
|
} // namespace cartographer
|
|
@ -14,8 +14,8 @@
|
||||||
* limitations under the License.
|
* limitations under the License.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#ifndef CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_CERES_SCAN_MATCHER_H_
|
#ifndef CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_CERES_SCAN_MATCHER_2D_H_
|
||||||
#define CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_CERES_SCAN_MATCHER_H_
|
#define CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_CERES_SCAN_MATCHER_2D_H_
|
||||||
|
|
||||||
#include <memory>
|
#include <memory>
|
||||||
#include <vector>
|
#include <vector>
|
||||||
|
@ -23,25 +23,25 @@
|
||||||
#include "Eigen/Core"
|
#include "Eigen/Core"
|
||||||
#include "cartographer/common/lua_parameter_dictionary.h"
|
#include "cartographer/common/lua_parameter_dictionary.h"
|
||||||
#include "cartographer/mapping_2d/probability_grid.h"
|
#include "cartographer/mapping_2d/probability_grid.h"
|
||||||
#include "cartographer/mapping_2d/scan_matching/proto/ceres_scan_matcher_options.pb.h"
|
#include "cartographer/mapping_2d/scan_matching/proto/ceres_scan_matcher_options_2d.pb.h"
|
||||||
#include "cartographer/sensor/point_cloud.h"
|
#include "cartographer/sensor/point_cloud.h"
|
||||||
#include "ceres/ceres.h"
|
#include "ceres/ceres.h"
|
||||||
|
|
||||||
namespace cartographer {
|
namespace cartographer {
|
||||||
namespace mapping_2d {
|
namespace mapping {
|
||||||
namespace scan_matching {
|
namespace scan_matching {
|
||||||
|
|
||||||
proto::CeresScanMatcherOptions CreateCeresScanMatcherOptions(
|
proto::CeresScanMatcherOptions2D CreateCeresScanMatcherOptions2D(
|
||||||
common::LuaParameterDictionary* parameter_dictionary);
|
common::LuaParameterDictionary* parameter_dictionary);
|
||||||
|
|
||||||
// Align scans with an existing map using Ceres.
|
// Align scans with an existing map using Ceres.
|
||||||
class CeresScanMatcher {
|
class CeresScanMatcher2D {
|
||||||
public:
|
public:
|
||||||
explicit CeresScanMatcher(const proto::CeresScanMatcherOptions& options);
|
explicit CeresScanMatcher2D(const proto::CeresScanMatcherOptions2D& options);
|
||||||
virtual ~CeresScanMatcher();
|
virtual ~CeresScanMatcher2D();
|
||||||
|
|
||||||
CeresScanMatcher(const CeresScanMatcher&) = delete;
|
CeresScanMatcher2D(const CeresScanMatcher2D&) = delete;
|
||||||
CeresScanMatcher& operator=(const CeresScanMatcher&) = delete;
|
CeresScanMatcher2D& operator=(const CeresScanMatcher2D&) = delete;
|
||||||
|
|
||||||
// Aligns 'point_cloud' within the 'probability_grid' given an
|
// Aligns 'point_cloud' within the 'probability_grid' given an
|
||||||
// 'initial_pose_estimate' and returns a 'pose_estimate' and the solver
|
// 'initial_pose_estimate' and returns a 'pose_estimate' and the solver
|
||||||
|
@ -49,17 +49,17 @@ class CeresScanMatcher {
|
||||||
void Match(const Eigen::Vector2d& target_translation,
|
void Match(const Eigen::Vector2d& target_translation,
|
||||||
const transform::Rigid2d& initial_pose_estimate,
|
const transform::Rigid2d& initial_pose_estimate,
|
||||||
const sensor::PointCloud& point_cloud,
|
const sensor::PointCloud& point_cloud,
|
||||||
const mapping::ProbabilityGrid& probability_grid,
|
const ProbabilityGrid& probability_grid,
|
||||||
transform::Rigid2d* pose_estimate,
|
transform::Rigid2d* pose_estimate,
|
||||||
ceres::Solver::Summary* summary) const;
|
ceres::Solver::Summary* summary) const;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
const proto::CeresScanMatcherOptions options_;
|
const proto::CeresScanMatcherOptions2D options_;
|
||||||
ceres::Solver::Options ceres_solver_options_;
|
ceres::Solver::Options ceres_solver_options_;
|
||||||
};
|
};
|
||||||
|
|
||||||
} // namespace scan_matching
|
} // namespace scan_matching
|
||||||
} // namespace mapping_2d
|
} // namespace mapping
|
||||||
} // namespace cartographer
|
} // namespace cartographer
|
||||||
|
|
||||||
#endif // CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_CERES_SCAN_MATCHER_H_
|
#endif // CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_CERES_SCAN_MATCHER_2D_H_
|
|
@ -14,7 +14,7 @@
|
||||||
* limitations under the License.
|
* limitations under the License.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#include "cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h"
|
#include "cartographer/mapping_2d/scan_matching/ceres_scan_matcher_2d.h"
|
||||||
|
|
||||||
#include <memory>
|
#include <memory>
|
||||||
|
|
||||||
|
@ -28,18 +28,18 @@
|
||||||
#include "gtest/gtest.h"
|
#include "gtest/gtest.h"
|
||||||
|
|
||||||
namespace cartographer {
|
namespace cartographer {
|
||||||
namespace mapping_2d {
|
namespace mapping {
|
||||||
namespace scan_matching {
|
namespace scan_matching {
|
||||||
namespace {
|
namespace {
|
||||||
|
|
||||||
class CeresScanMatcherTest : public ::testing::Test {
|
class CeresScanMatcherTest : public ::testing::Test {
|
||||||
protected:
|
protected:
|
||||||
CeresScanMatcherTest()
|
CeresScanMatcherTest()
|
||||||
: probability_grid_(mapping::MapLimits(1., Eigen::Vector2d(10., 10.),
|
: probability_grid_(
|
||||||
mapping::CellLimits(20, 20))) {
|
MapLimits(1., Eigen::Vector2d(10., 10.), CellLimits(20, 20))) {
|
||||||
probability_grid_.SetProbability(
|
probability_grid_.SetProbability(
|
||||||
probability_grid_.limits().GetCellIndex(Eigen::Vector2f(-3.5f, 2.5f)),
|
probability_grid_.limits().GetCellIndex(Eigen::Vector2f(-3.5f, 2.5f)),
|
||||||
mapping::kMaxProbability);
|
kMaxProbability);
|
||||||
|
|
||||||
point_cloud_.emplace_back(-3.f, 2.f, 0.f);
|
point_cloud_.emplace_back(-3.f, 2.f, 0.f);
|
||||||
|
|
||||||
|
@ -54,9 +54,9 @@ class CeresScanMatcherTest : public ::testing::Test {
|
||||||
num_threads = 1,
|
num_threads = 1,
|
||||||
},
|
},
|
||||||
})text");
|
})text");
|
||||||
const proto::CeresScanMatcherOptions options =
|
const proto::CeresScanMatcherOptions2D options =
|
||||||
CreateCeresScanMatcherOptions(parameter_dictionary.get());
|
CreateCeresScanMatcherOptions2D(parameter_dictionary.get());
|
||||||
ceres_scan_matcher_ = common::make_unique<CeresScanMatcher>(options);
|
ceres_scan_matcher_ = common::make_unique<CeresScanMatcher2D>(options);
|
||||||
}
|
}
|
||||||
|
|
||||||
void TestFromInitialPose(const transform::Rigid2d& initial_pose) {
|
void TestFromInitialPose(const transform::Rigid2d& initial_pose) {
|
||||||
|
@ -73,9 +73,9 @@ class CeresScanMatcherTest : public ::testing::Test {
|
||||||
<< "\nExpected: " << transform::ToProto(expected_pose).DebugString();
|
<< "\nExpected: " << transform::ToProto(expected_pose).DebugString();
|
||||||
}
|
}
|
||||||
|
|
||||||
mapping::ProbabilityGrid probability_grid_;
|
ProbabilityGrid probability_grid_;
|
||||||
sensor::PointCloud point_cloud_;
|
sensor::PointCloud point_cloud_;
|
||||||
std::unique_ptr<CeresScanMatcher> ceres_scan_matcher_;
|
std::unique_ptr<CeresScanMatcher2D> ceres_scan_matcher_;
|
||||||
};
|
};
|
||||||
|
|
||||||
TEST_F(CeresScanMatcherTest, testPerfectEstimate) {
|
TEST_F(CeresScanMatcherTest, testPerfectEstimate) {
|
||||||
|
@ -96,5 +96,5 @@ TEST_F(CeresScanMatcherTest, testOptimizeAlongXY) {
|
||||||
|
|
||||||
} // namespace
|
} // namespace
|
||||||
} // namespace scan_matching
|
} // namespace scan_matching
|
||||||
} // namespace mapping_2d
|
} // namespace mapping
|
||||||
} // namespace cartographer
|
} // namespace cartographer
|
|
@ -14,14 +14,14 @@
|
||||||
* limitations under the License.
|
* limitations under the License.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#include "cartographer/mapping_2d/scan_matching/correlative_scan_matcher.h"
|
#include "cartographer/mapping_2d/scan_matching/correlative_scan_matcher_2d.h"
|
||||||
|
|
||||||
#include <cmath>
|
#include <cmath>
|
||||||
|
|
||||||
#include "cartographer/common/math.h"
|
#include "cartographer/common/math.h"
|
||||||
|
|
||||||
namespace cartographer {
|
namespace cartographer {
|
||||||
namespace mapping_2d {
|
namespace mapping {
|
||||||
namespace scan_matching {
|
namespace scan_matching {
|
||||||
|
|
||||||
SearchParameters::SearchParameters(const double linear_search_window,
|
SearchParameters::SearchParameters(const double linear_search_window,
|
||||||
|
@ -71,7 +71,7 @@ SearchParameters::SearchParameters(const int num_linear_perturbations,
|
||||||
}
|
}
|
||||||
|
|
||||||
void SearchParameters::ShrinkToFit(const std::vector<DiscreteScan>& scans,
|
void SearchParameters::ShrinkToFit(const std::vector<DiscreteScan>& scans,
|
||||||
const mapping::CellLimits& cell_limits) {
|
const CellLimits& cell_limits) {
|
||||||
CHECK_EQ(scans.size(), num_scans);
|
CHECK_EQ(scans.size(), num_scans);
|
||||||
CHECK_EQ(linear_bounds.size(), num_scans);
|
CHECK_EQ(linear_bounds.size(), num_scans);
|
||||||
for (int i = 0; i != num_scans; ++i) {
|
for (int i = 0; i != num_scans; ++i) {
|
||||||
|
@ -109,8 +109,7 @@ std::vector<sensor::PointCloud> GenerateRotatedScans(
|
||||||
}
|
}
|
||||||
|
|
||||||
std::vector<DiscreteScan> DiscretizeScans(
|
std::vector<DiscreteScan> DiscretizeScans(
|
||||||
const mapping::MapLimits& map_limits,
|
const MapLimits& map_limits, const std::vector<sensor::PointCloud>& scans,
|
||||||
const std::vector<sensor::PointCloud>& scans,
|
|
||||||
const Eigen::Translation2f& initial_translation) {
|
const Eigen::Translation2f& initial_translation) {
|
||||||
std::vector<DiscreteScan> discrete_scans;
|
std::vector<DiscreteScan> discrete_scans;
|
||||||
discrete_scans.reserve(scans.size());
|
discrete_scans.reserve(scans.size());
|
||||||
|
@ -128,5 +127,5 @@ std::vector<DiscreteScan> DiscretizeScans(
|
||||||
}
|
}
|
||||||
|
|
||||||
} // namespace scan_matching
|
} // namespace scan_matching
|
||||||
} // namespace mapping_2d
|
} // namespace mapping
|
||||||
} // namespace cartographer
|
} // namespace cartographer
|
|
@ -14,8 +14,8 @@
|
||||||
* limitations under the License.
|
* limitations under the License.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#ifndef CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_CORRELATIVE_SCAN_MATCHER_H_
|
#ifndef CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_CORRELATIVE_SCAN_MATCHER_2D_H_
|
||||||
#define CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_CORRELATIVE_SCAN_MATCHER_H_
|
#define CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_CORRELATIVE_SCAN_MATCHER_2D_H_
|
||||||
|
|
||||||
#include <vector>
|
#include <vector>
|
||||||
|
|
||||||
|
@ -26,7 +26,7 @@
|
||||||
#include "cartographer/sensor/point_cloud.h"
|
#include "cartographer/sensor/point_cloud.h"
|
||||||
|
|
||||||
namespace cartographer {
|
namespace cartographer {
|
||||||
namespace mapping_2d {
|
namespace mapping {
|
||||||
namespace scan_matching {
|
namespace scan_matching {
|
||||||
|
|
||||||
typedef std::vector<Eigen::Array2i> DiscreteScan;
|
typedef std::vector<Eigen::Array2i> DiscreteScan;
|
||||||
|
@ -50,7 +50,7 @@ struct SearchParameters {
|
||||||
|
|
||||||
// Tightens the search window as much as possible.
|
// Tightens the search window as much as possible.
|
||||||
void ShrinkToFit(const std::vector<DiscreteScan>& scans,
|
void ShrinkToFit(const std::vector<DiscreteScan>& scans,
|
||||||
const mapping::CellLimits& cell_limits);
|
const CellLimits& cell_limits);
|
||||||
|
|
||||||
int num_angular_perturbations;
|
int num_angular_perturbations;
|
||||||
double angular_perturbation_step_size;
|
double angular_perturbation_step_size;
|
||||||
|
@ -67,8 +67,7 @@ std::vector<sensor::PointCloud> GenerateRotatedScans(
|
||||||
// Translates and discretizes the rotated scans into a vector of integer
|
// Translates and discretizes the rotated scans into a vector of integer
|
||||||
// indices.
|
// indices.
|
||||||
std::vector<DiscreteScan> DiscretizeScans(
|
std::vector<DiscreteScan> DiscretizeScans(
|
||||||
const mapping::MapLimits& map_limits,
|
const MapLimits& map_limits, const std::vector<sensor::PointCloud>& scans,
|
||||||
const std::vector<sensor::PointCloud>& scans,
|
|
||||||
const Eigen::Translation2f& initial_translation);
|
const Eigen::Translation2f& initial_translation);
|
||||||
|
|
||||||
// A possible solution.
|
// A possible solution.
|
||||||
|
@ -104,7 +103,7 @@ struct Candidate {
|
||||||
};
|
};
|
||||||
|
|
||||||
} // namespace scan_matching
|
} // namespace scan_matching
|
||||||
} // namespace mapping_2d
|
} // namespace mapping
|
||||||
} // namespace cartographer
|
} // namespace cartographer
|
||||||
|
|
||||||
#endif // CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_CORRELATIVE_SCAN_MATCHER_H_
|
#endif // CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_CORRELATIVE_SCAN_MATCHER_2D_H_
|
|
@ -14,13 +14,13 @@
|
||||||
* limitations under the License.
|
* limitations under the License.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#include "cartographer/mapping_2d/scan_matching/correlative_scan_matcher.h"
|
#include "cartographer/mapping_2d/scan_matching/correlative_scan_matcher_2d.h"
|
||||||
|
|
||||||
#include "cartographer/sensor/point_cloud.h"
|
#include "cartographer/sensor/point_cloud.h"
|
||||||
#include "gtest/gtest.h"
|
#include "gtest/gtest.h"
|
||||||
|
|
||||||
namespace cartographer {
|
namespace cartographer {
|
||||||
namespace mapping_2d {
|
namespace mapping {
|
||||||
namespace scan_matching {
|
namespace scan_matching {
|
||||||
namespace {
|
namespace {
|
||||||
|
|
||||||
|
@ -79,8 +79,8 @@ TEST(DiscretizeScans, DiscretizeScans) {
|
||||||
point_cloud.emplace_back(-0.125f, 0.125f, 0.f);
|
point_cloud.emplace_back(-0.125f, 0.125f, 0.f);
|
||||||
point_cloud.emplace_back(-0.125f, 0.075f, 0.f);
|
point_cloud.emplace_back(-0.125f, 0.075f, 0.f);
|
||||||
point_cloud.emplace_back(-0.125f, 0.025f, 0.f);
|
point_cloud.emplace_back(-0.125f, 0.025f, 0.f);
|
||||||
const mapping::MapLimits map_limits(0.05, Eigen::Vector2d(0.05, 0.25),
|
const MapLimits map_limits(0.05, Eigen::Vector2d(0.05, 0.25),
|
||||||
mapping::CellLimits(6, 6));
|
CellLimits(6, 6));
|
||||||
const std::vector<sensor::PointCloud> scans =
|
const std::vector<sensor::PointCloud> scans =
|
||||||
GenerateRotatedScans(point_cloud, SearchParameters(0, 0, 0., 0.));
|
GenerateRotatedScans(point_cloud, SearchParameters(0, 0, 0., 0.));
|
||||||
const std::vector<DiscreteScan> discrete_scans =
|
const std::vector<DiscreteScan> discrete_scans =
|
||||||
|
@ -98,5 +98,5 @@ TEST(DiscretizeScans, DiscretizeScans) {
|
||||||
|
|
||||||
} // namespace
|
} // namespace
|
||||||
} // namespace scan_matching
|
} // namespace scan_matching
|
||||||
} // namespace mapping_2d
|
} // namespace mapping
|
||||||
} // namespace cartographer
|
} // namespace cartographer
|
||||||
|
|
|
@ -14,7 +14,7 @@
|
||||||
* limitations under the License.
|
* limitations under the License.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#include "cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h"
|
#include "cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_2d.h"
|
||||||
|
|
||||||
#include <algorithm>
|
#include <algorithm>
|
||||||
#include <cmath>
|
#include <cmath>
|
||||||
|
@ -30,9 +30,8 @@
|
||||||
#include "glog/logging.h"
|
#include "glog/logging.h"
|
||||||
|
|
||||||
namespace cartographer {
|
namespace cartographer {
|
||||||
namespace mapping_2d {
|
namespace mapping {
|
||||||
namespace scan_matching {
|
namespace scan_matching {
|
||||||
|
|
||||||
namespace {
|
namespace {
|
||||||
|
|
||||||
// A collection of values which can be added and later removed, and the maximum
|
// A collection of values which can be added and later removed, and the maximum
|
||||||
|
@ -75,10 +74,10 @@ class SlidingWindowMaximum {
|
||||||
|
|
||||||
} // namespace
|
} // namespace
|
||||||
|
|
||||||
proto::FastCorrelativeScanMatcherOptions
|
proto::FastCorrelativeScanMatcherOptions2D
|
||||||
CreateFastCorrelativeScanMatcherOptions(
|
CreateFastCorrelativeScanMatcherOptions2D(
|
||||||
common::LuaParameterDictionary* const parameter_dictionary) {
|
common::LuaParameterDictionary* const parameter_dictionary) {
|
||||||
proto::FastCorrelativeScanMatcherOptions options;
|
proto::FastCorrelativeScanMatcherOptions2D options;
|
||||||
options.set_linear_search_window(
|
options.set_linear_search_window(
|
||||||
parameter_dictionary->GetDouble("linear_search_window"));
|
parameter_dictionary->GetDouble("linear_search_window"));
|
||||||
options.set_angular_search_window(
|
options.set_angular_search_window(
|
||||||
|
@ -89,9 +88,8 @@ CreateFastCorrelativeScanMatcherOptions(
|
||||||
}
|
}
|
||||||
|
|
||||||
PrecomputationGrid::PrecomputationGrid(
|
PrecomputationGrid::PrecomputationGrid(
|
||||||
const mapping::ProbabilityGrid& probability_grid,
|
const ProbabilityGrid& probability_grid, const CellLimits& limits,
|
||||||
const mapping::CellLimits& limits, const int width,
|
const int width, std::vector<float>* reusable_intermediate_grid)
|
||||||
std::vector<float>* reusable_intermediate_grid)
|
|
||||||
: offset_(-width + 1, -width + 1),
|
: offset_(-width + 1, -width + 1),
|
||||||
wide_limits_(limits.num_x_cells + width - 1,
|
wide_limits_(limits.num_x_cells + width - 1,
|
||||||
limits.num_y_cells + width - 1),
|
limits.num_y_cells + width - 1),
|
||||||
|
@ -160,9 +158,9 @@ PrecomputationGrid::PrecomputationGrid(
|
||||||
}
|
}
|
||||||
|
|
||||||
uint8 PrecomputationGrid::ComputeCellValue(const float probability) const {
|
uint8 PrecomputationGrid::ComputeCellValue(const float probability) const {
|
||||||
const int cell_value = common::RoundToInt(
|
const int cell_value =
|
||||||
(probability - mapping::kMinProbability) *
|
common::RoundToInt((probability - kMinProbability) *
|
||||||
(255.f / (mapping::kMaxProbability - mapping::kMinProbability)));
|
(255.f / (kMaxProbability - kMinProbability)));
|
||||||
CHECK_GE(cell_value, 0);
|
CHECK_GE(cell_value, 0);
|
||||||
CHECK_LE(cell_value, 255);
|
CHECK_LE(cell_value, 255);
|
||||||
return cell_value;
|
return cell_value;
|
||||||
|
@ -171,13 +169,13 @@ uint8 PrecomputationGrid::ComputeCellValue(const float probability) const {
|
||||||
class PrecomputationGridStack {
|
class PrecomputationGridStack {
|
||||||
public:
|
public:
|
||||||
PrecomputationGridStack(
|
PrecomputationGridStack(
|
||||||
const mapping::ProbabilityGrid& probability_grid,
|
const ProbabilityGrid& probability_grid,
|
||||||
const proto::FastCorrelativeScanMatcherOptions& options) {
|
const proto::FastCorrelativeScanMatcherOptions2D& options) {
|
||||||
CHECK_GE(options.branch_and_bound_depth(), 1);
|
CHECK_GE(options.branch_and_bound_depth(), 1);
|
||||||
const int max_width = 1 << (options.branch_and_bound_depth() - 1);
|
const int max_width = 1 << (options.branch_and_bound_depth() - 1);
|
||||||
precomputation_grids_.reserve(options.branch_and_bound_depth());
|
precomputation_grids_.reserve(options.branch_and_bound_depth());
|
||||||
std::vector<float> reusable_intermediate_grid;
|
std::vector<float> reusable_intermediate_grid;
|
||||||
const mapping::CellLimits limits = probability_grid.limits().cell_limits();
|
const CellLimits limits = probability_grid.limits().cell_limits();
|
||||||
reusable_intermediate_grid.reserve((limits.num_x_cells + max_width - 1) *
|
reusable_intermediate_grid.reserve((limits.num_x_cells + max_width - 1) *
|
||||||
limits.num_y_cells);
|
limits.num_y_cells);
|
||||||
for (int i = 0; i != options.branch_and_bound_depth(); ++i) {
|
for (int i = 0; i != options.branch_and_bound_depth(); ++i) {
|
||||||
|
@ -197,17 +195,17 @@ class PrecomputationGridStack {
|
||||||
std::vector<PrecomputationGrid> precomputation_grids_;
|
std::vector<PrecomputationGrid> precomputation_grids_;
|
||||||
};
|
};
|
||||||
|
|
||||||
FastCorrelativeScanMatcher::FastCorrelativeScanMatcher(
|
FastCorrelativeScanMatcher2D::FastCorrelativeScanMatcher2D(
|
||||||
const mapping::ProbabilityGrid& probability_grid,
|
const ProbabilityGrid& probability_grid,
|
||||||
const proto::FastCorrelativeScanMatcherOptions& options)
|
const proto::FastCorrelativeScanMatcherOptions2D& options)
|
||||||
: options_(options),
|
: options_(options),
|
||||||
limits_(probability_grid.limits()),
|
limits_(probability_grid.limits()),
|
||||||
precomputation_grid_stack_(
|
precomputation_grid_stack_(
|
||||||
new PrecomputationGridStack(probability_grid, options)) {}
|
new PrecomputationGridStack(probability_grid, options)) {}
|
||||||
|
|
||||||
FastCorrelativeScanMatcher::~FastCorrelativeScanMatcher() {}
|
FastCorrelativeScanMatcher2D::~FastCorrelativeScanMatcher2D() {}
|
||||||
|
|
||||||
bool FastCorrelativeScanMatcher::Match(
|
bool FastCorrelativeScanMatcher2D::Match(
|
||||||
const transform::Rigid2d& initial_pose_estimate,
|
const transform::Rigid2d& initial_pose_estimate,
|
||||||
const sensor::PointCloud& point_cloud, const float min_score, float* score,
|
const sensor::PointCloud& point_cloud, const float min_score, float* score,
|
||||||
transform::Rigid2d* pose_estimate) const {
|
transform::Rigid2d* pose_estimate) const {
|
||||||
|
@ -219,7 +217,7 @@ bool FastCorrelativeScanMatcher::Match(
|
||||||
pose_estimate);
|
pose_estimate);
|
||||||
}
|
}
|
||||||
|
|
||||||
bool FastCorrelativeScanMatcher::MatchFullSubmap(
|
bool FastCorrelativeScanMatcher2D::MatchFullSubmap(
|
||||||
const sensor::PointCloud& point_cloud, float min_score, float* score,
|
const sensor::PointCloud& point_cloud, float min_score, float* score,
|
||||||
transform::Rigid2d* pose_estimate) const {
|
transform::Rigid2d* pose_estimate) 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
|
||||||
|
@ -236,7 +234,7 @@ bool FastCorrelativeScanMatcher::MatchFullSubmap(
|
||||||
min_score, score, pose_estimate);
|
min_score, score, pose_estimate);
|
||||||
}
|
}
|
||||||
|
|
||||||
bool FastCorrelativeScanMatcher::MatchWithSearchParameters(
|
bool FastCorrelativeScanMatcher2D::MatchWithSearchParameters(
|
||||||
SearchParameters search_parameters,
|
SearchParameters search_parameters,
|
||||||
const transform::Rigid2d& initial_pose_estimate,
|
const transform::Rigid2d& initial_pose_estimate,
|
||||||
const sensor::PointCloud& point_cloud, float min_score, float* score,
|
const sensor::PointCloud& point_cloud, float min_score, float* score,
|
||||||
|
@ -274,7 +272,7 @@ bool FastCorrelativeScanMatcher::MatchWithSearchParameters(
|
||||||
}
|
}
|
||||||
|
|
||||||
std::vector<Candidate>
|
std::vector<Candidate>
|
||||||
FastCorrelativeScanMatcher::ComputeLowestResolutionCandidates(
|
FastCorrelativeScanMatcher2D::ComputeLowestResolutionCandidates(
|
||||||
const std::vector<DiscreteScan>& discrete_scans,
|
const std::vector<DiscreteScan>& discrete_scans,
|
||||||
const SearchParameters& search_parameters) const {
|
const SearchParameters& search_parameters) const {
|
||||||
std::vector<Candidate> lowest_resolution_candidates =
|
std::vector<Candidate> lowest_resolution_candidates =
|
||||||
|
@ -286,7 +284,7 @@ FastCorrelativeScanMatcher::ComputeLowestResolutionCandidates(
|
||||||
}
|
}
|
||||||
|
|
||||||
std::vector<Candidate>
|
std::vector<Candidate>
|
||||||
FastCorrelativeScanMatcher::GenerateLowestResolutionCandidates(
|
FastCorrelativeScanMatcher2D::GenerateLowestResolutionCandidates(
|
||||||
const SearchParameters& search_parameters) const {
|
const SearchParameters& search_parameters) const {
|
||||||
const int linear_step_size = 1 << precomputation_grid_stack_->max_depth();
|
const int linear_step_size = 1 << precomputation_grid_stack_->max_depth();
|
||||||
int num_candidates = 0;
|
int num_candidates = 0;
|
||||||
|
@ -323,7 +321,7 @@ FastCorrelativeScanMatcher::GenerateLowestResolutionCandidates(
|
||||||
return candidates;
|
return candidates;
|
||||||
}
|
}
|
||||||
|
|
||||||
void FastCorrelativeScanMatcher::ScoreCandidates(
|
void FastCorrelativeScanMatcher2D::ScoreCandidates(
|
||||||
const PrecomputationGrid& precomputation_grid,
|
const PrecomputationGrid& precomputation_grid,
|
||||||
const std::vector<DiscreteScan>& discrete_scans,
|
const std::vector<DiscreteScan>& discrete_scans,
|
||||||
const SearchParameters& search_parameters,
|
const SearchParameters& search_parameters,
|
||||||
|
@ -343,7 +341,7 @@ void FastCorrelativeScanMatcher::ScoreCandidates(
|
||||||
std::sort(candidates->begin(), candidates->end(), std::greater<Candidate>());
|
std::sort(candidates->begin(), candidates->end(), std::greater<Candidate>());
|
||||||
}
|
}
|
||||||
|
|
||||||
Candidate FastCorrelativeScanMatcher::BranchAndBound(
|
Candidate FastCorrelativeScanMatcher2D::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,
|
||||||
|
@ -389,5 +387,5 @@ Candidate FastCorrelativeScanMatcher::BranchAndBound(
|
||||||
}
|
}
|
||||||
|
|
||||||
} // namespace scan_matching
|
} // namespace scan_matching
|
||||||
} // namespace mapping_2d
|
} // namespace mapping
|
||||||
} // namespace cartographer
|
} // namespace cartographer
|
|
@ -22,8 +22,8 @@
|
||||||
// precomputation done for a given map. However, this map is immutable after
|
// precomputation done for a given map. However, this map is immutable after
|
||||||
// construction.
|
// construction.
|
||||||
|
|
||||||
#ifndef CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_FAST_CORRELATIVE_SCAN_MATCHER_H_
|
#ifndef CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_FAST_CORRELATIVE_SCAN_MATCHER_2D_H_
|
||||||
#define CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_FAST_CORRELATIVE_SCAN_MATCHER_H_
|
#define CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_FAST_CORRELATIVE_SCAN_MATCHER_2D_H_
|
||||||
|
|
||||||
#include <memory>
|
#include <memory>
|
||||||
#include <vector>
|
#include <vector>
|
||||||
|
@ -32,16 +32,16 @@
|
||||||
#include "cartographer/common/port.h"
|
#include "cartographer/common/port.h"
|
||||||
#include "cartographer/mapping/probability_values.h"
|
#include "cartographer/mapping/probability_values.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_2d.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_2d.pb.h"
|
||||||
#include "cartographer/sensor/point_cloud.h"
|
#include "cartographer/sensor/point_cloud.h"
|
||||||
|
|
||||||
namespace cartographer {
|
namespace cartographer {
|
||||||
namespace mapping_2d {
|
namespace mapping {
|
||||||
namespace scan_matching {
|
namespace scan_matching {
|
||||||
|
|
||||||
proto::FastCorrelativeScanMatcherOptions
|
proto::FastCorrelativeScanMatcherOptions2D
|
||||||
CreateFastCorrelativeScanMatcherOptions(
|
CreateFastCorrelativeScanMatcherOptions2D(
|
||||||
common::LuaParameterDictionary* parameter_dictionary);
|
common::LuaParameterDictionary* parameter_dictionary);
|
||||||
|
|
||||||
// A precomputed grid that contains in each cell (x0, y0) the maximum
|
// A precomputed grid that contains in each cell (x0, y0) the maximum
|
||||||
|
@ -49,8 +49,8 @@ CreateFastCorrelativeScanMatcherOptions(
|
||||||
// y0 <= y < y0.
|
// y0 <= y < y0.
|
||||||
class PrecomputationGrid {
|
class PrecomputationGrid {
|
||||||
public:
|
public:
|
||||||
PrecomputationGrid(const mapping::ProbabilityGrid& probability_grid,
|
PrecomputationGrid(const ProbabilityGrid& probability_grid,
|
||||||
const mapping::CellLimits& limits, int width,
|
const CellLimits& limits, int width,
|
||||||
std::vector<float>* reusable_intermediate_grid);
|
std::vector<float>* reusable_intermediate_grid);
|
||||||
|
|
||||||
// Returns a value between 0 and 255 to represent probabilities between
|
// Returns a value between 0 and 255 to represent probabilities between
|
||||||
|
@ -74,9 +74,8 @@ class PrecomputationGrid {
|
||||||
|
|
||||||
// Maps values from [0, 255] to [kMinProbability, kMaxProbability].
|
// Maps values from [0, 255] to [kMinProbability, kMaxProbability].
|
||||||
static float ToProbability(float value) {
|
static float ToProbability(float value) {
|
||||||
return mapping::kMinProbability +
|
return kMinProbability +
|
||||||
value *
|
value * ((kMaxProbability - kMinProbability) / 255.f);
|
||||||
((mapping::kMaxProbability - mapping::kMinProbability) / 255.f);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
@ -87,7 +86,7 @@ class PrecomputationGrid {
|
||||||
const Eigen::Array2i offset_;
|
const Eigen::Array2i offset_;
|
||||||
|
|
||||||
// Size of the precomputation grid.
|
// Size of the precomputation grid.
|
||||||
const mapping::CellLimits wide_limits_;
|
const CellLimits wide_limits_;
|
||||||
|
|
||||||
// Probabilites mapped to 0 to 255.
|
// Probabilites mapped to 0 to 255.
|
||||||
std::vector<uint8> cells_;
|
std::vector<uint8> cells_;
|
||||||
|
@ -96,15 +95,15 @@ class PrecomputationGrid {
|
||||||
class PrecomputationGridStack;
|
class PrecomputationGridStack;
|
||||||
|
|
||||||
// An implementation of "Real-Time Correlative Scan Matching" by Olson.
|
// An implementation of "Real-Time Correlative Scan Matching" by Olson.
|
||||||
class FastCorrelativeScanMatcher {
|
class FastCorrelativeScanMatcher2D {
|
||||||
public:
|
public:
|
||||||
FastCorrelativeScanMatcher(
|
FastCorrelativeScanMatcher2D(
|
||||||
const mapping::ProbabilityGrid& probability_grid,
|
const ProbabilityGrid& probability_grid,
|
||||||
const proto::FastCorrelativeScanMatcherOptions& options);
|
const proto::FastCorrelativeScanMatcherOptions2D& options);
|
||||||
~FastCorrelativeScanMatcher();
|
~FastCorrelativeScanMatcher2D();
|
||||||
|
|
||||||
FastCorrelativeScanMatcher(const FastCorrelativeScanMatcher&) = delete;
|
FastCorrelativeScanMatcher2D(const FastCorrelativeScanMatcher2D&) = delete;
|
||||||
FastCorrelativeScanMatcher& operator=(const FastCorrelativeScanMatcher&) =
|
FastCorrelativeScanMatcher2D& operator=(const FastCorrelativeScanMatcher2D&) =
|
||||||
delete;
|
delete;
|
||||||
|
|
||||||
// Aligns 'point_cloud' within the 'probability_grid' given an
|
// Aligns 'point_cloud' within the 'probability_grid' given an
|
||||||
|
@ -145,13 +144,13 @@ class FastCorrelativeScanMatcher {
|
||||||
const std::vector<Candidate>& candidates,
|
const std::vector<Candidate>& candidates,
|
||||||
int candidate_depth, float min_score) const;
|
int candidate_depth, float min_score) const;
|
||||||
|
|
||||||
const proto::FastCorrelativeScanMatcherOptions options_;
|
const proto::FastCorrelativeScanMatcherOptions2D options_;
|
||||||
mapping::MapLimits limits_;
|
MapLimits limits_;
|
||||||
std::unique_ptr<PrecomputationGridStack> precomputation_grid_stack_;
|
std::unique_ptr<PrecomputationGridStack> precomputation_grid_stack_;
|
||||||
};
|
};
|
||||||
|
|
||||||
} // namespace scan_matching
|
} // namespace scan_matching
|
||||||
} // namespace mapping_2d
|
} // namespace mapping
|
||||||
} // namespace cartographer
|
} // namespace cartographer
|
||||||
|
|
||||||
#endif // CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_FAST_CORRELATIVE_SCAN_MATCHER_H_
|
#endif // CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_FAST_CORRELATIVE_SCAN_MATCHER_2D_H_
|
|
@ -14,7 +14,7 @@
|
||||||
* limitations under the License.
|
* limitations under the License.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#include "cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h"
|
#include "cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_2d.h"
|
||||||
|
|
||||||
#include <algorithm>
|
#include <algorithm>
|
||||||
#include <cmath>
|
#include <cmath>
|
||||||
|
@ -30,7 +30,7 @@
|
||||||
#include "gtest/gtest.h"
|
#include "gtest/gtest.h"
|
||||||
|
|
||||||
namespace cartographer {
|
namespace cartographer {
|
||||||
namespace mapping_2d {
|
namespace mapping {
|
||||||
namespace scan_matching {
|
namespace scan_matching {
|
||||||
namespace {
|
namespace {
|
||||||
|
|
||||||
|
@ -39,10 +39,10 @@ TEST(PrecomputationGridTest, CorrectValues) {
|
||||||
// represented by uint8 values.
|
// represented by uint8 values.
|
||||||
std::mt19937 prng(42);
|
std::mt19937 prng(42);
|
||||||
std::uniform_int_distribution<int> distribution(0, 255);
|
std::uniform_int_distribution<int> distribution(0, 255);
|
||||||
mapping::ProbabilityGrid probability_grid(mapping::MapLimits(
|
ProbabilityGrid probability_grid(
|
||||||
0.05, Eigen::Vector2d(5., 5.), mapping::CellLimits(250, 250)));
|
MapLimits(0.05, Eigen::Vector2d(5., 5.), CellLimits(250, 250)));
|
||||||
for (const Eigen::Array2i& xy_index : mapping::XYIndexRangeIterator(
|
for (const Eigen::Array2i& xy_index :
|
||||||
Eigen::Array2i(50, 50), Eigen::Array2i(249, 249))) {
|
XYIndexRangeIterator(Eigen::Array2i(50, 50), Eigen::Array2i(249, 249))) {
|
||||||
probability_grid.SetProbability(
|
probability_grid.SetProbability(
|
||||||
xy_index, PrecomputationGrid::ToProbability(distribution(prng)));
|
xy_index, PrecomputationGrid::ToProbability(distribution(prng)));
|
||||||
}
|
}
|
||||||
|
@ -52,8 +52,8 @@ TEST(PrecomputationGridTest, CorrectValues) {
|
||||||
PrecomputationGrid precomputation_grid(
|
PrecomputationGrid precomputation_grid(
|
||||||
probability_grid, probability_grid.limits().cell_limits(), width,
|
probability_grid, probability_grid.limits().cell_limits(), width,
|
||||||
&reusable_intermediate_grid);
|
&reusable_intermediate_grid);
|
||||||
for (const Eigen::Array2i& xy_index : mapping::XYIndexRangeIterator(
|
for (const Eigen::Array2i& xy_index :
|
||||||
probability_grid.limits().cell_limits())) {
|
XYIndexRangeIterator(probability_grid.limits().cell_limits())) {
|
||||||
float max_score = -std::numeric_limits<float>::infinity();
|
float max_score = -std::numeric_limits<float>::infinity();
|
||||||
for (int y = 0; y != width; ++y) {
|
for (int y = 0; y != width; ++y) {
|
||||||
for (int x = 0; x != width; ++x) {
|
for (int x = 0; x != width; ++x) {
|
||||||
|
@ -73,10 +73,10 @@ TEST(PrecomputationGridTest, CorrectValues) {
|
||||||
TEST(PrecomputationGridTest, TinyProbabilityGrid) {
|
TEST(PrecomputationGridTest, TinyProbabilityGrid) {
|
||||||
std::mt19937 prng(42);
|
std::mt19937 prng(42);
|
||||||
std::uniform_int_distribution<int> distribution(0, 255);
|
std::uniform_int_distribution<int> distribution(0, 255);
|
||||||
mapping::ProbabilityGrid probability_grid(mapping::MapLimits(
|
ProbabilityGrid probability_grid(
|
||||||
0.05, Eigen::Vector2d(0.1, 0.1), mapping::CellLimits(4, 4)));
|
MapLimits(0.05, Eigen::Vector2d(0.1, 0.1), CellLimits(4, 4)));
|
||||||
for (const Eigen::Array2i& xy_index :
|
for (const Eigen::Array2i& xy_index :
|
||||||
mapping::XYIndexRangeIterator(probability_grid.limits().cell_limits())) {
|
XYIndexRangeIterator(probability_grid.limits().cell_limits())) {
|
||||||
probability_grid.SetProbability(
|
probability_grid.SetProbability(
|
||||||
xy_index, PrecomputationGrid::ToProbability(distribution(prng)));
|
xy_index, PrecomputationGrid::ToProbability(distribution(prng)));
|
||||||
}
|
}
|
||||||
|
@ -86,8 +86,8 @@ TEST(PrecomputationGridTest, TinyProbabilityGrid) {
|
||||||
PrecomputationGrid precomputation_grid(
|
PrecomputationGrid precomputation_grid(
|
||||||
probability_grid, probability_grid.limits().cell_limits(), width,
|
probability_grid, probability_grid.limits().cell_limits(), width,
|
||||||
&reusable_intermediate_grid);
|
&reusable_intermediate_grid);
|
||||||
for (const Eigen::Array2i& xy_index : mapping::XYIndexRangeIterator(
|
for (const Eigen::Array2i& xy_index :
|
||||||
probability_grid.limits().cell_limits())) {
|
XYIndexRangeIterator(probability_grid.limits().cell_limits())) {
|
||||||
float max_score = -std::numeric_limits<float>::infinity();
|
float max_score = -std::numeric_limits<float>::infinity();
|
||||||
for (int y = 0; y != width; ++y) {
|
for (int y = 0; y != width; ++y) {
|
||||||
for (int x = 0; x != width; ++x) {
|
for (int x = 0; x != width; ++x) {
|
||||||
|
@ -104,8 +104,9 @@ TEST(PrecomputationGridTest, TinyProbabilityGrid) {
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
proto::FastCorrelativeScanMatcherOptions
|
proto::FastCorrelativeScanMatcherOptions2D
|
||||||
CreateFastCorrelativeScanMatcherTestOptions(const int branch_and_bound_depth) {
|
CreateFastCorrelativeScanMatcherTestOptions2D(
|
||||||
|
const int branch_and_bound_depth) {
|
||||||
auto parameter_dictionary =
|
auto parameter_dictionary =
|
||||||
common::MakeDictionary(R"text(
|
common::MakeDictionary(R"text(
|
||||||
return {
|
return {
|
||||||
|
@ -113,7 +114,7 @@ CreateFastCorrelativeScanMatcherTestOptions(const int branch_and_bound_depth) {
|
||||||
angular_search_window = 1.,
|
angular_search_window = 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 CreateFastCorrelativeScanMatcherOptions2D(parameter_dictionary.get());
|
||||||
}
|
}
|
||||||
|
|
||||||
mapping::proto::RangeDataInserterOptions2D
|
mapping::proto::RangeDataInserterOptions2D
|
||||||
|
@ -130,10 +131,10 @@ CreateRangeDataInserterTestOptions2D() {
|
||||||
TEST(FastCorrelativeScanMatcherTest, CorrectPose) {
|
TEST(FastCorrelativeScanMatcherTest, CorrectPose) {
|
||||||
std::mt19937 prng(42);
|
std::mt19937 prng(42);
|
||||||
std::uniform_real_distribution<float> distribution(-1.f, 1.f);
|
std::uniform_real_distribution<float> distribution(-1.f, 1.f);
|
||||||
mapping::RangeDataInserter2D range_data_inserter(
|
RangeDataInserter2D range_data_inserter(
|
||||||
CreateRangeDataInserterTestOptions2D());
|
CreateRangeDataInserterTestOptions2D());
|
||||||
constexpr float kMinScore = 0.1f;
|
constexpr float kMinScore = 0.1f;
|
||||||
const auto options = CreateFastCorrelativeScanMatcherTestOptions(3);
|
const auto options = CreateFastCorrelativeScanMatcherTestOptions2D(3);
|
||||||
|
|
||||||
sensor::PointCloud point_cloud;
|
sensor::PointCloud point_cloud;
|
||||||
point_cloud.emplace_back(-2.5f, 0.5f, 0.f);
|
point_cloud.emplace_back(-2.5f, 0.5f, 0.f);
|
||||||
|
@ -148,8 +149,8 @@ TEST(FastCorrelativeScanMatcherTest, CorrectPose) {
|
||||||
{2. * distribution(prng), 2. * distribution(prng)},
|
{2. * distribution(prng), 2. * distribution(prng)},
|
||||||
0.5 * distribution(prng));
|
0.5 * distribution(prng));
|
||||||
|
|
||||||
mapping::ProbabilityGrid probability_grid(mapping::MapLimits(
|
ProbabilityGrid probability_grid(
|
||||||
0.05, Eigen::Vector2d(5., 5.), mapping::CellLimits(200, 200)));
|
MapLimits(0.05, Eigen::Vector2d(5., 5.), CellLimits(200, 200)));
|
||||||
range_data_inserter.Insert(
|
range_data_inserter.Insert(
|
||||||
sensor::RangeData{
|
sensor::RangeData{
|
||||||
Eigen::Vector3f(expected_pose.translation().x(),
|
Eigen::Vector3f(expected_pose.translation().x(),
|
||||||
|
@ -160,7 +161,7 @@ TEST(FastCorrelativeScanMatcherTest, CorrectPose) {
|
||||||
&probability_grid);
|
&probability_grid);
|
||||||
probability_grid.FinishUpdate();
|
probability_grid.FinishUpdate();
|
||||||
|
|
||||||
FastCorrelativeScanMatcher fast_correlative_scan_matcher(probability_grid,
|
FastCorrelativeScanMatcher2D fast_correlative_scan_matcher(probability_grid,
|
||||||
options);
|
options);
|
||||||
transform::Rigid2d pose_estimate;
|
transform::Rigid2d pose_estimate;
|
||||||
float score;
|
float score;
|
||||||
|
@ -178,10 +179,10 @@ TEST(FastCorrelativeScanMatcherTest, CorrectPose) {
|
||||||
TEST(FastCorrelativeScanMatcherTest, FullSubmapMatching) {
|
TEST(FastCorrelativeScanMatcherTest, FullSubmapMatching) {
|
||||||
std::mt19937 prng(42);
|
std::mt19937 prng(42);
|
||||||
std::uniform_real_distribution<float> distribution(-1.f, 1.f);
|
std::uniform_real_distribution<float> distribution(-1.f, 1.f);
|
||||||
mapping::RangeDataInserter2D range_data_inserter(
|
RangeDataInserter2D range_data_inserter(
|
||||||
CreateRangeDataInserterTestOptions2D());
|
CreateRangeDataInserterTestOptions2D());
|
||||||
constexpr float kMinScore = 0.1f;
|
constexpr float kMinScore = 0.1f;
|
||||||
const auto options = CreateFastCorrelativeScanMatcherTestOptions(6);
|
const auto options = CreateFastCorrelativeScanMatcherTestOptions2D(6);
|
||||||
|
|
||||||
sensor::PointCloud unperturbed_point_cloud;
|
sensor::PointCloud unperturbed_point_cloud;
|
||||||
unperturbed_point_cloud.emplace_back(-2.5f, 0.5f, 0.f);
|
unperturbed_point_cloud.emplace_back(-2.5f, 0.5f, 0.f);
|
||||||
|
@ -202,8 +203,8 @@ TEST(FastCorrelativeScanMatcherTest, FullSubmapMatching) {
|
||||||
0.5 * distribution(prng)) *
|
0.5 * distribution(prng)) *
|
||||||
perturbation.inverse();
|
perturbation.inverse();
|
||||||
|
|
||||||
mapping::ProbabilityGrid probability_grid(mapping::MapLimits(
|
ProbabilityGrid probability_grid(
|
||||||
0.05, Eigen::Vector2d(5., 5.), mapping::CellLimits(200, 200)));
|
MapLimits(0.05, Eigen::Vector2d(5., 5.), CellLimits(200, 200)));
|
||||||
range_data_inserter.Insert(
|
range_data_inserter.Insert(
|
||||||
sensor::RangeData{
|
sensor::RangeData{
|
||||||
transform::Embed3D(expected_pose * perturbation).translation(),
|
transform::Embed3D(expected_pose * perturbation).translation(),
|
||||||
|
@ -213,7 +214,7 @@ TEST(FastCorrelativeScanMatcherTest, FullSubmapMatching) {
|
||||||
&probability_grid);
|
&probability_grid);
|
||||||
probability_grid.FinishUpdate();
|
probability_grid.FinishUpdate();
|
||||||
|
|
||||||
FastCorrelativeScanMatcher fast_correlative_scan_matcher(probability_grid,
|
FastCorrelativeScanMatcher2D fast_correlative_scan_matcher(probability_grid,
|
||||||
options);
|
options);
|
||||||
transform::Rigid2d pose_estimate;
|
transform::Rigid2d pose_estimate;
|
||||||
float score;
|
float score;
|
||||||
|
@ -229,5 +230,5 @@ TEST(FastCorrelativeScanMatcherTest, FullSubmapMatching) {
|
||||||
|
|
||||||
} // namespace
|
} // namespace
|
||||||
} // namespace scan_matching
|
} // namespace scan_matching
|
||||||
} // namespace mapping_2d
|
} // namespace mapping
|
||||||
} // namespace cartographer
|
} // namespace cartographer
|
|
@ -19,16 +19,13 @@
|
||||||
#include "glog/logging.h"
|
#include "glog/logging.h"
|
||||||
|
|
||||||
namespace cartographer {
|
namespace cartographer {
|
||||||
namespace mapping_2d {
|
namespace mapping {
|
||||||
namespace scan_matching {
|
namespace scan_matching {
|
||||||
|
|
||||||
bool PerformGlobalLocalization(
|
bool PerformGlobalLocalization(
|
||||||
const float cutoff,
|
const float cutoff, const sensor::AdaptiveVoxelFilter& voxel_filter,
|
||||||
const cartographer::sensor::AdaptiveVoxelFilter& voxel_filter,
|
const std::vector<FastCorrelativeScanMatcher2D*>& matchers,
|
||||||
const std::vector<
|
const sensor::PointCloud& point_cloud,
|
||||||
cartographer::mapping_2d::scan_matching::FastCorrelativeScanMatcher*>&
|
|
||||||
matchers,
|
|
||||||
const cartographer::sensor::PointCloud& point_cloud,
|
|
||||||
transform::Rigid2d* const best_pose_estimate, float* const best_score) {
|
transform::Rigid2d* const best_pose_estimate, float* const best_score) {
|
||||||
CHECK(best_pose_estimate != nullptr)
|
CHECK(best_pose_estimate != nullptr)
|
||||||
<< "Need a non-null output_pose_estimate!";
|
<< "Need a non-null output_pose_estimate!";
|
||||||
|
@ -57,5 +54,5 @@ bool PerformGlobalLocalization(
|
||||||
}
|
}
|
||||||
|
|
||||||
} // namespace scan_matching
|
} // namespace scan_matching
|
||||||
} // namespace mapping_2d
|
} // namespace mapping
|
||||||
} // namespace cartographer
|
} // namespace cartographer
|
||||||
|
|
|
@ -20,11 +20,11 @@
|
||||||
#include <vector>
|
#include <vector>
|
||||||
|
|
||||||
#include "Eigen/Geometry"
|
#include "Eigen/Geometry"
|
||||||
#include "cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h"
|
#include "cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_2d.h"
|
||||||
#include "cartographer/sensor/voxel_filter.h"
|
#include "cartographer/sensor/voxel_filter.h"
|
||||||
|
|
||||||
namespace cartographer {
|
namespace cartographer {
|
||||||
namespace mapping_2d {
|
namespace mapping {
|
||||||
namespace scan_matching {
|
namespace scan_matching {
|
||||||
|
|
||||||
// Perform global localization against the provided 'matchers'. The 'cutoff'
|
// Perform global localization against the provided 'matchers'. The 'cutoff'
|
||||||
|
@ -37,15 +37,13 @@ namespace scan_matching {
|
||||||
// should not be trusted if the function returns false. The 'cutoff' and
|
// should not be trusted if the function returns false. The 'cutoff' and
|
||||||
// 'best_score' are in the range [0., 1.].
|
// 'best_score' are in the range [0., 1.].
|
||||||
bool PerformGlobalLocalization(
|
bool PerformGlobalLocalization(
|
||||||
float cutoff, const cartographer::sensor::AdaptiveVoxelFilter& voxel_filter,
|
float cutoff, const sensor::AdaptiveVoxelFilter& voxel_filter,
|
||||||
const std::vector<
|
const std::vector<scan_matching::FastCorrelativeScanMatcher2D*>& matchers,
|
||||||
cartographer::mapping_2d::scan_matching::FastCorrelativeScanMatcher*>&
|
const sensor::PointCloud& point_cloud,
|
||||||
matchers,
|
|
||||||
const cartographer::sensor::PointCloud& point_cloud,
|
|
||||||
transform::Rigid2d* best_pose_estimate, float* best_score);
|
transform::Rigid2d* best_pose_estimate, float* best_score);
|
||||||
|
|
||||||
} // namespace scan_matching
|
} // namespace scan_matching
|
||||||
} // namespace mapping_2d
|
} // namespace mapping
|
||||||
} // namespace cartographer
|
} // namespace cartographer
|
||||||
|
|
||||||
#endif // CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_FAST_GLOBAL_LOCALIZER_H_
|
#endif // CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_FAST_GLOBAL_LOCALIZER_H_
|
||||||
|
|
|
@ -14,12 +14,12 @@
|
||||||
|
|
||||||
syntax = "proto3";
|
syntax = "proto3";
|
||||||
|
|
||||||
package cartographer.mapping_2d.scan_matching.proto;
|
package cartographer.mapping.scan_matching.proto;
|
||||||
|
|
||||||
import "cartographer/common/proto/ceres_solver_options.proto";
|
import "cartographer/common/proto/ceres_solver_options.proto";
|
||||||
|
|
||||||
// NEXT ID: 10
|
// NEXT ID: 10
|
||||||
message CeresScanMatcherOptions {
|
message CeresScanMatcherOptions2D {
|
||||||
// Scaling parameters for each cost functor.
|
// Scaling parameters for each cost functor.
|
||||||
double occupied_space_weight = 1;
|
double occupied_space_weight = 1;
|
||||||
double translation_weight = 2;
|
double translation_weight = 2;
|
|
@ -14,9 +14,9 @@
|
||||||
|
|
||||||
syntax = "proto3";
|
syntax = "proto3";
|
||||||
|
|
||||||
package cartographer.mapping_2d.scan_matching.proto;
|
package cartographer.mapping.scan_matching.proto;
|
||||||
|
|
||||||
message FastCorrelativeScanMatcherOptions {
|
message FastCorrelativeScanMatcherOptions2D {
|
||||||
// Minimum linear search window in which the best possible scan alignment
|
// Minimum linear search window in which the best possible scan alignment
|
||||||
// will be found.
|
// will be found.
|
||||||
double linear_search_window = 3;
|
double linear_search_window = 3;
|
|
@ -14,7 +14,7 @@
|
||||||
* limitations under the License.
|
* limitations under the License.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#include "cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.h"
|
#include "cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_2d.h"
|
||||||
|
|
||||||
#include <algorithm>
|
#include <algorithm>
|
||||||
#include <cmath>
|
#include <cmath>
|
||||||
|
@ -30,32 +30,15 @@
|
||||||
#include "glog/logging.h"
|
#include "glog/logging.h"
|
||||||
|
|
||||||
namespace cartographer {
|
namespace cartographer {
|
||||||
namespace mapping_2d {
|
namespace mapping {
|
||||||
namespace scan_matching {
|
namespace scan_matching {
|
||||||
|
|
||||||
proto::RealTimeCorrelativeScanMatcherOptions
|
RealTimeCorrelativeScanMatcher2D::RealTimeCorrelativeScanMatcher2D(
|
||||||
CreateRealTimeCorrelativeScanMatcherOptions(
|
|
||||||
common::LuaParameterDictionary* const parameter_dictionary) {
|
|
||||||
proto::RealTimeCorrelativeScanMatcherOptions options;
|
|
||||||
options.set_linear_search_window(
|
|
||||||
parameter_dictionary->GetDouble("linear_search_window"));
|
|
||||||
options.set_angular_search_window(
|
|
||||||
parameter_dictionary->GetDouble("angular_search_window"));
|
|
||||||
options.set_translation_delta_cost_weight(
|
|
||||||
parameter_dictionary->GetDouble("translation_delta_cost_weight"));
|
|
||||||
options.set_rotation_delta_cost_weight(
|
|
||||||
parameter_dictionary->GetDouble("rotation_delta_cost_weight"));
|
|
||||||
CHECK_GE(options.translation_delta_cost_weight(), 0.);
|
|
||||||
CHECK_GE(options.rotation_delta_cost_weight(), 0.);
|
|
||||||
return options;
|
|
||||||
}
|
|
||||||
|
|
||||||
RealTimeCorrelativeScanMatcher::RealTimeCorrelativeScanMatcher(
|
|
||||||
const proto::RealTimeCorrelativeScanMatcherOptions& options)
|
const proto::RealTimeCorrelativeScanMatcherOptions& options)
|
||||||
: options_(options) {}
|
: options_(options) {}
|
||||||
|
|
||||||
std::vector<Candidate>
|
std::vector<Candidate>
|
||||||
RealTimeCorrelativeScanMatcher::GenerateExhaustiveSearchCandidates(
|
RealTimeCorrelativeScanMatcher2D::GenerateExhaustiveSearchCandidates(
|
||||||
const SearchParameters& search_parameters) const {
|
const SearchParameters& search_parameters) const {
|
||||||
int num_candidates = 0;
|
int num_candidates = 0;
|
||||||
for (int scan_index = 0; scan_index != search_parameters.num_scans;
|
for (int scan_index = 0; scan_index != search_parameters.num_scans;
|
||||||
|
@ -88,10 +71,10 @@ RealTimeCorrelativeScanMatcher::GenerateExhaustiveSearchCandidates(
|
||||||
return candidates;
|
return candidates;
|
||||||
}
|
}
|
||||||
|
|
||||||
double RealTimeCorrelativeScanMatcher::Match(
|
double RealTimeCorrelativeScanMatcher2D::Match(
|
||||||
const transform::Rigid2d& initial_pose_estimate,
|
const transform::Rigid2d& initial_pose_estimate,
|
||||||
const sensor::PointCloud& point_cloud,
|
const sensor::PointCloud& point_cloud,
|
||||||
const mapping::ProbabilityGrid& probability_grid,
|
const ProbabilityGrid& probability_grid,
|
||||||
transform::Rigid2d* pose_estimate) const {
|
transform::Rigid2d* pose_estimate) const {
|
||||||
CHECK_NOTNULL(pose_estimate);
|
CHECK_NOTNULL(pose_estimate);
|
||||||
|
|
||||||
|
@ -124,8 +107,8 @@ double RealTimeCorrelativeScanMatcher::Match(
|
||||||
return best_candidate.score;
|
return best_candidate.score;
|
||||||
}
|
}
|
||||||
|
|
||||||
void RealTimeCorrelativeScanMatcher::ScoreCandidates(
|
void RealTimeCorrelativeScanMatcher2D::ScoreCandidates(
|
||||||
const mapping::ProbabilityGrid& probability_grid,
|
const ProbabilityGrid& probability_grid,
|
||||||
const std::vector<DiscreteScan>& discrete_scans,
|
const std::vector<DiscreteScan>& discrete_scans,
|
||||||
const SearchParameters& search_parameters,
|
const SearchParameters& search_parameters,
|
||||||
std::vector<Candidate>* const candidates) const {
|
std::vector<Candidate>* const candidates) const {
|
||||||
|
@ -152,5 +135,5 @@ void RealTimeCorrelativeScanMatcher::ScoreCandidates(
|
||||||
}
|
}
|
||||||
|
|
||||||
} // namespace scan_matching
|
} // namespace scan_matching
|
||||||
} // namespace mapping_2d
|
} // namespace mapping
|
||||||
} // namespace cartographer
|
} // namespace cartographer
|
|
@ -33,43 +33,40 @@
|
||||||
// This can be made even faster by transforming the scan exactly once over some
|
// This can be made even faster by transforming the scan exactly once over some
|
||||||
// discretized range.
|
// discretized range.
|
||||||
|
|
||||||
#ifndef CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_REAL_TIME_CORRELATIVE_SCAN_MATCHER_H_
|
#ifndef CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_REAL_TIME_CORRELATIVE_SCAN_MATCHER_2D_H_
|
||||||
#define CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_REAL_TIME_CORRELATIVE_SCAN_MATCHER_H_
|
#define CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_REAL_TIME_CORRELATIVE_SCAN_MATCHER_2D_H_
|
||||||
|
|
||||||
#include <iostream>
|
#include <iostream>
|
||||||
#include <memory>
|
#include <memory>
|
||||||
#include <vector>
|
#include <vector>
|
||||||
|
|
||||||
#include "Eigen/Core"
|
#include "Eigen/Core"
|
||||||
|
#include "cartographer/mapping/scan_matching/proto/real_time_correlative_scan_matcher_options.pb.h"
|
||||||
|
#include "cartographer/mapping/scan_matching/real_time_correlative_scan_matcher.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_2d.h"
|
||||||
#include "cartographer/mapping_2d/scan_matching/proto/real_time_correlative_scan_matcher_options.pb.h"
|
|
||||||
|
|
||||||
namespace cartographer {
|
namespace cartographer {
|
||||||
namespace mapping_2d {
|
namespace mapping {
|
||||||
namespace scan_matching {
|
namespace scan_matching {
|
||||||
|
|
||||||
proto::RealTimeCorrelativeScanMatcherOptions
|
|
||||||
CreateRealTimeCorrelativeScanMatcherOptions(
|
|
||||||
common::LuaParameterDictionary* const parameter_dictionary);
|
|
||||||
|
|
||||||
// An implementation of "Real-Time Correlative Scan Matching" by Olson.
|
// An implementation of "Real-Time Correlative Scan Matching" by Olson.
|
||||||
class RealTimeCorrelativeScanMatcher {
|
class RealTimeCorrelativeScanMatcher2D {
|
||||||
public:
|
public:
|
||||||
explicit RealTimeCorrelativeScanMatcher(
|
explicit RealTimeCorrelativeScanMatcher2D(
|
||||||
const proto::RealTimeCorrelativeScanMatcherOptions& options);
|
const proto::RealTimeCorrelativeScanMatcherOptions& options);
|
||||||
|
|
||||||
RealTimeCorrelativeScanMatcher(const RealTimeCorrelativeScanMatcher&) =
|
RealTimeCorrelativeScanMatcher2D(const RealTimeCorrelativeScanMatcher2D&) =
|
||||||
delete;
|
delete;
|
||||||
RealTimeCorrelativeScanMatcher& operator=(
|
RealTimeCorrelativeScanMatcher2D& operator=(
|
||||||
const RealTimeCorrelativeScanMatcher&) = delete;
|
const RealTimeCorrelativeScanMatcher2D&) = delete;
|
||||||
|
|
||||||
// Aligns 'point_cloud' within the 'probability_grid' given an
|
// Aligns 'point_cloud' within the 'probability_grid' given an
|
||||||
// 'initial_pose_estimate' then updates 'pose_estimate' with the result and
|
// 'initial_pose_estimate' then updates 'pose_estimate' with the result and
|
||||||
// returns the score.
|
// returns the score.
|
||||||
double Match(const transform::Rigid2d& initial_pose_estimate,
|
double Match(const transform::Rigid2d& initial_pose_estimate,
|
||||||
const sensor::PointCloud& point_cloud,
|
const sensor::PointCloud& point_cloud,
|
||||||
const mapping::ProbabilityGrid& probability_grid,
|
const ProbabilityGrid& probability_grid,
|
||||||
transform::Rigid2d* pose_estimate) const;
|
transform::Rigid2d* pose_estimate) const;
|
||||||
|
|
||||||
// Computes the score for each Candidate in a collection. The cost is computed
|
// Computes the score for each Candidate in a collection. The cost is computed
|
||||||
|
@ -77,7 +74,7 @@ class RealTimeCorrelativeScanMatcher {
|
||||||
// http://ceres-solver.org/modeling.html
|
// http://ceres-solver.org/modeling.html
|
||||||
//
|
//
|
||||||
// Visible for testing.
|
// Visible for testing.
|
||||||
void ScoreCandidates(const mapping::ProbabilityGrid& probability_grid,
|
void ScoreCandidates(const ProbabilityGrid& probability_grid,
|
||||||
const std::vector<DiscreteScan>& discrete_scans,
|
const std::vector<DiscreteScan>& discrete_scans,
|
||||||
const SearchParameters& search_parameters,
|
const SearchParameters& search_parameters,
|
||||||
std::vector<Candidate>* candidates) const;
|
std::vector<Candidate>* candidates) const;
|
||||||
|
@ -90,7 +87,7 @@ class RealTimeCorrelativeScanMatcher {
|
||||||
};
|
};
|
||||||
|
|
||||||
} // namespace scan_matching
|
} // namespace scan_matching
|
||||||
} // namespace mapping_2d
|
} // namespace mapping
|
||||||
} // namespace cartographer
|
} // namespace cartographer
|
||||||
|
|
||||||
#endif // CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_REAL_TIME_CORRELATIVE_SCAN_MATCHER_H_
|
#endif // CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_REAL_TIME_CORRELATIVE_SCAN_MATCHER_2D_H_
|
|
@ -14,7 +14,7 @@
|
||||||
* limitations under the License.
|
* limitations under the License.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#include "cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.h"
|
#include "cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_2d.h"
|
||||||
|
|
||||||
#include <cmath>
|
#include <cmath>
|
||||||
#include <memory>
|
#include <memory>
|
||||||
|
@ -29,15 +29,15 @@
|
||||||
#include "gtest/gtest.h"
|
#include "gtest/gtest.h"
|
||||||
|
|
||||||
namespace cartographer {
|
namespace cartographer {
|
||||||
namespace mapping_2d {
|
namespace mapping {
|
||||||
namespace scan_matching {
|
namespace scan_matching {
|
||||||
namespace {
|
namespace {
|
||||||
|
|
||||||
class RealTimeCorrelativeScanMatcherTest : public ::testing::Test {
|
class RealTimeCorrelativeScanMatcherTest : public ::testing::Test {
|
||||||
protected:
|
protected:
|
||||||
RealTimeCorrelativeScanMatcherTest()
|
RealTimeCorrelativeScanMatcherTest()
|
||||||
: probability_grid_(mapping::MapLimits(0.05, Eigen::Vector2d(0.05, 0.25),
|
: probability_grid_(
|
||||||
mapping::CellLimits(6, 6))) {
|
MapLimits(0.05, Eigen::Vector2d(0.05, 0.25), CellLimits(6, 6))) {
|
||||||
{
|
{
|
||||||
auto parameter_dictionary = common::MakeDictionary(
|
auto parameter_dictionary = common::MakeDictionary(
|
||||||
"return { "
|
"return { "
|
||||||
|
@ -45,9 +45,8 @@ class RealTimeCorrelativeScanMatcherTest : public ::testing::Test {
|
||||||
"hit_probability = 0.7, "
|
"hit_probability = 0.7, "
|
||||||
"miss_probability = 0.4, "
|
"miss_probability = 0.4, "
|
||||||
"}");
|
"}");
|
||||||
range_data_inserter_ = common::make_unique<mapping::RangeDataInserter2D>(
|
range_data_inserter_ = common::make_unique<RangeDataInserter2D>(
|
||||||
mapping::CreateRangeDataInserterOptions2D(
|
CreateRangeDataInserterOptions2D(parameter_dictionary.get()));
|
||||||
parameter_dictionary.get()));
|
|
||||||
}
|
}
|
||||||
point_cloud_.emplace_back(0.025f, 0.175f, 0.f);
|
point_cloud_.emplace_back(0.025f, 0.175f, 0.f);
|
||||||
point_cloud_.emplace_back(-0.025f, 0.175f, 0.f);
|
point_cloud_.emplace_back(-0.025f, 0.175f, 0.f);
|
||||||
|
@ -69,16 +68,16 @@ class RealTimeCorrelativeScanMatcherTest : public ::testing::Test {
|
||||||
"rotation_delta_cost_weight = 0., "
|
"rotation_delta_cost_weight = 0., "
|
||||||
"}");
|
"}");
|
||||||
real_time_correlative_scan_matcher_ =
|
real_time_correlative_scan_matcher_ =
|
||||||
common::make_unique<RealTimeCorrelativeScanMatcher>(
|
common::make_unique<RealTimeCorrelativeScanMatcher2D>(
|
||||||
CreateRealTimeCorrelativeScanMatcherOptions(
|
CreateRealTimeCorrelativeScanMatcherOptions(
|
||||||
parameter_dictionary.get()));
|
parameter_dictionary.get()));
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
mapping::ProbabilityGrid probability_grid_;
|
ProbabilityGrid probability_grid_;
|
||||||
std::unique_ptr<mapping::RangeDataInserter2D> range_data_inserter_;
|
std::unique_ptr<RangeDataInserter2D> range_data_inserter_;
|
||||||
sensor::PointCloud point_cloud_;
|
sensor::PointCloud point_cloud_;
|
||||||
std::unique_ptr<RealTimeCorrelativeScanMatcher>
|
std::unique_ptr<RealTimeCorrelativeScanMatcher2D>
|
||||||
real_time_correlative_scan_matcher_;
|
real_time_correlative_scan_matcher_;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
@ -121,5 +120,5 @@ TEST_F(RealTimeCorrelativeScanMatcherTest,
|
||||||
|
|
||||||
} // namespace
|
} // namespace
|
||||||
} // namespace scan_matching
|
} // namespace scan_matching
|
||||||
} // namespace mapping_2d
|
} // namespace mapping
|
||||||
} // namespace cartographer
|
} // namespace cartographer
|
|
@ -14,24 +14,24 @@
|
||||||
* limitations under the License.
|
* limitations under the License.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#ifndef CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_ROTATION_DELTA_COST_FUNCTOR_H_
|
#ifndef CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_ROTATION_DELTA_COST_FUNCTOR_2D_H_
|
||||||
#define CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_ROTATION_DELTA_COST_FUNCTOR_H_
|
#define CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_ROTATION_DELTA_COST_FUNCTOR_2D_H_
|
||||||
|
|
||||||
#include "Eigen/Core"
|
#include "Eigen/Core"
|
||||||
|
|
||||||
namespace cartographer {
|
namespace cartographer {
|
||||||
namespace mapping_2d {
|
namespace mapping {
|
||||||
namespace scan_matching {
|
namespace scan_matching {
|
||||||
|
|
||||||
// Computes the cost of rotating 'pose' to 'target_angle'. Cost increases with
|
// Computes the cost of rotating 'pose' to 'target_angle'. Cost increases with
|
||||||
// the solution's distance from 'target_angle'.
|
// the solution's distance from 'target_angle'.
|
||||||
class RotationDeltaCostFunctor {
|
class RotationDeltaCostFunctor2D {
|
||||||
public:
|
public:
|
||||||
static ceres::CostFunction* CreateAutoDiffCostFunction(
|
static ceres::CostFunction* CreateAutoDiffCostFunction(
|
||||||
const double scaling_factor, const double target_angle) {
|
const double scaling_factor, const double target_angle) {
|
||||||
return new ceres::AutoDiffCostFunction<
|
return new ceres::AutoDiffCostFunction<
|
||||||
RotationDeltaCostFunctor, 1 /* residuals */, 3 /* pose variables */>(
|
RotationDeltaCostFunctor2D, 1 /* residuals */, 3 /* pose variables */>(
|
||||||
new RotationDeltaCostFunctor(scaling_factor, target_angle));
|
new RotationDeltaCostFunctor2D(scaling_factor, target_angle));
|
||||||
}
|
}
|
||||||
|
|
||||||
template <typename T>
|
template <typename T>
|
||||||
|
@ -41,19 +41,20 @@ class RotationDeltaCostFunctor {
|
||||||
}
|
}
|
||||||
|
|
||||||
private:
|
private:
|
||||||
explicit RotationDeltaCostFunctor(const double scaling_factor,
|
explicit RotationDeltaCostFunctor2D(const double scaling_factor,
|
||||||
const double target_angle)
|
const double target_angle)
|
||||||
: scaling_factor_(scaling_factor), angle_(target_angle) {}
|
: scaling_factor_(scaling_factor), angle_(target_angle) {}
|
||||||
|
|
||||||
RotationDeltaCostFunctor(const RotationDeltaCostFunctor&) = delete;
|
RotationDeltaCostFunctor2D(const RotationDeltaCostFunctor2D&) = delete;
|
||||||
RotationDeltaCostFunctor& operator=(const RotationDeltaCostFunctor&) = delete;
|
RotationDeltaCostFunctor2D& operator=(const RotationDeltaCostFunctor2D&) =
|
||||||
|
delete;
|
||||||
|
|
||||||
const double scaling_factor_;
|
const double scaling_factor_;
|
||||||
const double angle_;
|
const double angle_;
|
||||||
};
|
};
|
||||||
|
|
||||||
} // namespace scan_matching
|
} // namespace scan_matching
|
||||||
} // namespace mapping_2d
|
} // namespace mapping
|
||||||
} // namespace cartographer
|
} // namespace cartographer
|
||||||
|
|
||||||
#endif // CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_ROTATION_DELTA_COST_FUNCTOR_H_
|
#endif // CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_ROTATION_DELTA_COST_FUNCTOR_2D_H_
|
|
@ -14,24 +14,25 @@
|
||||||
* limitations under the License.
|
* limitations under the License.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#ifndef CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_TRANSLATION_DELTA_COST_FUNCTOR_H_
|
#ifndef CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_TRANSLATION_DELTA_COST_FUNCTOR_2D_H_
|
||||||
#define CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_TRANSLATION_DELTA_COST_FUNCTOR_H_
|
#define CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_TRANSLATION_DELTA_COST_FUNCTOR_2D_H_
|
||||||
|
|
||||||
#include "Eigen/Core"
|
#include "Eigen/Core"
|
||||||
|
|
||||||
namespace cartographer {
|
namespace cartographer {
|
||||||
namespace mapping_2d {
|
namespace mapping {
|
||||||
namespace scan_matching {
|
namespace scan_matching {
|
||||||
|
|
||||||
// Computes the cost of translating 'pose' to 'target_translation'.
|
// Computes the cost of translating 'pose' to 'target_translation'.
|
||||||
// Cost increases with the solution's distance from 'target_translation'.
|
// Cost increases with the solution's distance from 'target_translation'.
|
||||||
class TranslationDeltaCostFunctor {
|
class TranslationDeltaCostFunctor2D {
|
||||||
public:
|
public:
|
||||||
static ceres::CostFunction* CreateAutoDiffCostFunction(
|
static ceres::CostFunction* CreateAutoDiffCostFunction(
|
||||||
const double scaling_factor, const Eigen::Vector2d& target_translation) {
|
const double scaling_factor, const Eigen::Vector2d& target_translation) {
|
||||||
return new ceres::AutoDiffCostFunction<
|
return new ceres::AutoDiffCostFunction<TranslationDeltaCostFunctor2D,
|
||||||
TranslationDeltaCostFunctor, 2 /* residuals */, 3 /* pose variables */>(
|
2 /* residuals */,
|
||||||
new TranslationDeltaCostFunctor(scaling_factor, target_translation));
|
3 /* pose variables */>(
|
||||||
|
new TranslationDeltaCostFunctor2D(scaling_factor, target_translation));
|
||||||
}
|
}
|
||||||
|
|
||||||
template <typename T>
|
template <typename T>
|
||||||
|
@ -42,17 +43,17 @@ class TranslationDeltaCostFunctor {
|
||||||
}
|
}
|
||||||
|
|
||||||
private:
|
private:
|
||||||
// Constructs a new TranslationDeltaCostFunctor from the given
|
// Constructs a new TranslationDeltaCostFunctor2D from the given
|
||||||
// 'target_translation' (x, y).
|
// 'target_translation' (x, y).
|
||||||
explicit TranslationDeltaCostFunctor(
|
explicit TranslationDeltaCostFunctor2D(
|
||||||
const double scaling_factor, const Eigen::Vector2d& target_translation)
|
const double scaling_factor, const Eigen::Vector2d& target_translation)
|
||||||
: scaling_factor_(scaling_factor),
|
: scaling_factor_(scaling_factor),
|
||||||
x_(target_translation.x()),
|
x_(target_translation.x()),
|
||||||
y_(target_translation.y()) {}
|
y_(target_translation.y()) {}
|
||||||
|
|
||||||
TranslationDeltaCostFunctor(const TranslationDeltaCostFunctor&) = delete;
|
TranslationDeltaCostFunctor2D(const TranslationDeltaCostFunctor2D&) = delete;
|
||||||
TranslationDeltaCostFunctor& operator=(const TranslationDeltaCostFunctor&) =
|
TranslationDeltaCostFunctor2D& operator=(
|
||||||
delete;
|
const TranslationDeltaCostFunctor2D&) = delete;
|
||||||
|
|
||||||
const double scaling_factor_;
|
const double scaling_factor_;
|
||||||
const double x_;
|
const double x_;
|
||||||
|
@ -60,7 +61,7 @@ class TranslationDeltaCostFunctor {
|
||||||
};
|
};
|
||||||
|
|
||||||
} // namespace scan_matching
|
} // namespace scan_matching
|
||||||
} // namespace mapping_2d
|
} // namespace mapping
|
||||||
} // namespace cartographer
|
} // namespace cartographer
|
||||||
|
|
||||||
#endif // CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_TRANSLATION_DELTA_COST_FUNCTOR_H_
|
#endif // CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_TRANSLATION_DELTA_COST_FUNCTOR_2D_H_
|
|
@ -17,7 +17,7 @@
|
||||||
#include "cartographer/mapping_3d/local_trajectory_builder_options_3d.h"
|
#include "cartographer/mapping_3d/local_trajectory_builder_options_3d.h"
|
||||||
|
|
||||||
#include "cartographer/internal/mapping/motion_filter.h"
|
#include "cartographer/internal/mapping/motion_filter.h"
|
||||||
#include "cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.h"
|
#include "cartographer/mapping/scan_matching/real_time_correlative_scan_matcher.h"
|
||||||
#include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h"
|
#include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h"
|
||||||
#include "cartographer/mapping_3d/submap_3d.h"
|
#include "cartographer/mapping_3d/submap_3d.h"
|
||||||
#include "cartographer/sensor/voxel_filter.h"
|
#include "cartographer/sensor/voxel_filter.h"
|
||||||
|
@ -48,7 +48,7 @@ proto::LocalTrajectoryBuilderOptions3D CreateLocalTrajectoryBuilderOptions3D(
|
||||||
options.set_use_online_correlative_scan_matching(
|
options.set_use_online_correlative_scan_matching(
|
||||||
parameter_dictionary->GetBool("use_online_correlative_scan_matching"));
|
parameter_dictionary->GetBool("use_online_correlative_scan_matching"));
|
||||||
*options.mutable_real_time_correlative_scan_matcher_options() =
|
*options.mutable_real_time_correlative_scan_matcher_options() =
|
||||||
mapping_2d::scan_matching::CreateRealTimeCorrelativeScanMatcherOptions(
|
mapping::scan_matching::CreateRealTimeCorrelativeScanMatcherOptions(
|
||||||
parameter_dictionary
|
parameter_dictionary
|
||||||
->GetDictionary("real_time_correlative_scan_matcher")
|
->GetDictionary("real_time_correlative_scan_matcher")
|
||||||
.get());
|
.get());
|
||||||
|
|
|
@ -18,7 +18,7 @@ package cartographer.mapping.proto;
|
||||||
|
|
||||||
import "cartographer/mapping/proto/motion_filter_options.proto";
|
import "cartographer/mapping/proto/motion_filter_options.proto";
|
||||||
import "cartographer/sensor/proto/adaptive_voxel_filter_options.proto";
|
import "cartographer/sensor/proto/adaptive_voxel_filter_options.proto";
|
||||||
import "cartographer/mapping_2d/scan_matching/proto/real_time_correlative_scan_matcher_options.proto";
|
import "cartographer/mapping/scan_matching/proto/real_time_correlative_scan_matcher_options.proto";
|
||||||
import "cartographer/mapping_3d/proto/submaps_options_3d.proto";
|
import "cartographer/mapping_3d/proto/submaps_options_3d.proto";
|
||||||
import "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.proto";
|
import "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.proto";
|
||||||
|
|
||||||
|
@ -45,7 +45,7 @@ message LocalTrajectoryBuilderOptions3D {
|
||||||
// Whether to solve the online scan matching first using the correlative scan
|
// Whether to solve the online scan matching first using the correlative scan
|
||||||
// matcher to generate a good starting point for Ceres.
|
// matcher to generate a good starting point for Ceres.
|
||||||
bool use_online_correlative_scan_matching = 13;
|
bool use_online_correlative_scan_matching = 13;
|
||||||
mapping_2d.scan_matching.proto.RealTimeCorrelativeScanMatcherOptions
|
mapping.scan_matching.proto.RealTimeCorrelativeScanMatcherOptions
|
||||||
real_time_correlative_scan_matcher_options = 14;
|
real_time_correlative_scan_matcher_options = 14;
|
||||||
mapping_3d.scan_matching.proto.CeresScanMatcherOptions
|
mapping_3d.scan_matching.proto.CeresScanMatcherOptions
|
||||||
ceres_scan_matcher_options = 6;
|
ceres_scan_matcher_options = 6;
|
||||||
|
|
|
@ -15,7 +15,7 @@
|
||||||
*/
|
*/
|
||||||
|
|
||||||
// This is an implementation of a 3D branch-and-bound algorithm similar to
|
// This is an implementation of a 3D branch-and-bound algorithm similar to
|
||||||
// mapping_2d::FastCorrelativeScanMatcher.
|
// FastCorrelativeScanMatcher2D.
|
||||||
|
|
||||||
#ifndef CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_FAST_CORRELATIVE_SCAN_MATCHER_H_
|
#ifndef CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_FAST_CORRELATIVE_SCAN_MATCHER_H_
|
||||||
#define CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_FAST_CORRELATIVE_SCAN_MATCHER_H_
|
#define CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_FAST_CORRELATIVE_SCAN_MATCHER_H_
|
||||||
|
@ -26,7 +26,7 @@
|
||||||
#include "Eigen/Core"
|
#include "Eigen/Core"
|
||||||
#include "cartographer/common/port.h"
|
#include "cartographer/common/port.h"
|
||||||
#include "cartographer/mapping/trajectory_node.h"
|
#include "cartographer/mapping/trajectory_node.h"
|
||||||
#include "cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h"
|
#include "cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_2d.h"
|
||||||
#include "cartographer/mapping_3d/hybrid_grid.h"
|
#include "cartographer/mapping_3d/hybrid_grid.h"
|
||||||
#include "cartographer/mapping_3d/scan_matching/proto/fast_correlative_scan_matcher_options.pb.h"
|
#include "cartographer/mapping_3d/scan_matching/proto/fast_correlative_scan_matcher_options.pb.h"
|
||||||
#include "cartographer/mapping_3d/scan_matching/rotational_scan_matcher.h"
|
#include "cartographer/mapping_3d/scan_matching/rotational_scan_matcher.h"
|
||||||
|
|
|
@ -28,8 +28,8 @@ namespace mapping_3d {
|
||||||
namespace scan_matching {
|
namespace scan_matching {
|
||||||
|
|
||||||
RealTimeCorrelativeScanMatcher::RealTimeCorrelativeScanMatcher(
|
RealTimeCorrelativeScanMatcher::RealTimeCorrelativeScanMatcher(
|
||||||
const mapping_2d::scan_matching::proto::
|
const mapping::scan_matching::proto::RealTimeCorrelativeScanMatcherOptions&
|
||||||
RealTimeCorrelativeScanMatcherOptions& options)
|
options)
|
||||||
: options_(options) {}
|
: options_(options) {}
|
||||||
|
|
||||||
float RealTimeCorrelativeScanMatcher::Match(
|
float RealTimeCorrelativeScanMatcher::Match(
|
||||||
|
|
|
@ -22,7 +22,7 @@
|
||||||
#include <vector>
|
#include <vector>
|
||||||
|
|
||||||
#include "Eigen/Core"
|
#include "Eigen/Core"
|
||||||
#include "cartographer/mapping_2d/scan_matching/proto/real_time_correlative_scan_matcher_options.pb.h"
|
#include "cartographer/mapping/scan_matching/proto/real_time_correlative_scan_matcher_options.pb.h"
|
||||||
#include "cartographer/mapping_3d/hybrid_grid.h"
|
#include "cartographer/mapping_3d/hybrid_grid.h"
|
||||||
#include "cartographer/sensor/point_cloud.h"
|
#include "cartographer/sensor/point_cloud.h"
|
||||||
|
|
||||||
|
@ -33,7 +33,7 @@ namespace scan_matching {
|
||||||
class RealTimeCorrelativeScanMatcher {
|
class RealTimeCorrelativeScanMatcher {
|
||||||
public:
|
public:
|
||||||
explicit RealTimeCorrelativeScanMatcher(
|
explicit RealTimeCorrelativeScanMatcher(
|
||||||
const mapping_2d::scan_matching::proto::
|
const mapping::scan_matching::proto::
|
||||||
RealTimeCorrelativeScanMatcherOptions& options);
|
RealTimeCorrelativeScanMatcherOptions& options);
|
||||||
|
|
||||||
RealTimeCorrelativeScanMatcher(const RealTimeCorrelativeScanMatcher&) =
|
RealTimeCorrelativeScanMatcher(const RealTimeCorrelativeScanMatcher&) =
|
||||||
|
@ -56,7 +56,7 @@ class RealTimeCorrelativeScanMatcher {
|
||||||
const sensor::PointCloud& transformed_point_cloud,
|
const sensor::PointCloud& transformed_point_cloud,
|
||||||
const transform::Rigid3f& transform) const;
|
const transform::Rigid3f& transform) const;
|
||||||
|
|
||||||
const mapping_2d::scan_matching::proto::RealTimeCorrelativeScanMatcherOptions
|
const mapping::scan_matching::proto::RealTimeCorrelativeScanMatcherOptions
|
||||||
options_;
|
options_;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
|
@ -20,7 +20,7 @@
|
||||||
|
|
||||||
#include "Eigen/Core"
|
#include "Eigen/Core"
|
||||||
#include "cartographer/common/lua_parameter_dictionary_test_helpers.h"
|
#include "cartographer/common/lua_parameter_dictionary_test_helpers.h"
|
||||||
#include "cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.h"
|
#include "cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_2d.h"
|
||||||
#include "cartographer/mapping_3d/hybrid_grid.h"
|
#include "cartographer/mapping_3d/hybrid_grid.h"
|
||||||
#include "cartographer/sensor/point_cloud.h"
|
#include "cartographer/sensor/point_cloud.h"
|
||||||
#include "cartographer/transform/rigid_transform.h"
|
#include "cartographer/transform/rigid_transform.h"
|
||||||
|
@ -58,8 +58,7 @@ class RealTimeCorrelativeScanMatcherTest : public ::testing::Test {
|
||||||
})text");
|
})text");
|
||||||
real_time_correlative_scan_matcher_.reset(
|
real_time_correlative_scan_matcher_.reset(
|
||||||
new RealTimeCorrelativeScanMatcher(
|
new RealTimeCorrelativeScanMatcher(
|
||||||
mapping_2d::scan_matching::
|
mapping::scan_matching::CreateRealTimeCorrelativeScanMatcherOptions(
|
||||||
CreateRealTimeCorrelativeScanMatcherOptions(
|
|
||||||
parameter_dictionary.get())));
|
parameter_dictionary.get())));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -16,14 +16,14 @@
|
||||||
|
|
||||||
#include "cartographer/metrics/register.h"
|
#include "cartographer/metrics/register.h"
|
||||||
|
|
||||||
#include "cartographer/mapping_2d/pose_graph/constraint_builder.h"
|
#include "cartographer/mapping_2d/pose_graph/constraint_builder_2d.h"
|
||||||
#include "cartographer/mapping_3d/pose_graph/constraint_builder.h"
|
#include "cartographer/mapping_3d/pose_graph/constraint_builder.h"
|
||||||
|
|
||||||
namespace cartographer {
|
namespace cartographer {
|
||||||
namespace metrics {
|
namespace metrics {
|
||||||
|
|
||||||
void RegisterAllMetrics(FamilyFactory* registry) {
|
void RegisterAllMetrics(FamilyFactory* registry) {
|
||||||
mapping_2d::pose_graph::ConstraintBuilder::RegisterMetrics(registry);
|
mapping::pose_graph::ConstraintBuilder2D::RegisterMetrics(registry);
|
||||||
mapping_3d::pose_graph::ConstraintBuilder::RegisterMetrics(registry);
|
mapping_3d::pose_graph::ConstraintBuilder::RegisterMetrics(registry);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue