testRotateFactor.cpp
Go to the documentation of this file.
1 /*
2  * @file testRotateFactor.cpp
3  * @brief Test RotateFactor class
4  * @author Frank Dellaert
5  * @date December 17, 2013
6  */
7 
9 #include <gtsam/base/Testable.h>
14 
15 #include <vector>
16 
17 using namespace std;
18 using namespace std::placeholders;
19 using namespace gtsam;
20 
21 static const double kDegree = M_PI / 180;
22 
23 //*************************************************************************
24 // Create some test data
25 // Let's assume IMU is aligned with aero (X-forward,Z down)
26 // And camera is looking forward.
27 static const Point3 cameraX(0, 1, 0), cameraY(0, 0, 1), cameraZ(1, 0, 0);
28 static const Rot3 iRc(cameraX, cameraY, cameraZ);
29 
30 // Now, let's create some rotations around IMU frame
31 static const Unit3 p1(1, 0, 0), p2(0, 1, 0), p3(0, 0, 1);
32 static const Rot3 i1Ri2 = Rot3::AxisAngle(p1, 1), //
33 i2Ri3 = Rot3::AxisAngle(p2, 1), //
34 i3Ri4 = Rot3::AxisAngle(p3, 1);
35 
36 // The corresponding rotations in the camera frame
37 static const Rot3 c1Zc2 = iRc.inverse() * i1Ri2 * iRc, //
38 c2Zc3 = iRc.inverse() * i2Ri3 * iRc, //
40 
41 // The corresponding rotated directions in the camera frame
42 static const Unit3 z1 = iRc.inverse() * p1, //
43 z2 = iRc.inverse() * p2, //
44 z3 = iRc.inverse() * p3;
45 
47 
48 //*************************************************************************
49 TEST (RotateFactor, checkMath) {
50  EXPECT(assert_equal(c1Zc2, Rot3::AxisAngle(z1, 1)));
51  EXPECT(assert_equal(c2Zc3, Rot3::AxisAngle(z2, 1)));
52  EXPECT(assert_equal(c3Zc4, Rot3::AxisAngle(z3, 1)));
53 }
54 
55 //*************************************************************************
57  Model model = noiseModel::Isotropic::Sigma(3, 0.01);
59  EXPECT(assert_equal(Z_3x1, f.evaluateError(iRc), 1e-8));
60 
61  Rot3 R = iRc.retract(Vector3(0.1, 0.2, 0.1));
62 #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS)
63  Vector expectedE = Vector3(-0.0248752, 0.202981, -0.0890529);
64 #else
65  Vector expectedE = Vector3(-0.0246305, 0.20197, -0.08867);
66 #endif
67  EXPECT( assert_equal(expectedE, f.evaluateError(R), 1e-5));
68 
69  Matrix actual, expected;
70  // Use numerical derivatives to calculate the expected Jacobian
71  {
72  expected = numericalDerivative11<Vector3, Rot3>(
73  [&f](const Rot3& r) { return f.evaluateError(r); }, iRc);
74  f.evaluateError(iRc, actual);
75  EXPECT(assert_equal(expected, actual, 1e-9));
76  }
77  {
78  expected = numericalDerivative11<Vector3, Rot3>(
79  [&f](const Rot3& r) { return f.evaluateError(r); }, R);
80  f.evaluateError(R, actual);
81  EXPECT(assert_equal(expected, actual, 1e-9));
82  }
83 }
84 
85 //*************************************************************************
86 TEST (RotateFactor, minimization) {
87  // Let's try to recover the correct iRc by minimizing
89  Model model = noiseModel::Isotropic::Sigma(3, 0.01);
93 
94  // Check error at ground truth
95  Values truth;
96  truth.insert(1, iRc);
97  EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8);
98 
99  // Check error at initial estimate
100  Values initial;
101  Rot3 initialE = iRc.retract(kDegree * Vector3(20, -20, 20));
102  initial.insert(1, initialE);
103 
104 #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS)
105  EXPECT_DOUBLES_EQUAL(3545.40, graph.error(initial), 1);
106 #else
108 #endif
109 
110  // Optimize
112  //parameters.setVerbosity("ERROR");
114  Values result = optimizer.optimize();
115 
116  // Check result
117  Rot3 actual = result.at<Rot3>(1);
118  EXPECT(assert_equal(iRc, actual,1e-1));
119 
120  // Check error at result
122 }
123 
124 //*************************************************************************
126  Model model = noiseModel::Isotropic::Sigma(2, 0.01);
128  EXPECT(assert_equal(Z_2x1, f.evaluateError(iRc), 1e-8));
129 
130  Rot3 R = iRc.retract(Vector3(0.1, 0.2, 0.1));
131 
132 #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS)
133  Vector expectedE = Vector2(-0.0890529, -0.202981);
134 #else
135  Vector expectedE = Vector2(-0.08867, -0.20197);
136 #endif
137 
138  EXPECT( assert_equal(expectedE, f.evaluateError(R), 1e-5));
139 
140  Matrix actual, expected;
141  // Use numerical derivatives to calculate the expected Jacobian
142  {
143  expected = numericalDerivative11<Vector,Rot3>(
144  [&f](const Rot3& r) {return f.evaluateError(r);}, iRc);
145  f.evaluateError(iRc, actual);
146  EXPECT(assert_equal(expected, actual, 1e-9));
147  }
148  {
149  expected = numericalDerivative11<Vector,Rot3>(
150  [&f](const Rot3& r) {return f.evaluateError(r);}, R);
151  f.evaluateError(R, actual);
152  EXPECT(assert_equal(expected, actual, 1e-9));
153  }
154 }
155 
156 //*************************************************************************
157 TEST (RotateDirectionsFactor, minimization) {
158  // Let's try to recover the correct iRc by minimizing
160  Model model = noiseModel::Isotropic::Sigma(2, 0.01);
164 
165  // Check error at ground truth
166  Values truth;
167  truth.insert(1, iRc);
168  EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8);
169 
170  // Check error at initial estimate
171  Values initial;
172  Rot3 initialE = iRc.retract(kDegree * Vector3(20, -20, 20));
173  initial.insert(1, initialE);
174 
175 #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS)
177 #else
179 #endif
180 
181  // Optimize
184  Values result = optimizer.optimize();
185 
186  // Check result
187  Rot3 actual = result.at<Rot3>(1);
188  EXPECT(assert_equal(iRc, actual,1e-1));
189 
190  // Check error at result
192 }
193 
194 //*************************************************************************
195 TEST(RotateDirectionsFactor, Initialization) {
196  // Create a gravity vector in a nav frame that has Z up
197  const Point3 n_gravity(0, 0, -10);
198  const Unit3 n_p(-n_gravity);
199 
200  // NOTE(frank): avoid singularities by using 85/275 instead of 90/270
201  std::vector<double> angles = {0, 45, 85, 135, 180, 225, 275, 315};
202  for (double yaw : angles) {
203  const Rot3 nRy = Rot3::Yaw(yaw * kDegree);
204  for (double pitch : angles) {
205  const Rot3 yRp = Rot3::Pitch(pitch * kDegree);
206  for (double roll : angles) {
207  const Rot3 pRb = Rot3::Roll(roll * kDegree);
208 
209  // Rotation from body to nav frame:
210  const Rot3 nRb = nRy * yRp * pRb;
211  const Vector3 rpy = nRb.rpy() / kDegree;
212 
213  // Simulate measurement of IMU in body frame:
214  const Point3 b_acc = nRb.unrotate(-n_gravity);
215  const Unit3 b_z(b_acc);
216 
217  // Check initialization
218  const Rot3 actual_nRb = RotateDirectionsFactor::Initialize(n_p, b_z);
219  const Vector3 actual_rpy = actual_nRb.rpy() / kDegree;
220  EXPECT_DOUBLES_EQUAL(rpy.x(), actual_rpy.x(), 1e-5);
221  EXPECT_DOUBLES_EQUAL(rpy.y(), actual_rpy.y(), 1e-5);
222  }
223  }
224  }
225 }
226 
227 /* ************************************************************************* */
228 int main() {
229  TestResult tr;
230  return TestRegistry::runAllTests(tr);
231 }
232 /* ************************************************************************* */
233 
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
i2Ri3
static const Rot3 i2Ri3
Definition: testRotateFactor.cpp:33
gtsam::NonlinearOptimizer::optimize
virtual const Values & optimize()
Definition: NonlinearOptimizer.h:98
iRc
static const Rot3 iRc(cameraX, cameraY, cameraZ)
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
Testable.h
Concept check for values that can be used in unit tests.
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
gtsam::Vector2
Eigen::Vector2d Vector2
Definition: Vector.h:42
gtsam::Z_3x1
static const Eigen::MatrixBase< Vector3 >::ConstantReturnType Z_3x1
Definition: Vector.h:46
gtsam::Rot3::rpy
Vector3 rpy(OptionalJacobian< 3, 3 > H={}) const
Definition: Rot3.cpp:191
Model
noiseModel::Isotropic::shared_ptr Model
Definition: testRotateFactor.cpp:46
TestHarness.h
gtsam::RotateFactor
Definition: RotateFactor.h:23
LevenbergMarquardtOptimizer.h
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
z1
static const Unit3 z1
Definition: testRotateFactor.cpp:42
initial
Values initial
Definition: OdometryOptimize.cpp:2
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
TEST
TEST(RotateFactor, checkMath)
Definition: testRotateFactor.cpp:49
initial::nRb
const Rot3 nRb
Definition: testScenarioRunner.cpp:149
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
c3Zc4
static const Rot3 c3Zc4
Definition: testRotateFactor.cpp:39
p2
static const Unit3 p2(0, 1, 0)
cameraX
static const Point3 cameraX(0, 1, 0)
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
result
Values result
Definition: OdometryOptimize.cpp:8
gtsam::Rot3::unrotate
Point3 unrotate(const Point3 &p, OptionalJacobian< 3, 3 > H1={}, OptionalJacobian< 3, 3 > H2={}) const
rotate point from world to rotated frame
Definition: Rot3.cpp:135
test
Definition: test.py:1
main
int main()
Definition: testRotateFactor.cpp:228
p3
static const Unit3 p3(0, 0, 1)
gtsam::NonlinearFactorGraph::error
double error(const Values &values) const
Definition: NonlinearFactorGraph.cpp:170
gtsam::Values::at
const ValueType at(Key j) const
Definition: Values-inl.h:261
i1Ri2
static const Rot3 i1Ri2
Definition: testRotateFactor.cpp:32
kDegree
static const double kDegree
Definition: testRotateFactor.cpp:21
numericalDerivative.h
Some functions to compute numerical derivatives.
z3
static const Unit3 z3
Definition: testRotateFactor.cpp:44
cameraY
static const Point3 cameraY(0, 0, 1)
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
parameters
static ConjugateGradientParameters parameters
Definition: testIterative.cpp:33
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
gtsam::LevenbergMarquardtOptimizer
Definition: LevenbergMarquardtOptimizer.h:35
gtsam::RotateDirectionsFactor
Definition: RotateFactor.h:69
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
EXPECT_DOUBLES_EQUAL
#define EXPECT_DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:161
gtsam::Rot3::inverse
Rot3 inverse() const
inverse of a rotation
Definition: Rot3.h:308
TestResult
Definition: TestResult.h:26
tree::f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Definition: testExpression.cpp:218
RotateFactor.h
p1
static const Unit3 p1(1, 0, 0)
gtsam
traits
Definition: SFMdata.h:40
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
gtsam::Values
Definition: Values.h:65
std
Definition: BFloat16.h:88
gtsam::Values::insert
void insert(Key j, const Value &val)
Definition: Values.cpp:155
gtsam::Z_2x1
static const Eigen::MatrixBase< Vector2 >::ConstantReturnType Z_2x1
Definition: Vector.h:45
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
gtsam::LevenbergMarquardtParams
Definition: LevenbergMarquardtParams.h:35
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
initial
Definition: testScenarioRunner.cpp:148
gtsam::LieGroup::retract
Class retract(const TangentVector &v) const
retract as required by manifold concept: applies v at *this
Definition: Lie.h:131
gtsam::noiseModel::Isotropic::shared_ptr
std::shared_ptr< Isotropic > shared_ptr
Definition: NoiseModel.h:559
gtsam::Unit3
Represents a 3D point on a unit sphere.
Definition: Unit3.h:42
M_PI
#define M_PI
Definition: mconf.h:117
gtsam.examples.ImuFactorISAM2Example.n_gravity
def n_gravity
Definition: ImuFactorISAM2Example.py:31
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
c2Zc3
static const Rot3 c2Zc3
Definition: testRotateFactor.cpp:38
i3Ri4
static const Rot3 i3Ri4
Definition: testRotateFactor.cpp:34
R
Rot2 R(Rot2::fromAngle(0.1))
gtsam::FactorGraph::emplace_shared
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
Definition: FactorGraph.h:153
cameraZ
static const Point3 cameraZ(1, 0, 0)
z2
static const Unit3 z2
Definition: testRotateFactor.cpp:43
c1Zc2
static const Rot3 c1Zc2
Definition: testRotateFactor.cpp:37


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:41:19