23 #include <boost/bind.hpp> 26 using namespace gtsam;
38 set.push_back(camera);
39 set.push_back(camera);
43 set2.push_back(camera);
48 ZZ
z =
set.project2(p);
57 camera.project2(p, F1, E1);
65 set.project2(p, Fs, E);
73 measured.push_back(
Point2(1, 2));
74 measured.push_back(
Point2(3, 4));
78 expectedV << -1, -2, -3, -4;
79 Vector actualV =
set.reprojectionError(p, measured);
84 F <<
F1, Matrix29::Zero(), Matrix29::Zero(),
F1;
86 Matrix34 Et = E.transpose();
90 Vector v = Ft * (b - E * P * Et *
b);
91 schur << Ft * F - Ft * E * P * Et *
F,
v, v.transpose(), 30;
97 Set::UpdateSchurComplement<3>(Fs,
E,
P,
b, allKeys,
keys, actualReduced);
102 Set::UpdateSchurComplement<3>(Fs,
E,
P,
b, allKeys, keys2, actualReduced);
104 reverse_b << b.tail<2>(), b.head<2>();
105 Vector reverse_v = Ft * (reverse_b - E * P * Et * reverse_b);
107 A << Ft * F - Ft * E * P * Et *
F, reverse_v, reverse_v.transpose(), 30;
114 camera.backprojectPointAtInfinity(
Point2(0,0))));
115 actualV =
set.reprojectionError(pointAtInfinity, measured, Fs, E);
120 camera.project2(pointAtInfinity, F1, E1);
135 set.push_back(camera);
136 set.push_back(camera);
142 ZZ
z =
set.project2(p);
158 set.project2(p, Fs, E);
ComplexSchur< MatrixXcf > schur(4)
std::vector< MatrixZD, Eigen::aligned_allocator< MatrixZD > > FBlocks
static int runAllTests(TestResult &result)
Base class to create smart factors on poses or cameras.
const vector< Matrix26, Eigen::aligned_allocator< Matrix26 > > FBlocks
Some functions to compute numerical derivatives.
A set of cameras, all with their own calibration.
Unit3 pointAtInfinity(0, 0,-1000)
StereoPoint2Vector MeasurementVector
Represents a 3D point on a unit sphere.
Base class for all pinhole cameras.
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
std::vector< Point2, Eigen::aligned_allocator< Point2 > > Point2Vector
#define EXPECT(condition)
StereoPoint2 project2(const Point3 &point, OptionalJacobian< 3, 6 > H1=boost::none, OptionalJacobian< 3, 3 > H2=boost::none) const
#define LONGS_EQUAL(expected, actual)
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
A Stereo Camera based on two Simple Cameras.
#define EXPECT_LONGS_EQUAL(expected, actual)
Eigen::SelfAdjointView< constBlock, Eigen::Upper > selfadjointView(DenseIndex I, DenseIndex J) const
Return the square sub-matrix that contains blocks(i:j, i:j).
PinholePose< Cal3_S2 > Camera
static const CalibratedCamera camera(kDefaultPose)
Calibration used by Bundler.