21 using namespace gtsam;
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);
virtual const Values & optimize()
static int runAllTests(TestResult &result)
Factor Graph consisting of non-linear factors.
void insert(Key j, const Value &val)
static const double sigma
EIGEN_DEVICE_FUNC const SqrtReturnType sqrt() const
static Cal3_S2 K(500, 500, 0.1, 640/2, 480/2)
const Point3 & translation(OptionalJacobian< 3, 6 > Hself=boost::none) const
get translation
IsDerived< DERIVEDFACTOR > push_back(boost::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
NonlinearFactorGraph graph
JacobianFactor factor3(keyY, A32, keyZ, A33, b3, noiseModel::Isotropic::Sigma(2, sigma3))
static Point3 landmark(0, 0, 5)
PinholeCamera< Cal3_S2 > camera2(pose2,*sharedCal)
Base class for all pinhole cameras.
EIGEN_DEVICE_FUNC const CosReturnType cos() const
const ValueType at(Key j) const
#define EXPECT(condition)
boost::shared_ptr< This > shared_ptr
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
Array< double, 1, 3 > e(1./3., 0.5, 2.)
static SmartStereoProjectionParams params
Jet< T, N > atan2(const Jet< T, N > &g, const Jet< T, N > &f)
static const Pose3 pose1(Rot3(), Point3(0, 1, 0.5))
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
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)
Point2 project(const Point3 &pw, OptionalJacobian< 2, 6 > Dpose=boost::none, OptionalJacobian< 2, 3 > Dpoint=boost::none, OptionalJacobian< 2, DimK > Dcal=boost::none) const
project a 3D point from world coordinates into the image
JacobianFactor factor2(keyX, A21, keyY, A22, b2, noiseModel::Isotropic::Sigma(2, sigma2))
EIGEN_DEVICE_FUNC const SinReturnType sin() const
Inverse Depth Factor based on Civera09tro, Montiel06rss. Landmarks are parameterized as (x...
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
static const Camera camera1(pose1, K)
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
boost::shared_ptr< Cal3_S2 > shared_ptr
std::uint64_t Key
Integer nonlinear key type.
noiseModel::Base::shared_ptr SharedNoiseModel
The most common 5DOF 3D->2D calibration.
JacobianFactor factor1(keyX, A11, b1, noiseModel::Isotropic::Sigma(2, sigma1))