diff --git a/gtsam.h b/gtsam.h index 826f8472ec..0ac2d4ad12 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2499,6 +2499,9 @@ virtual class SmartProjectionPoseFactor: gtsam::NonlinearFactor { SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise, const CALIBRATION* K, const gtsam::Pose3& body_P_sensor); + SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise, + const CALIBRATION* K, + const gtsam::SmartProjectionParams& params); SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise, const CALIBRATION* K, const gtsam::Pose3& body_P_sensor, diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index cbeed77c50..15d632cda2 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -79,15 +79,15 @@ class SmartProjectionFactor: public SmartFactorBase { /** * Constructor - * @param body_P_sensor pose of the camera in the body frame - * @param params internal parameters of the smart factors + * @param sharedNoiseModel isotropic noise model for the 2D feature measurements + * @param params parameters for the smart projection factors */ - SmartProjectionFactor(const SharedNoiseModel& sharedNoiseModel, - const boost::optional body_P_sensor = boost::none, - const SmartProjectionParams& params = SmartProjectionParams()) : - Base(sharedNoiseModel, body_P_sensor), params_(params), // - result_(TriangulationResult::Degenerate()) { - } + SmartProjectionFactor( + const SharedNoiseModel& sharedNoiseModel, + const SmartProjectionParams& params = SmartProjectionParams()) + : Base(sharedNoiseModel), + params_(params), + result_(TriangulationResult::Degenerate()) {} /** Virtual destructor */ virtual ~SmartProjectionFactor() { @@ -443,7 +443,26 @@ class SmartProjectionFactor: public SmartFactorBase { /** return the farPoint state */ bool isFarPoint() const { return result_.farPoint(); } -private: +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 + /// @name Deprecated + /// @{ + // It does not make sense to optimize for a camera where the pose would not be + // the actual pose of the camera. An unfortunate consequence of deprecating + // this constructor means that we cannot optimize for calibration when the + // camera is offset from the body pose. That would need a new factor with + // (body) pose and calibration as variables. However, that use case is + // unlikely: when a global offset is know, calibration is typically known. + SmartProjectionFactor( + const SharedNoiseModel& sharedNoiseModel, + const boost::optional body_P_sensor, + const SmartProjectionParams& params = SmartProjectionParams()) + : Base(sharedNoiseModel, body_P_sensor), + params_(params), + result_(TriangulationResult::Degenerate()) {} + /// @} +#endif + + private: /// Serialization function friend class boost::serialization::access; diff --git a/gtsam/slam/SmartProjectionPoseFactor.h b/gtsam/slam/SmartProjectionPoseFactor.h index be15841957..b5be46258a 100644 --- a/gtsam/slam/SmartProjectionPoseFactor.h +++ b/gtsam/slam/SmartProjectionPoseFactor.h @@ -66,16 +66,31 @@ class SmartProjectionPoseFactor: public SmartProjectionFactor< /** * Constructor - * @param Isotropic measurement noise + * @param sharedNoiseModel isotropic noise model for the 2D feature measurements * @param K (fixed) calibration, assumed to be the same for all cameras - * @param body_P_sensor pose of the camera in the body frame - * @param params internal parameters of the smart factors + * @param params parameters for the smart projection factors */ - SmartProjectionPoseFactor(const SharedNoiseModel& sharedNoiseModel, + SmartProjectionPoseFactor( + const SharedNoiseModel& sharedNoiseModel, const boost::shared_ptr K, - const boost::optional body_P_sensor = boost::none, - const SmartProjectionParams& params = SmartProjectionParams()) : - Base(sharedNoiseModel, body_P_sensor, params), K_(K) { + const SmartProjectionParams& params = SmartProjectionParams()) + : Base(sharedNoiseModel, params), K_(K) { + } + + /** + * Constructor + * @param sharedNoiseModel isotropic noise model for the 2D feature measurements + * @param K (fixed) calibration, assumed to be the same for all cameras + * @param body_P_sensor pose of the camera in the body frame (optional) + * @param params parameters for the smart projection factors + */ + SmartProjectionPoseFactor( + const SharedNoiseModel& sharedNoiseModel, + const boost::shared_ptr K, + const boost::optional body_P_sensor, + const SmartProjectionParams& params = SmartProjectionParams()) + : SmartProjectionPoseFactor(sharedNoiseModel, K, params) { + this->body_P_sensor_ = body_P_sensor; } /** Virtual destructor */ diff --git a/gtsam/slam/tests/testSmartProjectionFactor.cpp b/gtsam/slam/tests/testSmartProjectionFactor.cpp index 336c7d2ea4..c6d1a5b2cf 100644 --- a/gtsam/slam/tests/testSmartProjectionFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionFactor.cpp @@ -71,7 +71,7 @@ TEST(SmartProjectionFactor, Constructor) { TEST(SmartProjectionFactor, Constructor2) { using namespace vanilla; params.setRankTolerance(rankTol); - SmartFactor factor1(unit2, boost::none, params); + SmartFactor factor1(unit2, params); } /* ************************************************************************* */ @@ -85,7 +85,7 @@ TEST(SmartProjectionFactor, Constructor3) { TEST(SmartProjectionFactor, Constructor4) { using namespace vanilla; params.setRankTolerance(rankTol); - SmartFactor factor1(unit2, boost::none, params); + SmartFactor factor1(unit2, params); factor1.add(measurement1, c1); } @@ -777,7 +777,7 @@ TEST(SmartProjectionFactor, implicitJacobianFactor ) { params.setEnableEPI(useEPI); SmartFactor::shared_ptr explicitFactor( - new SmartFactor(unit2, boost::none, params)); + new SmartFactor(unit2, params)); explicitFactor->add(level_uv, c1); explicitFactor->add(level_uv_right, c2); @@ -789,7 +789,7 @@ TEST(SmartProjectionFactor, implicitJacobianFactor ) { // Implicit Schur version params.setLinearizationMode(gtsam::IMPLICIT_SCHUR); SmartFactor::shared_ptr implicitFactor( - new SmartFactor(unit2, boost::none, params)); + new SmartFactor(unit2, params)); implicitFactor->add(level_uv, c1); implicitFactor->add(level_uv_right, c2); GaussianFactor::shared_ptr gaussianImplicitSchurFactor = @@ -810,65 +810,6 @@ TEST(SmartProjectionFactor, implicitJacobianFactor ) { EXPECT(assert_equal(yActual, yExpected, 1e-7)); } - -/* *************************************************************************/ -TEST(SmartProjectionFactor, smartFactorWithSensorBodyTransform) { - using namespace vanilla; - - // create arbitrary body_T_sensor (transforms from sensor to body) - Pose3 body_T_sensor = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(1, 1, 1)); - - // These are the poses we want to estimate, from camera measurements - const Pose3 sensor_T_body = body_T_sensor.inverse(); - Pose3 wTb1 = cam1.pose() * sensor_T_body; - Pose3 wTb2 = cam2.pose() * sensor_T_body; - Pose3 wTb3 = cam3.pose() * sensor_T_body; - - // three landmarks ~5 meters infront of camera - 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; - - // 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 - KeyVector views {1, 2, 3}; - - SmartProjectionParams params; - params.setRankTolerance(1.0); - params.setDegeneracyMode(IGNORE_DEGENERACY); - params.setEnableEPI(false); - - SmartFactor smartFactor1(unit2, body_T_sensor, params); - smartFactor1.add(measurements_cam1, views); - - SmartFactor smartFactor2(unit2, body_T_sensor, params); - smartFactor2.add(measurements_cam2, views); - - SmartFactor smartFactor3(unit2, body_T_sensor, params); - smartFactor3.add(measurements_cam3, views); - - 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); - - // Check errors at ground truth poses - Values gtValues; - gtValues.insert(1, cam1); - gtValues.insert(2, cam2); - gtValues.insert(3, cam3); - double actualError = graph.error(gtValues); - double expectedError = 0.0; - DOUBLES_EQUAL(expectedError, actualError, 1e-7); -} - /* ************************************************************************* */ BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained"); BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal"); diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index 0197ba1b07..f833941bcc 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -62,7 +62,7 @@ TEST( SmartProjectionPoseFactor, Constructor2) { using namespace vanillaPose; SmartProjectionParams params; params.setRankTolerance(rankTol); - SmartFactor factor1(model, sharedK, boost::none, params); + SmartFactor factor1(model, sharedK, params); } /* ************************************************************************* */ @@ -77,7 +77,7 @@ TEST( SmartProjectionPoseFactor, Constructor4) { using namespace vanillaPose; SmartProjectionParams params; params.setRankTolerance(rankTol); - SmartFactor factor1(model, sharedK, boost::none, params); + SmartFactor factor1(model, sharedK, params); factor1.add(measurement1, x1); } @@ -569,18 +569,18 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ) { params.setLinearizationMode(gtsam::JACOBIAN_SVD); params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); params.setEnableEPI(false); - SmartFactor factor1(model, sharedK, boost::none, params); + SmartFactor factor1(model, sharedK, params); SmartFactor::shared_ptr smartFactor1( - new SmartFactor(model, sharedK, boost::none, params)); + new SmartFactor(model, sharedK, params)); smartFactor1->add(measurements_cam1, views); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(model, sharedK, boost::none, params)); + new SmartFactor(model, sharedK, params)); smartFactor2->add(measurements_cam2, views); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(model, sharedK, boost::none, params)); + new SmartFactor(model, sharedK, params)); smartFactor3->add(measurements_cam3, views); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -630,15 +630,15 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ) { params.setEnableEPI(false); SmartFactor::shared_ptr smartFactor1( - new SmartFactor(model, sharedK, boost::none, params)); + new SmartFactor(model, sharedK, params)); smartFactor1->add(measurements_cam1, views); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(model, sharedK, boost::none, params)); + new SmartFactor(model, sharedK, params)); smartFactor2->add(measurements_cam2, views); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(model, sharedK, boost::none, params)); + new SmartFactor(model, sharedK, params)); smartFactor3->add(measurements_cam3, views); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -694,19 +694,19 @@ TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) { params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold); SmartFactor::shared_ptr smartFactor1( - new SmartFactor(model, sharedK, boost::none, params)); + new SmartFactor(model, sharedK, params)); smartFactor1->add(measurements_cam1, views); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(model, sharedK, boost::none, params)); + new SmartFactor(model, sharedK, params)); smartFactor2->add(measurements_cam2, views); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(model, sharedK, boost::none, params)); + new SmartFactor(model, sharedK, params)); smartFactor3->add(measurements_cam3, views); SmartFactor::shared_ptr smartFactor4( - new SmartFactor(model, sharedK, boost::none, params)); + new SmartFactor(model, sharedK, params)); smartFactor4->add(measurements_cam4, views); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -749,15 +749,15 @@ TEST( SmartProjectionPoseFactor, jacobianQ ) { params.setLinearizationMode(gtsam::JACOBIAN_Q); SmartFactor::shared_ptr smartFactor1( - new SmartFactor(model, sharedK, boost::none, params)); + new SmartFactor(model, sharedK, params)); smartFactor1->add(measurements_cam1, views); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(model, sharedK, boost::none, params)); + new SmartFactor(model, sharedK, params)); smartFactor2->add(measurements_cam2, views); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(model, sharedK, boost::none, params)); + new SmartFactor(model, sharedK, params)); smartFactor3->add(measurements_cam3, views); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -854,15 +854,15 @@ TEST( SmartProjectionPoseFactor, CheckHessian) { params.setRankTolerance(10); SmartFactor::shared_ptr smartFactor1( - new SmartFactor(model, sharedK, boost::none, params)); // HESSIAN, by default + new SmartFactor(model, sharedK, params)); // HESSIAN, by default smartFactor1->add(measurements_cam1, views); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(model, sharedK, boost::none, params)); // HESSIAN, by default + new SmartFactor(model, sharedK, params)); // HESSIAN, by default smartFactor2->add(measurements_cam2, views); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(model, sharedK, boost::none, params)); // HESSIAN, by default + new SmartFactor(model, sharedK, params)); // HESSIAN, by default smartFactor3->add(measurements_cam3, views); NonlinearFactorGraph graph; @@ -934,11 +934,11 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac params.setDegeneracyMode(gtsam::HANDLE_INFINITY); SmartFactor::shared_ptr smartFactor1( - new SmartFactor(model, sharedK2, boost::none, params)); + new SmartFactor(model, sharedK2, params)); smartFactor1->add(measurements_cam1, views); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(model, sharedK2, boost::none, params)); + new SmartFactor(model, sharedK2, params)); smartFactor2->add(measurements_cam2, views); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -992,15 +992,15 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ) params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); SmartFactor::shared_ptr smartFactor1( - new SmartFactor(model, sharedK, boost::none, params)); + new SmartFactor(model, sharedK, params)); smartFactor1->add(measurements_cam1, views); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(model, sharedK, boost::none, params)); + new SmartFactor(model, sharedK, params)); smartFactor2->add(measurements_cam2, views); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(model, sharedK, boost::none, params)); + new SmartFactor(model, sharedK, params)); smartFactor3->add(measurements_cam3, views); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -1187,7 +1187,7 @@ TEST( SmartProjectionPoseFactor, ConstructorWithCal3Bundler) { using namespace bundlerPose; SmartProjectionParams params; params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); - SmartFactor factor(model, sharedBundlerK, boost::none, params); + SmartFactor factor(model, sharedBundlerK, params); factor.add(measurement1, x1); } @@ -1276,15 +1276,15 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) { params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); SmartFactor::shared_ptr smartFactor1( - new SmartFactor(model, sharedBundlerK, boost::none, params)); + new SmartFactor(model, sharedBundlerK, params)); smartFactor1->add(measurements_cam1, views); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(model, sharedBundlerK, boost::none, params)); + new SmartFactor(model, sharedBundlerK, params)); smartFactor2->add(measurements_cam2, views); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(model, sharedBundlerK, boost::none, params)); + new SmartFactor(model, sharedBundlerK, params)); smartFactor3->add(measurements_cam3, views); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -1345,7 +1345,7 @@ TEST(SmartProjectionPoseFactor, serialize) { using namespace gtsam::serializationTestHelpers; SmartProjectionParams params; params.setRankTolerance(rankTol); - SmartFactor factor(model, sharedK, boost::none, params); + SmartFactor factor(model, sharedK, params); EXPECT(equalsObj(factor)); EXPECT(equalsXML(factor));