testExpression.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  * -------------------------------1------------------------------------------- */
11 
20 #include <gtsam/nonlinear/expressions.h>
21 #include <gtsam/geometry/Cal3_S2.h>
22 #include <gtsam/geometry/PinholeCamera.h>
23 #include <gtsam/geometry/Point3.h>
24 #include <gtsam/base/Testable.h>
25 
26 #include <CppUnitLite/TestHarness.h>
27 
28 #include <boost/assign/list_of.hpp>
29 using boost::assign::list_of;
30 using boost::assign::map_list_of;
31 
32 using namespace std;
33 using namespace gtsam;
34 
35 typedef Expression<Point3> Point3_;
36 typedef Expression<Pose3> Pose3_;
37 typedef Expression<Rot3> Rot3_;
38 
39 /* ************************************************************************* */
40 template <class CAL>
41 Point2 uncalibrate(const CAL& K, const Point2& p, OptionalJacobian<2, 5> Dcal,
42  OptionalJacobian<2, 2> Dp) {
43  return K.uncalibrate(p, Dcal, Dp);
44 }
45 
46 static const Rot3 someR = Rot3::RzRyRx(1, 2, 3);
47 
48 /* ************************************************************************* */
49 // Constant
50 TEST(Expression, Constant) {
51  Rot3_ R(someR);
52  Values values;
53  Rot3 actual = R.value(values);
54  EXPECT(assert_equal(someR, actual));
55  EXPECT_LONGS_EQUAL(0, R.traceSize())
56 }
57 
58 /* ************************************************************************* */
59 // Leaf
60 TEST(Expression, Leaf) {
61  Rot3_ R(100);
62  Values values;
63  values.insert(100, someR);
64 
65  Rot3 actual2 = R.value(values);
66  EXPECT(assert_equal(someR, actual2));
67 }
68 
69 /* ************************************************************************* */
70 // Many Leaves
71 TEST(Expression, Leaves) {
72  Values values;
73  Point3 somePoint(1, 2, 3);
74  values.insert(Symbol('p', 10), somePoint);
75  std::vector<Point3_> points = createUnknowns<Point3>(10, 'p', 1);
76  EXPECT(assert_equal(somePoint, points.back().value(values)));
77 }
78 
79 /* ************************************************************************* */
80 // Unary(Leaf)
81 namespace unary {
82 Point2 f1(const Point3& p, OptionalJacobian<2, 3> H) {
83  return Point2(0,0);
84 }
85 double f2(const Point3& p, OptionalJacobian<1, 3> H) {
86  return 0.0;
87 }
88 Vector f3(const Point3& p, OptionalJacobian<Eigen::Dynamic, 3> H) {
89  return p;
90 }
91 Point3_ p(1);
92 set<Key> expected = list_of(1);
93 } // namespace unary
94 
95 TEST(Expression, Unary1) {
96  using namespace unary;
97  Expression<Point2> e(f1, p);
98  EXPECT(expected == e.keys());
99 }
100 TEST(Expression, Unary2) {
101  using namespace unary;
102  Double_ e(f2, p);
103  EXPECT(expected == e.keys());
104 }
105 
106 /* ************************************************************************* */
107 // Unary(Leaf), dynamic
108 TEST(Expression, Unary3) {
109  using namespace unary;
110  // Expression<Vector> e(f3, p);
111 }
112 
113 /* ************************************************************************* */
114 class Class : public Point3 {
115  public:
116  enum {dimension = 3};
117  using Point3::Point3;
118  const Vector3& vector() const { return *this; }
119  inline static Class identity() { return Class(0,0,0); }
120  double norm(OptionalJacobian<1,3> H = boost::none) const {
121  return norm3(*this, H);
122  }
123  bool equals(const Class &q, double tol) const {
124  return (std::abs(x() - q.x()) < tol && std::abs(y() - q.y()) < tol && std::abs(z() - q.z()) < tol);
125  }
126  void print(const string& s) const { cout << s << *this << endl;}
127 };
128 
129 namespace gtsam {
130 template<> struct traits<Class> : public internal::VectorSpace<Class> {};
131 }
132 
133 // Nullary Method
134 TEST(Expression, NullaryMethod) {
135  // Create expression
136  Expression<Class> p(67);
137  Expression<double> norm_(p, &Class::norm);
138 
139  // Create Values
140  Values values;
141  values.insert(67, Class(3, 4, 5));
142 
143  // Check dims as map
144  std::map<Key, int> map;
145  norm_.dims(map);
146  LONGS_EQUAL(1, map.size());
147 
148  // Get value and Jacobians
149  std::vector<Matrix> H(1);
150  double actual = norm_.value(values, H);
151 
152  // Check all
153  EXPECT(actual == sqrt(50));
154  Matrix expected(1, 3);
155  expected << 3.0 / sqrt(50.0), 4.0 / sqrt(50.0), 5.0 / sqrt(50.0);
156  EXPECT(assert_equal(expected, H[0]));
157 }
158 
159 /* ************************************************************************* */
160 // Binary(Leaf,Leaf)
161 namespace binary {
162 // Create leaves
163 double doubleF(const Pose3& pose, //
164  const Point3& point, OptionalJacobian<1, 6> H1, OptionalJacobian<1, 3> H2) {
165  return 0.0;
166 }
167 Pose3_ x(1);
168 Point3_ p(2);
169 Point3_ p_cam(x, &Pose3::transformTo, p);
170 }
171 
172 /* ************************************************************************* */
173 // Check that creating an expression to double compiles
174 TEST(Expression, BinaryToDouble) {
175  using namespace binary;
176  Double_ p_cam(doubleF, x, p);
177 }
178 
179 /* ************************************************************************* */
180 // keys
181 TEST(Expression, BinaryKeys) {
182  set<Key> expected = list_of(1)(2);
183  EXPECT(expected == binary::p_cam.keys());
184 }
185 
186 /* ************************************************************************* */
187 // dimensions
188 TEST(Expression, BinaryDimensions) {
189  map<Key, int> actual, expected = map_list_of<Key, int>(1, 6)(2, 3);
190  binary::p_cam.dims(actual);
191  EXPECT(actual == expected);
192 }
193 
194 /* ************************************************************************* */
195 // dimensions
196 TEST(Expression, BinaryTraceSize) {
197  typedef internal::BinaryExpression<Point3, Pose3, Point3> Binary;
198  size_t expectedTraceSize = sizeof(Binary::Record);
199  internal::upAlign(expectedTraceSize);
200  EXPECT_LONGS_EQUAL(expectedTraceSize, binary::p_cam.traceSize());
201 }
202 
203 /* ************************************************************************* */
204 // Binary(Leaf,Unary(Binary(Leaf,Leaf)))
205 namespace tree {
206 using namespace binary;
207 // Create leaves
208 Expression<Cal3_S2> K(3);
209 
210 // Create expression tree
211 Point2 (*f)(const Point3&, OptionalJacobian<2, 3>) = &PinholeBase::Project;
212 Expression<Point2> projection(f, p_cam);
213 Expression<Point2> uv_hat(uncalibrate<Cal3_S2>, K, projection);
214 }
215 
216 /* ************************************************************************* */
217 // keys
218 TEST(Expression, TreeKeys) {
219  set<Key> expected = list_of(1)(2)(3);
220  EXPECT(expected == tree::uv_hat.keys());
221 }
222 
223 /* ************************************************************************* */
224 // dimensions
225 TEST(Expression, TreeDimensions) {
226  map<Key, int> actual, expected = map_list_of<Key, int>(1, 6)(2, 3)(3, 5);
227  tree::uv_hat.dims(actual);
228  EXPECT(actual == expected);
229 }
230 
231 /* ************************************************************************* */
232 // TraceSize
233 TEST(Expression, TreeTraceSize) {
234  typedef internal::BinaryExpression<Point3, Pose3, Point3> Binary1;
235  EXPECT_LONGS_EQUAL(internal::upAligned(sizeof(Binary1::Record)), tree::p_cam.traceSize());
236 
237  typedef internal::UnaryExpression<Point2, Point3> Unary;
238  EXPECT_LONGS_EQUAL(internal::upAligned(sizeof(Unary::Record)) + tree::p_cam.traceSize(),
239  tree::projection.traceSize());
240 
241  EXPECT_LONGS_EQUAL(0, tree::K.traceSize());
242 
243  typedef internal::BinaryExpression<Point2, Cal3_S2, Point2> Binary2;
244  EXPECT_LONGS_EQUAL(internal::upAligned(sizeof(Binary2::Record)) + tree::K.traceSize() +
245  tree::projection.traceSize(),
246  tree::uv_hat.traceSize());
247 }
248 
249 /* ************************************************************************* */
250 TEST(Expression, compose1) {
251  // Create expression
252  Rot3_ R1(1), R2(2);
253  Rot3_ R3 = R1 * R2;
254 
255  // Check keys
256  set<Key> expected = list_of(1)(2);
257  EXPECT(expected == R3.keys());
258 }
259 
260 /* ************************************************************************* */
261 // Test compose with arguments referring to the same rotation
262 TEST(Expression, compose2) {
263  // Create expression
264  Rot3_ R1(1), R2(1);
265  Rot3_ R3 = R1 * R2;
266 
267  // Check keys
268  set<Key> expected = list_of(1);
269  EXPECT(expected == R3.keys());
270 }
271 
272 /* ************************************************************************* */
273 // Test compose with one arguments referring to constant rotation
274 TEST(Expression, compose3) {
275  // Create expression
276  Rot3_ R1(Rot3::identity()), R2(3);
277  Rot3_ R3 = R1 * R2;
278 
279  // Check keys
280  set<Key> expected = list_of(3);
281  EXPECT(expected == R3.keys());
282 }
283 
284 /* ************************************************************************* */
285 // Test with ternary function
286 Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3, OptionalJacobian<3, 3> H1,
287  OptionalJacobian<3, 3> H2, OptionalJacobian<3, 3> H3) {
288  // return dummy derivatives (not correct, but that's ok for testing here)
289  if (H1)
290  *H1 = I_3x3;
291  if (H2)
292  *H2 = I_3x3;
293  if (H3)
294  *H3 = I_3x3;
295  return R1 * (R2 * R3);
296 }
297 
298 TEST(Expression, ternary) {
299  // Create expression
300  Rot3_ A(1), B(2), C(3);
301  Rot3_ ABC(composeThree, A, B, C);
302 
303  // Check keys
304  set<Key> expected = list_of(1)(2)(3);
305  EXPECT(expected == ABC.keys());
306 }
307 
308 /* ************************************************************************* */
309 TEST(Expression, ScalarMultiply) {
310  const Key key(67);
311  const Point3_ expr = 23 * Point3_(key);
312 
313  set<Key> expected_keys = list_of(key);
314  EXPECT(expected_keys == expr.keys());
315 
316  map<Key, int> actual_dims, expected_dims = map_list_of<Key, int>(key, 3);
317  expr.dims(actual_dims);
318  EXPECT(actual_dims == expected_dims);
319 
320  // Check dims as map
321  std::map<Key, int> map;
322  expr.dims(map);
323  LONGS_EQUAL(1, map.size());
324 
325  Values values;
326  values.insert<Point3>(key, Point3(1, 0, 2));
327 
328  // Check value
329  const Point3 expected(23, 0, 46);
330  EXPECT(assert_equal(expected, expr.value(values)));
331 
332  // Check value + Jacobians
333  std::vector<Matrix> H(1);
334  EXPECT(assert_equal(expected, expr.value(values, H)));
335  EXPECT(assert_equal(23 * I_3x3, H[0]));
336 }
337 
338 /* ************************************************************************* */
339 TEST(Expression, BinarySum) {
340  const Key key(67);
341  const Point3_ sum_ = Point3_(key) + Point3_(Point3(1, 1, 1));
342 
343  set<Key> expected_keys = list_of(key);
344  EXPECT(expected_keys == sum_.keys());
345 
346  map<Key, int> actual_dims, expected_dims = map_list_of<Key, int>(key, 3);
347  sum_.dims(actual_dims);
348  EXPECT(actual_dims == expected_dims);
349 
350  // Check dims as map
351  std::map<Key, int> map;
352  sum_.dims(map);
353  LONGS_EQUAL(1, map.size());
354 
355  Values values;
356  values.insert<Point3>(key, Point3(2, 2, 2));
357 
358  // Check value
359  const Point3 expected(3, 3, 3);
360  EXPECT(assert_equal(expected, sum_.value(values)));
361 
362  // Check value + Jacobians
363  std::vector<Matrix> H(1);
364  EXPECT(assert_equal(expected, sum_.value(values, H)));
365  EXPECT(assert_equal(I_3x3, H[0]));
366 }
367 
368 /* ************************************************************************* */
369 TEST(Expression, TripleSum) {
370  const Key key(67);
371  const Point3_ p1_(Point3(1, 1, 1)), p2_(key);
372  const Expression<Point3> sum_ = p1_ + p2_ + p1_;
373 
374  LONGS_EQUAL(1, sum_.keys().size());
375 
376  Values values;
377  values.insert<Point3>(key, Point3(2, 2, 2));
378 
379  // Check value
380  const Point3 expected(4, 4, 4);
381  EXPECT(assert_equal(expected, sum_.value(values)));
382 
383  // Check value + Jacobians
384  std::vector<Matrix> H(1);
385  EXPECT(assert_equal(expected, sum_.value(values, H)));
386  EXPECT(assert_equal(I_3x3, H[0]));
387 }
388 
389 /* ************************************************************************* */
390 TEST(Expression, PlusEqual) {
391  const Key key(67);
392  const Point3_ p1_(Point3(1, 1, 1)), p2_(key);
393  Expression<Point3> sum_ = p1_;
394  sum_ += p2_;
395  sum_ += p1_;
396 
397  LONGS_EQUAL(1, sum_.keys().size());
398 
399  Values values;
400  values.insert<Point3>(key, Point3(2, 2, 2));
401 
402  // Check value
403  const Point3 expected(4, 4, 4);
404  EXPECT(assert_equal(expected, sum_.value(values)));
405 
406  // Check value + Jacobians
407  std::vector<Matrix> H(1);
408  EXPECT(assert_equal(expected, sum_.value(values, H)));
409  EXPECT(assert_equal(I_3x3, H[0]));
410 }
411 
412 /* ************************************************************************* */
413 TEST(Expression, SumOfUnaries) {
414  const Key key(67);
415  const Double_ norm_(&gtsam::norm3, Point3_(key));
416  const Double_ sum_ = norm_ + norm_;
417 
418  Values values;
419  values.insert<Point3>(key, Point3(6, 0, 0));
420 
421  // Check value
422  EXPECT_DOUBLES_EQUAL(12, sum_.value(values), 1e-9);
423 
424  // Check value + Jacobians
425  std::vector<Matrix> H(1);
426  EXPECT_DOUBLES_EQUAL(12, sum_.value(values, H), 1e-9);
427  EXPECT(assert_equal(Vector3(2, 0, 0).transpose(), H[0]));
428 }
429 
430 /* ************************************************************************* */
431 TEST(Expression, UnaryOfSum) {
432  const Key key1(42), key2(67);
433  const Point3_ sum_ = Point3_(key1) + Point3_(key2);
434  const Double_ norm_(&gtsam::norm3, sum_);
435 
436  map<Key, int> actual_dims, expected_dims = map_list_of<Key, int>(key1, 3)(key2, 3);
437  norm_.dims(actual_dims);
438  EXPECT(actual_dims == expected_dims);
439 
440  Values values;
441  values.insert<Point3>(key1, Point3(1, 0, 0));
442  values.insert<Point3>(key2, Point3(0, 1, 0));
443 
444  // Check value
445  EXPECT_DOUBLES_EQUAL(sqrt(2), norm_.value(values), 1e-9);
446 
447  // Check value + Jacobians
448  std::vector<Matrix> H(2);
449  EXPECT_DOUBLES_EQUAL(sqrt(2), norm_.value(values, H), 1e-9);
450  EXPECT(assert_equal(0.5 * sqrt(2) * Vector3(1, 1, 0).transpose(), H[0]));
451  EXPECT(assert_equal(0.5 * sqrt(2) * Vector3(1, 1, 0).transpose(), H[1]));
452 }
453 
454 /* ************************************************************************* */
455 TEST(Expression, WeightedSum) {
456  const Key key1(42), key2(67);
457  const Point3_ weighted_sum_ = 17 * Point3_(key1) + 23 * Point3_(key2);
458 
459  map<Key, int> actual_dims, expected_dims = map_list_of<Key, int>(key1, 3)(key2, 3);
460  weighted_sum_.dims(actual_dims);
461  EXPECT(actual_dims == expected_dims);
462 
463  Values values;
464  values.insert<Point3>(key1, Point3(1, 0, 0));
465  values.insert<Point3>(key2, Point3(0, 1, 0));
466 
467  // Check value
468  const Point3 expected = 17 * Point3(1, 0, 0) + 23 * Point3(0, 1, 0);
469  EXPECT(assert_equal(expected, weighted_sum_.value(values)));
470 
471  // Check value + Jacobians
472  std::vector<Matrix> H(2);
473  EXPECT(assert_equal(expected, weighted_sum_.value(values, H)));
474  EXPECT(assert_equal(17 * I_3x3, H[0]));
475  EXPECT(assert_equal(23 * I_3x3, H[1]));
476 }
477 
478 /* ************************************************************************* */
479 TEST(Expression, Subtract) {
480  const Vector3 p = Vector3::Random(), q = Vector3::Random();
481  Values values;
482  values.insert(0, p);
483  values.insert(1, q);
484  const Vector3_ expression = Vector3_(0) - Vector3_(1);
485  set<Key> expected_keys = {0, 1};
486  EXPECT(expression.keys() == expected_keys);
487 
488  // Check value + Jacobians
489  std::vector<Matrix> H(2);
490  EXPECT(assert_equal<Vector3>(p - q, expression.value(values, H)));
491  EXPECT(assert_equal(I_3x3, H[0]));
492  EXPECT(assert_equal(-I_3x3, H[1]));
493 }
494 
495 /* ************************************************************************* */
496 TEST(Expression, LinearExpression) {
497  const Key key(67);
498  const boost::function<Vector3(Point3)> f = [](const Point3& p) { return (Vector3)p; };
499  const Matrix3 kIdentity = I_3x3;
500  const Expression<Vector3> linear_ = linearExpression(f, Point3_(key), kIdentity);
501 
502  Values values;
503  values.insert<Point3>(key, Point3(1, 0, 2));
504 
505  // Check value
506  const Vector3 expected(1, 0, 2);
507  EXPECT(assert_equal(expected, linear_.value(values)));
508 
509  // Check value + Jacobians
510  std::vector<Matrix> H(1);
511  EXPECT(assert_equal(expected, linear_.value(values, H)));
512  EXPECT(assert_equal(I_3x3, H[0]));
513 }
514 
515 /* ************************************************************************* */
516 int main() {
517  TestResult tr;
518  return TestRegistry::runAllTests(tr);
519 }
520 /* ************************************************************************* */
Point3_
Expression< Point3 > Point3_
Definition: testExpression.cpp:35
y
Scalar * y
Definition: level1_cplx_impl.h:102
Class::identity
static Class identity()
Definition: testExpression.cpp:119
Testable.h
Concept check for values that can be used in unit tests.
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
TEST
TEST(Expression, Constant)
Definition: testExpression.cpp:50
gtsam::internal::VectorSpace
VectorSpace provides both Testable and VectorSpaceTraits.
Definition: VectorSpace.h:207
gtsam::Values::insert
void insert(Key j, const Value &val)
Definition: Values.cpp:140
Class::norm
double norm(OptionalJacobian< 1, 3 > H=boost::none) const
Definition: testExpression.cpp:120
gtsam::Point2
Vector2 Point2
Definition: Point2.h:27
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:43
Pose3_
Expression< Pose3 > Pose3_
Definition: testExpression.cpp:36
main
int main()
Definition: testExpression.cpp:516
R
Rot2 R(Rot2::fromAngle(0.1))
leaf::values
leaf::MyValues values
sqrt
EIGEN_DEVICE_FUNC const SqrtReturnType sqrt() const
Definition: ArrayCwiseUnaryOps.h:152
std
Definition: Half.h:150
f2
double f2(const Vector2 &x)
Definition: testNumericalDerivative.cpp:56
unary::expected
set< Key > expected
Definition: testExpression.cpp:92
H
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 y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics set mxtics default set mytics default set mx2tics default set my2tics default set xtics border mirror norotate autofreq set ytics border mirror norotate autofreq set ztics border nomirror norotate autofreq set nox2tics set noy2tics set timestamp bottom norotate set rrange[*:*] noreverse nowriteback set trange[*:*] noreverse nowriteback set urange[*:*] noreverse nowriteback set vrange[*:*] noreverse nowriteback set xlabel matrix size set x2label set timefmt d m y n H
Definition: gnuplot_common_settings.hh:12
B
Matrix< SCALARB, Dynamic, Dynamic > B
Definition: bench_gemm.cpp:36
EXPECT_DOUBLES_EQUAL
#define EXPECT_DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:162
gtsam::internal::upAligned
T upAligned(T value, unsigned requiredAlignment=TraceAlignment)
Definition: ExpressionNode.h:50
Rot3_
Expression< Rot3 > Rot3_
Definition: testExpression.cpp:37
example::point
Point3 point(10, 0,-5)
key1
const Symbol key1('v', 1)
Class::equals
bool equals(const Class &q, double tol) const
Definition: testExpression.cpp:123
PinholeCamera.h
Base class for all pinhole cameras.
composeThree
Rot3 composeThree(const Rot3 &R1, const Rot3 &R2, const Rot3 &R3, OptionalJacobian< 3, 3 > H1, OptionalJacobian< 3, 3 > H2, OptionalJacobian< 3, 3 > H3)
Definition: testExpression.cpp:286
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
pose
static const Pose3 pose(Rot3(Vector3(1,-1,-1).asDiagonal()), Point3(0, 0, 0.5))
kIdentity
static const NavState kIdentity
Definition: testNavState.cpp:31
EXPECT
#define EXPECT(condition)
Definition: Test.h:151
gtsam::internal::upAlign
T & upAlign(T &value, unsigned requiredAlignment=TraceAlignment)
Definition: ExpressionNode.h:37
binary::p_cam
Point3_ p_cam(x,&Pose3::transformTo, p)
tree::f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Definition: testExpression.cpp:211
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
s
RealScalar s
Definition: level1_cplx_impl.h:104
Eigen::numext::q
EIGEN_DEVICE_FUNC const Scalar & q
Definition: SpecialFunctionsImpl.h:1520
someR
static const Rot3 someR
Definition: testExpression.cpp:46
Class::print
void print(const string &s) const
Definition: testExpression.cpp:126
R1
Q R1(Eigen::AngleAxisd(1, Q_z_axis))
gtsam::norm3
double norm3(const Point3 &p, OptionalJacobian< 1, 3 > H)
Distance of the point from the origin, with Jacobian.
Definition: Point3.cpp:40
gtsam::Expression::dims
void dims(std::map< Key, int > &map) const
Return dimensions for each argument, as a map.
Definition: Expression-inl.h:122
gtsam::Expression::keys
std::set< Key > keys() const
Return keys that play in this expression.
Definition: Expression-inl.h:117
tree::uv_hat
Expression< Point2 > uv_hat(uncalibrate< Cal3_S2 >, K, projection)
tree::K
Expression< Cal3_S2 > K(3)
C
Matrix< Scalar, Dynamic, Dynamic > C
Definition: bench_gemm.cpp:37
LONGS_EQUAL
#define LONGS_EQUAL(expected, actual)
Definition: Test.h:135
gtsam
traits
Definition: chartTesting.h:28
simple::R3
static Rot3 R3
Definition: testInitializePose3.cpp:55
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:42
EXPECT_LONGS_EQUAL
#define EXPECT_LONGS_EQUAL(expected, actual)
Definition: Test.h:155
tree::projection
Expression< Point2 > projection(f, p_cam)
unary::f1
Point2 f1(const Point3 &p, OptionalJacobian< 2, 3 > H)
Definition: testExpression.cpp:82
key2
const Symbol key2('v', 2)
gtsam::Expression::value
T value(const Values &values, boost::optional< std::vector< Matrix > & > H=boost::none) const
Return value and optional derivatives, reverse AD version Notes: this is not terribly efficient...
Definition: Expression-inl.h:132
Point3.h
3D Point
p
float * p
Definition: Tutorial_Map_using.cpp:9
Class::vector
const Vector3 & vector() const
Definition: testExpression.cpp:118
Project
Point2(* Project)(const Point3 &, OptionalJacobian< 2, 3 >)
Definition: testExpressionFactor.cpp:43
f3
double f3(double x1, double x2)
Definition: testNumericalDerivative.cpp:76
pybind_wrapper_test_script.z
z
Definition: pybind_wrapper_test_script.py:61
gtsam::Expression::traceSize
size_t traceSize() const
Return size needed for memory buffer in traceExecution.
Definition: Expression-inl.h:152
gtsam::transformTo
Line3 transformTo(const Pose3 &wTc, const Line3 &wL, OptionalJacobian< 4, 6 > Dpose, OptionalJacobian< 4, 4 > Dline)
Definition: Line3.cpp:94
gtsam::tol
const G double tol
Definition: Group.h:83
gtsam::Point3
Vector3 Point3
Definition: Point3.h:35
keys
const KeyVector keys
Definition: testRegularImplicitSchurFactor.cpp:45
gtsam::linearExpression
Expression< T > linearExpression(const boost::function< T(A)> &f, const Expression< A > &expression, const Eigen::Matrix< double, traits< T >::dimension, traits< A >::dimension > &dTdA)
Definition: Expression.h:242
abs
#define abs(x)
Definition: datatypes.h:17
R2
Q R2(Eigen::AngleAxisd(2, Vector3(0, 1, 0)))
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
binary::doubleF
double doubleF(const Pose3 &pose, const Point3 &point, OptionalJacobian< 1, 6 > H1, OptionalJacobian< 1, 3 > H2)
Definition: testExpression.cpp:163
Cal3_S2.h
The most common 5DOF 3D->2D calibration.
uncalibrate
Point2 uncalibrate(const CAL &K, const Point2 &p, OptionalJacobian< 2, 5 > Dcal, OptionalJacobian< 2, 2 > Dp)
Definition: testExpression.cpp:41
gtsam::Vector3_
Expression< Vector3 > Vector3_
Definition: nonlinear/expressions.h:31

gtsam
Author(s):
autogenerated on Sat May 8 2021 02:46:32