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 #if defined(__GNUC__) && !defined(__clang__)
18 #pragma GCC diagnostic ignored "-Wmaybe-uninitialized"
19 #endif
20 using namespace std;
21 using namespace std::placeholders;
22 using namespace gtsam;
23 
24 static const double kDegree = M_PI / 180;
25 
26 //*************************************************************************
27 // Create some test data
28 // Let's assume IMU is aligned with aero (X-forward,Z down)
29 // And camera is looking forward.
30 static const Point3 cameraX(0, 1, 0), cameraY(0, 0, 1), cameraZ(1, 0, 0);
31 static const Rot3 iRc(cameraX, cameraY, cameraZ);
32 
33 // Now, let's create some rotations around IMU frame
34 static const Unit3 p1(1, 0, 0), p2(0, 1, 0), p3(0, 0, 1);
35 static const Rot3 i1Ri2 = Rot3::AxisAngle(p1, 1), //
36 i2Ri3 = Rot3::AxisAngle(p2, 1), //
37 i3Ri4 = Rot3::AxisAngle(p3, 1);
38 
39 // The corresponding rotations in the camera frame
40 static const Rot3 c1Zc2 = iRc.inverse() * i1Ri2 * iRc, //
41 c2Zc3 = iRc.inverse() * i2Ri3 * iRc, //
43 
44 // The corresponding rotated directions in the camera frame
45 static const Unit3 z1 = iRc.inverse() * p1, //
46 z2 = iRc.inverse() * p2, //
47 z3 = iRc.inverse() * p3;
48 
50 
51 //*************************************************************************
52 TEST (RotateFactor, checkMath) {
53  EXPECT(assert_equal(c1Zc2, Rot3::AxisAngle(z1, 1)));
54  EXPECT(assert_equal(c2Zc3, Rot3::AxisAngle(z2, 1)));
55  EXPECT(assert_equal(c3Zc4, Rot3::AxisAngle(z3, 1)));
56 }
57 
58 //*************************************************************************
60  Model model = noiseModel::Isotropic::Sigma(3, 0.01);
62  EXPECT(assert_equal(Z_3x1, f.evaluateError(iRc), 1e-8));
63 
64  Rot3 R = iRc.retract(Vector3(0.1, 0.2, 0.1));
65 #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS)
66  Vector expectedE = Vector3(-0.0248752, 0.202981, -0.0890529);
67 #else
68  Vector expectedE = Vector3(-0.0246305, 0.20197, -0.08867);
69 #endif
70  EXPECT( assert_equal(expectedE, f.evaluateError(R), 1e-5));
71 
72  Matrix actual, expected;
73  // Use numerical derivatives to calculate the expected Jacobian
74  {
75  expected = numericalDerivative11<Vector3, Rot3>(
76  [&f](const Rot3& r) { return f.evaluateError(r); }, iRc);
77  f.evaluateError(iRc, actual);
78  EXPECT(assert_equal(expected, actual, 1e-9));
79  }
80  {
81  expected = numericalDerivative11<Vector3, Rot3>(
82  [&f](const Rot3& r) { return f.evaluateError(r); }, R);
83  f.evaluateError(R, actual);
84  EXPECT(assert_equal(expected, actual, 1e-9));
85  }
86 }
87 
88 //*************************************************************************
89 TEST (RotateFactor, minimization) {
90  // Let's try to recover the correct iRc by minimizing
92  Model model = noiseModel::Isotropic::Sigma(3, 0.01);
96 
97  // Check error at ground truth
98  Values truth;
99  truth.insert(1, iRc);
100  EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8);
101 
102  // Check error at initial estimate
103  Values initial;
104  Rot3 initialE = iRc.retract(kDegree * Vector3(20, -20, 20));
105  initial.insert(1, initialE);
106 
107 #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS)
108  EXPECT_DOUBLES_EQUAL(3545.40, graph.error(initial), 1);
109 #else
111 #endif
112 
113  // Optimize
115  //parameters.setVerbosity("ERROR");
117  Values result = optimizer.optimize();
118 
119  // Check result
120  Rot3 actual = result.at<Rot3>(1);
121  EXPECT(assert_equal(iRc, actual,1e-1));
122 
123  // Check error at result
125 }
126 
127 //*************************************************************************
129  Model model = noiseModel::Isotropic::Sigma(2, 0.01);
131  EXPECT(assert_equal(Z_2x1, f.evaluateError(iRc), 1e-8));
132 
133  Rot3 R = iRc.retract(Vector3(0.1, 0.2, 0.1));
134 
135 #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS)
136  Vector expectedE = Vector2(-0.0890529, -0.202981);
137 #else
138  Vector expectedE = Vector2(-0.08867, -0.20197);
139 #endif
140 
141  EXPECT( assert_equal(expectedE, f.evaluateError(R), 1e-5));
142 
143  Matrix actual, expected;
144  // Use numerical derivatives to calculate the expected Jacobian
145  {
146  expected = numericalDerivative11<Vector,Rot3>(
147  [&f](const Rot3& r) {return f.evaluateError(r);}, iRc);
148  f.evaluateError(iRc, actual);
149  EXPECT(assert_equal(expected, actual, 1e-9));
150  }
151  {
152  expected = numericalDerivative11<Vector,Rot3>(
153  [&f](const Rot3& r) {return f.evaluateError(r);}, R);
154  f.evaluateError(R, actual);
155  EXPECT(assert_equal(expected, actual, 1e-9));
156  }
157 }
158 
159 //*************************************************************************
160 TEST (RotateDirectionsFactor, minimization) {
161  // Let's try to recover the correct iRc by minimizing
163  Model model = noiseModel::Isotropic::Sigma(2, 0.01);
167 
168  // Check error at ground truth
169  Values truth;
170  truth.insert(1, iRc);
171  EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8);
172 
173  // Check error at initial estimate
174  Values initial;
175  Rot3 initialE = iRc.retract(kDegree * Vector3(20, -20, 20));
176  initial.insert(1, initialE);
177 
178 #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS)
180 #else
182 #endif
183 
184  // Optimize
187  Values result = optimizer.optimize();
188 
189  // Check result
190  Rot3 actual = result.at<Rot3>(1);
191  EXPECT(assert_equal(iRc, actual,1e-1));
192 
193  // Check error at result
195 }
196 
197 //*************************************************************************
198 TEST(RotateDirectionsFactor, Initialization) {
199  // Create a gravity vector in a nav frame that has Z up
200  const Point3 n_gravity(0, 0, -10);
201  const Unit3 n_p(-n_gravity);
202 
203  // NOTE(frank): avoid singularities by using 85/275 instead of 90/270
204  std::vector<double> angles = {0, 45, 85, 135, 180, 225, 275, 315};
205  for (double yaw : angles) {
206  const Rot3 nRy = Rot3::Yaw(yaw * kDegree);
207  for (double pitch : angles) {
208  const Rot3 yRp = Rot3::Pitch(pitch * kDegree);
209  for (double roll : angles) {
210  const Rot3 pRb = Rot3::Roll(roll * kDegree);
211 
212  // Rotation from body to nav frame:
213  const Rot3 nRb = nRy * yRp * pRb;
214  const Vector3 rpy = nRb.rpy() / kDegree;
215 
216  // Simulate measurement of IMU in body frame:
217  const Point3 b_acc = nRb.unrotate(-n_gravity);
218  const Unit3 b_z(b_acc);
219 
220  // Check initialization
221  const Rot3 actual_nRb = RotateDirectionsFactor::Initialize(n_p, b_z);
222  const Vector3 actual_rpy = actual_nRb.rpy() / kDegree;
223  EXPECT_DOUBLES_EQUAL(rpy.x(), actual_rpy.x(), 1e-5);
224  EXPECT_DOUBLES_EQUAL(rpy.y(), actual_rpy.y(), 1e-5);
225  }
226  }
227  }
228 }
229 
230 /* ************************************************************************* */
231 int main() {
232  TestResult tr;
233  return TestRegistry::runAllTests(tr);
234 }
235 /* ************************************************************************* */
236 
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
i2Ri3
static const Rot3 i2Ri3
Definition: testRotateFactor.cpp:36
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:43
gtsam::Z_3x1
static const Eigen::MatrixBase< Vector3 >::ConstantReturnType Z_3x1
Definition: Vector.h:47
gtsam::Rot3::rpy
Vector3 rpy(OptionalJacobian< 3, 3 > H={}) const
Definition: Rot3.cpp:192
Model
noiseModel::Isotropic::shared_ptr Model
Definition: testRotateFactor.cpp:49
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:45
initial
Values initial
Definition: OdometryOptimize.cpp:2
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
TEST
TEST(RotateFactor, checkMath)
Definition: testRotateFactor.cpp:52
initial::nRb
const Rot3 nRb
Definition: testScenarioRunner.cpp:149
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:44
c3Zc4
static const Rot3 c3Zc4
Definition: testRotateFactor.cpp:42
p2
static const Unit3 p2(0, 1, 0)
cameraX
static const Point3 cameraX(0, 1, 0)
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:39
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:136
test
Definition: test.py:1
main
int main()
Definition: testRotateFactor.cpp:231
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:35
kDegree
static const double kDegree
Definition: testRotateFactor.cpp:24
numericalDerivative.h
Some functions to compute numerical derivatives.
z3
static const Unit3 z3
Definition: testRotateFactor.cpp:47
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:312
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:156
gtsam::Z_2x1
static const Eigen::MatrixBase< Vector2 >::ConstantReturnType Z_2x1
Definition: Vector.h:46
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:41
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:41
i3Ri4
static const Rot3 i3Ri4
Definition: testRotateFactor.cpp:37
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:46
c1Zc2
static const Rot3 c1Zc2
Definition: testRotateFactor.cpp:40


gtsam
Author(s):
autogenerated on Sun Feb 16 2025 04:07:15