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

halfway there

release/4.3a0
lcarlone 2021-10-02 21:51:44 -04:00
parent 117f0d1f45
commit eb82878044
1 changed files with 155 additions and 151 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

306
gtsam/slam/tests/testSmartProjectionRigFactor.cpp
View File

@ -351,157 +351,161 @@ TEST( SmartProjectionRigFactor, 3poses_smart_projection_factor ) {
EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-6)); EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-6));
} }
///* *************************************************************************/ /* *************************************************************************/
//TEST( SmartProjectionRigFactor, Factors ) { TEST( SmartProjectionRigFactor, Factors ) {
//
// using namespace vanillaPose; using namespace vanillaPose;
//
// // Default cameras for simple derivatives // Default cameras for simple derivatives
// Rot3 R; Rot3 R;
// static Cal3_S2::shared_ptr sharedK(new Cal3_S2(100, 100, 0, 0, 0)); static Cal3_S2::shared_ptr sharedK(new Cal3_S2(100, 100, 0, 0, 0));
// Camera cam1(Pose3(R, Point3(0, 0, 0)), sharedK), cam2( Camera cam1(Pose3(R, Point3(0, 0, 0)), sharedK), cam2(
// Pose3(R, Point3(1, 0, 0)), sharedK); Pose3(R, Point3(1, 0, 0)), sharedK);
//
// // one landmarks 1m in front of camera // one landmarks 1m in front of camera
// Point3 landmark1(0, 0, 10); Point3 landmark1(0, 0, 10);
//
// Point2Vector measurements_cam1; Point2Vector measurements_cam1;
//
// // Project 2 landmarks into 2 cameras // Project 2 landmarks into 2 cameras
// measurements_cam1.push_back(cam1.project(landmark1)); measurements_cam1.push_back(cam1.project(landmark1));
// measurements_cam1.push_back(cam2.project(landmark1)); measurements_cam1.push_back(cam2.project(landmark1));
//
// // Create smart factors // Create smart factors
// KeyVector views { x1, x2 }; Cameras cameraRig; // single camera in the rig
// cameraRig.push_back( Camera(Pose3::identity(), sharedK) );
// std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs;
// sharedKs.push_back(sharedK); KeyVector views { x1, x2 };
// sharedKs.push_back(sharedK); FastVector<size_t> cameraIds { 0, 0 };
//
// SmartFactorP::shared_ptr smartFactor1 = boost::make_shared < SmartFactorP SmartFactorP::shared_ptr smartFactor1 = boost::make_shared < SmartFactorP
// > (model); > (model,cameraRig);
// smartFactor1->add(measurements_cam1, views, sharedKs); smartFactor1->add(measurements_cam1, views, cameraIds);
//
// SmartFactorP::Cameras cameras; SmartFactorP::Cameras cameras;
// cameras.push_back(cam1); cameras.push_back(cam1);
// cameras.push_back(cam2); cameras.push_back(cam2);
//
// // Make sure triangulation works // Make sure triangulation works
// CHECK(smartFactor1->triangulateSafe(cameras)); CHECK(smartFactor1->triangulateSafe(cameras));
// CHECK(!smartFactor1->isDegenerate()); CHECK(!smartFactor1->isDegenerate());
// CHECK(!smartFactor1->isPointBehindCamera()); CHECK(!smartFactor1->isPointBehindCamera());
// boost::optional<Point3> p = smartFactor1->point(); boost::optional<Point3> p = smartFactor1->point();
// CHECK(p); CHECK(p);
// EXPECT(assert_equal(landmark1, *p)); EXPECT(assert_equal(landmark1, *p));
//
// VectorValues zeroDelta; VectorValues zeroDelta;
// Vector6 delta; Vector6 delta;
// delta.setZero(); delta.setZero();
// zeroDelta.insert(x1, delta); zeroDelta.insert(x1, delta);
// zeroDelta.insert(x2, delta); zeroDelta.insert(x2, delta);
//
// VectorValues perturbedDelta; VectorValues perturbedDelta;
// delta.setOnes(); delta.setOnes();
// perturbedDelta.insert(x1, delta); perturbedDelta.insert(x1, delta);
// perturbedDelta.insert(x2, delta); perturbedDelta.insert(x2, delta);
// double expectedError = 2500; double expectedError = 2500;
//
// // After eliminating the point, A1 and A2 contain 2-rank information on cameras: // After eliminating the point, A1 and A2 contain 2-rank information on cameras:
// Matrix16 A1, A2; Matrix16 A1, A2;
// A1 << -10, 0, 0, 0, 1, 0; A1 << -10, 0, 0, 0, 1, 0;
// A2 << 10, 0, 1, 0, -1, 0; A2 << 10, 0, 1, 0, -1, 0;
// A1 *= 10. / sigma; A1 *= 10. / sigma;
// A2 *= 10. / sigma; A2 *= 10. / sigma;
// Matrix expectedInformation; // filled below Matrix expectedInformation; // filled below
// { {
// // createHessianFactor // createHessianFactor
// Matrix66 G11 = 0.5 * A1.transpose() * A1; Matrix66 G11 = 0.5 * A1.transpose() * A1;
// Matrix66 G12 = 0.5 * A1.transpose() * A2; Matrix66 G12 = 0.5 * A1.transpose() * A2;
// Matrix66 G22 = 0.5 * A2.transpose() * A2; Matrix66 G22 = 0.5 * A2.transpose() * A2;
//
// Vector6 g1; Vector6 g1;
// g1.setZero(); g1.setZero();
// Vector6 g2; Vector6 g2;
// g2.setZero(); g2.setZero();
//
// double f = 0; double f = 0;
//
// RegularHessianFactor<6> expected(x1, x2, G11, G12, g1, G22, g2, f); RegularHessianFactor<6> expected(x1, x2, G11, G12, g1, G22, g2, f);
// expectedInformation = expected.information(); expectedInformation = expected.information();
//
// Values values; Values values;
// values.insert(x1, cam1.pose()); values.insert(x1, cam1.pose());
// values.insert(x2, cam2.pose()); values.insert(x2, cam2.pose());
//
// boost::shared_ptr < RegularHessianFactor<6> > actual = smartFactor1 boost::shared_ptr < RegularHessianFactor<6> > actual = smartFactor1
// ->createHessianFactor(values, 0.0); ->createHessianFactor(values, 0.0);
// EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6)); EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6));
// EXPECT(assert_equal(expected, *actual, 1e-6)); EXPECT(assert_equal(expected, *actual, 1e-6));
// EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-6); EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-6);
// EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-6); EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-6);
// } }
//} }
//
///* *************************************************************************/ /* *************************************************************************/
//TEST( SmartProjectionRigFactor, 3poses_iterative_smart_projection_factor ) { TEST( SmartProjectionRigFactor, 3poses_iterative_smart_projection_factor ) {
//
// using namespace vanillaPose; using namespace vanillaPose;
//
// KeyVector views { x1, x2, x3 }; KeyVector views { x1, x2, x3 };
//
// Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
//
// // Project three landmarks into three cameras // Project three landmarks into three cameras
// projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
// projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
// projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
//
// std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs;
// sharedKs.push_back(sharedK); sharedKs.push_back(sharedK);
// sharedKs.push_back(sharedK); sharedKs.push_back(sharedK);
// sharedKs.push_back(sharedK); sharedKs.push_back(sharedK);
//
// SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model)); // create smart factor
// smartFactor1->add(measurements_cam1, views, sharedKs); Cameras cameraRig; // single camera in the rig
// cameraRig.push_back( Camera(Pose3::identity(), sharedK) );
// SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model)); FastVector<size_t> cameraIds { 0, 0, 0};
// smartFactor2->add(measurements_cam2, views, sharedKs); SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, cameraRig));
// smartFactor1->add(measurements_cam1, views, cameraIds);
// SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model));
// smartFactor3->add(measurements_cam3, views, sharedKs); SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model, cameraRig));
// smartFactor2->add(measurements_cam2, views, cameraIds);
// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
// SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model, cameraRig));
// NonlinearFactorGraph graph; smartFactor3->add(measurements_cam3, views, cameraIds);
// graph.push_back(smartFactor1);
// graph.push_back(smartFactor2); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
// graph.push_back(smartFactor3);
// graph.addPrior(x1, cam1.pose(), noisePrior); NonlinearFactorGraph graph;
// graph.addPrior(x2, cam2.pose(), noisePrior); graph.push_back(smartFactor1);
// graph.push_back(smartFactor2);
// // 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 graph.push_back(smartFactor3);
// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), graph.addPrior(x1, cam1.pose(), noisePrior);
// Point3(0.1, 0.1, 0.1)); // smaller noise graph.addPrior(x2, cam2.pose(), noisePrior);
// Values values;
// values.insert(x1, cam1.pose()); // 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
// values.insert(x2, cam2.pose()); Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
// // initialize third pose with some noise, we expect it to move back to original pose_above Point3(0.1, 0.1, 0.1)); // smaller noise
// values.insert(x3, pose_above * noise_pose); Values values;
// EXPECT( values.insert(x1, cam1.pose());
// assert_equal( values.insert(x2, cam2.pose());
// Pose3( // initialize third pose with some noise, we expect it to move back to original pose_above
// Rot3(1.11022302e-16, -0.0314107591, 0.99950656, -0.99950656, values.insert(x3, pose_above * noise_pose);
// -0.0313952598, -0.000986635786, 0.0314107591, -0.999013364, EXPECT(
// -0.0313952598), assert_equal(
// Point3(0.1, -0.1, 1.9)), Pose3(
// values.at<Pose3>(x3))); Rot3(1.11022302e-16, -0.0314107591, 0.99950656, -0.99950656,
// -0.0313952598, -0.000986635786, 0.0314107591, -0.999013364,
// Values result; -0.0313952598),
// LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); Point3(0.1, -0.1, 1.9)),
// result = optimizer.optimize(); values.at<Pose3>(x3)));
// EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-7));
//} Values result;
// LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
result = optimizer.optimize();
EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-7));
}
///* *************************************************************************/ ///* *************************************************************************/
//TEST( SmartProjectionRigFactor, landmarkDistance ) { //TEST( SmartProjectionRigFactor, landmarkDistance ) {
// //