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);
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);
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);
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);
333 #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
336 poses.push_back(pose4);
337 measurements.emplace_back(400, 400);
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);
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);
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);
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);
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);
554 measurements.push_back(
z3 +
Point2(10, -10));
556 landmarkDistanceThreshold = 10;
557 double outlierThreshold = 100;
564 outlierThreshold = 5;
572 TEST(triangulation, twoIdenticalPoses) {
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,
746 measurements.at(1) =
u2.retract(
Vector2(-0.02, 0.03));
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);
838 Unit3(measured_px_calibrated[0], measured_px_calibrated[1], 1.0);
839 Unit3 expected_measured_u =
845 Vector2 expectedErrorPinhole =
Vector2(-px_noise[0], -px_noise[1]);
852 Vector2 expectedErrorSpherical(-0.00360842, 0.00180419);