testInvDepthFactor3.cpp
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
3  * Atlanta, Georgia 30332-0415
4  * All Rights Reserved
5  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
6  * See LICENSE for the license information
7  * -------------------------------------------------------------------------- */
8 
19 
24 #include <gtsam/inference/Symbol.h>
26 
28 
29 using namespace std;
30 using namespace gtsam;
31 
32 static Cal3_S2::shared_ptr K(new Cal3_S2(1500, 1200, 0, 640, 480));
33 static SharedNoiseModel sigma(noiseModel::Unit::Create(2));
34 
35 // camera pose at (0,0,1) looking straight along the x-axis.
36 Pose3 level_pose = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
38 
41 
42 Matrix factorError(const Pose3& pose, const Vector5& point, double invDepth,
43  const InverseDepthFactor& factor) {
44  return factor.evaluateError(pose, point, invDepth);
45 }
46 
47 /* ************************************************************************* */
48 TEST( InvDepthFactor, optimize) {
49 
50  // landmark 5 meters infront of camera (camera center at (0,0,1))
51  Point3 landmark(5, 0, 1);
52 
53  // get expected projection using pinhole camera
54  Point2 expected_uv = level_camera.project(landmark);
55 
57  Vector5 inv_landmark((Vector(5) << 0., 0., 1., 0., 0.).finished());
58  // initialize inverse depth with "incorrect" depth of 1/4
59  // in reality this is 1/5, but initial depth is guessed
60  double inv_depth(1./4);
61 
64 
66  Symbol('x',1), Symbol('l',1), Symbol('d',1), K);
68  initial.insert(Symbol('x',1), level_pose);
69  initial.insert(Symbol('l',1), inv_landmark);
70  initial.insert(Symbol('d',1), inv_depth);
71 
74 
75  // with a single factor the incorrect initialization of 1/4 should not move!
77 
79 
80  // add a camera 2 meters to the right
81  Pose3 right_pose = level_pose * Pose3(Rot3(), Point3(2,0,0));
82  PinholeCamera<Cal3_S2> right_camera(right_pose, *K);
83 
84  // projection measurement of landmark into right camera
85  // this measurement disagrees with the depth initialization
86  // and will push it to 1/5
87  Point2 right_uv = right_camera.project(landmark);
88 
90  Symbol('x',2), Symbol('l',1),Symbol('d',1),K));
92 
93  graph.emplace_shared<PoseConstraint>(Symbol('x',2),right_pose);
94 
95  initial.insert(Symbol('x',2), right_pose);
96 
98 
100  result2.at<Vector5>(Symbol('l',1)),
101  result2.at<double>(Symbol('d',1)));
102  EXPECT(assert_equal(landmark, result2_lmk, 1e-9));
103 
104  // TODO: need to add priors to make this work with
105  // Values result2 = optimize<NonlinearFactorGraph>(graph, initial,
106  // NonlinearOptimizationParameters(),MULTIFRONTAL, GN);
107 }
108 
109 
110 /* ************************************************************************* */
111 TEST( InvDepthFactor, Jacobian3D ) {
112 
113  // landmark 5 meters infront of camera (camera center at (0,0,1))
114  Point3 landmark(5, 0, 1);
115 
116  // get expected projection using pinhole camera
117  Point2 expected_uv = level_camera.project(landmark);
118 
119  // get expected landmark representation using backprojection
121  const auto [inv_landmark, inv_depth] = inv_camera.backproject(expected_uv, 5);
122  Vector5 expected_inv_landmark((Vector(5) << 0., 0., 1., 0., 0.).finished());
123 
124  CHECK(assert_equal(expected_inv_landmark, inv_landmark, 1e-6));
125  CHECK(assert_equal(inv_depth, 1./5, 1e-6));
126 
127  Symbol poseKey('x',1);
128  Symbol pointKey('l',1);
129  Symbol invDepthKey('d',1);
130  InverseDepthFactor factor(expected_uv, sigma, poseKey, pointKey, invDepthKey, K);
131 
132  std::vector<Matrix> actualHs(3);
133  factor.unwhitenedError({{poseKey, genericValue(level_pose)},
134  {pointKey, genericValue(inv_landmark)},
135  {invDepthKey,genericValue(inv_depth)}},
136  actualHs);
137 
138  const Matrix& H1Actual = actualHs.at(0);
139  const Matrix& H2Actual = actualHs.at(1);
140  const Matrix& H3Actual = actualHs.at(2);
141 
142  // Use numerical derivatives to verify the Jacobians
143  Matrix H1Expected, H2Expected, H3Expected;
144 
145  std::function<Matrix(const Pose3 &, const Vector5 &, const double &)>
146  func = std::bind(&factorError, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, factor);
147  H1Expected = numericalDerivative31(func, level_pose, inv_landmark, inv_depth);
148  H2Expected = numericalDerivative32(func, level_pose, inv_landmark, inv_depth);
149  H3Expected = numericalDerivative33(func, level_pose, inv_landmark, inv_depth);
150 
151  // Verify the Jacobians
152  CHECK(assert_equal(H1Expected, H1Actual, 1e-6))
153  CHECK(assert_equal(H2Expected, H2Actual, 1e-6))
154  CHECK(assert_equal(H3Expected, H3Actual, 1e-6))
155 }
156 
157 /* ************************************************************************* */
158 int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
159 /* ************************************************************************* */
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
gtsam::numericalDerivative33
internal::FixedSizeMatrix< Y, X3 >::type numericalDerivative33(std::function< Y(const X1 &, const X2 &, const X3 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
Definition: numericalDerivative.h:292
level_camera
PinholeCamera< Cal3_S2 > level_camera(level_pose, *K)
gtsam::NonlinearOptimizer::optimize
virtual const Values & optimize()
Definition: NonlinearOptimizer.h:98
K
static Cal3_S2::shared_ptr K(new Cal3_S2(1500, 1200, 0, 640, 480))
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
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::InvDepthFactor3
Definition: InvDepthFactor3.h:27
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
optimize
int optimize(const SfmData &db, const NonlinearFactorGraph &graph, const Values &initial, bool separateCalibration=false)
Definition: timeSFMBAL.h:64
factorError
Matrix factorError(const Pose3 &pose, const Vector5 &point, double invDepth, const InverseDepthFactor &factor)
Definition: testInvDepthFactor3.cpp:42
TestHarness.h
LevenbergMarquardtOptimizer.h
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
initial
Values initial
Definition: OdometryOptimize.cpp:2
InverseDepthFactor
InvDepthFactor3< Pose3, Vector5, double > InverseDepthFactor
Definition: testInvDepthFactor3.cpp:39
TEST
TEST(InvDepthFactor, optimize)
Definition: testInvDepthFactor3.cpp:48
lmParams
LevenbergMarquardtParams lmParams
Definition: testSmartProjectionRigFactor.cpp:55
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
gtsam::genericValue
GenericValue< T > genericValue(const T &v)
Definition: GenericValue.h:210
result
Values result
Definition: OdometryOptimize.cpp:8
gtsam::NonlinearEquality
Definition: NonlinearEquality.h:43
gtsam::PinholeBaseK< Calibration >::project
Point2 project(const Point3 &pw, OptionalJacobian< 2, 6 > Dpose={}, OptionalJacobian< 2, 3 > Dpoint={}, OptionalJacobian< 2, DimK > Dcal={}) const
project a 3D point from world coordinates into the image
Definition: PinholePose.h:112
poseKey
const gtsam::Key poseKey
Definition: testPoseRotationPrior.cpp:29
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
ceres::Matrix
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > Matrix
Definition: gtsam/3rdparty/ceres/eigen.h:42
gtsam::Values::at
const ValueType at(Key j) const
Definition: Values-inl.h:261
numericalDerivative.h
Some functions to compute numerical derivatives.
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
gtsam::numericalDerivative32
internal::FixedSizeMatrix< Y, X2 >::type numericalDerivative32(std::function< Y(const X1 &, const X2 &, const X3 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
Definition: numericalDerivative.h:259
gtsam::Pose3
Definition: Pose3.h:37
gtsam::InvDepthCamera3
Definition: InvDepthCamera3.h:34
gtsam::PinholeCamera
Definition: PinholeCamera.h:33
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
gtsam::numericalDerivative31
internal::FixedSizeMatrix< Y, X1 >::type numericalDerivative31(std::function< Y(const X1 &, const X2 &, const X3 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
Definition: numericalDerivative.h:226
landmark
static Point3 landmark(0, 0, 5)
PinholeCamera.h
Base class for all pinhole cameras.
gtsam::LevenbergMarquardtOptimizer
Definition: LevenbergMarquardtOptimizer.h:35
Symbol.h
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:762
gtsam::Cal3_S2::shared_ptr
std::shared_ptr< Cal3_S2 > shared_ptr
Definition: Cal3_S2.h:39
main
int main()
Definition: testInvDepthFactor3.cpp:158
NonlinearEquality.h
InvDepthFactor3.h
Inverse Depth Factor based on Civera09tro, Montiel06rss. Landmarks are initialized from the first cam...
TestResult
Definition: TestResult.h:26
sigma
static SharedNoiseModel sigma(noiseModel::Unit::Create(2))
PoseConstraint
NonlinearEquality< Pose3 > PoseConstraint
Definition: testInvDepthFactor3.cpp:40
gtsam
traits
Definition: SFMdata.h:40
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
gtsam::FactorGraph::push_back
IsDerived< DERIVEDFACTOR > push_back(std::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition: FactorGraph.h:147
gtsam::Values
Definition: Values.h:65
CHECK
#define CHECK(condition)
Definition: Test.h:108
gtsam::InvDepthCamera3::backproject
std::pair< Vector5, double > backproject(const gtsam::Point2 &pi, const double depth) const
Definition: InvDepthCamera3.h:159
std
Definition: BFloat16.h:88
level_pose
Pose3 level_pose
Definition: testInvDepthFactor3.cpp:36
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:41
gtsam::LevenbergMarquardtParams
Definition: LevenbergMarquardtParams.h:35
gtsam::Cal3_S2
The most common 5DOF 3D->2D calibration.
Definition: Cal3_S2.h:34
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
initial
Definition: testScenarioRunner.cpp:148
M_PI
#define M_PI
Definition: mconf.h:117
func
Definition: benchGeometry.cpp:23
gtsam::InvDepthFactor3::shared_ptr
std::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor
Definition: InvDepthFactor3.h:46
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
ceres::Vector
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Definition: gtsam/3rdparty/ceres/eigen.h:38
simple_graph::factor1
auto factor1
Definition: testJacobianFactor.cpp:196
gtsam::FactorGraph::emplace_shared
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
Definition: FactorGraph.h:153
gtsam::Symbol
Definition: inference/Symbol.h:37


gtsam
Author(s):
autogenerated on Sun Dec 22 2024 04:16:33