31 #define DISABLE_TIMING
33 using namespace gtsam;
34 using namespace std::placeholders;
38 static const double sigma = 0.1;
88 std::shared_ptr<Cameras> cameraRig(
new Cameras());
97 std::shared_ptr<Cameras> cameraRig(
new Cameras());
106 std::shared_ptr<Cameras> cameraRig(
new Cameras());
123 std::vector<std::pair<Key, Key>> key_pairs;
124 key_pairs.push_back(std::make_pair(
x1,
x2));
125 key_pairs.push_back(std::make_pair(
x2,
x3));
126 key_pairs.push_back(std::make_pair(
x3,
x4));
128 std::vector<double> interp_factors;
135 std::shared_ptr<Cameras> cameraRig(
new Cameras());
190 std::shared_ptr<Cameras> cameraRig2(
new Cameras());
217 static const int ZDim = 2;
221 typedef std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD>>
233 std::shared_ptr<Cameras> cameraRig(
new Cameras());
246 double expectedError = 0.0;
264 EXPECT(actualE.rows() == 4);
265 EXPECT(actualE.cols() == 3);
266 EXPECT(actualb.rows() == 4);
267 EXPECT(actualb.cols() == 1);
268 EXPECT(actualFs.size() == 2);
273 Matrix expectedF11, expectedF12, expectedE1;
287 Matrix expectedF21, expectedF22, expectedE2;
289 pose_right, pose_above,
landmark1, expectedF21, expectedF22, expectedE2);
306 Point2 pixelError(0.5, 1.0);
311 std::shared_ptr<Cameras> cameraRig(
new Cameras());
336 EXPECT(actualE.rows() == 4);
337 EXPECT(actualE.cols() == 3);
338 EXPECT(actualb.rows() == 4);
339 EXPECT(actualb.cols() == 1);
340 EXPECT(actualFs.size() == 2);
345 Matrix expectedF11, expectedF12, expectedE1;
348 expectedF12, expectedE1);
361 Matrix expectedF21, expectedF22, expectedE2;
363 factor2.evaluateError(pose_right, pose_above, landmarkNoisy, expectedF21,
364 expectedF22, expectedE2);
387 Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3;
395 std::vector<std::pair<Key, Key>> key_pairs;
396 key_pairs.push_back(std::make_pair(
x1,
x2));
397 key_pairs.push_back(std::make_pair(
x2,
x3));
398 key_pairs.push_back(std::make_pair(
x3,
x1));
400 std::vector<double> interp_factors;
405 std::shared_ptr<Cameras> cameraRig(
new Cameras());
410 smartFactor1->add(measurements_lmk1, key_pairs, interp_factors);
414 smartFactor2->add(measurements_lmk2, key_pairs, interp_factors);
418 smartFactor3->add(measurements_lmk3, key_pairs, interp_factors);
447 -0.0313952598, -0.000986635786, 0.0314107591,
448 -0.999013364, -0.0313952598),
461 Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3;
469 std::vector<std::pair<Key, Key>> key_pairs;
470 key_pairs.push_back(std::make_pair(
x1,
x2));
471 key_pairs.push_back(std::make_pair(
x2,
x3));
472 key_pairs.push_back(std::make_pair(
x3,
x1));
474 std::vector<double> interp_factors;
479 std::shared_ptr<Cameras> cameraRig(
new Cameras());
485 smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, {1, 1, 1});
489 smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, {1, 1, 1});
493 smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, {1, 1, 1});
522 -0.0313952598, -0.000986635786, 0.0314107591,
523 -0.999013364, -0.0313952598),
537 Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3;
557 std::vector<std::pair<Key, Key>> key_pairs;
558 key_pairs.push_back(std::make_pair(
x1,
x2));
559 key_pairs.push_back(std::make_pair(
x2,
x3));
560 key_pairs.push_back(std::make_pair(
x3,
x1));
562 std::vector<double> interp_factors;
567 std::shared_ptr<Cameras> cameraRig(
new Cameras());
574 smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, {0, 1, 2});
578 smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, {0, 1, 2});
582 smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, {0, 1, 2});
611 -0.0313952598, -0.000986635786, 0.0314107591,
612 -0.999013364, -0.0313952598),
649 std::shared_ptr<Cameras> cameraRig(
new Cameras());
650 cameraRig->push_back(
Camera(body_P_sensorId, sharedKSimple));
660 cameras.push_back(
cam1);
661 cameras.push_back(
cam2);
664 EXPECT(smartFactor1->triangulateSafe(cameras));
665 EXPECT(!smartFactor1->isDegenerate());
666 EXPECT(!smartFactor1->isPointBehindCamera());
667 std::optional<Point3>
p = smartFactor1->point();
681 double expectedError = 2500;
686 A1 << -10, 0, 0, 0, 1, 0;
687 A2 << 10, 0, 1, 0, -1, 0;
690 Matrix expectedInformation;
693 Matrix66 G11 = 0.5 *
A1.transpose() *
A1;
694 Matrix66 G12 = 0.5 *
A1.transpose() *
A2;
695 Matrix66 G22 = 0.5 *
A2.transpose() *
A2;
705 expectedInformation =
expected.information();
710 std::shared_ptr<RegularHessianFactor<6>> actual =
711 smartFactor1->createHessianFactor(
values);
721 Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3;
729 std::vector<std::pair<Key, Key>> key_pairs;
730 key_pairs.push_back(std::make_pair(
x1,
x2));
731 key_pairs.push_back(std::make_pair(
x2,
x3));
732 key_pairs.push_back(std::make_pair(
x3,
x1));
734 std::vector<double> interp_factors;
739 double excludeLandmarksFutherThanDist = 1e10;
747 std::shared_ptr<Cameras> cameraRig(
new Cameras());
751 smartFactor1.
add(measurements_lmk1, key_pairs, interp_factors);
754 smartFactor2.
add(measurements_lmk2, key_pairs, interp_factors);
757 smartFactor3.
add(measurements_lmk3, key_pairs, interp_factors);
786 optimization_3poses_landmarkDistance) {
788 Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3;
796 std::vector<std::pair<Key, Key>> key_pairs;
797 key_pairs.push_back(std::make_pair(
x1,
x2));
798 key_pairs.push_back(std::make_pair(
x2,
x3));
799 key_pairs.push_back(std::make_pair(
x3,
x1));
801 std::vector<double> interp_factors;
806 double excludeLandmarksFutherThanDist = 2;
816 std::shared_ptr<Cameras> cameraRig(
new Cameras());
820 smartFactor1.
add(measurements_lmk1, key_pairs, interp_factors);
823 smartFactor2.
add(measurements_lmk2, key_pairs, interp_factors);
826 smartFactor3.
add(measurements_lmk3, key_pairs, interp_factors);
856 optimization_3poses_dynamicOutlierRejection) {
859 Point3 landmark4(5, -0.5, 1);
861 Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3,
868 measurements_lmk4.at(0) =
869 measurements_lmk4.at(0) +
Point2(10, 10);
872 std::vector<std::pair<Key, Key>> key_pairs;
873 key_pairs.push_back(std::make_pair(
x1,
x2));
874 key_pairs.push_back(std::make_pair(
x2,
x3));
875 key_pairs.push_back(std::make_pair(
x3,
x1));
877 std::vector<double> interp_factors;
882 double excludeLandmarksFutherThanDist = 1e10;
883 double dynamicOutlierRejectionThreshold =
894 std::shared_ptr<Cameras> cameraRig(
new Cameras());
899 smartFactor1->add(measurements_lmk1, key_pairs, interp_factors);
903 smartFactor2->add(measurements_lmk2, key_pairs, interp_factors);
907 smartFactor3->add(measurements_lmk3, key_pairs, interp_factors);
911 smartFactor4->add(measurements_lmk4, key_pairs, interp_factors);
943 hessianComparedToProjFactorsRollingShutter) {
951 std::vector<std::pair<Key, Key>> key_pairs;
952 key_pairs.push_back(std::make_pair(
x1,
x2));
953 key_pairs.push_back(std::make_pair(
x2,
x3));
954 key_pairs.push_back(std::make_pair(
x3,
x1));
956 std::vector<double> interp_factors;
961 std::shared_ptr<Cameras> cameraRig(
new Cameras());
966 smartFactor1->add(measurements_lmk1, key_pairs, interp_factors);
978 -0.0313952598, -0.000986635786, 0.0314107591,
979 -0.999013364, -0.0313952598),
990 std::shared_ptr<GaussianFactor> linearfactor1 =
991 smartFactor1->linearize(
values);
992 Matrix actualHessian = linearfactor1->information();
996 smartFactor1->triangulateSafe(smartFactor1->cameras(
values));
1001 Matrix F = Matrix::Zero(2 * 3, 6 * 3);
1002 Matrix E = Matrix::Zero(2 * 3, 3);
1008 Matrix H1Actual, H2Actual, H3Actual;
1012 H2Actual, H3Actual);
1013 F.block<2, 6>(0, 0) = H1Actual;
1014 F.block<2, 6>(0, 6) = H2Actual;
1015 E.block<2, 3>(0, 0) = H3Actual;
1020 H2Actual, H3Actual);
1021 F.block<2, 6>(2, 6) = H1Actual;
1022 F.block<2, 6>(2, 12) = H2Actual;
1023 E.block<2, 3>(2, 0) = H3Actual;
1028 H2Actual, H3Actual);
1029 F.block<2, 6>(4, 12) = H1Actual;
1030 F.block<2, 6>(4, 0) = H2Actual;
1031 E.block<2, 3>(4, 0) = H3Actual;
1040 F.transpose() *
F - (
F.transpose() *
E *
P *
E.transpose() *
F);
1045 gfg.
add(linearfactor1);
1056 Vector expectedInfoVector =
F.transpose() * (
b -
E *
P *
E.transpose() *
b);
1061 nfg_projFactorsRS.
add(factor11);
1062 nfg_projFactorsRS.
add(factor12);
1063 nfg_projFactorsRS.
add(factor13);
1066 double actualError = smartFactor1->error(
values);
1067 double expectedError = nfg_projFactorsRS.
error(
values);
1073 hessianComparedToProjFactorsRollingShutter_measurementsFromSamePose) {
1086 measurements_lmk1_redundant.push_back(
1087 measurements_lmk1.at(0));
1090 std::vector<std::pair<Key, Key>> key_pairs;
1091 key_pairs.push_back(std::make_pair(
x1,
x2));
1092 key_pairs.push_back(std::make_pair(
x2,
x3));
1093 key_pairs.push_back(std::make_pair(
x3,
x1));
1094 key_pairs.push_back(std::make_pair(
x1,
x2));
1096 std::vector<double> interp_factors;
1102 std::shared_ptr<Cameras> cameraRig(
new Cameras());
1107 smartFactor1->add(measurements_lmk1_redundant, key_pairs, interp_factors);
1119 -0.0313952598, -0.000986635786, 0.0314107591,
1120 -0.999013364, -0.0313952598),
1131 std::shared_ptr<GaussianFactor> linearfactor1 =
1132 smartFactor1->linearize(
values);
1133 Matrix actualHessian = linearfactor1->information();
1137 smartFactor1->triangulateSafe(smartFactor1->cameras(
values));
1143 Matrix F = Matrix::Zero(2 * 4, 6 * 3);
1144 Matrix E = Matrix::Zero(2 * 4, 3);
1151 Matrix H1Actual, H2Actual, H3Actual;
1155 H2Actual, H3Actual);
1156 F.block<2, 6>(0, 0) = H1Actual;
1157 F.block<2, 6>(0, 6) = H2Actual;
1158 E.block<2, 3>(0, 0) = H3Actual;
1164 H2Actual, H3Actual);
1165 F.block<2, 6>(2, 6) = H1Actual;
1166 F.block<2, 6>(2, 12) = H2Actual;
1167 E.block<2, 3>(2, 0) = H3Actual;
1173 H2Actual, H3Actual);
1174 F.block<2, 6>(4, 12) = H1Actual;
1175 F.block<2, 6>(4, 0) = H2Actual;
1176 E.block<2, 3>(4, 0) = H3Actual;
1182 H2Actual, H3Actual);
1183 F.block<2, 6>(6, 0) = H1Actual;
1184 F.block<2, 6>(6, 6) = H2Actual;
1185 E.block<2, 3>(6, 0) = H3Actual;
1194 F.transpose() *
F - (
F.transpose() *
E *
P *
E.transpose() *
F);
1199 gfg.
add(linearfactor1);
1210 Vector expectedInfoVector =
F.transpose() * (
b -
E *
P *
E.transpose() *
b);
1215 nfg_projFactorsRS.
add(factor11);
1216 nfg_projFactorsRS.
add(factor12);
1217 nfg_projFactorsRS.
add(factor13);
1218 nfg_projFactorsRS.
add(factor14);
1221 double actualError = smartFactor1->error(
values);
1222 double expectedError = nfg_projFactorsRS.
error(
values);
1228 optimization_3poses_measurementsFromSamePose) {
1230 Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3;
1238 std::vector<std::pair<Key, Key>> key_pairs;
1239 key_pairs.push_back(std::make_pair(
x1,
x2));
1240 key_pairs.push_back(std::make_pair(
x2,
x3));
1241 key_pairs.push_back(std::make_pair(
x3,
x1));
1243 std::vector<double> interp_factors;
1252 measurements_lmk1_redundant.push_back(
1253 measurements_lmk1.at(0));
1254 std::vector<std::pair<Key, Key>> key_pairs_redundant = key_pairs;
1255 key_pairs_redundant.push_back(
1257 std::vector<double> interp_factors_redundant = interp_factors;
1258 interp_factors_redundant.push_back(
1259 interp_factors.at(0));
1261 std::shared_ptr<Cameras> cameraRig(
new Cameras());
1266 smartFactor1->add(measurements_lmk1_redundant, key_pairs_redundant,
1267 interp_factors_redundant);
1271 smartFactor2->add(measurements_lmk2, key_pairs, interp_factors);
1275 smartFactor3->add(measurements_lmk3, key_pairs, interp_factors);
1288 groundTruth.
insert(
x2, pose_right);
1289 groundTruth.
insert(
x3, pose_above);
1304 -0.0313952598, -0.000986635786, 0.0314107591,
1305 -0.999013364, -0.0313952598),
1315 #ifndef DISABLE_TIMING
1347 size_t nrTests = 10000;
1349 for (
size_t i = 0;
i < nrTests;
i++) {
1350 std::shared_ptr<Cameras> cameraRig(
new Cameras());
1351 cameraRig->push_back(
Camera(body_P_sensorId, sharedKSimple));
1364 smartFactorRS->linearize(
values);
1368 for (
size_t i = 0;
i < nrTests;
i++) {
1371 smartFactor->add(measurements_lmk1[0],
x1);
1372 smartFactor->add(measurements_lmk1[1],
x2);
1378 smartFactor->linearize(
values);
1403 optimization_3poses_sphericalCameras) {
1405 std::vector<Unit3> measurements_lmk1, measurements_lmk2, measurements_lmk3;
1412 projectToMultipleCameras<Camera>(
cam1,
cam2,
cam3, landmark3,
1416 std::vector<std::pair<Key, Key>> key_pairs;
1417 key_pairs.push_back(std::make_pair(
x1,
x2));
1418 key_pairs.push_back(std::make_pair(
x2,
x3));
1419 key_pairs.push_back(std::make_pair(
x3,
x1));
1421 std::vector<double> interp_factors;
1431 std::shared_ptr<Cameras> cameraRig(
new Cameras());
1436 smartFactor1->add(measurements_lmk1, key_pairs, interp_factors);
1440 smartFactor2->add(measurements_lmk2, key_pairs, interp_factors);
1444 smartFactor3->add(measurements_lmk3, key_pairs, interp_factors);
1457 groundTruth.
insert(
x2, pose_right);
1458 groundTruth.
insert(
x3, pose_above);
1473 -0.0313952598, -0.000986635786, 0.0314107591,
1474 -0.999013364, -0.0313952598),