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

working on tests

release/4.3a0
lcarlone 2021-10-02 22:00:09 -04:00
parent eb82878044
commit fa4de18742
2 changed files with 250 additions and 251 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
gtsam/slam/SmartProjectionRigFactor.h
View File

@ -127,8 +127,10 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
*/ */
void add(const Point2Vector& measurements, const KeyVector& poseKeys, void add(const Point2Vector& measurements, const KeyVector& poseKeys,
const FastVector<size_t>& cameraIds) { const FastVector<size_t>& cameraIds) {
assert(poseKeys.size() == measurements.size()); if(poseKeys.size() != measurements.size() || poseKeys.size() != cameraIds.size()){
assert(poseKeys.size() == cameraIds.size()); throw std::runtime_error("SmartProjectionRigFactor: "
"trying to add inconsistent inputs");
}
for (size_t i = 0; i < measurements.size(); i++) { for (size_t i = 0; i < measurements.size(); i++) {
add(measurements[i], poseKeys[i], cameraIds[i]); add(measurements[i], poseKeys[i], cameraIds[i]);
} }

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

@ -506,255 +506,252 @@ TEST( SmartProjectionRigFactor, 3poses_iterative_smart_projection_factor ) {
EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-7)); EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-7));
} }
///* *************************************************************************/ /* *************************************************************************/
//TEST( SmartProjectionRigFactor, landmarkDistance ) { TEST( SmartProjectionRigFactor, landmarkDistance ) {
//
// using namespace vanillaPose; using namespace vanillaPose;
//
// double excludeLandmarksFutherThanDist = 2; double excludeLandmarksFutherThanDist = 2;
//
// 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; SmartProjectionParams params;
// sharedKs.push_back(sharedK); params.setRankTolerance(1.0);
// sharedKs.push_back(sharedK); params.setLinearizationMode(gtsam::JACOBIAN_SVD);
// sharedKs.push_back(sharedK); params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY);
// params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist);
// SmartProjectionParams params; params.setEnableEPI(false);
// params.setRankTolerance(1.0);
// params.setLinearizationMode(gtsam::JACOBIAN_SVD); Cameras cameraRig; // single camera in the rig
// params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); cameraRig.push_back( Camera(Pose3::identity(), sharedK) );
// params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); FastVector<size_t> cameraIds { 0, 0, 0};
// params.setEnableEPI(false);
// SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, cameraRig, params));
// SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, params)); smartFactor1->add(measurements_cam1, views, cameraIds);
// smartFactor1->add(measurements_cam1, views, sharedKs);
// SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model, cameraRig, params));
// SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model, params)); smartFactor2->add(measurements_cam2, views, cameraIds);
// smartFactor2->add(measurements_cam2, views, sharedKs);
// SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model, cameraRig, params));
// SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model, params)); smartFactor3->add(measurements_cam3, views, cameraIds);
// smartFactor3->add(measurements_cam3, views, sharedKs);
// 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, cam1.pose(), noisePrior);
// graph.addPrior(x1, cam1.pose(), noisePrior); graph.addPrior(x2, cam2.pose(), noisePrior);
// graph.addPrior(x2, cam2.pose(), noisePrior);
// // 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
// // 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 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, cam1.pose());
// values.insert(x1, cam1.pose()); values.insert(x2, cam2.pose());
// values.insert(x2, cam2.pose()); values.insert(x3, pose_above * noise_pose);
// values.insert(x3, pose_above * noise_pose);
// // All factors are disabled and pose should remain where it is
// // All factors are disabled and pose should remain where it is Values result;
// Values result; LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
// LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); result = optimizer.optimize();
// result = optimizer.optimize(); EXPECT(assert_equal(values.at<Pose3>(x3), result.at<Pose3>(x3)));
// EXPECT(assert_equal(values.at<Pose3>(x3), result.at<Pose3>(x3))); }
//}
// /* *************************************************************************/
///* *************************************************************************/ TEST( SmartProjectionRigFactor, dynamicOutlierRejection ) {
//TEST( SmartProjectionRigFactor, dynamicOutlierRejection ) {
// using namespace vanillaPose;
// using namespace vanillaPose;
// double excludeLandmarksFutherThanDist = 1e10;
// double excludeLandmarksFutherThanDist = 1e10; double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error
// double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error
// KeyVector views { x1, x2, x3 };
// KeyVector views { x1, x2, x3 };
// // add fourth landmark
// std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; Point3 landmark4(5, -0.5, 1);
// sharedKs.push_back(sharedK);
// sharedKs.push_back(sharedK); Point2Vector measurements_cam1, measurements_cam2, measurements_cam3,
// sharedKs.push_back(sharedK); measurements_cam4;
//
// // add fourth landmark // Project 4 landmarks into three cameras
// Point3 landmark4(5, -0.5, 1); projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
// projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
// Point2Vector measurements_cam1, measurements_cam2, measurements_cam3, projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
// measurements_cam4; projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4);
// measurements_cam4.at(0) = measurements_cam4.at(0) + Point2(10, 10); // add outlier
// // Project 4 landmarks into three cameras
// projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); SmartProjectionParams params;
// projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); params.setLinearizationMode(gtsam::HESSIAN);
// projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY);
// projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4); params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist);
// measurements_cam4.at(0) = measurements_cam4.at(0) + Point2(10, 10); // add outlier params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold);
//
// SmartProjectionParams params; Cameras cameraRig; // single camera in the rig
// params.setLinearizationMode(gtsam::HESSIAN); cameraRig.push_back( Camera(Pose3::identity(), sharedK) );
// params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); FastVector<size_t> cameraIds { 0, 0, 0};
// params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist);
// params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold); SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, cameraRig, params));
// smartFactor1->add(measurements_cam1, views, cameraIds);
// SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, params));
// smartFactor1->add(measurements_cam1, views, sharedKs); SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model, cameraRig, params));
// smartFactor2->add(measurements_cam2, views, cameraIds);
// SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model, params));
// smartFactor2->add(measurements_cam2, views, sharedKs); SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model, cameraRig, params));
// smartFactor3->add(measurements_cam3, views, cameraIds);
// SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model, params));
// smartFactor3->add(measurements_cam3, views, sharedKs); SmartFactorP::shared_ptr smartFactor4(new SmartFactorP(model, cameraRig, params));
// smartFactor4->add(measurements_cam4, views, cameraIds);
// SmartFactorP::shared_ptr smartFactor4(new SmartFactorP(model, params));
// smartFactor4->add(measurements_cam4, views, sharedKs); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
//
// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); NonlinearFactorGraph graph;
// graph.push_back(smartFactor1);
// NonlinearFactorGraph graph; graph.push_back(smartFactor2);
// graph.push_back(smartFactor1); graph.push_back(smartFactor3);
// graph.push_back(smartFactor2); graph.push_back(smartFactor4);
// graph.push_back(smartFactor3); graph.addPrior(x1, cam1.pose(), noisePrior);
// graph.push_back(smartFactor4); graph.addPrior(x2, cam2.pose(), noisePrior);
// graph.addPrior(x1, cam1.pose(), noisePrior);
// graph.addPrior(x2, cam2.pose(), noisePrior); Values values;
// values.insert(x1, cam1.pose());
// Values values; values.insert(x2, cam2.pose());
// values.insert(x1, cam1.pose()); values.insert(x3, cam3.pose());
// values.insert(x2, cam2.pose());
// values.insert(x3, cam3.pose()); // All factors are disabled and pose should remain where it is
// Values result;
// // All factors are disabled and pose should remain where it is LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
// Values result; result = optimizer.optimize();
// LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); EXPECT(assert_equal(cam3.pose(), result.at<Pose3>(x3)));
// result = optimizer.optimize(); }
// EXPECT(assert_equal(cam3.pose(), result.at<Pose3>(x3)));
//} /* *************************************************************************/
// TEST( SmartProjectionRigFactor, CheckHessian) {
///* *************************************************************************/
//TEST( SmartProjectionRigFactor, CheckHessian) { KeyVector views { x1, x2, x3 };
//
// KeyVector views { x1, x2, x3 }; using namespace vanillaPose;
//
// using namespace vanillaPose; // Two slightly different cameras
// Pose3 pose2 = level_pose
// // Two slightly different cameras * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0));
// Pose3 pose2 = level_pose Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0));
// * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); Camera cam2(pose2, sharedK);
// Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); Camera cam3(pose3, sharedK);
// Camera cam2(pose2, sharedK);
// Camera cam3(pose3, sharedK); Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
//
// Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; // Project three landmarks into three cameras
// projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
// // Project three landmarks into three cameras projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
// projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
// projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
// projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); SmartProjectionParams params;
// params.setRankTolerance(10);
// std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY);
// sharedKs.push_back(sharedK);
// sharedKs.push_back(sharedK); Cameras cameraRig; // single camera in the rig
// sharedKs.push_back(sharedK); cameraRig.push_back( Camera(Pose3::identity(), sharedK) );
// FastVector<size_t> cameraIds { 0, 0, 0};
// SmartProjectionParams params;
// params.setRankTolerance(10); SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, cameraRig, params)); // HESSIAN, by default
// params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); smartFactor1->add(measurements_cam1, views, cameraIds);
//
// SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, params)); // HESSIAN, by default SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model, cameraRig, params)); // HESSIAN, by default
// smartFactor1->add(measurements_cam1, views, sharedKs); smartFactor2->add(measurements_cam2, views, cameraIds);
//
// SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model, params)); // HESSIAN, by default SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model, cameraRig, params)); // HESSIAN, by default
// smartFactor2->add(measurements_cam2, views, sharedKs); smartFactor3->add(measurements_cam3, views, cameraIds);
//
// SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model, params)); // HESSIAN, by default NonlinearFactorGraph graph;
// smartFactor3->add(measurements_cam3, views, sharedKs); graph.push_back(smartFactor1);
// graph.push_back(smartFactor2);
// NonlinearFactorGraph graph; graph.push_back(smartFactor3);
// 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),
// Point3(0.1, 0.1, 0.1)); // smaller noise
// // 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 values;
// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), values.insert(x1, cam1.pose());
// Point3(0.1, 0.1, 0.1)); // smaller noise values.insert(x2, cam2.pose());
// Values values; // initialize third pose with some noise, we expect it to move back to original pose_above
// values.insert(x1, cam1.pose()); values.insert(x3, pose3 * noise_pose);
// values.insert(x2, cam2.pose()); EXPECT(
// // initialize third pose with some noise, we expect it to move back to original pose_above assert_equal(
// values.insert(x3, pose3 * noise_pose); Pose3(
// EXPECT( Rot3(0.00563056869, -0.130848107, 0.991386438, -0.991390265,
// assert_equal( -0.130426831, -0.0115837907, 0.130819108, -0.98278564,
// Pose3( -0.130455917),
// Rot3(0.00563056869, -0.130848107, 0.991386438, -0.991390265, Point3(0.0897734171, -0.110201006, 0.901022872)),
// -0.130426831, -0.0115837907, 0.130819108, -0.98278564, values.at<Pose3>(x3)));
// -0.130455917),
// Point3(0.0897734171, -0.110201006, 0.901022872)), boost::shared_ptr<GaussianFactor> factor1 = smartFactor1->linearize(values);
// values.at<Pose3>(x3))); boost::shared_ptr<GaussianFactor> factor2 = smartFactor2->linearize(values);
// boost::shared_ptr<GaussianFactor> factor3 = smartFactor3->linearize(values);
// boost::shared_ptr<GaussianFactor> factor1 = smartFactor1->linearize(values);
// boost::shared_ptr<GaussianFactor> factor2 = smartFactor2->linearize(values); Matrix CumulativeInformation = factor1->information() + factor2->information()
// boost::shared_ptr<GaussianFactor> factor3 = smartFactor3->linearize(values); + factor3->information();
//
// Matrix CumulativeInformation = factor1->information() + factor2->information() boost::shared_ptr<GaussianFactorGraph> GaussianGraph = graph.linearize(
// + factor3->information(); values);
// Matrix GraphInformation = GaussianGraph->hessian().first;
// boost::shared_ptr<GaussianFactorGraph> GaussianGraph = graph.linearize(
// values); // Check Hessian
// Matrix GraphInformation = GaussianGraph->hessian().first; EXPECT(assert_equal(GraphInformation, CumulativeInformation, 1e-6));
//
// // Check Hessian Matrix AugInformationMatrix = factor1->augmentedInformation()
// EXPECT(assert_equal(GraphInformation, CumulativeInformation, 1e-6)); + factor2->augmentedInformation() + factor3->augmentedInformation();
//
// Matrix AugInformationMatrix = factor1->augmentedInformation() // Check Information vector
// + factor2->augmentedInformation() + factor3->augmentedInformation(); Vector InfoVector = AugInformationMatrix.block(0, 18, 18, 1); // 18x18 Hessian + information vector
//
// // Check Information vector // Check Hessian
// Vector InfoVector = AugInformationMatrix.block(0, 18, 18, 1); // 18x18 Hessian + information vector EXPECT(assert_equal(InfoVector, GaussianGraph->hessian().second, 1e-6));
// }
// // Check Hessian
// EXPECT(assert_equal(InfoVector, GaussianGraph->hessian().second, 1e-6)); /* *************************************************************************/
//} TEST( SmartProjectionRigFactor, Hessian ) {
//
///* *************************************************************************/ using namespace vanillaPose2;
//TEST( SmartProjectionRigFactor, Hessian ) {
// KeyVector views { x1, x2 };
// using namespace vanillaPose2;
// // Project three landmarks into 2 cameras
// KeyVector views { x1, x2 }; Point2 cam1_uv1 = cam1.project(landmark1);
// Point2 cam2_uv1 = cam2.project(landmark1);
// // Project three landmarks into 2 cameras Point2Vector measurements_cam1;
// Point2 cam1_uv1 = cam1.project(landmark1); measurements_cam1.push_back(cam1_uv1);
// Point2 cam2_uv1 = cam2.project(landmark1); measurements_cam1.push_back(cam2_uv1);
// Point2Vector measurements_cam1;
// measurements_cam1.push_back(cam1_uv1); Cameras cameraRig; // single camera in the rig
// measurements_cam1.push_back(cam2_uv1); cameraRig.push_back( Camera(Pose3::identity(), sharedK2) );
// FastVector<size_t> cameraIds { 0, 0 };
// std::vector < boost::shared_ptr < Cal3_S2 >> sharedK2s;
// sharedK2s.push_back(sharedK2); SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, cameraRig));
// sharedK2s.push_back(sharedK2); smartFactor1->add(measurements_cam1, views, cameraIds);
//
// SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model)); Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10),
// smartFactor1->add(measurements_cam1, views, sharedK2s); Point3(0.5, 0.1, 0.3));
// Values values;
// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), values.insert(x1, cam1.pose());
// Point3(0.5, 0.1, 0.3)); values.insert(x2, cam2.pose());
// Values values;
// values.insert(x1, cam1.pose()); boost::shared_ptr<GaussianFactor> factor = smartFactor1->linearize(values);
// values.insert(x2, cam2.pose());
// // compute triangulation from linearization point
// boost::shared_ptr<GaussianFactor> factor = smartFactor1->linearize(values); // compute reprojection errors (sum squared)
// // compare with factor.info(): the bottom right element is the squared sum of the reprojection errors (normalized by the covariance)
// // compute triangulation from linearization point // check that it is correctly scaled when using noiseProjection = [1/4 0; 0 1/4]
// // compute reprojection errors (sum squared) }
// // compare with factor.info(): the bottom right element is the squared sum of the reprojection errors (normalized by the covariance)
// // check that it is correctly scaled when using noiseProjection = [1/4 0; 0 1/4]
//}
//
///* ************************************************************************* */ ///* ************************************************************************* */
//TEST( SmartProjectionRigFactor, ConstructorWithCal3Bundler) { //TEST( SmartProjectionRigFactor, ConstructorWithCal3Bundler) {
// using namespace bundlerPose; // using namespace bundlerPose;