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/TestableAssertions.h>
26 
27 #include <CppUnitLite/TestHarness.h>
28 
29 using namespace std::placeholders;
30 using namespace std;
31 using namespace gtsam;
32 
33 typedef RangeFactor<Pose2, Point2> RangeFactor2D;
34 typedef RangeFactor<Pose3, Point3> RangeFactor3D;
35 typedef RangeFactorWithTransform<Pose2, Point2> RangeFactorWithTransform2D;
36 typedef RangeFactorWithTransform<Pose3, Point3> RangeFactorWithTransform3D;
37 
38 // Keys are deliberately *not* in sorted order to test that case.
39 namespace {
40 // Create a noise model for the pixel error
41 static SharedNoiseModel model(noiseModel::Unit::Create(1));
42 
43 constexpr Key poseKey(2);
44 constexpr Key pointKey(1);
45 constexpr double measurement(10.0);
46 
47 Vector factorError2D(const Pose2& pose, const Point2& point,
48  const RangeFactor2D& factor) {
49  return factor.evaluateError(pose, point);
50 }
51 
52 Vector factorError3D(const Pose3& pose, const Point3& point,
53  const RangeFactor3D& factor) {
54  return factor.evaluateError(pose, point);
55 }
56 
57 Vector factorErrorWithTransform2D(const Pose2& pose, const Point2& point,
58  const RangeFactorWithTransform2D& factor) {
59  return factor.evaluateError(pose, point);
60 }
61 
62 Vector factorErrorWithTransform3D(const Pose3& pose, const Point3& point,
63  const RangeFactorWithTransform3D& factor) {
64  return factor.evaluateError(pose, point);
65 }
66 } // namespace
67 
68 /* ************************************************************************* */
69 TEST( RangeFactor, Constructor) {
70  RangeFactor2D factor2D(poseKey, pointKey, measurement, model);
71  RangeFactor3D factor3D(poseKey, pointKey, measurement, model);
72 }
73 
74 /* ************************************************************************* */
75 TEST( RangeFactor, ConstructorWithTransform) {
76  Pose2 body_P_sensor_2D(0.25, -0.10, -M_PI_2);
77  Pose3 body_P_sensor_3D(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2),
78  Point3(0.25, -0.10, 1.0));
79 
80  RangeFactorWithTransform2D factor2D(poseKey, pointKey, measurement, model,
81  body_P_sensor_2D);
82  KeyVector expected {2, 1};
83  CHECK(factor2D.keys() == expected);
84  RangeFactorWithTransform3D factor3D(poseKey, pointKey, measurement, model,
85  body_P_sensor_3D);
86  CHECK(factor3D.keys() == expected);
87 }
88 
89 /* ************************************************************************* */
90 TEST( RangeFactor, Equals ) {
91  // Create two identical factors and make sure they're equal
92  RangeFactor2D factor2D_1(poseKey, pointKey, measurement, model);
93  RangeFactor2D factor2D_2(poseKey, pointKey, measurement, model);
94  CHECK(assert_equal(factor2D_1, factor2D_2));
95 
96  RangeFactor3D factor3D_1(poseKey, pointKey, measurement, model);
97  RangeFactor3D factor3D_2(poseKey, pointKey, measurement, model);
98  CHECK(assert_equal(factor3D_1, factor3D_2));
99 }
100 
101 /* ************************************************************************* */
102 TEST( RangeFactor, EqualsWithTransform ) {
103  // Create two identical factors and make sure they're equal
104  Pose2 body_P_sensor_2D(0.25, -0.10, -M_PI_2);
105  Pose3 body_P_sensor_3D(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2),
106  Point3(0.25, -0.10, 1.0));
107 
108  RangeFactorWithTransform2D factor2D_1(poseKey, pointKey, measurement, model,
109  body_P_sensor_2D);
110  RangeFactorWithTransform2D factor2D_2(poseKey, pointKey, measurement, model,
111  body_P_sensor_2D);
112  CHECK(assert_equal(factor2D_1, factor2D_2));
113 
114  RangeFactorWithTransform3D factor3D_1(poseKey, pointKey, measurement, model,
115  body_P_sensor_3D);
116  RangeFactorWithTransform3D factor3D_2(poseKey, pointKey, measurement, model,
117  body_P_sensor_3D);
118  CHECK(assert_equal(factor3D_1, factor3D_2));
119 }
120 /* ************************************************************************* */
121 TEST( RangeFactor, Error2D ) {
122  // Create a factor
123  RangeFactor2D factor(poseKey, pointKey, measurement, model);
124 
125  // Set the linearization point
126  Pose2 pose(1.0, 2.0, 0.57);
127  Point2 point(-4.0, 11.0);
128 
129  // Use the factor to calculate the error
130  Vector actualError(factor.unwhitenedError({{poseKey, genericValue(pose)}, {pointKey, genericValue(point)}}));
131 
132  // The expected error is ||(5.0, 9.0)|| - 10.0 = 0.295630141 meter / UnitCovariance
133  Vector expectedError = (Vector(1) << 0.295630141).finished();
134 
135  // Verify we get the expected error
136  CHECK(assert_equal(expectedError, actualError, 1e-9));
137 }
138 
139 /* ************************************************************************* */
140 TEST( RangeFactor, Error2DWithTransform ) {
141  // Create a factor
142  Pose2 body_P_sensor(0.25, -0.10, -M_PI_2);
143  RangeFactorWithTransform2D factor(poseKey, pointKey, measurement, model,
144  body_P_sensor);
145 
146  // Set the linearization point
147  Rot2 R(0.57);
148  Point2 t = Point2(1.0, 2.0) - R.rotate(body_P_sensor.translation());
149  Pose2 pose(R, t);
150  Point2 point(-4.0, 11.0);
151 
152  // Use the factor to calculate the error
153  Vector actualError(factor.unwhitenedError({{poseKey, genericValue(pose)}, {pointKey, genericValue(point)}}));
154 
155  // The expected error is ||(5.0, 9.0)|| - 10.0 = 0.295630141 meter / UnitCovariance
156  Vector expectedError = (Vector(1) << 0.295630141).finished();
157 
158  // Verify we get the expected error
159  CHECK(assert_equal(expectedError, actualError, 1e-9));
160 }
161 
162 /* ************************************************************************* */
163 TEST( RangeFactor, Error3D ) {
164  // Create a factor
165  RangeFactor3D factor(poseKey, pointKey, measurement, model);
166 
167  // Set the linearization point
168  Pose3 pose(Rot3::RzRyRx(0.2, -0.3, 1.75), Point3(1.0, 2.0, -3.0));
169  Point3 point(-2.0, 11.0, 1.0);
170 
171  // Use the factor to calculate the error
172  Vector actualError(factor.unwhitenedError({{poseKey, genericValue(pose)}, {pointKey, genericValue(point)}}));
173 
174  // The expected error is ||(3.0, 9.0, 4.0)|| - 10.0 = 0.295630141 meter / UnitCovariance
175  Vector expectedError = (Vector(1) << 0.295630141).finished();
176 
177  // Verify we get the expected error
178  CHECK(assert_equal(expectedError, actualError, 1e-9));
179 }
180 
181 /* ************************************************************************* */
182 TEST( RangeFactor, Error3DWithTransform ) {
183  // Create a factor
184  Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2),
185  Point3(0.25, -0.10, 1.0));
186  RangeFactorWithTransform3D factor(poseKey, pointKey, measurement, model,
187  body_P_sensor);
188 
189  // Set the linearization point
190  Rot3 R = Rot3::RzRyRx(0.2, -0.3, 1.75);
191  Point3 t = Point3(1.0, 2.0, -3.0) - R.rotate(body_P_sensor.translation());
192  Pose3 pose(R, t);
193  Point3 point(-2.0, 11.0, 1.0);
194 
195  // Use the factor to calculate the error
196  Vector actualError(factor.unwhitenedError({{poseKey, genericValue(pose)}, {pointKey, genericValue(point)}}));
197 
198  // The expected error is ||(3.0, 9.0, 4.0)|| - 10.0 = 0.295630141 meter / UnitCovariance
199  Vector expectedError = (Vector(1) << 0.295630141).finished();
200 
201  // Verify we get the expected error
202  CHECK(assert_equal(expectedError, actualError, 1e-9));
203 }
204 
205 /* ************************************************************************* */
206 TEST( RangeFactor, Jacobian2D ) {
207  // Create a factor
208  RangeFactor2D factor(poseKey, pointKey, measurement, model);
209 
210  // Set the linearization point
211  Pose2 pose(1.0, 2.0, 0.57);
212  Point2 point(-4.0, 11.0);
213 
214  // Use the factor to calculate the Jacobians
215  Matrix H1Actual, H2Actual;
216  factor.evaluateError(pose, point, &H1Actual, &H2Actual);
217 
218  // Use numerical derivatives to calculate the Jacobians
219  Matrix H1Expected, H2Expected;
220  H1Expected = numericalDerivative11<Vector, Pose2>(
221  std::bind(&factorError2D, std::placeholders::_1, point, factor), pose);
222  H2Expected = numericalDerivative11<Vector, Point2>(
223  std::bind(&factorError2D, pose, std::placeholders::_1, factor), point);
224 
225  // Verify the Jacobians are correct
226  CHECK(assert_equal(H1Expected, H1Actual, 1e-9));
227  CHECK(assert_equal(H2Expected, H2Actual, 1e-9));
228 }
229 
230 /* ************************************************************************* */
231 TEST( RangeFactor, Jacobian2DWithTransform ) {
232  // Create a factor
233  Pose2 body_P_sensor(0.25, -0.10, -M_PI_2);
234  RangeFactorWithTransform2D factor(poseKey, pointKey, measurement, model,
235  body_P_sensor);
236 
237  // Set the linearization point
238  Rot2 R(0.57);
239  Point2 t = Point2(1.0, 2.0) - R.rotate(body_P_sensor.translation());
240  Pose2 pose(R, t);
241  Point2 point(-4.0, 11.0);
242 
243  // Use the factor to calculate the Jacobians
244  std::vector<Matrix> actualHs(2);
245  factor.unwhitenedError({{poseKey, genericValue(pose)}, {pointKey, genericValue(point)}}, actualHs);
246  const Matrix& H1Actual = actualHs.at(0);
247  const Matrix& H2Actual = actualHs.at(1);
248 
249  // Use numerical derivatives to calculate the Jacobians
250  Matrix H1Expected, H2Expected;
251  H1Expected = numericalDerivative11<Vector, Pose2>(
252  std::bind(&factorErrorWithTransform2D, std::placeholders::_1, point, factor), pose);
253  H2Expected = numericalDerivative11<Vector, Point2>(
254  std::bind(&factorErrorWithTransform2D, pose, std::placeholders::_1, factor), point);
255 
256  // Verify the Jacobians are correct
257  CHECK(assert_equal(H1Expected, H1Actual, 1e-9));
258  CHECK(assert_equal(H2Expected, H2Actual, 1e-9));
259 }
260 
261 /* ************************************************************************* */
262 TEST( RangeFactor, Jacobian3D ) {
263  // Create a factor
264  RangeFactor3D factor(poseKey, pointKey, measurement, model);
265 
266  // Set the linearization point
267  Pose3 pose(Rot3::RzRyRx(0.2, -0.3, 1.75), Point3(1.0, 2.0, -3.0));
268  Point3 point(-2.0, 11.0, 1.0);
269 
270  // Use the factor to calculate the Jacobians
271  std::vector<Matrix> actualHs(2);
272  factor.unwhitenedError({{poseKey, genericValue(pose)}, {pointKey, genericValue(point)}}, actualHs);
273  const Matrix& H1Actual = actualHs.at(0);
274  const Matrix& H2Actual = actualHs.at(1);
275 
276  // Use numerical derivatives to calculate the Jacobians
277  Matrix H1Expected, H2Expected;
278  H1Expected = numericalDerivative11<Vector, Pose3>(
279  std::bind(&factorError3D, std::placeholders::_1, point, factor), pose);
280  H2Expected = numericalDerivative11<Vector, Point3>(
281  std::bind(&factorError3D, pose, std::placeholders::_1, factor), point);
282 
283  // Verify the Jacobians are correct
284  CHECK(assert_equal(H1Expected, H1Actual, 1e-9));
285  CHECK(assert_equal(H2Expected, H2Actual, 1e-9));
286 }
287 
288 /* ************************************************************************* */
289 TEST( RangeFactor, Jacobian3DWithTransform ) {
290  // Create a factor
291  Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2),
292  Point3(0.25, -0.10, 1.0));
293  RangeFactorWithTransform3D factor(poseKey, pointKey, measurement, model,
294  body_P_sensor);
295 
296  // Set the linearization point
297  Rot3 R = Rot3::RzRyRx(0.2, -0.3, 1.75);
298  Point3 t = Point3(1.0, 2.0, -3.0) - R.rotate(body_P_sensor.translation());
299  Pose3 pose(R, t);
300  Point3 point(-2.0, 11.0, 1.0);
301 
302  // Use the factor to calculate the Jacobians
303  std::vector<Matrix> actualHs(2);
304  factor.unwhitenedError({{poseKey, genericValue(pose)}, {pointKey, genericValue(point)}}, actualHs);
305  const Matrix& H1Actual = actualHs.at(0);
306  const Matrix& H2Actual = actualHs.at(1);
307 
308  // Use numerical derivatives to calculate the Jacobians
309  Matrix H1Expected, H2Expected;
310  H1Expected = numericalDerivative11<Vector, Pose3>(
311  std::bind(&factorErrorWithTransform3D, std::placeholders::_1, point, factor), pose);
312  H2Expected = numericalDerivative11<Vector, Point3>(
313  std::bind(&factorErrorWithTransform3D, pose, std::placeholders::_1, factor), point);
314 
315  // Verify the Jacobians are correct
316  CHECK(assert_equal(H1Expected, H1Actual, 1e-9));
317  CHECK(assert_equal(H2Expected, H2Actual, 1e-9));
318 }
319 
320 /* ************************************************************************* */
321 // Do a test with Point2
322 TEST(RangeFactor, Point2) {
323  // Create a factor
324  RangeFactor<Point2> factor(11, 22, measurement, model);
325 
326  // Set the linearization point
327  Point2 p11(1.0, 2.0), p22(-4.0, 11.0);
328 
329  // The expected error is ||(5.0, 9.0)|| - 10.0 = 0.295630141 meter
330  Vector expectedError = (Vector(1) << 0.295630141).finished();
331 
332  // Verify we get the expected error
333  Values values {{11, genericValue(p11)}, {22, genericValue(p22)}};
334  CHECK(assert_equal(expectedError, factor.unwhitenedError(values), 1e-9));
335 }
336 
337 /* ************************************************************************* */
338 // Do a test with Point3
339 TEST(RangeFactor, Point3) {
340  // Create a factor
341  RangeFactor<Point3> factor(11, 22, measurement, model);
342 
343  // Set the linearization point
344  Point3 p11(1.0, 2.0, 0.0), p22(-4.0, 11.0, 0);
345 
346  // The expected error is ||(5.0, 9.0)|| - 10.0 = 0.295630141 meter
347  Vector expectedError = (Vector(1) << 0.295630141).finished();
348 
349  // Verify we get the expected error
350  Values values {{11, genericValue(p11)}, {22, genericValue(p22)}};
351  CHECK(assert_equal(expectedError, factor.unwhitenedError(values), 1e-9));
352 }
353 
354 /* ************************************************************************* */
355 // Do tests with PinholeCamera<Cal3_S2>
356 TEST( RangeFactor, Camera) {
357  using Camera = PinholeCamera<Cal3_S2>;
358 
359  RangeFactor<Camera, Point3> factor1(poseKey, pointKey, measurement, model);
360  RangeFactor<Camera, Pose3> factor2(poseKey, pointKey, measurement, model);
361  RangeFactor<Camera, Camera> factor3(poseKey, pointKey, measurement, model);
362 }
363 
364 /* ************************************************************************* */
365 // Do a test with non GTSAM types
366 
367 namespace gtsam {
368 template <>
369 struct Range<Vector4, Vector4> {
370  typedef double result_type;
371  double operator()(const Vector4& v1, const Vector4& v2,
372  OptionalJacobian<1, 4> H1, OptionalJacobian<1, 4> H2) {
373  return (v2 - v1).norm();
374  // derivatives not implemented
375  }
376 };
377 } // namespace gtsam
378 
379 TEST(RangeFactor, NonGTSAM) {
380  // Create a factor
381  Key poseKey(1);
382  Key pointKey(2);
383  double measurement(10.0);
384  RangeFactor<Vector4> factor(poseKey, pointKey, measurement, model);
385 
386  // Set the linearization point
387  Vector4 pose(1.0, 2.0, 00, 0);
388  Vector4 point(-4.0, 11.0, 0, 0);
389 
390  // The expected error is ||(5.0, 9.0)|| - 10.0 = 0.295630141 meter / UnitCovariance
391  Vector expectedError = (Vector(1) << 0.295630141).finished();
392 
393  // Verify we get the expected error
394  CHECK(assert_equal(expectedError, factor.unwhitenedError({{poseKey, genericValue(pose)}, {pointKey, genericValue(point)}}), 1e-9));
395 }
396 
397 /* ************************************************************************* */
398 int main() {
399  TestResult tr;
400  return TestRegistry::runAllTests(tr);
401 }
402 /* ************************************************************************* */
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
Pose2.h
2D Pose
RangeFactorWithTransform3D
RangeFactorWithTransform< Pose3, Point3 > RangeFactorWithTransform3D
Definition: testRangeFactor.cpp:36
gtsam::RangeFactor
Definition: sam/RangeFactor.h:35
pose
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
pointKey
const gtsam::Key pointKey
Definition: testRelativeElevationFactor.cpp:25
simple_graph::factor2
auto factor2
Definition: testJacobianFactor.cpp:203
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
Cal3_S2.h
The most common 5DOF 3D->2D calibration.
TestHarness.h
Camera
Definition: camera.h:36
RangeFactor.h
Serializable factor induced by a range measurement.
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
different_sigmas::values
HybridValues values
Definition: testHybridBayesNet.cpp:247
gtsam::genericValue
GenericValue< T > genericValue(const T &v)
Definition: GenericValue.h:211
body_P_sensor
static Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0))
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:39
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:91
gtsam::Range< Vector4, Vector4 >::operator()
double operator()(const Vector4 &v1, const Vector4 &v2, OptionalJacobian< 1, 4 > H1, OptionalJacobian< 1, 4 > H2)
Definition: testRangeFactor.cpp:371
main
int main()
Definition: testRangeFactor.cpp:398
poseKey
const gtsam::Key poseKey
Definition: testPoseRotationPrior.cpp:29
TestableAssertions.h
Provides additional testing facilities for common data structures.
pruning_fixture::factor
DecisionTreeFactor factor(D &C &B &A, "0.0 0.0 0.0 0.60658897 0.61241912 0.61241969 0.61247685 0.61247742 0.0 " "0.0 0.0 0.99995287 1.0 1.0 1.0 1.0")
gtsam_unstable.tests.test_ProjectionFactorRollingShutter.point
point
Definition: test_ProjectionFactorRollingShutter.py:25
numericalDerivative.h
Some functions to compute numerical derivatives.
gtsam::Range< Vector4, Vector4 >::result_type
double result_type
Definition: testRangeFactor.cpp:370
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
RangeFactor3D
RangeFactor< Pose3, Point3 > RangeFactor3D
Definition: testRangeFactor.cpp:34
gtsam::Pose3
Definition: Pose3.h:37
RangeFactor2D
RangeFactor< Pose2, Point2 > RangeFactor2D
Definition: testRangeFactor.cpp:33
gtsam::PinholeCamera
Definition: PinholeCamera.h:33
RangeFactorWithTransform2D
RangeFactorWithTransform< Pose2, Point2 > RangeFactorWithTransform2D
Definition: testRangeFactor.cpp:35
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:916
gtsam::Pose3::translation
const Point3 & translation(OptionalJacobian< 3, 6 > Hself={}) const
get translation
Definition: Pose3.cpp:342
PinholeCamera.h
Base class for all pinhole cameras.
TEST
TEST(RangeFactor, Constructor)
Definition: testRangeFactor.cpp:69
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:762
gtsam::Range
Definition: BearingRange.h:41
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
TestResult
Definition: TestResult.h:26
M_PI_2
#define M_PI_2
Definition: mconf.h:118
simple_graph::factor3
auto factor3
Definition: testJacobianFactor.cpp:210
gtsam::Rot2::rotate
Point2 rotate(const Point2 &p, OptionalJacobian< 2, 1 > H1={}, OptionalJacobian< 2, 2 > H2={}) const
Definition: Rot2.cpp:115
gtsam::Rot2
Definition: Rot2.h:35
gtsam::RangeFactorWithTransform
Definition: sam/RangeFactor.h:104
gtsam
traits
Definition: ABC.h:17
gtsam::OptionalJacobian
Definition: OptionalJacobian.h:38
gtsam::Values
Definition: Values.h:65
CHECK
#define CHECK(condition)
Definition: Test.h:108
std
Definition: BFloat16.h:88
v2
Vector v2
Definition: testSerializationBase.cpp:39
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:41
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
pybind_wrapper_test_script.p22
p22
Definition: pybind_wrapper_test_script.py:9
align_3::t
Point2 t(10, 10)
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
ceres::Vector
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Definition: gtsam/3rdparty/ceres/eigen.h:38
R
Rot2 R(Rot2::fromAngle(0.1))
simple_graph::factor1
auto factor1
Definition: testJacobianFactor.cpp:196
measurement
static Point2 measurement(323.0, 240.0)
Pose3.h
3D Pose manifold SO(3) x R^3 and group SE(3)
v1
Vector v1
Definition: testSerializationBase.cpp:38
gtsam::Pose2
Definition: Pose2.h:39

gtsam
Author(s):
autogenerated on Wed May 28 2025 03:07:21