testRangeFactor.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 
19 #include <gtsam/sam/RangeFactor.h>
20 #include <gtsam/geometry/Pose3.h>
21 #include <gtsam/geometry/Pose2.h>
22 #include <gtsam/geometry/PinholeCamera.h>
23 #include <gtsam/geometry/Cal3_S2.h>
24 #include <gtsam/base/numericalDerivative.h>
25 #include <gtsam/base/serializationTestHelpers.h>
26 #include <gtsam/base/TestableAssertions.h>
27 
28 #include <CppUnitLite/TestHarness.h>
29 #include <boost/bind.hpp>
30 
31 using namespace std;
32 using namespace gtsam;
33 
34 // Create a noise model for the pixel error
35 static SharedNoiseModel model(noiseModel::Unit::Create(1));
36 
37 typedef RangeFactor<Pose2, Point2> RangeFactor2D;
38 typedef RangeFactor<Pose3, Point3> RangeFactor3D;
39 typedef RangeFactorWithTransform<Pose2, Point2> RangeFactorWithTransform2D;
40 typedef RangeFactorWithTransform<Pose3, Point3> RangeFactorWithTransform3D;
41 
42 // Keys are deliberately *not* in sorted order to test that case.
43 Key poseKey(2);
44 Key pointKey(1);
45 double measurement(10.0);
46 
47 /* ************************************************************************* */
48 Vector factorError2D(const Pose2& pose, const Point2& point,
49  const RangeFactor2D& factor) {
50  return factor.evaluateError(pose, point);
51 }
52 
53 /* ************************************************************************* */
54 Vector factorError3D(const Pose3& pose, const Point3& point,
55  const RangeFactor3D& factor) {
56  return factor.evaluateError(pose, point);
57 }
58 
59 /* ************************************************************************* */
60 Vector factorErrorWithTransform2D(const Pose2& pose, const Point2& point,
61  const RangeFactorWithTransform2D& factor) {
62  return factor.evaluateError(pose, point);
63 }
64 
65 /* ************************************************************************* */
66 Vector factorErrorWithTransform3D(const Pose3& pose, const Point3& point,
67  const RangeFactorWithTransform3D& factor) {
68  return factor.evaluateError(pose, point);
69 }
70 
71 /* ************************************************************************* */
72 TEST( RangeFactor, Constructor) {
73  RangeFactor2D factor2D(poseKey, pointKey, measurement, model);
74  RangeFactor3D factor3D(poseKey, pointKey, measurement, model);
75 }
76 
77 /* ************************************************************************* */
78 // Export Noisemodels
79 // See http://www.boost.org/doc/libs/1_32_0/libs/serialization/doc/special.html
80 BOOST_CLASS_EXPORT(gtsam::noiseModel::Unit);
81 
82 /* ************************************************************************* */
83 TEST(RangeFactor, Serialization2D) {
84  RangeFactor2D factor2D(poseKey, pointKey, measurement, model);
85  EXPECT(serializationTestHelpers::equalsObj(factor2D));
86  EXPECT(serializationTestHelpers::equalsXML(factor2D));
87  EXPECT(serializationTestHelpers::equalsBinary(factor2D));
88 }
89 
90 /* ************************************************************************* */
91 TEST(RangeFactor, Serialization3D) {
92  RangeFactor3D factor3D(poseKey, pointKey, measurement, model);
93  EXPECT(serializationTestHelpers::equalsObj(factor3D));
94  EXPECT(serializationTestHelpers::equalsXML(factor3D));
95  EXPECT(serializationTestHelpers::equalsBinary(factor3D));
96 }
97 
98 /* ************************************************************************* */
99 TEST( RangeFactor, ConstructorWithTransform) {
100  Pose2 body_P_sensor_2D(0.25, -0.10, -M_PI_2);
101  Pose3 body_P_sensor_3D(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2),
102  Point3(0.25, -0.10, 1.0));
103 
104  RangeFactorWithTransform2D factor2D(poseKey, pointKey, measurement, model,
105  body_P_sensor_2D);
106  KeyVector expected {2, 1};
107  CHECK(factor2D.keys() == expected);
108  RangeFactorWithTransform3D factor3D(poseKey, pointKey, measurement, model,
109  body_P_sensor_3D);
110  CHECK(factor3D.keys() == expected);
111 }
112 
113 /* ************************************************************************* */
114 TEST( RangeFactor, Equals ) {
115  // Create two identical factors and make sure they're equal
116  RangeFactor2D factor2D_1(poseKey, pointKey, measurement, model);
117  RangeFactor2D factor2D_2(poseKey, pointKey, measurement, model);
118  CHECK(assert_equal(factor2D_1, factor2D_2));
119 
120  RangeFactor3D factor3D_1(poseKey, pointKey, measurement, model);
121  RangeFactor3D factor3D_2(poseKey, pointKey, measurement, model);
122  CHECK(assert_equal(factor3D_1, factor3D_2));
123 }
124 
125 /* ************************************************************************* */
126 TEST( RangeFactor, EqualsWithTransform ) {
127  // Create two identical factors and make sure they're equal
128  Pose2 body_P_sensor_2D(0.25, -0.10, -M_PI_2);
129  Pose3 body_P_sensor_3D(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2),
130  Point3(0.25, -0.10, 1.0));
131 
132  RangeFactorWithTransform2D factor2D_1(poseKey, pointKey, measurement, model,
133  body_P_sensor_2D);
134  RangeFactorWithTransform2D factor2D_2(poseKey, pointKey, measurement, model,
135  body_P_sensor_2D);
136  CHECK(assert_equal(factor2D_1, factor2D_2));
137 
138  RangeFactorWithTransform3D factor3D_1(poseKey, pointKey, measurement, model,
139  body_P_sensor_3D);
140  RangeFactorWithTransform3D factor3D_2(poseKey, pointKey, measurement, model,
141  body_P_sensor_3D);
142  CHECK(assert_equal(factor3D_1, factor3D_2));
143 }
144 /* ************************************************************************* */
145 TEST( RangeFactor, EqualsAfterDeserializing) {
146  // Check that the same results are obtained after deserializing:
147  Pose3 body_P_sensor_3D(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2),
148  Point3(0.25, -0.10, 1.0));
149 
150  RangeFactorWithTransform3D factor3D_1(poseKey, pointKey, measurement, model,
151  body_P_sensor_3D), factor3D_2;
152 
153  // check with Equal() trait:
154  gtsam::serializationTestHelpers::roundtripXML(factor3D_1, factor3D_2);
155  CHECK(assert_equal(factor3D_1, factor3D_2));
156 
157  const Pose3 pose(Rot3::RzRyRx(0.2, -0.3, 1.75), Point3(1.0, 2.0, -3.0));
158  const Point3 point(-2.0, 11.0, 1.0);
159  const Values values = {{poseKey, genericValue(pose)}, {pointKey, genericValue(point)}};
160 
161  const Vector error_1 = factor3D_1.unwhitenedError(values);
162  const Vector error_2 = factor3D_2.unwhitenedError(values);
163  CHECK(assert_equal(error_1, error_2));
164 }
165 
166 /* ************************************************************************* */
167 TEST( RangeFactor, Error2D ) {
168  // Create a factor
169  RangeFactor2D factor(poseKey, pointKey, measurement, model);
170 
171  // Set the linearization point
172  Pose2 pose(1.0, 2.0, 0.57);
173  Point2 point(-4.0, 11.0);
174 
175  // Use the factor to calculate the error
176  Vector actualError(factor.unwhitenedError({{poseKey, genericValue(pose)}, {pointKey, genericValue(point)}}));
177 
178  // The expected error is ||(5.0, 9.0)|| - 10.0 = 0.295630141 meter / UnitCovariance
179  Vector expectedError = (Vector(1) << 0.295630141).finished();
180 
181  // Verify we get the expected error
182  CHECK(assert_equal(expectedError, actualError, 1e-9));
183 }
184 
185 /* ************************************************************************* */
186 TEST( RangeFactor, Error2DWithTransform ) {
187  // Create a factor
188  Pose2 body_P_sensor(0.25, -0.10, -M_PI_2);
189  RangeFactorWithTransform2D factor(poseKey, pointKey, measurement, model,
190  body_P_sensor);
191 
192  // Set the linearization point
193  Rot2 R(0.57);
194  Point2 t = Point2(1.0, 2.0) - R.rotate(body_P_sensor.translation());
195  Pose2 pose(R, t);
196  Point2 point(-4.0, 11.0);
197 
198  // Use the factor to calculate the error
199  Vector actualError(factor.unwhitenedError({{poseKey, genericValue(pose)}, {pointKey, genericValue(point)}}));
200 
201  // The expected error is ||(5.0, 9.0)|| - 10.0 = 0.295630141 meter / UnitCovariance
202  Vector expectedError = (Vector(1) << 0.295630141).finished();
203 
204  // Verify we get the expected error
205  CHECK(assert_equal(expectedError, actualError, 1e-9));
206 }
207 
208 /* ************************************************************************* */
209 TEST( RangeFactor, Error3D ) {
210  // Create a factor
211  RangeFactor3D factor(poseKey, pointKey, measurement, model);
212 
213  // Set the linearization point
214  Pose3 pose(Rot3::RzRyRx(0.2, -0.3, 1.75), Point3(1.0, 2.0, -3.0));
215  Point3 point(-2.0, 11.0, 1.0);
216 
217  // Use the factor to calculate the error
218  Vector actualError(factor.unwhitenedError({{poseKey, genericValue(pose)}, {pointKey, genericValue(point)}}));
219 
220  // The expected error is ||(3.0, 9.0, 4.0)|| - 10.0 = 0.295630141 meter / UnitCovariance
221  Vector expectedError = (Vector(1) << 0.295630141).finished();
222 
223  // Verify we get the expected error
224  CHECK(assert_equal(expectedError, actualError, 1e-9));
225 }
226 
227 /* ************************************************************************* */
228 TEST( RangeFactor, Error3DWithTransform ) {
229  // Create a factor
230  Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2),
231  Point3(0.25, -0.10, 1.0));
232  RangeFactorWithTransform3D factor(poseKey, pointKey, measurement, model,
233  body_P_sensor);
234 
235  // Set the linearization point
236  Rot3 R = Rot3::RzRyRx(0.2, -0.3, 1.75);
237  Point3 t = Point3(1.0, 2.0, -3.0) - R.rotate(body_P_sensor.translation());
238  Pose3 pose(R, t);
239  Point3 point(-2.0, 11.0, 1.0);
240 
241  // Use the factor to calculate the error
242  Vector actualError(factor.unwhitenedError({{poseKey, genericValue(pose)}, {pointKey, genericValue(point)}}));
243 
244  // The expected error is ||(3.0, 9.0, 4.0)|| - 10.0 = 0.295630141 meter / UnitCovariance
245  Vector expectedError = (Vector(1) << 0.295630141).finished();
246 
247  // Verify we get the expected error
248  CHECK(assert_equal(expectedError, actualError, 1e-9));
249 }
250 
251 /* ************************************************************************* */
252 TEST( RangeFactor, Jacobian2D ) {
253  // Create a factor
254  RangeFactor2D factor(poseKey, pointKey, measurement, model);
255 
256  // Set the linearization point
257  Pose2 pose(1.0, 2.0, 0.57);
258  Point2 point(-4.0, 11.0);
259 
260  // Use the factor to calculate the Jacobians
261  Matrix H1Actual, H2Actual;
262  factor.evaluateError(pose, point, H1Actual, H2Actual);
263 
264  // Use numerical derivatives to calculate the Jacobians
265  Matrix H1Expected, H2Expected;
266  H1Expected = numericalDerivative11<Vector, Pose2>(
267  boost::bind(&factorError2D, _1, point, factor), pose);
268  H2Expected = numericalDerivative11<Vector, Point2>(
269  boost::bind(&factorError2D, pose, _1, factor), point);
270 
271  // Verify the Jacobians are correct
272  CHECK(assert_equal(H1Expected, H1Actual, 1e-9));
273  CHECK(assert_equal(H2Expected, H2Actual, 1e-9));
274 }
275 
276 /* ************************************************************************* */
277 TEST( RangeFactor, Jacobian2DWithTransform ) {
278  // Create a factor
279  Pose2 body_P_sensor(0.25, -0.10, -M_PI_2);
280  RangeFactorWithTransform2D factor(poseKey, pointKey, measurement, model,
281  body_P_sensor);
282 
283  // Set the linearization point
284  Rot2 R(0.57);
285  Point2 t = Point2(1.0, 2.0) - R.rotate(body_P_sensor.translation());
286  Pose2 pose(R, t);
287  Point2 point(-4.0, 11.0);
288 
289  // Use the factor to calculate the Jacobians
290  std::vector<Matrix> actualHs(2);
291  factor.unwhitenedError({{poseKey, genericValue(pose)}, {pointKey, genericValue(point)}}, actualHs);
292  const Matrix& H1Actual = actualHs.at(0);
293  const Matrix& H2Actual = actualHs.at(1);
294 
295  // Use numerical derivatives to calculate the Jacobians
296  Matrix H1Expected, H2Expected;
297  H1Expected = numericalDerivative11<Vector, Pose2>(
298  boost::bind(&factorErrorWithTransform2D, _1, point, factor), pose);
299  H2Expected = numericalDerivative11<Vector, Point2>(
300  boost::bind(&factorErrorWithTransform2D, pose, _1, factor), point);
301 
302  // Verify the Jacobians are correct
303  CHECK(assert_equal(H1Expected, H1Actual, 1e-9));
304  CHECK(assert_equal(H2Expected, H2Actual, 1e-9));
305 }
306 
307 /* ************************************************************************* */
308 TEST( RangeFactor, Jacobian3D ) {
309  // Create a factor
310  RangeFactor3D factor(poseKey, pointKey, measurement, model);
311 
312  // Set the linearization point
313  Pose3 pose(Rot3::RzRyRx(0.2, -0.3, 1.75), Point3(1.0, 2.0, -3.0));
314  Point3 point(-2.0, 11.0, 1.0);
315 
316  // Use the factor to calculate the Jacobians
317  std::vector<Matrix> actualHs(2);
318  factor.unwhitenedError({{poseKey, genericValue(pose)}, {pointKey, genericValue(point)}}, actualHs);
319  const Matrix& H1Actual = actualHs.at(0);
320  const Matrix& H2Actual = actualHs.at(1);
321 
322  // Use numerical derivatives to calculate the Jacobians
323  Matrix H1Expected, H2Expected;
324  H1Expected = numericalDerivative11<Vector, Pose3>(
325  boost::bind(&factorError3D, _1, point, factor), pose);
326  H2Expected = numericalDerivative11<Vector, Point3>(
327  boost::bind(&factorError3D, pose, _1, factor), point);
328 
329  // Verify the Jacobians are correct
330  CHECK(assert_equal(H1Expected, H1Actual, 1e-9));
331  CHECK(assert_equal(H2Expected, H2Actual, 1e-9));
332 }
333 
334 /* ************************************************************************* */
335 TEST( RangeFactor, Jacobian3DWithTransform ) {
336  // Create a factor
337  Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2),
338  Point3(0.25, -0.10, 1.0));
339  RangeFactorWithTransform3D factor(poseKey, pointKey, measurement, model,
340  body_P_sensor);
341 
342  // Set the linearization point
343  Rot3 R = Rot3::RzRyRx(0.2, -0.3, 1.75);
344  Point3 t = Point3(1.0, 2.0, -3.0) - R.rotate(body_P_sensor.translation());
345  Pose3 pose(R, t);
346  Point3 point(-2.0, 11.0, 1.0);
347 
348  // Use the factor to calculate the Jacobians
349  std::vector<Matrix> actualHs(2);
350  factor.unwhitenedError({{poseKey, genericValue(pose)}, {pointKey, genericValue(point)}}, actualHs);
351  const Matrix& H1Actual = actualHs.at(0);
352  const Matrix& H2Actual = actualHs.at(1);
353 
354  // Use numerical derivatives to calculate the Jacobians
355  Matrix H1Expected, H2Expected;
356  H1Expected = numericalDerivative11<Vector, Pose3>(
357  boost::bind(&factorErrorWithTransform3D, _1, point, factor), pose);
358  H2Expected = numericalDerivative11<Vector, Point3>(
359  boost::bind(&factorErrorWithTransform3D, pose, _1, factor), point);
360 
361  // Verify the Jacobians are correct
362  CHECK(assert_equal(H1Expected, H1Actual, 1e-9));
363  CHECK(assert_equal(H2Expected, H2Actual, 1e-9));
364 }
365 
366 /* ************************************************************************* */
367 // Do a test with Point3
368 TEST(RangeFactor, Point3) {
369  // Create a factor
370  RangeFactor<Point3> factor(poseKey, pointKey, measurement, model);
371 
372  // Set the linearization point
373  Point3 pose(1.0, 2.0, 00);
374  Point3 point(-4.0, 11.0, 0);
375 
376  // The expected error is ||(5.0, 9.0)|| - 10.0 = 0.295630141 meter / UnitCovariance
377  Vector expectedError = (Vector(1) << 0.295630141).finished();
378 
379  // Verify we get the expected error
380  CHECK(assert_equal(expectedError, factor.unwhitenedError({{poseKey, genericValue(pose)}, {pointKey, genericValue(point)}}), 1e-9));
381 }
382 
383 /* ************************************************************************* */
384 // Do tests with PinholeCamera<Cal3_S2>
385 TEST( RangeFactor, Camera) {
386  using Camera = PinholeCamera<Cal3_S2>;
387 
388  RangeFactor<Camera, Point3> factor1(poseKey, pointKey, measurement, model);
389  RangeFactor<Camera, Pose3> factor2(poseKey, pointKey, measurement, model);
390  RangeFactor<Camera, Camera> factor3(poseKey, pointKey, measurement, model);
391 }
392 
393 /* ************************************************************************* */
394 // Do a test with non GTSAM types
395 
396 namespace gtsam{
397 template <>
398 struct Range<Vector4, Vector4> {
399  typedef double result_type;
400  double operator()(const Vector4& v1, const Vector4& v2,
401  OptionalJacobian<1, 4> H1, OptionalJacobian<1, 4> H2) {
402  return (v2 - v1).norm();
403  // derivatives not implemented
404  }
405 };
406 }
407 
408 TEST(RangeFactor, NonGTSAM) {
409  // Create a factor
410  Key poseKey(1);
411  Key pointKey(2);
412  double measurement(10.0);
413  RangeFactor<Vector4> factor(poseKey, pointKey, measurement, model);
414 
415  // Set the linearization point
416  Vector4 pose(1.0, 2.0, 00, 0);
417  Vector4 point(-4.0, 11.0, 0, 0);
418 
419  // The expected error is ||(5.0, 9.0)|| - 10.0 = 0.295630141 meter / UnitCovariance
420  Vector expectedError = (Vector(1) << 0.295630141).finished();
421 
422  // Verify we get the expected error
423  CHECK(assert_equal(expectedError, factor.unwhitenedError({{poseKey, genericValue(pose)}, {pointKey, genericValue(point)}}), 1e-9));
424 }
425 
426 /* ************************************************************************* */
427 int main() {
428  TestResult tr;
429  return TestRegistry::runAllTests(tr);
430 }
431 /* ************************************************************************* */
TestableAssertions.h
Provides additional testing facilities for common data structures.
CHECK
#define CHECK(condition)
Definition: Test.h:109
v2
Vector v2
Definition: testSerializationBase.cpp:36
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
v1
Vector v1
Definition: testSerializationBase.cpp:35
gtsam::Rot2::rotate
Point2 rotate(const Point2 &p, OptionalJacobian< 2, 1 > H1=boost::none, OptionalJacobian< 2, 2 > H2=boost::none) const
Definition: Rot2.cpp:104
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:974
gtsam::Point2
Vector2 Point2
Definition: Point2.h:27
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:43
gtsam::Range< Vector4, Vector4 >::operator()
double operator()(const Vector4 &v1, const Vector4 &v2, OptionalJacobian< 1, 4 > H1, OptionalJacobian< 1, 4 > H2)
Definition: testRangeFactor.cpp:400
R
Rot2 R(Rot2::fromAngle(0.1))
leaf::values
leaf::MyValues values
RangeFactorWithTransform3D
RangeFactorWithTransform< Pose3, Point3 > RangeFactorWithTransform3D
Definition: testRangeFactor.cpp:40
gtsam::ExpressionFactor::unwhitenedError
Vector unwhitenedError(const Values &x, boost::optional< std::vector< Matrix > & > H=boost::none) const override
Definition: ExpressionFactor.h:99
measurement
double measurement(10.0)
std
Definition: Half.h:150
numericalDerivative.h
Some functions to compute numerical derivatives.
gtsam::Pose3::translation
const Point3 & translation(OptionalJacobian< 3, 6 > Hself=boost::none) const
get translation
Definition: Pose3.cpp:259
RangeFactor3D
RangeFactor< Pose3, Point3 > RangeFactor3D
Definition: testRangeFactor.cpp:38
gtsam::Pose2::translation
const Point2 & translation() const
translation
Definition: Pose2.h:230
gtsam::Rot3::rotate
Point3 rotate(const Point3 &p, OptionalJacobian< 3, 3 > H1=boost::none, OptionalJacobian< 3, 3 > H2=boost::none) const
Definition: Rot3M.cpp:149
factorErrorWithTransform2D
Vector factorErrorWithTransform2D(const Pose2 &pose, const Point2 &point, const RangeFactorWithTransform2D &factor)
Definition: testRangeFactor.cpp:60
factor3D
BearingFactor3D factor3D(poseKey, pointKey, measurement3D, model3D)
gtsam::serializationTestHelpers::roundtripXML
void roundtripXML(const T &input, T &output)
Definition: serializationTestHelpers.h:95
simple_graph::factor3
JacobianFactor factor3(keyY, A32, keyZ, A33, b3, noiseModel::Isotropic::Sigma(2, sigma3))
main
int main()
Definition: testRangeFactor.cpp:427
example::point
Point3 point(10, 0,-5)
PinholeCamera.h
Base class for all pinhole cameras.
pointKey
Key pointKey(1)
factorError3D
Vector factorError3D(const Pose3 &pose, const Point3 &point, const RangeFactor3D &factor)
Definition: testRangeFactor.cpp:54
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
TEST
TEST(RangeFactor, Constructor)
Definition: testRangeFactor.cpp:72
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))
EXPECT
#define EXPECT(condition)
Definition: Test.h:151
factorError2D
Vector factorError2D(const Pose2 &pose, const Point2 &point, const RangeFactor2D &factor)
Definition: testRangeFactor.cpp:48
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
factor2D
BearingFactor2D factor2D(poseKey, pointKey, measurement2D, model2D)
RangeFactor2D
RangeFactor< Pose2, Point2 > RangeFactor2D
Definition: testRangeFactor.cpp:37
gtsam::RangeFactor::evaluateError
Vector evaluateError(const A1 &a1, const A2 &a2, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const
Definition: sam/RangeFactor.h:62
gtsam
traits
Definition: chartTesting.h:28
model
static SharedNoiseModel model(noiseModel::Unit::Create(1))
gtsam::genericValue
GenericValue< T > genericValue(const T &v)
Definition: GenericValue.h:212
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:42
RangeFactorWithTransform2D
RangeFactorWithTransform< Pose2, Point2 > RangeFactorWithTransform2D
Definition: testRangeFactor.cpp:39
gtsam::Factor::keys
const KeyVector & keys() const
Access the factor&#39;s involved variable keys.
Definition: Factor.h:124
factorErrorWithTransform3D
Vector factorErrorWithTransform3D(const Pose3 &pose, const Point3 &point, const RangeFactorWithTransform3D &factor)
Definition: testRangeFactor.cpp:66
gtsam::RangeFactorWithTransform::evaluateError
Vector evaluateError(const A1 &a1, const A2 &a2, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const
Definition: sam/RangeFactor.h:139
simple_graph::factor2
JacobianFactor factor2(keyX, A21, keyY, A22, b2, noiseModel::Isotropic::Sigma(2, sigma2))
gtsam::Point3
Vector3 Point3
Definition: Point3.h:35
Pose2.h
2D Pose
Camera
Definition: camera.h:36
Pose3.h
3D Pose
ceres::Vector
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Definition: gtsam/3rdparty/ceres/eigen.h:38
poseKey
Key poseKey(2)
BOOST_CLASS_EXPORT
BOOST_CLASS_EXPORT(gtsam::noiseModel::Unit)
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
align_3::t
Point2 t(10, 10)
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:734
Cal3_S2.h
The most common 5DOF 3D->2D calibration.
simple_graph::factor1
JacobianFactor factor1(keyX, A11, b1, noiseModel::Isotropic::Sigma(2, sigma1))

gtsam
Author(s):
autogenerated on Sat May 8 2021 02:49:17