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


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