12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

all tests are passing!

release/4.3a0
lcarlone 2021-10-05 22:32:02 -04:00
parent 4fd6c2cb5d
commit 0797eae9a8
1 changed files with 419 additions and 423 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

842
gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp
View File

@ -724,429 +724,425 @@ TEST(SmartProjectionPoseFactorRollingShutter,
EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-6)); EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-6));
} }
///* *************************************************************************/ /* *************************************************************************/
//TEST(SmartProjectionPoseFactorRollingShutter, TEST(SmartProjectionPoseFactorRollingShutter,
// hessianComparedToProjFactorsRollingShutter) { hessianComparedToProjFactorsRollingShutter) {
// using namespace vanillaPoseRS; using namespace vanillaPoseRS;
// Point2Vector measurements_lmk1; Point2Vector measurements_lmk1;
//
// // Project three landmarks into three cameras // Project three landmarks into three cameras
// projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1);
//
// // create inputs // create inputs
// std::vector<std::pair<Key, Key>> key_pairs; std::vector<std::pair<Key, Key>> key_pairs;
// key_pairs.push_back(std::make_pair(x1, x2)); key_pairs.push_back(std::make_pair(x1, x2));
// key_pairs.push_back(std::make_pair(x2, x3)); key_pairs.push_back(std::make_pair(x2, x3));
// key_pairs.push_back(std::make_pair(x3, x1)); key_pairs.push_back(std::make_pair(x3, x1));
//
// std::vector<double> interp_factors; std::vector<double> interp_factors;
// interp_factors.push_back(interp_factor1); interp_factors.push_back(interp_factor1);
// interp_factors.push_back(interp_factor2); interp_factors.push_back(interp_factor2);
// interp_factors.push_back(interp_factor3); interp_factors.push_back(interp_factor3);
//
// SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model, Camera(Pose3::identity(),sharedK)));
// smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, sharedK); smartFactor1->add(measurements_lmk1, key_pairs, interp_factors);
//
// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
// Point3(0.1, 0.1, 0.1)); // smaller noise Point3(0.1, 0.1, 0.1)); // smaller noise
// Values values; Values values;
// values.insert(x1, level_pose); values.insert(x1, level_pose);
// values.insert(x2, pose_right); values.insert(x2, pose_right);
// // initialize third pose with some noise to get a nontrivial linearization // initialize third pose with some noise to get a nontrivial linearization
// // point // point
// values.insert(x3, pose_above * noise_pose); values.insert(x3, pose_above * noise_pose);
// EXPECT( // check that the pose is actually noisy EXPECT( // check that the pose is actually noisy
// assert_equal(Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656, assert_equal(Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656,
// -0.0313952598, -0.000986635786, 0.0314107591, -0.0313952598, -0.000986635786, 0.0314107591,
// -0.999013364, -0.0313952598), -0.999013364, -0.0313952598),
// Point3(0.1, -0.1, 1.9)), Point3(0.1, -0.1, 1.9)),
// values.at<Pose3>(x3))); values.at<Pose3>(x3)));
//
// // linearization point for the poses // linearization point for the poses
// Pose3 pose1 = level_pose; Pose3 pose1 = level_pose;
// Pose3 pose2 = pose_right; Pose3 pose2 = pose_right;
// Pose3 pose3 = pose_above * noise_pose; Pose3 pose3 = pose_above * noise_pose;
//
// // ==== check Hessian of smartFactor1 ===== // ==== check Hessian of smartFactor1 =====
// // -- compute actual Hessian // -- compute actual Hessian
// boost::shared_ptr<GaussianFactor> linearfactor1 = boost::shared_ptr<GaussianFactor> linearfactor1 =
// smartFactor1->linearize(values); smartFactor1->linearize(values);
// Matrix actualHessian = linearfactor1->information(); Matrix actualHessian = linearfactor1->information();
//
// // -- compute expected Hessian from manual Schur complement from Jacobians // -- compute expected Hessian from manual Schur complement from Jacobians
// // linearization point for the 3D point // linearization point for the 3D point
// smartFactor1->triangulateSafe(smartFactor1->cameras(values)); smartFactor1->triangulateSafe(smartFactor1->cameras(values));
// TriangulationResult point = smartFactor1->point(); TriangulationResult point = smartFactor1->point();
// EXPECT(point.valid()); // check triangulated point is valid EXPECT(point.valid()); // check triangulated point is valid
//
// // Use the factor to calculate the Jacobians // Use the factor to calculate the Jacobians
// Matrix F = Matrix::Zero(2 * 3, 6 * 3); Matrix F = Matrix::Zero(2 * 3, 6 * 3);
// Matrix E = Matrix::Zero(2 * 3, 3); Matrix E = Matrix::Zero(2 * 3, 3);
// Vector b = Vector::Zero(6); Vector b = Vector::Zero(6);
//
// // create projection factors rolling shutter // create projection factors rolling shutter
// ProjectionFactorRollingShutter factor11(measurements_lmk1[0], interp_factor1, ProjectionFactorRollingShutter factor11(measurements_lmk1[0], interp_factor1,
// model, x1, x2, l0, sharedK); model, x1, x2, l0, sharedK);
// Matrix H1Actual, H2Actual, H3Actual; Matrix H1Actual, H2Actual, H3Actual;
// // note: b is minus the reprojection error, cf the smart factor jacobian // note: b is minus the reprojection error, cf the smart factor jacobian
// // computation // computation
// b.segment<2>(0) = -factor11.evaluateError(pose1, pose2, *point, H1Actual, b.segment<2>(0) = -factor11.evaluateError(pose1, pose2, *point, H1Actual,
// H2Actual, H3Actual); H2Actual, H3Actual);
// F.block<2, 6>(0, 0) = H1Actual; F.block<2, 6>(0, 0) = H1Actual;
// F.block<2, 6>(0, 6) = H2Actual; F.block<2, 6>(0, 6) = H2Actual;
// E.block<2, 3>(0, 0) = H3Actual; E.block<2, 3>(0, 0) = H3Actual;
//
// ProjectionFactorRollingShutter factor12(measurements_lmk1[1], interp_factor2, ProjectionFactorRollingShutter factor12(measurements_lmk1[1], interp_factor2,
// model, x2, x3, l0, sharedK); model, x2, x3, l0, sharedK);
// b.segment<2>(2) = -factor12.evaluateError(pose2, pose3, *point, H1Actual, b.segment<2>(2) = -factor12.evaluateError(pose2, pose3, *point, H1Actual,
// H2Actual, H3Actual); H2Actual, H3Actual);
// F.block<2, 6>(2, 6) = H1Actual; F.block<2, 6>(2, 6) = H1Actual;
// F.block<2, 6>(2, 12) = H2Actual; F.block<2, 6>(2, 12) = H2Actual;
// E.block<2, 3>(2, 0) = H3Actual; E.block<2, 3>(2, 0) = H3Actual;
//
// ProjectionFactorRollingShutter factor13(measurements_lmk1[2], interp_factor3, ProjectionFactorRollingShutter factor13(measurements_lmk1[2], interp_factor3,
// model, x3, x1, l0, sharedK); model, x3, x1, l0, sharedK);
// b.segment<2>(4) = -factor13.evaluateError(pose3, pose1, *point, H1Actual, b.segment<2>(4) = -factor13.evaluateError(pose3, pose1, *point, H1Actual,
// H2Actual, H3Actual); H2Actual, H3Actual);
// F.block<2, 6>(4, 12) = H1Actual; F.block<2, 6>(4, 12) = H1Actual;
// F.block<2, 6>(4, 0) = H2Actual; F.block<2, 6>(4, 0) = H2Actual;
// E.block<2, 3>(4, 0) = H3Actual; E.block<2, 3>(4, 0) = H3Actual;
//
// // whiten // whiten
// F = (1 / sigma) * F; F = (1 / sigma) * F;
// E = (1 / sigma) * E; E = (1 / sigma) * E;
// b = (1 / sigma) * b; b = (1 / sigma) * b;
// //* G = F' * F - F' * E * P * E' * F //* G = F' * F - F' * E * P * E' * F
// Matrix P = (E.transpose() * E).inverse(); Matrix P = (E.transpose() * E).inverse();
// Matrix expectedHessian = Matrix expectedHessian =
// F.transpose() * F - (F.transpose() * E * P * E.transpose() * F); F.transpose() * F - (F.transpose() * E * P * E.transpose() * F);
// EXPECT(assert_equal(expectedHessian, actualHessian, 1e-6)); EXPECT(assert_equal(expectedHessian, actualHessian, 1e-6));
//
// // ==== check Information vector of smartFactor1 ===== // ==== check Information vector of smartFactor1 =====
// GaussianFactorGraph gfg; GaussianFactorGraph gfg;
// gfg.add(linearfactor1); gfg.add(linearfactor1);
// Matrix actualHessian_v2 = gfg.hessian().first; Matrix actualHessian_v2 = gfg.hessian().first;
// EXPECT(assert_equal(actualHessian_v2, actualHessian, EXPECT(assert_equal(actualHessian_v2, actualHessian,
// 1e-6)); // sanity check on hessian 1e-6)); // sanity check on hessian
//
// // -- compute actual information vector // -- compute actual information vector
// Vector actualInfoVector = gfg.hessian().second; Vector actualInfoVector = gfg.hessian().second;
//
// // -- compute expected information vector from manual Schur complement from // -- compute expected information vector from manual Schur complement from
// // Jacobians // Jacobians
// //* g = F' * (b - E * P * E' * b) //* g = F' * (b - E * P * E' * b)
// Vector expectedInfoVector = F.transpose() * (b - E * P * E.transpose() * b); Vector expectedInfoVector = F.transpose() * (b - E * P * E.transpose() * b);
// EXPECT(assert_equal(expectedInfoVector, actualInfoVector, 1e-6)); EXPECT(assert_equal(expectedInfoVector, actualInfoVector, 1e-6));
//
// // ==== check error of smartFactor1 (again) ===== // ==== check error of smartFactor1 (again) =====
// NonlinearFactorGraph nfg_projFactorsRS; NonlinearFactorGraph nfg_projFactorsRS;
// nfg_projFactorsRS.add(factor11); nfg_projFactorsRS.add(factor11);
// nfg_projFactorsRS.add(factor12); nfg_projFactorsRS.add(factor12);
// nfg_projFactorsRS.add(factor13); nfg_projFactorsRS.add(factor13);
// values.insert(l0, *point); values.insert(l0, *point);
//
// double actualError = smartFactor1->error(values); double actualError = smartFactor1->error(values);
// double expectedError = nfg_projFactorsRS.error(values); double expectedError = nfg_projFactorsRS.error(values);
// EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7);
//} }
//
///* *************************************************************************/ /* *************************************************************************/
//TEST(SmartProjectionPoseFactorRollingShutter, TEST(SmartProjectionPoseFactorRollingShutter,
// hessianComparedToProjFactorsRollingShutter_measurementsFromSamePose) { hessianComparedToProjFactorsRollingShutter_measurementsFromSamePose) {
// // in this test we make sure the fact works even if we have multiple pixel // in this test we make sure the fact works even if we have multiple pixel
// // measurements of the same landmark at a single pose, a setup that occurs in // measurements of the same landmark at a single pose, a setup that occurs in
// // multi-camera systems // multi-camera systems
//
// using namespace vanillaPoseRS; using namespace vanillaPoseRS;
// Point2Vector measurements_lmk1; Point2Vector measurements_lmk1;
//
// // Project three landmarks into three cameras // Project three landmarks into three cameras
// projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1);
//
// // create redundant measurements: // create redundant measurements:
// Camera::MeasurementVector measurements_lmk1_redundant = measurements_lmk1; Camera::MeasurementVector measurements_lmk1_redundant = measurements_lmk1;
// measurements_lmk1_redundant.push_back( measurements_lmk1_redundant.push_back(
// measurements_lmk1.at(0)); // we readd the first measurement measurements_lmk1.at(0)); // we readd the first measurement
//
// // create inputs // create inputs
// std::vector<std::pair<Key, Key>> key_pairs; std::vector<std::pair<Key, Key>> key_pairs;
// key_pairs.push_back(std::make_pair(x1, x2)); key_pairs.push_back(std::make_pair(x1, x2));
// key_pairs.push_back(std::make_pair(x2, x3)); key_pairs.push_back(std::make_pair(x2, x3));
// key_pairs.push_back(std::make_pair(x3, x1)); key_pairs.push_back(std::make_pair(x3, x1));
// key_pairs.push_back(std::make_pair(x1, x2)); key_pairs.push_back(std::make_pair(x1, x2));
//
// std::vector<double> interp_factors; std::vector<double> interp_factors;
// interp_factors.push_back(interp_factor1); interp_factors.push_back(interp_factor1);
// interp_factors.push_back(interp_factor2); interp_factors.push_back(interp_factor2);
// interp_factors.push_back(interp_factor3); interp_factors.push_back(interp_factor3);
// interp_factors.push_back(interp_factor1); interp_factors.push_back(interp_factor1);
//
// SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model, Camera(Pose3::identity(),sharedK)));
// smartFactor1->add(measurements_lmk1_redundant, key_pairs, interp_factors, smartFactor1->add(measurements_lmk1_redundant, key_pairs, interp_factors);
// sharedK);
// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise
// Point3(0.1, 0.1, 0.1)); // smaller noise Values values;
// Values values; values.insert(x1, level_pose);
// values.insert(x1, level_pose); values.insert(x2, pose_right);
// values.insert(x2, pose_right); // initialize third pose with some noise to get a nontrivial linearization
// // initialize third pose with some noise to get a nontrivial linearization // point
// // point values.insert(x3, pose_above * noise_pose);
// values.insert(x3, pose_above * noise_pose); EXPECT( // check that the pose is actually noisy
// EXPECT( // check that the pose is actually noisy assert_equal(Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656,
// assert_equal(Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, -0.000986635786, 0.0314107591,
// -0.0313952598, -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598),
// -0.999013364, -0.0313952598), Point3(0.1, -0.1, 1.9)),
// Point3(0.1, -0.1, 1.9)), values.at<Pose3>(x3)));
// values.at<Pose3>(x3)));
// // linearization point for the poses
// // linearization point for the poses Pose3 pose1 = level_pose;
// Pose3 pose1 = level_pose; Pose3 pose2 = pose_right;
// Pose3 pose2 = pose_right; Pose3 pose3 = pose_above * noise_pose;
// Pose3 pose3 = pose_above * noise_pose;
// // ==== check Hessian of smartFactor1 =====
// // ==== check Hessian of smartFactor1 ===== // -- compute actual Hessian
// // -- compute actual Hessian boost::shared_ptr<GaussianFactor> linearfactor1 =
// boost::shared_ptr<GaussianFactor> linearfactor1 = smartFactor1->linearize(values);
// smartFactor1->linearize(values); Matrix actualHessian = linearfactor1->information();
// Matrix actualHessian = linearfactor1->information();
// // -- compute expected Hessian from manual Schur complement from Jacobians
// // -- compute expected Hessian from manual Schur complement from Jacobians // linearization point for the 3D point
// // linearization point for the 3D point smartFactor1->triangulateSafe(smartFactor1->cameras(values));
// smartFactor1->triangulateSafe(smartFactor1->cameras(values)); TriangulationResult point = smartFactor1->point();
// TriangulationResult point = smartFactor1->point(); EXPECT(point.valid()); // check triangulated point is valid
// EXPECT(point.valid()); // check triangulated point is valid
// // Use standard ProjectionFactorRollingShutter factor to calculate the
// // Use standard ProjectionFactorRollingShutter factor to calculate the // Jacobians
// // Jacobians Matrix F = Matrix::Zero(2 * 4, 6 * 3);
// Matrix F = Matrix::Zero(2 * 4, 6 * 3); Matrix E = Matrix::Zero(2 * 4, 3);
// Matrix E = Matrix::Zero(2 * 4, 3); Vector b = Vector::Zero(8);
// Vector b = Vector::Zero(8);
// // create projection factors rolling shutter
// // create projection factors rolling shutter ProjectionFactorRollingShutter factor11(measurements_lmk1_redundant[0],
// ProjectionFactorRollingShutter factor11(measurements_lmk1_redundant[0], interp_factor1, model, x1, x2, l0,
// interp_factor1, model, x1, x2, l0, sharedK);
// sharedK); Matrix H1Actual, H2Actual, H3Actual;
// Matrix H1Actual, H2Actual, H3Actual; // note: b is minus the reprojection error, cf the smart factor jacobian
// // note: b is minus the reprojection error, cf the smart factor jacobian // computation
// // computation b.segment<2>(0) = -factor11.evaluateError(pose1, pose2, *point, H1Actual,
// b.segment<2>(0) = -factor11.evaluateError(pose1, pose2, *point, H1Actual, H2Actual, H3Actual);
// H2Actual, H3Actual); F.block<2, 6>(0, 0) = H1Actual;
// F.block<2, 6>(0, 0) = H1Actual; F.block<2, 6>(0, 6) = H2Actual;
// F.block<2, 6>(0, 6) = H2Actual; E.block<2, 3>(0, 0) = H3Actual;
// E.block<2, 3>(0, 0) = H3Actual;
// ProjectionFactorRollingShutter factor12(measurements_lmk1_redundant[1],
// ProjectionFactorRollingShutter factor12(measurements_lmk1_redundant[1], interp_factor2, model, x2, x3, l0,
// interp_factor2, model, x2, x3, l0, sharedK);
// sharedK); b.segment<2>(2) = -factor12.evaluateError(pose2, pose3, *point, H1Actual,
// b.segment<2>(2) = -factor12.evaluateError(pose2, pose3, *point, H1Actual, H2Actual, H3Actual);
// H2Actual, H3Actual); F.block<2, 6>(2, 6) = H1Actual;
// F.block<2, 6>(2, 6) = H1Actual; F.block<2, 6>(2, 12) = H2Actual;
// F.block<2, 6>(2, 12) = H2Actual; E.block<2, 3>(2, 0) = H3Actual;
// E.block<2, 3>(2, 0) = H3Actual;
// ProjectionFactorRollingShutter factor13(measurements_lmk1_redundant[2],
// ProjectionFactorRollingShutter factor13(measurements_lmk1_redundant[2], interp_factor3, model, x3, x1, l0,
// interp_factor3, model, x3, x1, l0, sharedK);
// sharedK); b.segment<2>(4) = -factor13.evaluateError(pose3, pose1, *point, H1Actual,
// b.segment<2>(4) = -factor13.evaluateError(pose3, pose1, *point, H1Actual, H2Actual, H3Actual);
// H2Actual, H3Actual); F.block<2, 6>(4, 12) = H1Actual;
// F.block<2, 6>(4, 12) = H1Actual; F.block<2, 6>(4, 0) = H2Actual;
// F.block<2, 6>(4, 0) = H2Actual; E.block<2, 3>(4, 0) = H3Actual;
// E.block<2, 3>(4, 0) = H3Actual;
// ProjectionFactorRollingShutter factor14(measurements_lmk1_redundant[3],
// ProjectionFactorRollingShutter factor14(measurements_lmk1_redundant[3], interp_factor1, model, x1, x2, l0,
// interp_factor1, model, x1, x2, l0, sharedK);
// sharedK); b.segment<2>(6) = -factor11.evaluateError(pose1, pose2, *point, H1Actual,
// b.segment<2>(6) = -factor11.evaluateError(pose1, pose2, *point, H1Actual, H2Actual, H3Actual);
// H2Actual, H3Actual); F.block<2, 6>(6, 0) = H1Actual;
// F.block<2, 6>(6, 0) = H1Actual; F.block<2, 6>(6, 6) = H2Actual;
// F.block<2, 6>(6, 6) = H2Actual; E.block<2, 3>(6, 0) = H3Actual;
// E.block<2, 3>(6, 0) = H3Actual;
// // whiten
// // whiten F = (1 / sigma) * F;
// F = (1 / sigma) * F; E = (1 / sigma) * E;
// E = (1 / sigma) * E; b = (1 / sigma) * b;
// b = (1 / sigma) * b; //* G = F' * F - F' * E * P * E' * F
// //* G = F' * F - F' * E * P * E' * F Matrix P = (E.transpose() * E).inverse();
// Matrix P = (E.transpose() * E).inverse(); Matrix expectedHessian =
// Matrix expectedHessian = F.transpose() * F - (F.transpose() * E * P * E.transpose() * F);
// F.transpose() * F - (F.transpose() * E * P * E.transpose() * F); EXPECT(assert_equal(expectedHessian, actualHessian, 1e-6));
// EXPECT(assert_equal(expectedHessian, actualHessian, 1e-6));
// // ==== check Information vector of smartFactor1 =====
// // ==== check Information vector of smartFactor1 ===== GaussianFactorGraph gfg;
// GaussianFactorGraph gfg; gfg.add(linearfactor1);
// gfg.add(linearfactor1); Matrix actualHessian_v2 = gfg.hessian().first;
// Matrix actualHessian_v2 = gfg.hessian().first; EXPECT(assert_equal(actualHessian_v2, actualHessian,
// EXPECT(assert_equal(actualHessian_v2, actualHessian, 1e-6)); // sanity check on hessian
// 1e-6)); // sanity check on hessian
// // -- compute actual information vector
// // -- compute actual information vector Vector actualInfoVector = gfg.hessian().second;
// Vector actualInfoVector = gfg.hessian().second;
// // -- compute expected information vector from manual Schur complement from
// // -- compute expected information vector from manual Schur complement from // Jacobians
// // Jacobians //* g = F' * (b - E * P * E' * b)
// //* g = F' * (b - E * P * E' * b) Vector expectedInfoVector = F.transpose() * (b - E * P * E.transpose() * b);
// Vector expectedInfoVector = F.transpose() * (b - E * P * E.transpose() * b); EXPECT(assert_equal(expectedInfoVector, actualInfoVector, 1e-6));
// EXPECT(assert_equal(expectedInfoVector, actualInfoVector, 1e-6));
// // ==== check error of smartFactor1 (again) =====
// // ==== check error of smartFactor1 (again) ===== NonlinearFactorGraph nfg_projFactorsRS;
// NonlinearFactorGraph nfg_projFactorsRS; nfg_projFactorsRS.add(factor11);
// nfg_projFactorsRS.add(factor11); nfg_projFactorsRS.add(factor12);
// nfg_projFactorsRS.add(factor12); nfg_projFactorsRS.add(factor13);
// nfg_projFactorsRS.add(factor13); nfg_projFactorsRS.add(factor14);
// nfg_projFactorsRS.add(factor14); values.insert(l0, *point);
// values.insert(l0, *point);
// double actualError = smartFactor1->error(values);
// double actualError = smartFactor1->error(values); double expectedError = nfg_projFactorsRS.error(values);
// double expectedError = nfg_projFactorsRS.error(values); EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7);
// EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); }
//}
// /* *************************************************************************/
///* *************************************************************************/ TEST(SmartProjectionPoseFactorRollingShutter,
//TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_measurementsFromSamePose) {
// optimization_3poses_measurementsFromSamePose) { using namespace vanillaPoseRS;
// using namespace vanillaPoseRS; Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3;
// Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3;
// // Project three landmarks into three cameras
// // Project three landmarks into three cameras projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1);
// projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2);
// projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3);
// projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3);
// // create inputs
// // create inputs std::vector<std::pair<Key, Key>> key_pairs;
// std::vector<std::pair<Key, Key>> key_pairs; key_pairs.push_back(std::make_pair(x1, x2));
// key_pairs.push_back(std::make_pair(x1, x2)); key_pairs.push_back(std::make_pair(x2, x3));
// key_pairs.push_back(std::make_pair(x2, x3)); key_pairs.push_back(std::make_pair(x3, x1));
// key_pairs.push_back(std::make_pair(x3, x1));
// std::vector<double> interp_factors;
// std::vector<double> interp_factors; interp_factors.push_back(interp_factor1);
// interp_factors.push_back(interp_factor1); interp_factors.push_back(interp_factor2);
// interp_factors.push_back(interp_factor2); interp_factors.push_back(interp_factor3);
// interp_factors.push_back(interp_factor3);
// // For first factor, we create redundant measurement (taken by the same keys
// // For first factor, we create redundant measurement (taken by the same keys // as factor 1, to make sure the redundancy in the keys does not create
// // as factor 1, to make sure the redundancy in the keys does not create // problems)
// // problems) Camera::MeasurementVector& measurements_lmk1_redundant = measurements_lmk1;
// Camera::MeasurementVector& measurements_lmk1_redundant = measurements_lmk1; measurements_lmk1_redundant.push_back(
// measurements_lmk1_redundant.push_back( measurements_lmk1.at(0)); // we readd the first measurement
// measurements_lmk1.at(0)); // we readd the first measurement std::vector<std::pair<Key, Key>> key_pairs_redundant = key_pairs;
// std::vector<std::pair<Key, Key>> key_pairs_redundant = key_pairs; key_pairs_redundant.push_back(
// key_pairs_redundant.push_back( key_pairs.at(0)); // we readd the first pair of keys
// key_pairs.at(0)); // we readd the first pair of keys std::vector<double> interp_factors_redundant = interp_factors;
// std::vector<double> interp_factors_redundant = interp_factors; interp_factors_redundant.push_back(
// interp_factors_redundant.push_back( interp_factors.at(0)); // we readd the first interp factor
// interp_factors.at(0)); // we readd the first interp factor
// SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model, Camera(Pose3::identity(),sharedK)));
// SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); smartFactor1->add(measurements_lmk1_redundant, key_pairs_redundant,
// smartFactor1->add(measurements_lmk1_redundant, key_pairs_redundant, interp_factors_redundant);
// interp_factors_redundant, sharedK);
// SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model, Camera(Pose3::identity(),sharedK)));
// SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model)); smartFactor2->add(measurements_lmk2, key_pairs, interp_factors);
// smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, sharedK);
// SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model, Camera(Pose3::identity(),sharedK)));
// SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model)); smartFactor3->add(measurements_lmk3, key_pairs, interp_factors);
// smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, sharedK);
// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
// NonlinearFactorGraph graph;
// NonlinearFactorGraph graph; graph.push_back(smartFactor1);
// graph.push_back(smartFactor1); graph.push_back(smartFactor2);
// graph.push_back(smartFactor2); graph.push_back(smartFactor3);
// graph.push_back(smartFactor3); graph.addPrior(x1, level_pose, noisePrior);
// graph.addPrior(x1, level_pose, noisePrior); graph.addPrior(x2, pose_right, noisePrior);
// graph.addPrior(x2, pose_right, noisePrior);
// Values groundTruth;
// Values groundTruth; groundTruth.insert(x1, level_pose);
// groundTruth.insert(x1, level_pose); groundTruth.insert(x2, pose_right);
// groundTruth.insert(x2, pose_right); groundTruth.insert(x3, pose_above);
// groundTruth.insert(x3, pose_above); DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9);
// DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9);
// // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10),
// // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), // Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
// // Point3(0.5,0.1,0.3)); // noise from regular projection factor test below Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise
// Point3(0.1, 0.1, 0.1)); // smaller noise Values values;
// Values values; values.insert(x1, level_pose);
// values.insert(x1, level_pose); values.insert(x2, pose_right);
// values.insert(x2, pose_right); // initialize third pose with some noise, we expect it to move back to
// // initialize third pose with some noise, we expect it to move back to // original pose_above
// // original pose_above values.insert(x3, pose_above * noise_pose);
// values.insert(x3, pose_above * noise_pose); EXPECT( // check that the pose is actually noisy
// EXPECT( // check that the pose is actually noisy assert_equal(Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656,
// assert_equal(Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, -0.000986635786, 0.0314107591,
// -0.0313952598, -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598),
// -0.999013364, -0.0313952598), Point3(0.1, -0.1, 1.9)),
// Point3(0.1, -0.1, 1.9)), values.at<Pose3>(x3)));
// values.at<Pose3>(x3)));
// Values result;
// Values result; LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
// LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); result = optimizer.optimize();
// result = optimizer.optimize(); EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-5));
// EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-5)); }
//}
// #ifndef DISABLE_TIMING
//#ifndef DISABLE_TIMING #include <gtsam/base/timing.h>
//#include <gtsam/base/timing.h> //-Total: 0 CPU (0 times, 0 wall, 0.21 children, min: 0 max: 0)
//// -Total: 0 CPU (0 times, 0 wall, 0.04 children, min: 0 max: 0) //| -SF RS LINEARIZE: 0.15 CPU (10000 times, 0.125521 wall, 0.15 children, min: 0 max: 0)
////| -SF RS LINEARIZE: 0.02 CPU (1000 times, 0.017244 wall, 0.02 children, min: //| -RS LINEARIZE: 0.06 CPU (10000 times, 0.06311 wall, 0.06 children, min: 0 max: 0)
//// 0 max: 0) | -RS LINEARIZE: 0.02 CPU (1000 times, 0.009035 wall, 0.02 /* *************************************************************************/
//// children, min: 0 max: 0) TEST(SmartProjectionPoseFactorRollingShutter, timing) {
///* *************************************************************************/ using namespace vanillaPose;
//TEST(SmartProjectionPoseFactorRollingShutter, timing) {
// using namespace vanillaPose; // Default cameras for simple derivatives
// static Cal3_S2::shared_ptr sharedKSimple(new Cal3_S2(100, 100, 0, 0, 0));
// // Default cameras for simple derivatives
// static Cal3_S2::shared_ptr sharedKSimple(new Cal3_S2(100, 100, 0, 0, 0)); Rot3 R = Rot3::identity();
// Pose3 pose1 = Pose3(R, Point3(0, 0, 0));
// Rot3 R = Rot3::identity(); Pose3 pose2 = Pose3(R, Point3(1, 0, 0));
// Pose3 pose1 = Pose3(R, Point3(0, 0, 0)); Camera cam1(pose1, sharedKSimple), cam2(pose2, sharedKSimple);
// Pose3 pose2 = Pose3(R, Point3(1, 0, 0)); Pose3 body_P_sensorId = Pose3::identity();
// Camera cam1(pose1, sharedKSimple), cam2(pose2, sharedKSimple);
// Pose3 body_P_sensorId = Pose3::identity(); // one landmarks 1m in front of camera
// Point3 landmark1(0, 0, 10);
// // one landmarks 1m in front of camera
// Point3 landmark1(0, 0, 10); Point2Vector measurements_lmk1;
//
// Point2Vector measurements_lmk1; // Project 2 landmarks into 2 cameras
// measurements_lmk1.push_back(cam1.project(landmark1));
// // Project 2 landmarks into 2 cameras measurements_lmk1.push_back(cam2.project(landmark1));
// measurements_lmk1.push_back(cam1.project(landmark1));
// measurements_lmk1.push_back(cam2.project(landmark1)); size_t nrTests = 10000;
//
// size_t nrTests = 1000; for (size_t i = 0; i < nrTests; i++) {
// SmartFactorRS::shared_ptr smartFactorRS(new SmartFactorRS(model, Camera(body_P_sensorId,sharedKSimple)));
// for (size_t i = 0; i < nrTests; i++) { double interp_factor = 0; // equivalent to measurement taken at pose 1
// SmartFactorRS::shared_ptr smartFactorRS(new SmartFactorRS(model)); smartFactorRS->add(measurements_lmk1[0], x1, x2, interp_factor);
// double interp_factor = 0; // equivalent to measurement taken at pose 1 interp_factor = 1; // equivalent to measurement taken at pose 2
// smartFactorRS->add(measurements_lmk1[0], x1, x2, interp_factor, smartFactorRS->add(measurements_lmk1[1], x1, x2, interp_factor);
// sharedKSimple, body_P_sensorId);
// interp_factor = 1; // equivalent to measurement taken at pose 2 Values values;
// smartFactorRS->add(measurements_lmk1[1], x1, x2, interp_factor, values.insert(x1, pose1);
// sharedKSimple, body_P_sensorId); values.insert(x2, pose2);
// gttic_(SF_RS_LINEARIZE);
// Values values; smartFactorRS->linearize(values);
// values.insert(x1, pose1); gttoc_(SF_RS_LINEARIZE);
// values.insert(x2, pose2); }
// gttic_(SF_RS_LINEARIZE);
// smartFactorRS->linearize(values); for (size_t i = 0; i < nrTests; i++) {
// gttoc_(SF_RS_LINEARIZE); SmartFactor::shared_ptr smartFactor(new SmartFactor(model, sharedKSimple));
// } smartFactor->add(measurements_lmk1[0], x1);
// smartFactor->add(measurements_lmk1[1], x2);
// for (size_t i = 0; i < nrTests; i++) {
// SmartFactor::shared_ptr smartFactor(new SmartFactor(model, sharedKSimple)); Values values;
// smartFactor->add(measurements_lmk1[0], x1); values.insert(x1, pose1);
// smartFactor->add(measurements_lmk1[1], x2); values.insert(x2, pose2);
// gttic_(RS_LINEARIZE);
// Values values; smartFactor->linearize(values);
// values.insert(x1, pose1); gttoc_(RS_LINEARIZE);
// values.insert(x2, pose2); }
// gttic_(RS_LINEARIZE); tictoc_print_();
// smartFactor->linearize(values); }
// gttoc_(RS_LINEARIZE); #endif
// }
// tictoc_print_();
//}
//#endif
/* ************************************************************************* */ /* ************************************************************************* */
int main() { int main() {