testSmartFactorBase.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 
22 #include <gtsam/geometry/Pose3.h>
25 
26 #include "PinholeFactor.h"
27 
28 namespace {
29 using namespace gtsam;
32 } // namespace
33 
34 using namespace std;
35 
36 namespace gtsam {
37 
38 TEST(SmartFactorBase, Pinhole) {
39  PinholeFactor f = PinholeFactor(unit2);
40  f.add(Point2(0, 0), 1);
41  f.add(Point2(0, 0), 2);
42  EXPECT_LONGS_EQUAL(2 * 2, f.dim());
43 }
44 
45 TEST(SmartFactorBase, PinholeWithSensor) {
46  Pose3 body_P_sensor(Rot3(), Point3(1, 0, 0));
48  EXPECT(assert_equal<Pose3>(f.body_P_sensor(), body_P_sensor));
49 
50  PinholeFactor::Cameras cameras;
51  // Assume body at origin.
52  Pose3 world_P_body = Pose3();
53  // Camera coordinates in world frame.
54  Pose3 wTc = world_P_body * body_P_sensor;
55  cameras.push_back(PinholeCamera<Cal3Bundler>(wTc));
56 
57  // Simple point to project slightly off image center
58  Point3 p(0, 0, 10);
59  Point2 measurement = cameras[0].project(p);
60  f.add(measurement, 1);
61 
63  Matrix E;
64  Vector error = f.unwhitenedError<Point3>(cameras, p, Fs, E);
65 
66  Vector expectedError = Vector::Zero(2);
67  Matrix29 expectedFs;
68  expectedFs << -0.001, -1.00001, 0, -0.1, 0, -0.01, 0, 0, 0, 1, 0, 0, 0, -0.1,
69  0, 0, 0, 0;
70  Matrix23 expectedE;
71  expectedE << 0.1, 0, 0.01, 0, 0.1, 0;
72 
73  EXPECT(assert_equal(error, expectedError));
74  // We only have the jacobian for the 1 camera
75  // Use of a lower tolerance value due to compiler precision mismatch.
76  EXPECT(assert_equal(expectedFs, Fs[0], 1e-3));
77  EXPECT(assert_equal(expectedE, E));
78 }
79 
80 class StereoFactor : public SmartFactorBase<StereoCamera> {
81  public:
84  StereoFactor(const SharedNoiseModel& sharedNoiseModel)
85  : Base(sharedNoiseModel) {}
86  double error(const Values& values) const override { return 0.0; }
87  std::shared_ptr<GaussianFactor> linearize(
88  const Values& values) const override {
89  return std::shared_ptr<GaussianFactor>(new JacobianFactor());
90  }
91 };
92 
94 template <>
95 struct traits<StereoFactor> : public Testable<StereoFactor> {};
96 
99  f.add(StereoPoint2(), 1);
100  f.add(StereoPoint2(), 2);
101  EXPECT_LONGS_EQUAL(2 * 3, f.dim());
102 }
103 } // namespace gtsam
104 
105 /* ************************************************************************* */
106 int main() {
107  TestResult tr;
108  return TestRegistry::runAllTests(tr);
109 }
110 /* ************************************************************************* */
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
SmartFactorBase.h
Base class to create smart factors on poses or cameras.
PinholeFactor.h
helper class for tests
gtsam::TEST
TEST(SmartFactorBase, Stereo)
Definition: testSmartFactorBase.cpp:97
gtsam::StereoFactor::Base
SmartFactorBase< StereoCamera > Base
Definition: testSmartFactorBase.cpp:82
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
EXPECT_LONGS_EQUAL
#define EXPECT_LONGS_EQUAL(expected, actual)
Definition: Test.h:154
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
gtsam::StereoFactor::StereoFactor
StereoFactor()
Definition: testSmartFactorBase.cpp:83
TestHarness.h
gtsam::JacobianFactor
Definition: JacobianFactor.h:91
gtsam::StereoFactor
Definition: testSmartFactorBase.cpp:80
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
different_sigmas::values
HybridValues values
Definition: testHybridBayesNet.cpp:245
gtsam::Factor
Definition: Factor.h:70
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:38
gtsam::CameraSet
A set of cameras, all with their own calibration.
Definition: CameraSet.h:36
gtsam::StereoFactor::linearize
std::shared_ptr< GaussianFactor > linearize(const Values &values) const override
Definition: testSmartFactorBase.cpp:87
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
gtsam::Pose3
Definition: Pose3.h:37
gtsam::PinholeCamera< Cal3Bundler >
gtsam::noiseModel::Unit::Create
static shared_ptr Create(size_t dim)
Definition: NoiseModel.h:631
PinholeCamera.h
Base class for all pinhole cameras.
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:762
gtsam::StereoFactor::error
double error(const Values &values) const override
Definition: testSmartFactorBase.cpp:86
TestResult
Definition: TestResult.h:26
E
DiscreteKey E(5, 2)
tree::f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Definition: testExpression.cpp:218
gtsam::SmartFactorBase
Base class for smart factors. This base class has no internal point, but it has a measurement,...
Definition: SmartFactorBase.h:51
gtsam::PinholeFactor
Definition: PinholeFactor.h:33
gtsam
traits
Definition: SFMdata.h:40
gtsam::Testable
Definition: Testable.h:152
error
static double error
Definition: testRot3.cpp:37
gtsam::traits
Definition: Group.h:36
gtsam::Values
Definition: Values.h:65
gtsam::StereoPoint2
Definition: StereoPoint2.h:34
std
Definition: BFloat16.h:88
p
float * p
Definition: Tutorial_Map_using.cpp:9
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
gtsam::StereoFactor::StereoFactor
StereoFactor(const SharedNoiseModel &sharedNoiseModel)
Definition: testSmartFactorBase.cpp:84
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
Cal3Bundler.h
Calibration used by Bundler.
gtsam::CameraSet::FBlocks
std::vector< MatrixZD, Eigen::aligned_allocator< MatrixZD > > FBlocks
Definition: CameraSet.h:78
measurement
static Point2 measurement(323.0, 240.0)
main
int main()
Definition: testSmartFactorBase.cpp:106
Pose3.h
3D Pose
unit3
static Unit3 unit3(1.0, 2.1, 3.4)
StereoCamera.h
A Stereo Camera based on two Simple Cameras.


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:41:38