testCustomChartExpression.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_unstable/nonlinear/expressionTesting.h>
22 
23 using namespace gtsam;
24 
25 // A simple prototype custom chart that does two things:
26 // 1. it reduces the dimension of the variable being estimated
27 // 2. it scales the input to retract.
28 //
29 // The Jacobian of this chart is:
30 // [ 2 0 ]
31 // [ 0 3 ]
32 // [ 0 0 ]
34  typedef Eigen::Vector3d type;
35  typedef Eigen::Vector2d vector;
36  static vector local(const type& origin, const type& other) {
37  return (other - origin).head<2>();
38  }
39  static type retract(const type& origin, const vector& d) {
40  return origin + Eigen::Vector3d(2.0 * d[0], 3.0 * d[1], 0.0);
41  }
42  static int getDimension(const type& origin) {
43  return 2;
44  }
45 };
46 
47 namespace gtsam {
48 namespace traits {
49 template<> struct is_chart<ProjectionChart> : public boost::true_type {};
50 template<> struct dimension<ProjectionChart> : public boost::integral_constant<int, 2> {};
51 } // namespace traits
52 } // namespace gtsam
53 
54 TEST(ExpressionCustomChart, projection) {
55  // Create expression
57 
58  Eigen::Vector3d pval(1.0, 2.0, 3.0);
59  Values standard;
60  standard.insert(1, pval);
61 
62  Values custom;
63  custom.insert(1, pval, ProjectionChart());
64 
65  Eigen::Vector3d pstandard = point.value(standard);
66  Eigen::Vector3d pcustom = point.value(custom);
67 
68  // The values should be the same.
69  EXPECT(assert_equal(Matrix(pstandard), Matrix(pcustom), 1e-10));
70 
71 
72  EXPECT_LONGS_EQUAL(3, standard.at(1).dim());
73  VectorValues vstandard = standard.zeroVectors();
74  EXPECT_LONGS_EQUAL(3, vstandard.at(1).size());
75 
76 
77  EXPECT_LONGS_EQUAL(2, custom.at(1).dim());
78  VectorValues vcustom = custom.zeroVectors();
79  EXPECT_LONGS_EQUAL(2, vcustom.at(1).size());
80 
82 
83  boost::shared_ptr<GaussianFactor> gfstandard = f.linearize(standard);
84  boost::shared_ptr<JacobianFactor> jfstandard = //
85  boost::dynamic_pointer_cast<JacobianFactor>(gfstandard);
86 
87  typedef std::pair<Eigen::MatrixXd, Eigen::VectorXd> Jacobian;
88  Jacobian Jstandard = jfstandard->jacobianUnweighted();
89  EXPECT(assert_equal(Eigen::Matrix3d::Identity(), Jstandard.first, 1e-10));
90 
91  boost::shared_ptr<GaussianFactor> gfcustom = f.linearize(custom);
92  boost::shared_ptr<JacobianFactor> jfcustom = //
93  boost::dynamic_pointer_cast<JacobianFactor>(gfcustom);
94 
95  Eigen::MatrixXd expectedJacobian = Eigen::MatrixXd::Zero(3,2);
96  expectedJacobian(0,0) = 2.0;
97  expectedJacobian(1,1) = 3.0;
98  Jacobian Jcustom = jfcustom->jacobianUnweighted();
99  EXPECT(assert_equal(expectedJacobian, Jcustom.first, 1e-10));
100 
101  // Amazingly, the finite differences get the expected Jacobian right.
102  const double fd_step = 1e-5;
103  const double tolerance = 1e-5;
104  EXPECT_CORRECT_EXPRESSION_JACOBIANS(point, custom, fd_step, tolerance);
105 }
106 
107 /* ************************************************************************* */
108 int main() {
109  TestResult tr;
110  return TestRegistry::runAllTests(tr);
111 }
112 /* ************************************************************************* */
113 
std::pair< Matrix, Vector > jacobianUnweighted() const
Returns (dense) A,b pair associated with factor, does not bake in weights.
static int runAllTests(TestResult &result)
VectorValues zeroVectors() const
Definition: Values.cpp:216
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 origin
void insert(Key j, const Value &val)
Definition: Values.cpp:140
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:43
static type retract(const type &origin, const vector &d)
static shared_ptr Create(size_t dim)
Definition: NoiseModel.h:608
#define EXPECT_CORRECT_EXPRESSION_JACOBIANS(expression, values, numerical_derivative_step, tolerance)
Check the Jacobians produced by an expression against finite differences.
Point3 point(10, 0,-5)
const ValueType at(Key j) const
Definition: Values-inl.h:342
static vector local(const type &origin, const type &other)
#define EXPECT(condition)
Definition: Test.h:151
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Array< double, 1, 3 > e(1./3., 0.5, 2.)
traits
Definition: chartTesting.h:28
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:42
#define EXPECT_LONGS_EQUAL(expected, actual)
Definition: Test.h:155
Expression< Point2 > projection(f, p_cam)
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...
TEST(LPInitSolver, InfiniteLoopSingleVar)
static int getDimension(const type &origin)


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