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

Purge 'mapping_3d::'. (#927) (#928) 

[Code structure RFC](https://github.com/googlecartographer/rfcs/blob/master/text/0016-code-structure.md)
master
Alexander Belyaev 2018-02-26 14:26:41 +01:00 committed by GitHub
parent 258aa715ba
commit e75e023ce2
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
61 changed files with 791 additions and 822 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
cartographer/internal/mapping/global_trajectory_builder.cc
View File

@ -139,13 +139,12 @@ std::unique_ptr<TrajectoryBuilderInterface> CreateGlobalTrajectoryBuilder2D(
}
std::unique_ptr<TrajectoryBuilderInterface> CreateGlobalTrajectoryBuilder3D(
std::unique_ptr<mapping_3d::LocalTrajectoryBuilder>
local_trajectory_builder,
std::unique_ptr<LocalTrajectoryBuilder3D> local_trajectory_builder,
const int trajectory_id, mapping::PoseGraph3D* const pose_graph,
const TrajectoryBuilderInterface::LocalSlamResultCallback&
local_slam_result_callback) {
return common::make_unique<GlobalTrajectoryBuilder<
mapping_3d::LocalTrajectoryBuilder, mapping::PoseGraph3D>>(
return common::make_unique<
GlobalTrajectoryBuilder<LocalTrajectoryBuilder3D, mapping::PoseGraph3D>>(
std::move(local_trajectory_builder), trajectory_id, pose_graph,
local_slam_result_callback);
}

5
cartographer/internal/mapping/global_trajectory_builder.h
View File

@ -20,7 +20,7 @@
#include <memory>
#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_3d.h"
#include "cartographer/mapping/local_slam_result_data.h"
#include "cartographer/mapping/trajectory_builder_interface.h"
#include "cartographer/mapping_2d/pose_graph_2d.h"
@ -36,8 +36,7 @@ std::unique_ptr<TrajectoryBuilderInterface> CreateGlobalTrajectoryBuilder2D(
local_slam_result_callback);
std::unique_ptr<TrajectoryBuilderInterface> CreateGlobalTrajectoryBuilder3D(
std::unique_ptr<mapping_3d::LocalTrajectoryBuilder>
local_trajectory_builder,
std::unique_ptr<LocalTrajectoryBuilder3D> local_trajectory_builder,
const int trajectory_id, mapping::PoseGraph3D* const pose_graph,
const TrajectoryBuilderInterface::LocalSlamResultCallback&
local_slam_result_callback);

28
cartographer/internal/mapping_3d/acceleration_cost_function.h → cartographer/internal/mapping_3d/acceleration_cost_function_3d.h
View File

@ -14,17 +14,17 @@
* limitations under the License.
*/
#ifndef CARTOGRAPHER_INTERNAL_MAPPING_3D_ACCELERATION_COST_FUNCTION_H_
#define CARTOGRAPHER_INTERNAL_MAPPING_3D_ACCELERATION_COST_FUNCTION_H_
#ifndef CARTOGRAPHER_INTERNAL_MAPPING_3D_ACCELERATION_COST_FUNCTION_3D_H_
#define CARTOGRAPHER_INTERNAL_MAPPING_3D_ACCELERATION_COST_FUNCTION_3D_H_
#include "Eigen/Core"
#include "Eigen/Geometry"
namespace cartographer {
namespace mapping_3d {
namespace mapping {
// Penalizes differences between IMU data and optimized accelerations.
class AccelerationCostFunction {
class AccelerationCostFunction3D {
public:
static ceres::CostFunction* CreateAutoDiffCostFunction(
const double scaling_factor,
@ -32,11 +32,12 @@ class AccelerationCostFunction {
const double first_delta_time_seconds,
const double second_delta_time_seconds) {
return new ceres::AutoDiffCostFunction<
AccelerationCostFunction, 3 /* residuals */, 4 /* rotation variables */,
AccelerationCostFunction3D, 3 /* residuals */,
4 /* rotation variables */, 3 /* position variables */,
3 /* position variables */, 3 /* position variables */,
3 /* position variables */, 1 /* gravity variables */,
4 /* rotation variables */>(new AccelerationCostFunction(
scaling_factor, delta_velocity_imu_frame, first_delta_time_seconds,
1 /* gravity variables */, 4 /* rotation variables */>(
new AccelerationCostFunction3D(scaling_factor, delta_velocity_imu_frame,
first_delta_time_seconds,
second_delta_time_seconds));
}
@ -71,7 +72,7 @@ class AccelerationCostFunction {
}
private:
AccelerationCostFunction(const double scaling_factor,
AccelerationCostFunction3D(const double scaling_factor,
const Eigen::Vector3d& delta_velocity_imu_frame,
const double first_delta_time_seconds,
const double second_delta_time_seconds)
@ -80,8 +81,9 @@ class AccelerationCostFunction {
first_delta_time_seconds_(first_delta_time_seconds),
second_delta_time_seconds_(second_delta_time_seconds) {}
AccelerationCostFunction(const AccelerationCostFunction&) = delete;
AccelerationCostFunction& operator=(const AccelerationCostFunction&) = delete;
AccelerationCostFunction3D(const AccelerationCostFunction3D&) = delete;
AccelerationCostFunction3D& operator=(const AccelerationCostFunction3D&) =
delete;
template <typename T>
static Eigen::Quaternion<T> ToEigen(const T* const quaternion) {
@ -95,7 +97,7 @@ class AccelerationCostFunction {
const double second_delta_time_seconds_;
};
} // namespace mapping_3d
} // namespace mapping
} // namespace cartographer
#endif // CARTOGRAPHER_INTERNAL_MAPPING_3D_ACCELERATION_COST_FUNCTION_H_
#endif // CARTOGRAPHER_INTERNAL_MAPPING_3D_ACCELERATION_COST_FUNCTION_3D_H_

35
cartographer/internal/mapping_3d/local_trajectory_builder.cc → cartographer/internal/mapping_3d/local_trajectory_builder_3d.cc
View File

@ -14,7 +14,7 @@
* limitations under the License.
*/
#include "cartographer/internal/mapping_3d/local_trajectory_builder.h"
#include "cartographer/internal/mapping_3d/local_trajectory_builder_3d.h"
#include <memory>
@ -23,28 +23,29 @@
#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/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_3d.pb.h"
#include "cartographer/mapping_3d/scan_matching/rotational_scan_matcher.h"
#include "glog/logging.h"
namespace cartographer {
namespace mapping_3d {
namespace mapping {
LocalTrajectoryBuilder::LocalTrajectoryBuilder(
LocalTrajectoryBuilder3D::LocalTrajectoryBuilder3D(
const mapping::proto::LocalTrajectoryBuilderOptions3D& options)
: options_(options),
active_submaps_(options.submaps_options()),
motion_filter_(options.motion_filter_options()),
real_time_correlative_scan_matcher_(
common::make_unique<scan_matching::RealTimeCorrelativeScanMatcher>(
common::make_unique<scan_matching::RealTimeCorrelativeScanMatcher3D>(
options_.real_time_correlative_scan_matcher_options())),
ceres_scan_matcher_(common::make_unique<scan_matching::CeresScanMatcher>(
ceres_scan_matcher_(
common::make_unique<scan_matching::CeresScanMatcher3D>(
options_.ceres_scan_matcher_options())),
accumulated_range_data_{Eigen::Vector3f::Zero(), {}, {}} {}
LocalTrajectoryBuilder::~LocalTrajectoryBuilder() {}
LocalTrajectoryBuilder3D::~LocalTrajectoryBuilder3D() {}
void LocalTrajectoryBuilder::AddImuData(const sensor::ImuData& imu_data) {
void LocalTrajectoryBuilder3D::AddImuData(const sensor::ImuData& imu_data) {
if (extrapolator_ != nullptr) {
extrapolator_->AddImuData(imu_data);
return;
@ -58,9 +59,9 @@ void LocalTrajectoryBuilder::AddImuData(const sensor::ImuData& imu_data) {
options_.imu_gravity_time_constant(), imu_data);
}
std::unique_ptr<LocalTrajectoryBuilder::MatchingResult>
LocalTrajectoryBuilder::AddRangeData(const common::Time time,
const sensor::TimedRangeData& range_data) {
std::unique_ptr<LocalTrajectoryBuilder3D::MatchingResult>
LocalTrajectoryBuilder3D::AddRangeData(
const common::Time time, const sensor::TimedRangeData& range_data) {
if (extrapolator_ == nullptr) {
// Until we've initialized the extrapolator with our first IMU message, we
// cannot compute the orientation of the rangefinder.
@ -130,8 +131,8 @@ LocalTrajectoryBuilder::AddRangeData(const common::Time time,
return nullptr;
}
std::unique_ptr<LocalTrajectoryBuilder::MatchingResult>
LocalTrajectoryBuilder::AddAccumulatedRangeData(
std::unique_ptr<LocalTrajectoryBuilder3D::MatchingResult>
LocalTrajectoryBuilder3D::AddAccumulatedRangeData(
const common::Time time,
const sensor::RangeData& filtered_range_data_in_tracking) {
if (filtered_range_data_in_tracking.returns.empty()) {
@ -199,7 +200,7 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData(
std::move(insertion_result)});
}
void LocalTrajectoryBuilder::AddOdometryData(
void LocalTrajectoryBuilder3D::AddOdometryData(
const sensor::OdometryData& odometry_data) {
if (extrapolator_ == nullptr) {
// Until we've initialized the extrapolator we cannot add odometry data.
@ -209,8 +210,8 @@ void LocalTrajectoryBuilder::AddOdometryData(
extrapolator_->AddOdometryData(odometry_data);
}
std::unique_ptr<LocalTrajectoryBuilder::InsertionResult>
LocalTrajectoryBuilder::InsertIntoSubmap(
std::unique_ptr<LocalTrajectoryBuilder3D::InsertionResult>
LocalTrajectoryBuilder3D::InsertIntoSubmap(
const common::Time time,
const sensor::RangeData& filtered_range_data_in_local,
const sensor::RangeData& filtered_range_data_in_tracking,
@ -249,5 +250,5 @@ LocalTrajectoryBuilder::InsertIntoSubmap(
std::move(insertion_submaps)});
}
} // namespace mapping_3d
} // namespace mapping
} // namespace cartographer

28
cartographer/internal/mapping_3d/local_trajectory_builder.h → cartographer/internal/mapping_3d/local_trajectory_builder_3d.h
View File

@ -14,8 +14,8 @@
* limitations under the License.
*/
#ifndef CARTOGRAPHER_INTERNAL_MAPPING_3D_LOCAL_TRAJECTORY_BUILDER_H_
#define CARTOGRAPHER_INTERNAL_MAPPING_3D_LOCAL_TRAJECTORY_BUILDER_H_
#ifndef CARTOGRAPHER_INTERNAL_MAPPING_3D_LOCAL_TRAJECTORY_BUILDER_3D_H_
#define CARTOGRAPHER_INTERNAL_MAPPING_3D_LOCAL_TRAJECTORY_BUILDER_3D_H_
#include <memory>
@ -23,8 +23,8 @@
#include "cartographer/internal/mapping/motion_filter.h"
#include "cartographer/mapping/pose_extrapolator.h"
#include "cartographer/mapping_3d/proto/local_trajectory_builder_options_3d.pb.h"
#include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h"
#include "cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher.h"
#include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher_3d.h"
#include "cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher_3d.h"
#include "cartographer/mapping_3d/submap_3d.h"
#include "cartographer/sensor/imu_data.h"
#include "cartographer/sensor/odometry_data.h"
@ -33,11 +33,11 @@
#include "cartographer/transform/rigid_transform.h"
namespace cartographer {
namespace mapping_3d {
namespace mapping {
// Wires up the local SLAM stack (i.e. pose extrapolator, scan matching, etc.)
// without loop closure.
class LocalTrajectoryBuilder {
class LocalTrajectoryBuilder3D {
public:
struct InsertionResult {
std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data;
@ -51,12 +51,12 @@ class LocalTrajectoryBuilder {
std::unique_ptr<const InsertionResult> insertion_result;
};
explicit LocalTrajectoryBuilder(
explicit LocalTrajectoryBuilder3D(
const mapping::proto::LocalTrajectoryBuilderOptions3D& options);
~LocalTrajectoryBuilder();
~LocalTrajectoryBuilder3D();
LocalTrajectoryBuilder(const LocalTrajectoryBuilder&) = delete;
LocalTrajectoryBuilder& operator=(const LocalTrajectoryBuilder&) = delete;
LocalTrajectoryBuilder3D(const LocalTrajectoryBuilder3D&) = delete;
LocalTrajectoryBuilder3D& operator=(const LocalTrajectoryBuilder3D&) = delete;
void AddImuData(const sensor::ImuData& imu_data);
// Returns 'MatchingResult' when range data accumulation completed,
@ -84,9 +84,9 @@ class LocalTrajectoryBuilder {
mapping::ActiveSubmaps3D active_submaps_;
mapping::MotionFilter motion_filter_;
std::unique_ptr<scan_matching::RealTimeCorrelativeScanMatcher>
std::unique_ptr<scan_matching::RealTimeCorrelativeScanMatcher3D>
real_time_correlative_scan_matcher_;
std::unique_ptr<scan_matching::CeresScanMatcher> ceres_scan_matcher_;
std::unique_ptr<scan_matching::CeresScanMatcher3D> ceres_scan_matcher_;
std::unique_ptr<mapping::PoseExtrapolator> extrapolator_;
@ -94,7 +94,7 @@ class LocalTrajectoryBuilder {
sensor::RangeData accumulated_range_data_;
};
} // namespace mapping_3d
} // namespace mapping
} // namespace cartographer
#endif // CARTOGRAPHER_INTERNAL_MAPPING_3D_LOCAL_TRAJECTORY_BUILDER_H_
#endif // CARTOGRAPHER_INTERNAL_MAPPING_3D_LOCAL_TRAJECTORY_BUILDER_3D_H_

12
cartographer/internal/mapping_3d/local_trajectory_builder_test.cc → cartographer/internal/mapping_3d/local_trajectory_builder_3d_test.cc
View File

@ -14,7 +14,7 @@
* limitations under the License.
*/
#include "cartographer/internal/mapping_3d/local_trajectory_builder.h"
#include "cartographer/internal/mapping_3d/local_trajectory_builder_3d.h"
#include <memory>
#include <random>
@ -32,7 +32,7 @@
#include "gmock/gmock.h"
namespace cartographer {
namespace mapping_3d {
namespace mapping {
namespace {
class LocalTrajectoryBuilderTest : public ::testing::Test {
@ -253,7 +253,7 @@ class LocalTrajectoryBuilderTest : public ::testing::Test {
for (const TrajectoryNode& node : expected_trajectory) {
AddLinearOnlyImuObservation(node.time, node.pose);
const auto range_data = GenerateRangeData(node.pose);
const std::unique_ptr<LocalTrajectoryBuilder::MatchingResult>
const std::unique_ptr<LocalTrajectoryBuilder3D::MatchingResult>
matching_result = local_trajectory_builder_->AddRangeData(
node.time, sensor::TimedRangeData{
range_data.origin, range_data.returns, {}});
@ -266,16 +266,16 @@ class LocalTrajectoryBuilderTest : public ::testing::Test {
}
}
std::unique_ptr<LocalTrajectoryBuilder> local_trajectory_builder_;
std::unique_ptr<LocalTrajectoryBuilder3D> local_trajectory_builder_;
std::vector<Eigen::Vector3f> bubbles_;
};
TEST_F(LocalTrajectoryBuilderTest, MoveInsideCubeUsingOnlyCeresScanMatcher) {
local_trajectory_builder_.reset(
new LocalTrajectoryBuilder(CreateTrajectoryBuilderOptions3D()));
new LocalTrajectoryBuilder3D(CreateTrajectoryBuilderOptions3D()));
VerifyAccuracy(GenerateCorkscrewTrajectory(), 1e-1);
}
} // namespace
} // namespace mapping_3d
} // namespace mapping
} // namespace cartographer

10
cartographer/internal/mapping_3d/rotation_cost_function.h → cartographer/internal/mapping_3d/rotation_cost_function_3d.h
View File

@ -14,14 +14,14 @@
* limitations under the License.
*/
#ifndef CARTOGRAPHER_INTERNAL_MAPPING_3D_ROTATION_COST_FUNCTION_H_
#define CARTOGRAPHER_INTERNAL_MAPPING_3D_ROTATION_COST_FUNCTION_H_
#ifndef CARTOGRAPHER_INTERNAL_MAPPING_3D_ROTATION_COST_FUNCTION_3D_H_
#define CARTOGRAPHER_INTERNAL_MAPPING_3D_ROTATION_COST_FUNCTION_3D_H_
#include "Eigen/Core"
#include "Eigen/Geometry"
namespace cartographer {
namespace mapping_3d {
namespace mapping {
// Penalizes differences between IMU data and optimized orientations.
class RotationCostFunction {
@ -67,7 +67,7 @@ class RotationCostFunction {
const Eigen::Quaterniond delta_rotation_imu_frame_;
};
} // namespace mapping_3d
} // namespace mapping
} // namespace cartographer
#endif // CARTOGRAPHER_INTERNAL_MAPPING_3D_ROTATION_COST_FUNCTION_H_
#endif // CARTOGRAPHER_INTERNAL_MAPPING_3D_ROTATION_COST_FUNCTION_3D_H_

23
cartographer/internal/mapping_3d/scan_matching/occupied_space_cost_function.h → cartographer/internal/mapping_3d/scan_matching/occupied_space_cost_function_3d.h
View File

@ -14,8 +14,8 @@
* limitations under the License.
*/
#ifndef CARTOGRAPHER_INTERNAL_MAPPING_3D_SCAN_MATCHING_OCCUPIED_SPACE_COST_FUNCTION_H_
#define CARTOGRAPHER_INTERNAL_MAPPING_3D_SCAN_MATCHING_OCCUPIED_SPACE_COST_FUNCTION_H_
#ifndef CARTOGRAPHER_INTERNAL_MAPPING_3D_SCAN_MATCHING_OCCUPIED_SPACE_COST_FUNCTION_3D_H_
#define CARTOGRAPHER_INTERNAL_MAPPING_3D_SCAN_MATCHING_OCCUPIED_SPACE_COST_FUNCTION_3D_H_
#include "Eigen/Core"
#include "cartographer/mapping_3d/hybrid_grid.h"
@ -25,21 +25,22 @@
#include "cartographer/transform/transform.h"
namespace cartographer {
namespace mapping_3d {
namespace mapping {
namespace scan_matching {
// Computes a cost for matching the 'point_cloud' to the 'hybrid_grid' with a
// 'translation' and 'rotation'. The cost increases when points fall into less
// occupied space, i.e. at voxels with lower values.
class OccupiedSpaceCostFunction {
class OccupiedSpaceCostFunction3D {
public:
static ceres::CostFunction* CreateAutoDiffCostFunction(
const double scaling_factor, const sensor::PointCloud& point_cloud,
const mapping::HybridGrid& hybrid_grid) {
return new ceres::AutoDiffCostFunction<
OccupiedSpaceCostFunction, ceres::DYNAMIC /* residuals */,
OccupiedSpaceCostFunction3D, ceres::DYNAMIC /* residuals */,
3 /* translation variables */, 4 /* rotation variables */>(
new OccupiedSpaceCostFunction(scaling_factor, point_cloud, hybrid_grid),
new OccupiedSpaceCostFunction3D(scaling_factor, point_cloud,
hybrid_grid),
point_cloud.size());
}
@ -54,15 +55,15 @@ class OccupiedSpaceCostFunction {
}
private:
OccupiedSpaceCostFunction(const double scaling_factor,
OccupiedSpaceCostFunction3D(const double scaling_factor,
const sensor::PointCloud& point_cloud,
const mapping::HybridGrid& hybrid_grid)
: scaling_factor_(scaling_factor),
point_cloud_(point_cloud),
interpolated_grid_(hybrid_grid) {}
OccupiedSpaceCostFunction(const OccupiedSpaceCostFunction&) = delete;
OccupiedSpaceCostFunction& operator=(const OccupiedSpaceCostFunction&) =
OccupiedSpaceCostFunction3D(const OccupiedSpaceCostFunction3D&) = delete;
OccupiedSpaceCostFunction3D& operator=(const OccupiedSpaceCostFunction3D&) =
delete;
template <typename T>
@ -84,7 +85,7 @@ class OccupiedSpaceCostFunction {
};
} // namespace scan_matching
} // namespace mapping_3d
} // namespace mapping
} // namespace cartographer
#endif // CARTOGRAPHER_INTERNAL_MAPPING_3D_SCAN_MATCHING_OCCUPIED_SPACE_COST_FUNCTION_H_
#endif // CARTOGRAPHER_INTERNAL_MAPPING_3D_SCAN_MATCHING_OCCUPIED_SPACE_COST_FUNCTION_3D_H_

8
cartographer/mapping/map_builder.cc
View File

@ -26,7 +26,7 @@
#include "cartographer/common/time.h"
#include "cartographer/internal/mapping/global_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_3d.h"
#include "cartographer/mapping/collated_trajectory_builder.h"
#include "cartographer/sensor/collator.h"
#include "cartographer/sensor/range_data.h"
@ -80,11 +80,9 @@ int MapBuilder::AddTrajectoryBuilder(
LocalSlamResultCallback local_slam_result_callback) {
const int trajectory_id = trajectory_builders_.size();
if (options_.use_trajectory_builder_3d()) {
std::unique_ptr<mapping_3d::LocalTrajectoryBuilder>
local_trajectory_builder;
std::unique_ptr<LocalTrajectoryBuilder3D> local_trajectory_builder;
if (trajectory_options.has_trajectory_builder_3d_options()) {
local_trajectory_builder =
common::make_unique<mapping_3d::LocalTrajectoryBuilder>(
local_trajectory_builder = common::make_unique<LocalTrajectoryBuilder3D>(
trajectory_options.trajectory_builder_3d_options());
}
trajectory_builders_.push_back(

8
cartographer/mapping/pose_graph/constraint_builder.cc
View File

@ -18,8 +18,8 @@
#include "cartographer/mapping_2d/scan_matching/ceres_scan_matcher_2d.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/fast_correlative_scan_matcher.h"
#include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher_3d.h"
#include "cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_3d.h"
#include "cartographer/sensor/voxel_filter.h"
namespace cartographer {
@ -48,12 +48,12 @@ proto::ConstraintBuilderOptions CreateConstraintBuilderOptions(
scan_matching::CreateCeresScanMatcherOptions2D(
parameter_dictionary->GetDictionary("ceres_scan_matcher").get());
*options.mutable_fast_correlative_scan_matcher_options_3d() =
mapping_3d::scan_matching::CreateFastCorrelativeScanMatcherOptions(
scan_matching::CreateFastCorrelativeScanMatcherOptions3D(
parameter_dictionary
->GetDictionary("fast_correlative_scan_matcher_3d")
.get());
*options.mutable_ceres_scan_matcher_options_3d() =
mapping_3d::scan_matching::CreateCeresScanMatcherOptions(
scan_matching::CreateCeresScanMatcherOptions3D(
parameter_dictionary->GetDictionary("ceres_scan_matcher_3d").get());
return options;
}

8
cartographer/mapping/pose_graph/proto/constraint_builder_options.proto
View File

@ -18,8 +18,8 @@ package cartographer.mapping.pose_graph.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_2d.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/ceres_scan_matcher_options_3d.proto";
import "cartographer/mapping_3d/scan_matching/proto/fast_correlative_scan_matcher_options_3d.proto";
message ConstraintBuilderOptions {
// A constraint will be added if the proportion of added constraints to
@ -52,8 +52,8 @@ message ConstraintBuilderOptions {
fast_correlative_scan_matcher_options = 9;
mapping.scan_matching.proto.CeresScanMatcherOptions2D
ceres_scan_matcher_options = 11;
mapping_3d.scan_matching.proto.FastCorrelativeScanMatcherOptions
mapping.scan_matching.proto.FastCorrelativeScanMatcherOptions3D
fast_correlative_scan_matcher_options_3d = 10;
mapping_3d.scan_matching.proto.CeresScanMatcherOptions
mapping.scan_matching.proto.CeresScanMatcherOptions3D
ceres_scan_matcher_options_3d = 12;
}

2
cartographer/mapping/proto/submap.proto
View File

@ -28,7 +28,7 @@ message Submap2D {
ProbabilityGrid probability_grid = 4;
}
// Serialized state of a mapping_3d::Submap.
// Serialized state of a Submap3D.
message Submap3D {
transform.proto.Rigid3d local_pose = 1;
int32 num_range_data = 2;

4
cartographer/mapping_2d/pose_graph/landmark_cost_function_2d.h
View File

@ -55,10 +55,6 @@ class LandmarkCostFunction2D {
bool operator()(const T* const prev_node_pose, const T* const next_node_pose,
const T* const landmark_rotation,
const T* const landmark_translation, T* const e) const {
using pose_graph::ComputeUnscaledError;
using pose_graph::ScaleError;
using pose_graph::SlerpQuaternions;
const std::array<T, 3> interpolated_pose_translation{
{prev_node_pose[0] +
interpolation_parameter_ * (next_node_pose[0] - prev_node_pose[0]),

2
cartographer/mapping_2d/pose_graph_2d_test.cc
View File

@ -40,7 +40,7 @@ class PoseGraph2DTest : public ::testing::Test {
PoseGraph2DTest() : thread_pool_(1) {
// Builds a wavy, irregularly circular point cloud that is unique
// rotationally. This gives us good rotational texture and avoids any
// possibility of the CeresScanMatcher preferring free space (>
// possibility of the CeresScanMatcher2D preferring free space (>
// kMinProbability) to unknown space (== kMinProbability).
for (float t = 0.f; t < 2.f * M_PI; t += 0.005f) {
const float r = (std::sin(20.f * t) + 2.f) * std::sin(t + 2.f);

6
cartographer/mapping_2d/scan_matching/correlative_scan_matcher_2d.cc
View File

@ -70,7 +70,7 @@ SearchParameters::SearchParameters(const int num_linear_perturbations,
}
}
void SearchParameters::ShrinkToFit(const std::vector<DiscreteScan>& scans,
void SearchParameters::ShrinkToFit(const std::vector<DiscreteScan2D>& scans,
const CellLimits& cell_limits) {
CHECK_EQ(scans.size(), num_scans);
CHECK_EQ(linear_bounds.size(), num_scans);
@ -108,10 +108,10 @@ std::vector<sensor::PointCloud> GenerateRotatedScans(
return rotated_scans;
}
std::vector<DiscreteScan> DiscretizeScans(
std::vector<DiscreteScan2D> DiscretizeScans(
const MapLimits& map_limits, const std::vector<sensor::PointCloud>& scans,
const Eigen::Translation2f& initial_translation) {
std::vector<DiscreteScan> discrete_scans;
std::vector<DiscreteScan2D> discrete_scans;
discrete_scans.reserve(scans.size());
for (const sensor::PointCloud& scan : scans) {
discrete_scans.emplace_back();

16
cartographer/mapping_2d/scan_matching/correlative_scan_matcher_2d.h
View File

@ -29,7 +29,7 @@ namespace cartographer {
namespace mapping {
namespace scan_matching {
typedef std::vector<Eigen::Array2i> DiscreteScan;
typedef std::vector<Eigen::Array2i> DiscreteScan2D;
// Describes the search space.
struct SearchParameters {
@ -49,7 +49,7 @@ struct SearchParameters {
double angular_perturbation_step_size, double resolution);
// Tightens the search window as much as possible.
void ShrinkToFit(const std::vector<DiscreteScan>& scans,
void ShrinkToFit(const std::vector<DiscreteScan2D>& scans,
const CellLimits& cell_limits);
int num_angular_perturbations;
@ -66,13 +66,13 @@ std::vector<sensor::PointCloud> GenerateRotatedScans(
// Translates and discretizes the rotated scans into a vector of integer
// indices.
std::vector<DiscreteScan> DiscretizeScans(
std::vector<DiscreteScan2D> DiscretizeScans(
const MapLimits& map_limits, const std::vector<sensor::PointCloud>& scans,
const Eigen::Translation2f& initial_translation);
// A possible solution.
struct Candidate {
Candidate(const int init_scan_index, const int init_x_index_offset,
struct Candidate2D {
Candidate2D(const int init_scan_index, const int init_x_index_offset,
const int init_y_index_offset,
const SearchParameters& search_parameters)
: scan_index(init_scan_index),
@ -90,7 +90,7 @@ struct Candidate {
int x_index_offset = 0;
int y_index_offset = 0;
// Pose of this Candidate relative to the initial pose.
// Pose of this Candidate2D relative to the initial pose.
double x = 0.;
double y = 0.;
double orientation = 0.;
@ -98,8 +98,8 @@ struct Candidate {
// Score, higher is better.
float score = 0.f;
bool operator<(const Candidate& other) const { return score < other.score; }
bool operator>(const Candidate& other) const { return score > other.score; }
bool operator<(const Candidate2D& other) const { return score < other.score; }
bool operator>(const Candidate2D& other) const { return score > other.score; }
};
} // namespace scan_matching

6
cartographer/mapping_2d/scan_matching/correlative_scan_matcher_test.cc
View File

@ -42,7 +42,7 @@ TEST(SearchParameters, Construction) {
TEST(Candidate, Construction) {
const SearchParameters search_parameters(4, 5, 0.03, 0.05);
const Candidate candidate(3, 4, -5, search_parameters);
const Candidate2D candidate(3, 4, -5, search_parameters);
EXPECT_EQ(3, candidate.scan_index);
EXPECT_EQ(4, candidate.x_index_offset);
EXPECT_EQ(-5, candidate.y_index_offset);
@ -51,7 +51,7 @@ TEST(Candidate, Construction) {
EXPECT_NEAR(-0.06, candidate.orientation, 1e-9);
EXPECT_NEAR(0., candidate.score, 1e-9);
Candidate bigger_candidate(3, 4, 5, search_parameters);
Candidate2D bigger_candidate(3, 4, 5, search_parameters);
bigger_candidate.score = 1.;
EXPECT_LT(candidate, bigger_candidate);
}
@ -83,7 +83,7 @@ TEST(DiscretizeScans, DiscretizeScans) {
CellLimits(6, 6));
const std::vector<sensor::PointCloud> scans =
GenerateRotatedScans(point_cloud, SearchParameters(0, 0, 0., 0.));
const std::vector<DiscreteScan> discrete_scans =
const std::vector<DiscreteScan2D> discrete_scans =
DiscretizeScans(map_limits, scans, Eigen::Translation2f::Identity());
EXPECT_EQ(1, discrete_scans.size());
EXPECT_EQ(7, discrete_scans[0].size());

49
cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_2d.cc
View File

@ -87,7 +87,7 @@ CreateFastCorrelativeScanMatcherOptions2D(
return options;
}
PrecomputationGrid::PrecomputationGrid(
PrecomputationGrid2D::PrecomputationGrid2D(
const ProbabilityGrid& probability_grid, const CellLimits& limits,
const int width, std::vector<float>* reusable_intermediate_grid)
: offset_(-width + 1, -width + 1),
@ -157,7 +157,7 @@ PrecomputationGrid::PrecomputationGrid(
}
}
uint8 PrecomputationGrid::ComputeCellValue(const float probability) const {
uint8 PrecomputationGrid2D::ComputeCellValue(const float probability) const {
const int cell_value =
common::RoundToInt((probability - kMinProbability) *
(255.f / (kMaxProbability - kMinProbability)));
@ -185,14 +185,14 @@ class PrecomputationGridStack {
}
}
const PrecomputationGrid& Get(int index) {
const PrecomputationGrid2D& Get(int index) {
return precomputation_grids_[index];
}
int max_depth() const { return precomputation_grids_.size() - 1; }
private:
std::vector<PrecomputationGrid> precomputation_grids_;
std::vector<PrecomputationGrid2D> precomputation_grids_;
};
FastCorrelativeScanMatcher2D::FastCorrelativeScanMatcher2D(
@ -249,15 +249,15 @@ bool FastCorrelativeScanMatcher2D::MatchWithSearchParameters(
initial_rotation.cast<float>().angle(), Eigen::Vector3f::UnitZ())));
const std::vector<sensor::PointCloud> rotated_scans =
GenerateRotatedScans(rotated_point_cloud, search_parameters);
const std::vector<DiscreteScan> discrete_scans = DiscretizeScans(
const std::vector<DiscreteScan2D> discrete_scans = DiscretizeScans(
limits_, rotated_scans,
Eigen::Translation2f(initial_pose_estimate.translation().x(),
initial_pose_estimate.translation().y()));
search_parameters.ShrinkToFit(discrete_scans, limits_.cell_limits());
const std::vector<Candidate> lowest_resolution_candidates =
const std::vector<Candidate2D> lowest_resolution_candidates =
ComputeLowestResolutionCandidates(discrete_scans, search_parameters);
const Candidate best_candidate = BranchAndBound(
const Candidate2D best_candidate = BranchAndBound(
discrete_scans, search_parameters, lowest_resolution_candidates,
precomputation_grid_stack_->max_depth(), min_score);
if (best_candidate.score > min_score) {
@ -271,11 +271,11 @@ bool FastCorrelativeScanMatcher2D::MatchWithSearchParameters(
return false;
}
std::vector<Candidate>
std::vector<Candidate2D>
FastCorrelativeScanMatcher2D::ComputeLowestResolutionCandidates(
const std::vector<DiscreteScan>& discrete_scans,
const std::vector<DiscreteScan2D>& discrete_scans,
const SearchParameters& search_parameters) const {
std::vector<Candidate> lowest_resolution_candidates =
std::vector<Candidate2D> lowest_resolution_candidates =
GenerateLowestResolutionCandidates(search_parameters);
ScoreCandidates(
precomputation_grid_stack_->Get(precomputation_grid_stack_->max_depth()),
@ -283,7 +283,7 @@ FastCorrelativeScanMatcher2D::ComputeLowestResolutionCandidates(
return lowest_resolution_candidates;
}
std::vector<Candidate>
std::vector<Candidate2D>
FastCorrelativeScanMatcher2D::GenerateLowestResolutionCandidates(
const SearchParameters& search_parameters) const {
const int linear_step_size = 1 << precomputation_grid_stack_->max_depth();
@ -301,7 +301,7 @@ FastCorrelativeScanMatcher2D::GenerateLowestResolutionCandidates(
num_candidates += num_lowest_resolution_linear_x_candidates *
num_lowest_resolution_linear_y_candidates;
}
std::vector<Candidate> candidates;
std::vector<Candidate2D> candidates;
candidates.reserve(num_candidates);
for (int scan_index = 0; scan_index != search_parameters.num_scans;
++scan_index) {
@ -322,11 +322,11 @@ FastCorrelativeScanMatcher2D::GenerateLowestResolutionCandidates(
}
void FastCorrelativeScanMatcher2D::ScoreCandidates(
const PrecomputationGrid& precomputation_grid,
const std::vector<DiscreteScan>& discrete_scans,
const PrecomputationGrid2D& precomputation_grid,
const std::vector<DiscreteScan2D>& discrete_scans,
const SearchParameters& search_parameters,
std::vector<Candidate>* const candidates) const {
for (Candidate& candidate : *candidates) {
std::vector<Candidate2D>* const candidates) const {
for (Candidate2D& candidate : *candidates) {
int sum = 0;
for (const Eigen::Array2i& xy_index :
discrete_scans[candidate.scan_index]) {
@ -335,29 +335,30 @@ void FastCorrelativeScanMatcher2D::ScoreCandidates(
xy_index.y() + candidate.y_index_offset);
sum += precomputation_grid.GetValue(proposed_xy_index);
}
candidate.score = PrecomputationGrid::ToProbability(
candidate.score = PrecomputationGrid2D::ToProbability(
sum / static_cast<float>(discrete_scans[candidate.scan_index].size()));
}
std::sort(candidates->begin(), candidates->end(), std::greater<Candidate>());
std::sort(candidates->begin(), candidates->end(),
std::greater<Candidate2D>());
}
Candidate FastCorrelativeScanMatcher2D::BranchAndBound(
const std::vector<DiscreteScan>& discrete_scans,
Candidate2D FastCorrelativeScanMatcher2D::BranchAndBound(
const std::vector<DiscreteScan2D>& discrete_scans,
const SearchParameters& search_parameters,
const std::vector<Candidate>& candidates, const int candidate_depth,
const std::vector<Candidate2D>& candidates, const int candidate_depth,
float min_score) const {
if (candidate_depth == 0) {
// Return the best candidate.
return *candidates.begin();
}
Candidate best_high_resolution_candidate(0, 0, 0, search_parameters);
Candidate2D best_high_resolution_candidate(0, 0, 0, search_parameters);
best_high_resolution_candidate.score = min_score;
for (const Candidate& candidate : candidates) {
for (const Candidate2D& candidate : candidates) {
if (candidate.score <= min_score) {
break;
}
std::vector<Candidate> higher_resolution_candidates;
std::vector<Candidate2D> higher_resolution_candidates;
const int half_width = 1 << (candidate_depth - 1);
for (int x_offset : {0, half_width}) {
if (candidate.x_index_offset + x_offset >

20
cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_2d.h
View File

@ -47,9 +47,9 @@ CreateFastCorrelativeScanMatcherOptions2D(
// A precomputed grid that contains in each cell (x0, y0) the maximum
// probability in the width x width area defined by x0 <= x < x0 + width and
// y0 <= y < y0.
class PrecomputationGrid {
class PrecomputationGrid2D {
public:
PrecomputationGrid(const ProbabilityGrid& probability_grid,
PrecomputationGrid2D(const ProbabilityGrid& probability_grid,
const CellLimits& limits, int width,
std::vector<float>* reusable_intermediate_grid);
@ -130,18 +130,18 @@ class FastCorrelativeScanMatcher2D {
const transform::Rigid2d& initial_pose_estimate,
const sensor::PointCloud& point_cloud, float min_score, float* score,
transform::Rigid2d* pose_estimate) const;
std::vector<Candidate> ComputeLowestResolutionCandidates(
const std::vector<DiscreteScan>& discrete_scans,
std::vector<Candidate2D> ComputeLowestResolutionCandidates(
const std::vector<DiscreteScan2D>& discrete_scans,
const SearchParameters& search_parameters) const;
std::vector<Candidate> GenerateLowestResolutionCandidates(
std::vector<Candidate2D> GenerateLowestResolutionCandidates(
const SearchParameters& search_parameters) const;
void ScoreCandidates(const PrecomputationGrid& precomputation_grid,
const std::vector<DiscreteScan>& discrete_scans,
void ScoreCandidates(const PrecomputationGrid2D& precomputation_grid,
const std::vector<DiscreteScan2D>& discrete_scans,
const SearchParameters& search_parameters,
std::vector<Candidate>* const candidates) const;
Candidate BranchAndBound(const std::vector<DiscreteScan>& discrete_scans,
std::vector<Candidate2D>* const candidates) const;
Candidate2D BranchAndBound(const std::vector<DiscreteScan2D>& discrete_scans,
const SearchParameters& search_parameters,
const std::vector<Candidate>& candidates,
const std::vector<Candidate2D>& candidates,
int candidate_depth, float min_score) const;
const proto::FastCorrelativeScanMatcherOptions2D options_;

12
cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_2d_test.cc
View File

@ -44,12 +44,12 @@ TEST(PrecomputationGridTest, CorrectValues) {
for (const Eigen::Array2i& xy_index :
XYIndexRangeIterator(Eigen::Array2i(50, 50), Eigen::Array2i(249, 249))) {
probability_grid.SetProbability(
xy_index, PrecomputationGrid::ToProbability(distribution(prng)));
xy_index, PrecomputationGrid2D::ToProbability(distribution(prng)));
}
std::vector<float> reusable_intermediate_grid;
for (const int width : {1, 2, 3, 8}) {
PrecomputationGrid precomputation_grid(
PrecomputationGrid2D precomputation_grid(
probability_grid, probability_grid.limits().cell_limits(), width,
&reusable_intermediate_grid);
for (const Eigen::Array2i& xy_index :
@ -63,7 +63,7 @@ TEST(PrecomputationGridTest, CorrectValues) {
}
}
EXPECT_NEAR(max_score,
PrecomputationGrid::ToProbability(
PrecomputationGrid2D::ToProbability(
precomputation_grid.GetValue(xy_index)),
1e-4);
}
@ -78,12 +78,12 @@ TEST(PrecomputationGridTest, TinyProbabilityGrid) {
for (const Eigen::Array2i& xy_index :
XYIndexRangeIterator(probability_grid.limits().cell_limits())) {
probability_grid.SetProbability(
xy_index, PrecomputationGrid::ToProbability(distribution(prng)));
xy_index, PrecomputationGrid2D::ToProbability(distribution(prng)));
}
std::vector<float> reusable_intermediate_grid;
for (const int width : {1, 2, 3, 8, 200}) {
PrecomputationGrid precomputation_grid(
PrecomputationGrid2D precomputation_grid(
probability_grid, probability_grid.limits().cell_limits(), width,
&reusable_intermediate_grid);
for (const Eigen::Array2i& xy_index :
@ -97,7 +97,7 @@ TEST(PrecomputationGridTest, TinyProbabilityGrid) {
}
}
EXPECT_NEAR(max_score,
PrecomputationGrid::ToProbability(
PrecomputationGrid2D::ToProbability(
precomputation_grid.GetValue(xy_index)),
1e-4);
}

16
cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_2d.cc
View File

@ -37,7 +37,7 @@ RealTimeCorrelativeScanMatcher2D::RealTimeCorrelativeScanMatcher2D(
const proto::RealTimeCorrelativeScanMatcherOptions& options)
: options_(options) {}
std::vector<Candidate>
std::vector<Candidate2D>
RealTimeCorrelativeScanMatcher2D::GenerateExhaustiveSearchCandidates(
const SearchParameters& search_parameters) const {
int num_candidates = 0;
@ -51,7 +51,7 @@ RealTimeCorrelativeScanMatcher2D::GenerateExhaustiveSearchCandidates(
search_parameters.linear_bounds[scan_index].min_y + 1);
num_candidates += num_linear_x_candidates * num_linear_y_candidates;
}
std::vector<Candidate> candidates;
std::vector<Candidate2D> candidates;
candidates.reserve(num_candidates);
for (int scan_index = 0; scan_index != search_parameters.num_scans;
++scan_index) {
@ -89,16 +89,16 @@ double RealTimeCorrelativeScanMatcher2D::Match(
const std::vector<sensor::PointCloud> rotated_scans =
GenerateRotatedScans(rotated_point_cloud, search_parameters);
const std::vector<DiscreteScan> discrete_scans = DiscretizeScans(
const std::vector<DiscreteScan2D> discrete_scans = DiscretizeScans(
probability_grid.limits(), rotated_scans,
Eigen::Translation2f(initial_pose_estimate.translation().x(),
initial_pose_estimate.translation().y()));
std::vector<Candidate> candidates =
std::vector<Candidate2D> candidates =
GenerateExhaustiveSearchCandidates(search_parameters);
ScoreCandidates(probability_grid, discrete_scans, search_parameters,
&candidates);
const Candidate& best_candidate =
const Candidate2D& best_candidate =
*std::max_element(candidates.begin(), candidates.end());
*pose_estimate = transform::Rigid2d(
{initial_pose_estimate.translation().x() + best_candidate.x,
@ -109,10 +109,10 @@ double RealTimeCorrelativeScanMatcher2D::Match(
void RealTimeCorrelativeScanMatcher2D::ScoreCandidates(
const ProbabilityGrid& probability_grid,
const std::vector<DiscreteScan>& discrete_scans,
const std::vector<DiscreteScan2D>& discrete_scans,
const SearchParameters& search_parameters,
std::vector<Candidate>* const candidates) const {
for (Candidate& candidate : *candidates) {
std::vector<Candidate2D>* const candidates) const {
for (Candidate2D& candidate : *candidates) {
candidate.score = 0.f;
for (const Eigen::Array2i& xy_index :
discrete_scans[candidate.scan_index]) {

12
cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_2d.h
View File

@ -69,18 +69,18 @@ class RealTimeCorrelativeScanMatcher2D {
const ProbabilityGrid& probability_grid,
transform::Rigid2d* pose_estimate) const;
// Computes the score for each Candidate in a collection. The cost is computed
// as the sum of probabilities, different from the Ceres CostFunctions:
// http://ceres-solver.org/modeling.html
// Computes the score for each Candidate2D in a collection. The cost is
// computed as the sum of probabilities, different from the Ceres
// CostFunctions: http://ceres-solver.org/modeling.html
//
// Visible for testing.
void ScoreCandidates(const ProbabilityGrid& probability_grid,
const std::vector<DiscreteScan>& discrete_scans,
const std::vector<DiscreteScan2D>& discrete_scans,
const SearchParameters& search_parameters,
std::vector<Candidate>* candidates) const;
std::vector<Candidate2D>* candidates) const;
private:
std::vector<Candidate> GenerateExhaustiveSearchCandidates(
std::vector<Candidate2D> GenerateExhaustiveSearchCandidates(
const SearchParameters& search_parameters) const;
const proto::RealTimeCorrelativeScanMatcherOptions options_;

8
cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_2d_test.cc
View File

@ -85,9 +85,9 @@ TEST_F(RealTimeCorrelativeScanMatcherTest,
ScorePerfectHighResolutionCandidate) {
const std::vector<sensor::PointCloud> scans =
GenerateRotatedScans(point_cloud_, SearchParameters(0, 0, 0., 0.));
const std::vector<DiscreteScan> discrete_scans = DiscretizeScans(
const std::vector<DiscreteScan2D> discrete_scans = DiscretizeScans(
probability_grid_.limits(), scans, Eigen::Translation2f::Identity());
std::vector<Candidate> candidates;
std::vector<Candidate2D> candidates;
candidates.emplace_back(0, 0, 0, SearchParameters(0, 0, 0., 0.));
real_time_correlative_scan_matcher_->ScoreCandidates(
probability_grid_, discrete_scans, SearchParameters(0, 0, 0., 0.),
@ -103,9 +103,9 @@ TEST_F(RealTimeCorrelativeScanMatcherTest,
ScorePartiallyCorrectHighResolutionCandidate) {
const std::vector<sensor::PointCloud> scans =
GenerateRotatedScans(point_cloud_, SearchParameters(0, 0, 0., 0.));
const std::vector<DiscreteScan> discrete_scans = DiscretizeScans(
const std::vector<DiscreteScan2D> discrete_scans = DiscretizeScans(
probability_grid_.limits(), scans, Eigen::Translation2f::Identity());
std::vector<Candidate> candidates;
std::vector<Candidate2D> candidates;
candidates.emplace_back(0, 0, 1, SearchParameters(0, 0, 0., 0.));
real_time_correlative_scan_matcher_->ScoreCandidates(
probability_grid_, discrete_scans, SearchParameters(0, 0, 0., 0.),

4
cartographer/mapping_3d/imu_integration.h
View File

@ -28,7 +28,7 @@
#include "glog/logging.h"
namespace cartographer {
namespace mapping_3d {
namespace mapping {
template <typename T>
struct IntegrateImuResult {
@ -92,7 +92,7 @@ IntegrateImuResult<double> IntegrateImu(const RangeType& imu_data,
start_time, end_time, it);
}
} // namespace mapping_3d
} // namespace mapping
} // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_3D_IMU_INTEGRATION_H_

4
cartographer/mapping_3d/local_trajectory_builder_options_3d.cc
View File

@ -18,7 +18,7 @@
#include "cartographer/internal/mapping/motion_filter.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_3d.h"
#include "cartographer/mapping_3d/submap_3d.h"
#include "cartographer/sensor/voxel_filter.h"
#include "glog/logging.h"
@ -53,7 +53,7 @@ proto::LocalTrajectoryBuilderOptions3D CreateLocalTrajectoryBuilderOptions3D(
->GetDictionary("real_time_correlative_scan_matcher")
.get());
*options.mutable_ceres_scan_matcher_options() =
mapping_3d::scan_matching::CreateCeresScanMatcherOptions(
mapping::scan_matching::CreateCeresScanMatcherOptions3D(
parameter_dictionary->GetDictionary("ceres_scan_matcher").get());
*options.mutable_motion_filter_options() = CreateMotionFilterOptions(
parameter_dictionary->GetDictionary("motion_filter").get());

78
cartographer/mapping_3d/pose_graph/constraint_builder.cc → cartographer/mapping_3d/pose_graph/constraint_builder_3d.cc
View File

@ -14,7 +14,7 @@
* limitations under the License.
*/
#include "cartographer/mapping_3d/pose_graph/constraint_builder.h"
#include "cartographer/mapping_3d/pose_graph/constraint_builder_3d.h"
#include <cmath>
#include <functional>
@ -29,8 +29,8 @@
#include "cartographer/common/make_unique.h"
#include "cartographer/common/math.h"
#include "cartographer/common/thread_pool.h"
#include "cartographer/mapping_3d/scan_matching/proto/ceres_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/proto/ceres_scan_matcher_options_3d.pb.h"
#include "cartographer/mapping_3d/scan_matching/proto/fast_correlative_scan_matcher_options_3d.pb.h"
#include "cartographer/metrics/counter.h"
#include "cartographer/metrics/gauge.h"
#include "cartographer/metrics/histogram.h"
@ -38,7 +38,7 @@
#include "glog/logging.h"
namespace cartographer {
namespace mapping_3d {
namespace mapping {
namespace pose_graph {
static auto* kConstraintsSearchedMetric = metrics::Counter::Null();
@ -55,15 +55,15 @@ static auto* kGlobalConstraintRotationalScoresMetric =
static auto* kGlobalConstraintLowResolutionScoresMetric =
metrics::Histogram::Null();
ConstraintBuilder::ConstraintBuilder(
const mapping::pose_graph::proto::ConstraintBuilderOptions& options,
ConstraintBuilder3D::ConstraintBuilder3D(
const proto::ConstraintBuilderOptions& options,
common::ThreadPool* const thread_pool)
: options_(options),
thread_pool_(thread_pool),
sampler_(options.sampling_ratio()),
ceres_scan_matcher_(options.ceres_scan_matcher_options_3d()) {}
ConstraintBuilder::~ConstraintBuilder() {
ConstraintBuilder3D::~ConstraintBuilder3D() {
common::MutexLocker locker(&mutex_);
CHECK_EQ(constraints_.size(), 0) << "WhenDone() was not called";
CHECK_EQ(pending_computations_.size(), 0);
@ -71,11 +71,10 @@ ConstraintBuilder::~ConstraintBuilder() {
CHECK(when_done_ == nullptr);
}
void ConstraintBuilder::MaybeAddConstraint(
const mapping::SubmapId& submap_id, const mapping::Submap3D* const submap,
const mapping::NodeId& node_id,
const mapping::TrajectoryNode::Data* const constant_data,
const std::vector<mapping::TrajectoryNode>& submap_nodes,
void ConstraintBuilder3D::MaybeAddConstraint(
const SubmapId& submap_id, const Submap3D* const submap,
const NodeId& node_id, const TrajectoryNode::Data* const constant_data,
const std::vector<TrajectoryNode>& submap_nodes,
const transform::Rigid3d& global_node_pose,
const transform::Rigid3d& global_submap_pose) {
if ((global_node_pose.translation() - global_submap_pose.translation())
@ -99,11 +98,10 @@ void ConstraintBuilder::MaybeAddConstraint(
}
}
void ConstraintBuilder::MaybeAddGlobalConstraint(
const mapping::SubmapId& submap_id, const mapping::Submap3D* const submap,
const mapping::NodeId& node_id,
const mapping::TrajectoryNode::Data* const constant_data,
const std::vector<mapping::TrajectoryNode>& submap_nodes,
void ConstraintBuilder3D::MaybeAddGlobalConstraint(
const SubmapId& submap_id, const Submap3D* const submap,
const NodeId& node_id, const TrajectoryNode::Data* const constant_data,
const std::vector<TrajectoryNode>& submap_nodes,
const Eigen::Quaterniond& global_node_rotation,
const Eigen::Quaterniond& global_submap_rotation) {
common::MutexLocker locker(&mutex_);
@ -122,13 +120,13 @@ void ConstraintBuilder::MaybeAddGlobalConstraint(
});
}
void ConstraintBuilder::NotifyEndOfNode() {
void ConstraintBuilder3D::NotifyEndOfNode() {
common::MutexLocker locker(&mutex_);
++current_computation_;
}
void ConstraintBuilder::WhenDone(
const std::function<void(const ConstraintBuilder::Result&)>& callback) {
void ConstraintBuilder3D::WhenDone(
const std::function<void(const ConstraintBuilder3D::Result&)>& callback) {
common::MutexLocker locker(&mutex_);
CHECK(when_done_ == nullptr);
when_done_ =
@ -139,11 +137,9 @@ void ConstraintBuilder::WhenDone(
[this, current_computation] { FinishComputation(current_computation); });
}
void ConstraintBuilder::ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
const mapping::SubmapId& submap_id,
const std::vector<mapping::TrajectoryNode>& submap_nodes,
const mapping::Submap3D* const submap,
const std::function<void()>& work_item) {
void ConstraintBuilder3D::ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
const SubmapId& submap_id, const std::vector<TrajectoryNode>& submap_nodes,
const Submap3D* const submap, const std::function<void()>& work_item) {
if (submap_scan_matchers_[submap_id].fast_correlative_scan_matcher !=
nullptr) {
thread_pool_->Schedule(work_item);
@ -157,12 +153,11 @@ void ConstraintBuilder::ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
}
}
void ConstraintBuilder::ConstructSubmapScanMatcher(
const mapping::SubmapId& submap_id,
const std::vector<mapping::TrajectoryNode>& submap_nodes,
const mapping::Submap3D* const submap) {
void ConstraintBuilder3D::ConstructSubmapScanMatcher(
const SubmapId& submap_id, const std::vector<TrajectoryNode>& submap_nodes,
const Submap3D* const submap) {
auto submap_scan_matcher =
common::make_unique<scan_matching::FastCorrelativeScanMatcher>(
common::make_unique<scan_matching::FastCorrelativeScanMatcher3D>(
submap->high_resolution_hybrid_grid(),
&submap->low_resolution_hybrid_grid(), submap_nodes,
options_.fast_correlative_scan_matcher_options_3d());
@ -177,8 +172,8 @@ void ConstraintBuilder::ConstructSubmapScanMatcher(
submap_queued_work_items_.erase(submap_id);
}
const ConstraintBuilder::SubmapScanMatcher*
ConstraintBuilder::GetSubmapScanMatcher(const mapping::SubmapId& submap_id) {
const ConstraintBuilder3D::SubmapScanMatcher*
ConstraintBuilder3D::GetSubmapScanMatcher(const SubmapId& submap_id) {
common::MutexLocker locker(&mutex_);
const SubmapScanMatcher* submap_scan_matcher =
&submap_scan_matchers_[submap_id];
@ -186,10 +181,9 @@ ConstraintBuilder::GetSubmapScanMatcher(const mapping::SubmapId& submap_id) {
return submap_scan_matcher;
}
void ConstraintBuilder::ComputeConstraint(
const mapping::SubmapId& submap_id, const mapping::NodeId& node_id,
bool match_full_submap,
const mapping::TrajectoryNode::Data* const constant_data,
void ConstraintBuilder3D::ComputeConstraint(
const SubmapId& submap_id, const NodeId& node_id, bool match_full_submap,
const TrajectoryNode::Data* const constant_data,
const transform::Rigid3d& global_node_pose,
const transform::Rigid3d& global_submap_pose,
std::unique_ptr<Constraint>* constraint) {
@ -199,7 +193,7 @@ void ConstraintBuilder::ComputeConstraint(
// The 'constraint_transform' (submap i <- node j) is computed from:
// - a 'high_resolution_point_cloud' in node j and
// - the initial guess 'initial_pose' (submap i <- node j).
std::unique_ptr<scan_matching::FastCorrelativeScanMatcher::Result>
std::unique_ptr<scan_matching::FastCorrelativeScanMatcher3D::Result>
match_result;
// Compute 'pose_estimate' in three stages:
@ -293,7 +287,7 @@ void ConstraintBuilder::ComputeConstraint(
}
}
void ConstraintBuilder::FinishComputation(const int computation_index) {
void ConstraintBuilder3D::FinishComputation(const int computation_index) {
Result result;
std::unique_ptr<std::function<void(const Result&)>> callback;
{
@ -330,7 +324,7 @@ void ConstraintBuilder::FinishComputation(const int computation_index) {
}
}
int ConstraintBuilder::GetNumFinishedNodes() {
int ConstraintBuilder3D::GetNumFinishedNodes() {
common::MutexLocker locker(&mutex_);
if (pending_computations_.empty()) {
return current_computation_;
@ -338,13 +332,13 @@ int ConstraintBuilder::GetNumFinishedNodes() {
return pending_computations_.begin()->first;
}
void ConstraintBuilder::DeleteScanMatcher(const mapping::SubmapId& submap_id) {
void ConstraintBuilder3D::DeleteScanMatcher(const SubmapId& submap_id) {
common::MutexLocker locker(&mutex_);
CHECK(pending_computations_.empty());
submap_scan_matchers_.erase(submap_id);
}
void ConstraintBuilder::RegisterMetrics(metrics::FamilyFactory* factory) {
void ConstraintBuilder3D::RegisterMetrics(metrics::FamilyFactory* factory) {
auto* counts = factory->NewCounterFamily(
"/mapping_3d/pose_graph/constraint_builder/constraints",
"Constraints computed");
@ -378,5 +372,5 @@ void ConstraintBuilder::RegisterMetrics(metrics::FamilyFactory* factory) {
}
} // namespace pose_graph
} // namespace mapping_3d
} // namespace mapping
} // namespace cartographer

82
cartographer/mapping_3d/pose_graph/constraint_builder.h → cartographer/mapping_3d/pose_graph/constraint_builder_3d.h
View File

@ -14,8 +14,8 @@
* limitations under the License.
*/
#ifndef CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_CONSTRAINT_BUILDER_H_
#define CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_CONSTRAINT_BUILDER_H_
#ifndef CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_CONSTRAINT_BUILDER_3D_H_
#define CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_CONSTRAINT_BUILDER_3D_H_
#include <array>
#include <deque>
@ -34,8 +34,8 @@
#include "cartographer/mapping/pose_graph/proto/constraint_builder_options.pb.h"
#include "cartographer/mapping/pose_graph_interface.h"
#include "cartographer/mapping/trajectory_node.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/ceres_scan_matcher_3d.h"
#include "cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_3d.h"
#include "cartographer/mapping_3d/submap_3d.h"
#include "cartographer/metrics/family_factory.h"
#include "cartographer/sensor/compressed_point_cloud.h"
@ -43,7 +43,7 @@
#include "cartographer/sensor/voxel_filter.h"
namespace cartographer {
namespace mapping_3d {
namespace mapping {
namespace pose_graph {
// Asynchronously computes constraints.
@ -54,18 +54,17 @@ namespace pose_graph {
// MaybeAdd(Global)Constraint()/WhenDone() cycle can follow.
//
// This class is thread-safe.
class ConstraintBuilder {
class ConstraintBuilder3D {
public:
using Constraint = mapping::PoseGraphInterface::Constraint;
using Result = std::vector<Constraint>;
ConstraintBuilder(
const mapping::pose_graph::proto::ConstraintBuilderOptions& options,
ConstraintBuilder3D(const proto::ConstraintBuilderOptions& options,
common::ThreadPool* thread_pool);
~ConstraintBuilder();
~ConstraintBuilder3D();
ConstraintBuilder(const ConstraintBuilder&) = delete;
ConstraintBuilder& operator=(const ConstraintBuilder&) = delete;
ConstraintBuilder3D(const ConstraintBuilder3D&) = delete;
ConstraintBuilder3D& operator=(const ConstraintBuilder3D&) = delete;
// Schedules exploring a new constraint between 'submap' identified by
// 'submap_id', and the 'compressed_point_cloud' for 'node_id'.
@ -75,11 +74,10 @@ class ConstraintBuilder {
//
// The pointees of 'submap' and 'compressed_point_cloud' must stay valid until
// all computations are finished.
void MaybeAddConstraint(
const mapping::SubmapId& submap_id, const mapping::Submap3D* submap,
const mapping::NodeId& node_id,
const mapping::TrajectoryNode::Data* const constant_data,
const std::vector<mapping::TrajectoryNode>& submap_nodes,
void MaybeAddConstraint(const SubmapId& submap_id, const Submap3D* submap,
const NodeId& node_id,
const TrajectoryNode::Data* const constant_data,
const std::vector<TrajectoryNode>& submap_nodes,
const transform::Rigid3d& global_node_pose,
const transform::Rigid3d& global_submap_pose);
@ -93,10 +91,9 @@ class ConstraintBuilder {
// The pointees of 'submap' and 'compressed_point_cloud' must stay valid until
// all computations are finished.
void MaybeAddGlobalConstraint(
const mapping::SubmapId& submap_id, const mapping::Submap3D* submap,
const mapping::NodeId& node_id,
const mapping::TrajectoryNode::Data* const constant_data,
const std::vector<mapping::TrajectoryNode>& submap_nodes,
const SubmapId& submap_id, const Submap3D* submap, const NodeId& node_id,
const TrajectoryNode::Data* const constant_data,
const std::vector<TrajectoryNode>& submap_nodes,
const Eigen::Quaterniond& global_node_rotation,
const Eigen::Quaterniond& global_submap_rotation);
@ -111,52 +108,51 @@ class ConstraintBuilder {
int GetNumFinishedNodes();
// 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);
private:
struct SubmapScanMatcher {
const mapping::HybridGrid* high_resolution_hybrid_grid;
const mapping::HybridGrid* low_resolution_hybrid_grid;
std::unique_ptr<scan_matching::FastCorrelativeScanMatcher>
const HybridGrid* high_resolution_hybrid_grid;
const HybridGrid* low_resolution_hybrid_grid;
std::unique_ptr<scan_matching::FastCorrelativeScanMatcher3D>
fast_correlative_scan_matcher;
};
// Either schedules the 'work_item', or if needed, schedules the scan matcher
// construction and queues the 'work_item'.
void ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
const mapping::SubmapId& submap_id,
const std::vector<mapping::TrajectoryNode>& submap_nodes,
const mapping::Submap3D* submap, const std::function<void()>& work_item)
REQUIRES(mutex_);
const SubmapId& submap_id,
const std::vector<TrajectoryNode>& submap_nodes, const Submap3D* submap,
const std::function<void()>& work_item) REQUIRES(mutex_);
// Constructs the scan matcher for a 'submap', then schedules its work items.
void ConstructSubmapScanMatcher(
const mapping::SubmapId& submap_id,
const std::vector<mapping::TrajectoryNode>& submap_nodes,
const mapping::Submap3D* submap) EXCLUDES(mutex_);
const SubmapId& submap_id,
const std::vector<TrajectoryNode>& submap_nodes, const Submap3D* submap)
EXCLUDES(mutex_);
// Returns the scan matcher for a submap, which has to exist.
const SubmapScanMatcher* GetSubmapScanMatcher(
const mapping::SubmapId& submap_id) EXCLUDES(mutex_);
const SubmapScanMatcher* GetSubmapScanMatcher(const SubmapId& submap_id)
EXCLUDES(mutex_);
// Runs in a background thread and does computations for an additional
// constraint.
// As output, it may create a new Constraint in 'constraint'.
void ComputeConstraint(
const mapping::SubmapId& submap_id, const mapping::NodeId& node_id,
void ComputeConstraint(const SubmapId& submap_id, const NodeId& node_id,
bool match_full_submap,
const mapping::TrajectoryNode::Data* const constant_data,
const TrajectoryNode::Data* const constant_data,
const transform::Rigid3d& global_node_pose,
const transform::Rigid3d& global_submap_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,
// runs the 'when_done_' callback and resets the state.
void FinishComputation(int computation_index) EXCLUDES(mutex_);
const mapping::pose_graph::proto::ConstraintBuilderOptions options_;
const proto::ConstraintBuilderOptions options_;
common::ThreadPool* thread_pool_;
common::Mutex mutex_;
@ -178,16 +174,16 @@ class ConstraintBuilder {
std::deque<std::unique_ptr<Constraint>> constraints_ GUARDED_BY(mutex_);
// 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_);
// Map by 'submap_id' of scan matchers under construction, and the work
// 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_);
common::FixedRatioSampler sampler_;
scan_matching::CeresScanMatcher ceres_scan_matcher_;
scan_matching::CeresScanMatcher3D ceres_scan_matcher_;
// Histograms of scan matcher scores.
common::Histogram score_histogram_ GUARDED_BY(mutex_);
@ -196,7 +192,7 @@ class ConstraintBuilder {
};
} // namespace pose_graph
} // namespace mapping_3d
} // namespace mapping
} // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_CONSTRAINT_BUILDER_H_
#endif // CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_CONSTRAINT_BUILDER_3D_H_

34
cartographer/mapping_3d/pose_graph/landmark_cost_function.h → cartographer/mapping_3d/pose_graph/landmark_cost_function_3d.h
View File

@ -14,42 +14,43 @@
* limitations under the License.
*/
#ifndef CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_LANDMARK_COST_FUNCTION_H_
#define CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_LANDMARK_COST_FUNCTION_H_
#ifndef CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_LANDMARK_COST_FUNCTION_3D_H_
#define CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_LANDMARK_COST_FUNCTION_3D_H_
#include "Eigen/Core"
#include "Eigen/Geometry"
#include "cartographer/mapping/pose_graph/cost_helpers.h"
#include "cartographer/mapping/pose_graph_interface.h"
#include "cartographer/mapping_3d/pose_graph/optimization_problem.h"
#include "cartographer/mapping_3d/pose_graph/optimization_problem_3d.h"
#include "cartographer/transform/rigid_transform.h"
#include "cartographer/transform/transform.h"
#include "ceres/ceres.h"
#include "ceres/jet.h"
namespace cartographer {
namespace mapping_3d {
namespace mapping {
namespace pose_graph {
// Cost function measuring the weighted error between the observed pose given by
// the landmark measurement and the linearly interpolated pose.
class LandmarkCostFunction {
class LandmarkCostFunction3D {
public:
using LandmarkObservation =
mapping::PoseGraphInterface::LandmarkNode::LandmarkObservation;
PoseGraphInterface::LandmarkNode::LandmarkObservation;
static ceres::CostFunction* CreateAutoDiffCostFunction(
const LandmarkObservation& observation, const NodeData& prev_node,
const NodeData& next_node) {
const LandmarkObservation& observation,
const OptimizationProblem3D::NodeData& prev_node,
const OptimizationProblem3D::NodeData& next_node) {
return new ceres::AutoDiffCostFunction<
LandmarkCostFunction, 6 /* residuals */,
LandmarkCostFunction3D, 6 /* residuals */,
4 /* previous node rotation variables */,
3 /* previous node translation variables */,
4 /* next node rotation variables */,
3 /* next node translation variables */,
4 /* landmark rotation variables */,
3 /* landmark translation variables */>(
new LandmarkCostFunction(observation, prev_node, next_node));
new LandmarkCostFunction3D(observation, prev_node, next_node));
}
template <typename T>
@ -59,10 +60,6 @@ class LandmarkCostFunction {
const T* const next_node_translation,
const T* const landmark_rotation,
const T* const landmark_translation, T* const e) const {
using mapping::pose_graph::ComputeUnscaledError;
using mapping::pose_graph::ScaleError;
using mapping::pose_graph::SlerpQuaternions;
const std::array<T, 3> interpolated_pose_translation{
{prev_node_translation[0] +
interpolation_parameter_ *
@ -87,8 +84,9 @@ class LandmarkCostFunction {
}
private:
LandmarkCostFunction(const LandmarkObservation& observation,
const NodeData& prev_node, const NodeData& next_node)
LandmarkCostFunction3D(const LandmarkObservation& observation,
const OptimizationProblem3D::NodeData& prev_node,
const OptimizationProblem3D::NodeData& next_node)
: landmark_to_tracking_transform_(
observation.landmark_to_tracking_transform),
translation_weight_(observation.translation_weight),
@ -104,7 +102,7 @@ class LandmarkCostFunction {
};
} // namespace pose_graph
} // namespace mapping_3d
} // namespace mapping
} // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_LANDMARK_COST_FUNCTION_H_
#endif // CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_LANDMARK_COST_FUNCTION_3D_H_

16
cartographer/mapping_3d/pose_graph/landmark_cost_function_test.cc → cartographer/mapping_3d/pose_graph/landmark_cost_function_3d_test.cc
View File

@ -14,7 +14,7 @@
* limitations under the License.
*/
#include "cartographer/mapping_3d/pose_graph/landmark_cost_function.h"
#include "cartographer/mapping_3d/pose_graph/landmark_cost_function_3d.h"
#include <memory>
@ -23,7 +23,7 @@
#include "gtest/gtest.h"
namespace cartographer {
namespace mapping_3d {
namespace mapping {
namespace pose_graph {
namespace {
@ -31,16 +31,16 @@ using ::testing::DoubleEq;
using ::testing::ElementsAre;
using LandmarkObservation =
mapping::PoseGraphInterface::LandmarkNode::LandmarkObservation;
PoseGraphInterface::LandmarkNode::LandmarkObservation;
TEST(LandmarkCostFunctionTest, SmokeTest) {
NodeData prev_node;
TEST(LandmarkCostFunction3DTest, SmokeTest) {
OptimizationProblem3D::NodeData prev_node;
prev_node.time = common::FromUniversal(0);
NodeData next_node;
OptimizationProblem3D::NodeData next_node;
next_node.time = common::FromUniversal(10);
std::unique_ptr<ceres::CostFunction> cost_function(
LandmarkCostFunction::CreateAutoDiffCostFunction(
LandmarkCostFunction3D::CreateAutoDiffCostFunction(
LandmarkObservation{
0 /* trajectory ID */,
common::FromUniversal(5) /* time */,
@ -73,5 +73,5 @@ TEST(LandmarkCostFunctionTest, SmokeTest) {
} // namespace
} // namespace pose_graph
} // namespace mapping_3d
} // namespace mapping
} // namespace cartographer

109
cartographer/mapping_3d/pose_graph/optimization_problem.cc → cartographer/mapping_3d/pose_graph/optimization_problem_3d.cc
View File

@ -14,7 +14,7 @@
* limitations under the License.
*/
#include "cartographer/mapping_3d/pose_graph/optimization_problem.h"
#include "cartographer/mapping_3d/pose_graph/optimization_problem_3d.h"
#include <algorithm>
#include <array>
@ -30,12 +30,12 @@
#include "cartographer/common/make_unique.h"
#include "cartographer/common/math.h"
#include "cartographer/common/time.h"
#include "cartographer/internal/mapping_3d/acceleration_cost_function.h"
#include "cartographer/internal/mapping_3d/rotation_cost_function.h"
#include "cartographer/internal/mapping_3d/acceleration_cost_function_3d.h"
#include "cartographer/internal/mapping_3d/rotation_cost_function_3d.h"
#include "cartographer/mapping/pose_graph/ceres_pose.h"
#include "cartographer/mapping_3d/imu_integration.h"
#include "cartographer/mapping_3d/pose_graph/landmark_cost_function.h"
#include "cartographer/mapping_3d/pose_graph/spa_cost_function.h"
#include "cartographer/mapping_3d/pose_graph/landmark_cost_function_3d.h"
#include "cartographer/mapping_3d/pose_graph/spa_cost_function_3d.h"
#include "cartographer/mapping_3d/rotation_parameterization.h"
#include "cartographer/transform/timestamped_transform.h"
#include "cartographer/transform/transform.h"
@ -45,14 +45,15 @@
#include "glog/logging.h"
namespace cartographer {
namespace mapping_3d {
namespace mapping {
namespace pose_graph {
namespace {
using ::cartographer::mapping::pose_graph::CeresPose;
using LandmarkNode = ::cartographer::mapping::PoseGraphInterface::LandmarkNode;
using TrajectoryData =
::cartographer::mapping::PoseGraphInterface::TrajectoryData;
using NodeData = OptimizationProblem3D::NodeData;
using SubmapData = OptimizationProblem3D::SubmapData;
// For odometry.
std::unique_ptr<transform::Rigid3d> Interpolate(
@ -116,8 +117,8 @@ transform::Rigid3d GetInitialLandmarkPose(
void AddLandmarkCostFunctions(
const std::map<std::string, LandmarkNode>& landmark_nodes,
const mapping::MapById<mapping::NodeId, NodeData>& node_data,
mapping::MapById<mapping::NodeId, CeresPose>* C_nodes,
const MapById<NodeId, NodeData>& node_data,
MapById<NodeId, CeresPose>* C_nodes,
std::map<std::string, CeresPose>* C_landmarks, ceres::Problem* problem) {
for (const auto& landmark_node : landmark_nodes) {
for (const auto& observation : landmark_node.second.landmark_observations) {
@ -153,7 +154,7 @@ void AddLandmarkCostFunctions(
problem));
}
problem->AddResidualBlock(
LandmarkCostFunction::CreateAutoDiffCostFunction(
LandmarkCostFunction3D::CreateAutoDiffCostFunction(
observation, prev->data, next->data),
nullptr /* loss function */, C_nodes->at(prev->id).rotation(),
C_nodes->at(prev->id).translation(), C_nodes->at(next->id).rotation(),
@ -166,30 +167,29 @@ void AddLandmarkCostFunctions(
} // namespace
OptimizationProblem::OptimizationProblem(
const mapping::pose_graph::proto::OptimizationProblemOptions& options,
FixZ fix_z)
OptimizationProblem3D::OptimizationProblem3D(
const pose_graph::proto::OptimizationProblemOptions& options, FixZ fix_z)
: options_(options), fix_z_(fix_z) {}
OptimizationProblem::~OptimizationProblem() {}
OptimizationProblem3D::~OptimizationProblem3D() {}
void OptimizationProblem::AddImuData(const int trajectory_id,
void OptimizationProblem3D::AddImuData(const int trajectory_id,
const sensor::ImuData& imu_data) {
imu_data_.Append(trajectory_id, imu_data);
}
void OptimizationProblem::AddOdometryData(
void OptimizationProblem3D::AddOdometryData(
const int trajectory_id, const sensor::OdometryData& odometry_data) {
odometry_data_.Append(trajectory_id, odometry_data);
}
void OptimizationProblem::AddFixedFramePoseData(
void OptimizationProblem3D::AddFixedFramePoseData(
const int trajectory_id,
const sensor::FixedFramePoseData& fixed_frame_pose_data) {
fixed_frame_pose_data_.Append(trajectory_id, fixed_frame_pose_data);
}
void OptimizationProblem::AddTrajectoryNode(
void OptimizationProblem3D::AddTrajectoryNode(
const int trajectory_id, const common::Time time,
const transform::Rigid3d& local_pose,
const transform::Rigid3d& global_pose) {
@ -197,20 +197,20 @@ void OptimizationProblem::AddTrajectoryNode(
trajectory_data_[trajectory_id];
}
void OptimizationProblem::SetTrajectoryData(
void OptimizationProblem3D::SetTrajectoryData(
int trajectory_id, const TrajectoryData& trajectory_data) {
trajectory_data_[trajectory_id] = trajectory_data;
}
void OptimizationProblem::InsertTrajectoryNode(
const mapping::NodeId& node_id, const common::Time time,
void OptimizationProblem3D::InsertTrajectoryNode(
const NodeId& node_id, const common::Time time,
const transform::Rigid3d& local_pose,
const transform::Rigid3d& global_pose) {
node_data_.Insert(node_id, NodeData{time, local_pose, global_pose});
trajectory_data_[node_id.trajectory_id];
}
void OptimizationProblem::TrimTrajectoryNode(const mapping::NodeId& node_id) {
void OptimizationProblem3D::TrimTrajectoryNode(const NodeId& node_id) {
imu_data_.Trim(node_data_, node_id);
odometry_data_.Trim(node_data_, node_id);
fixed_frame_pose_data_.Trim(node_data_, node_id);
@ -220,27 +220,27 @@ void OptimizationProblem::TrimTrajectoryNode(const mapping::NodeId& node_id) {
}
}
void OptimizationProblem::AddSubmap(
void OptimizationProblem3D::AddSubmap(
const int trajectory_id, const transform::Rigid3d& global_submap_pose) {
submap_data_.Append(trajectory_id, SubmapData{global_submap_pose});
}
void OptimizationProblem::InsertSubmap(
const mapping::SubmapId& submap_id,
const transform::Rigid3d& global_submap_pose) {
void OptimizationProblem3D::InsertSubmap(
const SubmapId& submap_id, const transform::Rigid3d& global_submap_pose) {
submap_data_.Insert(submap_id, SubmapData{global_submap_pose});
}
void OptimizationProblem::TrimSubmap(const mapping::SubmapId& submap_id) {
void OptimizationProblem3D::TrimSubmap(const SubmapId& submap_id) {
submap_data_.Trim(submap_id);
}
void OptimizationProblem::SetMaxNumIterations(const int32 max_num_iterations) {
void OptimizationProblem3D::SetMaxNumIterations(
const int32 max_num_iterations) {
options_.mutable_ceres_solver_options()->set_max_num_iterations(
max_num_iterations);
}
void OptimizationProblem::Solve(
void OptimizationProblem3D::Solve(
const std::vector<Constraint>& constraints,
const std::set<int>& frozen_trajectories,
const std::map<std::string, LandmarkNode>& landmark_nodes) {
@ -262,9 +262,9 @@ void OptimizationProblem::Solve(
// Set the starting point.
CHECK(!submap_data_.empty());
CHECK(submap_data_.Contains(mapping::SubmapId{0, 0}));
mapping::MapById<mapping::SubmapId, CeresPose> C_submaps;
mapping::MapById<mapping::NodeId, CeresPose> C_nodes;
CHECK(submap_data_.Contains(SubmapId{0, 0}));
MapById<SubmapId, CeresPose> C_submaps;
MapById<NodeId, CeresPose> C_nodes;
std::map<std::string, CeresPose> C_landmarks;
bool first_submap = true;
for (const auto& submap_id_data : submap_data_) {
@ -279,7 +279,7 @@ void OptimizationProblem::Solve(
CeresPose(submap_id_data.data.global_pose,
translation_parameterization(),
common::make_unique<ceres::AutoDiffLocalParameterization<
mapping::ConstantYawQuaternionPlus, 4, 2>>(),
ConstantYawQuaternionPlus, 4, 2>>(),
&problem));
problem.SetParameterBlockConstant(
C_submaps.at(submap_id_data.id).translation());
@ -315,7 +315,7 @@ void OptimizationProblem::Solve(
// Add cost functions for intra- and inter-submap constraints.
for (const Constraint& constraint : constraints) {
problem.AddResidualBlock(
SpaCostFunction::CreateAutoDiffCostFunction(constraint.pose),
SpaCostFunction3D::CreateAutoDiffCostFunction(constraint.pose),
// Only loop closure constraints should have a loss function.
constraint.tag == Constraint::INTER_SUBMAP
? new ceres::HuberLoss(options_.huber_scale())
@ -350,10 +350,10 @@ void OptimizationProblem::Solve(
auto imu_it = imu_data.begin();
auto prev_node_it = 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;
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;
if (second_node_id.node_index != first_node_id.node_index + 1) {
@ -372,7 +372,7 @@ void OptimizationProblem::Solve(
const auto next_node_it = std::next(node_it);
if (next_node_it != trajectory_end &&
next_node_it->id.node_index == second_node_id.node_index + 1) {
const mapping::NodeId third_node_id = next_node_it->id;
const NodeId third_node_id = next_node_it->id;
const NodeData& third_node_data = next_node_it->data;
const common::Time first_time = first_node_data.time;
const common::Time second_time = second_node_data.time;
@ -395,7 +395,7 @@ void OptimizationProblem::Solve(
result_to_first_center.delta_rotation) *
result_center_to_center.delta_velocity;
problem.AddResidualBlock(
AccelerationCostFunction::CreateAutoDiffCostFunction(
AccelerationCostFunction3D::CreateAutoDiffCostFunction(
options_.acceleration_weight(), delta_velocity,
common::ToSeconds(first_duration),
common::ToSeconds(second_duration)),
@ -430,10 +430,10 @@ void OptimizationProblem::Solve(
auto prev_node_it = 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;
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;
if (second_node_id.node_index != first_node_id.node_index + 1) {
@ -443,7 +443,7 @@ void OptimizationProblem::Solve(
const transform::Rigid3d relative_pose = ComputeRelativePose(
trajectory_id, first_node_data, second_node_data);
problem.AddResidualBlock(
SpaCostFunction::CreateAutoDiffCostFunction(Constraint::Pose{
SpaCostFunction3D::CreateAutoDiffCostFunction(Constraint::Pose{
relative_pose, options_.consecutive_node_translation_weight(),
options_.consecutive_node_rotation_weight()}),
nullptr /* loss function */, C_nodes.at(first_node_id).rotation(),
@ -467,7 +467,7 @@ void OptimizationProblem::Solve(
const TrajectoryData& trajectory_data = trajectory_data_.at(trajectory_id);
bool fixed_frame_pose_initialized = false;
for (; node_it != trajectory_end; ++node_it) {
const mapping::NodeId node_id = node_it->id;
const NodeId node_id = node_it->id;
const NodeData& node_data = node_it->data;
const std::unique_ptr<transform::Rigid3d> fixed_frame_pose =
@ -499,13 +499,13 @@ void OptimizationProblem::Solve(
Eigen::Vector3d::UnitZ())),
nullptr,
common::make_unique<ceres::AutoDiffLocalParameterization<
mapping::YawOnlyQuaternionPlus, 4, 1>>(),
YawOnlyQuaternionPlus, 4, 1>>(),
&problem));
fixed_frame_pose_initialized = true;
}
problem.AddResidualBlock(
SpaCostFunction::CreateAutoDiffCostFunction(constraint_pose),
SpaCostFunction3D::CreateAutoDiffCostFunction(constraint_pose),
nullptr /* loss function */,
C_fixed_frames.at(trajectory_id).rotation(),
C_fixed_frames.at(trajectory_id).translation(),
@ -553,42 +553,41 @@ void OptimizationProblem::Solve(
}
}
const mapping::MapById<mapping::NodeId, NodeData>&
OptimizationProblem::node_data() const {
const MapById<NodeId, NodeData>& OptimizationProblem3D::node_data() const {
return node_data_;
}
const mapping::MapById<mapping::SubmapId, SubmapData>&
OptimizationProblem::submap_data() const {
const MapById<SubmapId, SubmapData>& OptimizationProblem3D::submap_data()
const {
return submap_data_;
}
const std::map<std::string, transform::Rigid3d>&
OptimizationProblem::landmark_data() const {
OptimizationProblem3D::landmark_data() const {
return landmark_data_;
}
const sensor::MapByTime<sensor::ImuData>& OptimizationProblem::imu_data()
const sensor::MapByTime<sensor::ImuData>& OptimizationProblem3D::imu_data()
const {
return imu_data_;
}
const sensor::MapByTime<sensor::OdometryData>&
OptimizationProblem::odometry_data() const {
OptimizationProblem3D::odometry_data() const {
return odometry_data_;
}
const sensor::MapByTime<sensor::FixedFramePoseData>&
OptimizationProblem::fixed_frame_pose_data() const {
OptimizationProblem3D::fixed_frame_pose_data() const {
return fixed_frame_pose_data_;
}
const std::map<int, TrajectoryData>& OptimizationProblem::trajectory_data()
const std::map<int, TrajectoryData>& OptimizationProblem3D::trajectory_data()
const {
return trajectory_data_;
}
transform::Rigid3d OptimizationProblem::ComputeRelativePose(
transform::Rigid3d OptimizationProblem3D::ComputeRelativePose(
const int trajectory_id, const NodeData& first_node_data,
const NodeData& second_node_data) const {
if (odometry_data_.HasTrajectory(trajectory_id)) {
@ -604,5 +603,5 @@ transform::Rigid3d OptimizationProblem::ComputeRelativePose(
}
} // namespace pose_graph
} // namespace mapping_3d
} // namespace mapping
} // namespace cartographer

59
cartographer/mapping_3d/pose_graph/optimization_problem.h → cartographer/mapping_3d/pose_graph/optimization_problem_3d.h
View File

@ -14,8 +14,8 @@
* limitations under the License.
*/
#ifndef CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_OPTIMIZATION_PROBLEM_H_
#define CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_OPTIMIZATION_PROBLEM_H_
#ifndef CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_OPTIMIZATION_PROBLEM_3D_H_
#define CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_OPTIMIZATION_PROBLEM_3D_H_
#include <array>
#include <map>
@ -37,9 +37,15 @@
#include "cartographer/transform/transform_interpolation_buffer.h"
namespace cartographer {
namespace mapping_3d {
namespace mapping {
namespace pose_graph {
// Implements the SPA loop closure method.
class OptimizationProblem3D {
public:
using Constraint = PoseGraphInterface::Constraint;
using LandmarkNode = PoseGraphInterface::LandmarkNode;
struct NodeData {
common::Time time;
transform::Rigid3d local_pose;
@ -50,21 +56,14 @@ struct SubmapData {
transform::Rigid3d global_pose;
};
// Implements the SPA loop closure method.
class OptimizationProblem {
public:
using Constraint = mapping::PoseGraphInterface::Constraint;
using LandmarkNode = mapping::PoseGraphInterface::LandmarkNode;
enum class FixZ { kYes, kNo };
OptimizationProblem(
const mapping::pose_graph::proto::OptimizationProblemOptions& options,
FixZ fix_z);
~OptimizationProblem();
OptimizationProblem3D(
const pose_graph::proto::OptimizationProblemOptions& options, FixZ fix_z);
~OptimizationProblem3D();
OptimizationProblem(const OptimizationProblem&) = delete;
OptimizationProblem& operator=(const OptimizationProblem&) = delete;
OptimizationProblem3D(const OptimizationProblem3D&) = delete;
OptimizationProblem3D& operator=(const OptimizationProblem3D&) = delete;
void AddImuData(int trajectory_id, const sensor::ImuData& imu_data);
void AddOdometryData(int trajectory_id,
@ -77,16 +76,16 @@ class OptimizationProblem {
const transform::Rigid3d& global_pose);
void SetTrajectoryData(
int trajectory_id,
const mapping::PoseGraphInterface::TrajectoryData& trajectory_data);
void InsertTrajectoryNode(const mapping::NodeId& node_id, common::Time time,
const PoseGraphInterface::TrajectoryData& trajectory_data);
void InsertTrajectoryNode(const NodeId& node_id, common::Time time,
const transform::Rigid3d& local_pose,
const transform::Rigid3d& global_pose);
void TrimTrajectoryNode(const mapping::NodeId& node_id);
void TrimTrajectoryNode(const NodeId& node_id);
void AddSubmap(int trajectory_id,
const transform::Rigid3d& global_submap_pose);
void InsertSubmap(const mapping::SubmapId& submap_id,
void InsertSubmap(const SubmapId& submap_id,
const transform::Rigid3d& global_submap_pose);
void TrimSubmap(const mapping::SubmapId& submap_id);
void TrimSubmap(const SubmapId& submap_id);
void SetMaxNumIterations(int32 max_num_iterations);
@ -95,15 +94,15 @@ class OptimizationProblem {
const std::set<int>& frozen_trajectories,
const std::map<std::string, LandmarkNode>& landmark_nodes);
const mapping::MapById<mapping::NodeId, NodeData>& node_data() const;
const mapping::MapById<mapping::SubmapId, SubmapData>& submap_data() const;
const MapById<NodeId, NodeData>& node_data() const;
const MapById<SubmapId, SubmapData>& submap_data() const;
const std::map<std::string, transform::Rigid3d>& landmark_data() const;
const sensor::MapByTime<sensor::ImuData>& imu_data() const;
const sensor::MapByTime<sensor::OdometryData>& odometry_data() const;
const sensor::MapByTime<sensor::FixedFramePoseData>& fixed_frame_pose_data()
const;
const std::map<int, mapping::PoseGraphInterface::TrajectoryData>&
trajectory_data() const;
const std::map<int, PoseGraphInterface::TrajectoryData>& trajectory_data()
const;
private:
// Uses odometry if available, otherwise the local SLAM results.
@ -111,19 +110,19 @@ class OptimizationProblem {
int trajectory_id, const NodeData& first_node_data,
const NodeData& second_node_data) const;
mapping::pose_graph::proto::OptimizationProblemOptions options_;
pose_graph::proto::OptimizationProblemOptions options_;
FixZ fix_z_;
mapping::MapById<mapping::NodeId, NodeData> node_data_;
mapping::MapById<mapping::SubmapId, SubmapData> submap_data_;
MapById<NodeId, NodeData> node_data_;
MapById<SubmapId, SubmapData> submap_data_;
std::map<std::string, transform::Rigid3d> landmark_data_;
sensor::MapByTime<sensor::ImuData> imu_data_;
sensor::MapByTime<sensor::OdometryData> odometry_data_;
sensor::MapByTime<sensor::FixedFramePoseData> fixed_frame_pose_data_;
std::map<int, mapping::PoseGraphInterface::TrajectoryData> trajectory_data_;
std::map<int, PoseGraphInterface::TrajectoryData> trajectory_data_;
};
} // namespace pose_graph
} // namespace mapping_3d
} // namespace mapping
} // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_OPTIMIZATION_PROBLEM_H_
#endif // CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_OPTIMIZATION_PROBLEM_3D_H_

65
cartographer/mapping_3d/pose_graph/optimization_problem_test.cc → cartographer/mapping_3d/pose_graph/optimization_problem_3d_test.cc
View File

@ -14,7 +14,7 @@
* limitations under the License.
*/
#include "cartographer/mapping_3d/pose_graph/optimization_problem.h"
#include "cartographer/mapping_3d/pose_graph/optimization_problem_3d.h"
#include <random>
@ -27,17 +27,18 @@
#include "gmock/gmock.h"
namespace cartographer {
namespace mapping_3d {
namespace mapping {
namespace pose_graph {
namespace {
class OptimizationProblemTest : public ::testing::Test {
class OptimizationProblem3DTest : public ::testing::Test {
protected:
OptimizationProblemTest()
: optimization_problem_(CreateOptions(), OptimizationProblem::FixZ::kNo),
OptimizationProblem3DTest()
: optimization_problem_(CreateOptions(),
OptimizationProblem3D::FixZ::kNo),
rng_(45387) {}
mapping::pose_graph::proto::OptimizationProblemOptions CreateOptions() {
pose_graph::proto::OptimizationProblemOptions CreateOptions() {
auto parameter_dictionary = common::MakeDictionary(R"text(
return {
acceleration_weight = 1e-4,
@ -54,7 +55,7 @@ class OptimizationProblemTest : public ::testing::Test {
num_threads = 4,
},
})text");
return mapping::pose_graph::CreateOptimizationProblemOptions(
return pose_graph::CreateOptimizationProblemOptions(
parameter_dictionary.get());
}
@ -90,7 +91,7 @@ class OptimizationProblemTest : public ::testing::Test {
Eigen::Vector3d(0., 0., rz)));
}
OptimizationProblem optimization_problem_;
OptimizationProblem3D optimization_problem_;
std::mt19937 rng_;
};
@ -102,7 +103,7 @@ transform::Rigid3d AddNoise(const transform::Rigid3d& transform,
noisy_rotation);
}
TEST_F(OptimizationProblemTest, ReducesNoise) {
TEST_F(OptimizationProblem3DTest, ReducesNoise) {
constexpr int kNumNodes = 100;
const transform::Rigid3d kSubmap0Transform = transform::Rigid3d::Identity();
const transform::Rigid3d kSubmap2Transform = transform::Rigid3d::Rotation(
@ -130,24 +131,24 @@ TEST_F(OptimizationProblemTest, ReducesNoise) {
now += common::FromSeconds(0.01);
}
std::vector<OptimizationProblem::Constraint> constraints;
std::vector<OptimizationProblem3D::Constraint> constraints;
for (int j = 0; j != kNumNodes; ++j) {
constraints.push_back(OptimizationProblem::Constraint{
mapping::SubmapId{kTrajectoryId, 0}, mapping::NodeId{kTrajectoryId, j},
OptimizationProblem::Constraint::Pose{
constraints.push_back(OptimizationProblem3D::Constraint{
SubmapId{kTrajectoryId, 0}, NodeId{kTrajectoryId, j},
OptimizationProblem3D::Constraint::Pose{
AddNoise(test_data[j].ground_truth_pose, test_data[j].noise), 1.,
1.}});
// We add an additional independent, but equally noisy observation.
constraints.push_back(OptimizationProblem::Constraint{
mapping::SubmapId{kTrajectoryId, 1}, mapping::NodeId{kTrajectoryId, j},
OptimizationProblem::Constraint::Pose{
constraints.push_back(OptimizationProblem3D::Constraint{
SubmapId{kTrajectoryId, 1}, NodeId{kTrajectoryId, j},
OptimizationProblem3D::Constraint::Pose{
AddNoise(test_data[j].ground_truth_pose,
RandomYawOnlyTransform(0.2, 0.3)),
1., 1.}});
// We add very noisy data with a low weight to verify it is mostly ignored.
constraints.push_back(OptimizationProblem::Constraint{
mapping::SubmapId{kTrajectoryId, 2}, mapping::NodeId{kTrajectoryId, j},
OptimizationProblem::Constraint::Pose{
constraints.push_back(OptimizationProblem3D::Constraint{
SubmapId{kTrajectoryId, 2}, NodeId{kTrajectoryId, j},
OptimizationProblem3D::Constraint::Pose{
kSubmap2Transform.inverse() * test_data[j].ground_truth_pose *
RandomTransform(1e3, 3.),
1e-9, 1e-9}});
@ -157,13 +158,13 @@ TEST_F(OptimizationProblemTest, ReducesNoise) {
double rotation_error_before = 0.;
const auto& node_data = optimization_problem_.node_data();
for (int j = 0; j != kNumNodes; ++j) {
translation_error_before += (test_data[j].ground_truth_pose.translation() -
node_data.at(mapping::NodeId{kTrajectoryId, j})
.global_pose.translation())
translation_error_before +=
(test_data[j].ground_truth_pose.translation() -
node_data.at(NodeId{kTrajectoryId, j}).global_pose.translation())
.norm();
rotation_error_before += transform::GetAngle(
test_data[j].ground_truth_pose.inverse() *
node_data.at(mapping::NodeId{kTrajectoryId, j}).global_pose);
rotation_error_before +=
transform::GetAngle(test_data[j].ground_truth_pose.inverse() *
node_data.at(NodeId{kTrajectoryId, j}).global_pose);
}
optimization_problem_.AddSubmap(kTrajectoryId, kSubmap0Transform);
@ -175,13 +176,13 @@ TEST_F(OptimizationProblemTest, ReducesNoise) {
double translation_error_after = 0.;
double rotation_error_after = 0.;
for (int j = 0; j != kNumNodes; ++j) {
translation_error_after += (test_data[j].ground_truth_pose.translation() -
node_data.at(mapping::NodeId{kTrajectoryId, j})
.global_pose.translation())
translation_error_after +=
(test_data[j].ground_truth_pose.translation() -
node_data.at(NodeId{kTrajectoryId, j}).global_pose.translation())
.norm();
rotation_error_after += transform::GetAngle(
test_data[j].ground_truth_pose.inverse() *
node_data.at(mapping::NodeId{kTrajectoryId, j}).global_pose);
rotation_error_after +=
transform::GetAngle(test_data[j].ground_truth_pose.inverse() *
node_data.at(NodeId{kTrajectoryId, j}).global_pose);
}
EXPECT_GT(0.8 * translation_error_before, translation_error_after);
@ -190,5 +191,5 @@ TEST_F(OptimizationProblemTest, ReducesNoise) {
} // namespace
} // namespace pose_graph
} // namespace mapping_3d
} // namespace mapping
} // namespace cartographer

28
cartographer/mapping_3d/pose_graph/spa_cost_function.h → cartographer/mapping_3d/pose_graph/spa_cost_function_3d.h
View File

@ -14,8 +14,8 @@
* limitations under the License.
*/
#ifndef CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_SPA_COST_FUNCTION_H_
#define CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_SPA_COST_FUNCTION_H_
#ifndef CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_SPA_COST_FUNCTION_3D_H_
#define CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_SPA_COST_FUNCTION_3D_H_
#include <array>
@ -30,28 +30,23 @@
#include "ceres/jet.h"
namespace cartographer {
namespace mapping_3d {
namespace mapping {
namespace pose_graph {
class SpaCostFunction {
class SpaCostFunction3D {
public:
using Constraint = mapping::PoseGraph::Constraint;
static ceres::CostFunction* CreateAutoDiffCostFunction(
const Constraint::Pose& pose) {
const PoseGraph::Constraint::Pose& pose) {
return new ceres::AutoDiffCostFunction<
SpaCostFunction, 6 /* residuals */, 4 /* rotation variables */,
SpaCostFunction3D, 6 /* residuals */, 4 /* rotation variables */,
3 /* translation variables */, 4 /* rotation variables */,
3 /* translation variables */>(new SpaCostFunction(pose));
3 /* translation variables */>(new SpaCostFunction3D(pose));
}
template <typename T>
bool operator()(const T* const c_i_rotation, const T* const c_i_translation,
const T* const c_j_rotation, const T* const c_j_translation,
T* const e) const {
using mapping::pose_graph::ComputeUnscaledError;
using mapping::pose_graph::ScaleError;
const std::array<T, 6> error = ScaleError(
ComputeUnscaledError(pose_.zbar_ij, c_i_rotation, c_i_translation,
c_j_rotation, c_j_translation),
@ -61,13 +56,14 @@ class SpaCostFunction {
}
private:
explicit SpaCostFunction(const Constraint::Pose& pose) : pose_(pose) {}
explicit SpaCostFunction3D(const PoseGraph::Constraint::Pose& pose)
: pose_(pose) {}
const Constraint::Pose pose_;
const PoseGraph::Constraint::Pose pose_;
};
} // namespace pose_graph
} // namespace mapping_3d
} // namespace mapping
} // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_SPA_COST_FUNCTION_H_
#endif // CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_SPA_COST_FUNCTION_3D_H_

16
cartographer/mapping_3d/pose_graph_3d.cc
View File

@ -42,9 +42,8 @@ namespace mapping {
PoseGraph3D::PoseGraph3D(const proto::PoseGraphOptions& options,
common::ThreadPool* thread_pool)
: options_(options),
optimization_problem_(
options_.optimization_problem_options(),
mapping_3d::pose_graph::OptimizationProblem::FixZ::kNo),
optimization_problem_(options_.optimization_problem_options(),
pose_graph::OptimizationProblem3D::FixZ::kNo),
constraint_builder_(options_.constraint_builder_options(), thread_pool) {}
PoseGraph3D::~PoseGraph3D() {
@ -340,7 +339,7 @@ void PoseGraph3D::UpdateTrajectoryConnectivity(const Constraint& constraint) {
void PoseGraph3D::HandleWorkQueue() {
constraint_builder_.WhenDone(
[this](const mapping_3d::pose_graph::ConstraintBuilder::Result& result) {
[this](const pose_graph::ConstraintBuilder3D::Result& result) {
{
common::MutexLocker locker(&mutex_);
constraints_.insert(constraints_.end(), result.begin(), result.end());
@ -401,8 +400,8 @@ void PoseGraph3D::WaitForAllComputations() {
}
std::cout << "\r\x1b[KOptimizing: Done. " << std::endl;
constraint_builder_.WhenDone(
[this, &notification](
const mapping_3d::pose_graph::ConstraintBuilder::Result& result) {
[this,
&notification](const pose_graph::ConstraintBuilder3D::Result& result) {
common::MutexLocker locker(&mutex_);
constraints_.insert(constraints_.end(), result.begin(), result.end());
notification = true;
@ -453,7 +452,8 @@ void PoseGraph3D::AddSubmapFromProto(
submap_data_.at(submap_id).submap = submap_ptr;
// Immediately show the submap at the 'global_submap_pose'.
global_submap_poses_.Insert(
submap_id, mapping_3d::pose_graph::SubmapData{global_submap_pose});
submap_id,
pose_graph::OptimizationProblem3D::SubmapData{global_submap_pose});
AddWorkItem([this, submap_id, global_submap_pose]() REQUIRES(mutex_) {
CHECK_EQ(frozen_trajectories_.count(submap_id.trajectory_id), 1);
submap_data_.at(submap_id).state = SubmapState::kFinished;
@ -759,7 +759,7 @@ PoseGraph3D::GetAllSubmapPoses() {
}
transform::Rigid3d PoseGraph3D::ComputeLocalToGlobalTransform(
const MapById<SubmapId, mapping_3d::pose_graph::SubmapData>&
const MapById<SubmapId, pose_graph::OptimizationProblem3D::SubmapData>&
global_submap_poses,
const int trajectory_id) const {
auto begin_it = global_submap_poses.BeginOfTrajectory(trajectory_id);

15
cartographer/mapping_3d/pose_graph_3d.h
View File

@ -35,8 +35,8 @@
#include "cartographer/mapping/pose_graph.h"
#include "cartographer/mapping/pose_graph_trimmer.h"
#include "cartographer/mapping/trajectory_connectivity_state.h"
#include "cartographer/mapping_3d/pose_graph/constraint_builder.h"
#include "cartographer/mapping_3d/pose_graph/optimization_problem.h"
#include "cartographer/mapping_3d/pose_graph/constraint_builder_3d.h"
#include "cartographer/mapping_3d/pose_graph/optimization_problem_3d.h"
#include "cartographer/mapping_3d/submap_3d.h"
#include "cartographer/sensor/fixed_frame_pose_data.h"
#include "cartographer/sensor/landmark_data.h"
@ -190,7 +190,7 @@ class PoseGraph3D : public PoseGraph {
// Computes the local to global map frame transform based on the given
// 'global_submap_poses'.
transform::Rigid3d ComputeLocalToGlobalTransform(
const MapById<SubmapId, mapping_3d::pose_graph::SubmapData>&
const MapById<SubmapId, pose_graph::OptimizationProblem3D::SubmapData>&
global_submap_poses,
int trajectory_id) const REQUIRES(mutex_);
@ -234,9 +234,8 @@ class PoseGraph3D : public PoseGraph {
void DispatchOptimization() REQUIRES(mutex_);
// Current optimization problem.
mapping_3d::pose_graph::OptimizationProblem optimization_problem_;
mapping_3d::pose_graph::ConstraintBuilder constraint_builder_
GUARDED_BY(mutex_);
pose_graph::OptimizationProblem3D optimization_problem_;
pose_graph::ConstraintBuilder3D constraint_builder_ GUARDED_BY(mutex_);
std::vector<Constraint> constraints_ GUARDED_BY(mutex_);
// Submaps get assigned an ID and state as soon as they are seen, even
@ -248,8 +247,8 @@ class PoseGraph3D : public PoseGraph {
int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0;
// Global submap poses currently used for displaying data.
MapById<SubmapId, mapping_3d::pose_graph::SubmapData> global_submap_poses_
GUARDED_BY(mutex_);
MapById<SubmapId, pose_graph::OptimizationProblem3D::SubmapData>
global_submap_poses_ GUARDED_BY(mutex_);
// Global landmark poses with all observations.
std::map<std::string /* landmark ID */, PoseGraph::LandmarkNode>

4
cartographer/mapping_3d/proto/local_trajectory_builder_options_3d.proto
View File

@ -20,7 +20,7 @@ import "cartographer/mapping/proto/motion_filter_options.proto";
import "cartographer/sensor/proto/adaptive_voxel_filter_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/scan_matching/proto/ceres_scan_matcher_options.proto";
import "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options_3d.proto";
// NEXT ID: 18
message LocalTrajectoryBuilderOptions3D {
@ -47,7 +47,7 @@ message LocalTrajectoryBuilderOptions3D {
bool use_online_correlative_scan_matching = 13;
mapping.scan_matching.proto.RealTimeCorrelativeScanMatcherOptions
real_time_correlative_scan_matcher_options = 14;
mapping_3d.scan_matching.proto.CeresScanMatcherOptions
mapping.scan_matching.proto.CeresScanMatcherOptions3D
ceres_scan_matcher_options = 6;
mapping.proto.MotionFilterOptions motion_filter_options = 7;

36
cartographer/mapping_3d/scan_matching/ceres_scan_matcher.cc → cartographer/mapping_3d/scan_matching/ceres_scan_matcher_3d.cc
View File

@ -14,7 +14,7 @@
* limitations under the License.
*/
#include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h"
#include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher_3d.h"
#include <string>
#include <utility>
@ -22,23 +22,23 @@
#include "cartographer/common/ceres_solver_options.h"
#include "cartographer/common/make_unique.h"
#include "cartographer/internal/mapping_3d/scan_matching/occupied_space_cost_function.h"
#include "cartographer/internal/mapping_3d/scan_matching/occupied_space_cost_function_3d.h"
#include "cartographer/mapping/pose_graph/ceres_pose.h"
#include "cartographer/mapping_3d/rotation_parameterization.h"
#include "cartographer/mapping_3d/scan_matching/rotation_delta_cost_functor.h"
#include "cartographer/mapping_3d/scan_matching/translation_delta_cost_functor.h"
#include "cartographer/mapping_3d/scan_matching/rotation_delta_cost_functor_3d.h"
#include "cartographer/mapping_3d/scan_matching/translation_delta_cost_functor_3d.h"
#include "cartographer/transform/rigid_transform.h"
#include "cartographer/transform/transform.h"
#include "ceres/ceres.h"
#include "glog/logging.h"
namespace cartographer {
namespace mapping_3d {
namespace mapping {
namespace scan_matching {
proto::CeresScanMatcherOptions CreateCeresScanMatcherOptions(
proto::CeresScanMatcherOptions3D CreateCeresScanMatcherOptions3D(
common::LuaParameterDictionary* const parameter_dictionary) {
proto::CeresScanMatcherOptions options;
proto::CeresScanMatcherOptions3D options;
for (int i = 0;; ++i) {
const std::string lua_identifier =
"occupied_space_weight_" + std::to_string(i);
@ -60,27 +60,28 @@ proto::CeresScanMatcherOptions CreateCeresScanMatcherOptions(
return options;
}
CeresScanMatcher::CeresScanMatcher(
const proto::CeresScanMatcherOptions& options)
CeresScanMatcher3D::CeresScanMatcher3D(
const proto::CeresScanMatcherOptions3D& options)
: options_(options),
ceres_solver_options_(
common::CreateCeresSolverOptions(options.ceres_solver_options())) {
ceres_solver_options_.linear_solver_type = ceres::DENSE_QR;
}
void CeresScanMatcher::Match(const Eigen::Vector3d& target_translation,
void CeresScanMatcher3D::Match(
const Eigen::Vector3d& target_translation,
const transform::Rigid3d& initial_pose_estimate,
const std::vector<PointCloudAndHybridGridPointers>&
point_clouds_and_hybrid_grids,
transform::Rigid3d* const pose_estimate,
ceres::Solver::Summary* const summary) {
ceres::Problem problem;
mapping::pose_graph::CeresPose ceres_pose(
pose_graph::CeresPose ceres_pose(
initial_pose_estimate, nullptr /* translation_parameterization */,
options_.only_optimize_yaw()
? std::unique_ptr<ceres::LocalParameterization>(
common::make_unique<ceres::AutoDiffLocalParameterization<
mapping::YawOnlyQuaternionPlus, 4, 1>>())
YawOnlyQuaternionPlus, 4, 1>>())
: std::unique_ptr<ceres::LocalParameterization>(
common::make_unique<ceres::QuaternionParameterization>()),
&problem);
@ -91,10 +92,9 @@ void CeresScanMatcher::Match(const Eigen::Vector3d& target_translation,
CHECK_GT(options_.occupied_space_weight(i), 0.);
const sensor::PointCloud& point_cloud =
*point_clouds_and_hybrid_grids[i].first;
const mapping::HybridGrid& hybrid_grid =
*point_clouds_and_hybrid_grids[i].second;
const HybridGrid& hybrid_grid = *point_clouds_and_hybrid_grids[i].second;
problem.AddResidualBlock(
OccupiedSpaceCostFunction::CreateAutoDiffCostFunction(
OccupiedSpaceCostFunction3D::CreateAutoDiffCostFunction(
options_.occupied_space_weight(i) /
std::sqrt(static_cast<double>(point_cloud.size())),
point_cloud, hybrid_grid),
@ -103,12 +103,12 @@ void CeresScanMatcher::Match(const Eigen::Vector3d& target_translation,
}
CHECK_GT(options_.translation_weight(), 0.);
problem.AddResidualBlock(
TranslationDeltaCostFunctor::CreateAutoDiffCostFunction(
TranslationDeltaCostFunctor3D::CreateAutoDiffCostFunction(
options_.translation_weight(), target_translation),
nullptr /* loss function */, ceres_pose.translation());
CHECK_GT(options_.rotation_weight(), 0.);
problem.AddResidualBlock(
RotationDeltaCostFunctor::CreateAutoDiffCostFunction(
RotationDeltaCostFunctor3D::CreateAutoDiffCostFunction(
options_.rotation_weight(), initial_pose_estimate.rotation()),
nullptr /* loss function */, ceres_pose.rotation());
@ -118,5 +118,5 @@ void CeresScanMatcher::Match(const Eigen::Vector3d& target_translation,
}
} // namespace scan_matching
} // namespace mapping_3d
} // namespace mapping
} // namespace cartographer

26
cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h → cartographer/mapping_3d/scan_matching/ceres_scan_matcher_3d.h
View File

@ -14,8 +14,8 @@
* limitations under the License.
*/
#ifndef CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_CERES_SCAN_MATCHER_H_
#define CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_CERES_SCAN_MATCHER_H_
#ifndef CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_CERES_SCAN_MATCHER_3D_H_
#define CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_CERES_SCAN_MATCHER_3D_H_
#include <utility>
#include <vector>
@ -23,27 +23,27 @@
#include "Eigen/Core"
#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/mapping_3d/hybrid_grid.h"
#include "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.pb.h"
#include "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options_3d.pb.h"
#include "cartographer/sensor/point_cloud.h"
#include "cartographer/transform/rigid_transform.h"
namespace cartographer {
namespace mapping_3d {
namespace mapping {
namespace scan_matching {
proto::CeresScanMatcherOptions CreateCeresScanMatcherOptions(
proto::CeresScanMatcherOptions3D CreateCeresScanMatcherOptions3D(
common::LuaParameterDictionary* parameter_dictionary);
using PointCloudAndHybridGridPointers =
std::pair<const sensor::PointCloud*, const mapping::HybridGrid*>;
std::pair<const sensor::PointCloud*, const HybridGrid*>;
// This scan matcher uses Ceres to align scans with an existing map.
class CeresScanMatcher {
class CeresScanMatcher3D {
public:
explicit CeresScanMatcher(const proto::CeresScanMatcherOptions& options);
explicit CeresScanMatcher3D(const proto::CeresScanMatcherOptions3D& options);
CeresScanMatcher(const CeresScanMatcher&) = delete;
CeresScanMatcher& operator=(const CeresScanMatcher&) = delete;
CeresScanMatcher3D(const CeresScanMatcher3D&) = delete;
CeresScanMatcher3D& operator=(const CeresScanMatcher3D&) = delete;
// Aligns 'point_clouds' within the 'hybrid_grids' given an
// 'initial_pose_estimate' and returns a 'pose_estimate' and the solver
@ -56,12 +56,12 @@ class CeresScanMatcher {
ceres::Solver::Summary* summary);
private:
const proto::CeresScanMatcherOptions options_;
const proto::CeresScanMatcherOptions3D options_;
ceres::Solver::Options ceres_solver_options_;
};
} // namespace scan_matching
} // namespace mapping_3d
} // namespace mapping
} // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_CERES_SCAN_MATCHER_H_
#endif // CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_CERES_SCAN_MATCHER_3D_H_

32
cartographer/mapping_3d/scan_matching/ceres_scan_matcher_test.cc → cartographer/mapping_3d/scan_matching/ceres_scan_matcher_3d_test.cc
View File

@ -14,7 +14,7 @@
* limitations under the License.
*/
#include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h"
#include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher_3d.h"
#include <memory>
@ -27,13 +27,13 @@
#include "gtest/gtest.h"
namespace cartographer {
namespace mapping_3d {
namespace mapping {
namespace scan_matching {
namespace {
class CeresScanMatcherTest : public ::testing::Test {
class CeresScanMatcher3DTest : public ::testing::Test {
protected:
CeresScanMatcherTest()
CeresScanMatcher3DTest()
: hybrid_grid_(1.f),
expected_pose_(
transform::Rigid3d::Translation(Eigen::Vector3d(-1., 0., 0.))) {
@ -59,8 +59,8 @@ class CeresScanMatcherTest : public ::testing::Test {
num_threads = 1,
},
})text");
options_ = CreateCeresScanMatcherOptions(parameter_dictionary.get());
ceres_scan_matcher_.reset(new CeresScanMatcher(options_));
options_ = CreateCeresScanMatcherOptions3D(parameter_dictionary.get());
ceres_scan_matcher_.reset(new CeresScanMatcher3D(options_));
}
void TestFromInitialPose(const transform::Rigid3d& initial_pose) {
@ -74,35 +74,35 @@ class CeresScanMatcherTest : public ::testing::Test {
EXPECT_THAT(pose, transform::IsNearly(expected_pose_, 3e-2));
}
mapping::HybridGrid hybrid_grid_;
HybridGrid hybrid_grid_;
transform::Rigid3d expected_pose_;
sensor::PointCloud point_cloud_;
proto::CeresScanMatcherOptions options_;
std::unique_ptr<CeresScanMatcher> ceres_scan_matcher_;
proto::CeresScanMatcherOptions3D options_;
std::unique_ptr<CeresScanMatcher3D> ceres_scan_matcher_;
};
TEST_F(CeresScanMatcherTest, PerfectEstimate) {
TEST_F(CeresScanMatcher3DTest, PerfectEstimate) {
TestFromInitialPose(
transform::Rigid3d::Translation(Eigen::Vector3d(-1., 0., 0.)));
}
TEST_F(CeresScanMatcherTest, AlongX) {
ceres_scan_matcher_.reset(new CeresScanMatcher(options_));
TEST_F(CeresScanMatcher3DTest, AlongX) {
ceres_scan_matcher_.reset(new CeresScanMatcher3D(options_));
TestFromInitialPose(
transform::Rigid3d::Translation(Eigen::Vector3d(-0.8, 0., 0.)));
}
TEST_F(CeresScanMatcherTest, AlongZ) {
TEST_F(CeresScanMatcher3DTest, AlongZ) {
TestFromInitialPose(
transform::Rigid3d::Translation(Eigen::Vector3d(-1., 0., -0.2)));
}
TEST_F(CeresScanMatcherTest, AlongXYZ) {
TEST_F(CeresScanMatcher3DTest, AlongXYZ) {
TestFromInitialPose(
transform::Rigid3d::Translation(Eigen::Vector3d(-0.9, -0.2, 0.2)));
}
TEST_F(CeresScanMatcherTest, FullPoseCorrection) {
TEST_F(CeresScanMatcher3DTest, FullPoseCorrection) {
// We try to find the rotation around z...
const auto additional_transform = transform::Rigid3d::Rotation(
Eigen::AngleAxisd(0.05, Eigen::Vector3d(0., 0., 1.)));
@ -117,5 +117,5 @@ TEST_F(CeresScanMatcherTest, FullPoseCorrection) {
} // namespace
} // namespace scan_matching
} // namespace mapping_3d
} // namespace mapping
} // namespace cartographer

154
cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc → cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_3d.cc
View File

@ -14,7 +14,7 @@
* limitations under the License.
*/
#include "cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h"
#include "cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_3d.h"
#include <algorithm>
#include <cmath>
@ -25,19 +25,19 @@
#include "cartographer/common/make_unique.h"
#include "cartographer/common/math.h"
#include "cartographer/mapping_3d/scan_matching/low_resolution_matcher.h"
#include "cartographer/mapping_3d/scan_matching/precomputation_grid.h"
#include "cartographer/mapping_3d/scan_matching/proto/fast_correlative_scan_matcher_options.pb.h"
#include "cartographer/mapping_3d/scan_matching/precomputation_grid_3d.h"
#include "cartographer/mapping_3d/scan_matching/proto/fast_correlative_scan_matcher_options_3d.pb.h"
#include "cartographer/transform/transform.h"
#include "glog/logging.h"
namespace cartographer {
namespace mapping_3d {
namespace mapping {
namespace scan_matching {
proto::FastCorrelativeScanMatcherOptions
CreateFastCorrelativeScanMatcherOptions(
proto::FastCorrelativeScanMatcherOptions3D
CreateFastCorrelativeScanMatcherOptions3D(
common::LuaParameterDictionary* const parameter_dictionary) {
proto::FastCorrelativeScanMatcherOptions options;
proto::FastCorrelativeScanMatcherOptions3D options;
options.set_branch_and_bound_depth(
parameter_dictionary->GetInt("branch_and_bound_depth"));
options.set_full_resolution_depth(
@ -58,8 +58,8 @@ CreateFastCorrelativeScanMatcherOptions(
class PrecomputationGridStack {
public:
PrecomputationGridStack(
const mapping::HybridGrid& hybrid_grid,
const proto::FastCorrelativeScanMatcherOptions& options) {
const HybridGrid& hybrid_grid,
const proto::FastCorrelativeScanMatcherOptions3D& options) {
CHECK_GE(options.branch_and_bound_depth(), 1);
CHECK_GE(options.full_resolution_depth(), 1);
precomputation_grids_.reserve(options.branch_and_bound_depth());
@ -80,29 +80,29 @@ class PrecomputationGridStack {
}
}
const PrecomputationGrid& Get(int depth) const {
const PrecomputationGrid3D& Get(int depth) const {
return precomputation_grids_.at(depth);
}
int max_depth() const { return precomputation_grids_.size() - 1; }
private:
std::vector<PrecomputationGrid> precomputation_grids_;
std::vector<PrecomputationGrid3D> precomputation_grids_;
};
struct DiscreteScan {
struct DiscreteScan3D {
transform::Rigid3f pose;
// Contains a vector of discretized scans for each 'depth'.
std::vector<std::vector<Eigen::Array3i>> cell_indices_per_depth;
float rotational_score;
};
struct Candidate {
Candidate(const int scan_index, const Eigen::Array3i& offset)
struct Candidate3D {
Candidate3D(const int scan_index, const Eigen::Array3i& offset)
: scan_index(scan_index), offset(offset) {}
static Candidate Unsuccessful() {
return Candidate(0, Eigen::Array3i::Zero());
static Candidate3D Unsuccessful() {
return Candidate3D(0, Eigen::Array3i::Zero());
}
// Index into the discrete scans vectors.
@ -119,14 +119,14 @@ struct Candidate {
// Score of the low resolution matcher.
float low_resolution_score = 0.f;
bool operator<(const Candidate& other) const { return score < other.score; }
bool operator>(const Candidate& other) const { return score > other.score; }
bool operator<(const Candidate3D& other) const { return score < other.score; }
bool operator>(const Candidate3D& other) const { return score > other.score; }
};
namespace {
std::vector<std::pair<Eigen::VectorXf, float>> HistogramsAtAnglesFromNodes(
const std::vector<mapping::TrajectoryNode>& nodes) {
const std::vector<TrajectoryNode>& nodes) {
std::vector<std::pair<Eigen::VectorXf, float>> histograms_at_angles;
for (const auto& node : nodes) {
histograms_at_angles.emplace_back(
@ -141,11 +141,11 @@ std::vector<std::pair<Eigen::VectorXf, float>> HistogramsAtAnglesFromNodes(
} // namespace
FastCorrelativeScanMatcher::FastCorrelativeScanMatcher(
const mapping::HybridGrid& hybrid_grid,
const mapping::HybridGrid* const low_resolution_hybrid_grid,
const std::vector<mapping::TrajectoryNode>& nodes,
const proto::FastCorrelativeScanMatcherOptions& options)
FastCorrelativeScanMatcher3D::FastCorrelativeScanMatcher3D(
const HybridGrid& hybrid_grid,
const HybridGrid* const low_resolution_hybrid_grid,
const std::vector<TrajectoryNode>& nodes,
const proto::FastCorrelativeScanMatcherOptions3D& options)
: options_(options),
resolution_(hybrid_grid.resolution()),
width_in_voxels_(hybrid_grid.grid_size()),
@ -154,14 +154,13 @@ FastCorrelativeScanMatcher::FastCorrelativeScanMatcher(
low_resolution_hybrid_grid_(low_resolution_hybrid_grid),
rotational_scan_matcher_(HistogramsAtAnglesFromNodes(nodes)) {}
FastCorrelativeScanMatcher::~FastCorrelativeScanMatcher() {}
FastCorrelativeScanMatcher3D::~FastCorrelativeScanMatcher3D() {}
std::unique_ptr<FastCorrelativeScanMatcher::Result>
FastCorrelativeScanMatcher::Match(
std::unique_ptr<FastCorrelativeScanMatcher3D::Result>
FastCorrelativeScanMatcher3D::Match(
const transform::Rigid3d& global_node_pose,
const transform::Rigid3d& global_submap_pose,
const mapping::TrajectoryNode::Data& constant_data,
const float min_score) const {
const TrajectoryNode::Data& constant_data, const float min_score) const {
const auto low_resolution_matcher = scan_matching::CreateLowResolutionMatcher(
low_resolution_hybrid_grid_, &constant_data.low_resolution_point_cloud);
const SearchParameters search_parameters{
@ -176,12 +175,11 @@ FastCorrelativeScanMatcher::Match(
constant_data.gravity_alignment, min_score);
}
std::unique_ptr<FastCorrelativeScanMatcher::Result>
FastCorrelativeScanMatcher::MatchFullSubmap(
std::unique_ptr<FastCorrelativeScanMatcher3D::Result>
FastCorrelativeScanMatcher3D::MatchFullSubmap(
const Eigen::Quaterniond& global_node_rotation,
const Eigen::Quaterniond& global_submap_rotation,
const mapping::TrajectoryNode::Data& constant_data,
const float min_score) const {
const TrajectoryNode::Data& constant_data, const float min_score) const {
float max_point_distance = 0.f;
for (const Eigen::Vector3f& point :
constant_data.high_resolution_point_cloud) {
@ -203,22 +201,22 @@ FastCorrelativeScanMatcher::MatchFullSubmap(
constant_data.gravity_alignment, min_score);
}
std::unique_ptr<FastCorrelativeScanMatcher::Result>
FastCorrelativeScanMatcher::MatchWithSearchParameters(
const FastCorrelativeScanMatcher::SearchParameters& search_parameters,
std::unique_ptr<FastCorrelativeScanMatcher3D::Result>
FastCorrelativeScanMatcher3D::MatchWithSearchParameters(
const FastCorrelativeScanMatcher3D::SearchParameters& search_parameters,
const transform::Rigid3f& global_node_pose,
const transform::Rigid3f& global_submap_pose,
const sensor::PointCloud& point_cloud,
const Eigen::VectorXf& rotational_scan_matcher_histogram,
const Eigen::Quaterniond& gravity_alignment, const float min_score) const {
const std::vector<DiscreteScan> discrete_scans = GenerateDiscreteScans(
const std::vector<DiscreteScan3D> discrete_scans = GenerateDiscreteScans(
search_parameters, point_cloud, rotational_scan_matcher_histogram,
gravity_alignment, global_node_pose, global_submap_pose);
const std::vector<Candidate> lowest_resolution_candidates =
const std::vector<Candidate3D> lowest_resolution_candidates =
ComputeLowestResolutionCandidates(search_parameters, discrete_scans);
const Candidate best_candidate = BranchAndBound(
const Candidate3D best_candidate = BranchAndBound(
search_parameters, discrete_scans, lowest_resolution_candidates,
precomputation_grid_stack_->max_depth(), min_score);
if (best_candidate.score > min_score) {
@ -231,12 +229,13 @@ FastCorrelativeScanMatcher::MatchWithSearchParameters(
return nullptr;
}
DiscreteScan FastCorrelativeScanMatcher::DiscretizeScan(
const FastCorrelativeScanMatcher::SearchParameters& search_parameters,
DiscreteScan3D FastCorrelativeScanMatcher3D::DiscretizeScan(
const FastCorrelativeScanMatcher3D::SearchParameters& search_parameters,
const sensor::PointCloud& point_cloud, const transform::Rigid3f& pose,
const float rotational_score) const {
std::vector<std::vector<Eigen::Array3i>> cell_indices_per_depth;
const PrecomputationGrid& original_grid = precomputation_grid_stack_->Get(0);
const PrecomputationGrid3D& original_grid =
precomputation_grid_stack_->Get(0);
std::vector<Eigen::Array3i> full_resolution_cell_indices;
for (const Eigen::Vector3f& point :
sensor::TransformPointCloud(point_cloud, pose)) {
@ -272,17 +271,17 @@ DiscreteScan FastCorrelativeScanMatcher::DiscretizeScan(
low_resolution_cell_at_start - low_resolution_search_window_start);
}
}
return DiscreteScan{pose, cell_indices_per_depth, rotational_score};
return DiscreteScan3D{pose, cell_indices_per_depth, rotational_score};
}
std::vector<DiscreteScan> FastCorrelativeScanMatcher::GenerateDiscreteScans(
const FastCorrelativeScanMatcher::SearchParameters& search_parameters,
std::vector<DiscreteScan3D> FastCorrelativeScanMatcher3D::GenerateDiscreteScans(
const FastCorrelativeScanMatcher3D::SearchParameters& search_parameters,
const sensor::PointCloud& point_cloud,
const Eigen::VectorXf& rotational_scan_matcher_histogram,
const Eigen::Quaterniond& gravity_alignment,
const transform::Rigid3f& global_node_pose,
const transform::Rigid3f& global_submap_pose) const {
std::vector<DiscreteScan> result;
std::vector<DiscreteScan3D> result;
// We set this value to something on the order of resolution to make sure that
// the std::acos() below is defined.
float max_scan_range = 3.f * resolution_;
@ -326,9 +325,9 @@ std::vector<DiscreteScan> FastCorrelativeScanMatcher::GenerateDiscreteScans(
return result;
}
std::vector<Candidate>
FastCorrelativeScanMatcher::GenerateLowestResolutionCandidates(
const FastCorrelativeScanMatcher::SearchParameters& search_parameters,
std::vector<Candidate3D>
FastCorrelativeScanMatcher3D::GenerateLowestResolutionCandidates(
const FastCorrelativeScanMatcher3D::SearchParameters& search_parameters,
const int num_discrete_scans) const {
const int linear_step_size = 1 << precomputation_grid_stack_->max_depth();
const int num_lowest_resolution_linear_xy_candidates =
@ -341,7 +340,7 @@ FastCorrelativeScanMatcher::GenerateLowestResolutionCandidates(
num_discrete_scans *
common::Power(num_lowest_resolution_linear_xy_candidates, 2) *
num_lowest_resolution_linear_z_candidates;
std::vector<Candidate> candidates;
std::vector<Candidate3D> candidates;
candidates.reserve(num_candidates);
for (int scan_index = 0; scan_index != num_discrete_scans; ++scan_index) {
for (int z = -search_parameters.linear_z_window_size;
@ -361,14 +360,14 @@ FastCorrelativeScanMatcher::GenerateLowestResolutionCandidates(
return candidates;
}
void FastCorrelativeScanMatcher::ScoreCandidates(
const int depth, const std::vector<DiscreteScan>& discrete_scans,
std::vector<Candidate>* const candidates) const {
void FastCorrelativeScanMatcher3D::ScoreCandidates(
const int depth, const std::vector<DiscreteScan3D>& discrete_scans,
std::vector<Candidate3D>* const candidates) const {
const int reduction_exponent =
std::max(0, depth - options_.full_resolution_depth() + 1);
for (Candidate& candidate : *candidates) {
for (Candidate3D& candidate : *candidates) {
int sum = 0;
const DiscreteScan& discrete_scan = discrete_scans[candidate.scan_index];
const DiscreteScan3D& discrete_scan = discrete_scans[candidate.scan_index];
const Eigen::Array3i offset(candidate.offset[0] >> reduction_exponent,
candidate.offset[1] >> reduction_exponent,
candidate.offset[2] >> reduction_exponent);
@ -378,18 +377,19 @@ void FastCorrelativeScanMatcher::ScoreCandidates(
const Eigen::Array3i proposed_cell_index = cell_index + offset;
sum += precomputation_grid_stack_->Get(depth).value(proposed_cell_index);
}
candidate.score = PrecomputationGrid::ToProbability(
candidate.score = PrecomputationGrid3D::ToProbability(
sum /
static_cast<float>(discrete_scan.cell_indices_per_depth[depth].size()));
}
std::sort(candidates->begin(), candidates->end(), std::greater<Candidate>());
std::sort(candidates->begin(), candidates->end(),
std::greater<Candidate3D>());
}
std::vector<Candidate>
FastCorrelativeScanMatcher::ComputeLowestResolutionCandidates(
const FastCorrelativeScanMatcher::SearchParameters& search_parameters,
const std::vector<DiscreteScan>& discrete_scans) const {
std::vector<Candidate> lowest_resolution_candidates =
std::vector<Candidate3D>
FastCorrelativeScanMatcher3D::ComputeLowestResolutionCandidates(
const FastCorrelativeScanMatcher3D::SearchParameters& search_parameters,
const std::vector<DiscreteScan3D>& discrete_scans) const {
std::vector<Candidate3D> lowest_resolution_candidates =
GenerateLowestResolutionCandidates(search_parameters,
discrete_scans.size());
ScoreCandidates(precomputation_grid_stack_->max_depth(), discrete_scans,
@ -397,48 +397,48 @@ FastCorrelativeScanMatcher::ComputeLowestResolutionCandidates(
return lowest_resolution_candidates;
}
transform::Rigid3f FastCorrelativeScanMatcher::GetPoseFromCandidate(
const std::vector<DiscreteScan>& discrete_scans,
const Candidate& candidate) const {
transform::Rigid3f FastCorrelativeScanMatcher3D::GetPoseFromCandidate(
const std::vector<DiscreteScan3D>& discrete_scans,
const Candidate3D& candidate) const {
return transform::Rigid3f::Translation(
resolution_ * candidate.offset.matrix().cast<float>()) *
discrete_scans[candidate.scan_index].pose;
}
Candidate FastCorrelativeScanMatcher::BranchAndBound(
const FastCorrelativeScanMatcher::SearchParameters& search_parameters,
const std::vector<DiscreteScan>& discrete_scans,
const std::vector<Candidate>& candidates, const int candidate_depth,
Candidate3D FastCorrelativeScanMatcher3D::BranchAndBound(
const FastCorrelativeScanMatcher3D::SearchParameters& search_parameters,
const std::vector<DiscreteScan3D>& discrete_scans,
const std::vector<Candidate3D>& candidates, const int candidate_depth,
float min_score) const {
if (candidate_depth == 0) {
for (const Candidate& candidate : candidates) {
for (const Candidate3D& candidate : candidates) {
if (candidate.score <= min_score) {
// Return if the candidate is bad because the following candidate will
// not have better score.
return Candidate::Unsuccessful();
return Candidate3D::Unsuccessful();
}
const float low_resolution_score =
(*search_parameters.low_resolution_matcher)(
GetPoseFromCandidate(discrete_scans, candidate));
if (low_resolution_score >= options_.min_low_resolution_score()) {
// We found the best candidate that passes the matching function.
Candidate best_candidate = candidate;
Candidate3D best_candidate = candidate;
best_candidate.low_resolution_score = low_resolution_score;
return best_candidate;
}
}
// All candidates have good scores but none passes the matching function.
return Candidate::Unsuccessful();
return Candidate3D::Unsuccessful();
}
Candidate best_high_resolution_candidate = Candidate::Unsuccessful();
Candidate3D best_high_resolution_candidate = Candidate3D::Unsuccessful();
best_high_resolution_candidate.score = min_score;
for (const Candidate& candidate : candidates) {
for (const Candidate3D& candidate : candidates) {
if (candidate.score <= min_score) {
break;
}
std::vector<Candidate> higher_resolution_candidates;
std::vector<Candidate3D> higher_resolution_candidates;
const int half_width = 1 << (candidate_depth - 1);
for (int z : {0, half_width}) {
if (candidate.offset.z() + z > search_parameters.linear_z_window_size) {
@ -471,5 +471,5 @@ Candidate FastCorrelativeScanMatcher::BranchAndBound(
}
} // namespace scan_matching
} // namespace mapping_3d
} // namespace mapping
} // namespace cartographer

74
cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h → cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_3d.h
View File

@ -17,8 +17,8 @@
// This is an implementation of a 3D branch-and-bound algorithm similar to
// FastCorrelativeScanMatcher2D.
#ifndef CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_FAST_CORRELATIVE_SCAN_MATCHER_H_
#define CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_FAST_CORRELATIVE_SCAN_MATCHER_H_
#ifndef CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_FAST_CORRELATIVE_SCAN_MATCHER_3D_H_
#define CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_FAST_CORRELATIVE_SCAN_MATCHER_3D_H_
#include <memory>
#include <vector>
@ -28,26 +28,26 @@
#include "cartographer/mapping/trajectory_node.h"
#include "cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_2d.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_3d.pb.h"
#include "cartographer/mapping_3d/scan_matching/rotational_scan_matcher.h"
#include "cartographer/sensor/point_cloud.h"
namespace cartographer {
namespace mapping_3d {
namespace mapping {
namespace scan_matching {
proto::FastCorrelativeScanMatcherOptions
CreateFastCorrelativeScanMatcherOptions(
proto::FastCorrelativeScanMatcherOptions3D
CreateFastCorrelativeScanMatcherOptions3D(
common::LuaParameterDictionary* parameter_dictionary);
class PrecomputationGridStack;
struct DiscreteScan;
struct Candidate;
struct DiscreteScan3D;
struct Candidate3D;
// Used to compute scores between 0 and 1 how well the given pose matches.
using MatchingFunction = std::function<float(const transform::Rigid3f&)>;
class FastCorrelativeScanMatcher {
class FastCorrelativeScanMatcher3D {
public:
struct Result {
float score;
@ -56,24 +56,23 @@ class FastCorrelativeScanMatcher {
float low_resolution_score;
};
FastCorrelativeScanMatcher(
const mapping::HybridGrid& hybrid_grid,
const mapping::HybridGrid* low_resolution_hybrid_grid,
const std::vector<mapping::TrajectoryNode>& nodes,
const proto::FastCorrelativeScanMatcherOptions& options);
~FastCorrelativeScanMatcher();
FastCorrelativeScanMatcher3D(
const HybridGrid& hybrid_grid,
const HybridGrid* low_resolution_hybrid_grid,
const std::vector<TrajectoryNode>& nodes,
const proto::FastCorrelativeScanMatcherOptions3D& options);
~FastCorrelativeScanMatcher3D();
FastCorrelativeScanMatcher(const FastCorrelativeScanMatcher&) = delete;
FastCorrelativeScanMatcher& operator=(const FastCorrelativeScanMatcher&) =
FastCorrelativeScanMatcher3D(const FastCorrelativeScanMatcher3D&) = delete;
FastCorrelativeScanMatcher3D& operator=(const FastCorrelativeScanMatcher3D&) =
delete;
// Aligns the node with the given 'constant_data' within the 'hybrid_grid'
// given 'global_node_pose' and 'global_submap_pose'. 'Result' is only
// returned if a score above 'min_score' (excluding equality) is possible.
std::unique_ptr<Result> Match(
const transform::Rigid3d& global_node_pose,
std::unique_ptr<Result> Match(const transform::Rigid3d& global_node_pose,
const transform::Rigid3d& global_submap_pose,
const mapping::TrajectoryNode::Data& constant_data,
const TrajectoryNode::Data& constant_data,
float min_score) const;
// Aligns the node with the given 'constant_data' within the 'hybrid_grid'
@ -83,8 +82,7 @@ class FastCorrelativeScanMatcher {
std::unique_ptr<Result> MatchFullSubmap(
const Eigen::Quaterniond& global_node_rotation,
const Eigen::Quaterniond& global_submap_rotation,
const mapping::TrajectoryNode::Data& constant_data,
float min_score) const;
const TrajectoryNode::Data& constant_data, float min_score) const;
private:
struct SearchParameters {
@ -101,43 +99,43 @@ class FastCorrelativeScanMatcher {
const sensor::PointCloud& point_cloud,
const Eigen::VectorXf& rotational_scan_matcher_histogram,
const Eigen::Quaterniond& gravity_alignment, float min_score) const;
DiscreteScan DiscretizeScan(const SearchParameters& search_parameters,
DiscreteScan3D DiscretizeScan(const SearchParameters& search_parameters,
const sensor::PointCloud& point_cloud,
const transform::Rigid3f& pose,
float rotational_score) const;
std::vector<DiscreteScan> GenerateDiscreteScans(
std::vector<DiscreteScan3D> GenerateDiscreteScans(
const SearchParameters& search_parameters,
const sensor::PointCloud& point_cloud,
const Eigen::VectorXf& rotational_scan_matcher_histogram,
const Eigen::Quaterniond& gravity_alignment,
const transform::Rigid3f& global_node_pose,
const transform::Rigid3f& global_submap_pose) const;
std::vector<Candidate> GenerateLowestResolutionCandidates(
std::vector<Candidate3D> GenerateLowestResolutionCandidates(
const SearchParameters& search_parameters, int num_discrete_scans) const;
void ScoreCandidates(int depth,
const std::vector<DiscreteScan>& discrete_scans,
std::vector<Candidate>* const candidates) const;
std::vector<Candidate> ComputeLowestResolutionCandidates(
const std::vector<DiscreteScan3D>& discrete_scans,
std::vector<Candidate3D>* const candidates) const;
std::vector<Candidate3D> ComputeLowestResolutionCandidates(
const SearchParameters& search_parameters,
const std::vector<DiscreteScan>& discrete_scans) const;
Candidate BranchAndBound(const SearchParameters& search_parameters,
const std::vector<DiscreteScan>& discrete_scans,
const std::vector<Candidate>& candidates,
const std::vector<DiscreteScan3D>& discrete_scans) const;
Candidate3D BranchAndBound(const SearchParameters& search_parameters,
const std::vector<DiscreteScan3D>& discrete_scans,
const std::vector<Candidate3D>& candidates,
int candidate_depth, float min_score) const;
transform::Rigid3f GetPoseFromCandidate(
const std::vector<DiscreteScan>& discrete_scans,
const Candidate& candidate) const;
const std::vector<DiscreteScan3D>& discrete_scans,
const Candidate3D& candidate) const;
const proto::FastCorrelativeScanMatcherOptions options_;
const proto::FastCorrelativeScanMatcherOptions3D options_;
const float resolution_;
const int width_in_voxels_;
std::unique_ptr<PrecomputationGridStack> precomputation_grid_stack_;
const mapping::HybridGrid* const low_resolution_hybrid_grid_;
const HybridGrid* const low_resolution_hybrid_grid_;
RotationalScanMatcher rotational_scan_matcher_;
};
} // namespace scan_matching
} // namespace mapping_3d
} // namespace mapping
} // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_FAST_CORRELATIVE_SCAN_MATCHER_H_
#endif // CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_FAST_CORRELATIVE_SCAN_MATCHER_3D_H_

64
cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_test.cc → cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_3d_test.cc
View File

@ -14,7 +14,7 @@
* limitations under the License.
*/
#include "cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h"
#include "cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_3d.h"
#include <algorithm>
#include <cmath>
@ -29,15 +29,15 @@
#include "gtest/gtest.h"
namespace cartographer {
namespace mapping_3d {
namespace mapping {
namespace scan_matching {
namespace {
class FastCorrelativeScanMatcherTest : public ::testing::Test {
class FastCorrelativeScanMatcher3DTest : public ::testing::Test {
protected:
FastCorrelativeScanMatcherTest()
: range_data_inserter_(CreateRangeDataInserterTestOptions()),
options_(CreateFastCorrelativeScanMatcherTestOptions(5)) {}
FastCorrelativeScanMatcher3DTest()
: range_data_inserter_(CreateRangeDataInserterTestOptions3D()),
options_(CreateFastCorrelativeScanMatcher3DTestOptions3D(5)) {}
void SetUp() override {
point_cloud_ = {
@ -59,8 +59,8 @@ class FastCorrelativeScanMatcherTest : public ::testing::Test {
Eigen::AngleAxisf(theta, Eigen::Vector3f::UnitZ()));
}
static proto::FastCorrelativeScanMatcherOptions
CreateFastCorrelativeScanMatcherTestOptions(
static proto::FastCorrelativeScanMatcherOptions3D
CreateFastCorrelativeScanMatcher3DTestOptions3D(
const int branch_and_bound_depth) {
auto parameter_dictionary = common::MakeDictionary(
"return {"
@ -78,25 +78,25 @@ class FastCorrelativeScanMatcherTest : public ::testing::Test {
"linear_z_search_window = 0.8, "
"angular_search_window = 0.3, "
"}");
return CreateFastCorrelativeScanMatcherOptions(parameter_dictionary.get());
return CreateFastCorrelativeScanMatcherOptions3D(
parameter_dictionary.get());
}
static mapping::proto::RangeDataInserterOptions3D
CreateRangeDataInserterTestOptions() {
CreateRangeDataInserterTestOptions3D() {
auto parameter_dictionary = common::MakeDictionary(
"return { "
"hit_probability = 0.7, "
"miss_probability = 0.4, "
"num_free_space_voxels = 5, "
"}");
return mapping::CreateRangeDataInserterOptions3D(
parameter_dictionary.get());
return CreateRangeDataInserterOptions3D(parameter_dictionary.get());
}
std::unique_ptr<FastCorrelativeScanMatcher> GetFastCorrelativeScanMatcher(
const proto::FastCorrelativeScanMatcherOptions& options,
std::unique_ptr<FastCorrelativeScanMatcher3D> GetFastCorrelativeScanMatcher(
const proto::FastCorrelativeScanMatcherOptions3D& options,
const transform::Rigid3f& pose) {
hybrid_grid_ = common::make_unique<mapping::HybridGrid>(0.05f);
hybrid_grid_ = common::make_unique<HybridGrid>(0.05f);
range_data_inserter_.Insert(
sensor::RangeData{pose.translation(),
sensor::TransformPointCloud(point_cloud_, pose),
@ -104,18 +104,18 @@ class FastCorrelativeScanMatcherTest : public ::testing::Test {
hybrid_grid_.get());
hybrid_grid_->FinishUpdate();
return common::make_unique<FastCorrelativeScanMatcher>(
return common::make_unique<FastCorrelativeScanMatcher3D>(
*hybrid_grid_, hybrid_grid_.get(),
std::vector<mapping::TrajectoryNode>(
{{std::make_shared<const mapping::TrajectoryNode::Data>(
std::vector<TrajectoryNode>(
{{std::make_shared<const TrajectoryNode::Data>(
CreateConstantData(point_cloud_)),
pose.cast<double>()}}),
options);
}
mapping::TrajectoryNode::Data CreateConstantData(
TrajectoryNode::Data CreateConstantData(
const sensor::PointCloud& low_resolution_point_cloud) {
return mapping::TrajectoryNode::Data{common::FromUniversal(0),
return TrajectoryNode::Data{common::FromUniversal(0),
Eigen::Quaterniond::Identity(),
{},
point_cloud_,
@ -126,22 +126,22 @@ class FastCorrelativeScanMatcherTest : public ::testing::Test {
std::mt19937 prng_ = std::mt19937(42);
std::uniform_real_distribution<float> distribution_ =
std::uniform_real_distribution<float>(-1.f, 1.f);
mapping::RangeDataInserter3D range_data_inserter_;
const proto::FastCorrelativeScanMatcherOptions options_;
RangeDataInserter3D range_data_inserter_;
const proto::FastCorrelativeScanMatcherOptions3D options_;
sensor::PointCloud point_cloud_;
std::unique_ptr<mapping::HybridGrid> hybrid_grid_;
std::unique_ptr<HybridGrid> hybrid_grid_;
};
constexpr float kMinScore = 0.1f;
TEST_F(FastCorrelativeScanMatcherTest, CorrectPoseForMatch) {
TEST_F(FastCorrelativeScanMatcher3DTest, CorrectPoseForMatch) {
for (int i = 0; i != 20; ++i) {
const auto expected_pose = GetRandomPose();
std::unique_ptr<FastCorrelativeScanMatcher> fast_correlative_scan_matcher(
std::unique_ptr<FastCorrelativeScanMatcher3D> fast_correlative_scan_matcher(
GetFastCorrelativeScanMatcher(options_, expected_pose));
const std::unique_ptr<FastCorrelativeScanMatcher::Result> result =
const std::unique_ptr<FastCorrelativeScanMatcher3D::Result> result =
fast_correlative_scan_matcher->Match(
transform::Rigid3d::Identity(), transform::Rigid3d::Identity(),
CreateConstantData(point_cloud_), kMinScore);
@ -154,7 +154,7 @@ TEST_F(FastCorrelativeScanMatcherTest, CorrectPoseForMatch) {
<< "Actual: " << transform::ToProto(result->pose_estimate).DebugString()
<< "\nExpected: " << transform::ToProto(expected_pose).DebugString();
const std::unique_ptr<FastCorrelativeScanMatcher::Result>
const std::unique_ptr<FastCorrelativeScanMatcher3D::Result>
low_resolution_result = fast_correlative_scan_matcher->Match(
transform::Rigid3d::Identity(), transform::Rigid3d::Identity(),
CreateConstantData({Eigen::Vector3f(42.f, 42.f, 42.f)}), kMinScore);
@ -163,13 +163,13 @@ TEST_F(FastCorrelativeScanMatcherTest, CorrectPoseForMatch) {
}
}
TEST_F(FastCorrelativeScanMatcherTest, CorrectPoseForMatchFullSubmap) {
TEST_F(FastCorrelativeScanMatcher3DTest, CorrectPoseForMatchFullSubmap) {
const auto expected_pose = GetRandomPose();
std::unique_ptr<FastCorrelativeScanMatcher> fast_correlative_scan_matcher(
std::unique_ptr<FastCorrelativeScanMatcher3D> fast_correlative_scan_matcher(
GetFastCorrelativeScanMatcher(options_, expected_pose));
const std::unique_ptr<FastCorrelativeScanMatcher::Result> result =
const std::unique_ptr<FastCorrelativeScanMatcher3D::Result> result =
fast_correlative_scan_matcher->MatchFullSubmap(
Eigen::Quaterniond::Identity(), Eigen::Quaterniond::Identity(),
CreateConstantData(point_cloud_), kMinScore);
@ -182,7 +182,7 @@ TEST_F(FastCorrelativeScanMatcherTest, CorrectPoseForMatchFullSubmap) {
<< "Actual: " << transform::ToProto(result->pose_estimate).DebugString()
<< "\nExpected: " << transform::ToProto(expected_pose).DebugString();
const std::unique_ptr<FastCorrelativeScanMatcher::Result>
const std::unique_ptr<FastCorrelativeScanMatcher3D::Result>
low_resolution_result = fast_correlative_scan_matcher->MatchFullSubmap(
Eigen::Quaterniond::Identity(), Eigen::Quaterniond::Identity(),
CreateConstantData({Eigen::Vector3f(42.f, 42.f, 42.f)}), kMinScore);
@ -192,5 +192,5 @@ TEST_F(FastCorrelativeScanMatcherTest, CorrectPoseForMatchFullSubmap) {
} // namespace
} // namespace scan_matching
} // namespace mapping_3d
} // namespace mapping
} // namespace cartographer

8
cartographer/mapping_3d/scan_matching/interpolated_grid.h
View File

@ -22,7 +22,7 @@
#include "cartographer/mapping_3d/hybrid_grid.h"
namespace cartographer {
namespace mapping_3d {
namespace mapping {
namespace scan_matching {
// Interpolates between HybridGrid probability voxels. We use the tricubic
@ -34,7 +34,7 @@ namespace scan_matching {
// continuously differentiable.
class InterpolatedGrid {
public:
explicit InterpolatedGrid(const mapping::HybridGrid& hybrid_grid)
explicit InterpolatedGrid(const HybridGrid& hybrid_grid)
: hybrid_grid_(hybrid_grid) {}
InterpolatedGrid(const InterpolatedGrid&) = delete;
@ -145,11 +145,11 @@ class InterpolatedGrid {
return CenterOfLowerVoxel(jet_x.a, jet_y.a, jet_z.a);
}
const mapping::HybridGrid& hybrid_grid_;
const HybridGrid& hybrid_grid_;
};
} // namespace scan_matching
} // namespace mapping_3d
} // namespace mapping
} // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_INTERPOLATED_GRID_H_

6
cartographer/mapping_3d/scan_matching/interpolated_grid_test.cc
View File

@ -21,7 +21,7 @@
#include "gtest/gtest.h"
namespace cartographer {
namespace mapping_3d {
namespace mapping {
namespace scan_matching {
namespace {
@ -43,7 +43,7 @@ class InterpolatedGridTest : public ::testing::Test {
hybrid_grid_.GetCellIndex(Eigen::Vector3f(x, y, z)));
}
mapping::HybridGrid hybrid_grid_;
HybridGrid hybrid_grid_;
InterpolatedGrid interpolated_grid_;
};
@ -85,5 +85,5 @@ TEST_F(InterpolatedGridTest, MonotonicBehaviorBetweenGridPointsInX) {
} // namespace
} // namespace scan_matching
} // namespace mapping_3d
} // namespace mapping
} // namespace cartographer

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

@ -17,12 +17,11 @@
#include "cartographer/mapping_3d/scan_matching/low_resolution_matcher.h"
namespace cartographer {
namespace mapping_3d {
namespace mapping {
namespace scan_matching {
std::function<float(const transform::Rigid3f&)> CreateLowResolutionMatcher(
const mapping::HybridGrid* low_resolution_grid,
const sensor::PointCloud* points) {
const HybridGrid* low_resolution_grid, const sensor::PointCloud* points) {
return [=](const transform::Rigid3f& pose) {
float score = 0.f;
for (const Eigen::Vector3f& point :
@ -36,5 +35,5 @@ std::function<float(const transform::Rigid3f&)> CreateLowResolutionMatcher(
}
} // namespace scan_matching
} // namespace mapping_3d
} // namespace mapping
} // namespace cartographer

7
cartographer/mapping_3d/scan_matching/low_resolution_matcher.h
View File

@ -24,15 +24,14 @@
#include "cartographer/transform/rigid_transform.h"
namespace cartographer {
namespace mapping_3d {
namespace mapping {
namespace scan_matching {
std::function<float(const transform::Rigid3f&)> CreateLowResolutionMatcher(
const mapping::HybridGrid* low_resolution_grid,
const sensor::PointCloud* points);
const HybridGrid* low_resolution_grid, const sensor::PointCloud* points);
} // namespace scan_matching
} // namespace mapping_3d
} // namespace mapping
} // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_LOW_RESOLUTION_MATCHER_H_

29
cartographer/mapping_3d/scan_matching/precomputation_grid.cc → cartographer/mapping_3d/scan_matching/precomputation_grid_3d.cc
View File

@ -14,7 +14,7 @@
* limitations under the License.
*/
#include "cartographer/mapping_3d/scan_matching/precomputation_grid.h"
#include "cartographer/mapping_3d/scan_matching/precomputation_grid_3d.h"
#include <algorithm>
@ -24,9 +24,8 @@
#include "glog/logging.h"
namespace cartographer {
namespace mapping_3d {
namespace mapping {
namespace scan_matching {
namespace {
// C++11 defines that integer division rounds towards zero. For index math, we
@ -47,15 +46,13 @@ Eigen::Array3i CellIndexAtHalfResolution(const Eigen::Array3i& cell_index) {
} // namespace
PrecomputationGrid ConvertToPrecomputationGrid(
const mapping::HybridGrid& hybrid_grid) {
PrecomputationGrid result(hybrid_grid.resolution());
for (auto it = mapping::HybridGrid::Iterator(hybrid_grid); !it.Done();
it.Next()) {
PrecomputationGrid3D ConvertToPrecomputationGrid(
const HybridGrid& hybrid_grid) {
PrecomputationGrid3D result(hybrid_grid.resolution());
for (auto it = HybridGrid::Iterator(hybrid_grid); !it.Done(); it.Next()) {
const int cell_value = common::RoundToInt(
(mapping::ValueToProbability(it.GetValue()) -
mapping::kMinProbability) *
(255.f / (mapping::kMaxProbability - mapping::kMinProbability)));
(ValueToProbability(it.GetValue()) - kMinProbability) *
(255.f / (kMaxProbability - kMinProbability)));
CHECK_GE(cell_value, 0);
CHECK_LE(cell_value, 255);
*result.mutable_value(it.GetCellIndex()) = cell_value;
@ -63,18 +60,18 @@ PrecomputationGrid ConvertToPrecomputationGrid(
return result;
}
PrecomputationGrid PrecomputeGrid(const PrecomputationGrid& grid,
PrecomputationGrid3D PrecomputeGrid(const PrecomputationGrid3D& grid,
const bool half_resolution,
const Eigen::Array3i& shift) {
PrecomputationGrid result(grid.resolution());
for (auto it = PrecomputationGrid::Iterator(grid); !it.Done(); it.Next()) {
PrecomputationGrid3D result(grid.resolution());
for (auto it = PrecomputationGrid3D::Iterator(grid); !it.Done(); it.Next()) {
for (int i = 0; i != 8; ++i) {
// We use this value to update 8 values in the resulting grid, at
// position (x - {0, 'shift'}, y - {0, 'shift'}, z - {0, 'shift'}).
// If 'shift' is 2 ** (depth - 1), where depth 0 is the original grid,
// this results in precomputation grids analogous to the 2D case.
const Eigen::Array3i cell_index =
it.GetCellIndex() - shift * PrecomputationGrid::GetOctant(i);
it.GetCellIndex() - shift * PrecomputationGrid3D::GetOctant(i);
auto* const cell_value = result.mutable_value(
half_resolution ? CellIndexAtHalfResolution(cell_index) : cell_index);
*cell_value = std::max(it.GetValue(), *cell_value);
@ -84,5 +81,5 @@ PrecomputationGrid PrecomputeGrid(const PrecomputationGrid& grid,
}
} // namespace scan_matching
} // namespace mapping_3d
} // namespace mapping
} // namespace cartographer

28
cartographer/mapping_3d/scan_matching/precomputation_grid.h → cartographer/mapping_3d/scan_matching/precomputation_grid_3d.h
View File

@ -14,32 +14,30 @@
* limitations under the License.
*/
#ifndef CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_PRECOMPUTATION_GRID_H_
#define CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_PRECOMPUTATION_GRID_H_
#ifndef CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_PRECOMPUTATION_GRID_3D_H_
#define CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_PRECOMPUTATION_GRID_3D_H_
#include "cartographer/mapping_3d/hybrid_grid.h"
namespace cartographer {
namespace mapping_3d {
namespace mapping {
namespace scan_matching {
class PrecomputationGrid : public mapping::HybridGridBase<uint8> {
class PrecomputationGrid3D : public HybridGridBase<uint8> {
public:
explicit PrecomputationGrid(const float resolution)
: mapping::HybridGridBase<uint8>(resolution) {}
explicit PrecomputationGrid3D(const float resolution)
: HybridGridBase<uint8>(resolution) {}
// Maps values from [0, 255] to [kMinProbability, kMaxProbability].
static float ToProbability(float value) {
return mapping::kMinProbability +
value *
((mapping::kMaxProbability - mapping::kMinProbability) / 255.f);
return kMinProbability +
value * ((kMaxProbability - kMinProbability) / 255.f);
}
};
// Converts a HybridGrid to a PrecomputationGrid representing the same data,
// Converts a HybridGrid to a PrecomputationGrid3D representing the same data,
// but only using 8 bit instead of 2 x 16 bit.
PrecomputationGrid ConvertToPrecomputationGrid(
const mapping::HybridGrid& hybrid_grid);
PrecomputationGrid3D ConvertToPrecomputationGrid(const HybridGrid& hybrid_grid);
// Returns a grid of the same resolution containing the maximum value of
// original voxels in 'grid'. This maximum is over the 8 voxels that have
@ -47,12 +45,12 @@ PrecomputationGrid ConvertToPrecomputationGrid(
// If 'shift' is 2 ** (depth - 1), where depth 0 is the original grid, and this
// is using the precomputed grid of one depth before, this results in
// precomputation grids analogous to the 2D case.
PrecomputationGrid PrecomputeGrid(const PrecomputationGrid& grid,
PrecomputationGrid3D PrecomputeGrid(const PrecomputationGrid3D& grid,
bool half_resolution,
const Eigen::Array3i& shift);
} // namespace scan_matching
} // namespace mapping_3d
} // namespace mapping
} // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_PRECOMPUTATION_GRID_H_
#endif // CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_PRECOMPUTATION_GRID_3D_H_

18
cartographer/mapping_3d/scan_matching/precomputation_grid_test.cc → cartographer/mapping_3d/scan_matching/precomputation_grid_3d_test.cc
View File

@ -14,7 +14,7 @@
* limitations under the License.
*/
#include "cartographer/mapping_3d/scan_matching/precomputation_grid.h"
#include "cartographer/mapping_3d/scan_matching/precomputation_grid_3d.h"
#include <random>
#include <tuple>
@ -24,17 +24,17 @@
#include "gmock/gmock.h"
namespace cartographer {
namespace mapping_3d {
namespace mapping {
namespace scan_matching {
namespace {
TEST(PrecomputedGridGeneratorTest, TestAgainstNaiveAlgorithm) {
mapping::HybridGrid hybrid_grid(2.f);
TEST(PrecomputedGridGenerator3DTest, TestAgainstNaiveAlgorithm) {
HybridGrid hybrid_grid(2.f);
std::mt19937 rng(23847);
std::uniform_int_distribution<int> coordinate_distribution(-50, 49);
std::uniform_real_distribution<float> value_distribution(
mapping::kMinProbability, mapping::kMaxProbability);
std::uniform_real_distribution<float> value_distribution(kMinProbability,
kMaxProbability);
for (int i = 0; i < 1000; ++i) {
const auto x = coordinate_distribution(rng);
const auto y = coordinate_distribution(rng);
@ -43,7 +43,7 @@ TEST(PrecomputedGridGeneratorTest, TestAgainstNaiveAlgorithm) {
hybrid_grid.SetProbability(cell_index, value_distribution(rng));
}
std::vector<PrecomputationGrid> precomputed_grids;
std::vector<PrecomputationGrid3D> precomputed_grids;
for (int depth = 0; depth <= 3; ++depth) {
if (depth == 0) {
precomputed_grids.push_back(ConvertToPrecomputationGrid(hybrid_grid));
@ -69,7 +69,7 @@ TEST(PrecomputedGridGeneratorTest, TestAgainstNaiveAlgorithm) {
}
EXPECT_NEAR(max_probability,
PrecomputationGrid::ToProbability(
PrecomputationGrid3D::ToProbability(
precomputed_grids.back().value(Eigen::Array3i(x, y, z))),
1e-2);
}
@ -78,5 +78,5 @@ TEST(PrecomputedGridGeneratorTest, TestAgainstNaiveAlgorithm) {
} // namespace
} // namespace scan_matching
} // namespace mapping_3d
} // namespace mapping
} // namespace cartographer

4
cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.proto → cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options_3d.proto
View File

@ -14,12 +14,12 @@
syntax = "proto3";
package cartographer.mapping_3d.scan_matching.proto;
package cartographer.mapping.scan_matching.proto;
import "cartographer/common/proto/ceres_solver_options.proto";
// NEXT ID: 7
message CeresScanMatcherOptions {
message CeresScanMatcherOptions3D {
// Scaling parameters for each cost functor.
repeated double occupied_space_weight = 1;
double translation_weight = 2;

4
cartographer/mapping_3d/scan_matching/proto/fast_correlative_scan_matcher_options.proto → cartographer/mapping_3d/scan_matching/proto/fast_correlative_scan_matcher_options_3d.proto
View File

@ -14,9 +14,9 @@
syntax = "proto3";
package cartographer.mapping_3d.scan_matching.proto;
package cartographer.mapping.scan_matching.proto;
message FastCorrelativeScanMatcherOptions {
message FastCorrelativeScanMatcherOptions3D {
// Number of precomputed grids to use.
int32 branch_and_bound_depth = 2;

22
cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher.cc → cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher_3d.cc
View File

@ -14,7 +14,7 @@
* limitations under the License.
*/
#include "cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher.h"
#include "cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher_3d.h"
#include <cmath>
@ -24,18 +24,16 @@
#include "glog/logging.h"
namespace cartographer {
namespace mapping_3d {
namespace mapping {
namespace scan_matching {
RealTimeCorrelativeScanMatcher::RealTimeCorrelativeScanMatcher(
const mapping::scan_matching::proto::RealTimeCorrelativeScanMatcherOptions&
options)
RealTimeCorrelativeScanMatcher3D::RealTimeCorrelativeScanMatcher3D(
const proto::RealTimeCorrelativeScanMatcherOptions& options)
: options_(options) {}
float RealTimeCorrelativeScanMatcher::Match(
float RealTimeCorrelativeScanMatcher3D::Match(
const transform::Rigid3d& initial_pose_estimate,
const sensor::PointCloud& point_cloud,
const mapping::HybridGrid& hybrid_grid,
const sensor::PointCloud& point_cloud, const HybridGrid& hybrid_grid,
transform::Rigid3d* pose_estimate) const {
CHECK_NOTNULL(pose_estimate);
float best_score = -1.f;
@ -55,7 +53,7 @@ float RealTimeCorrelativeScanMatcher::Match(
}
std::vector<transform::Rigid3f>
RealTimeCorrelativeScanMatcher::GenerateExhaustiveSearchTransforms(
RealTimeCorrelativeScanMatcher3D::GenerateExhaustiveSearchTransforms(
const float resolution, const sensor::PointCloud& point_cloud) const {
std::vector<transform::Rigid3f> result;
const int linear_window_size =
@ -96,8 +94,8 @@ RealTimeCorrelativeScanMatcher::GenerateExhaustiveSearchTransforms(
return result;
}
float RealTimeCorrelativeScanMatcher::ScoreCandidate(
const mapping::HybridGrid& hybrid_grid,
float RealTimeCorrelativeScanMatcher3D::ScoreCandidate(
const HybridGrid& hybrid_grid,
const sensor::PointCloud& transformed_point_cloud,
const transform::Rigid3f& transform) const {
float score = 0.f;
@ -115,5 +113,5 @@ float RealTimeCorrelativeScanMatcher::ScoreCandidate(
}
} // namespace scan_matching
} // namespace mapping_3d
} // namespace mapping
} // namespace cartographer

35
cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher.h → cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher_3d.h
View File

@ -14,10 +14,8 @@
* limitations under the License.
*/
// A voxel accurate scan matcher, exhaustively evaluating the scan matching
// search space.
#ifndef CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_REAL_TIME_CORRELATIVE_SCAN_MATCHER_H_
#define CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_REAL_TIME_CORRELATIVE_SCAN_MATCHER_H_
#ifndef CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_REAL_TIME_CORRELATIVE_SCAN_MATCHER_3D_H_
#define CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_REAL_TIME_CORRELATIVE_SCAN_MATCHER_3D_H_
#include <vector>
@ -27,41 +25,42 @@
#include "cartographer/sensor/point_cloud.h"
namespace cartographer {
namespace mapping_3d {
namespace mapping {
namespace scan_matching {
class RealTimeCorrelativeScanMatcher {
// A voxel accurate scan matcher, exhaustively evaluating the scan matching
// search space.
class RealTimeCorrelativeScanMatcher3D {
public:
explicit RealTimeCorrelativeScanMatcher(
const mapping::scan_matching::proto::
RealTimeCorrelativeScanMatcherOptions& options);
explicit RealTimeCorrelativeScanMatcher3D(
const scan_matching::proto::RealTimeCorrelativeScanMatcherOptions&
options);
RealTimeCorrelativeScanMatcher(const RealTimeCorrelativeScanMatcher&) =
RealTimeCorrelativeScanMatcher3D(const RealTimeCorrelativeScanMatcher3D&) =
delete;
RealTimeCorrelativeScanMatcher& operator=(
const RealTimeCorrelativeScanMatcher&) = delete;
RealTimeCorrelativeScanMatcher3D& operator=(
const RealTimeCorrelativeScanMatcher3D&) = delete;
// Aligns 'point_cloud' within the 'hybrid_grid' given an
// 'initial_pose_estimate' then updates 'pose_estimate' with the result and
// returns the score.
float Match(const transform::Rigid3d& initial_pose_estimate,
const sensor::PointCloud& point_cloud,
const mapping::HybridGrid& hybrid_grid,
const HybridGrid& hybrid_grid,
transform::Rigid3d* pose_estimate) const;
private:
std::vector<transform::Rigid3f> GenerateExhaustiveSearchTransforms(
float resolution, const sensor::PointCloud& point_cloud) const;
float ScoreCandidate(const mapping::HybridGrid& hybrid_grid,
float ScoreCandidate(const HybridGrid& hybrid_grid,
const sensor::PointCloud& transformed_point_cloud,
const transform::Rigid3f& transform) const;
const mapping::scan_matching::proto::RealTimeCorrelativeScanMatcherOptions
options_;
const proto::RealTimeCorrelativeScanMatcherOptions options_;
};
} // namespace scan_matching
} // namespace mapping_3d
} // namespace mapping
} // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_REAL_TIME_CORRELATIVE_SCAN_MATCHER_H_
#endif // CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_REAL_TIME_CORRELATIVE_SCAN_MATCHER_3D_H_

32
cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher_test.cc → cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher_3d_test.cc
View File

@ -14,7 +14,7 @@
* limitations under the License.
*/
#include "cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher.h"
#include "cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher_3d.h"
#include <memory>
@ -29,13 +29,13 @@
#include "gtest/gtest.h"
namespace cartographer {
namespace mapping_3d {
namespace mapping {
namespace scan_matching {
namespace {
class RealTimeCorrelativeScanMatcherTest : public ::testing::Test {
class RealTimeCorrelativeScanMatcher3DTest : public ::testing::Test {
protected:
RealTimeCorrelativeScanMatcherTest()
RealTimeCorrelativeScanMatcher3DTest()
: hybrid_grid_(0.1f),
expected_pose_(Eigen::Vector3d(-1., 0., 0.),
Eigen::Quaterniond::Identity()) {
@ -57,8 +57,8 @@ class RealTimeCorrelativeScanMatcherTest : public ::testing::Test {
rotation_delta_cost_weight = 1.,
})text");
real_time_correlative_scan_matcher_.reset(
new RealTimeCorrelativeScanMatcher(
mapping::scan_matching::CreateRealTimeCorrelativeScanMatcherOptions(
new RealTimeCorrelativeScanMatcher3D(
CreateRealTimeCorrelativeScanMatcherOptions(
parameter_dictionary.get())));
}
@ -71,46 +71,46 @@ class RealTimeCorrelativeScanMatcherTest : public ::testing::Test {
EXPECT_THAT(pose, transform::IsNearly(expected_pose_, 1e-3));
}
mapping::HybridGrid hybrid_grid_;
HybridGrid hybrid_grid_;
transform::Rigid3d expected_pose_;
sensor::PointCloud point_cloud_;
std::unique_ptr<RealTimeCorrelativeScanMatcher>
std::unique_ptr<RealTimeCorrelativeScanMatcher3D>
real_time_correlative_scan_matcher_;
};
TEST_F(RealTimeCorrelativeScanMatcherTest, PerfectEstimate) {
TEST_F(RealTimeCorrelativeScanMatcher3DTest, PerfectEstimate) {
TestFromInitialPose(
transform::Rigid3d::Translation(Eigen::Vector3d(-1., 0., 0.)));
}
TEST_F(RealTimeCorrelativeScanMatcherTest, AlongX) {
TEST_F(RealTimeCorrelativeScanMatcher3DTest, AlongX) {
TestFromInitialPose(
transform::Rigid3d::Translation(Eigen::Vector3d(-0.8, 0., 0.)));
}
TEST_F(RealTimeCorrelativeScanMatcherTest, AlongZ) {
TEST_F(RealTimeCorrelativeScanMatcher3DTest, AlongZ) {
TestFromInitialPose(
transform::Rigid3d::Translation(Eigen::Vector3d(-1., 0., -0.2)));
}
TEST_F(RealTimeCorrelativeScanMatcherTest, AlongXYZ) {
TEST_F(RealTimeCorrelativeScanMatcher3DTest, AlongXYZ) {
TestFromInitialPose(
transform::Rigid3d::Translation(Eigen::Vector3d(-0.9, -0.2, 0.2)));
}
TEST_F(RealTimeCorrelativeScanMatcherTest, RotationAroundX) {
TEST_F(RealTimeCorrelativeScanMatcher3DTest, RotationAroundX) {
TestFromInitialPose(transform::Rigid3d(
Eigen::Vector3d(-1., 0., 0.),
Eigen::AngleAxisd(0.8 / 180. * M_PI, Eigen::Vector3d(1., 0., 0.))));
}
TEST_F(RealTimeCorrelativeScanMatcherTest, RotationAroundY) {
TEST_F(RealTimeCorrelativeScanMatcher3DTest, RotationAroundY) {
TestFromInitialPose(transform::Rigid3d(
Eigen::Vector3d(-1., 0., 0.),
Eigen::AngleAxisd(0.8 / 180. * M_PI, Eigen::Vector3d(0., 1., 0.))));
}
TEST_F(RealTimeCorrelativeScanMatcherTest, RotationAroundYZ) {
TEST_F(RealTimeCorrelativeScanMatcher3DTest, RotationAroundYZ) {
TestFromInitialPose(transform::Rigid3d(
Eigen::Vector3d(-1., 0., 0.),
Eigen::AngleAxisd(0.8 / 180. * M_PI, Eigen::Vector3d(0., 1., 1.))));
@ -118,5 +118,5 @@ TEST_F(RealTimeCorrelativeScanMatcherTest, RotationAroundYZ) {
} // namespace
} // namespace scan_matching
} // namespace mapping_3d
} // namespace mapping
} // namespace cartographer

26
cartographer/mapping_3d/scan_matching/rotation_delta_cost_functor.h → cartographer/mapping_3d/scan_matching/rotation_delta_cost_functor_3d.h
View File

@ -14,8 +14,8 @@
* limitations under the License.
*/
#ifndef CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_ROTATION_DELTA_COST_FUNCTOR_H_
#define CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_ROTATION_DELTA_COST_FUNCTOR_H_
#ifndef CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_ROTATION_DELTA_COST_FUNCTOR_3D_H_
#define CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_ROTATION_DELTA_COST_FUNCTOR_3D_H_
#include <cmath>
@ -24,19 +24,19 @@
#include "ceres/rotation.h"
namespace cartographer {
namespace mapping_3d {
namespace mapping {
namespace scan_matching {
// Computes the cost of rotating 'rotation_quaternion' to 'target_rotation'.
// Cost increases with the solution's distance from 'target_rotation'.
class RotationDeltaCostFunctor {
class RotationDeltaCostFunctor3D {
public:
static ceres::CostFunction* CreateAutoDiffCostFunction(
const double scaling_factor, const Eigen::Quaterniond& target_rotation) {
return new ceres::AutoDiffCostFunction<RotationDeltaCostFunctor,
return new ceres::AutoDiffCostFunction<RotationDeltaCostFunctor3D,
3 /* residuals */,
4 /* rotation variables */>(
new RotationDeltaCostFunctor(scaling_factor, target_rotation));
new RotationDeltaCostFunctor3D(scaling_factor, target_rotation));
}
template <typename T>
@ -56,8 +56,9 @@ class RotationDeltaCostFunctor {
}
private:
// Constructs a new RotationDeltaCostFunctor from the given 'target_rotation'.
explicit RotationDeltaCostFunctor(const double scaling_factor,
// Constructs a new RotationDeltaCostFunctor3D from the given
// 'target_rotation'.
explicit RotationDeltaCostFunctor3D(const double scaling_factor,
const Eigen::Quaterniond& target_rotation)
: scaling_factor_(scaling_factor) {
target_rotation_inverse_[0] = target_rotation.w();
@ -66,15 +67,16 @@ class RotationDeltaCostFunctor {
target_rotation_inverse_[3] = -target_rotation.z();
}
RotationDeltaCostFunctor(const RotationDeltaCostFunctor&) = delete;
RotationDeltaCostFunctor& operator=(const RotationDeltaCostFunctor&) = delete;
RotationDeltaCostFunctor3D(const RotationDeltaCostFunctor3D&) = delete;
RotationDeltaCostFunctor3D& operator=(const RotationDeltaCostFunctor3D&) =
delete;
const double scaling_factor_;
double target_rotation_inverse_[4];
};
} // namespace scan_matching
} // namespace mapping_3d
} // namespace mapping
} // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_ROTATION_DELTA_COST_FUNCTOR_H_
#endif // CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_ROTATION_DELTA_COST_FUNCTOR_3D_H_

12
cartographer/mapping_3d/scan_matching/rotation_delta_cost_functor_test.cc → cartographer/mapping_3d/scan_matching/rotation_delta_cost_functor_3d_test.cc
View File

@ -14,12 +14,12 @@
* limitations under the License.
*/
#include "cartographer/mapping_3d/scan_matching/rotation_delta_cost_functor.h"
#include "cartographer/mapping_3d/scan_matching/rotation_delta_cost_functor_3d.h"
#include "gtest/gtest.h"
namespace cartographer {
namespace mapping_3d {
namespace mapping {
namespace scan_matching {
namespace {
@ -29,7 +29,7 @@ double ComputeRotationDeltaSquaredCost(
const Eigen::Quaterniond& rotation, const double scaling_factor,
const Eigen::Quaterniond& target_rotation) {
std::unique_ptr<ceres::CostFunction> cost_function(
RotationDeltaCostFunctor::CreateAutoDiffCostFunction(scaling_factor,
RotationDeltaCostFunctor3D::CreateAutoDiffCostFunction(scaling_factor,
target_rotation));
const std::array<double, 4> parameter_quaternion = {
{rotation.w(), rotation.x(), rotation.y(), rotation.z()}};
@ -44,7 +44,7 @@ double ComputeRotationDeltaSquaredCost(
return sum_of_squares;
}
TEST(RotationDeltaCostFunctorTest, SameRotationGivesZeroCost) {
TEST(RotationDeltaCostFunctor3DTest, SameRotationGivesZeroCost) {
EXPECT_NEAR(
0.,
ComputeRotationDeltaSquaredCost(Eigen::Quaterniond::Identity(), 1.0,
@ -57,7 +57,7 @@ TEST(RotationDeltaCostFunctorTest, SameRotationGivesZeroCost) {
kPrecision);
}
TEST(RotationDeltaCostFunctorTest, ComputesCorrectCost) {
TEST(RotationDeltaCostFunctor3DTest, ComputesCorrectCost) {
double scaling_factor = 1.2;
double angle = 0.8;
Eigen::Quaterniond rotation(
@ -81,5 +81,5 @@ TEST(RotationDeltaCostFunctorTest, ComputesCorrectCost) {
} // namespace
} // namespace scan_matching
} // namespace mapping_3d
} // namespace mapping
} // namespace cartographer

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

@ -23,7 +23,7 @@
#include "cartographer/common/port.h"
namespace cartographer {
namespace mapping_3d {
namespace mapping {
namespace scan_matching {
namespace {
@ -190,5 +190,5 @@ std::vector<float> RotationalScanMatcher::Match(
}
} // namespace scan_matching
} // namespace mapping_3d
} // namespace mapping
} // namespace cartographer

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

@ -23,7 +23,7 @@
#include "cartographer/sensor/point_cloud.h"
namespace cartographer {
namespace mapping_3d {
namespace mapping {
namespace scan_matching {
class RotationalScanMatcher {
@ -51,7 +51,7 @@ class RotationalScanMatcher {
};
} // namespace scan_matching
} // namespace mapping_3d
} // namespace mapping
} // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_ROTATIONAL_SCAN_MATCHER_H_

8
cartographer/mapping_3d/scan_matching/rotational_scan_matcher_test.cc
View File

@ -21,11 +21,11 @@
#include "gtest/gtest.h"
namespace cartographer {
namespace mapping_3d {
namespace mapping {
namespace scan_matching {
namespace {
TEST(RotationalScanMatcherTest, OnlySameHistogramIsScoreOne) {
TEST(RotationalScanMatcher3DTest, OnlySameHistogramIsScoreOne) {
Eigen::VectorXf histogram(7);
histogram << 1.f, 43.f, 0.5f, 0.3123f, 23.f, 42.f, 0.f;
RotationalScanMatcher matcher({{histogram, 0.f}});
@ -35,7 +35,7 @@ TEST(RotationalScanMatcherTest, OnlySameHistogramIsScoreOne) {
EXPECT_GT(1.f, scores[1]);
}
TEST(RotationalScanMatcherTest, InterpolatesAsExpected) {
TEST(RotationalScanMatcher3DTest, InterpolatesAsExpected) {
constexpr int kNumBuckets = 10;
constexpr float kAnglePerBucket = M_PI / kNumBuckets;
constexpr float kNoInitialRotation = 0.f;
@ -67,5 +67,5 @@ TEST(RotationalScanMatcherTest, InterpolatesAsExpected) {
} // namespace
} // namespace scan_matching
} // namespace mapping_3d
} // namespace mapping
} // namespace cartographer

26
cartographer/mapping_3d/scan_matching/translation_delta_cost_functor.h → cartographer/mapping_3d/scan_matching/translation_delta_cost_functor_3d.h
View File

@ -14,25 +14,25 @@
* limitations under the License.
*/
#ifndef CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_TRANSLATION_DELTA_COST_FUNCTOR_H_
#define CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_TRANSLATION_DELTA_COST_FUNCTOR_H_
#ifndef CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_TRANSLATION_DELTA_COST_FUNCTOR_3D_H_
#define CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_TRANSLATION_DELTA_COST_FUNCTOR_3D_H_
#include "Eigen/Core"
namespace cartographer {
namespace mapping_3d {
namespace mapping {
namespace scan_matching {
// Computes the cost of translating 'translation' to 'target_translation'.
// Cost increases with the solution's distance from 'target_translation'.
class TranslationDeltaCostFunctor {
class TranslationDeltaCostFunctor3D {
public:
static ceres::CostFunction* CreateAutoDiffCostFunction(
const double scaling_factor, const Eigen::Vector3d& target_translation) {
return new ceres::AutoDiffCostFunction<TranslationDeltaCostFunctor,
return new ceres::AutoDiffCostFunction<TranslationDeltaCostFunctor3D,
3 /* residuals */,
3 /* translation variables */>(
new TranslationDeltaCostFunctor(scaling_factor, target_translation));
new TranslationDeltaCostFunctor3D(scaling_factor, target_translation));
}
template <typename T>
@ -44,18 +44,18 @@ class TranslationDeltaCostFunctor {
}
private:
// Constructs a new TranslationDeltaCostFunctor from the given
// Constructs a new TranslationDeltaCostFunctor3D from the given
// 'target_translation'.
explicit TranslationDeltaCostFunctor(
explicit TranslationDeltaCostFunctor3D(
const double scaling_factor, const Eigen::Vector3d& target_translation)
: scaling_factor_(scaling_factor),
x_(target_translation.x()),
y_(target_translation.y()),
z_(target_translation.z()) {}
TranslationDeltaCostFunctor(const TranslationDeltaCostFunctor&) = delete;
TranslationDeltaCostFunctor& operator=(const TranslationDeltaCostFunctor&) =
delete;
TranslationDeltaCostFunctor3D(const TranslationDeltaCostFunctor3D&) = delete;
TranslationDeltaCostFunctor3D& operator=(
const TranslationDeltaCostFunctor3D&) = delete;
const double scaling_factor_;
const double x_;
@ -64,7 +64,7 @@ class TranslationDeltaCostFunctor {
};
} // namespace scan_matching
} // namespace mapping_3d
} // namespace mapping
} // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_TRANSLATION_DELTA_COST_FUNCTOR_H_
#endif // CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_TRANSLATION_DELTA_COST_FUNCTOR_3D_H_

4
cartographer/metrics/register.cc
View File

@ -17,14 +17,14 @@
#include "cartographer/metrics/register.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_3d.h"
namespace cartographer {
namespace metrics {
void RegisterAllMetrics(FamilyFactory* registry) {
mapping::pose_graph::ConstraintBuilder2D::RegisterMetrics(registry);
mapping_3d::pose_graph::ConstraintBuilder::RegisterMetrics(registry);
mapping::pose_graph::ConstraintBuilder3D::RegisterMetrics(registry);
}
} // namespace metrics