testAdaptAutoDiff.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 
24 #include <gtsam/geometry/Pose3.h>
25 #include <gtsam/geometry/Cal3_S2.h>
28 #include <gtsam/base/Testable.h>
29 
31 
32 
33 namespace gtsam {
34 
35 // Special version of Cal3Bundler so that default constructor = 0,0,0
36 struct Cal3Bundler0 : public Cal3Bundler {
37  Cal3Bundler0(double f = 0, double k1 = 0, double k2 = 0, double u0 = 0,
38  double v0 = 0)
39  : Cal3Bundler(f, k1, k2, u0, v0) {}
40  Cal3Bundler0 retract(const Vector& d) const {
41  return Cal3Bundler0(fx() + d(0), k1() + d(1), k2() + d(2), px(), py());
42  }
44  return T2.vector() - vector();
45  }
46 };
47 
48 template <>
49 struct traits<Cal3Bundler0> : public internal::Manifold<Cal3Bundler0> {};
50 
51 // With that, camera below behaves like Snavely's 9-dim vector
53 }
54 
55 using namespace std;
56 using namespace gtsam;
57 
58 /* ************************************************************************* */
59 // Check that ceres rotation convention is the same
60 TEST(AdaptAutoDiff, Rotation) {
61  Vector3 axisAngle(0.1, 0.2, 0.3);
62  Matrix3 expected = Rot3::Rodrigues(axisAngle).matrix();
63  Matrix3 actual;
64  ceres::AngleAxisToRotationMatrix(axisAngle.data(), actual.data());
65  EXPECT(assert_equal(expected, actual));
66 }
67 
68 /* ************************************************************************* */
69 // Some Ceres Snippets copied for testing
70 // Copyright 2010, 2011, 2012 Google Inc. All rights reserved.
71 template <typename T>
72 inline T& RowMajorAccess(T* base, int rows, int cols, int i, int j) {
73  return base[cols * i + j];
74 }
75 
76 inline double RandDouble() {
77  double r = static_cast<double>(rand());
78  return r / RAND_MAX;
79 }
80 
81 // A structure for projecting a 3x4 camera matrix and a
82 // homogeneous 3D point, to a 2D inhomogeneous point.
83 struct Projective {
84  // Function that takes P and X as separate vectors:
85  // P, X -> x
86  template <typename A>
87  bool operator()(A const P[12], A const X[4], A x[2]) const {
88  A PX[3];
89  for (int i = 0; i < 3; ++i) {
90  PX[i] = RowMajorAccess(P, 3, 4, i, 0) * X[0] +
91  RowMajorAccess(P, 3, 4, i, 1) * X[1] +
92  RowMajorAccess(P, 3, 4, i, 2) * X[2] +
93  RowMajorAccess(P, 3, 4, i, 3) * X[3];
94  }
95  if (PX[2] != 0.0) {
96  x[0] = PX[0] / PX[2];
97  x[1] = PX[1] / PX[2];
98  return true;
99  }
100  return false;
101  }
102 
103  // Adapt to eigen types
104  Vector2 operator()(const MatrixRowMajor& P, const Vector4& X) const {
105  Vector2 x;
106  if (operator()(P.data(), X.data(), x.data()))
107  return x;
108  else
109  throw std::runtime_error("Projective fail");
110  }
111 };
112 
113 /* ************************************************************************* */
114 // Test Ceres AutoDiff
115 TEST(AdaptAutoDiff, AutoDiff) {
117 
118  // Instantiate function
119  Projective projective;
120 
121  // Make arguments
122  typedef Eigen::Matrix<double, 3, 4, Eigen::RowMajor> RowMajorMatrix34;
123  RowMajorMatrix34 P;
124  P << 1, 0, 0, 0, 0, 1, 0, 5, 0, 0, 1, 0;
125  Vector4 X(10, 0, 5, 1);
126 
127  // Apply the mapping, to get image point b_x.
128  Vector expected = Vector2(2, 1);
129  Vector2 actual = projective(P, X);
130  EXPECT(assert_equal(expected, actual, 1e-9));
131 
132  // Get expected derivatives
133  Matrix E1 = numericalDerivative21<Vector2, RowMajorMatrix34, Vector4>(
134  Projective(), P, X);
135  Matrix E2 = numericalDerivative22<Vector2, RowMajorMatrix34, Vector4>(
136  Projective(), P, X);
137 
138  // Get derivatives with AutoDiff
139  Vector2 actual2;
140  MatrixRowMajor H1(2, 12), H2(2, 4);
141  double* parameters[] = {P.data(), X.data()};
142  double* jacobians[] = {H1.data(), H2.data()};
143  CHECK((AutoDiff<Projective, double, 12, 4>::Differentiate(
144  projective, parameters, 2, actual2.data(), jacobians)));
145  EXPECT(assert_equal(E1, H1, 1e-8));
146  EXPECT(assert_equal(E2, H2, 1e-8));
147 }
148 
149 /* ************************************************************************* */
150 // Test Ceres AutoDiff on Snavely, defined in ceres_example.h
151 // Adapt to GTSAM types
152 Vector2 adapted(const Vector9& P, const Vector3& X) {
153  SnavelyProjection snavely;
154  Vector2 x;
155  if (snavely(P.data(), X.data(), x.data()))
156  return x;
157  else
158  throw std::runtime_error("Snavely fail");
159 }
160 
161 /* ************************************************************************* */
162 namespace example {
163 Camera camera(Pose3(Rot3().retract(Vector3(0.1, 0.2, 0.3)), Point3(0, 5, 0)),
164  Cal3Bundler0(1, 0, 0));
165 Point3 point(10, 0, -5); // negative Z-axis convention of Snavely!
166 Vector9 P = Camera().localCoordinates(camera);
168 #ifdef GTSAM_POSE3_EXPMAP
169 Vector2 expectedMeasurement(1.3124675, 1.2057287);
170 #else
171 Vector2 expectedMeasurement(1.2431567, 1.2525694);
172 #endif
173 Matrix E1 = numericalDerivative21<Vector2, Vector9, Vector3>(adapted, P, X);
174 Matrix E2 = numericalDerivative22<Vector2, Vector9, Vector3>(adapted, P, X);
175 }
176 
177 /* ************************************************************************* */
178 // Check that Local worked as expected
180  using namespace example;
181 #ifdef GTSAM_POSE3_EXPMAP
182  Vector9 expectedP = (Vector9() << 0.1, 0.2, 0.3, 0.7583528428, 4.9582357859, -0.224941471539, 1, 0, 0).finished();
183 #else
184  Vector9 expectedP = (Vector9() << 0.1, 0.2, 0.3, 0, 5, 0, 1, 0, 0).finished();
185 #endif
186  EXPECT(equal_with_abs_tol(expectedP, P));
187  Vector3 expectedX(10, 0, -5); // negative Z-axis convention of Snavely!
188  EXPECT(equal_with_abs_tol(expectedX, X));
189 }
190 
191 /* ************************************************************************* */
192 // Test Ceres AutoDiff
193 TEST(AdaptAutoDiff, AutoDiff2) {
194  using namespace example;
196 
197  // Apply the mapping, to get image point b_x.
198  Vector2 actual = adapted(P, X);
200 
201  // Instantiate function
202  SnavelyProjection snavely;
203 
204  // Get derivatives with AutoDiff
205  Vector2 actual2;
206  MatrixRowMajor H1(2, 9), H2(2, 3);
207  double* parameters[] = {P.data(), X.data()};
208  double* jacobians[] = {H1.data(), H2.data()};
209  CHECK((AutoDiff<SnavelyProjection, double, 9, 3>::Differentiate(
210  snavely, parameters, 2, actual2.data(), jacobians)));
211  EXPECT(assert_equal(E1, H1, 1e-8));
212  EXPECT(assert_equal(E2, H2, 1e-8));
213 }
214 
215 /* ************************************************************************* */
216 // Test AutoDiff wrapper Snavely
218  using namespace example;
219 
221  Adaptor snavely;
222 
223  // Apply the mapping, to get image point b_x.
224  Vector2 actual = snavely(P, X);
226 
227  // Get derivatives with AutoDiff, not gives RowMajor results!
228  Matrix29 H1;
229  Matrix23 H2;
230  Vector2 actual2 = snavely(P, X, H1, H2);
231  EXPECT(assert_equal(expectedMeasurement, actual2, 1e-6));
232  EXPECT(assert_equal(E1, H1, 1e-8));
233  EXPECT(assert_equal(E2, H2, 1e-8));
234 }
235 
236 /* ************************************************************************* */
237 // Test AutoDiff wrapper in an expression
238 TEST(AdaptAutoDiff, SnavelyExpression) {
240 
243 
244  Expression<Vector2> expression(Adaptor(), P, X);
245 
246  std::size_t RecordSize =
248 
250  internal::upAligned(RecordSize) + P.traceSize() + X.traceSize(),
251  expression.traceSize());
252 
253  const set<Key> expected{1, 2};
254  EXPECT(expected == expression.keys());
255 }
256 
257 /* ************************************************************************* */
258 int main() {
259  TestResult tr;
260  return TestRegistry::runAllTests(tr);
261 }
262 /* ************************************************************************* */
#define CHECK(condition)
Definition: Test.h:108
Both ManifoldTraits and Testable.
Definition: Manifold.h:117
Adaptor for Ceres style auto-differentiated functions.
Vector3 localCoordinates(const Cal3Bundler0 &T2) const
Concept check for values that can be used in unit tests.
double RandDouble()
static int runAllTests(TestResult &result)
Eigen::Vector3d Vector3
Definition: Vector.h:43
Vector3 vector() const
Definition: Cal3Bundler.cpp:42
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar * data() const
Matrix expected
Definition: testMatrix.cpp:971
PinholeCamera< Cal3Bundler0 > Camera
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
Vector2 adapted(const Vector9 &P, const Vector3 &X)
T & RowMajorAccess(T *base, int rows, int cols, int i, int j)
Definition: BFloat16.h:88
Some functions to compute numerical derivatives.
static const Pose3 T2(Rot3::Rodrigues(0.3, 0.2, 0.1), P2)
Matrix< SCALARA, Dynamic, Dynamic, opt_A > A
Definition: bench_gemm.cpp:48
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
T upAligned(T value, unsigned requiredAlignment=TraceAlignment)
Cal3Bundler0(double f=0, double k1=0, double k2=0, double u0=0, double v0=0)
Expressions for Block Automatic Differentiation.
Base class for all pinhole cameras.
int main()
static Rot3 Rodrigues(const Vector3 &w)
Definition: Rot3.h:240
Eigen::VectorXd Vector
Definition: Vector.h:38
#define EXPECT(condition)
Definition: Test.h:150
double k1() const
distorsion parameter k1
Definition: Cal3Bundler.h:87
double fx() const
focal length x
Definition: Cal3.h:139
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Array< double, 1, 3 > e(1./3., 0.5, 2.)
static const double u0
static ConjugateGradientParameters parameters
bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen::DenseBase< MATRIX > &B, double tol=1e-9)
Definition: base/Matrix.h:80
traits
Definition: chartTesting.h:28
double k2() const
distorsion parameter k2
Definition: Cal3Bundler.h:90
Calibration used by Bundler.
Definition: Cal3Bundler.h:32
std::set< Key > keys() const
Return keys that play in this expression.
Eigen::Vector2d Vector2
Definition: Vector.h:42
#define EXPECT_LONGS_EQUAL(expected, actual)
Definition: Test.h:154
bool operator()(A const P[12], A const X[4], A x[2]) const
static const double v0
Matrix3 matrix() const
Definition: Rot3M.cpp:218
Vector2 operator()(const MatrixRowMajor &P, const Vector4 &X) const
double px() const
image center in x
Definition: Cal3Bundler.h:93
Vector2 expectedMeasurement(1.2431567, 1.2525694)
Cal3Bundler0 retract(const Vector &d) const
Annotation indicating that a class derives from another given type.
Definition: attr.h:61
Vector3 Point3
Definition: Point3.h:38
TEST(SmartFactorBase, Pinhole)
Definition: camera.h:36
The matrix class, also used for vectors and row-vectors.
Point3 point(10, 0, -5)
Camera camera(Pose3(Rot3().retract(Vector3(0.1, 0.2, 0.3)), Point3(0, 5, 0)), Cal3Bundler0(1, 0, 0))
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
3D Pose
Calibration used by Bundler.
std::ptrdiff_t j
double py() const
image center in y
Definition: Cal3Bundler.h:96
The most common 5DOF 3D->2D calibration.
size_t traceSize() const
Return size needed for memory buffer in traceExecution.
void AngleAxisToRotationMatrix(const T *angle_axis, T *R)
Definition: rotation.h:390


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:37:47