Go to the documentation of this file.
18 using namespace gtsam;
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);
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));
56 TEST( InvDepthFactor, Project3) {
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));
71 TEST( InvDepthFactor, Project4) {
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.));
90 TEST( InvDepthFactor, Dproject_pose)
93 double inv_depth(1./4);
102 TEST( InvDepthFactor, Dproject_landmark)
105 double inv_depth(1./4);
114 TEST( InvDepthFactor, Dproject_inv_depth)
117 double inv_depth(1./4);
129 double inv_depth(1./4);
133 const auto [actual_vec, actual_inv] = inv_camera.
backproject(
z, 4);
139 TEST(InvDepthFactor, backproject2)
143 double inv_depth(1./10);
147 const auto [actual_vec, actual_inv] = inv_camera.
backproject(
z, 10);
static int runAllTests(TestResult &result)
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 const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
Array< double, 1, 3 > e(1./3., 0.5, 2.)
const EIGEN_DEVICE_FUNC AtanReturnType atan() const
Concept check for values that can be used in unit tests.
#define EXPECT(condition)
The most common 5DOF 3D->2D calibration.
TEST(InvDepthFactor, Project1)
static Point3 backproject(const Pose3 &pose, const Point2 &point, const double &depth)
Some functions to compute numerical derivatives.
static Point2 Project1(const Point3 &point)
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)
Inverse Depth Camera based on Civera09tro, Montiel06rss. Landmarks are initialized from the first cam...
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)
Point2 project_(const Pose3 &pose, const Vector5 &landmark, const double &inv_depth)
static Point3 landmark(0, 0, 5)
Base class for all pinhole cameras.
std::shared_ptr< Cal3_S2 > shared_ptr
#define EXPECT_DOUBLES_EQUAL(expected, actual, threshold)
gtsam::Point2 project(const Vector5 &pw, double rho, OptionalJacobian< 2, 6 > H1={}, OptionalJacobian< 2, 5 > H2={}, OptionalJacobian< 2, 1 > H3={}) const
std::pair< Vector5, double > backproject(const gtsam::Point2 &pi, const double depth) const
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
The most common 5DOF 3D->2D calibration.
static Point2 Project2(const Unit3 &point)
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
static Cal3_S2::shared_ptr K(new Cal3_S2(1500, 1200, 0, 640, 480))
Jet< T, N > sqrt(const Jet< T, N > &f)
PinholeCamera< Cal3_S2 > level_camera(level_pose, *K)
gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:40:18