31 using namespace gtsam;
59 static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2),
62 static double missing_uR = std::numeric_limits<double>::quiet_NaN();
64 vector<StereoPoint2> stereo_projectToMultipleCameras(
const StereoCamera&
cam1,
68 vector<StereoPoint2> measurements_cam;
73 measurements_cam.push_back(cam1_uv1);
74 measurements_cam.push_back(cam2_uv1);
75 measurements_cam.push_back(cam3_uv1);
77 return measurements_cam;
165 values.
insert(
x2, level_pose_right);
169 factor1.
add(level_uv_right,
x2,
K2);
171 double actualError = factor1.
error(values);
172 double expectedError = 0.0;
202 StereoPoint2 level_uv_right_missing(level_uv_right.
uL(),missing_uR,level_uv_right.
v());
206 values.insert(
x2, level_pose_right);
210 factor1.
add(level_uv_right_missing,
x2,
K2);
212 double actualError = factor1.
error(values);
213 double expectedError = 0.0;
229 factor2.
add(level_uv_missing,
x1,
K2);
230 factor2.
add(level_uv_right_missing,
x2,
K2);
257 Pose3 noise_pose = Pose3(Rot3::Ypr(-
M_PI / 10, 0., -
M_PI / 10),
262 factor1->add(level_uv,
x1,
K);
263 factor1->add(level_uv_right,
x2,
K);
265 double actualError1 = factor1->error(values);
268 vector<StereoPoint2> measurements;
269 measurements.push_back(level_uv);
270 measurements.push_back(level_uv_right);
272 vector<std::shared_ptr<Cal3_S2Stereo> > Ks;
280 factor2->add(measurements, views, Ks);
282 double actualError2 = factor2->error(values);
305 Point3 landmark3(3, 0, 3.0);
308 vector<StereoPoint2> measurements_l1 = stereo_projectToMultipleCameras(cam1,
309 cam2, cam3, landmark1);
310 vector<StereoPoint2> measurements_l2 = stereo_projectToMultipleCameras(cam1,
311 cam2, cam3, landmark2);
312 vector<StereoPoint2> measurements_l3 = stereo_projectToMultipleCameras(cam1,
313 cam2, cam3, landmark3);
323 smartFactor1->add(measurements_l1, views,
K2);
326 smartFactor2->add(measurements_l2, views,
K2);
329 smartFactor3->add(measurements_l3, views,
K2);
331 const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
341 Pose3 noise_pose = Pose3(Rot3::Ypr(-
M_PI / 100, 0., -
M_PI / 100),
347 values.
insert(
x3, pose3 * noise_pose);
351 Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598,
352 -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598),
353 Point3(0.1, -0.1, 1.9)), values.
at<Pose3>(
x3)));
359 Point3 landmark1_smart = *smartFactor1->point();
360 Point3 landmark2_smart = *smartFactor2->point();
361 Point3 landmark3_smart = *smartFactor3->point();
391 values.
insert(
L(1), landmark1_smart);
392 values.
insert(
L(2), landmark2_smart);
393 values.
insert(
L(3), landmark3_smart);
403 bool verboseCheirality =
true;
405 graph2.
push_back(ProjectionFactor(measurements_l1[0],
model,
x1,
L(1),
K2,
false, verboseCheirality));
406 graph2.
push_back(ProjectionFactor(measurements_l1[1],
model,
x2,
L(1),
K2,
false, verboseCheirality));
407 graph2.
push_back(ProjectionFactor(measurements_l1[2],
model,
x3,
L(1),
K2,
false, verboseCheirality));
409 graph2.
push_back(ProjectionFactor(measurements_l2[0],
model,
x1,
L(2),
K2,
false, verboseCheirality));
410 graph2.
push_back(ProjectionFactor(measurements_l2[1],
model,
x2,
L(2),
K2,
false, verboseCheirality));
411 graph2.
push_back(ProjectionFactor(measurements_l2[2],
model,
x3,
L(2),
K2,
false, verboseCheirality));
413 graph2.
push_back(ProjectionFactor(measurements_l3[0],
model,
x1,
L(3),
K2,
false, verboseCheirality));
414 graph2.
push_back(ProjectionFactor(measurements_l3[1],
model,
x2,
L(3),
K2,
false, verboseCheirality));
415 graph2.
push_back(ProjectionFactor(measurements_l3[2],
model,
x3,
L(3),
K2,
false, verboseCheirality));
421 Values result2 = optimizer2.optimize();
447 Point3 landmark3(3, 0, 3.0);
450 vector<StereoPoint2> measurements_l1 = stereo_projectToMultipleCameras(cam1,
451 cam2, cam3, landmark1);
452 vector<StereoPoint2> measurements_l2 = stereo_projectToMultipleCameras(cam1,
453 cam2, cam3, landmark2);
454 vector<StereoPoint2> measurements_l3 = stereo_projectToMultipleCameras(cam1,
455 cam2, cam3, landmark3);
465 smartFactor1->add(measurements_l1, views,
K2);
468 smartFactor2->add(measurements_l2, views,
K2);
471 smartFactor3->add(measurements_l3, views,
K2);
473 const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
483 Pose3 noise_pose = Pose3(Rot3::Ypr(-
M_PI / 100, 0., -
M_PI / 100),
489 values.
insert(
x3, pose3 * noise_pose);
493 Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598,
494 -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598),
495 Point3(0.1, -0.1, 1.9)), values.
at<Pose3>(
x3)));
523 Pose3 cameraPose3 = cameraPose1 * Pose3(
Rot3(),
Point3(0,-1,0));
540 Point3 landmark3(5, 0, 3.0);
542 Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
556 vector<StereoPoint2> measurements_cam1_stereo, measurements_cam2_stereo, measurements_cam3_stereo;
557 for(
size_t k=0; k<measurements_cam1.size();k++)
558 measurements_cam1_stereo.push_back(
StereoPoint2(measurements_cam1[k].
x() , missing_uR , measurements_cam1[k].
y()));
560 for(
size_t k=0; k<measurements_cam2.size();k++)
561 measurements_cam2_stereo.push_back(
StereoPoint2(measurements_cam2[k].
x() , missing_uR , measurements_cam2[k].
y()));
563 for(
size_t k=0; k<measurements_cam3.size();k++)
564 measurements_cam3_stereo.push_back(
StereoPoint2(measurements_cam3[k].
x() , missing_uR , measurements_cam3[k].
y()));
573 smartFactor1.
add(measurements_cam1_stereo, views, Kmono);
576 smartFactor2.
add(measurements_cam2_stereo, views, Kmono);
579 smartFactor3.
add(measurements_cam3_stereo, views, Kmono);
581 const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
596 double actualError = graph.
error(gtValues);
597 double expectedError = 0.0;
605 values.
insert(
x3, bodyPose3*noise_pose);
634 Point3 landmark3(3, 0, 3.0);
637 vector<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1,
638 cam2, cam3, landmark1);
639 vector<StereoPoint2> measurements_cam2 = stereo_projectToMultipleCameras(cam1,
640 cam2, cam3, landmark2);
641 vector<StereoPoint2> measurements_cam3 = stereo_projectToMultipleCameras(cam1,
642 cam2, cam3, landmark3);
648 smartFactor1->add(measurements_cam1, views,
K);
651 smartFactor2->add(measurements_cam2, views,
K);
654 smartFactor3->add(measurements_cam3, views,
K);
656 const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
666 Pose3 noise_pose = Pose3(Rot3::Ypr(-
M_PI / 100, 0., -
M_PI / 100),
671 values.
insert(
x3, pose3 * noise_pose);
700 Point3 landmark3(3, 0, 3.0);
703 vector<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1,
704 cam2, cam3, landmark1);
705 vector<StereoPoint2> measurements_cam2 = stereo_projectToMultipleCameras(cam1,
706 cam2, cam3, landmark2);
707 vector<StereoPoint2> measurements_cam3 = stereo_projectToMultipleCameras(cam1,
708 cam2, cam3, landmark3);
713 sp = measurements_cam2[2];
720 smartFactor1->add(measurements_cam1, views,
K);
723 smartFactor2->add(measurements_cam2, views,
K);
726 smartFactor3->add(measurements_cam3, views,
K);
728 const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
738 Pose3 noise_pose = Pose3(Rot3::Ypr(-
M_PI / 100, 0., -
M_PI / 100),
743 values.
insert(
x3, pose3 * noise_pose);
774 Point3 landmark3(3, 0, 3.0);
777 vector<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1,
778 cam2, cam3, landmark1);
779 vector<StereoPoint2> measurements_cam2 = stereo_projectToMultipleCameras(cam1,
780 cam2, cam3, landmark2);
781 vector<StereoPoint2> measurements_cam3 = stereo_projectToMultipleCameras(cam1,
782 cam2, cam3, landmark3);
789 smartFactor1->add(measurements_cam1, views,
K);
792 smartFactor2->add(measurements_cam2, views,
K);
795 smartFactor3->add(measurements_cam3, views,
K);
797 const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
807 Pose3 noise_pose = Pose3(Rot3::Ypr(-
M_PI / 100, 0., -
M_PI / 100),
812 values.
insert(
x3, pose3 * noise_pose);
842 Point3 landmark3(3, 0, 3.0);
843 Point3 landmark4(5, -0.5, 1);
846 vector<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1,
847 cam2, cam3, landmark1);
848 vector<StereoPoint2> measurements_cam2 = stereo_projectToMultipleCameras(cam1,
849 cam2, cam3, landmark2);
850 vector<StereoPoint2> measurements_cam3 = stereo_projectToMultipleCameras(cam1,
851 cam2, cam3, landmark3);
852 vector<StereoPoint2> measurements_cam4 = stereo_projectToMultipleCameras(cam1,
853 cam2, cam3, landmark4);
855 measurements_cam4.at(0) = measurements_cam4.at(0) +
StereoPoint2(10, 10, 1);
863 smartFactor1->add(measurements_cam1, views,
K);
866 smartFactor2->add(measurements_cam2, views,
K);
869 smartFactor3->add(measurements_cam3, views,
K);
872 smartFactor4->add(measurements_cam4, views,
K);
876 smartFactor4b->add(measurements_cam4, views,
K);
878 const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
888 Pose3 noise_pose = Pose3(Rot3::Ypr(-
M_PI / 100, 0., -
M_PI / 100),
905 EXPECT(smartFactor1->point());
906 EXPECT(smartFactor2->point());
907 EXPECT(smartFactor3->point());
908 EXPECT(smartFactor4->point().outlier());
909 EXPECT(smartFactor4b->point());
1043 views.push_back(
x1);
1044 views.push_back(
x2);
1045 views.push_back(
x3);
1056 Pose3
pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05),
Point3(0, 0, 0));
1062 Point3 landmark3(3, 0, 3.0);
1065 vector<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1,
1066 cam2, cam3, landmark1);
1067 vector<StereoPoint2> measurements_cam2 = stereo_projectToMultipleCameras(cam1,
1068 cam2, cam3, landmark2);
1069 vector<StereoPoint2> measurements_cam3 = stereo_projectToMultipleCameras(cam1,
1070 cam2, cam3, landmark3);
1076 smartFactor1->add(measurements_cam1, views,
K);
1079 smartFactor2->add(measurements_cam2, views,
K);
1082 smartFactor3->add(measurements_cam3, views,
K);
1094 Pose3 noise_pose = Pose3(Rot3::Ypr(-
M_PI / 100, 0., -
M_PI / 100),
1096 values.
insert(
x3, pose3 * noise_pose);
1099 std::shared_ptr<GaussianFactor> hessianFactor1 = smartFactor1->linearize(
1101 std::shared_ptr<GaussianFactor> hessianFactor2 = smartFactor2->linearize(
1103 std::shared_ptr<GaussianFactor> hessianFactor3 = smartFactor3->linearize(
1106 Matrix CumulativeInformation = hessianFactor1->information()
1107 + hessianFactor2->information() + hessianFactor3->information();
1109 std::shared_ptr<GaussianFactorGraph> GaussianGraph = graph.
linearize(
1111 Matrix GraphInformation = GaussianGraph->hessian().first;
1116 Matrix AugInformationMatrix = hessianFactor1->augmentedInformation()
1117 + hessianFactor2->augmentedInformation()
1118 + hessianFactor3->augmentedInformation();
1121 Vector InfoVector = AugInformationMatrix.block(0, 18, 18, 1);
1312 views.push_back(
x1);
1313 views.push_back(
x2);
1314 views.push_back(
x3);
1330 vector<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1,
1331 cam2, cam3, landmark1);
1334 smartFactorInstance->add(measurements_cam1, views,
K);
1341 std::shared_ptr<GaussianFactor> hessianFactor =
1342 smartFactorInstance->linearize(values);
1345 Pose3 poseDrift = Pose3(Rot3::Ypr(-
M_PI / 2, 0., -
M_PI / 2),
Point3(0, 0, 0));
1352 std::shared_ptr<GaussianFactor> hessianFactorRot =
1353 smartFactorInstance->linearize(rotValues);
1359 hessianFactorRot->information(), 1
e-7));
1361 Pose3 poseDrift2 = Pose3(Rot3::Ypr(-
M_PI / 2, -
M_PI / 3, -
M_PI / 2),
1369 std::shared_ptr<GaussianFactor> hessianFactorRotTran =
1370 smartFactorInstance->linearize(tranValues);
1375 hessianFactorRotTran->information(), 1
e-6));
1382 views.push_back(
x1);
1383 views.push_back(
x2);
1384 views.push_back(
x3);
1398 vector<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1,
1399 cam2, cam3, landmark1);
1402 smartFactor->add(measurements_cam1, views,
K2);
1409 std::shared_ptr<GaussianFactor> hessianFactor = smartFactor->linearize(
1413 EXPECT(smartFactor->isValid());
1422 std::shared_ptr<GaussianFactor> hessianFactorRot = smartFactor->linearize(
1426 EXPECT(smartFactor->isValid());
1431 hessianFactorRot->information(), 1
e-6));
1441 std::shared_ptr<GaussianFactor> hessianFactorRotTran =
1442 smartFactor->linearize(tranValues);
1447 #ifdef GTSAM_USE_EIGEN_MKL 1448 hessianFactorRotTran->information(), 1
e-5));
1450 hessianFactorRotTran->information(), 1
e-6));
static PinholeCamera< Cal3_S2 > cam1(pose3, cal1)
std::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor
virtual const Values & optimize()
Camera cam3(interp_pose3, sharedK)
bool getVerboseCheirality() const
static int runAllTests(TestResult &result)
noiseModel::Diagonal::shared_ptr model
const ValueType at(Key j) const
void setRetriangulationThreshold(double retriangulationTh)
static StereoCamera cam2(pose3, cal4ptr)
#define DOUBLES_EQUAL(expected, actual, threshold)
IsDerived< DERIVEDFACTOR > push_back(std::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
static const Camera level_camera(level_pose, K2)
void setEnableEPI(bool enableEPI)
CameraSet< Camera > Cameras
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
TriangulationParameters triangulation
static Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0))
double dynamicOutlierRejectionThreshold
static Cal3_S2 K(500, 500, 0.1, 640/2, 480/2)
Pose3 x2(Rot3::Ypr(0.0, 0.0, 0.0), l2)
void add(const StereoPoint2 &measured, const Key &poseKey, const std::shared_ptr< Cal3_S2Stereo > &K)
static const Camera level_camera_right(pose_right, K2)
NonlinearFactorGraph graph
A set of cameras, all with their own calibration.
Point2 landmark1(5.0, 1.5)
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
#define EXPECT_DOUBLES_EQUAL(expected, actual, threshold)
DegeneracyMode getDegeneracyMode() const
Smart stereo factor on poses, assuming camera calibration is fixed.
Pose3 inverse() const
inverse transformation with derivatives
static const SmartProjectionParams params
static Point3 landmark(0, 0, 5)
std::shared_ptr< GaussianFactorGraph > linearize(const Values &linearizationPoint) const
Linearize a nonlinear factor graph.
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
double totalReprojectionError(const Cameras &cameras, std::optional< Point3 > externalPoint={}) const
void setRankTolerance(double rankTol)
Implements a prior on the translation component of a pose.
static const Cal3Bundler K2(625, 1e-3, 1e-3)
Point2 landmark2(7.0, 1.5)
double getRetriangulationThreshold() const
double landmarkDistanceThreshold
std::shared_ptr< Cal3_S2 > shared_ptr
double error(const Values &values) const override
The most common 5DOF 3D->2D calibration, stereo version.
bool enableEPI
if set to true, will refine triangulation using LM
double rankTolerance
(the rank is the number of singular values of the triangulation matrix which are larger than rankTole...
#define EXPECT(condition)
static Pose3 pose3(rt3, pt3)
static Point2 measurement1(323.0, 240.0)
void projectToMultipleCameras(const CAMERA &cam1, const CAMERA &cam2, const CAMERA &cam3, Point3 landmark, typename CAMERA::MeasurementVector &measurements_cam)
LevenbergMarquardtParams lmParams
void setLandmarkDistanceThreshold(double landmarkDistanceThreshold)
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
Array< double, 1, 3 > e(1./3., 0.5, 2.)
The most common 5DOF 3D->2D calibration.
bool getThrowCheirality() const
StereoPoint2 project(const Point3 &point) const
Project 3D point to StereoPoint2 (uL,uR,v)
std::shared_ptr< This > shared_ptr
shared_ptr to this class
double error(const Values &values) const
Class compose(const Class &g) const
Reprojection of a LANDMARK to a 2D point.
Base::Cameras cameras(const Values &values) const override
Pose3 x3(Rot3::Ypr(M_PI/4.0, 0.0, 0.0), l2)
std::shared_ptr< Cal3_S2Stereo > shared_ptr
static const Pose3 pose1(Rot3(), Point3(0, 1, 0.5))
A non-linear factor for stereo measurements.
NonlinearFactorGraph graph2()
noiseModel::Diagonal::shared_ptr SharedDiagonal
static Key poseKey1(X(1))
TriangulationResult triangulateSafe(const Cameras &cameras) const
triangulateSafe
TEST(SmartStereoProjectionPoseFactor, params)
void setDegeneracyMode(DegeneracyMode degMode)
std::vector< Point2, Eigen::aligned_allocator< Point2 > > Point2Vector
static const Point2 level_uv_right
void insert(Key j, const Value &val)
void setLinearizationMode(LinearizationMode linMode)
void tictoc_finishedIteration_()
LinearizationMode getLinearizationMode() const
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
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
void setDynamicOutlierRejectionThreshold(double dynOutRejectionThreshold)
static const Point2 level_uv
std::uint64_t Key
Integer nonlinear key type.
noiseModel::Base::shared_ptr SharedNoiseModel
TriangulationParameters getTriangulationParameters() const
TEST_UNSAFE(SmartStereoProjectionPoseFactor, noiseless)