testRot3Optimization.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 
19 #include <gtsam/base/Testable.h>
20 #include <boost/math/constants/constants.hpp>
22 #include <gtsam/base/lieProxies.h>
23 #include <gtsam/geometry/Point3.h>
24 #include <gtsam/geometry/Rot3.h>
27 #include <gtsam/inference/Symbol.h>
29 
30 using namespace gtsam;
31 
34 
35 /* ************************************************************************* */
37 
38  // Optimize a circle
39  Values truth;
41  Graph fg;
42  fg.addPrior(Symbol('r',0), Rot3(), noiseModel::Isotropic::Sigma(3, 0.01));
43  for(int j=0; j<6; ++j) {
44  truth.insert(Symbol('r',j), Rot3::Rz(M_PI/3.0 * double(j)));
45  initial.insert(Symbol('r',j), Rot3::Rz(M_PI/3.0 * double(j) + 0.1 * double(j%2)));
46  fg += Between(Symbol('r',j), Symbol('r',(j+1)%6), Rot3::Rz(M_PI/3.0), noiseModel::Isotropic::Sigma(3, 0.01));
47  }
48 
49  Values final = GaussNewtonOptimizer(fg, initial).optimize();
50 
51  EXPECT(assert_equal(truth, final, 1e-5));
52 }
53 
54 /* ************************************************************************* */
55 int main() {
56  TestResult tr;
57  return TestRegistry::runAllTests(tr);
58 }
59 /* ************************************************************************* */
virtual const Values & optimize()
Concept check for values that can be used in unit tests.
Provides convenient mappings of common member functions for testing.
static int runAllTests(TestResult &result)
Factor Graph consisting of non-linear factors.
void insert(Key j, const Value &val)
Definition: Values.cpp:140
#define M_PI
Definition: main.h:78
Values initial
Some functions to compute numerical derivatives.
int main()
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
#define EXPECT(condition)
Definition: Test.h:151
BetweenFactor< Rot3 > Between
Array< double, 1, 3 > e(1./3., 0.5, 2.)
NonlinearFactorGraph Graph
traits
Definition: chartTesting.h:28
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:42
static Rot3 Rz(double t)
Rotation around Z axis as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when look...
Definition: Rot3M.cpp:75
3D Point
TEST(LPInitSolver, InfiniteLoopSingleVar)
std::ptrdiff_t j
static shared_ptr Sigma(size_t dim, double sigma, bool smart=true)
Definition: NoiseModel.cpp:567
3D rotation represented as a rotation matrix or quaternion


gtsam
Author(s):
autogenerated on Sat May 8 2021 02:49:22