34 using namespace gtsam;
39 std::make_shared<Cal3_S2>(1500, 1200, 0.1, 640, 480);
63 TEST(triangulation, twoPoses) {
66 double rank_tol = 1
e-9;
70 std::optional<Point3> actual1 =
76 std::optional<Point3> actual2 =
82 measurements.at(0) +=
Point2(0.1, 0.5);
83 measurements.at(1) +=
Point2(-0.2, 0.3);
85 std::optional<Point3> actual3 =
91 std::optional<Point3> actual4 =
96 TEST(triangulation, twoCamerasUsingLOST) {
103 double rank_tol = 1
e-9;
106 std::optional<Point3> actual1 =
107 triangulatePoint3<PinholeCamera<Cal3_S2>>(cameras, measurements, rank_tol,
115 measurements[0] +=
Point2(0.1, 0.5);
116 measurements[1] +=
Point2(-0.2, 0.3);
117 std::optional<Point3> actual2 =
118 triangulatePoint3<PinholeCamera<Cal3_S2>>(
119 cameras, measurements, rank_tol,
120 false, measurementNoise,
125 TEST(triangulation, twoCamerasLOSTvsDLT) {
132 cameras.emplace_back(pose1, identityK);
133 cameras.emplace_back(pose2, identityK);
136 Point2 x1_noisy = cameras[0].project(landmark) +
Point2(0.00817, 0.00977);
137 Point2 x2_noisy = cameras[1].project(landmark) +
Point2(-0.00610, 0.01969);
140 const double rank_tol = 1
e-9;
143 std::optional<Point3> estimateLOST =
144 triangulatePoint3<PinholeCamera<Cal3_S2>>(cameras, measurements, rank_tol,
152 std::optional<Point3> estimateDLT =
153 triangulatePoint3<Cal3_S2>(cameras, measurements, rank_tol,
false);
156 EXPECT((landmark - *estimateLOST).norm() <= (landmark - *estimateDLT).norm());
161 TEST(triangulation, twoPosesCal3DS2) {
162 static const std::shared_ptr<Cal3DS2> sharedDistortedCal =
163 std::make_shared<Cal3DS2>(1500, 1200, 0, 640, 480, -.3, 0.1, 0.0001,
176 double rank_tol = 1
e-9;
180 std::optional<Point3> actual1 =
181 triangulatePoint3<Cal3DS2>(
kPoses, sharedDistortedCal, measurements,
187 std::optional<Point3> actual2 =
188 triangulatePoint3<Cal3DS2>(
kPoses, sharedDistortedCal, measurements,
194 measurements.at(0) +=
Point2(0.1, 0.5);
195 measurements.at(1) +=
Point2(-0.2, 0.3);
197 std::optional<Point3> actual3 =
198 triangulatePoint3<Cal3DS2>(
kPoses, sharedDistortedCal, measurements,
204 std::optional<Point3> actual4 =
205 triangulatePoint3<Cal3DS2>(
kPoses, sharedDistortedCal, measurements,
213 TEST(triangulation, twoPosesFisheye) {
215 static const std::shared_ptr<Calibration> sharedDistortedCal =
216 std::make_shared<Calibration>(1500, 1200, .1, 640, 480, -.3, 0.1,
229 double rank_tol = 1
e-9;
233 std::optional<Point3> actual1 =
234 triangulatePoint3<Calibration>(
kPoses, sharedDistortedCal, measurements,
240 std::optional<Point3> actual2 =
241 triangulatePoint3<Calibration>(
kPoses, sharedDistortedCal, measurements,
247 measurements.at(0) +=
Point2(0.1, 0.5);
248 measurements.at(1) +=
Point2(-0.2, 0.3);
250 std::optional<Point3> actual3 =
251 triangulatePoint3<Calibration>(
kPoses, sharedDistortedCal, measurements,
257 std::optional<Point3> actual4 =
258 triangulatePoint3<Calibration>(
kPoses, sharedDistortedCal, measurements,
265 TEST(triangulation, twoPosesBundler) {
266 std::shared_ptr<Cal3Bundler> bundlerCal =
267 std::make_shared<Cal3Bundler>(1500, 0.1, 0.2, 640, 480);
272 Point2 z1 = camera1.project(kLandmark);
278 double rank_tol = 1
e-9;
280 std::optional<Point3> actual =
281 triangulatePoint3<Cal3Bundler>(
kPoses, bundlerCal, measurements, rank_tol,
286 measurements.at(0) +=
Point2(0.1, 0.5);
287 measurements.at(1) +=
Point2(-0.2, 0.3);
289 std::optional<Point3> actual2 =
290 triangulatePoint3<Cal3Bundler>(
kPoses, bundlerCal, measurements, rank_tol,
296 TEST(triangulation, fourPoses) {
299 std::optional<Point3> actual =
300 triangulatePoint3<Cal3_S2>(poses,
kSharedCal, measurements);
305 measurements.at(0) +=
Point2(0.1, 0.5);
306 measurements.at(1) +=
Point2(-0.2, 0.3);
308 std::optional<Point3> actual2 =
309 triangulatePoint3<Cal3_S2>(poses,
kSharedCal, measurements);
317 poses.push_back(pose3);
318 measurements.push_back(z3 +
Point2(0.1, -0.1));
320 std::optional<Point3> triangulated_3cameras =
321 triangulatePoint3<Cal3_S2>(poses,
kSharedCal, measurements);
325 std::optional<Point3> triangulated_3cameras_opt =
326 triangulatePoint3<Cal3_S2>(poses,
kSharedCal, measurements, 1
e-9,
true);
330 Pose3 pose4 = Pose3(Rot3::Ypr(
M_PI / 2, 0., -
M_PI / 2),
Point3(0, 0, 1));
333 #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION 336 poses.push_back(pose4);
337 measurements.emplace_back(400, 400);
339 CHECK_EXCEPTION(triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements),
345 TEST(triangulation, threePoses_robustNoiseModel) {
355 std::optional<Point3> actual =
356 triangulatePoint3<Cal3_S2>(poses,
kSharedCal, measurements);
360 measurements.at(0) +=
Point2(100, 120);
363 std::optional<Point3> actual2 =
364 triangulatePoint3<Cal3_S2>(poses,
kSharedCal, measurements);
366 EXPECT( (kLandmark - *actual2).norm() >= 0.2);
367 EXPECT( (kLandmark - *actual2).norm() <= 0.5);
370 std::optional<Point3> actual3 =
371 triangulatePoint3<Cal3_S2>(poses,
kSharedCal, measurements, 1
e-9,
true);
376 auto model = noiseModel::Robust::Create(
377 noiseModel::mEstimator::Huber::Create(1.345), noiseModel::Unit::Create(2));
378 std::optional<Point3> actual4 = triangulatePoint3<Cal3_S2>(
385 TEST(triangulation, fourPoses_robustNoiseModel) {
396 std::optional<Point3> actual =
397 triangulatePoint3<Cal3_S2>(poses,
kSharedCal, measurements);
401 measurements.at(0) +=
Point2(100, 120);
403 measurements.at(1) +=
Point2(0.1, 0.2);
404 measurements.at(2) +=
Point2(0.2, 0.2);
405 measurements.at(3) +=
Point2(0.3, 0.1);
408 std::optional<Point3> actual2 =
409 triangulatePoint3<Cal3_S2>(poses,
kSharedCal, measurements);
411 EXPECT( (kLandmark - *actual2).norm() >= 0.1);
412 EXPECT( (kLandmark - *actual2).norm() <= 0.5);
415 std::optional<Point3> actual3 =
416 triangulatePoint3<Cal3_S2>(poses,
kSharedCal, measurements, 1
e-9,
true);
421 auto model = noiseModel::Robust::Create(
422 noiseModel::mEstimator::Huber::Create(1.345), noiseModel::Unit::Create(2));
423 std::optional<Point3> actual4 = triangulatePoint3<Cal3_S2>(
430 TEST(triangulation, fourPoses_distinct_Ks) {
446 std::optional<Point3> actual =
447 triangulatePoint3<Cal3_S2>(cameras, measurements);
452 measurements.at(0) +=
Point2(0.1, 0.5);
453 measurements.at(1) +=
Point2(-0.2, 0.3);
455 std::optional<Point3> actual2 =
456 triangulatePoint3<Cal3_S2>(cameras, measurements);
461 Cal3_S2 K3(700, 500, 0, 640, 480);
465 cameras.push_back(camera3);
466 measurements.push_back(z3 +
Point2(0.1, -0.1));
468 std::optional<Point3> triangulated_3cameras =
469 triangulatePoint3<Cal3_S2>(cameras, measurements);
473 std::optional<Point3> triangulated_3cameras_opt =
474 triangulatePoint3<Cal3_S2>(cameras, measurements, 1
e-9,
true);
478 Pose3 pose4 = Pose3(Rot3::Ypr(
M_PI / 2, 0., -
M_PI / 2),
Point3(0, 0, 1));
482 #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION 485 cameras.push_back(camera4);
486 measurements.emplace_back(400, 400);
493 TEST(triangulation, fourPoses_distinct_Ks_distortion) {
494 Cal3DS2 K1(1500, 1200, 0, 640, 480, -.3, 0.1, 0.0001, -0.0003);
499 Cal3DS2 K2(1600, 1300, 0, 650, 440, -.2, 0.05, 0.0002, -0.0001);
509 std::optional<Point3> actual =
510 triangulatePoint3<Cal3DS2>(cameras, measurements);
515 TEST(triangulation, outliersAndFarLandmarks) {
531 double landmarkDistanceThreshold = 10;
533 1.0,
false, landmarkDistanceThreshold);
539 landmarkDistanceThreshold = 4;
541 1.0,
false, landmarkDistanceThreshold);
549 Cal3_S2 K3(700, 500, 0, 640, 480);
553 cameras.push_back(camera3);
554 measurements.push_back(z3 +
Point2(10, -10));
556 landmarkDistanceThreshold = 10;
557 double outlierThreshold = 100;
564 outlierThreshold = 5;
572 TEST(triangulation, twoIdenticalPoses) {
579 const vector<Pose3> poses{
kPose1, kPose1};
591 const vector<Pose3> poses{
Pose3()};
599 TEST(triangulation, StereoTriangulateNonlinear) {
600 auto stereoK = std::make_shared<Cal3_S2Stereo>(1733.75, 1733.75, 0, 689.645,
605 m1 << 0.796888717, 0.603404026, -0.0295271487, 46.6673779, 0.592783835,
606 -0.77156583, 0.230856632, 66.2186159, 0.116517574, -0.201470143,
607 -0.9725393, -4.28382528, 0, 0, 0, 1;
609 m2 << -0.955959025, -0.29288915, -0.0189328569, 45.7169799, -0.29277519,
610 0.947083213, 0.131587097, 65.843136, -0.0206094928, 0.131334858,
611 -0.991123524, -4.3525033, 0, 0, 0, 1;
614 const StereoCameras cameras{{
Pose3(m1), stereoK}, {
Pose3(m2), stereoK}};
617 measurements.push_back(
StereoPoint2(226.936, 175.212, 424.469));
618 measurements.push_back(
StereoPoint2(339.571, 285.547, 669.973));
621 Point3(46.0536958, 66.4621179, -6.56285929);
623 Point3 actual = triangulateNonlinear<StereoCamera>(cameras, measurements,
initial);
646 const SharedDiagonal posePrior = noiseModel::Isotropic::Sigma(6, 1
e-9);
665 cameras[0], measurements[0], unit,
Symbol(
'l', 1));
667 cameras[1], measurements[1], unit,
Symbol(
'l', 1));
703 TEST(triangulation, twoPoses_sphericalCamera) {
711 std::vector<Unit3> measurements{u1, u2};
714 cameras.push_back(cam1);
715 cameras.push_back(cam2);
717 double rank_tol = 1
e-9;
725 point = triangulateNonlinear<SphericalCamera>(cameras, measurements,
point);
730 std::optional<Point3> actual1 =
731 triangulatePoint3<SphericalCamera>(cameras, measurements, rank_tol,
737 std::optional<Point3> actual2 =
738 triangulatePoint3<SphericalCamera>(cameras, measurements, rank_tol,
748 std::optional<Point3> actual3 =
749 triangulatePoint3<SphericalCamera>(cameras, measurements, rank_tol,
755 std::optional<Point3> actual4 =
756 triangulatePoint3<SphericalCamera>(cameras, measurements, rank_tol,
762 TEST(triangulation, twoPoses_sphericalCamera_extremeFOV) {
765 Rot3::Ypr(-
M_PI / 2, 0., -
M_PI / 2),
782 const std::vector<Unit3> measurements{u1, u2};
785 cameras.push_back(cam1);
786 cameras.push_back(cam2);
788 double rank_tol = 1
e-9;
793 #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION 794 CHECK_EXCEPTION(triangulatePoint3<SphericalCamera>(cameras, measurements,
797 #else // otherwise project should not throw the exception 798 std::optional<Point3> actual1 =
799 triangulatePoint3<SphericalCamera>(cameras, measurements, rank_tol,
807 #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION 808 CHECK_EXCEPTION(triangulatePoint3<SphericalCamera>(cameras, measurements,
811 #else // otherwise project should not throw the exception 812 std::optional<Point3> actual1 =
813 triangulatePoint3<SphericalCamera>(cameras, measurements, rank_tol,
821 TEST(triangulation, reprojectionError_cameraComparison) {
823 Rot3::Ypr(-
M_PI / 2, 0., -
M_PI / 2),
825 Point3 landmarkL(5.0, 0.0, 0.0);
835 Vector2 measured_px = px + px_noise;
836 Vector2 measured_px_calibrated = sharedK->calibrate(measured_px);
838 Unit3(measured_px_calibrated[0], measured_px_calibrated[1], 1.0);
839 Unit3 expected_measured_u =
840 Unit3(px_noise[0] / sharedK->fx(), px_noise[1] / sharedK->fy(), 1.0);
845 Vector2 expectedErrorPinhole =
Vector2(-px_noise[0], -px_noise[1]);
852 Vector2 expectedErrorSpherical(-0.00360842, 0.00180419);
static Point2 project2(const CalibratedCamera &camera, const Point3 &point)
static PinholeCamera< Cal3_S2 > cam1(pose3, cal1)
Vector2 reprojectionError(const Point3 &point, const Unit3 &measured, OptionalJacobian< 2, 6 > Dpose={}, OptionalJacobian< 2, 3 > Dpoint={}) const
std::vector< Pose3 > Pose3Vector
virtual const Values & optimize()
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
static int runAllTests(TestResult &result)
static const Point2Vector kMeasurements
noiseModel::Diagonal::shared_ptr model
const ValueType at(Key j) const
Base class to create smart factors on poses or cameras.
static StereoCamera cam2(pose3, cal4ptr)
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
static const Rot3 upright
Calibrated camera with spherical projection.
Calibration of a camera with radial distortion, calculations in base class Cal3DS2_Base.
static const Pose3 kPose1
static const PinholeCamera< Cal3_S2 > kCamera2(kPose2, *kSharedCal)
#define CHECK_EXCEPTION(condition, exception_name)
static const std::vector< Pose3 > kPoses
NonlinearFactorGraph graph
A set of cameras, all with their own calibration.
Unit3 project(const Point3 &point, OptionalJacobian< 2, 6 > Dpose={}, OptionalJacobian< 2, 3 > Dpoint={}) const
static const Pose3 kPose2
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
static const SmartProjectionParams params
std::vector< StereoPoint2 > StereoPoint2Vector
static Point3 landmark(0, 0, 5)
static const CalibratedCamera camera3(pose1)
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
static const Cal3Bundler K2(625, 1e-3, 1e-3)
Represents a 3D point on a unit sphere.
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
TEST(triangulation, twoPoses)
std::vector< Matrix34, Eigen::aligned_allocator< Matrix34 > > projectionMatricesFromCameras(const CameraSet< CAMERA > &cameras)
std::shared_ptr< Cal3_S2 > shared_ptr
Point3 triangulateDLT(const std::vector< Matrix34, Eigen::aligned_allocator< Matrix34 >> &projection_matrices, const Point2Vector &measurements, double rank_tol)
void addExpressionFactor(const SharedNoiseModel &R, const T &z, const Expression< T > &h)
static const std::shared_ptr< Cal3_S2 > kSharedCal
#define EXPECT(condition)
static Pose3 pose3(rt3, pt3)
Functions for triangulation.
static const Camera2 camera2(pose1, K2)
RealScalar RealScalar * px
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
Array< double, 1, 3 > e(1./3., 0.5, 2.)
static Cal3_S2::shared_ptr K1(new Cal3_S2(fov, w, h))
Exception thrown by triangulateDLT when SVD returns rank < 3.
The most common 5DOF 3D->2D calibration.
TriangulationResult triangulateSafe(const CameraSet< CAMERA > &cameras, const typename CAMERA::MeasurementVector &measured, const TriangulationParameters ¶ms)
triangulateSafe: extensive checking of the outcome
static const Cal3_S2::shared_ptr sharedK(new Cal3_S2(fov, w, h))
Calibration of a camera with radial distortion that also supports Lie-group behaviors for optimizatio...
Exception thrown by triangulateDLT when landmark is behind one or more of the cameras.
static const Pose3 pose1(Rot3(), Point3(0, 1, 0.5))
A non-linear factor for stereo measurements.
Point2 reprojectionError(const Point3 &pw, const Point2 &measured, OptionalJacobian< 2, 6 > Dpose={}, OptionalJacobian< 2, 3 > Dpoint={}, OptionalJacobian< 2, DimK > Dcal={}) const
project a 3D point from world coordinates into the image
noiseModel::Diagonal::shared_ptr SharedDiagonal
A Stereo Camera based on two Simple Cameras.
static const Point3 kLandmark(5, 0.5, 1.2)
std::vector< Point2, Eigen::aligned_allocator< Point2 > > Point2Vector
Unit3 retract(const Vector2 &v, OptionalJacobian< 2, 2 > H={}) const
The retract function.
int optimize(const SfmData &db, const NonlinearFactorGraph &graph, const Values &initial, bool separateCalibration=false)
static const PinholeCamera< Cal3_S2 > kCamera1(kPose1, *kSharedCal)
void insert(Key j, const Value &val)
PinholeCamera< Cal3_S2 > camera1(pose1, *sharedCal)
Calibration used by Bundler.
Calibration of a fisheye camera.
noiseModel::Base::shared_ptr SharedNoiseModel