Skip to content

Commit 3ad3899

Browse files
zjwoodyzjwoody
committed
Use Manifold instead of LocalParameterization.
1 parent b8228ee commit 3ad3899

8 files changed

+121
-42
lines changed

‎cartographer/mapping/internal/3d/rotation_parameterization.h

+38-4
Original file line numberDiff line numberDiff line change
@@ -24,9 +24,11 @@
2424
namespace cartographer {
2525
namespace mapping {
2626

27-
struct YawOnlyQuaternionPlus {
27+
// Provides operations used to create a Ceres Manifold with a 4-D ambient
28+
// space and a 1-D tangent space that represents a yaw rotation only.
29+
struct YawOnlyQuaternionOperations {
2830
template <typename T>
29-
bool operator()(const T* x, const T* delta, T* x_plus_delta) const {
31+
bool Plus(const T* x, const T* delta, T* x_plus_delta) const {
3032
const T clamped_delta = common::Clamp(delta[0], T(-0.5), T(0.5));
3133
T q_delta[4];
3234
q_delta[0] = ceres::sqrt(1. - clamped_delta * clamped_delta);
@@ -36,11 +38,22 @@ struct YawOnlyQuaternionPlus {
3638
ceres::QuaternionProduct(q_delta, x, x_plus_delta);
3739
return true;
3840
}
41+
template <typename T>
42+
bool Minus(const T* y, const T* x, T* y_minus_x) const {
43+
T minus_x[4] = {x[0], -x[1], -x[2], -x[3]};
44+
T q_delta[4];
45+
ceres::QuaternionProduct(y, minus_x, q_delta);
46+
y_minus_x[0] = q_delta[3];
47+
return true;
48+
}
3949
};
4050

41-
struct ConstantYawQuaternionPlus {
51+
// Provides operations used to create a Ceres Manifold with a 4-D ambient
52+
// space and a 2-D tangent space that represents a rotation only in pitch and
53+
// roll, but no yaw.
54+
struct ConstantYawQuaternionOperations {
4255
template <typename T>
43-
bool operator()(const T* x, const T* delta, T* x_plus_delta) const {
56+
bool Plus(const T* x, const T* delta, T* x_plus_delta) const {
4457
const T delta_norm =
4558
ceres::sqrt(common::Pow2(delta[0]) + common::Pow2(delta[1]));
4659
const T sin_delta_over_delta =
@@ -59,8 +72,29 @@ struct ConstantYawQuaternionPlus {
5972
ceres::QuaternionProduct(x, q_delta, x_plus_delta);
6073
return true;
6174
}
75+
template <typename T>
76+
bool Minus(const T* y, const T* x, T* y_minus_x) const {
77+
T minus_x[4] = {x[0], -x[1], -x[2], -x[3]};
78+
T q_delta[4];
79+
ceres::QuaternionProduct(minus_x, y, q_delta);
80+
const T& cos_delta_norm = q_delta[0];
81+
const T sin_delta_norm =
82+
ceres::sqrt(common::Pow2(q_delta[1]) + common::Pow2(q_delta[2]));
83+
const T delta_norm = atan2(sin_delta_norm, cos_delta_norm);
84+
const T delta_over_sin_delta =
85+
delta_norm < 1e-6 ? T(1.) : delta_norm / sin_delta_norm;
86+
y_minus_x[0] = q_delta[1] * delta_over_sin_delta;
87+
y_minus_x[1] = q_delta[2] * delta_over_sin_delta;
88+
return true;
89+
}
6290
};
6391

92+
// Type aliases used to create manifolds.
93+
using YawOnlyQuaternionManifold =
94+
ceres::AutoDiffManifold<YawOnlyQuaternionOperations, 4, 1>;
95+
using ConstantYawQuaternionManifold =
96+
ceres::AutoDiffManifold<ConstantYawQuaternionOperations, 4, 2>;
97+
6498
} // namespace mapping
6599
} // namespace cartographer
66100

‎cartographer/mapping/internal/3d/rotation_parameterization_test.cc

+48
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,48 @@
1+
#include "cartographer/mapping/internal/3d/rotation_parameterization.h"
2+
3+
#include "ceres/manifold_test_utils.h"
4+
#include "gtest/gtest.h"
5+
6+
namespace cartographer::mapping {
7+
8+
template <typename T>
9+
class RotationParameterizationTests : public ::testing::Test {};
10+
11+
using TestTypes =
12+
::testing::Types<YawOnlyQuaternionManifold, ConstantYawQuaternionManifold>;
13+
TYPED_TEST_SUITE(RotationParameterizationTests, TestTypes);
14+
15+
TYPED_TEST(RotationParameterizationTests, ManifoldInvariantsHold) {
16+
const TypeParam manifold;
17+
18+
constexpr static int kNumTrials = 10;
19+
constexpr static double kTolerance = 1.e-5;
20+
const std::vector<double> delta_magnitutes = {0.0, 1.e-9, 1.e-3, 0.5};
21+
for (int trial = 0; trial < kNumTrials; ++trial) {
22+
const Eigen::VectorXd x =
23+
Eigen::VectorXd::Random(manifold.AmbientSize()).normalized();
24+
25+
for (const double delta_magnitude : delta_magnitutes) {
26+
const Eigen::VectorXd delta =
27+
Eigen::VectorXd::Random(manifold.TangentSize()) * delta_magnitude;
28+
EXPECT_THAT(manifold, ceres::XPlusZeroIsXAt(x, kTolerance));
29+
EXPECT_THAT(manifold, ceres::XMinusXIsZeroAt(x, kTolerance));
30+
EXPECT_THAT(manifold, ceres::MinusPlusIsIdentityAt(x, delta, kTolerance));
31+
const Eigen::VectorXd zero_tangent =
32+
Eigen::VectorXd::Zero(manifold.TangentSize());
33+
EXPECT_THAT(manifold,
34+
ceres::MinusPlusIsIdentityAt(x, zero_tangent, kTolerance));
35+
36+
Eigen::VectorXd y(manifold.AmbientSize());
37+
ASSERT_TRUE(manifold.Plus(x.data(), delta.data(), y.data()));
38+
EXPECT_THAT(manifold, ceres::PlusMinusIsIdentityAt(x, x, kTolerance));
39+
EXPECT_THAT(manifold, ceres::PlusMinusIsIdentityAt(x, y, kTolerance));
40+
EXPECT_THAT(manifold, ceres::HasCorrectPlusJacobianAt(x, kTolerance));
41+
EXPECT_THAT(manifold, ceres::HasCorrectMinusJacobianAt(x, kTolerance));
42+
EXPECT_THAT(manifold,
43+
ceres::MinusPlusJacobianIsIdentityAt(x, kTolerance));
44+
}
45+
}
46+
}
47+
48+
} // namespace cartographer::mapping

‎cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d.cc

+5-5
Original file line numberDiff line numberDiff line change
@@ -31,6 +31,7 @@
3131
#include "cartographer/transform/rigid_transform.h"
3232
#include "cartographer/transform/transform.h"
3333
#include "ceres/ceres.h"
34+
#include "ceres/manifold.h"
3435
#include "glog/logging.h"
3536

3637
namespace cartographer {
@@ -98,11 +99,10 @@ void CeresScanMatcher3D::Match(
9899
optimization::CeresPose ceres_pose(
99100
initial_pose_estimate, nullptr /* translation_parameterization */,
100101
options_.only_optimize_yaw()
101-
? std::unique_ptr<ceres::LocalParameterization>(
102-
absl::make_unique<ceres::AutoDiffLocalParameterization<
103-
YawOnlyQuaternionPlus, 4, 1>>())
104-
: std::unique_ptr<ceres::LocalParameterization>(
105-
absl::make_unique<ceres::QuaternionParameterization>()),
102+
? std::unique_ptr<ceres::Manifold>(
103+
absl::make_unique<YawOnlyQuaternionManifold>())
104+
: std::unique_ptr<ceres::Manifold>(
105+
absl::make_unique<ceres::QuaternionManifold>()),
106106
&problem);
107107

108108
CHECK_EQ(options_.occupied_space_weight_size(),

‎cartographer/mapping/internal/imu_based_pose_extrapolator.cc

+6-6
Original file line numberDiff line numberDiff line change
@@ -28,6 +28,7 @@
2828
#include "cartographer/mapping/internal/optimization/cost_functions/spa_cost_function_3d.h"
2929
#include "cartographer/mapping/pose_graph_interface.h"
3030
#include "cartographer/transform/transform.h"
31+
#include "ceres/manifold.h"
3132
#include "glog/logging.h"
3233

3334
namespace cartographer {
@@ -137,7 +138,7 @@ ImuBasedPoseExtrapolator::ExtrapolatePosesWithGravity(
137138
// we can estimate the gravity alignment of the current pose.
138139
optimization::CeresPose gravity_from_local(
139140
gravity_from_local_, nullptr,
140-
absl::make_unique<ceres::QuaternionParameterization>(), &problem);
141+
absl::make_unique<ceres::QuaternionManifold>(), &problem);
141142
// Use deque so addresses stay constant during problem formulation.
142143
std::deque<optimization::CeresPose> nodes;
143144
std::vector<common::Time> node_times;
@@ -162,13 +163,12 @@ ImuBasedPoseExtrapolator::ExtrapolatePosesWithGravity(
162163

163164
if (is_last) {
164165
nodes.emplace_back(gravity_from_node, nullptr,
165-
absl::make_unique<ceres::AutoDiffLocalParameterization<
166-
ConstantYawQuaternionPlus, 4, 2>>(),
166+
absl::make_unique<ConstantYawQuaternionManifold>(),
167167
&problem);
168168
problem.SetParameterBlockConstant(nodes.back().translation());
169169
} else {
170170
nodes.emplace_back(gravity_from_node, nullptr,
171-
absl::make_unique<ceres::QuaternionParameterization>(),
171+
absl::make_unique<ceres::QuaternionManifold>(),
172172
&problem);
173173
}
174174
}
@@ -200,7 +200,7 @@ ImuBasedPoseExtrapolator::ExtrapolatePosesWithGravity(
200200
&imu_it_prev_prev)
201201
.pose;
202202
nodes.emplace_back(initial_estimate, nullptr,
203-
absl::make_unique<ceres::QuaternionParameterization>(),
203+
absl::make_unique<ceres::QuaternionManifold>(),
204204
&problem);
205205
node_times.push_back(time);
206206

@@ -223,7 +223,7 @@ ImuBasedPoseExtrapolator::ExtrapolatePosesWithGravity(
223223
std::array<double, 4> imu_calibration{{1., 0., 0., 0.}};
224224

225225
problem.AddParameterBlock(imu_calibration.data(), 4,
226-
new ceres::QuaternionParameterization());
226+
new ceres::QuaternionManifold());
227227
problem.SetParameterBlockConstant(imu_calibration.data());
228228

229229
auto imu_it = imu_data_.begin();

‎cartographer/mapping/internal/optimization/ceres_pose.cc

+4-4
Original file line numberDiff line numberDiff line change
@@ -29,14 +29,14 @@ CeresPose::Data FromPose(const transform::Rigid3d& pose) {
2929

3030
CeresPose::CeresPose(
3131
const transform::Rigid3d& pose,
32-
std::unique_ptr<ceres::LocalParameterization> translation_parametrization,
33-
std::unique_ptr<ceres::LocalParameterization> rotation_parametrization,
32+
std::unique_ptr<ceres::Manifold> translation_manifold,
33+
std::unique_ptr<ceres::Manifold> rotation_manifold,
3434
ceres::Problem* problem)
3535
: data_(std::make_shared<CeresPose::Data>(FromPose(pose))) {
3636
problem->AddParameterBlock(data_->translation.data(), 3,
37-
translation_parametrization.release());
37+
translation_manifold.release());
3838
problem->AddParameterBlock(data_->rotation.data(), 4,
39-
rotation_parametrization.release());
39+
rotation_manifold.release());
4040
}
4141

4242
const transform::Rigid3d CeresPose::ToRigid() const {

‎cartographer/mapping/internal/optimization/ceres_pose.h

+4-3
Original file line numberDiff line numberDiff line change
@@ -23,6 +23,7 @@
2323
#include "Eigen/Core"
2424
#include "cartographer/transform/rigid_transform.h"
2525
#include "ceres/ceres.h"
26+
#include "ceres/manifold.h"
2627

2728
namespace cartographer {
2829
namespace mapping {
@@ -31,9 +32,9 @@ namespace optimization {
3132
class CeresPose {
3233
public:
3334
CeresPose(
34-
const transform::Rigid3d& rigid,
35-
std::unique_ptr<ceres::LocalParameterization> translation_parametrization,
36-
std::unique_ptr<ceres::LocalParameterization> rotation_parametrization,
35+
const transform::Rigid3d& pose,
36+
std::unique_ptr<ceres::Manifold> translation_manifold,
37+
std::unique_ptr<ceres::Manifold> rotation_manifold,
3738
ceres::Problem* problem);
3839

3940
const transform::Rigid3d ToRigid() const;

‎cartographer/mapping/internal/optimization/optimization_problem_2d.cc

+2-2
Original file line numberDiff line numberDiff line change
@@ -144,8 +144,8 @@ void AddLandmarkCostFunctions(
144144
*prev_node_pose, *next_node_pose);
145145
C_landmarks->emplace(
146146
landmark_id,
147-
CeresPose(starting_point, nullptr /* translation_parametrization */,
148-
absl::make_unique<ceres::QuaternionParameterization>(),
147+
CeresPose(starting_point, nullptr /* translation_manifold */,
148+
absl::make_unique<ceres::QuaternionManifold>(),
149149
problem));
150150
// Set landmark constant if it is frozen.
151151
if (landmark_node.second.frozen) {

‎cartographer/mapping/internal/optimization/optimization_problem_3d.cc

+14-18
Original file line numberDiff line numberDiff line change
@@ -160,8 +160,8 @@ void AddLandmarkCostFunctions(
160160
*prev_node_pose, *next_node_pose);
161161
C_landmarks->emplace(
162162
landmark_id,
163-
CeresPose(starting_point, nullptr /* translation_parametrization */,
164-
absl::make_unique<ceres::QuaternionParameterization>(),
163+
CeresPose(starting_point, nullptr /* translation_manifold */,
164+
absl::make_unique<ceres::QuaternionManifold>(),
165165
problem));
166166
// Set landmark constant if it is frozen.
167167
if (landmark_node.second.frozen) {
@@ -274,12 +274,11 @@ void OptimizationProblem3D::Solve(
274274
ceres::Problem::Options problem_options;
275275
ceres::Problem problem(problem_options);
276276

277-
const auto translation_parameterization =
278-
[this]() -> std::unique_ptr<ceres::LocalParameterization> {
279-
return options_.fix_z_in_3d()
280-
? absl::make_unique<ceres::SubsetParameterization>(
281-
3, std::vector<int>{2})
282-
: nullptr;
277+
const auto translation_manifold =
278+
[this]() -> std::unique_ptr<ceres::Manifold> {
279+
return options_.fix_z_in_3d() ? absl::make_unique<ceres::SubsetManifold>(
280+
3, std::vector<int>{2})
281+
: nullptr;
283282
};
284283

285284
// Set the starting point.
@@ -298,18 +297,17 @@ void OptimizationProblem3D::Solve(
298297
C_submaps.Insert(
299298
submap_id_data.id,
300299
CeresPose(submap_id_data.data.global_pose,
301-
translation_parameterization(),
302-
absl::make_unique<ceres::AutoDiffLocalParameterization<
303-
ConstantYawQuaternionPlus, 4, 2>>(),
300+
translation_manifold(),
301+
absl::make_unique<ConstantYawQuaternionManifold>(),
304302
&problem));
305303
problem.SetParameterBlockConstant(
306304
C_submaps.at(submap_id_data.id).translation());
307305
} else {
308306
C_submaps.Insert(
309307
submap_id_data.id,
310308
CeresPose(submap_id_data.data.global_pose,
311-
translation_parameterization(),
312-
absl::make_unique<ceres::QuaternionParameterization>(),
309+
translation_manifold(),
310+
absl::make_unique<ceres::QuaternionManifold>(),
313311
&problem));
314312
}
315313
if (frozen) {
@@ -324,8 +322,8 @@ void OptimizationProblem3D::Solve(
324322
frozen_trajectories.count(node_id_data.id.trajectory_id) != 0;
325323
C_nodes.Insert(
326324
node_id_data.id,
327-
CeresPose(node_id_data.data.global_pose, translation_parameterization(),
328-
absl::make_unique<ceres::QuaternionParameterization>(),
325+
CeresPose(node_id_data.data.global_pose, translation_manifold(),
326+
absl::make_unique<ceres::QuaternionManifold>(),
329327
&problem));
330328
if (frozen) {
331329
problem.SetParameterBlockConstant(C_nodes.at(node_id_data.id).rotation());
@@ -544,9 +542,7 @@ void OptimizationProblem3D::Solve(
544542
Eigen::AngleAxisd(
545543
transform::GetYaw(fixed_frame_pose_in_map.rotation()),
546544
Eigen::Vector3d::UnitZ())),
547-
nullptr,
548-
absl::make_unique<ceres::AutoDiffLocalParameterization<
549-
YawOnlyQuaternionPlus, 4, 1>>(),
545+
nullptr, absl::make_unique<YawOnlyQuaternionManifold>(),
550546
&problem));
551547
fixed_frame_pose_initialized = true;
552548
}

0 commit comments

Comments
 (0)