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

gtsam
Author(s):
autogenerated on Sat May 8 2021 02:47:27