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 /* ************************************************************************* */
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
example::point
Point3 point(10, 0, -5)
base
Annotation indicating that a class derives from another given type.
Definition: attr.h:64
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
d
static const double d[K][N]
Definition: igam.h:11
example::X
Vector3 X
Definition: testAdaptAutoDiff.cpp:167
v0
static const double v0
Definition: testCal3DFisheye.cpp:31
EXPECT_LONGS_EQUAL
#define EXPECT_LONGS_EQUAL(expected, actual)
Definition: Test.h:154
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
Cal3_S2.h
The most common 5DOF 3D->2D calibration.
TestHarness.h
Camera
Definition: camera.h:36
x
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
Definition: gnuplot_common_settings.hh:12
gtsam::Cal3Bundler0
Definition: testAdaptAutoDiff.cpp:36
E2
E2
Definition: test_numpy_dtypes.cpp:103
gtsam::internal::upAligned
T upAligned(T value, unsigned requiredAlignment=TraceAlignment)
Definition: ExpressionNode.h:50
SnavelyProjection
Definition: example.h:42
gtsam::equal_with_abs_tol
bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen::DenseBase< MATRIX > &B, double tol=1e-9)
Definition: base/Matrix.h:80
gtsam::utils.numerical_derivative.retract
def retract(a, np.ndarray xi)
Definition: numerical_derivative.py:44
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
main
int main()
Definition: testAdaptAutoDiff.cpp:258
gtsam::Cal3Bundler::vector
Vector3 vector() const
Definition: Cal3Bundler.cpp:42
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
gtsam::internal::Manifold
Both ManifoldTraits and Testable.
Definition: Manifold.h:117
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
rows
int rows
Definition: Tutorial_commainit_02.cpp:1
A
Matrix< SCALARA, Dynamic, Dynamic, opt_A > A
Definition: bench_gemm.cpp:48
gtsam::Expression
Definition: Expression.h:47
adapted
Vector2 adapted(const Vector9 &P, const Vector3 &X)
Definition: testAdaptAutoDiff.cpp:152
example
Definition: testOrdering.cpp:28
gtsam::Expression::keys
std::set< Key > keys() const
Return keys that play in this expression.
Definition: Expression-inl.h:127
gtsam::Cal3Bundler0::retract
Cal3Bundler0 retract(const Vector &d) const
Definition: testAdaptAutoDiff.cpp:40
RowMajorAccess
T & RowMajorAccess(T *base, int rows, int cols, int i, int j)
Definition: testAdaptAutoDiff.cpp:72
numericalDerivative.h
Some functions to compute numerical derivatives.
j
std::ptrdiff_t j
Definition: tut_arithmetic_redux_minmax.cpp:2
gtsam::Cal3Bundler::k1
double k1() const
distorsion parameter k1
Definition: Cal3Bundler.h:87
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
gtsam::AdaptAutoDiff
Definition: AdaptAutoDiff.h:35
gtsam::Pose3
Definition: Pose3.h:37
parameters
static ConjugateGradientParameters parameters
Definition: testIterative.cpp:33
gtsam::PinholeCamera
Definition: PinholeCamera.h:33
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
PinholeCamera.h
Base class for all pinhole cameras.
gtsam::Cal3Bundler
Calibration used by Bundler.
Definition: Cal3Bundler.h:32
gtsam::Cal3Bundler0::localCoordinates
Vector3 localCoordinates(const Cal3Bundler0 &T2) const
Definition: testAdaptAutoDiff.cpp:43
Eigen::Projective
@ Projective
Definition: Constants.h:464
Vector2
Definition: test_operator_overloading.cpp:18
T2
static const Pose3 T2(Rot3::Rodrigues(0.3, 0.2, 0.1), P2)
Eigen::Triplet< double >
gtsam::Cal3Bundler::py
double py() const
image center in y
Definition: Cal3Bundler.h:96
size_t
std::size_t size_t
Definition: wrap/pybind11/include/pybind11/detail/common.h:490
TestResult
Definition: TestResult.h:26
gtsam::Cal3Bundler::k2
double k2() const
distorsion parameter k2
Definition: Cal3Bundler.h:90
u0
static const double u0
Definition: testCal3DFisheye.cpp:31
tree::f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Definition: testExpression.cpp:218
gtsam::internal::BinaryExpression::Record
Definition: ExpressionNode.h:388
gtsam::Cal3::fx
double fx() const
focal length x
Definition: Cal3.h:139
Projective::operator()
bool operator()(A const P[12], A const X[4], A x[2]) const
Definition: testAdaptAutoDiff.cpp:87
gtsam
traits
Definition: SFMdata.h:40
gtsam::Cal3Bundler::px
double px() const
image center in x
Definition: Cal3Bundler.h:93
gtsam::traits
Definition: Group.h:36
CHECK
#define CHECK(condition)
Definition: Test.h:108
gtsam::Expression::traceSize
size_t traceSize() const
Return size needed for memory buffer in traceExecution.
Definition: Expression-inl.h:160
std
Definition: BFloat16.h:88
AdaptAutoDiff.h
Adaptor for Ceres style auto-differentiated functions.
example::camera
Camera camera(Pose3(Rot3().retract(Vector3(0.1, 0.2, 0.3)), Point3(0, 5, 0)), Cal3Bundler0(1, 0, 0))
ceres::internal::AutoDiff
Definition: autodiff.h:209
example::P
Vector9 P
Definition: testAdaptAutoDiff.cpp:166
Projective::operator()
Vector2 operator()(const MatrixRowMajor &P, const Vector4 &X) const
Definition: testAdaptAutoDiff.cpp:104
gtsam::Camera
PinholeCamera< Cal3Bundler0 > Camera
Definition: testAdaptAutoDiff.cpp:52
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
example.h
Eigen::PlainObjectBase::data
EIGEN_DEVICE_FUNC const EIGEN_STRONG_INLINE Scalar * data() const
Definition: PlainObjectBase.h:247
gtsam::Cal3Bundler0::Cal3Bundler0
Cal3Bundler0(double f=0, double k1=0, double k2=0, double u0=0, double v0=0)
Definition: testAdaptAutoDiff.cpp:37
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
Eigen::Matrix
The matrix class, also used for vectors and row-vectors.
Definition: 3rdparty/Eigen/Eigen/src/Core/Matrix.h:178
TEST
TEST(AdaptAutoDiff, Rotation)
Definition: testAdaptAutoDiff.cpp:60
cols
int cols
Definition: Tutorial_commainit_02.cpp:1
ceres::AngleAxisToRotationMatrix
void AngleAxisToRotationMatrix(const T *angle_axis, T *R)
Definition: rotation.h:390
Expression.h
Expressions for Block Automatic Differentiation.
Cal3Bundler.h
Calibration used by Bundler.
Projective
Definition: testAdaptAutoDiff.cpp:83
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
RandDouble
double RandDouble()
Definition: testAdaptAutoDiff.cpp:76
Pose3.h
3D Pose
E1
E1
Definition: test_numpy_dtypes.cpp:102
example::expectedMeasurement
Vector2 expectedMeasurement(1.2431567, 1.2525694)


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:39:33