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 
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);
47 
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);
62 
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);
77 
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);
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);
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);
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);
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 /* ************************************************************************* */
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
pose
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
atan
const EIGEN_DEVICE_FUNC AtanReturnType atan() const
Definition: ArrayCwiseUnaryOps.h:283
level_pose
Pose3 level_pose
Definition: testInvDepthCamera3.cpp:21
Testable.h
Concept check for values that can be used in unit tests.
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
Cal3_S2.h
The most common 5DOF 3D->2D calibration.
TestHarness.h
TEST
TEST(InvDepthFactor, Project1)
Definition: testInvDepthCamera3.cpp:25
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
backproject
static Point3 backproject(const Pose3 &pose, const Point2 &point, const double &depth)
Definition: testCalibratedCamera.cpp:207
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:39
numericalDerivative.h
Some functions to compute numerical derivatives.
Project1
static Point2 Project1(const Point3 &point)
Definition: testCalibratedCamera.cpp:100
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
InvDepthCamera3.h
Inverse Depth Camera based on Civera09tro, Montiel06rss. Landmarks are initialized from the first cam...
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
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
project_
Point2 project_(const Pose3 &pose, const Vector5 &landmark, const double &inv_depth)
Definition: testInvDepthCamera3.cpp:87
landmark
static Point3 landmark(0, 0, 5)
PinholeCamera.h
Base class for all pinhole cameras.
pybind_wrapper_test_script.z
z
Definition: pybind_wrapper_test_script.py:61
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
gtsam::Cal3_S2::shared_ptr
std::shared_ptr< Cal3_S2 > shared_ptr
Definition: Cal3_S2.h:39
EXPECT_DOUBLES_EQUAL
#define EXPECT_DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:161
TestResult
Definition: TestResult.h:26
gtsam::InvDepthCamera3::project
gtsam::Point2 project(const Vector5 &pw, double rho, OptionalJacobian< 2, 6 > H1={}, OptionalJacobian< 2, 5 > H2={}, OptionalJacobian< 2, 1 > H3={}) const
Definition: InvDepthCamera3.h:87
gtsam
traits
Definition: SFMdata.h:40
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
main
int main()
Definition: testInvDepthCamera3.cpp:153
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:41
gtsam::Cal3_S2
The most common 5DOF 3D->2D calibration.
Definition: Cal3_S2.h:34
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
M_PI
#define M_PI
Definition: mconf.h:117
Project2
static Point2 Project2(const Unit3 &point)
Definition: testCalibratedCamera.cpp:113
ceres::Vector
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Definition: gtsam/3rdparty/ceres/eigen.h:38
K
static Cal3_S2::shared_ptr K(new Cal3_S2(1500, 1200, 0, 640, 480))
ceres::sqrt
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
level_camera
PinholeCamera< Cal3_S2 > level_camera(level_pose, *K)


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