Clean up
Frank Dellaert
parent
80a42fe2cd
commit
67f3b51ab2
|
|
@ -10,8 +10,8 @@
|
|||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file testSmartProjectionCameraFactor.cpp
|
||||
* @brief Unit tests for SmartProjectionCameraFactor Class
|
||||
* @file testSmartProjectionFactor.cpp
|
||||
* @brief Unit tests for SmartProjectionFactor Class
|
||||
* @author Chris Beall
|
||||
* @author Luca Carlone
|
||||
* @author Zsolt Kira
|
||||
|
|
@ -29,19 +29,11 @@
|
|||
|
||||
using namespace boost::assign;
|
||||
|
||||
static bool isDebugTest = false;
|
||||
|
||||
// Convenience for named keys
|
||||
using symbol_shorthand::X;
|
||||
using symbol_shorthand::L;
|
||||
|
||||
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;
|
||||
static const bool isDebugTest = false;
|
||||
static const Symbol l1('l', 1), l2('l', 2), l3('l', 3);
|
||||
static const Key c1 = 1, c2 = 2, c3 = 3;
|
||||
static const Point2 measurement1(323.0, 240.0);
|
||||
static const double rankTol = 1.0;
|
||||
|
||||
template<class CALIBRATION>
|
||||
PinholeCamera<CALIBRATION> perturbCameraPoseAndCalibration(
|
||||
|
|
@ -59,7 +51,7 @@ PinholeCamera<CALIBRATION> perturbCameraPoseAndCalibration(
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( SmartProjectionCameraFactor, perturbCameraPose) {
|
||||
TEST(SmartProjectionFactor, perturbCameraPose) {
|
||||
using namespace vanilla;
|
||||
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10),
|
||||
Point3(0.5, 0.1, 0.3));
|
||||
|
|
@ -71,45 +63,45 @@ TEST( SmartProjectionCameraFactor, perturbCameraPose) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( SmartProjectionCameraFactor, Constructor) {
|
||||
TEST(SmartProjectionFactor, Constructor) {
|
||||
using namespace vanilla;
|
||||
SmartFactor::shared_ptr factor1(new SmartFactor(unit2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( SmartProjectionCameraFactor, Constructor2) {
|
||||
TEST(SmartProjectionFactor, Constructor2) {
|
||||
using namespace vanilla;
|
||||
params.setRankTolerance(rankTol);
|
||||
SmartFactor factor1(unit2, boost::none, params);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( SmartProjectionCameraFactor, Constructor3) {
|
||||
TEST(SmartProjectionFactor, Constructor3) {
|
||||
using namespace vanilla;
|
||||
SmartFactor::shared_ptr factor1(new SmartFactor(unit2));
|
||||
factor1->add(measurement1, x1);
|
||||
factor1->add(measurement1, c1);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( SmartProjectionCameraFactor, Constructor4) {
|
||||
TEST(SmartProjectionFactor, Constructor4) {
|
||||
using namespace vanilla;
|
||||
params.setRankTolerance(rankTol);
|
||||
SmartFactor factor1(unit2, boost::none, params);
|
||||
factor1.add(measurement1, x1);
|
||||
factor1.add(measurement1, c1);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( SmartProjectionCameraFactor, Equals ) {
|
||||
TEST(SmartProjectionFactor, Equals ) {
|
||||
using namespace vanilla;
|
||||
SmartFactor::shared_ptr factor1(new SmartFactor(unit2));
|
||||
factor1->add(measurement1, x1);
|
||||
factor1->add(measurement1, c1);
|
||||
|
||||
SmartFactor::shared_ptr factor2(new SmartFactor(unit2));
|
||||
factor2->add(measurement1, x1);
|
||||
factor2->add(measurement1, c1);
|
||||
}
|
||||
|
||||
/* *************************************************************************/
|
||||
TEST( SmartProjectionCameraFactor, noiseless ) {
|
||||
TEST(SmartProjectionFactor, noiseless ) {
|
||||
using namespace vanilla;
|
||||
|
||||
Values values;
|
||||
|
|
@ -128,7 +120,7 @@ TEST( SmartProjectionCameraFactor, noiseless ) {
|
|||
}
|
||||
|
||||
/* *************************************************************************/
|
||||
TEST( SmartProjectionCameraFactor, noisy ) {
|
||||
TEST(SmartProjectionFactor, noisy ) {
|
||||
|
||||
using namespace vanilla;
|
||||
|
||||
|
|
@ -179,7 +171,7 @@ TEST( SmartProjectionCameraFactor, noisy ) {
|
|||
}
|
||||
|
||||
/* *************************************************************************/
|
||||
TEST( SmartProjectionCameraFactor, perturbPoseAndOptimize ) {
|
||||
TEST(SmartProjectionFactor, perturbPoseAndOptimize ) {
|
||||
|
||||
using namespace vanilla;
|
||||
|
||||
|
|
@ -278,7 +270,7 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimize ) {
|
|||
}
|
||||
|
||||
/* *************************************************************************/
|
||||
TEST( SmartProjectionCameraFactor, perturbPoseAndOptimizeFromSfM_tracks ) {
|
||||
TEST(SmartProjectionFactor, perturbPoseAndOptimizeFromSfM_tracks ) {
|
||||
|
||||
using namespace vanilla;
|
||||
|
||||
|
|
@ -348,7 +340,7 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimizeFromSfM_tracks ) {
|
|||
}
|
||||
|
||||
/* *************************************************************************/
|
||||
TEST( SmartProjectionCameraFactor, perturbCamerasAndOptimize ) {
|
||||
TEST(SmartProjectionFactor, perturbCamerasAndOptimize ) {
|
||||
|
||||
using namespace vanilla;
|
||||
|
||||
|
|
@ -425,7 +417,7 @@ TEST( SmartProjectionCameraFactor, perturbCamerasAndOptimize ) {
|
|||
}
|
||||
|
||||
/* *************************************************************************/
|
||||
TEST( SmartProjectionCameraFactor, Cal3Bundler ) {
|
||||
TEST(SmartProjectionFactor, Cal3Bundler ) {
|
||||
|
||||
using namespace bundler;
|
||||
|
||||
|
|
@ -498,7 +490,7 @@ TEST( SmartProjectionCameraFactor, Cal3Bundler ) {
|
|||
}
|
||||
|
||||
/* *************************************************************************/
|
||||
TEST( SmartProjectionCameraFactor, Cal3Bundler2 ) {
|
||||
TEST(SmartProjectionFactor, Cal3Bundler2 ) {
|
||||
|
||||
using namespace bundler;
|
||||
|
||||
|
|
@ -571,7 +563,7 @@ TEST( SmartProjectionCameraFactor, Cal3Bundler2 ) {
|
|||
}
|
||||
|
||||
/* *************************************************************************/
|
||||
TEST( SmartProjectionCameraFactor, noiselessBundler ) {
|
||||
TEST(SmartProjectionFactor, noiselessBundler ) {
|
||||
|
||||
using namespace bundler;
|
||||
Values values;
|
||||
|
|
@ -599,7 +591,7 @@ TEST( SmartProjectionCameraFactor, noiselessBundler ) {
|
|||
}
|
||||
|
||||
/* *************************************************************************/
|
||||
TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor ) {
|
||||
TEST(SmartProjectionFactor, comparisonGeneralSfMFactor ) {
|
||||
|
||||
using namespace bundler;
|
||||
Values values;
|
||||
|
|
@ -638,7 +630,7 @@ TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor ) {
|
|||
}
|
||||
|
||||
/* *************************************************************************/
|
||||
TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor1 ) {
|
||||
TEST(SmartProjectionFactor, comparisonGeneralSfMFactor1 ) {
|
||||
|
||||
using namespace bundler;
|
||||
Values values;
|
||||
|
|
@ -682,7 +674,7 @@ TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor1 ) {
|
|||
|
||||
/* *************************************************************************/
|
||||
// Have to think about how to compare multiplyHessianAdd in generalSfMFactor and smartFactors
|
||||
//TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor2 ){
|
||||
//TEST(SmartProjectionFactor, comparisonGeneralSfMFactor2 ){
|
||||
//
|
||||
// Values values;
|
||||
// values.insert(c1, level_camera);
|
||||
|
|
@ -730,7 +722,7 @@ TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor1 ) {
|
|||
// EXPECT(assert_equal(yActual,yExpected, 1e-7));
|
||||
//}
|
||||
/* *************************************************************************/
|
||||
TEST( SmartProjectionCameraFactor, computeImplicitJacobian ) {
|
||||
TEST(SmartProjectionFactor, computeImplicitJacobian ) {
|
||||
|
||||
using namespace bundler;
|
||||
Values values;
|
||||
|
|
@ -768,7 +760,7 @@ TEST( SmartProjectionCameraFactor, computeImplicitJacobian ) {
|
|||
}
|
||||
|
||||
/* *************************************************************************/
|
||||
TEST( SmartProjectionCameraFactor, implicitJacobianFactor ) {
|
||||
TEST(SmartProjectionFactor, implicitJacobianFactor ) {
|
||||
|
||||
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::SharedDiagonal, "gtsam_SharedDiagonal");
|
||||
|
||||
TEST( SmartProjectionCameraFactor, serialize) {
|
||||
TEST(SmartProjectionFactor, serialize) {
|
||||
using namespace vanilla;
|
||||
using namespace gtsam::serializationTestHelpers;
|
||||
SmartFactor factor(unit2);
|
||||
|
|
|
|||
|
|
@ -189,9 +189,7 @@ TEST( SmartProjectionPoseFactor, noisy ) {
|
|||
measurements.push_back(level_uv);
|
||||
measurements.push_back(level_uv_right);
|
||||
|
||||
KeyVector views;
|
||||
views.push_back(x1);
|
||||
views.push_back(x2);
|
||||
KeyVector views {x1, x2};
|
||||
|
||||
factor2->add(measurements, views);
|
||||
double actualError2 = factor2->error(values);
|
||||
|
|
@ -212,9 +210,7 @@ TEST(SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform) {
|
|||
Pose3 wTb3 = cam3.pose() * sensor_T_body;
|
||||
|
||||
// three landmarks ~5 meters infront of camera
|
||||
Point3 landmark1(5, 0.5, 1.2);
|
||||
Point3 landmark2(5, -0.5, 1.2);
|
||||
Point3 landmark3(5, 0, 3.0);
|
||||
Point3 landmark1(5, 0.5, 1.2), landmark2(5, -0.5, 1.2), landmark3(5, 0, 3.0);
|
||||
|
||||
Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
|
||||
|
||||
|
|
@ -224,10 +220,7 @@ TEST(SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform) {
|
|||
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
|
||||
|
||||
// Create smart factors
|
||||
KeyVector views;
|
||||
views.push_back(x1);
|
||||
views.push_back(x2);
|
||||
views.push_back(x3);
|
||||
KeyVector views {x1, x2, x3};
|
||||
|
||||
SmartProjectionParams params;
|
||||
params.setRankTolerance(1.0);
|
||||
|
|
@ -360,9 +353,7 @@ TEST( SmartProjectionPoseFactor, Factors ) {
|
|||
measurements_cam1.push_back(cam2.project(landmark1));
|
||||
|
||||
// Create smart factors
|
||||
KeyVector views;
|
||||
views.push_back(x1);
|
||||
views.push_back(x2);
|
||||
KeyVector views {x1, x2};
|
||||
|
||||
SmartFactor::shared_ptr smartFactor1 = boost::make_shared<SmartFactor>(model, sharedK);
|
||||
smartFactor1->add(measurements_cam1, views);
|
||||
|
|
@ -510,10 +501,7 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) {
|
|||
|
||||
using namespace vanillaPose;
|
||||
|
||||
KeyVector views;
|
||||
views.push_back(x1);
|
||||
views.push_back(x2);
|
||||
views.push_back(x3);
|
||||
KeyVector views {x1, x2, x3};
|
||||
|
||||
Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
|
||||
|
||||
|
|
@ -567,10 +555,7 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ) {
|
|||
|
||||
using namespace vanillaPose;
|
||||
|
||||
KeyVector views;
|
||||
views.push_back(x1);
|
||||
views.push_back(x2);
|
||||
views.push_back(x3);
|
||||
KeyVector views {x1, x2, x3};
|
||||
|
||||
Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
|
||||
|
||||
|
|
@ -628,10 +613,7 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ) {
|
|||
|
||||
double excludeLandmarksFutherThanDist = 2;
|
||||
|
||||
KeyVector views;
|
||||
views.push_back(x1);
|
||||
views.push_back(x2);
|
||||
views.push_back(x3);
|
||||
KeyVector views {x1, x2, x3};
|
||||
|
||||
Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
|
||||
|
||||
|
|
@ -691,10 +673,7 @@ TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) {
|
|||
double excludeLandmarksFutherThanDist = 1e10;
|
||||
double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error
|
||||
|
||||
KeyVector views;
|
||||
views.push_back(x1);
|
||||
views.push_back(x2);
|
||||
views.push_back(x3);
|
||||
KeyVector views {x1, x2, x3};
|
||||
|
||||
// add fourth landmark
|
||||
Point3 landmark4(5, -0.5, 1);
|
||||
|
|
@ -757,10 +736,7 @@ TEST( SmartProjectionPoseFactor, jacobianQ ) {
|
|||
|
||||
using namespace vanillaPose;
|
||||
|
||||
KeyVector views;
|
||||
views.push_back(x1);
|
||||
views.push_back(x2);
|
||||
views.push_back(x3);
|
||||
KeyVector views {x1, x2, x3};
|
||||
|
||||
Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
|
||||
|
||||
|
|
@ -811,10 +787,7 @@ TEST( SmartProjectionPoseFactor, 3poses_projection_factor ) {
|
|||
|
||||
using namespace vanillaPose2;
|
||||
|
||||
KeyVector views;
|
||||
views.push_back(x1);
|
||||
views.push_back(x2);
|
||||
views.push_back(x3);
|
||||
KeyVector views {x1, x2, x3};
|
||||
|
||||
typedef GenericProjectionFactor<Pose3, Point3> ProjectionFactor;
|
||||
NonlinearFactorGraph graph;
|
||||
|
|
@ -859,10 +832,7 @@ TEST( SmartProjectionPoseFactor, 3poses_projection_factor ) {
|
|||
/* *************************************************************************/
|
||||
TEST( SmartProjectionPoseFactor, CheckHessian) {
|
||||
|
||||
KeyVector views;
|
||||
views.push_back(x1);
|
||||
views.push_back(x2);
|
||||
views.push_back(x3);
|
||||
KeyVector views {x1, x2, x3};
|
||||
|
||||
using namespace vanillaPose;
|
||||
|
||||
|
|
@ -945,10 +915,7 @@ TEST( SmartProjectionPoseFactor, CheckHessian) {
|
|||
TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_factor ) {
|
||||
using namespace vanillaPose2;
|
||||
|
||||
KeyVector views;
|
||||
views.push_back(x1);
|
||||
views.push_back(x2);
|
||||
views.push_back(x3);
|
||||
KeyVector views {x1, x2, x3};
|
||||
|
||||
// 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));
|
||||
|
|
@ -1004,10 +971,7 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor )
|
|||
// this test considers a condition in which the cheirality constraint is triggered
|
||||
using namespace vanillaPose;
|
||||
|
||||
KeyVector views;
|
||||
views.push_back(x1);
|
||||
views.push_back(x2);
|
||||
views.push_back(x3);
|
||||
KeyVector views {x1, x2, x3};
|
||||
|
||||
// Two different cameras, at the same position, but different rotations
|
||||
Pose3 pose2 = level_pose
|
||||
|
|
@ -1089,9 +1053,7 @@ TEST( SmartProjectionPoseFactor, Hessian ) {
|
|||
|
||||
using namespace vanillaPose2;
|
||||
|
||||
KeyVector views;
|
||||
views.push_back(x1);
|
||||
views.push_back(x2);
|
||||
KeyVector views {x1, x2};
|
||||
|
||||
// Project three landmarks into 2 cameras
|
||||
Point2 cam1_uv1 = cam1.project(landmark1);
|
||||
|
|
@ -1123,10 +1085,7 @@ TEST( SmartProjectionPoseFactor, HessianWithRotation ) {
|
|||
|
||||
using namespace vanillaPose;
|
||||
|
||||
KeyVector views;
|
||||
views.push_back(x1);
|
||||
views.push_back(x2);
|
||||
views.push_back(x3);
|
||||
KeyVector views {x1, x2, x3};
|
||||
|
||||
Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
|
||||
|
||||
|
|
@ -1176,10 +1135,7 @@ TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ) {
|
|||
|
||||
using namespace vanillaPose2;
|
||||
|
||||
KeyVector views;
|
||||
views.push_back(x1);
|
||||
views.push_back(x2);
|
||||
views.push_back(x3);
|
||||
KeyVector views {x1, x2, x3};
|
||||
|
||||
// All cameras have the same pose so will be degenerate !
|
||||
Camera cam2(level_pose, sharedK2);
|
||||
|
|
@ -1250,10 +1206,7 @@ TEST( SmartProjectionPoseFactor, Cal3Bundler ) {
|
|||
projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
|
||||
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
|
||||
|
||||
KeyVector views;
|
||||
views.push_back(x1);
|
||||
views.push_back(x2);
|
||||
views.push_back(x3);
|
||||
KeyVector views {x1, x2, x3};
|
||||
|
||||
SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedBundlerK));
|
||||
smartFactor1->add(measurements_cam1, views);
|
||||
|
|
@ -1299,10 +1252,7 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) {
|
|||
|
||||
using namespace bundlerPose;
|
||||
|
||||
KeyVector views;
|
||||
views.push_back(x1);
|
||||
views.push_back(x2);
|
||||
views.push_back(x3);
|
||||
KeyVector views {x1, x2, x3};
|
||||
|
||||
// Two different cameras
|
||||
Pose3 pose2 = level_pose
|
||||
|
|
|
|||
Loading…
Reference in New Issue