21 using namespace gtsam;
45 double theta =
atan2(ray.y(), ray.x());
46 double phi =
atan2(ray.z(),
sqrt(ray.x()*ray.x()+ray.y()*ray.y()));
47 double rho = 1./ray.norm();
48 Vector6
expected((
Vector(6) << x, y, z, theta, phi, rho).finished());
69 values.
insert(poseKey1, pose1.
retract((
Vector(6) << +0.01, -0.02, +0.03, -0.10, +0.20, -0.30).finished()));
70 values.
insert(poseKey2, pose2.
retract((
Vector(6) << +0.01, +0.02, -0.03, -0.10, +0.20, +0.30).finished()));
71 values.
insert(landmarkKey, Vector6(expected + (
Vector(6) << -0.20, +0.20, -0.35, +0.02, -0.04, +0.05).finished()));
92 Point3 world_landmarkBefore(0,0,0);
94 Vector6 landmarkBefore = values.
at<Vector6>(landmarkKey);
95 double x = landmarkBefore(0), y = landmarkBefore(1), z = landmarkBefore(2);
96 double theta = landmarkBefore(3), phi = landmarkBefore(4), rho = landmarkBefore(5);
99 Point3 world_landmarkAfter(0,0,0);
101 Vector6 landmarkAfter = result.
at<Vector6>(landmarkKey);
102 double x = landmarkAfter(0), y = landmarkAfter(1), z = landmarkAfter(2);
103 double theta = landmarkAfter(3), phi = landmarkAfter(4), rho = landmarkAfter(5);
Jet< T, N > cos(const Jet< T, N > &f)
const Point3 & translation(OptionalJacobian< 3, 6 > Hself={}) const
get translation
virtual const Values & optimize()
static int runAllTests(TestResult &result)
Factor Graph consisting of non-linear factors.
const ValueType at(Key j) const
IsDerived< DERIVEDFACTOR > push_back(std::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Jet< T, N > sin(const Jet< T, N > &f)
static Cal3_S2 K(500, 500, 0.1, 640/2, 480/2)
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
std::shared_ptr< Cal3_S2 > shared_ptr
#define EXPECT(condition)
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.
static const Pose3 pose1(Rot3(), Point3(0, 1, 0.5))
AnnoyingScalar atan2(const AnnoyingScalar &y, const AnnoyingScalar &x)
static Key poseKey1(X(1))
Class retract(const TangentVector &v) const
retract as required by manifold concept: applies v at *this
int optimize(const SfmData &db, const NonlinearFactorGraph &graph, const Values &initial, bool separateCalibration=false)
static const double sigma
void insert(Key j, const Value &val)
Jet< T, N > sqrt(const Jet< T, N > &f)
Inverse Depth Factor based on Civera09tro, Montiel06rss. Landmarks are parameterized as (x...
static Key poseKey2(X(2))
TEST(InvDepthFactorVariant1, optimize)
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
PinholeCamera< Cal3_S2 > camera1(pose1, *sharedCal)
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
std::uint64_t Key
Integer nonlinear key type.
noiseModel::Base::shared_ptr SharedNoiseModel
The most common 5DOF 3D->2D calibration.