30 using namespace std::placeholders;
33 static const double rankTol = 1.0;
35 static const double sigma = 0.1;
125 double expectedError = 0.0;
134 std::bind(&SmartFactor::whitenedError<Point3>, factor, cameras, std::placeholders::_1);
160 double actualError3 =
b.squaredNorm();
171 Point2 pixelError(0.2, 0.2);
185 double actualError1 = factor->error(
values);
208 Pose3 wTb1 =
cam1.pose() * sensor_T_body;
215 Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
231 smartFactor1.
add(measurements_cam1, views);
234 smartFactor2.
add(measurements_cam2, views);
237 smartFactor3.
add(measurements_cam3, views);
239 const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
255 double expectedError = 0.0;
278 Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
291 smartFactor1->add(measurements_cam1, views);
294 smartFactor2->add(measurements_cam2, views);
297 smartFactor3->add(measurements_cam3, views);
299 const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
325 Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598,
326 -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598),
359 smartFactor1->add(measurements_cam1, views);
362 cameras.push_back(
cam1);
363 cameras.push_back(
cam2);
366 CHECK(smartFactor1->triangulateSafe(cameras));
367 CHECK(!smartFactor1->isDegenerate());
368 CHECK(!smartFactor1->isPointBehindCamera());
369 auto p = smartFactor1->point();
383 double expectedError = 2500;
387 A1 << -10, 0, 0, 0, 1, 0;
388 A2 << 10, 0, 1, 0, -1, 0;
391 Matrix expectedInformation;
394 Matrix66 G11 = 0.5 *
A1.transpose() *
A1;
395 Matrix66 G12 = 0.5 *
A1.transpose() *
A2;
396 Matrix66 G22 = 0.5 *
A2.transpose() *
A2;
406 expectedInformation =
expected.information();
408 std::shared_ptr<RegularHessianFactor<6> > actual =
409 smartFactor1->createHessianFactor(cameras, 0.0);
453 std::shared_ptr<JacobianFactorQ<6, 2> > actualQ =
454 smartFactor1->createJacobianQFactor(cameras, 0.0);
463 Matrix3 whiteP = (
E.transpose() *
E).
inverse();
464 Fs[0] =
model->Whiten(Fs[0]);
465 Fs[1] =
model->Whiten(Fs[1]);
470 std::shared_ptr<RegularImplicitSchurFactor<Camera> > actual =
471 smartFactor1->createRegularImplicitSchurFactor(cameras, 0.0);
489 std::shared_ptr<JacobianFactor> actual =
490 smartFactor1->createJacobianSVDFactor(cameras, 0.0);
506 Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
514 smartFactor1->add(measurements_cam1, views);
517 smartFactor2->add(measurements_cam2, views);
520 smartFactor3->add(measurements_cam3, views);
522 const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
542 Rot3(1.11022302
e-16, -0.0314107591, 0.99950656, -0.99950656,
543 -0.0313952598, -0.000986635786, 0.0314107591, -0.999013364,
544 -0.0313952598),
Point3(0.1, -0.1, 1.9)),
560 Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
576 smartFactor1->add(measurements_cam1, views);
580 smartFactor2->add(measurements_cam2, views);
584 smartFactor3->add(measurements_cam3, views);
586 const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
614 double excludeLandmarksFutherThanDist = 2;
618 Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
634 smartFactor1->add(measurements_cam1, views);
638 smartFactor2->add(measurements_cam2, views);
642 smartFactor3->add(measurements_cam3, views);
644 const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
673 double excludeLandmarksFutherThanDist = 1e10;
674 double dynamicOutlierRejectionThreshold = 1;
679 Point3 landmark4(5, -0.5, 1);
681 Point2Vector measurements_cam1, measurements_cam2, measurements_cam3,
689 measurements_cam4.at(0) = measurements_cam4.at(0) +
Point2(10, 10);
698 smartFactor1->add(measurements_cam1, views);
702 smartFactor2->add(measurements_cam2, views);
706 smartFactor3->add(measurements_cam3, views);
710 smartFactor4->add(measurements_cam4, views);
712 const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
741 Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
753 smartFactor1->add(measurements_cam1, views);
757 smartFactor2->add(measurements_cam2, views);
761 smartFactor3->add(measurements_cam3, views);
763 const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
808 const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
841 *
Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05),
Point3(0, 0, 0));
846 Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
858 smartFactor1->add(measurements_cam1, views);
862 smartFactor2->add(measurements_cam2, views);
866 smartFactor3->add(measurements_cam3, views);
884 Rot3(0.00563056869, -0.130848107, 0.991386438, -0.991390265,
885 -0.130426831, -0.0115837907, 0.130819108, -0.98278564,
887 Point3(0.0897734171, -0.110201006, 0.901022872)),
890 std::shared_ptr<GaussianFactor>
factor1 = smartFactor1->linearize(
values);
891 std::shared_ptr<GaussianFactor>
factor2 = smartFactor2->linearize(
values);
892 std::shared_ptr<GaussianFactor>
factor3 = smartFactor3->linearize(
values);
897 std::shared_ptr<GaussianFactorGraph> GaussianGraph =
graph.
linearize(
899 Matrix GraphInformation = GaussianGraph->hessian().first;
904 Matrix AugInformationMatrix =
factor1->augmentedInformation()
905 +
factor2->augmentedInformation() +
factor3->augmentedInformation();
908 Vector InfoVector = AugInformationMatrix.block(0, 18, 18, 1);
938 smartFactor1->add(measurements_cam1, views);
942 smartFactor2->add(measurements_cam2, views);
944 const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
945 const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, 0.10);
978 *
Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05),
Point3(0, 0, 0));
983 Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
996 smartFactor1->add(measurements_cam1, views);
1000 smartFactor2->add(measurements_cam2, views);
1004 smartFactor3->add(measurements_cam3, views);
1006 const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
1007 const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3,
1029 Rot3(0.00563056869, -0.130848107, 0.991386438, -0.991390265,
1030 -0.130426831, -0.0115837907, 0.130819108, -0.98278564,
1032 Point3(0.0897734171, -0.110201006, 0.901022872)),
1041 #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
1062 measurements_cam1.push_back(cam1_uv1);
1063 measurements_cam1.push_back(cam2_uv1);
1066 smartFactor1->add(measurements_cam1, views);
1074 std::shared_ptr<GaussianFactor> factor = smartFactor1->linearize(
values);
1090 Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
1095 smartFactorInstance->add(measurements_cam1, views);
1102 std::shared_ptr<GaussianFactor> factor = smartFactorInstance->linearize(
1112 std::shared_ptr<GaussianFactor> factorRot = smartFactorInstance->linearize(
1126 std::shared_ptr<GaussianFactor> factorRotTran =
1127 smartFactorInstance->linearize(tranValues);
1148 smartFactor->add(measurements_cam1, views);
1155 std::shared_ptr<GaussianFactor> factor = smartFactor->linearize(
values);
1164 std::shared_ptr<GaussianFactor> factorRot =
1165 smartFactor->linearize(rotValues);
1178 std::shared_ptr<GaussianFactor> factorRotTran = smartFactor->linearize(
1200 Point3 landmark3(3, 0, 3.0);
1202 Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
1212 smartFactor1->add(measurements_cam1, views);
1215 smartFactor2->add(measurements_cam2, views);
1218 smartFactor3->add(measurements_cam3, views);
1220 const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
1240 Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598,
1241 -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598),
1259 *
Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05),
Point3(0, 0, 0));
1265 Point3 landmark3(3, 0, 3.0);
1267 Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
1280 smartFactor1->add(measurements_cam1, views);
1284 smartFactor2->add(measurements_cam2, views);
1288 smartFactor3->add(measurements_cam3, views);
1290 const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
1291 const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3,
1314 Rot3(0.00563056869, -0.130848107, 0.991386438, -0.991390265,
1315 -0.130426831, -0.0115837907, 0.130819108, -0.98278564,
1317 Point3(0.0897734171, -0.110201006, 0.901022872)),
1327 Rot3(0.00563056869, -0.130848107, 0.991386438, -0.991390265,
1328 -0.130426831, -0.0115837907, 0.130819108, -0.98278564,
1330 Point3(0.0897734171, -0.110201006, 0.901022872)),