31 #define DISABLE_TIMING
33 using namespace std::placeholders;
37 static const double sigma = 0.1;
69 std::shared_ptr<Cameras> cameraRig(
new Cameras());
78 std::shared_ptr<Cameras> cameraRig(
new Cameras());
89 std::shared_ptr<Cameras> cameraRig(
new Cameras());
99 std::shared_ptr<Cameras> cameraRig(
new Cameras());
112 std::shared_ptr<Cameras> cameraRig(
new Cameras());
140 std::shared_ptr<Cameras> cameraRig(
new Cameras());
152 double expectedError = 0.0;
156 double actualError2 =
factor.totalReprojectionError(
cameras);
161 std::bind(&SmartRigFactor::whitenedError<Point3>,
factor,
cameras,
162 std::placeholders::_1);
188 double actualError3 =
b.squaredNorm();
197 std::shared_ptr<Cameras> cameraRig(
new Cameras());
201 Point2 pixelError(0.2, 0.2);
239 Pose3 body_T_sensor =
241 std::shared_ptr<Cameras> cameraRig(
new Cameras());
246 Pose3 wTb1 =
cam1.pose() * sensor_T_body;
253 Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
275 smartFactor2.
add(measurements_cam2, views);
278 smartFactor3.
add(measurements_cam3, views);
280 const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
296 double expectedError = 0.0;
320 Pose3 body_T_sensor1 =
322 Pose3 body_T_sensor2 =
326 std::shared_ptr<Cameras> cameraRig(
new Cameras());
335 Pose3 wTb1 =
cam1.pose() * sensor_T_body1;
342 Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
359 smartFactor1.
add(measurements_cam1, views, cameraIds);
362 smartFactor2.
add(measurements_cam2, views, cameraIds);
365 smartFactor3.
add(measurements_cam3, views, cameraIds);
367 const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
383 double expectedError = 0.0;
405 Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
407 std::shared_ptr<Cameras> cameraRig(
new Cameras());
425 smartFactor1->add(measurements_cam1, views, cameraIds);
429 smartFactor2->add(measurements_cam2, views, cameraIds);
433 smartFactor3->add(measurements_cam3, views, cameraIds);
435 const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
461 Pose3(
Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598,
462 -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598),
495 std::shared_ptr<Cameras> cameraRig(
new Cameras());
500 smartFactor1->add(measurements_cam1,
509 CHECK(!smartFactor1->isDegenerate());
510 CHECK(!smartFactor1->isPointBehindCamera());
511 auto p = smartFactor1->point();
525 double expectedError = 2500;
530 A1 << -10, 0, 0, 0, 1, 0;
531 A2 << 10, 0, 1, 0, -1, 0;
534 Matrix expectedInformation;
537 Matrix66 G11 = 0.5 *
A1.transpose() *
A1;
538 Matrix66 G12 = 0.5 *
A1.transpose() *
A2;
539 Matrix66 G22 = 0.5 *
A2.transpose() *
A2;
549 expectedInformation =
expected.information();
555 std::shared_ptr<RegularHessianFactor<6>> actual =
556 smartFactor1->createHessianFactor(
values, 0.0);
570 Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
578 std::shared_ptr<Cameras> cameraRig(
new Cameras());
583 smartFactor1->add(measurements_cam1, views, cameraIds);
587 smartFactor2->add(measurements_cam2, views, cameraIds);
591 smartFactor3->add(measurements_cam3, views, cameraIds);
593 const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
613 -0.99950656, -0.0313952598, -0.000986635786,
614 0.0314107591, -0.999013364, -0.0313952598),
628 double excludeLandmarksFutherThanDist = 2;
632 Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
646 std::shared_ptr<Cameras> cameraRig(
new Cameras());
652 smartFactor1->add(measurements_cam1, views, cameraIds);
656 smartFactor2->add(measurements_cam2, views, cameraIds);
660 smartFactor3->add(measurements_cam3, views, cameraIds);
662 const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
691 double excludeLandmarksFutherThanDist = 1e10;
692 double dynamicOutlierRejectionThreshold =
698 Point3 landmark4(5, -0.5, 1);
700 Point2Vector measurements_cam1, measurements_cam2, measurements_cam3,
708 measurements_cam4.at(0) =
709 measurements_cam4.at(0) +
Point2(10, 10);
717 std::shared_ptr<Cameras> cameraRig(
new Cameras());
723 smartFactor1->add(measurements_cam1, views, cameraIds);
727 smartFactor2->add(measurements_cam2, views, cameraIds);
731 smartFactor3->add(measurements_cam3, views, cameraIds);
735 smartFactor4->add(measurements_cam4, views, cameraIds);
737 const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
772 Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
783 std::shared_ptr<Cameras> cameraRig(
new Cameras());
789 smartFactor1->add(measurements_cam1, views, cameraIds);
793 smartFactor2->add(measurements_cam2, views, cameraIds);
797 smartFactor3->add(measurements_cam3, views, cameraIds);
815 -0.991390265, -0.130426831, -0.0115837907,
816 0.130819108, -0.98278564, -0.130455917),
817 Point3(0.0897734171, -0.110201006, 0.901022872)),
820 std::shared_ptr<GaussianFactor>
factor1 = smartFactor1->linearize(
values);
821 std::shared_ptr<GaussianFactor>
factor2 = smartFactor2->linearize(
values);
822 std::shared_ptr<GaussianFactor>
factor3 = smartFactor3->linearize(
values);
824 Matrix CumulativeInformation =
827 std::shared_ptr<GaussianFactorGraph> GaussianGraph =
829 Matrix GraphInformation = GaussianGraph->hessian().first;
834 Matrix AugInformationMatrix =
factor1->augmentedInformation() +
835 factor2->augmentedInformation() +
836 factor3->augmentedInformation();
839 Vector InfoVector = AugInformationMatrix.block(
856 measurements_cam1.push_back(cam1_uv1);
857 measurements_cam1.push_back(cam2_uv1);
859 std::shared_ptr<Cameras> cameraRig(
new Cameras());
869 smartFactor1->add(measurements_cam1, views, cameraIds);
877 std::shared_ptr<GaussianFactor>
factor = smartFactor1->linearize(
values);
889 std::shared_ptr<Cameras> cameraRig(
new Cameras());
906 Point3 landmark3(3, 0, 3.0);
908 Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
917 std::shared_ptr<Cameras> cameraRig(
new Cameras());
923 smartFactor1->add(measurements_cam1, views, cameraIds);
927 smartFactor2->add(measurements_cam2, views, cameraIds);
931 smartFactor3->add(measurements_cam3, views, cameraIds);
933 const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
953 Pose3(
Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598,
954 -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598),
969 hessianComparedToProjFactors_measurementsFromSamePose) {
982 measurements_lmk1_redundant.push_back(
983 measurements_lmk1.at(0));
988 std::shared_ptr<Cameras> cameraRig(
new Cameras());
994 smartFactor1->add(measurements_lmk1_redundant,
keys, cameraIds);
1006 -0.0313952598, -0.000986635786, 0.0314107591,
1007 -0.999013364, -0.0313952598),
1018 std::shared_ptr<GaussianFactor> linearfactor1 =
1019 smartFactor1->linearize(
values);
1020 Matrix actualHessian = linearfactor1->information();
1024 smartFactor1->triangulateSafe(smartFactor1->cameras(
values));
1029 Matrix F = Matrix::Zero(2 * 4, 6 * 3);
1030 Matrix E = Matrix::Zero(2 * 4, 3);
1031 Vector b = Vector::Zero(2 * 4);
1036 Matrix HPoseActual, HEActual;
1041 F.block<2, 6>(0, 0) = HPoseActual;
1042 E.block<2, 3>(0, 0) = HEActual;
1048 F.block<2, 6>(2, 6) = HPoseActual;
1049 E.block<2, 3>(2, 0) = HEActual;
1055 F.block<2, 6>(4, 12) = HPoseActual;
1056 E.block<2, 3>(4, 0) = HEActual;
1062 F.block<2, 6>(6, 0) = HPoseActual;
1063 E.block<2, 3>(6, 0) = HEActual;
1072 F.transpose() *
F - (
F.transpose() *
E *
P *
E.transpose() *
F);
1077 gfg.
add(linearfactor1);
1088 Vector expectedInfoVector =
F.transpose() * (
b -
E *
P *
E.transpose() *
b);
1093 nfg_projFactors.
add(factor11);
1094 nfg_projFactors.
add(factor12);
1095 nfg_projFactors.
add(factor13);
1096 nfg_projFactors.
add(factor14);
1099 double actualError = smartFactor1->error(
values);
1100 double expectedError = nfg_projFactors.
error(
values);
1107 Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3;
1116 std::shared_ptr<Cameras> cameraRig(
new Cameras());
1125 measurements_lmk1_redundant.push_back(
1126 measurements_lmk1.at(0));
1128 keys_redundant.push_back(
keys.at(0));
1132 smartFactor1->add(measurements_lmk1_redundant, keys_redundant,
1133 cameraIdsRedundant);
1137 smartFactor2->add(measurements_lmk2,
keys, cameraIds);
1141 smartFactor3->add(measurements_lmk3,
keys, cameraIds);
1143 const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
1154 groundTruth.
insert(
x2, pose_right);
1155 groundTruth.
insert(
x3, pose_above);
1170 -0.0313952598, -0.000986635786, 0.0314107591,
1171 -0.999013364, -0.0313952598),
1181 #ifndef DISABLE_TIMING
1196 Rot3 R = Rot3::Identity();
1202 std::shared_ptr<Cameras> cameraRig(
new Cameras());
1203 cameraRig->push_back(
Camera(body_P_sensorId, sharedKSimple));
1214 size_t nrTests = 10000;
1216 for (
size_t i = 0;
i < nrTests;
i++) {
1219 smartRigFactor->add(measurements_lmk1[0],
x1,
cameraId1);
1220 smartRigFactor->add(measurements_lmk1[1],
x1,
cameraId1);
1225 gttic_(SmartRigFactor_LINEARIZE);
1226 smartRigFactor->linearize(
values);
1227 gttoc_(SmartRigFactor_LINEARIZE);
1230 for (
size_t i = 0;
i < nrTests;
i++) {
1233 smartFactor->add(measurements_lmk1[0],
x1);
1234 smartFactor->add(measurements_lmk1[1],
x2);
1239 gttic_(SmartPoseFactor_LINEARIZE);
1240 smartFactor->linearize(
values);
1241 gttoc_(SmartPoseFactor_LINEARIZE);
1248 TEST(SmartProjectionFactorP, optimization_3poses_sphericalCamera) {
1258 projectToMultipleCameras<Camera>(
cam1,
cam2,
cam3, landmark3,
1267 std::shared_ptr<Cameras> cameraRig(
new Cameras());
1277 smartFactor1->add(measurements_lmk1,
keys);
1281 smartFactor2->add(measurements_lmk2,
keys);
1285 smartFactor3->add(measurements_lmk3,
keys);
1287 const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
1298 groundTruth.
insert(
x2, pose_right);
1299 groundTruth.
insert(
x3, pose_above);
1321 #ifndef DISABLE_TIMING
1329 TEST(SmartProjectionFactorP, timing_sphericalCamera) {
1331 Rot3 R = Rot3::Identity();
1341 std::vector<Unit3> measurements_lmk1_sphere;
1342 measurements_lmk1_sphere.push_back(cam1_sphere.project(
landmark1));
1343 measurements_lmk1_sphere.push_back(cam2_sphere.project(
landmark1));
1349 std::vector<Point2> measurements_lmk1;
1357 size_t nrTests = 1000;
1359 for (
size_t i = 0;
i < nrTests;
i++) {
1360 std::shared_ptr<CameraSet<SphericalCamera>> cameraRig(
1367 smartFactorP->add(measurements_lmk1_sphere[0],
x1);
1368 smartFactorP->add(measurements_lmk1_sphere[1],
x1);
1373 gttic_(SmartFactorP_spherical_LINEARIZE);
1374 smartFactorP->linearize(
values);
1375 gttoc_(SmartFactorP_spherical_LINEARIZE);
1378 for (
size_t i = 0;
i < nrTests;
i++) {
1379 std::shared_ptr<CameraSet<PinholePose<Cal3_S2>>> cameraRig(
1386 smartFactorP2->add(measurements_lmk1[0],
x1);
1387 smartFactorP2->add(measurements_lmk1[1],
x1);
1392 gttic_(SmartFactorP_pinhole_LINEARIZE);
1393 smartFactorP2->linearize(
values);
1394 gttoc_(SmartFactorP_pinhole_LINEARIZE);
1401 TEST(SmartProjectionFactorP, 2poses_rankTol) {
1403 Rot3::Ypr(-
M_PI / 2, 0., -
M_PI / 2),
1422 std::shared_ptr<CameraSet<PinholePose<Cal3_S2>>> cameraRig(
1428 smartFactor1->add(
cam1.project(landmarkL),
x1);
1450 new Cal3_S2(1.0, 1.0, 0.0, 0.0, 0.0));
1460 std::shared_ptr<CameraSet<PinholePose<Cal3_S2>>> cameraRig(
1466 smartFactor1->add(
cam1.project(landmarkL),
x1);
1487 new Cal3_S2(1.0, 1.0, 0.0, 0.0, 0.0));
1497 std::shared_ptr<CameraSet<PinholePose<Cal3_S2>>> cameraRig(
1503 smartFactor1->add(
cam1.project(landmarkL),
x1);
1522 TEST(SmartProjectionFactorP, 2poses_sphericalCamera_rankTol) {
1527 Rot3::Ypr(-
M_PI / 2, 0., -
M_PI / 2),
1536 std::shared_ptr<CameraSet<SphericalCamera>> cameraRig(
1550 smartFactor1->add(
cam1.project(landmarkL),
x1);
1578 smartFactor1->add(
cam1.project(landmarkL),
x1);