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
12/**
13 * @file testRot3.cpp
14 * @brief Unit tests for Rot3Q class
15 * @author Richard Roberts
16 */
17
18#include <CppUnitLite/TestHarness.h>
19#include <gtsam/base/Testable.h>
20#include <gtsam/base/numericalDerivative.h>
21#include <gtsam/base/lieProxies.h>
22#include <gtsam/geometry/Point3.h>
23#include <gtsam/geometry/Rot3.h>
24#include <gtsam/nonlinear/NonlinearFactorGraph.h>
25#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
26#include <gtsam/inference/Symbol.h>
27#include <gtsam/slam/BetweenFactor.h>
28
29using namespace gtsam;
30
31typedef BetweenFactor<Rot3> Between;
32typedef NonlinearFactorGraph Graph;
33
34using symbol_shorthand::R;
35
36/* ************************************************************************* */
37TEST(Rot3, optimize) {
38
39 // Optimize a circle
40 Values truth;
41 Values initial;
42 Graph fg;
43 fg.addPrior(key: R(j: 0), prior: Rot3(), model: noiseModel::Isotropic::Sigma(dim: 3, sigma: 0.01));
44 for(int j=0; j<6; ++j) {
45 truth.insert(j: R(j), val: Rot3::Rz(M_PI/3.0 * double(j)));
46 initial.insert(j: R(j), val: Rot3::Rz(M_PI/3.0 * double(j) + 0.1 * double(j%2)));
47 fg.emplace_shared<Between>(args: R(j), args: R(j: (j+1)%6), args: Rot3::Rz(M_PI/3.0), args: noiseModel::Isotropic::Sigma(dim: 3, sigma: 0.01));
48 }
49
50 Values final = GaussNewtonOptimizer(fg, initial).optimize();
51
52 EXPECT(assert_equal(truth, final, 1e-5));
53}
54
55/* ************************************************************************* */
56int main() {
57 TestResult tr;
58 return TestRegistry::runAllTests(result&: tr);
59}
60/* ************************************************************************* */
61