diff --git a/gtsam/slam/SmartProjectionPoseFactor.h b/gtsam/slam/SmartProjectionPoseFactor.h index 7a5137bfc..e86f8e555 100644 --- a/gtsam/slam/SmartProjectionPoseFactor.h +++ b/gtsam/slam/SmartProjectionPoseFactor.h @@ -48,8 +48,8 @@ private: protected: + boost::shared_ptr K_; ///< calibration object (one for all cameras) boost::optional body_P_sensor_; ///< Pose of the camera in the body frame - std::vector > sharedKs_; ///< shared pointer to calibration object (one for each camera) public: @@ -65,13 +65,13 @@ public: * @param enableEPI if set to true linear triangulation is refined with embedded LM iterations * @param body_P_sensor is the transform from sensor to body frame (default identity) */ - SmartProjectionPoseFactor(const double rankTol = 1, + SmartProjectionPoseFactor(const boost::shared_ptr K, const double rankTol = 1, const double linThreshold = -1, const DegeneracyMode manageDegeneracy = IGNORE_DEGENERACY, const bool enableEPI = false, boost::optional body_P_sensor = boost::none, LinearizationMode linearizeTo = HESSIAN, double landmarkDistanceThreshold = 1e10, double dynamicOutlierRejectionThreshold = -1) : - Base(linearizeTo, rankTol, manageDegeneracy, enableEPI, - landmarkDistanceThreshold, dynamicOutlierRejectionThreshold), body_P_sensor_(body_P_sensor) {} + Base(linearizeTo, rankTol, manageDegeneracy, enableEPI, landmarkDistanceThreshold, + dynamicOutlierRejectionThreshold), K_(K), body_P_sensor_(body_P_sensor) {} /** Virtual destructor */ virtual ~SmartProjectionPoseFactor() {} @@ -128,8 +128,8 @@ public: } /** return calibration shared pointers */ - inline const std::vector > calibration() const { - return sharedKs_; + inline const boost::shared_ptr calibration() const { + return K_; } Pose3 body_P_sensor() const{ @@ -146,7 +146,7 @@ private: template void serialize(ARCHIVE & ar, const unsigned int version) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - ar & BOOST_SERIALIZATION_NVP(sharedKs_); + ar & BOOST_SERIALIZATION_NVP(K_); } }; // end of class declaration diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index 790e85dfb..14e15f6c9 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -53,36 +53,36 @@ LevenbergMarquardtParams params; /* ************************************************************************* */ TEST( SmartProjectionPoseFactor, Constructor) { using namespace vanillaPose; - SmartFactor::shared_ptr factor1(new SmartFactor()); + SmartFactor::shared_ptr factor1(new SmartFactor(sharedK)); } /* ************************************************************************* */ TEST( SmartProjectionPoseFactor, Constructor2) { using namespace vanillaPose; - SmartFactor factor1(gtsam::HESSIAN, rankTol); + SmartFactor factor1(sharedK, rankTol); } /* ************************************************************************* */ TEST( SmartProjectionPoseFactor, Constructor3) { using namespace vanillaPose; - SmartFactor::shared_ptr factor1(new SmartFactor()); + SmartFactor::shared_ptr factor1(new SmartFactor(sharedK)); factor1->add(measurement1, x1, model); } /* ************************************************************************* */ TEST( SmartProjectionPoseFactor, Constructor4) { using namespace vanillaPose; - SmartFactor factor1(gtsam::HESSIAN, rankTol); + SmartFactor factor1(sharedK, rankTol); factor1.add(measurement1, x1, model); } /* ************************************************************************* */ TEST( SmartProjectionPoseFactor, Equals ) { using namespace vanillaPose; - SmartFactor::shared_ptr factor1(new SmartFactor()); + SmartFactor::shared_ptr factor1(new SmartFactor(sharedK)); factor1->add(measurement1, x1, model); - SmartFactor::shared_ptr factor2(new SmartFactor()); + SmartFactor::shared_ptr factor2(new SmartFactor(sharedK)); factor2->add(measurement1, x1, model); CHECK(assert_equal(*factor1, *factor2)); @@ -97,7 +97,7 @@ TEST_UNSAFE( SmartProjectionPoseFactor, noiseless ) { Point2 level_uv = level_camera.project(landmark1); Point2 level_uv_right = level_camera_right.project(landmark1); - SmartFactor factor; + SmartFactor factor(sharedK); factor.add(level_uv, x1, model); factor.add(level_uv_right, x2, model); @@ -162,13 +162,13 @@ TEST( SmartProjectionPoseFactor, noisy ) { Point3(0.5, 0.1, 0.3)); values.insert(x2, Camera(pose_right.compose(noise_pose), sharedK)); - SmartFactor::shared_ptr factor(new SmartFactor()); + SmartFactor::shared_ptr factor(new SmartFactor((sharedK))); factor->add(level_uv, x1, model); factor->add(level_uv_right, x2, model); double actualError1 = factor->error(values); - SmartFactor::shared_ptr factor2(new SmartFactor()); + SmartFactor::shared_ptr factor2(new SmartFactor((sharedK))); vector measurements; measurements.push_back(level_uv); measurements.push_back(level_uv_right); @@ -186,6 +186,96 @@ TEST( SmartProjectionPoseFactor, noisy ) { DOUBLES_EQUAL(actualError1, actualError2, 1e-7); } +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){ + // make a realistic calibration matrix + double fov = 60; // degrees + size_t w=640,h=480; + + Cal3_S2::shared_ptr K(new Cal3_S2(fov,w,h)); + + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 cameraPose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); // body poses + Pose3 cameraPose2 = cameraPose1 * Pose3(Rot3(), Point3(1,0,0)); + Pose3 cameraPose3 = cameraPose1 * Pose3(Rot3(), Point3(0,-1,0)); + + SimpleCamera cam1(cameraPose1, *K); // with camera poses + SimpleCamera cam2(cameraPose2, *K); + SimpleCamera cam3(cameraPose3, *K); + + // create arbitrary body_Pose_sensor (transforms from sensor to body) + Pose3 sensor_to_body = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(1, 1, 1)); // Pose3(); // + + // These are the poses we want to estimate, from camera measurements + Pose3 bodyPose1 = cameraPose1.compose(sensor_to_body.inverse()); + Pose3 bodyPose2 = cameraPose2.compose(sensor_to_body.inverse()); + Pose3 bodyPose3 = cameraPose3.compose(sensor_to_body.inverse()); + + // 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); + + vector measurements_cam1, measurements_cam2, measurements_cam3; + + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + + // Create smart factors + std::vector views; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); + + bool enableEPI = false; + + SmartProjectionPoseFactor smartFactor1(K, 1.0, -1.0, gtsam::IGNORE_DEGENERACY, enableEPI, sensor_to_body); + smartFactor1.add(measurements_cam1, views, model); + + SmartProjectionPoseFactor smartFactor2(K, 1.0, -1.0, gtsam::IGNORE_DEGENERACY, enableEPI, sensor_to_body); + smartFactor2.add(measurements_cam2, views, model); + + SmartProjectionPoseFactor smartFactor3(K, 1.0, -1.0, gtsam::IGNORE_DEGENERACY, enableEPI, sensor_to_body); + smartFactor3.add(measurements_cam3, views, model); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + // Put all factors in factor graph, adding priors + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.push_back(PriorFactor(x1, bodyPose1, noisePrior)); + graph.push_back(PriorFactor(x2, bodyPose2, noisePrior)); + + // Check errors at ground truth poses + Values gtValues; + gtValues.insert(x1, bodyPose1); + gtValues.insert(x2, bodyPose2); + gtValues.insert(x3, bodyPose3); + double actualError = graph.error(gtValues); + double expectedError = 0.0; + DOUBLES_EQUAL(expectedError, actualError, 1e-7) + + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); + Values values; + values.insert(x1, bodyPose1); + values.insert(x2, bodyPose2); + // initialize third pose with some noise, we expect it to move back to original pose3 + values.insert(x3, bodyPose3*noise_pose); + + LevenbergMarquardtParams params; + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, params); + result = optimizer.optimize(); + + // result.print("results of 3 camera, 3 landmark optimization \n"); + // result.at(x3).print("Smart: Pose3 after optimization: "); + EXPECT(assert_equal(bodyPose3,result.at(x3))); +} + /* *************************************************************************/ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) { @@ -277,7 +367,7 @@ TEST( SmartProjectionPoseFactor, Factors ) { views.push_back(x1); views.push_back(x2); - SmartFactor::shared_ptr smartFactor1 = boost::make_shared(); + SmartFactor::shared_ptr smartFactor1 = boost::make_shared(sharedK); smartFactor1->add(measurements_cam1, views, model); SmartFactor::Cameras cameras; @@ -435,13 +525,13 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) { projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - SmartFactor::shared_ptr smartFactor1(new SmartFactor()); + SmartFactor::shared_ptr smartFactor1(new SmartFactor(sharedK)); smartFactor1->add(measurements_cam1, views, model); - SmartFactor::shared_ptr smartFactor2(new SmartFactor()); + SmartFactor::shared_ptr smartFactor2(new SmartFactor(sharedK)); smartFactor2->add(measurements_cam2, views, model); - SmartFactor::shared_ptr smartFactor3(new SmartFactor()); + SmartFactor::shared_ptr smartFactor3(new SmartFactor(sharedK)); smartFactor3->add(measurements_cam3, views, model); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -495,15 +585,15 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ) { projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); SmartFactor::shared_ptr smartFactor1( - new SmartFactor(gtsam::JACOBIAN_SVD)); + new SmartFactor(sharedK, 1, -1, gtsam::IGNORE_DEGENERACY, false, Pose3(), gtsam::JACOBIAN_SVD)); smartFactor1->add(measurements_cam1, views, model); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(gtsam::JACOBIAN_SVD)); + new SmartFactor(sharedK, 1, -1, gtsam::IGNORE_DEGENERACY, false, Pose3(), gtsam::JACOBIAN_SVD)); smartFactor2->add(measurements_cam2, views, model); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(gtsam::JACOBIAN_SVD)); + new SmartFactor(sharedK, 1, -1, gtsam::IGNORE_DEGENERACY, false, Pose3(), gtsam::JACOBIAN_SVD)); smartFactor3->add(measurements_cam3, views, model); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -549,17 +639,17 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ) { projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); SmartFactor::shared_ptr smartFactor1( - new SmartFactor(1, -1, gtsam::IGNORE_DEGENERACY, false, Pose3(), + new SmartFactor(sharedK, 1, -1, gtsam::IGNORE_DEGENERACY, false, Pose3(), gtsam::JACOBIAN_SVD, excludeLandmarksFutherThanDist)); smartFactor1->add(measurements_cam1, views, model); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(1, -1, gtsam::IGNORE_DEGENERACY, false, Pose3(), + new SmartFactor(sharedK, 1, -1, gtsam::IGNORE_DEGENERACY, false, Pose3(), gtsam::JACOBIAN_SVD, excludeLandmarksFutherThanDist)); smartFactor2->add(measurements_cam2, views, model); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(1, -1, gtsam::IGNORE_DEGENERACY, false, Pose3(), + new SmartFactor(sharedK, 1, -1, gtsam::IGNORE_DEGENERACY, false, Pose3(), gtsam::JACOBIAN_SVD, excludeLandmarksFutherThanDist)); smartFactor3->add(measurements_cam3, views, model); @@ -614,22 +704,22 @@ TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) { measurements_cam4.at(0) = measurements_cam4.at(0) + Point2(10, 10); // add outlier SmartFactor::shared_ptr smartFactor1( - new SmartFactor(1, -1, gtsam::IGNORE_DEGENERACY, false, Pose3(), + new SmartFactor(sharedK, 1, -1, gtsam::IGNORE_DEGENERACY, false, Pose3(), gtsam::JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); smartFactor1->add(measurements_cam1, views, model); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(1, -1, gtsam::IGNORE_DEGENERACY, false, Pose3(), + new SmartFactor(sharedK, 1, -1, gtsam::IGNORE_DEGENERACY, false, Pose3(), gtsam::JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); smartFactor2->add(measurements_cam2, views, model); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(1, -1, gtsam::IGNORE_DEGENERACY, false, Pose3(), + new SmartFactor(sharedK, 1, -1, gtsam::IGNORE_DEGENERACY, false, Pose3(), gtsam::JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); smartFactor3->add(measurements_cam3, views, model); SmartFactor::shared_ptr smartFactor4( - new SmartFactor(1, -1, gtsam::IGNORE_DEGENERACY, false, Pose3(), + new SmartFactor(sharedK, 1, -1, gtsam::IGNORE_DEGENERACY, false, Pose3(), gtsam::JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); smartFactor4->add(measurements_cam4, views, model); @@ -673,15 +763,19 @@ TEST( SmartProjectionPoseFactor, jacobianQ ) { projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); SmartFactor::shared_ptr smartFactor1( - new SmartFactor(gtsam::JACOBIAN_Q)); + new SmartFactor(sharedK, 1, -1, gtsam::IGNORE_DEGENERACY, + false, Pose3(), gtsam::JACOBIAN_Q)); + smartFactor1->add(measurements_cam1, views, model); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(gtsam::JACOBIAN_Q)); + new SmartFactor(sharedK, 1, -1, gtsam::IGNORE_DEGENERACY, + false, Pose3(), gtsam::JACOBIAN_Q)); smartFactor2->add(measurements_cam2, views, model); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(gtsam::JACOBIAN_Q)); + new SmartFactor(sharedK, 1, -1, gtsam::IGNORE_DEGENERACY, + false, Pose3(), gtsam::JACOBIAN_Q)); smartFactor3->add(measurements_cam3, views, model); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -792,15 +886,15 @@ TEST( SmartProjectionPoseFactor, CheckHessian) { double rankTol = 10; SmartFactor::shared_ptr smartFactor1( - new SmartFactor(gtsam::HESSIAN, rankTol)); + new SmartFactor(sharedK, rankTol)); // HESSIAN, by default smartFactor1->add(measurements_cam1, views, model); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(gtsam::HESSIAN, rankTol)); + new SmartFactor(sharedK, rankTol)); // HESSIAN, by default smartFactor2->add(measurements_cam2, views, model); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(gtsam::HESSIAN, rankTol)); + new SmartFactor(sharedK, rankTol)); // HESSIAN, by default smartFactor3->add(measurements_cam3, views, model); NonlinearFactorGraph graph; @@ -954,18 +1048,15 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ) double rankTol = 10; SmartFactor::shared_ptr smartFactor1( - new SmartFactor(gtsam::HESSIAN, rankTol, - gtsam::ZERO_ON_DEGENERACY)); + new SmartFactor(sharedK, rankTol, -1, gtsam::ZERO_ON_DEGENERACY)); smartFactor1->add(measurements_cam1, views, model); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(gtsam::HESSIAN, rankTol, - gtsam::ZERO_ON_DEGENERACY)); + new SmartFactor(sharedK, rankTol, -1, gtsam::ZERO_ON_DEGENERACY)); smartFactor2->add(measurements_cam2, views, model); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(gtsam::HESSIAN, rankTol, - gtsam::ZERO_ON_DEGENERACY)); + new SmartFactor(sharedK, rankTol, -1, gtsam::ZERO_ON_DEGENERACY)); smartFactor3->add(measurements_cam3, views, model); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -1062,7 +1153,7 @@ TEST( SmartProjectionPoseFactor, HessianWithRotation ) { projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); - SmartFactor::shared_ptr smartFactorInstance(new SmartFactor()); + SmartFactor::shared_ptr smartFactorInstance(new SmartFactor(sharedK)); smartFactorInstance->add(measurements_cam1, views, model); Values values; @@ -1161,7 +1252,7 @@ TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ) { /* ************************************************************************* */ TEST( SmartProjectionPoseFactor, ConstructorWithCal3Bundler) { using namespace bundlerPose; - SmartFactor factor(gtsam::HESSIAN, rankTol); + SmartFactor factor(sharedBundlerK, rankTol, -1, gtsam::ZERO_ON_DEGENERACY, false, Pose3(), gtsam::HESSIAN); factor.add(measurement1, x1, model); } @@ -1185,13 +1276,13 @@ TEST( SmartProjectionPoseFactor, Cal3Bundler ) { views.push_back(x2); views.push_back(x3); - SmartFactor::shared_ptr smartFactor1(new SmartFactor()); + SmartFactor::shared_ptr smartFactor1(new SmartFactor(sharedBundlerK)); smartFactor1->add(measurements_cam1, views, model); - SmartFactor::shared_ptr smartFactor2(new SmartFactor()); + SmartFactor::shared_ptr smartFactor2(new SmartFactor(sharedBundlerK)); smartFactor2->add(measurements_cam2, views, model); - SmartFactor::shared_ptr smartFactor3(new SmartFactor()); + SmartFactor::shared_ptr smartFactor3(new SmartFactor(sharedBundlerK)); smartFactor3->add(measurements_cam3, views, model); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -1255,17 +1346,17 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) { double rankTol = 10; SmartFactor::shared_ptr smartFactor1( - new SmartFactor(rankTol, -1.0, gtsam::ZERO_ON_DEGENERACY, + new SmartFactor(sharedBundlerK, rankTol, -1.0, gtsam::ZERO_ON_DEGENERACY, false, Pose3(), gtsam::HESSIAN)); smartFactor1->add(measurements_cam1, views, model); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(rankTol, -1, gtsam::ZERO_ON_DEGENERACY, + new SmartFactor(sharedBundlerK, rankTol, -1, gtsam::ZERO_ON_DEGENERACY, false, Pose3(), gtsam::HESSIAN)); smartFactor2->add(measurements_cam2, views, model); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(rankTol, -1, gtsam::ZERO_ON_DEGENERACY, + new SmartFactor(sharedBundlerK, rankTol, -1, gtsam::ZERO_ON_DEGENERACY, false, Pose3(), gtsam::HESSIAN)); smartFactor3->add(measurements_cam3, views, model);