testInitializePose3.cpp
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
21 #include <gtsam/slam/InitializePose3.h>
22 #include <gtsam/slam/dataset.h>
23 #include <gtsam/slam/BetweenFactor.h>
24 #include <gtsam/inference/Symbol.h>
25 #include <gtsam/geometry/Pose3.h>
26 #include <CppUnitLite/TestHarness.h>
27 
28 #include <cmath>
29 
30 using namespace std;
31 using namespace gtsam;
32 
33 static Symbol x0('x', 0), x1('x', 1), x2('x', 2), x3('x', 3);
34 static SharedNoiseModel model(noiseModel::Isotropic::Sigma(6, 0.1));
35 
36 namespace simple {
37 // We consider a small graph:
38 // symbolic FG
39 // x2 0 1
40 // / | \ 1 2
41 // / | \ 2 3
42 // x3 | x1 2 0
43 // \ | / 0 3
44 // \ | /
45 // x0
46 //
47 static Point3 p0 = Point3(0,0,0);
48 static Rot3 R0 = Rot3::Expmap( ( Vector(3) << 0.0,0.0,0.0 ).finished() );
49 static Point3 p1 = Point3(1,2,0);
50 static Rot3 R1 = Rot3::Expmap( ( Vector(3) << 0.0,0.0,1.570796 ).finished() );
51 static Point3 p2 = Point3(0,2,0);
52 static Rot3 R2 = Rot3::Expmap( ( Vector(3) << 0.0,0.0,3.141593 ).finished() );
53 static Point3 p3 = Point3(-1,1,0);
54 static Rot3 R3 = Rot3::Expmap( ( Vector(3) << 0.0,0.0,4.712389 ).finished() );
55 
56 static Pose3 pose0 = Pose3(R0,p0);
57 static Pose3 pose1 = Pose3(R1,p1);
58 static Pose3 pose2 = Pose3(R2,p2);
59 static Pose3 pose3 = Pose3(R3,p3);
60 
61 NonlinearFactorGraph graph() {
62  NonlinearFactorGraph g;
63  g.add(BetweenFactor<Pose3>(x0, x1, pose0.between(pose1), model));
64  g.add(BetweenFactor<Pose3>(x1, x2, pose1.between(pose2), model));
65  g.add(BetweenFactor<Pose3>(x2, x3, pose2.between(pose3), model));
66  g.add(BetweenFactor<Pose3>(x2, x0, pose2.between(pose0), model));
67  g.add(BetweenFactor<Pose3>(x0, x3, pose0.between(pose3), model));
68  g.addPrior(x0, pose0, model);
69  return g;
70 }
71 
72 NonlinearFactorGraph graph2() {
73  NonlinearFactorGraph g;
74  g.add(BetweenFactor<Pose3>(x0, x1, pose0.between(pose1), noiseModel::Isotropic::Precision(6, 1.0)));
75  g.add(BetweenFactor<Pose3>(x1, x2, pose1.between(pose2), noiseModel::Isotropic::Precision(6, 1.0)));
76  g.add(BetweenFactor<Pose3>(x2, x3, pose2.between(pose3), noiseModel::Isotropic::Precision(6, 1.0)));
77  // random pose, but zero information
78  auto noise_zero_info = noiseModel::Isotropic::Precision(6, 0.0);
79  g.add(BetweenFactor<Pose3>(
80  x2, x0, Pose3(Rot3::Ypr(0.1, 0.0, 0.1), Point3(0.0, 0.0, 0.0)),
81  noise_zero_info));
82  // random pose, but zero information
83  g.add(BetweenFactor<Pose3>(
84  x0, x3, Pose3(Rot3::Ypr(0.5, -0.2, 0.2), Point3(10, 20, 30)),
85  noise_zero_info));
86  g.addPrior(x0, pose0, model);
87  return g;
88 }
89 }
90 
91 /* *************************************************************************** */
92 TEST( InitializePose3, buildPose3graph ) {
93  NonlinearFactorGraph pose3graph = InitializePose3::buildPose3graph(simple::graph());
94  // pose3graph.print("");
95 }
96 
97 /* *************************************************************************** */
98 TEST( InitializePose3, orientations ) {
99  NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph());
100 
101  Values initial = InitializePose3::computeOrientationsChordal(pose3Graph);
102 
103  // comparison is up to M_PI, that's why we add some multiples of 2*M_PI
104  EXPECT(assert_equal(simple::R0, initial.at<Rot3>(x0), 1e-6));
105  EXPECT(assert_equal(simple::R1, initial.at<Rot3>(x1), 1e-6));
106  EXPECT(assert_equal(simple::R2, initial.at<Rot3>(x2), 1e-6));
107  EXPECT(assert_equal(simple::R3, initial.at<Rot3>(x3), 1e-6));
108 }
109 
110 /* *************************************************************************** */
111 TEST( InitializePose3, orientationsPrecisions ) {
112  NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph2());
113 
114  Values initial = InitializePose3::computeOrientationsChordal(pose3Graph);
115 
116  // comparison is up to M_PI, that's why we add some multiples of 2*M_PI
117  EXPECT(assert_equal(simple::R0, initial.at<Rot3>(x0), 1e-6));
118  EXPECT(assert_equal(simple::R1, initial.at<Rot3>(x1), 1e-6));
119  EXPECT(assert_equal(simple::R2, initial.at<Rot3>(x2), 1e-6));
120  EXPECT(assert_equal(simple::R3, initial.at<Rot3>(x3), 1e-6));
121 }
122 
123 /* *************************************************************************** */
124 TEST( InitializePose3, orientationsGradientSymbolicGraph ) {
125  NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph());
126 
127  KeyVectorMap adjEdgesMap;
128  KeyRotMap factorId2RotMap;
129 
130  InitializePose3::createSymbolicGraph(pose3Graph, &adjEdgesMap, &factorId2RotMap);
131 
132  EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x0)[0], 0, 1e-9);
133  EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x0)[1], 3, 1e-9);
134  EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x0)[2], 4, 1e-9);
135  EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x0)[3], 5, 1e-9);
136  EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x0).size(), 4, 1e-9);
137 
138  EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x1)[0], 0, 1e-9);
139  EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x1)[1], 1, 1e-9);
140  EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x1).size(), 2, 1e-9);
141 
142  EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x2)[0], 1, 1e-9);
143  EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x2)[1], 2, 1e-9);
144  EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x2)[2], 3, 1e-9);
145  EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x2).size(), 3, 1e-9);
146 
147  EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x3)[0], 2, 1e-9);
148  EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x3)[1], 4, 1e-9);
149  EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x3).size(), 2, 1e-9);
150 
151  // This includes the anchor
152  EXPECT_DOUBLES_EQUAL(adjEdgesMap.size(), 5, 1e-9);
153 }
154 
155 /* *************************************************************************** */
156 TEST( InitializePose3, singleGradient ) {
157  Rot3 R1 = Rot3();
158  Matrix M = Z_3x3;
159  M(0,1) = -1; M(1,0) = 1; M(2,2) = 1;
160  Rot3 R2 = Rot3(M);
161  double a = 6.010534238540223;
162  double b = 1.0;
163 
164  Vector actual = InitializePose3::gradientTron(R1, R2, a, b);
165  Vector expected = Vector3::Zero();
166  expected(2) = 1.962658662803917;
167 
168  // comparison is up to M_PI, that's why we add some multiples of 2*M_PI
169  EXPECT(assert_equal(expected, actual, 1e-6));
170 }
171 
172 /* *************************************************************************** */
173 TEST( InitializePose3, iterationGradient ) {
174  NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph());
175 
176  // Wrong initial guess - initialization should fix the rotations
177  Rot3 Rpert = Rot3::Expmap(Vector3(0.01, 0.01, 0.01));
178  Values givenPoses;
179  givenPoses.insert(x0,simple::pose0);
180  givenPoses.insert(x1,(simple::pose0).compose( Pose3(Rpert,Point3(0,0,0)) ));
181  givenPoses.insert(x2, (simple::pose0).compose( Pose3(Rpert.inverse(),Point3(0,0,0)) ));
182  givenPoses.insert(x3, (simple::pose0).compose( Pose3(Rpert,Point3(0,0,0)) ));
183 
184  size_t maxIter = 1; // test gradient at the first iteration
185  bool setRefFrame = false;
186  Values orientations = InitializePose3::computeOrientationsGradient(pose3Graph, givenPoses, maxIter, setRefFrame);
187 
188  Matrix M0 = (Matrix(3,3) << 0.999435813876064, -0.033571481675497, 0.001004768630281,
189  0.033572116359134, 0.999436104312325, -0.000621610948719,
190  -0.000983333645009, 0.000654992453817, 0.999999302019670).finished();
191  Rot3 R0Expected = Rot3(M0);
192  EXPECT(assert_equal(R0Expected, orientations.at<Rot3>(x0), 1e-5));
193 
194  Matrix M1 = (Matrix(3,3) << 0.999905367545392, -0.010866391403031, 0.008436675399114,
195  0.010943459008004, 0.999898317528125, -0.009143047050380,
196  -0.008336465609239, 0.009234508232789, 0.999922610604863).finished();
197  Rot3 R1Expected = Rot3(M1);
198  EXPECT(assert_equal(R1Expected, orientations.at<Rot3>(x1), 1e-5));
199 
200  Matrix M2 = (Matrix(3,3) << 0.998936644682875, 0.045376417678595, -0.008158469732553,
201  -0.045306446926148, 0.998936408933058, 0.008566024448664,
202  0.008538487960253, -0.008187284445083, 0.999930028850403).finished();
203  Rot3 R2Expected = Rot3(M2);
204  EXPECT(assert_equal(R2Expected, orientations.at<Rot3>(x2), 1e-5));
205 
206  Matrix M3 = (Matrix(3,3) << 0.999898767273093, -0.010834701971459, 0.009223038487275,
207  0.010911315499947, 0.999906044037258, -0.008297366559388,
208  -0.009132272433995, 0.008397162077148, 0.999923041673329).finished();
209  Rot3 R3Expected = Rot3(M3);
210  EXPECT(assert_equal(R3Expected, orientations.at<Rot3>(x3), 1e-5));
211 }
212 
213 /* *************************************************************************** */
214 TEST( InitializePose3, orientationsGradient ) {
215  NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph());
216 
217  // Wrong initial guess - initialization should fix the rotations
218  Rot3 Rpert = Rot3::Expmap(Vector3(0.01, 0.01, 0.01));
219  Values givenPoses;
220  givenPoses.insert(x0,simple::pose0);
221  givenPoses.insert(x1,(simple::pose0).compose( Pose3(Rpert,Point3(0,0,0)) ));
222  givenPoses.insert(x2, (simple::pose0).compose( Pose3(Rpert.inverse(),Point3(0,0,0)) ));
223  givenPoses.insert(x3, (simple::pose0).compose( Pose3(Rpert,Point3(0,0,0)) ));
224  // do 10 gradient iterations
225  bool setRefFrame = false;
226  Values orientations = InitializePose3::computeOrientationsGradient(pose3Graph, givenPoses, 10, setRefFrame);
227 
228  // const Key keyAnchor = symbol('Z', 9999999);
229  // givenPoses.insert(keyAnchor,simple::pose0);
230  // string g2oFile = "/home/aspn/Desktop/toyExample.g2o";
231  // writeG2o(pose3Graph, givenPoses, g2oFile);
232 
233  const string matlabResultsfile = findExampleDataFile("simpleGraph10gradIter");
234  bool is3D = true;
235  const auto [matlabGraph, matlabValues] = readG2o(matlabResultsfile, is3D);
236 
237  Rot3 R0Expected = matlabValues->at<Pose3>(1).rotation();
238  EXPECT(assert_equal(R0Expected, orientations.at<Rot3>(x0), 1e-4));
239 
240  Rot3 R1Expected = matlabValues->at<Pose3>(2).rotation();
241  EXPECT(assert_equal(R1Expected, orientations.at<Rot3>(x1), 1e-4));
242 
243  Rot3 R2Expected = matlabValues->at<Pose3>(3).rotation();
244  EXPECT(assert_equal(R2Expected, orientations.at<Rot3>(x2), 1e-3));
245 
246  Rot3 R3Expected = matlabValues->at<Pose3>(4).rotation();
247  EXPECT(assert_equal(R3Expected, orientations.at<Rot3>(x3), 1e-4));
248 }
249 
250 /* *************************************************************************** */
251 TEST( InitializePose3, posesWithGivenGuess ) {
252  Values givenPoses;
253  givenPoses.insert(x0,simple::pose0);
254  givenPoses.insert(x1,simple::pose1);
255  givenPoses.insert(x2,simple::pose2);
256  givenPoses.insert(x3,simple::pose3);
257 
258  Values initial = InitializePose3::initialize(simple::graph(), givenPoses);
259 
260  // comparison is up to M_PI, that's why we add some multiples of 2*M_PI
261  EXPECT(assert_equal(givenPoses, initial, 1e-6));
262 }
263 
264 /* ************************************************************************* */
265 TEST(InitializePose3, initializePoses) {
266  const string g2oFile = findExampleDataFile("pose3example-grid");
267  bool is3D = true;
268  const auto [inputGraph, posesInFile] = readG2o(g2oFile, is3D);
269 
270  auto priorModel = noiseModel::Unit::Create(6);
271  inputGraph->addPrior(0, Pose3(), priorModel);
272 
273  Values initial = InitializePose3::initialize(*inputGraph);
274  EXPECT(assert_equal(*posesInFile, initial,
275  0.1)); // TODO(frank): very loose !!
276 }
277 
278 /* ************************************************************************* */
279 int main() {
280  TestResult tr;
281  return TestRegistry::runAllTests(tr);
282 }
283 /* ************************************************************************* */
284 
InitializePose3.h
Initialize Pose3 in a factor graph.
main
int main()
Definition: testInitializePose3.cpp:279
M3
RowMajorMatrixXf M3(M1)
x1
static Symbol x1('x', 1)
simple::graph
NonlinearFactorGraph graph()
Definition: testInitializePose3.cpp:61
M
Matrix< RealScalar, Dynamic, Dynamic > M
Definition: bench_gemm.cpp:51
b
Scalar * b
Definition: benchVecAdd.cpp:17
simple::p3
static Point3 p3
Definition: testInitializePose3.cpp:53
simple::p1
static Point3 p1
Definition: testInitializePose3.cpp:49
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
gtsam::Values::at
const ValueType at(Key j) const
Definition: Values-inl.h:204
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
Expmap
Pose2_ Expmap(const Vector3_ &xi)
Definition: InverseKinematicsExampleExpressions.cpp:47
x3
static Symbol x3('x', 3)
M1
MatrixXf M1
Definition: Tutorial_SlicingCol.cpp:1
std
Definition: BFloat16.h:88
p0
Vector3f p0
Definition: MatrixBase_all.cpp:2
gtsam::KeyRotMap
std::map< Key, Rot3 > KeyRotMap
Definition: InitializePose3.h:34
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
EXPECT_DOUBLES_EQUAL
#define EXPECT_DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:161
g
void g(const string &key, int i)
Definition: testBTree.cpp:41
gtsam::FactorGraph::add
IsDerived< DERIVEDFACTOR > add(std::shared_ptr< DERIVEDFACTOR > factor)
add is a synonym for push_back.
Definition: FactorGraph.h:214
gtsam::testing::compose
T compose(const T &t1, const T &t2)
Definition: lieProxies.h:39
gtsam::NonlinearFactorGraph::addPrior
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
Definition: NonlinearFactorGraph.h:199
gtsam::Rot3::inverse
Rot3 inverse() const
inverse of a rotation
Definition: Rot3.h:308
gtsam::KeyVectorMap
std::map< Key, std::vector< size_t > > KeyVectorMap
Definition: InitializePose3.h:33
simple::R2
static Rot3 R2
Definition: testInitializePose3.cpp:52
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
M2
M1<< 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12;Map< MatrixXf > M2(M1.data(), 6, 2)
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
model
static SharedNoiseModel model(noiseModel::Isotropic::Sigma(6, 0.1))
simple::pose3
static Pose3 pose3
Definition: testInitializePose3.cpp:59
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::readG2o
GraphAndValues readG2o(const std::string &g2oFile, const bool is3D, KernelFunctionType kernelFunctionType)
This function parses a g2o file and stores the measurements into a NonlinearFactorGraph and the initi...
Definition: dataset.cpp:621
simple::graph2
NonlinearFactorGraph graph2()
Definition: testInitializePose3.cpp:72
simple::pose0
static Pose3 pose0
Definition: testInitializePose3.cpp:56
gtsam
traits
Definition: chartTesting.h:28
simple::R3
static Rot3 R3
Definition: testInitializePose3.cpp:54
x0
static Symbol x0('x', 0)
x2
static Symbol x2('x', 2)
gtsam::findExampleDataFile
GTSAM_EXPORT std::string findExampleDataFile(const std::string &name)
Definition: dataset.cpp:70
gtsam::LieGroup::between
Class between(const Class &g) const
Definition: Lie.h:52
simple::pose1
static Pose3 pose1
Definition: testInitializePose3.cpp:57
simple::R1
static Rot3 R1
Definition: testInitializePose3.cpp:50
simple::pose2
static Pose3 pose2
Definition: testInitializePose3.cpp:58
gtsam::Values::insert
void insert(Key j, const Value &val)
Definition: Values.cpp:155
simple::p2
static Point3 p2
Definition: testInitializePose3.cpp:51
a
ArrayXXi a
Definition: Array_initializer_list_23_cxx11.cpp:1
simple::R0
static Rot3 R0
Definition: testInitializePose3.cpp:48
ceres::Matrix
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > Matrix
Definition: gtsam/3rdparty/ceres/eigen.h:42
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
gtsam::internal::rotation
Rot3 rotation(const Pose3 &pose, OptionalJacobian< 3, 6 > H)
Definition: slam/expressions.h:80
Pose3.h
3D Pose
ceres::Vector
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Definition: gtsam/3rdparty/ceres/eigen.h:38
TEST
TEST(InitializePose3, buildPose3graph)
Definition: testInitializePose3.cpp:92
gtsam::lago::initialize
Values initialize(const NonlinearFactorGraph &graph, bool useOdometricPath)
Definition: lago.cpp:375
dataset.h
utility functions for loading datasets
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:741

gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:38:26