testInvDepthFactorVariant2.cpp
Go to the documentation of this file.
1 /*
2  * testInvDepthFactorVariant1.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 /* ************************************************************************* */
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 referencePoint = pose1.translation();
42  Point3 ray = landmark - referencePoint;
43  double theta = atan2(ray.y(), ray.x());
44  double phi = atan2(ray.z(), sqrt(ray.x()*ray.x()+ray.y()*ray.y()));
45  double rho = 1./ray.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 InvDepthFactorVariant2(poseKey1, landmarkKey, pixel1, K, referencePoint, sigma));
58  NonlinearFactor::shared_ptr factor4(new InvDepthFactorVariant2(poseKey2, landmarkKey, pixel2, K, referencePoint, 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 // values.at<Pose3>(poseKey1).print("Pose1 Before:\n");
77 // result.at<Pose3>(poseKey1).print("Pose1 After:\n");
78 // pose1.print("Pose1 Truth:\n");
79 // std::cout << std::endl << std::endl;
80 // values.at<Pose3>(poseKey2).print("Pose2 Before:\n");
81 // result.at<Pose3>(poseKey2).print("Pose2 After:\n");
82 // pose2.print("Pose2 Truth:\n");
83 // std::cout << std::endl << std::endl;
84 // values.at<Vector3>(landmarkKey).print("Landmark Before:\n");
85 // result.at<Vector3>(landmarkKey).print("Landmark After:\n");
86 // expected.print("Landmark Truth:\n");
87 // std::cout << std::endl << std::endl;
88 
89  // Calculate world coordinates of landmark versions
90  Point3 world_landmarkBefore(0,0,0);
91  {
92  Vector3 landmarkBefore = values.at<Vector3>(landmarkKey);
93  double theta = landmarkBefore(0), phi = landmarkBefore(1), rho = landmarkBefore(2);
94  world_landmarkBefore = referencePoint + Point3(cos(theta)*cos(phi)/rho, sin(theta)*cos(phi)/rho, sin(phi)/rho);
95  }
96  Point3 world_landmarkAfter(0,0,0);
97  {
98  Vector3 landmarkAfter = result.at<Vector3>(landmarkKey);
99  double theta = landmarkAfter(0), phi = landmarkAfter(1), rho = landmarkAfter(2);
100  world_landmarkAfter = referencePoint + Point3(cos(theta)*cos(phi)/rho, sin(theta)*cos(phi)/rho, sin(phi)/rho);
101  }
102 
103 // world_landmarkBefore.print("World Landmark Before:\n");
104 // world_landmarkAfter.print("World Landmark After:\n");
105 // landmark.print("World Landmark Truth:\n");
106 // std::cout << std::endl << std::endl;
107 
108  // Test that the correct landmark parameters have been recovered
109  EXPECT(assert_equal((Vector)expected, actual, 1e-9));
110 }
111 
112 
113 /* ************************************************************************* */
114 int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
115 /* ************************************************************************* */
Jet< T, N > cos(const Jet< T, N > &f)
Definition: jet.h:426
const Point3 & translation(OptionalJacobian< 3, 6 > Hself={}) const
get translation
Definition: Pose3.cpp:308
virtual const Values & optimize()
static int runAllTests(TestResult &result)
Eigen::Vector3d Vector3
Definition: Vector.h:43
Factor Graph consisting of non-linear factors.
const ValueType at(Key j) const
Definition: Values-inl.h:204
Matrix expected
Definition: testMatrix.cpp:971
Vector2 Point2
Definition: Point2.h:32
IsDerived< DERIVEDFACTOR > push_back(std::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition: FactorGraph.h:190
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
#define M_PI
Definition: main.h:106
leaf::MyValues values
Jet< T, N > sin(const Jet< T, N > &f)
Definition: jet.h:439
static Cal3_S2 K(500, 500, 0.1, 640/2, 480/2)
TEST(InvDepthFactorVariant2, optimize)
Definition: BFloat16.h:88
NonlinearFactorGraph graph
static const SmartProjectionParams params
static Point3 landmark(0, 0, 5)
Base class for all pinhole cameras.
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
std::shared_ptr< Cal3_S2 > shared_ptr
Definition: Cal3_S2.h:39
Eigen::VectorXd Vector
Definition: Vector.h:38
Values result
#define EXPECT(condition)
Definition: Test.h:150
static const Camera2 camera2(pose1, K2)
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
Array< double, 1, 3 > e(1./3., 0.5, 2.)
The most common 5DOF 3D->2D calibration.
Definition: Cal3_S2.h:34
static const Pose3 pose1(Rot3(), Point3(0, 1, 0.5))
AnnoyingScalar atan2(const AnnoyingScalar &y, const AnnoyingScalar &x)
static Key poseKey1(X(1))
traits
Definition: chartTesting.h:28
Class retract(const TangentVector &v) const
retract as required by manifold concept: applies v at *this
Definition: Lie.h:131
Inverse Depth Factor based on Civera09tro, Montiel06rss. Landmarks are parameterized as (theta...
3D Point
int optimize(const SfmData &db, const NonlinearFactorGraph &graph, const Values &initial, bool separateCalibration=false)
Definition: timeSFMBAL.h:64
static Pose3 pose2
static const double sigma
void insert(Key j, const Value &val)
Definition: Values.cpp:155
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
Vector3 Point3
Definition: Point3.h:38
static Key poseKey2(X(2))
2D Point
3D Pose
PinholeCamera< Cal3_S2 > camera1(pose1, *sharedCal)
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:741
The most common 5DOF 3D->2D calibration.


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:38:31