testInvDepthFactorVariant3.cpp
Go to the documentation of this file.
1 /*
2  * testInvDepthFactorVariant3.cpp
3  *
4  * Created on: Apr 13, 2012
5  * Author: cbeall3
6  */
7 
13 #include <gtsam/inference/Symbol.h>
15 #include <gtsam/geometry/Cal3_S2.h>
16 #include <gtsam/geometry/Pose3.h>
17 #include <gtsam/geometry/Point3.h>
18 #include <gtsam/geometry/Point2.h>
19 
20 using namespace std;
21 using namespace gtsam;
22 
23 /* ************************************************************************* */
24 TEST( InvDepthFactorVariant3, optimize) {
25 
26  // Create two poses looking in the x-direction
27  Pose3 pose1(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1.0));
28  Pose3 pose2(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1.5));
29 
30  // Create a landmark 5 meters in front of pose1 (camera center at (0,0,1))
31  Point3 landmark(5, 0, 1);
32 
33  // Create image observations
34  Cal3_S2::shared_ptr K(new Cal3_S2(1500, 1200, 0, 640, 480));
37  Point2 pixel1 = camera1.project(landmark);
38  Point2 pixel2 = camera2.project(landmark);
39 
40  // Create expected landmark
41  Point3 landmark_p1 = pose1.transformTo(landmark);
42  // landmark_p1.print("Landmark in Pose1 Frame:\n");
43  double theta = atan2(landmark_p1.x(), landmark_p1.z());
44  double phi = atan2(landmark_p1.y(), sqrt(landmark_p1.x()*landmark_p1.x()+landmark_p1.z()*landmark_p1.z()));
45  double rho = 1./landmark_p1.norm();
46  Vector3 expected((Vector(3) << theta, phi, rho).finished());
47 
48 
49 
50  // Create a factor graph with two inverse depth factors and two pose priors
51  Key poseKey1(1);
52  Key poseKey2(2);
53  Key landmarkKey(100);
54  SharedNoiseModel sigma(noiseModel::Unit::Create(2));
55  NonlinearFactor::shared_ptr factor1(new NonlinearEquality<Pose3>(poseKey1, pose1, 100000));
56  NonlinearFactor::shared_ptr factor2(new NonlinearEquality<Pose3>(poseKey2, pose2, 100000));
57  NonlinearFactor::shared_ptr factor3(new InvDepthFactorVariant3a(poseKey1, landmarkKey, pixel1, K, sigma));
58  NonlinearFactor::shared_ptr factor4(new InvDepthFactorVariant3b(poseKey1, poseKey2, landmarkKey, pixel2, K, sigma));
60  graph.push_back(factor1);
61  graph.push_back(factor2);
62  graph.push_back(factor3);
63  graph.push_back(factor4);
64 
65  // Create a values with slightly incorrect initial conditions
66  Values values;
67  values.insert(poseKey1, pose1.retract((Vector(6) << +0.01, -0.02, +0.03, -0.10, +0.20, -0.30).finished()));
68  values.insert(poseKey2, pose2.retract((Vector(6) << +0.01, +0.02, -0.03, -0.10, +0.20, +0.30).finished()));
69  values.insert(landmarkKey, Vector3(expected + Vector3(+0.02, -0.04, +0.05)));
70 
71  // Optimize the graph to recover the actual landmark position
73  Values result = LevenbergMarquardtOptimizer(graph, values, params).optimize();
74  Vector3 actual = result.at<Vector3>(landmarkKey);
75 
76 
77 
78  // Test that the correct landmark parameters have been recovered
79  EXPECT(assert_equal((Vector)expected, actual, 1e-9));
80 }
81 
82 
83 /* ************************************************************************* */
84 int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
85 /* ************************************************************************* */
virtual const Values & optimize()
static int runAllTests(TestResult &result)
Eigen::Vector3d Vector3
Definition: Vector.h:43
Factor Graph consisting of non-linear factors.
void insert(Key j, const Value &val)
Definition: Values.cpp:140
Matrix expected
Definition: testMatrix.cpp:974
Vector2 Point2
Definition: Point2.h:27
static const double sigma
#define M_PI
Definition: main.h:78
leaf::MyValues values
EIGEN_DEVICE_FUNC const SqrtReturnType sqrt() const
static Cal3_S2 K(500, 500, 0.1, 640/2, 480/2)
Definition: Half.h:150
IsDerived< DERIVEDFACTOR > push_back(boost::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition: FactorGraph.h:166
Rot2 theta
NonlinearFactorGraph graph
JacobianFactor factor3(keyY, A32, keyZ, A33, b3, noiseModel::Isotropic::Sigma(2, sigma3))
TEST(InvDepthFactorVariant3, optimize)
static Point3 landmark(0, 0, 5)
PinholeCamera< Cal3_S2 > camera2(pose2,*sharedCal)
Base class for all pinhole cameras.
Point3 transformTo(const Point3 &point, OptionalJacobian< 3, 6 > Hself=boost::none, OptionalJacobian< 3, 3 > Hpoint=boost::none) const
takes point in world coordinates and transforms it to Pose coordinates
Definition: Pose3.cpp:314
Eigen::VectorXd Vector
Definition: Vector.h:38
Values result
const ValueType at(Key j) const
Definition: Values-inl.h:342
#define EXPECT(condition)
Definition: Test.h:151
boost::shared_ptr< This > shared_ptr
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
Array< double, 1, 3 > e(1./3., 0.5, 2.)
static SmartStereoProjectionParams params
Inverse Depth Factor based on Civera09tro, Montiel06rss. Landmarks are parameterized as (theta...
Jet< T, N > atan2(const Jet< T, N > &g, const Jet< T, N > &f)
Definition: jet.h:556
static const Pose3 pose1(Rot3(), Point3(0, 1, 0.5))
traits
Definition: chartTesting.h:28
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:42
Class retract(const TangentVector &v) const
retract as required by manifold concept: applies v at *this
Definition: Lie.h:130
3D Point
int optimize(const SfmData &db, const NonlinearFactorGraph &graph, const Values &initial, bool separateCalibration=false)
Definition: timeSFMBAL.h:63
Point2 project(const Point3 &pw, OptionalJacobian< 2, 6 > Dpose=boost::none, OptionalJacobian< 2, 3 > Dpoint=boost::none, OptionalJacobian< 2, DimK > Dcal=boost::none) const
project a 3D point from world coordinates into the image
Definition: PinholePose.h:118
JacobianFactor factor2(keyX, A21, keyY, A22, b2, noiseModel::Isotropic::Sigma(2, sigma2))
static const Pose3 pose2
Vector3 Point3
Definition: Point3.h:35
static const Camera camera1(pose1, K)
2D Point
3D Pose
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
boost::shared_ptr< Cal3_S2 > shared_ptr
Definition: Cal3_S2.h:39
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
static Key poseKey1(x1)
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:734
The most common 5DOF 3D->2D calibration.
JacobianFactor factor1(keyX, A11, b1, noiseModel::Isotropic::Sigma(2, sigma1))


gtsam
Author(s):
autogenerated on Sat May 8 2021 02:47:42