testInvDepthCamera3.cpp
Go to the documentation of this file.
1 /*
2  * testInvDepthFactor.cpp
3  *
4  * Created on: Apr 13, 2012
5  * Author: cbeall3
6  */
7 
9 
11 #include <gtsam/base/Testable.h>
13 #include <gtsam/geometry/Cal3_S2.h>
14 
16 
17 using namespace std;
18 using namespace gtsam;
19 
20 static Cal3_S2::shared_ptr K(new Cal3_S2(1500, 1200, 0, 640, 480));
21 Pose3 level_pose = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
23 
24 /* ************************************************************************* */
25 TEST( InvDepthFactor, Project1) {
26 
27  // landmark 5 meters infront of camera
28  Point3 landmark(5, 0, 1);
29 
30  Point2 expected_uv = level_camera.project(landmark);
31 
32  InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K);
33  Vector5 inv_landmark((Vector(5) << 1., 0., 1., 0., 0.).finished());
34  double inv_depth(1./4);
35  Point2 actual_uv = inv_camera.project(inv_landmark, inv_depth);
36  EXPECT(assert_equal(expected_uv, actual_uv,1e-8));
37  EXPECT(assert_equal(Point2(640,480), actual_uv));
38 }
39 
40 /* ************************************************************************* */
41 TEST( InvDepthFactor, Project2) {
42 
43  // landmark 1m to the left and 1m up from camera
44  // inv landmark xyz is same as camera xyz, so depth actually doesn't matter
45  Point3 landmark(1, 1, 2);
46  Point2 expected = level_camera.project(landmark);
47 
48  InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K);
49  Vector5 diag_landmark((Vector(5) << 0., 0., 1., M_PI/4., atan(1.0/sqrt(2.0))).finished());
50  double inv_depth(1/sqrt(3.0));
51  Point2 actual = inv_camera.project(diag_landmark, inv_depth);
52  EXPECT(assert_equal(expected, actual));
53 }
54 
55 /* ************************************************************************* */
56 TEST( InvDepthFactor, Project3) {
57 
58  // landmark 1m to the left and 1m up from camera
59  // inv depth landmark xyz at origion
60  Point3 landmark(1, 1, 2);
61  Point2 expected = level_camera.project(landmark);
62 
63  InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K);
64  Vector5 diag_landmark((Vector(5) << 0., 0., 0., M_PI/4., atan(2./sqrt(2.0))).finished());
65  double inv_depth( 1./sqrt(1.0+1+4));
66  Point2 actual = inv_camera.project(diag_landmark, inv_depth);
67  EXPECT(assert_equal(expected, actual));
68 }
69 
70 /* ************************************************************************* */
71 TEST( InvDepthFactor, Project4) {
72 
73  // landmark 4m to the left and 1m up from camera
74  // inv depth landmark xyz at origion
75  Point3 landmark(1, 4, 2);
76  Point2 expected = level_camera.project(landmark);
77 
78  InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K);
79  Vector5 diag_landmark((Vector(5) << 0., 0., 0., atan(4.0/1), atan(2./sqrt(1.+16.))).finished());
80  double inv_depth(1./sqrt(1.+16.+4.));
81  Point2 actual = inv_camera.project(diag_landmark, inv_depth);
82  EXPECT(assert_equal(expected, actual));
83 }
84 
85 
86 /* ************************************************************************* */
87 Point2 project_(const Pose3& pose, const Vector5& landmark, const double& inv_depth) {
88  return InvDepthCamera3<Cal3_S2>(pose,K).project(landmark, inv_depth); }
89 
90 TEST( InvDepthFactor, Dproject_pose)
91 {
92  Vector5 landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2).finished());
93  double inv_depth(1./4);
94  Matrix expected = numericalDerivative31(project_,level_pose, landmark, inv_depth);
95  InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K);
96  Matrix actual;
97  inv_camera.project(landmark, inv_depth, actual, {}, {});
98  EXPECT(assert_equal(expected,actual,1e-6));
99 }
100 
101 /* ************************************************************************* */
102 TEST( InvDepthFactor, Dproject_landmark)
103 {
104  Vector5 landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2).finished());
105  double inv_depth(1./4);
106  Matrix expected = numericalDerivative32(project_,level_pose, landmark, inv_depth);
107  InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K);
108  Matrix actual;
109  inv_camera.project(landmark, inv_depth, {}, actual, {});
110  EXPECT(assert_equal(expected,actual,1e-7));
111 }
112 
113 /* ************************************************************************* */
114 TEST( InvDepthFactor, Dproject_inv_depth)
115 {
116  Vector5 landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2).finished());
117  double inv_depth(1./4);
118  Matrix expected = numericalDerivative33(project_,level_pose, landmark, inv_depth);
119  InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K);
120  Matrix actual;
121  inv_camera.project(landmark, inv_depth, {}, {}, actual);
122  EXPECT(assert_equal(expected,actual,1e-7));
123 }
124 
125 /* ************************************************************************* */
126 TEST(InvDepthFactor, backproject)
127 {
128  Vector expected((Vector(5) << 0.,0.,1., 0.1,0.2).finished());
129  double inv_depth(1./4);
130  InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K);
131  Point2 z = inv_camera.project(expected, inv_depth);
132 
133  const auto [actual_vec, actual_inv] = inv_camera.backproject(z, 4);
134  EXPECT(assert_equal(expected,actual_vec,1e-7));
135  EXPECT_DOUBLES_EQUAL(inv_depth,actual_inv,1e-7);
136 }
137 
138 /* ************************************************************************* */
139 TEST(InvDepthFactor, backproject2)
140 {
141  // backwards facing camera
142  Vector expected((Vector(5) << -5.,-5.,2., 3., -0.1).finished());
143  double inv_depth(1./10);
144  InvDepthCamera3<Cal3_S2> inv_camera(Pose3(Rot3::Ypr(1.5,0.1, -1.5), Point3(-5, -5, 2)),K);
145  Point2 z = inv_camera.project(expected, inv_depth);
146 
147  const auto [actual_vec, actual_inv] = inv_camera.backproject(z, 10);
148  EXPECT(assert_equal(expected,actual_vec,1e-7));
149  EXPECT_DOUBLES_EQUAL(inv_depth,actual_inv,1e-7);
150 }
151 
152 /* ************************************************************************* */
153 int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
154 /* ************************************************************************* */
TEST(InvDepthFactor, Project1)
static Cal3_S2::shared_ptr K(new Cal3_S2(1500, 1200, 0, 640, 480))
static Point2 project(const Pose3 &pose, const Unit3 &pointAtInfinity, const Cal3_S2::shared_ptr &cal)
Concept check for values that can be used in unit tests.
static int runAllTests(TestResult &result)
Matrix expected
Definition: testMatrix.cpp:971
Vector2 Point2
Definition: Point2.h:32
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
#define M_PI
Definition: main.h:106
Definition: BFloat16.h:88
Some functions to compute numerical derivatives.
#define EXPECT_DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:161
static Point3 backproject(const Pose3 &pose, const Point2 &point, const double &depth)
PinholeCamera< Cal3_S2 > level_camera(level_pose, *K)
static Point3 landmark(0, 0, 5)
EIGEN_DEVICE_FUNC const AtanReturnType atan() const
Base class for all pinhole cameras.
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)
static Point2 Project2(const Unit3 &point)
std::shared_ptr< Cal3_S2 > shared_ptr
Definition: Cal3_S2.h:39
Eigen::VectorXd Vector
Definition: Vector.h:38
Point2 project_(const Pose3 &pose, const Vector5 &landmark, const double &inv_depth)
std::pair< Vector5, double > backproject(const gtsam::Point2 &pi, const double depth) const
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)
#define EXPECT(condition)
Definition: Test.h:150
Pose3 level_pose
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)
Inverse Depth Camera based on Civera09tro, Montiel06rss. Landmarks are initialized from the first cam...
Array< double, 1, 3 > e(1./3., 0.5, 2.)
The most common 5DOF 3D->2D calibration.
Definition: Cal3_S2.h:34
traits
Definition: chartTesting.h:28
gtsam::Point2 project(const Vector5 &pw, double rho, OptionalJacobian< 2, 6 > H1={}, OptionalJacobian< 2, 5 > H2={}, OptionalJacobian< 2, 1 > H3={}) const
static Point2 Project1(const Point3 &point)
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
Vector3 Point3
Definition: Point3.h:38
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
int main()
The most common 5DOF 3D->2D calibration.


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