28 using namespace gtsam;
30 #define time timeSingleThreaded 37 return camera.
project(point, H1, H2, boost::none);
63 boost::make_shared<GeneralSFMFactor2<Cal3_S2> >(
z,
model, 1, 2, 3);
64 time(
"GeneralSFMFactor2<Cal3_S2> : ", f1, values);
70 boost::make_shared<ExpressionFactor<Point2> >(
model,
z,
72 time(
"Bin(Leaf,Un(Bin(Leaf,Leaf))): ", f2, values);
78 boost::make_shared<ExpressionFactor<Point2> >(
model,
z,
80 time(
"Ternary(Leaf,Leaf,Leaf) : ", f3, values);
89 time(
"GenericProjectionFactor<P,P>: ", g1, values);
95 boost::make_shared<ExpressionFactor<Point2> >(
model,
z,
97 time(
"Bin(Cnst,Un(Bin(Leaf,Leaf))): ", g2, values);
105 boost::make_shared<ExpressionFactor<Point2> >(
model,
z,
107 time(
"Binary(Leaf,Leaf) : ", g3, values);
gtsam::enable_if_t< needs_eigen_aligned_allocator< T >::value, boost::shared_ptr< T > > make_shared(Args &&...args)
static Point2 project(const Pose3 &pose, const Unit3 &pointAtInfinity, const Cal3_S2::shared_ptr &cal)
noiseModel::Diagonal::shared_ptr model
Point2 myProject(const Pose3 &pose, const Point3 &point, OptionalJacobian< 2, 6 > H1, OptionalJacobian< 2, 3 > H2)
void insert(Key j, const Value &val)
static Cal3_S2 K(500, 500, 0.1, 640/2, 480/2)
Expression< Point2 > Point2_
double f2(const Vector2 &x)
static const Pose3 pose(Rot3(Vector3(1,-1,-1).asDiagonal()), Point3(0, 0, 0.5))
boost::shared_ptr< This > shared_ptr
Basic bearing factor from 2D measurement.
Expression< Cal3_S2 > Cal3_S2_
Point2 f1(const Point3 &p, OptionalJacobian< 2, 3 > H)
Pose3 g1(Rot3(), Point3(100.0, 0.0, 300.0))
double f3(double x1, double x2)
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
Line3 transformTo(const Pose3 &wTc, const Line3 &wL, OptionalJacobian< 4, 6 > Dpose, OptionalJacobian< 4, 4 > Dline)
PinholePose< Cal3_S2 > Camera
static const CalibratedCamera camera(kDefaultPose)
a general SFM factor with an unknown calibration
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
Pose3 g2(g1.expmap(h *V1_g1))
static Point2 project3(const Pose3 &pose, const Point3 &point, const Cal3_S2 &cal)
boost::shared_ptr< Cal3_S2 > fixedK(new Cal3_S2())
noiseModel::Base::shared_ptr SharedNoiseModel
The most common 5DOF 3D->2D calibration.
Point2 uncalibrate(const CAL &K, const Point2 &p, OptionalJacobian< 2, 5 > Dcal, OptionalJacobian< 2, 2 > Dp)