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

Clean up

release/4.3a0
Frank Dellaert 2019-01-04 15:50:12 -05:00
parent 80a42fe2cd
commit 67f3b51ab2
2 changed files with 49 additions and 107 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

70
gtsam/slam/tests/testSmartProjectionFactor.cpp
View File

@ -10,8 +10,8 @@
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/** /**
* @file testSmartProjectionCameraFactor.cpp * @file testSmartProjectionFactor.cpp
* @brief Unit tests for SmartProjectionCameraFactor Class * @brief Unit tests for SmartProjectionFactor Class
* @author Chris Beall * @author Chris Beall
* @author Luca Carlone * @author Luca Carlone
* @author Zsolt Kira * @author Zsolt Kira
@ -29,19 +29,11 @@
using namespace boost::assign; using namespace boost::assign;
static bool isDebugTest = false; static const bool isDebugTest = false;
static const Symbol l1('l', 1), l2('l', 2), l3('l', 3);
// Convenience for named keys static const Key c1 = 1, c2 = 2, c3 = 3;
using symbol_shorthand::X; static const Point2 measurement1(323.0, 240.0);
using symbol_shorthand::L; static const double rankTol = 1.0;
static Key x1(1);
Symbol l1('l', 1), l2('l', 2), l3('l', 3);
Key c1 = 1, c2 = 2, c3 = 3;
static Point2 measurement1(323.0, 240.0);
static double rankTol = 1.0;
template<class CALIBRATION> template<class CALIBRATION>
PinholeCamera<CALIBRATION> perturbCameraPoseAndCalibration( PinholeCamera<CALIBRATION> perturbCameraPoseAndCalibration(
@ -59,7 +51,7 @@ PinholeCamera<CALIBRATION> perturbCameraPoseAndCalibration(
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( SmartProjectionCameraFactor, perturbCameraPose) { TEST(SmartProjectionFactor, perturbCameraPose) {
using namespace vanilla; using namespace vanilla;
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)); Point3(0.5, 0.1, 0.3));
@ -71,45 +63,45 @@ TEST( SmartProjectionCameraFactor, perturbCameraPose) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( SmartProjectionCameraFactor, Constructor) { TEST(SmartProjectionFactor, Constructor) {
using namespace vanilla; using namespace vanilla;
SmartFactor::shared_ptr factor1(new SmartFactor(unit2)); SmartFactor::shared_ptr factor1(new SmartFactor(unit2));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( SmartProjectionCameraFactor, Constructor2) { TEST(SmartProjectionFactor, Constructor2) {
using namespace vanilla; using namespace vanilla;
params.setRankTolerance(rankTol); params.setRankTolerance(rankTol);
SmartFactor factor1(unit2, boost::none, params); SmartFactor factor1(unit2, boost::none, params);
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( SmartProjectionCameraFactor, Constructor3) { TEST(SmartProjectionFactor, Constructor3) {
using namespace vanilla; using namespace vanilla;
SmartFactor::shared_ptr factor1(new SmartFactor(unit2)); SmartFactor::shared_ptr factor1(new SmartFactor(unit2));
factor1->add(measurement1, x1); factor1->add(measurement1, c1);
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( SmartProjectionCameraFactor, Constructor4) { TEST(SmartProjectionFactor, Constructor4) {
using namespace vanilla; using namespace vanilla;
params.setRankTolerance(rankTol); params.setRankTolerance(rankTol);
SmartFactor factor1(unit2, boost::none, params); SmartFactor factor1(unit2, boost::none, params);
factor1.add(measurement1, x1); factor1.add(measurement1, c1);
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( SmartProjectionCameraFactor, Equals ) { TEST(SmartProjectionFactor, Equals ) {
using namespace vanilla; using namespace vanilla;
SmartFactor::shared_ptr factor1(new SmartFactor(unit2)); SmartFactor::shared_ptr factor1(new SmartFactor(unit2));
factor1->add(measurement1, x1); factor1->add(measurement1, c1);
SmartFactor::shared_ptr factor2(new SmartFactor(unit2)); SmartFactor::shared_ptr factor2(new SmartFactor(unit2));
factor2->add(measurement1, x1); factor2->add(measurement1, c1);
} }
/* *************************************************************************/ /* *************************************************************************/
TEST( SmartProjectionCameraFactor, noiseless ) { TEST(SmartProjectionFactor, noiseless ) {
using namespace vanilla; using namespace vanilla;
Values values; Values values;
@ -128,7 +120,7 @@ TEST( SmartProjectionCameraFactor, noiseless ) {
} }
/* *************************************************************************/ /* *************************************************************************/
TEST( SmartProjectionCameraFactor, noisy ) { TEST(SmartProjectionFactor, noisy ) {
using namespace vanilla; using namespace vanilla;
@ -179,7 +171,7 @@ TEST( SmartProjectionCameraFactor, noisy ) {
} }
/* *************************************************************************/ /* *************************************************************************/
TEST( SmartProjectionCameraFactor, perturbPoseAndOptimize ) { TEST(SmartProjectionFactor, perturbPoseAndOptimize ) {
using namespace vanilla; using namespace vanilla;
@ -278,7 +270,7 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimize ) {
} }
/* *************************************************************************/ /* *************************************************************************/
TEST( SmartProjectionCameraFactor, perturbPoseAndOptimizeFromSfM_tracks ) { TEST(SmartProjectionFactor, perturbPoseAndOptimizeFromSfM_tracks ) {
using namespace vanilla; using namespace vanilla;
@ -348,7 +340,7 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimizeFromSfM_tracks ) {
} }
/* *************************************************************************/ /* *************************************************************************/
TEST( SmartProjectionCameraFactor, perturbCamerasAndOptimize ) { TEST(SmartProjectionFactor, perturbCamerasAndOptimize ) {
using namespace vanilla; using namespace vanilla;
@ -425,7 +417,7 @@ TEST( SmartProjectionCameraFactor, perturbCamerasAndOptimize ) {
} }
/* *************************************************************************/ /* *************************************************************************/
TEST( SmartProjectionCameraFactor, Cal3Bundler ) { TEST(SmartProjectionFactor, Cal3Bundler ) {
using namespace bundler; using namespace bundler;
@ -498,7 +490,7 @@ TEST( SmartProjectionCameraFactor, Cal3Bundler ) {
} }
/* *************************************************************************/ /* *************************************************************************/
TEST( SmartProjectionCameraFactor, Cal3Bundler2 ) { TEST(SmartProjectionFactor, Cal3Bundler2 ) {
using namespace bundler; using namespace bundler;
@ -571,7 +563,7 @@ TEST( SmartProjectionCameraFactor, Cal3Bundler2 ) {
} }
/* *************************************************************************/ /* *************************************************************************/
TEST( SmartProjectionCameraFactor, noiselessBundler ) { TEST(SmartProjectionFactor, noiselessBundler ) {
using namespace bundler; using namespace bundler;
Values values; Values values;
@ -599,7 +591,7 @@ TEST( SmartProjectionCameraFactor, noiselessBundler ) {
} }
/* *************************************************************************/ /* *************************************************************************/
TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor ) { TEST(SmartProjectionFactor, comparisonGeneralSfMFactor ) {
using namespace bundler; using namespace bundler;
Values values; Values values;
@ -638,7 +630,7 @@ TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor ) {
} }
/* *************************************************************************/ /* *************************************************************************/
TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor1 ) { TEST(SmartProjectionFactor, comparisonGeneralSfMFactor1 ) {
using namespace bundler; using namespace bundler;
Values values; Values values;
@ -682,7 +674,7 @@ TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor1 ) {
/* *************************************************************************/ /* *************************************************************************/
// Have to think about how to compare multiplyHessianAdd in generalSfMFactor and smartFactors // Have to think about how to compare multiplyHessianAdd in generalSfMFactor and smartFactors
//TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor2 ){ //TEST(SmartProjectionFactor, comparisonGeneralSfMFactor2 ){
// //
// Values values; // Values values;
// values.insert(c1, level_camera); // values.insert(c1, level_camera);
@ -730,7 +722,7 @@ TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor1 ) {
// EXPECT(assert_equal(yActual,yExpected, 1e-7)); // EXPECT(assert_equal(yActual,yExpected, 1e-7));
//} //}
/* *************************************************************************/ /* *************************************************************************/
TEST( SmartProjectionCameraFactor, computeImplicitJacobian ) { TEST(SmartProjectionFactor, computeImplicitJacobian ) {
using namespace bundler; using namespace bundler;
Values values; Values values;
@ -768,7 +760,7 @@ TEST( SmartProjectionCameraFactor, computeImplicitJacobian ) {
} }
/* *************************************************************************/ /* *************************************************************************/
TEST( SmartProjectionCameraFactor, implicitJacobianFactor ) { TEST(SmartProjectionFactor, implicitJacobianFactor ) {
using namespace bundler; using namespace bundler;
@ -829,7 +821,7 @@ BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropi
BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel"); BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel");
BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal");
TEST( SmartProjectionCameraFactor, serialize) { TEST(SmartProjectionFactor, serialize) {
using namespace vanilla; using namespace vanilla;
using namespace gtsam::serializationTestHelpers; using namespace gtsam::serializationTestHelpers;
SmartFactor factor(unit2); SmartFactor factor(unit2);

86
gtsam/slam/tests/testSmartProjectionPoseFactor.cpp
View File

@ -189,9 +189,7 @@ TEST( SmartProjectionPoseFactor, noisy ) {
measurements.push_back(level_uv); measurements.push_back(level_uv);
measurements.push_back(level_uv_right); measurements.push_back(level_uv_right);
KeyVector views; KeyVector views {x1, x2};
views.push_back(x1);
views.push_back(x2);
factor2->add(measurements, views); factor2->add(measurements, views);
double actualError2 = factor2->error(values); double actualError2 = factor2->error(values);
@ -212,9 +210,7 @@ TEST(SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform) {
Pose3 wTb3 = cam3.pose() * sensor_T_body; Pose3 wTb3 = cam3.pose() * sensor_T_body;
// three landmarks ~5 meters infront of camera // three landmarks ~5 meters infront of camera
Point3 landmark1(5, 0.5, 1.2); Point3 landmark1(5, 0.5, 1.2), landmark2(5, -0.5, 1.2), landmark3(5, 0, 3.0);
Point3 landmark2(5, -0.5, 1.2);
Point3 landmark3(5, 0, 3.0);
Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
@ -224,10 +220,7 @@ TEST(SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform) {
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
// Create smart factors // Create smart factors
KeyVector views; KeyVector views {x1, x2, x3};
views.push_back(x1);
views.push_back(x2);
views.push_back(x3);
SmartProjectionParams params; SmartProjectionParams params;
params.setRankTolerance(1.0); params.setRankTolerance(1.0);
@ -360,9 +353,7 @@ TEST( SmartProjectionPoseFactor, Factors ) {
measurements_cam1.push_back(cam2.project(landmark1)); measurements_cam1.push_back(cam2.project(landmark1));
// Create smart factors // Create smart factors
KeyVector views; KeyVector views {x1, x2};
views.push_back(x1);
views.push_back(x2);
SmartFactor::shared_ptr smartFactor1 = boost::make_shared<SmartFactor>(model, sharedK); SmartFactor::shared_ptr smartFactor1 = boost::make_shared<SmartFactor>(model, sharedK);
smartFactor1->add(measurements_cam1, views); smartFactor1->add(measurements_cam1, views);
@ -510,10 +501,7 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) {
using namespace vanillaPose; using namespace vanillaPose;
KeyVector views; KeyVector views {x1, x2, x3};
views.push_back(x1);
views.push_back(x2);
views.push_back(x3);
Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
@ -567,10 +555,7 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ) {
using namespace vanillaPose; using namespace vanillaPose;
KeyVector views; KeyVector views {x1, x2, x3};
views.push_back(x1);
views.push_back(x2);
views.push_back(x3);
Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
@ -628,10 +613,7 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ) {
double excludeLandmarksFutherThanDist = 2; double excludeLandmarksFutherThanDist = 2;
KeyVector views; KeyVector views {x1, x2, x3};
views.push_back(x1);
views.push_back(x2);
views.push_back(x3);
Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
@ -691,10 +673,7 @@ TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) {
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; KeyVector views {x1, x2, x3};
views.push_back(x1);
views.push_back(x2);
views.push_back(x3);
// add fourth landmark // add fourth landmark
Point3 landmark4(5, -0.5, 1); Point3 landmark4(5, -0.5, 1);
@ -757,10 +736,7 @@ TEST( SmartProjectionPoseFactor, jacobianQ ) {
using namespace vanillaPose; using namespace vanillaPose;
KeyVector views; KeyVector views {x1, x2, x3};
views.push_back(x1);
views.push_back(x2);
views.push_back(x3);
Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
@ -811,10 +787,7 @@ TEST( SmartProjectionPoseFactor, 3poses_projection_factor ) {
using namespace vanillaPose2; using namespace vanillaPose2;
KeyVector views; KeyVector views {x1, x2, x3};
views.push_back(x1);
views.push_back(x2);
views.push_back(x3);
typedef GenericProjectionFactor<Pose3, Point3> ProjectionFactor; typedef GenericProjectionFactor<Pose3, Point3> ProjectionFactor;
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
@ -859,10 +832,7 @@ TEST( SmartProjectionPoseFactor, 3poses_projection_factor ) {
/* *************************************************************************/ /* *************************************************************************/
TEST( SmartProjectionPoseFactor, CheckHessian) { TEST( SmartProjectionPoseFactor, CheckHessian) {
KeyVector views; KeyVector views {x1, x2, x3};
views.push_back(x1);
views.push_back(x2);
views.push_back(x3);
using namespace vanillaPose; using namespace vanillaPose;
@ -945,10 +915,7 @@ TEST( SmartProjectionPoseFactor, CheckHessian) {
TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_factor ) { TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_factor ) {
using namespace vanillaPose2; using namespace vanillaPose2;
KeyVector views; KeyVector views {x1, x2, x3};
views.push_back(x1);
views.push_back(x2);
views.push_back(x3);
// Two different cameras, at the same position, but different rotations // Two different cameras, at the same position, but different rotations
Pose3 pose2 = level_pose * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); Pose3 pose2 = level_pose * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0));
@ -1004,10 +971,7 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor )
// this test considers a condition in which the cheirality constraint is triggered // this test considers a condition in which the cheirality constraint is triggered
using namespace vanillaPose; using namespace vanillaPose;
KeyVector views; KeyVector views {x1, x2, x3};
views.push_back(x1);
views.push_back(x2);
views.push_back(x3);
// Two different cameras, at the same position, but different rotations // Two different cameras, at the same position, but different rotations
Pose3 pose2 = level_pose Pose3 pose2 = level_pose
@ -1089,9 +1053,7 @@ TEST( SmartProjectionPoseFactor, Hessian ) {
using namespace vanillaPose2; using namespace vanillaPose2;
KeyVector views; KeyVector views {x1, x2};
views.push_back(x1);
views.push_back(x2);
// Project three landmarks into 2 cameras // Project three landmarks into 2 cameras
Point2 cam1_uv1 = cam1.project(landmark1); Point2 cam1_uv1 = cam1.project(landmark1);
@ -1123,10 +1085,7 @@ TEST( SmartProjectionPoseFactor, HessianWithRotation ) {
using namespace vanillaPose; using namespace vanillaPose;
KeyVector views; KeyVector views {x1, x2, x3};
views.push_back(x1);
views.push_back(x2);
views.push_back(x3);
Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
@ -1176,10 +1135,7 @@ TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ) {
using namespace vanillaPose2; using namespace vanillaPose2;
KeyVector views; KeyVector views {x1, x2, x3};
views.push_back(x1);
views.push_back(x2);
views.push_back(x3);
// All cameras have the same pose so will be degenerate ! // All cameras have the same pose so will be degenerate !
Camera cam2(level_pose, sharedK2); Camera cam2(level_pose, sharedK2);
@ -1250,10 +1206,7 @@ TEST( SmartProjectionPoseFactor, Cal3Bundler ) {
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);
KeyVector views; KeyVector views {x1, x2, x3};
views.push_back(x1);
views.push_back(x2);
views.push_back(x3);
SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedBundlerK)); SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedBundlerK));
smartFactor1->add(measurements_cam1, views); smartFactor1->add(measurements_cam1, views);
@ -1299,10 +1252,7 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) {
using namespace bundlerPose; using namespace bundlerPose;
KeyVector views; KeyVector views {x1, x2, x3};
views.push_back(x1);
views.push_back(x2);
views.push_back(x3);
// Two different cameras // Two different cameras
Pose3 pose2 = level_pose Pose3 pose2 = level_pose