From d6f54475c3cbe41006cceed5685a8288a3f889bc Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 23 Feb 2015 12:43:43 +0100 Subject: [PATCH] BIG change: SmartFactorBase and SmartProjectionFactor now templated on CAMERA --- gtsam/slam/SmartFactorBase.h | 73 +-- gtsam/slam/SmartProjectionCameraFactor.h | 36 +- gtsam/slam/SmartProjectionFactor.h | 108 ++-- gtsam/slam/SmartProjectionPoseFactor.h | 33 +- .../tests/testSmartProjectionCameraFactor.cpp | 4 +- .../tests/testSmartProjectionPoseFactor.cpp | 488 ++++++++---------- .../slam/SmartStereoProjectionFactor.h | 48 +- 7 files changed, 352 insertions(+), 438 deletions(-) diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 10352405a..a184a7956 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -45,6 +45,9 @@ template class SmartFactorBase: public NonlinearFactor { private: + typedef NonlinearFactor Base; + typedef SmartFactorBase This; + typedef typename CAMERA::Measurement Z; /** * As of Feb 22, 2015, the noise model is the same for all measurements and @@ -56,9 +59,6 @@ private: protected: - // Figure out the measurement type - typedef typename CAMERA::Measurement Z; - /** * 2D measurement and noise model for each of the m views * We keep a copy of measurements for I/O and computing the error. @@ -66,19 +66,11 @@ protected: */ std::vector measured_; - /// shorthand for base class type - typedef NonlinearFactor Base; - - /// shorthand for this class - typedef SmartFactorBase This; - static const int ZDim = traits::dimension; ///< Measurement dimension static const int Dim = traits::dimension; ///< Camera dimension - // Definitions for blocks of F - typedef Eigen::Matrix Matrix2D; // F + // Definitions for block matrices used internally typedef Eigen::Matrix MatrixD2; // F' - typedef std::pair KeyMatrix2D; // Fblocks typedef Eigen::Matrix MatrixDD; // camera hessian block typedef Eigen::Matrix Matrix23; typedef Eigen::Matrix VectorD; @@ -105,6 +97,10 @@ protected: public: + // Definitions for blocks of F, externally visible + typedef Eigen::Matrix Matrix2D; // F + typedef std::pair KeyMatrix2D; // Fblocks + EIGEN_MAKE_ALIGNED_OPERATOR_NEW /// shorthand for a smart pointer to a factor @@ -256,57 +252,19 @@ public: } /** - * Compute whitenedError, returning only the derivative E, i.e., - * the stacked ZDim*3 derivatives of project with respect to the point. - * Assumes non-degenerate ! TODO explain this - */ - Vector reprojectionErrors(const Cameras& cameras, const Point3& point, - Matrix& E) const { - - // Project into CameraSet, calculating derivatives - std::vector predicted; - predicted = cameras.project2(point, boost::none, E); - - // if needed, whiten - size_t m = keys_.size(); - Vector b(ZDim * m); - for (size_t i = 0, row = 0; i < m; i++, row += ZDim) { - - // Calculate error - const Z& zi = measured_.at(i); - Vector bi = (zi - predicted[i]).vector(); - b.segment(row) = bi; - } - return b; - } - - /** - * Compute F, E, and optionally H, where F and E are the stacked derivatives - * with respect to the cameras, point, and calibration, respectively. - * The value of cameras/point are passed as parameters. - * Returns error vector b + * Compute reprojection errors and derivatives * TODO: the treatment of body_P_sensor_ is weird: the transformation * is applied in the caller, but the derivatives are computed here. */ Vector reprojectionErrors(const Cameras& cameras, const Point3& point, typename Cameras::FBlocks& F, Matrix& E) const { - // Project into CameraSet, calculating derivatives - std::vector predicted; - predicted = cameras.project2(point, F, E); + Vector b = cameras.reprojectionErrors(point, measured_, F, E); - // Calculate error and whiten derivatives if needed - size_t m = keys_.size(); - Vector b(ZDim * m); - for (size_t i = 0; i < m; i++) { - - // Calculate error - const Z& zi = measured_.at(i); - Vector bi = (zi - predicted[i]).vector(); - - // if we have a sensor offset, correct camera derivatives - if (body_P_sensor_) { - // TODO: no simpler way ?? + // Apply sensor chain rule if needed TODO: no simpler way ?? + if (body_P_sensor_) { + size_t m = keys_.size(); + for (size_t i = 0; i < m; i++) { const Pose3& pose_i = cameras[i].pose(); Pose3 w_Pose_body = pose_i.compose(body_P_sensor_->inverse()); Matrix66 J; @@ -314,6 +272,7 @@ public: F[i].leftCols(6) *= J; } } + return b; } @@ -344,7 +303,7 @@ public: /// Assumes non-degenerate ! void computeEP(Matrix& E, Matrix& P, const Cameras& cameras, const Point3& point) const { - reprojectionErrors(cameras, point, E); + cameras.reprojectionErrors(point, measured_, boost::none, E); P = PointCov(E); } diff --git a/gtsam/slam/SmartProjectionCameraFactor.h b/gtsam/slam/SmartProjectionCameraFactor.h index d5bdc2e84..8381c847e 100644 --- a/gtsam/slam/SmartProjectionCameraFactor.h +++ b/gtsam/slam/SmartProjectionCameraFactor.h @@ -25,23 +25,29 @@ namespace gtsam { /** * @addtogroup SLAM */ -template -class SmartProjectionCameraFactor: public SmartProjectionFactor { +template +class SmartProjectionCameraFactor: public SmartProjectionFactor< + PinholeCamera > { + +private: + typedef PinholeCamera Camera; + typedef SmartProjectionFactor Base; + typedef SmartProjectionCameraFactor This; + protected: + static const int Dim = traits::dimension; ///< CAMERA dimension + bool isImplicit_; public: - /// shorthand for base class type - typedef SmartProjectionFactor Base; - - /// shorthand for this class - typedef SmartProjectionCameraFactor This; - /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; + // A set of cameras + typedef CameraSet Cameras; + /** * Constructor * @param rankTol tolerance used to check if point triangulation is degenerate @@ -88,14 +94,14 @@ public: /// get the dimension of the factor (number of rows on linearization) virtual size_t dim() const { - return D * this->keys_.size(); // 6 for the pose and 3 for the calibration + return Dim * this->keys_.size(); // 6 for the pose and 3 for the calibration } /// Collect all cameras: important that in key order - typename Base::Cameras cameras(const Values& values) const { - typename Base::Cameras cameras; + Cameras cameras(const Values& values) const { + Cameras cameras; BOOST_FOREACH(const Key& k, this->keys_) - cameras.push_back(values.at(k)); + cameras.push_back(values.at(k)); return cameras; } @@ -109,19 +115,19 @@ public: } /// linearize returns a Hessianfactor that is an approximation of error(p) - virtual boost::shared_ptr > linearizeToHessian( + virtual boost::shared_ptr > linearizeToHessian( const Values& values, double lambda=0.0) const { return Base::createHessianFactor(cameras(values),lambda); } /// linearize returns a Hessianfactor that is an approximation of error(p) - virtual boost::shared_ptr > linearizeToImplicit( + virtual boost::shared_ptr > linearizeToImplicit( const Values& values, double lambda=0.0) const { return Base::createRegularImplicitSchurFactor(cameras(values),lambda); } /// linearize returns a Jacobianfactor that is an approximation of error(p) - virtual boost::shared_ptr > linearizeToJacobian( + virtual boost::shared_ptr > linearizeToJacobian( const Values& values, double lambda=0.0) const { return Base::createJacobianQFactor(cameras(values),lambda); } diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index 8c11d7ec4..7a73a0c49 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -60,11 +60,17 @@ enum LinearizationMode { /** * SmartProjectionFactor: triangulates point and keeps an estimate of it around. */ -template -class SmartProjectionFactor: public SmartFactorBase, - D> { +template +class SmartProjectionFactor: public SmartFactorBase { + +private: + typedef SmartFactorBase Base; + typedef SmartProjectionFactor This; + protected: + static const int Dim = traits::dimension; ///< CAMERA dimension + // Some triangulation parameters const double rankTolerance_; ///< threshold to decide whether triangulation is degenerate_ const double retriangulationThreshold_; ///< threshold to decide whether to re-triangulate @@ -91,9 +97,6 @@ protected: /// shorthand for smart projection factor state variable typedef boost::shared_ptr SmartFactorStatePtr; - /// shorthand for base class type - typedef SmartFactorBase, D> Base; - double landmarkDistanceThreshold_; // if the landmark is triangulated at a // distance larger than that the factor is considered degenerate @@ -101,17 +104,13 @@ protected: // average reprojection error is smaller than this threshold after triangulation, // and the factor is disregarded if the error is large - /// shorthand for this class - typedef SmartProjectionFactor This; - public: /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; - /// shorthand for a pinhole camera - typedef PinholeCamera Camera; - typedef CameraSet Cameras; + /// shorthand for a set of cameras + typedef CameraSet Cameras; /** * Constructor @@ -243,7 +242,7 @@ public: // We triangulate the 3D position of the landmark try { // std::cout << "triangulatePoint3 i \n" << rankTolerance << std::endl; - point_ = triangulatePoint3(cameras, this->measured_, + point_ = triangulatePoint3(cameras, this->measured_, rankTolerance_, enableEPI_); degenerate_ = false; cheiralityException_ = false; @@ -251,7 +250,7 @@ public: // Check landmark distance and reprojection errors to avoid outliers double totalReprojError = 0.0; size_t i = 0; - BOOST_FOREACH(const Camera& camera, cameras) { + BOOST_FOREACH(const CAMERA& camera, cameras) { Point3 cameraTranslation = camera.pose().translation(); // we discard smart factors corresponding to points that are far away if (cameraTranslation.distance(point_) > landmarkDistanceThreshold_) { @@ -314,7 +313,7 @@ public: } /// linearize returns a Hessianfactor that is an approximation of error(p) - boost::shared_ptr > createHessianFactor( + boost::shared_ptr > createHessianFactor( const Cameras& cameras, const double lambda = 0.0) const { bool isDebug = false; @@ -338,10 +337,10 @@ public: && (this->cheiralityException_ || this->degenerate_))) { // std::cout << "In linearize: exception" << std::endl; BOOST_FOREACH(Matrix& m, Gs) - m = zeros(D, D); + m = zeros(Dim, Dim); BOOST_FOREACH(Vector& v, gs) - v = zero(D); - return boost::make_shared >(this->keys_, Gs, gs, + v = zero(Dim); + return boost::make_shared >(this->keys_, Gs, gs, 0.0); } @@ -366,7 +365,7 @@ public: << "something wrong in SmartProjectionHessianFactor: selective relinearization should be disabled" << std::endl; exit(1); - return boost::make_shared >(this->keys_, + return boost::make_shared >(this->keys_, this->state_->Gs, this->state_->gs, this->state_->f); } @@ -378,7 +377,7 @@ public: // Schur complement trick // Frank says: should be possible to do this more efficiently? // And we care, as in grouped factors this is called repeatedly - Matrix H(D * numKeys, D * numKeys); + Matrix H(Dim * numKeys, Dim * numKeys); Vector gs_vector; Matrix3 P = Base::PointCov(E, lambda); @@ -390,11 +389,11 @@ public: // Populate Gs and gs int GsCount2 = 0; for (DenseIndex i1 = 0; i1 < (DenseIndex) numKeys; i1++) { // for each camera - DenseIndex i1D = i1 * D; - gs.at(i1) = gs_vector.segment(i1D); + DenseIndex i1D = i1 * Dim; + gs.at(i1) = gs_vector.segment(i1D); for (DenseIndex i2 = 0; i2 < (DenseIndex) numKeys; i2++) { if (i2 >= i1) { - Gs.at(GsCount2) = H.block(i1D, i2 * D); + Gs.at(GsCount2) = H.block(i1D, i2 * Dim); GsCount2++; } } @@ -405,37 +404,37 @@ public: this->state_->gs = gs; this->state_->f = f; } - return boost::make_shared >(this->keys_, Gs, gs, f); + return boost::make_shared >(this->keys_, Gs, gs, f); } // create factor - boost::shared_ptr > createRegularImplicitSchurFactor( + boost::shared_ptr > createRegularImplicitSchurFactor( const Cameras& cameras, double lambda) const { if (triangulateForLinearize(cameras)) return Base::createRegularImplicitSchurFactor(cameras, point_, lambda); else - return boost::shared_ptr >(); + return boost::shared_ptr >(); } /// create factor - boost::shared_ptr > createJacobianQFactor( + boost::shared_ptr > createJacobianQFactor( const Cameras& cameras, double lambda) const { if (triangulateForLinearize(cameras)) return Base::createJacobianQFactor(cameras, point_, lambda); else - return boost::make_shared >(this->keys_); + return boost::make_shared >(this->keys_); } /// Create a factor, takes values - boost::shared_ptr > createJacobianQFactor( + boost::shared_ptr > createJacobianQFactor( const Values& values, double lambda) const { - Cameras myCameras; + Cameras cameras; // TODO triangulate twice ?? - bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras); + bool nonDegenerate = computeCamerasAndTriangulate(values, cameras); if (nonDegenerate) - return createJacobianQFactor(myCameras, lambda); + return createJacobianQFactor(cameras, lambda); else - return boost::make_shared >(this->keys_); + return boost::make_shared >(this->keys_); } /// different (faster) way to compute Jacobian factor @@ -444,20 +443,20 @@ public: if (triangulateForLinearize(cameras)) return Base::createJacobianSVDFactor(cameras, point_, lambda); else - return boost::make_shared >(this->keys_); + return boost::make_shared >(this->keys_); } /// Returns true if nonDegenerate bool computeCamerasAndTriangulate(const Values& values, - Cameras& myCameras) const { + Cameras& cameras) const { Values valuesFactor; // Select only the cameras BOOST_FOREACH(const Key key, this->keys_) valuesFactor.insert(key, values.at(key)); - myCameras = this->cameras(valuesFactor); - size_t nrCameras = this->triangulateSafe(myCameras); + cameras = this->cameras(valuesFactor); + size_t nrCameras = this->triangulateSafe(cameras); if (nrCameras < 2 || (!this->manageDegeneracy_ @@ -477,27 +476,22 @@ public: return true; } - /// Assumes non-degenerate ! - void computeEP(Matrix& E, Matrix& P, const Cameras& cameras) const { - return Base::computeEP(E, P, cameras, point_); - } - /// Takes values bool computeEP(Matrix& E, Matrix& P, const Values& values) const { - Cameras myCameras; - bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras); + Cameras cameras; + bool nonDegenerate = computeCamerasAndTriangulate(values, cameras); if (nonDegenerate) - computeEP(E, P, myCameras); + Base::computeEP(E, P, cameras, point_); return nonDegenerate; } /// Version that takes values, and creates the point bool computeJacobians(std::vector& Fblocks, Matrix& E, Vector& b, const Values& values) const { - Cameras myCameras; - bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras); + Cameras cameras; + bool nonDegenerate = computeCamerasAndTriangulate(values, cameras); if (nonDegenerate) - computeJacobians(Fblocks, E, b, myCameras); + computeJacobians(Fblocks, E, b, cameras); return nonDegenerate; } @@ -513,12 +507,13 @@ public: std::cout << "SmartProjectionFactor: Management of degeneracy is disabled - not ready to be used" << std::endl; - if (D > 6) { + if (Dim > 6) { std::cout << "Management of degeneracy is not yet ready when one also optimizes for the calibration " << std::endl; } + // TODO replace all this by Call to CameraSet int numKeys = this->keys_.size(); E = zeros(2 * numKeys, 2); b = zero(2 * numKeys); @@ -533,7 +528,6 @@ public: Vector bi = -(cameras[i].projectPointAtInfinity(this->point_, Fi, Ei) - this->measured_.at(i)).vector(); - this->noiseModel_->WhitenSystem(Fi, Ei, bi); f += bi.squaredNorm(); Fblocks.push_back(typename Base::KeyMatrix2D(this->keys_[i], Fi)); E.block<2, 2>(2 * i, 0) = Ei; @@ -549,10 +543,10 @@ public: /// takes values bool computeJacobiansSVD(std::vector& Fblocks, Matrix& Enull, Vector& b, const Values& values) const { - typename Base::Cameras myCameras; - double good = computeCamerasAndTriangulate(values, myCameras); + typename Base::Cameras cameras; + double good = computeCamerasAndTriangulate(values, cameras); if (good) - computeJacobiansSVD(Fblocks, Enull, b, myCameras); + computeJacobiansSVD(Fblocks, Enull, b, cameras); return true; } @@ -583,12 +577,12 @@ public: /// Calculate vector of re-projection errors, before applying noise model Vector reprojectionError(const Values& values) const { - Cameras myCameras; - bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras); + Cameras cameras; + bool nonDegenerate = computeCamerasAndTriangulate(values, cameras); if (nonDegenerate) - return reprojectionError(myCameras); + return reprojectionError(cameras); else - return zero(myCameras.size() * 2); + return zero(cameras.size() * 2); } /** diff --git a/gtsam/slam/SmartProjectionPoseFactor.h b/gtsam/slam/SmartProjectionPoseFactor.h index 70476ba3e..5cfd74913 100644 --- a/gtsam/slam/SmartProjectionPoseFactor.h +++ b/gtsam/slam/SmartProjectionPoseFactor.h @@ -38,21 +38,22 @@ namespace gtsam { * @addtogroup SLAM */ template -class SmartProjectionPoseFactor: public SmartProjectionFactor { +class SmartProjectionPoseFactor: public SmartProjectionFactor< + PinholePose > { + +private: + typedef PinholePose Camera; + typedef SmartProjectionFactor Base; + typedef SmartProjectionPoseFactor This; + protected: LinearizationMode linearizeTo_; ///< How to linearize the factor (HESSIAN, JACOBIAN_SVD, JACOBIAN_Q) - std::vector > K_all_; ///< shared pointer to calibration object (one for each camera) + std::vector > sharedKs_; ///< shared pointer to calibration object (one for each camera) public: - /// shorthand for base class type - typedef SmartProjectionFactor Base; - - /// shorthand for this class - typedef SmartProjectionPoseFactor This; - /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; @@ -87,7 +88,7 @@ public: const SharedNoiseModel noise_i, const boost::shared_ptr K_i) { Base::add(measured_i, poseKey_i, noise_i); - K_all_.push_back(K_i); + sharedKs_.push_back(K_i); } /** @@ -102,7 +103,7 @@ public: std::vector > Ks) { Base::add(measurements, poseKeys, noises); for (size_t i = 0; i < measurements.size(); i++) { - K_all_.push_back(Ks.at(i)); + sharedKs_.push_back(Ks.at(i)); } } @@ -117,7 +118,7 @@ public: const SharedNoiseModel noise, const boost::shared_ptr K) { for (size_t i = 0; i < measurements.size(); i++) { Base::add(measurements.at(i), poseKeys.at(i), noise); - K_all_.push_back(K); + sharedKs_.push_back(K); } } @@ -129,7 +130,7 @@ public: void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { std::cout << s << "SmartProjectionPoseFactor, z = \n "; - BOOST_FOREACH(const boost::shared_ptr& K, K_all_) + BOOST_FOREACH(const boost::shared_ptr& K, sharedKs_) K->print("calibration = "); Base::print("", keyFormatter); } @@ -155,7 +156,7 @@ public: if(Base::body_P_sensor_) pose = pose.compose(*(Base::body_P_sensor_)); - typename Base::Camera camera(pose, *K_all_[i++]); + Camera camera(pose, sharedKs_[i++]); cameras.push_back(camera); } return cameras; @@ -193,9 +194,9 @@ public: } } - /** return the calibration object */ + /** return calibration shared pointers */ inline const std::vector > calibration() const { - return K_all_; + return sharedKs_; } private: @@ -205,7 +206,7 @@ private: template void serialize(ARCHIVE & ar, const unsigned int version) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - ar & BOOST_SERIALIZATION_NVP(K_all_); + ar & BOOST_SERIALIZATION_NVP(sharedKs_); } }; // end of class declaration diff --git a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp index 88a0c0521..b9634025b 100644 --- a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp @@ -72,8 +72,8 @@ static Point2 measurement1(323.0, 240.0); static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); typedef PinholeCamera CameraCal3_S2; -typedef SmartProjectionCameraFactor SmartFactor; -typedef SmartProjectionCameraFactor SmartFactorBundler; +typedef SmartProjectionCameraFactor SmartFactor; +typedef SmartProjectionCameraFactor SmartFactorBundler; typedef GeneralSFMFactor SFMFactor; template diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index 9b36b645a..e1ef82b82 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -32,22 +32,23 @@ using namespace std; using namespace boost::assign; using namespace gtsam; -static bool isDebugTest = false; +static bool isDebugTest = true; // make a realistic calibration matrix static double fov = 60; // degrees static size_t w = 640, h = 480; -static Cal3_S2::shared_ptr K(new Cal3_S2(fov, w, h)); -static Cal3_S2::shared_ptr K2(new Cal3_S2(1500, 1200, 0, 640, 480)); -static boost::shared_ptr Kbundler( +static Cal3_S2::shared_ptr sharedK(new Cal3_S2(fov, w, h)); +static Cal3_S2::shared_ptr sharedK2(new Cal3_S2(1500, 1200, 0, 640, 480)); +static boost::shared_ptr sharedBundlerK( new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000)); -static double rankTol = 1.0; -static double linThreshold = -1.0; -static bool manageDegeneracy = true; +static const double rankTol = 1.0; +static const double linThreshold = -1.0; +static const bool manageDegeneracy = true; // Create a noise model for the pixel error -static SharedNoiseModel model(noiseModel::Isotropic::Sigma(2, 0.1)); +static const double sigma = 0.1; +static SharedNoiseModel model(noiseModel::Isotropic::Sigma(2, sigma)); // Convenience for named keys using symbol_shorthand::X; @@ -63,11 +64,28 @@ static Point2 measurement1(323.0, 240.0); static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); +typedef PinholePose Camera; typedef SmartProjectionPoseFactor SmartFactor; + +typedef PinholePose CameraBundler; typedef SmartProjectionPoseFactor SmartFactorBundler; -void projectToMultipleCameras(SimpleCamera cam1, SimpleCamera cam2, - SimpleCamera cam3, Point3 landmark, vector& measurements_cam) { +// create first camera. Looking along X-axis, 1 meter above ground plane (x-y) +Pose3 level_pose = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), + Point3(0, 0, 1)); +Camera level_camera(level_pose, sharedK2); + +// create second camera 1 meter to the right of first camera +Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1, 0, 0)); +Camera level_camera_right(level_pose_right, sharedK2); + +// landmarks ~5 meters in front of camera +Point3 landmark1(5, 0.5, 1.2); +Point3 landmark2(5, -0.5, 1.2); +Point3 landmark3(5, 0, 3.0); + +void projectToMultipleCameras(Camera cam1, Camera cam2, Camera cam3, + Point3 landmark, vector& measurements_cam) { Point2 cam1_uv1 = cam1.project(landmark); Point2 cam2_uv1 = cam2.project(landmark); @@ -90,13 +108,13 @@ TEST( SmartProjectionPoseFactor, Constructor2) { /* ************************************************************************* */ TEST( SmartProjectionPoseFactor, Constructor3) { SmartFactor::shared_ptr factor1(new SmartFactor()); - factor1->add(measurement1, poseKey1, model, K); + factor1->add(measurement1, poseKey1, model, sharedK); } /* ************************************************************************* */ TEST( SmartProjectionPoseFactor, Constructor4) { SmartFactor factor1(rankTol, linThreshold); - factor1.add(measurement1, poseKey1, model, K); + factor1.add(measurement1, poseKey1, model, sharedK); } /* ************************************************************************* */ @@ -105,16 +123,16 @@ TEST( SmartProjectionPoseFactor, ConstructorWithTransform) { bool enableEPI = false; SmartFactor factor1(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor1); - factor1.add(measurement1, poseKey1, model, K); + factor1.add(measurement1, poseKey1, model, sharedK); } /* ************************************************************************* */ TEST( SmartProjectionPoseFactor, Equals ) { SmartFactor::shared_ptr factor1(new SmartFactor()); - factor1->add(measurement1, poseKey1, model, K); + factor1->add(measurement1, poseKey1, model, sharedK); SmartFactor::shared_ptr factor2(new SmartFactor()); - factor2->add(measurement1, poseKey1, model, K); + factor2->add(measurement1, poseKey1, model, sharedK); CHECK(assert_equal(*factor1, *factor2)); } @@ -123,30 +141,18 @@ TEST( SmartProjectionPoseFactor, Equals ) { TEST_UNSAFE( SmartProjectionPoseFactor, noiseless ) { // cout << " ************************ SmartProjectionPoseFactor: noisy ****************************" << endl; - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 level_pose = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), - Point3(0, 0, 1)); - SimpleCamera level_camera(level_pose, *K2); - - // create second camera 1 meter to the right of first camera - Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1, 0, 0)); - SimpleCamera level_camera_right(level_pose_right, *K2); - - // landmark ~5 meters infront of camera - Point3 landmark(5, 0.5, 1.2); - // Project two landmarks into two cameras - Point2 level_uv = level_camera.project(landmark); - Point2 level_uv_right = level_camera_right.project(landmark); + Point2 level_uv = level_camera.project(landmark1); + Point2 level_uv_right = level_camera_right.project(landmark1); + + SmartFactor factor; + factor.add(level_uv, x1, model, sharedK); + factor.add(level_uv_right, x2, model, sharedK); Values values; values.insert(x1, level_pose); values.insert(x2, level_pose_right); - SmartFactor factor; - factor.add(level_uv, x1, model, K); - factor.add(level_uv_right, x2, model, K); - double actualError = factor.error(values); double expectedError = 0.0; EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); @@ -155,51 +161,44 @@ TEST_UNSAFE( SmartProjectionPoseFactor, noiseless ) { double actualError2 = factor.totalReprojectionError(cameras); EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1e-7); - // test vector of errors - //Vector actual = factor.unwhitenedError(values); - //EXPECT(assert_equal(zero(4),actual,1e-8)); - - // Check derivatives - // Calculate expected derivative for point (easiest to check) boost::function f = // boost::bind(&SmartFactor::whitenedErrors, factor, cameras, _1); boost::optional point = factor.point(); CHECK(point); - Matrix expectedE = numericalDerivative11(f, *point); + + // Note ! After refactor the noiseModel is only in the factors, not these matrices + Matrix expectedE = sigma * numericalDerivative11(f, *point); // Calculate using computeEP Matrix actualE, PointCov; - factor.computeEP(actualE, PointCov, cameras); + factor.computeEP(actualE, PointCov, values); EXPECT(assert_equal(expectedE, actualE, 1e-7)); - // Calculate using whitenedError - Matrix F, actualE2; - Vector actualErrors = factor.whitenedError(cameras, *point, F, actualE2); - EXPECT(assert_equal(expectedE, actualE2, 1e-7)); - EXPECT(assert_equal(f(*point), actualErrors, 1e-7)); + // Calculate using reprojectionErrors, note not yet divided by sigma ! + SmartFactor::Cameras::FBlocks F; + Matrix E; + Vector actualErrors = factor.reprojectionErrors(cameras, *point, F, E); + EXPECT(assert_equal(expectedE, E, 1e-7)); + + EXPECT(assert_equal(zero(4), actualErrors, 1e-7)); + + // Calculate using computeJacobians + Vector b; + vector Fblocks; + double actualError3 = factor.computeJacobians(Fblocks, E, b, cameras); + EXPECT(assert_equal(expectedE, E, 1e-7)); + EXPECT_DOUBLES_EQUAL(expectedError, actualError3, 1e-8); } /* *************************************************************************/ TEST( SmartProjectionPoseFactor, noisy ) { // cout << " ************************ SmartProjectionPoseFactor: noisy ****************************" << endl; - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 level_pose = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), - Point3(0, 0, 1)); - SimpleCamera level_camera(level_pose, *K2); - - // create second camera 1 meter to the right of first camera - Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1, 0, 0)); - SimpleCamera level_camera_right(level_pose_right, *K2); - - // landmark ~5 meters infront of camera - Point3 landmark(5, 0.5, 1.2); - // 1. Project two landmarks into two cameras and triangulate Point2 pixelError(0.2, 0.2); - Point2 level_uv = level_camera.project(landmark) + pixelError; - Point2 level_uv_right = level_camera_right.project(landmark); + Point2 level_uv = level_camera.project(landmark1) + pixelError; + Point2 level_uv_right = level_camera_right.project(landmark1); Values values; values.insert(x1, level_pose); @@ -208,8 +207,8 @@ TEST( SmartProjectionPoseFactor, noisy ) { values.insert(x2, level_pose_right.compose(noise_pose)); SmartFactor::shared_ptr factor(new SmartFactor()); - factor->add(level_uv, x1, model, K); - factor->add(level_uv_right, x2, model, K); + factor->add(level_uv, x1, model, sharedK); + factor->add(level_uv_right, x2, model, sharedK); double actualError1 = factor->error(values); @@ -223,8 +222,8 @@ TEST( SmartProjectionPoseFactor, noisy ) { noises.push_back(model); vector > Ks; ///< shared pointer to calibration object (one for each camera) - Ks.push_back(K); - Ks.push_back(K); + Ks.push_back(sharedK); + Ks.push_back(sharedK); vector views; views.push_back(x1); @@ -243,20 +242,15 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) { // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); - SimpleCamera cam1(pose1, *K2); + Camera cam1(pose1, sharedK2); // create second camera 1 meter to the right of first camera Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); - SimpleCamera cam2(pose2, *K2); + Camera cam2(pose2, sharedK2); // create third camera 1 meter above the first camera Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); - SimpleCamera cam3(pose3, *K2); - - // three landmarks ~5 meters infront of camera - Point3 landmark1(5, 0.5, 1.2); - Point3 landmark2(5, -0.5, 1.2); - Point3 landmark3(3, 0, 3.0); + Camera cam3(pose3, sharedK2); vector measurements_cam1, measurements_cam2, measurements_cam3; @@ -271,13 +265,13 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) { views.push_back(x3); SmartFactor::shared_ptr smartFactor1(new SmartFactor()); - smartFactor1->add(measurements_cam1, views, model, K2); + smartFactor1->add(measurements_cam1, views, model, sharedK2); SmartFactor::shared_ptr smartFactor2(new SmartFactor()); - smartFactor2->add(measurements_cam2, views, model, K2); + smartFactor2->add(measurements_cam2, views, model, sharedK2); SmartFactor::shared_ptr smartFactor3(new SmartFactor()); - smartFactor3->add(measurements_cam3, views, model, K2); + smartFactor3->add(measurements_cam3, views, model, sharedK2); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -312,8 +306,8 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) { gttoc_(SmartProjectionPoseFactor); tictoc_finishedIteration_(); -// GaussianFactorGraph::shared_ptr GFG = graph.linearize(values); -// VectorValues delta = GFG->optimize(); + GaussianFactorGraph::shared_ptr GFG = graph.linearize(values); + VectorValues delta = GFG->optimize(); // result.print("results of 3 camera, 3 landmark optimization \n"); if (isDebugTest) @@ -332,9 +326,9 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ) { 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); + Camera cam1(cameraPose1, sharedK); // with camera poses + Camera cam2(cameraPose2, sharedK); + Camera cam3(cameraPose3, sharedK); // create arbitrary body_Pose_sensor (transforms from sensor to body) Pose3 sensor_to_body = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), @@ -345,11 +339,6 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ) { 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 @@ -371,17 +360,17 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ) { SmartFactor::shared_ptr smartFactor1( new SmartFactor(rankTol, linThreshold, manageDegeneracy, enableEPI, sensor_to_body)); - smartFactor1->add(measurements_cam1, views, model, K); + smartFactor1->add(measurements_cam1, views, model, sharedK); SmartFactor::shared_ptr smartFactor2( new SmartFactor(rankTol, linThreshold, manageDegeneracy, enableEPI, sensor_to_body)); - smartFactor2->add(measurements_cam2, views, model, K); + smartFactor2->add(measurements_cam2, views, model, sharedK); SmartFactor::shared_ptr smartFactor3( new SmartFactor(rankTol, linThreshold, manageDegeneracy, enableEPI, sensor_to_body)); - smartFactor3->add(measurements_cam3, views, model, K); + smartFactor3->add(measurements_cam3, views, model, sharedK); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -428,34 +417,36 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ) { boost::bind(&SmartFactor::whitenedErrors, *smartFactor1, cameras, _1); boost::optional point = smartFactor1->point(); CHECK(point); - Matrix expectedE = numericalDerivative11(f, *point); + + // Note ! After refactor the noiseModel is only in the factors, not these matrices + Matrix expectedE = sigma * numericalDerivative11(f, *point); // Calculate using computeEP Matrix actualE, PointCov; - smartFactor1->computeEP(actualE, PointCov, cameras); + smartFactor1->computeEP(actualE, PointCov, values); EXPECT(assert_equal(expectedE, actualE, 1e-7)); // Calculate using whitenedError - Matrix F, actualE2; - Vector actualErrors = smartFactor1->whitenedError(cameras, *point, F, - actualE2); - EXPECT(assert_equal(expectedE, actualE2, 1e-7)); + Matrix E; + SmartFactor::Cameras::FBlocks F; + Vector actualErrors = smartFactor1->reprojectionErrors(cameras, *point, F, E); + EXPECT(assert_equal(expectedE, E, 1e-7)); - // TODO the derivatives agree with f, but returned errors are -f :-( - // TODO We should merge the two whitenedErrors functions and make derivatives optional - EXPECT(assert_equal(-f(*point), actualErrors, 1e-7)); + // Success ! The derivatives of reprojectionErrors now agree with f ! + EXPECT(assert_equal(f(*point) * sigma, actualErrors, 1e-7)); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, Factors ){ +TEST( SmartProjectionPoseFactor, Factors ) { // Default cameras for simple derivatives Rot3 R; - static Cal3_S2::shared_ptr K(new Cal3_S2(100, 100, 0, 0, 0)); - SimpleCamera cam1(Pose3(R,Point3(0,0,0)),*K), cam2(Pose3(R,Point3(1,0,0)),*K); + static Cal3_S2::shared_ptr sharedK(new Cal3_S2(100, 100, 0, 0, 0)); + PinholePose cam1(Pose3(R, Point3(0, 0, 0)), sharedK), cam2( + Pose3(R, Point3(1, 0, 0)), sharedK); // one landmarks 1m in front of camera - Point3 landmark1(0,0,10); + Point3 landmark1(0, 0, 10); vector measurements_cam1; @@ -464,24 +455,24 @@ TEST( SmartProjectionPoseFactor, Factors ){ measurements_cam1.push_back(cam2.project(landmark1)); // Create smart factors - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); SmartFactor::shared_ptr smartFactor1 = boost::make_shared(); - smartFactor1->add(measurements_cam1, views, model, K); + smartFactor1->add(measurements_cam1, views, model, sharedK); SmartFactor::Cameras cameras; cameras.push_back(cam1); cameras.push_back(cam2); // Make sure triangulation works - LONGS_EQUAL(2,smartFactor1->triangulateSafe(cameras)); + LONGS_EQUAL(2, smartFactor1->triangulateSafe(cameras)); CHECK(!smartFactor1->isDegenerate()); CHECK(!smartFactor1->isPointBehindCamera()); boost::optional p = smartFactor1->point(); CHECK(p); - EXPECT(assert_equal(landmark1,*p)); + EXPECT(assert_equal(landmark1, *p)); // After eliminating the point, A1 and A2 contain 2-rank information on cameras: Matrix16 A1, A2; @@ -489,12 +480,14 @@ TEST( SmartProjectionPoseFactor, Factors ){ A2 << 1000, 0, 100, 0, -100, 0; { // createHessianFactor - Matrix66 G11 = 0.5*A1.transpose()*A1; - Matrix66 G12 = 0.5*A1.transpose()*A2; - Matrix66 G22 = 0.5*A2.transpose()*A2; + Matrix66 G11 = 0.5 * A1.transpose() * A1; + Matrix66 G12 = 0.5 * A1.transpose() * A2; + Matrix66 G22 = 0.5 * A2.transpose() * A2; - Vector6 g1; g1.setZero(); - Vector6 g2; g2.setZero(); + Vector6 g1; + g1.setZero(); + Vector6 g2; + g2.setZero(); double f = 0; @@ -502,18 +495,37 @@ TEST( SmartProjectionPoseFactor, Factors ){ boost::shared_ptr > actual = smartFactor1->createHessianFactor(cameras, 0.0); - CHECK(assert_equal(expected.augmentedInformation(), actual->augmentedInformation(), 1e-8)); - CHECK(assert_equal(expected, *actual, 1e-8)); + EXPECT(assert_equal(expected.information(), actual->information(), 1e-8)); + EXPECT(assert_equal(expected, *actual, 1e-8)); } { - Matrix26 F1; F1.setZero(); F1(0,1)=-100; F1(0,3)=-10; F1(1,0)=100; F1(1,4)=-10; - Matrix26 F2; F2.setZero(); F2(0,1)=-101; F2(0,3)=-10; F2(0,5)=-1; F2(1,0)=100; F2(1,2)=10; F2(1,4)=-10; - Matrix43 E; E.setZero(); E(0,0)=100; E(1,1)=100; E(2,0)=100; E(2,2)=10;E(3,1)=100; + Matrix26 F1; + F1.setZero(); + F1(0, 1) = -100; + F1(0, 3) = -10; + F1(1, 0) = 100; + F1(1, 4) = -10; + Matrix26 F2; + F2.setZero(); + F2(0, 1) = -101; + F2(0, 3) = -10; + F2(0, 5) = -1; + F2(1, 0) = 100; + F2(1, 2) = 10; + F2(1, 4) = -10; + Matrix43 E; + E.setZero(); + E(0, 0) = 100; + E(1, 1) = 100; + E(2, 0) = 100; + E(2, 2) = 10; + E(3, 1) = 100; const vector > Fblocks = list_of > // - (make_pair(x1, 10*F1))(make_pair(x2, 10*F2)); + (make_pair(x1, 10 * F1))(make_pair(x2, 10 * F2)); Matrix3 P = (E.transpose() * E).inverse(); - Vector4 b; b.setZero(); + Vector4 b; + b.setZero(); // createRegularImplicitSchurFactor RegularImplicitSchurFactor<6> expected(Fblocks, E, P, b); @@ -524,26 +536,27 @@ TEST( SmartProjectionPoseFactor, Factors ){ CHECK(assert_equal(expected, *actual)); // createJacobianQFactor - JacobianFactorQ < 6, 2 > expectedQ(Fblocks, E, P, b); + JacobianFactorQ<6, 2> expectedQ(Fblocks, E, P, b); boost::shared_ptr > actualQ = smartFactor1->createJacobianQFactor(cameras, 0.0); CHECK(actual); - CHECK(assert_equal(expectedQ.augmentedInformation(), actualQ->augmentedInformation(), 1e-8)); - CHECK(assert_equal(expectedQ, *actualQ)); + EXPECT(assert_equal(expectedQ.information(), actualQ->information(), 1e-8)); + EXPECT(assert_equal(expectedQ, *actualQ)); } { // createJacobianSVDFactor - Vector1 b; b.setZero(); + Vector1 b; + b.setZero(); double s = sin(M_PI_4); - JacobianFactor expected(x1, s*A1, x2, s*A2, b); + JacobianFactor expected(x1, s * A1, x2, s * A2, b); boost::shared_ptr actual = smartFactor1->createJacobianSVDFactor(cameras, 0.0); CHECK(actual); - CHECK(assert_equal(expected.augmentedInformation(), actual->augmentedInformation(), 1e-8)); - CHECK(assert_equal(expected, *actual)); + EXPECT(assert_equal(expected.information(), actual->information(), 1e-8)); + EXPECT(assert_equal(expected, *actual)); } } @@ -558,20 +571,15 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) { // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); - SimpleCamera cam1(pose1, *K); + Camera cam1(pose1, sharedK); // create second camera 1 meter to the right of first camera Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); - SimpleCamera cam2(pose2, *K); + Camera cam2(pose2, sharedK); // create third camera 1 meter above the first camera Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); - SimpleCamera cam3(pose3, *K); - - // three landmarks ~5 meters infront of camera - Point3 landmark1(5, 0.5, 1.2); - Point3 landmark2(5, -0.5, 1.2); - Point3 landmark3(3, 0, 3.0); + Camera cam3(pose3, sharedK); vector measurements_cam1, measurements_cam2, measurements_cam3; @@ -581,13 +589,13 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) { projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); SmartFactor::shared_ptr smartFactor1(new SmartFactor()); - smartFactor1->add(measurements_cam1, views, model, K); + smartFactor1->add(measurements_cam1, views, model, sharedK); SmartFactor::shared_ptr smartFactor2(new SmartFactor()); - smartFactor2->add(measurements_cam2, views, model, K); + smartFactor2->add(measurements_cam2, views, model, sharedK); SmartFactor::shared_ptr smartFactor3(new SmartFactor()); - smartFactor3->add(measurements_cam3, views, model, K); + smartFactor3->add(measurements_cam3, views, model, sharedK); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -640,18 +648,13 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ) { // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); - SimpleCamera cam1(pose1, *K); + Camera cam1(pose1, sharedK); // create second camera 1 meter to the right of first camera Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); - SimpleCamera cam2(pose2, *K); + Camera cam2(pose2, sharedK); // create third camera 1 meter above the first camera Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); - SimpleCamera cam3(pose3, *K); - - // three landmarks ~5 meters infront of camera - Point3 landmark1(5, 0.5, 1.2); - Point3 landmark2(5, -0.5, 1.2); - Point3 landmark3(3, 0, 3.0); + Camera cam3(pose3, sharedK); vector measurements_cam1, measurements_cam2, measurements_cam3; @@ -662,15 +665,15 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ) { SmartFactor::shared_ptr smartFactor1( new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); - smartFactor1->add(measurements_cam1, views, model, K); + smartFactor1->add(measurements_cam1, views, model, sharedK); SmartFactor::shared_ptr smartFactor2( new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); - smartFactor2->add(measurements_cam2, views, model, K); + smartFactor2->add(measurements_cam2, views, model, sharedK); SmartFactor::shared_ptr smartFactor3( new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); - smartFactor3->add(measurements_cam3, views, model, K); + smartFactor3->add(measurements_cam3, views, model, sharedK); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -708,18 +711,13 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ) { // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); - SimpleCamera cam1(pose1, *K); + Camera cam1(pose1, sharedK); // create second camera 1 meter to the right of first camera Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); - SimpleCamera cam2(pose2, *K); + Camera cam2(pose2, sharedK); // create third camera 1 meter above the first camera Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); - SimpleCamera cam3(pose3, *K); - - // three landmarks ~5 meters infront of camera - Point3 landmark1(5, 0.5, 1.2); - Point3 landmark2(5, -0.5, 1.2); - Point3 landmark3(3, 0, 3.0); + Camera cam3(pose3, sharedK); vector measurements_cam1, measurements_cam2, measurements_cam3; @@ -731,17 +729,17 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ) { SmartFactor::shared_ptr smartFactor1( new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); - smartFactor1->add(measurements_cam1, views, model, K); + smartFactor1->add(measurements_cam1, views, model, sharedK); SmartFactor::shared_ptr smartFactor2( new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); - smartFactor2->add(measurements_cam2, views, model, K); + smartFactor2->add(measurements_cam2, views, model, sharedK); SmartFactor::shared_ptr smartFactor3( new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); - smartFactor3->add(measurements_cam3, views, model, K); + smartFactor3->add(measurements_cam3, views, model, sharedK); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -781,18 +779,15 @@ TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) { // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); - SimpleCamera cam1(pose1, *K); + Camera cam1(pose1, sharedK); // create second camera 1 meter to the right of first camera Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); - SimpleCamera cam2(pose2, *K); + Camera cam2(pose2, sharedK); // create third camera 1 meter above the first camera Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); - SimpleCamera cam3(pose3, *K); + Camera cam3(pose3, sharedK); - // three landmarks ~5 meters infront of camera - Point3 landmark1(5, 0.5, 1.2); - Point3 landmark2(5, -0.5, 1.2); - Point3 landmark3(3, 0, 3.0); + // add fourth landmark Point3 landmark4(5, -0.5, 1); vector measurements_cam1, measurements_cam2, measurements_cam3, @@ -808,22 +803,22 @@ TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) { SmartFactor::shared_ptr smartFactor1( new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); - smartFactor1->add(measurements_cam1, views, model, K); + smartFactor1->add(measurements_cam1, views, model, sharedK); SmartFactor::shared_ptr smartFactor2( new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); - smartFactor2->add(measurements_cam2, views, model, K); + smartFactor2->add(measurements_cam2, views, model, sharedK); SmartFactor::shared_ptr smartFactor3( new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); - smartFactor3->add(measurements_cam3, views, model, K); + smartFactor3->add(measurements_cam3, views, model, sharedK); SmartFactor::shared_ptr smartFactor4( new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); - smartFactor4->add(measurements_cam4, views, model, K); + smartFactor4->add(measurements_cam4, views, model, sharedK); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -860,18 +855,13 @@ TEST( SmartProjectionPoseFactor, jacobianQ ) { // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); - SimpleCamera cam1(pose1, *K); + Camera cam1(pose1, sharedK); // create second camera 1 meter to the right of first camera Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); - SimpleCamera cam2(pose2, *K); + Camera cam2(pose2, sharedK); // create third camera 1 meter above the first camera Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); - SimpleCamera cam3(pose3, *K); - - // three landmarks ~5 meters infront of camera - Point3 landmark1(5, 0.5, 1.2); - Point3 landmark2(5, -0.5, 1.2); - Point3 landmark3(3, 0, 3.0); + Camera cam3(pose3, sharedK); vector measurements_cam1, measurements_cam2, measurements_cam3; @@ -882,15 +872,15 @@ TEST( SmartProjectionPoseFactor, jacobianQ ) { SmartFactor::shared_ptr smartFactor1( new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q)); - smartFactor1->add(measurements_cam1, views, model, K); + smartFactor1->add(measurements_cam1, views, model, sharedK); SmartFactor::shared_ptr smartFactor2( new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q)); - smartFactor2->add(measurements_cam2, views, model, K); + smartFactor2->add(measurements_cam2, views, model, sharedK); SmartFactor::shared_ptr smartFactor3( new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q)); - smartFactor3->add(measurements_cam3, views, model, K); + smartFactor3->add(measurements_cam3, views, model, sharedK); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -927,45 +917,40 @@ TEST( SmartProjectionPoseFactor, 3poses_projection_factor ) { // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); - SimpleCamera cam1(pose1, *K2); + Camera cam1(pose1, sharedK2); // create second camera 1 meter to the right of first camera Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); - SimpleCamera cam2(pose2, *K2); + Camera cam2(pose2, sharedK2); // create third camera 1 meter above the first camera Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); - SimpleCamera cam3(pose3, *K2); - - // three landmarks ~5 meters infront of camera - Point3 landmark1(5, 0.5, 1.2); - Point3 landmark2(5, -0.5, 1.2); - Point3 landmark3(3, 0, 3.0); + Camera cam3(pose3, sharedK2); typedef GenericProjectionFactor ProjectionFactor; NonlinearFactorGraph graph; // 1. Project three landmarks into three cameras and triangulate graph.push_back( - ProjectionFactor(cam1.project(landmark1), model, x1, L(1), K2)); + ProjectionFactor(cam1.project(landmark1), model, x1, L(1), sharedK2)); graph.push_back( - ProjectionFactor(cam2.project(landmark1), model, x2, L(1), K2)); + ProjectionFactor(cam2.project(landmark1), model, x2, L(1), sharedK2)); graph.push_back( - ProjectionFactor(cam3.project(landmark1), model, x3, L(1), K2)); + ProjectionFactor(cam3.project(landmark1), model, x3, L(1), sharedK2)); graph.push_back( - ProjectionFactor(cam1.project(landmark2), model, x1, L(2), K2)); + ProjectionFactor(cam1.project(landmark2), model, x1, L(2), sharedK2)); graph.push_back( - ProjectionFactor(cam2.project(landmark2), model, x2, L(2), K2)); + ProjectionFactor(cam2.project(landmark2), model, x2, L(2), sharedK2)); graph.push_back( - ProjectionFactor(cam3.project(landmark2), model, x3, L(2), K2)); + ProjectionFactor(cam3.project(landmark2), model, x3, L(2), sharedK2)); graph.push_back( - ProjectionFactor(cam1.project(landmark3), model, x1, L(3), K2)); + ProjectionFactor(cam1.project(landmark3), model, x1, L(3), sharedK2)); graph.push_back( - ProjectionFactor(cam2.project(landmark3), model, x2, L(3), K2)); + ProjectionFactor(cam2.project(landmark3), model, x2, L(3), sharedK2)); graph.push_back( - ProjectionFactor(cam3.project(landmark3), model, x3, L(3), K2)); + ProjectionFactor(cam3.project(landmark3), model, x3, L(3), sharedK2)); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); graph.push_back(PriorFactor(x1, pose1, noisePrior)); @@ -1006,20 +991,15 @@ TEST( SmartProjectionPoseFactor, CheckHessian) { // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); - SimpleCamera cam1(pose1, *K); + Camera cam1(pose1, sharedK); // create second camera 1 meter to the right of first camera Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); - SimpleCamera cam2(pose2, *K); + Camera cam2(pose2, sharedK); // create third camera 1 meter above the first camera Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); - SimpleCamera cam3(pose3, *K); - - // three landmarks ~5 meters infront of camera - Point3 landmark1(5, 0.5, 1.2); - Point3 landmark2(5, -0.5, 1.2); - Point3 landmark3(3, 0, 3.0); + Camera cam3(pose3, sharedK); vector measurements_cam1, measurements_cam2, measurements_cam3; @@ -1031,13 +1011,13 @@ TEST( SmartProjectionPoseFactor, CheckHessian) { double rankTol = 10; SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol)); - smartFactor1->add(measurements_cam1, views, model, K); + smartFactor1->add(measurements_cam1, views, model, sharedK); SmartFactor::shared_ptr smartFactor2(new SmartFactor(rankTol)); - smartFactor2->add(measurements_cam2, views, model, K); + smartFactor2->add(measurements_cam2, views, model, sharedK); SmartFactor::shared_ptr smartFactor3(new SmartFactor(rankTol)); - smartFactor3->add(measurements_cam3, views, model, K); + smartFactor3->add(measurements_cam3, views, model, sharedK); NonlinearFactorGraph graph; graph.push_back(smartFactor1); @@ -1095,19 +1075,15 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); - SimpleCamera cam1(pose1, *K2); + Camera cam1(pose1, sharedK2); // create second camera 1 meter to the right of first camera Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); - SimpleCamera cam2(pose2, *K2); + Camera cam2(pose2, sharedK2); // create third camera 1 meter above the first camera Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); - SimpleCamera cam3(pose3, *K2); - - // three landmarks ~5 meters infront of camera - Point3 landmark1(5, 0.5, 1.2); - Point3 landmark2(5, -0.5, 1.2); + Camera cam3(pose3, sharedK2); vector measurements_cam1, measurements_cam2, measurements_cam3; @@ -1118,11 +1094,11 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac double rankTol = 50; SmartFactor::shared_ptr smartFactor1( new SmartFactor(rankTol, linThreshold, manageDegeneracy)); - smartFactor1->add(measurements_cam1, views, model, K2); + smartFactor1->add(measurements_cam1, views, model, sharedK2); SmartFactor::shared_ptr smartFactor2( new SmartFactor(rankTol, linThreshold, manageDegeneracy)); - smartFactor2->add(measurements_cam2, views, model, K2); + smartFactor2->add(measurements_cam2, views, model, sharedK2); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, @@ -1183,20 +1159,15 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ) // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); - SimpleCamera cam1(pose1, *K); + Camera cam1(pose1, sharedK); // create second camera 1 meter to the right of first camera Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); - SimpleCamera cam2(pose2, *K); + Camera cam2(pose2, sharedK); // create third camera 1 meter above the first camera Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); - SimpleCamera cam3(pose3, *K); - - // three landmarks ~5 meters infront of camera - Point3 landmark1(5, 0.5, 1.2); - Point3 landmark2(5, -0.5, 1.2); - Point3 landmark3(3, 0, 3.0); + Camera cam3(pose3, sharedK); vector measurements_cam1, measurements_cam2, measurements_cam3; @@ -1209,15 +1180,15 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ) SmartFactor::shared_ptr smartFactor1( new SmartFactor(rankTol, linThreshold, manageDegeneracy)); - smartFactor1->add(measurements_cam1, views, model, K); + smartFactor1->add(measurements_cam1, views, model, sharedK); SmartFactor::shared_ptr smartFactor2( new SmartFactor(rankTol, linThreshold, manageDegeneracy)); - smartFactor2->add(measurements_cam2, views, model, K); + smartFactor2->add(measurements_cam2, views, model, sharedK); SmartFactor::shared_ptr smartFactor3( new SmartFactor(rankTol, linThreshold, manageDegeneracy)); - smartFactor3->add(measurements_cam3, views, model, K); + smartFactor3->add(measurements_cam3, views, model, sharedK); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, @@ -1279,14 +1250,11 @@ TEST( SmartProjectionPoseFactor, Hessian ) { // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); - SimpleCamera cam1(pose1, *K2); + Camera cam1(pose1, sharedK2); // create second camera 1 meter to the right of first camera Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); - SimpleCamera cam2(pose2, *K2); - - // three landmarks ~5 meters infront of camera - Point3 landmark1(5, 0.5, 1.2); + Camera cam2(pose2, sharedK2); // 1. Project three landmarks into three cameras and triangulate Point2 cam1_uv1 = cam1.project(landmark1); @@ -1296,7 +1264,7 @@ TEST( SmartProjectionPoseFactor, Hessian ) { measurements_cam1.push_back(cam2_uv1); SmartFactor::shared_ptr smartFactor1(new SmartFactor()); - smartFactor1->add(measurements_cam1, views, model, K2); + smartFactor1->add(measurements_cam1, views, model, sharedK2); Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10), Point3(0.5, 0.1, 0.3)); @@ -1326,24 +1294,22 @@ TEST( SmartProjectionPoseFactor, HessianWithRotation ) { // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); - SimpleCamera cam1(pose1, *K); + Camera cam1(pose1, sharedK); // create second camera 1 meter to the right of first camera Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); - SimpleCamera cam2(pose2, *K); + Camera cam2(pose2, sharedK); // create third camera 1 meter above the first camera Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); - SimpleCamera cam3(pose3, *K); - - Point3 landmark1(5, 0.5, 1.2); + Camera cam3(pose3, sharedK); vector measurements_cam1, measurements_cam2, measurements_cam3; projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); SmartFactor::shared_ptr smartFactorInstance(new SmartFactor()); - smartFactorInstance->add(measurements_cam1, views, model, K); + smartFactorInstance->add(measurements_cam1, views, model, sharedK); Values values; values.insert(x1, pose1); @@ -1398,24 +1364,22 @@ TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ) { // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); - SimpleCamera cam1(pose1, *K2); + Camera cam1(pose1, sharedK2); // create second camera 1 meter to the right of first camera Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(0, 0, 0)); - SimpleCamera cam2(pose2, *K2); + Camera cam2(pose2, sharedK2); // create third camera 1 meter above the first camera Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, 0, 0)); - SimpleCamera cam3(pose3, *K2); - - Point3 landmark1(5, 0.5, 1.2); + Camera cam3(pose3, sharedK2); vector measurements_cam1, measurements_cam2, measurements_cam3; projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); SmartFactor::shared_ptr smartFactor(new SmartFactor()); - smartFactor->add(measurements_cam1, views, model, K2); + smartFactor->add(measurements_cam1, views, model, sharedK2); Values values; values.insert(x1, pose1); @@ -1464,9 +1428,9 @@ TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ) { /* ************************************************************************* */ TEST( SmartProjectionPoseFactor, ConstructorWithCal3Bundler) { SmartFactorBundler factor(rankTol, linThreshold); - boost::shared_ptr Kbundler( + boost::shared_ptr sharedBundlerK( new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000)); - factor.add(measurement1, poseKey1, model, Kbundler); + factor.add(measurement1, poseKey1, model, sharedBundlerK); } /* *************************************************************************/ @@ -1475,19 +1439,17 @@ TEST( SmartProjectionPoseFactor, Cal3Bundler ) { // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); - PinholeCamera cam1(pose1, *Kbundler); + CameraBundler cam1(pose1, sharedBundlerK); // create second camera 1 meter to the right of first camera Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); - PinholeCamera cam2(pose2, *Kbundler); + CameraBundler cam2(pose2, sharedBundlerK); // create third camera 1 meter above the first camera Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); - PinholeCamera cam3(pose3, *Kbundler); + CameraBundler cam3(pose3, sharedBundlerK); // three landmarks ~5 meters infront of camera - Point3 landmark1(5, 0.5, 1.2); - Point3 landmark2(5, -0.5, 1.2); Point3 landmark3(3, 0, 3.0); vector measurements_cam1, measurements_cam2, measurements_cam3; @@ -1520,13 +1482,13 @@ TEST( SmartProjectionPoseFactor, Cal3Bundler ) { views.push_back(x3); SmartFactorBundler::shared_ptr smartFactor1(new SmartFactorBundler()); - smartFactor1->add(measurements_cam1, views, model, Kbundler); + smartFactor1->add(measurements_cam1, views, model, sharedBundlerK); SmartFactorBundler::shared_ptr smartFactor2(new SmartFactorBundler()); - smartFactor2->add(measurements_cam2, views, model, Kbundler); + smartFactor2->add(measurements_cam2, views, model, sharedBundlerK); SmartFactorBundler::shared_ptr smartFactor3(new SmartFactorBundler()); - smartFactor3->add(measurements_cam3, views, model, Kbundler); + smartFactor3->add(measurements_cam3, views, model, sharedBundlerK); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -1579,19 +1541,17 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) { // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); - PinholeCamera cam1(pose1, *Kbundler); + CameraBundler cam1(pose1, sharedBundlerK); // create second camera 1 meter to the right of first camera Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); - PinholeCamera cam2(pose2, *Kbundler); + CameraBundler cam2(pose2, sharedBundlerK); // create third camera 1 meter above the first camera Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); - PinholeCamera cam3(pose3, *Kbundler); + CameraBundler cam3(pose3, sharedBundlerK); - // three landmarks ~5 meters infront of camera - Point3 landmark1(5, 0.5, 1.2); - Point3 landmark2(5, -0.5, 1.2); + // landmark3 at 3 meters now Point3 landmark3(3, 0, 3.0); vector measurements_cam1, measurements_cam2, measurements_cam3; @@ -1622,15 +1582,15 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) { SmartFactorBundler::shared_ptr smartFactor1( new SmartFactorBundler(rankTol, linThreshold, manageDegeneracy)); - smartFactor1->add(measurements_cam1, views, model, Kbundler); + smartFactor1->add(measurements_cam1, views, model, sharedBundlerK); SmartFactorBundler::shared_ptr smartFactor2( new SmartFactorBundler(rankTol, linThreshold, manageDegeneracy)); - smartFactor2->add(measurements_cam2, views, model, Kbundler); + smartFactor2->add(measurements_cam2, views, model, sharedBundlerK); SmartFactorBundler::shared_ptr smartFactor3( new SmartFactorBundler(rankTol, linThreshold, manageDegeneracy)); - smartFactor3->add(measurements_cam3, views, model, Kbundler); + smartFactor3->add(measurements_cam3, views, model, sharedBundlerK); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index 11513d19f..0e70add9f 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -64,7 +64,7 @@ enum LinearizationMode { * SmartStereoProjectionFactor: triangulates point */ template -class SmartStereoProjectionFactor: public SmartFactorBase { +class SmartStereoProjectionFactor: public SmartFactorBase { protected: // Some triangulation parameters @@ -94,7 +94,7 @@ protected: typedef boost::shared_ptr SmartFactorStatePtr; /// shorthand for base class type - typedef SmartFactorBase Base; + typedef SmartFactorBase Base; double landmarkDistanceThreshold_; // if the landmark is triangulated at a // distance larger than that the factor is considered degenerate @@ -465,11 +465,11 @@ public: // /// Create a factor, takes values // boost::shared_ptr > createJacobianQFactor( // const Values& values, double lambda) const { -// Cameras myCameras; +// Cameras cameras; // // TODO triangulate twice ?? -// bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras); +// bool nonDegenerate = computeCamerasAndTriangulate(values, cameras); // if (nonDegenerate) -// return createJacobianQFactor(myCameras, lambda); +// return createJacobianQFactor(cameras, lambda); // else // return boost::make_shared< JacobianFactorQ >(this->keys_); // } @@ -485,15 +485,15 @@ public: /// Returns true if nonDegenerate bool computeCamerasAndTriangulate(const Values& values, - Cameras& myCameras) const { + Cameras& cameras) const { Values valuesFactor; // Select only the cameras BOOST_FOREACH(const Key key, this->keys_) valuesFactor.insert(key, values.at(key)); - myCameras = this->cameras(valuesFactor); - size_t nrCameras = this->triangulateSafe(myCameras); + cameras = this->cameras(valuesFactor); + size_t nrCameras = this->triangulateSafe(cameras); if (nrCameras < 2 || (!this->manageDegeneracy_ @@ -520,20 +520,20 @@ public: /// Takes values bool computeEP(Matrix& E, Matrix& PointCov, const Values& values) const { - Cameras myCameras; - bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras); + Cameras cameras; + bool nonDegenerate = computeCamerasAndTriangulate(values, cameras); if (nonDegenerate) - computeEP(E, PointCov, myCameras); + computeEP(E, PointCov, cameras); return nonDegenerate; } /// Version that takes values, and creates the point bool computeJacobians(std::vector& Fblocks, Matrix& E, Vector& b, const Values& values) const { - Cameras myCameras; - bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras); + Cameras cameras; + bool nonDegenerate = computeCamerasAndTriangulate(values, cameras); if (nonDegenerate) - computeJacobians(Fblocks, E, b, myCameras, 0.0); + computeJacobians(Fblocks, E, b, cameras, 0.0); return nonDegenerate; } @@ -598,10 +598,10 @@ public: // /// takes values // bool computeJacobiansSVD(std::vector& Fblocks, // Matrix& Enull, Vector& b, const Values& values) const { -// typename Base::Cameras myCameras; -// double good = computeCamerasAndTriangulate(values, myCameras); +// typename Base::Cameras cameras; +// double good = computeCamerasAndTriangulate(values, cameras); // if (good) -// computeJacobiansSVD(Fblocks, Enull, b, myCameras); +// computeJacobiansSVD(Fblocks, Enull, b, cameras); // return true; // } // @@ -624,20 +624,14 @@ public: return Base::computeJacobians(F, E, b, cameras, point_); } - /// Calculate vector of re-projection errors, before applying noise model - /// Assumes triangulation was done and degeneracy handled - Vector reprojectionError(const Cameras& cameras) const { - return Base::reprojectionError(cameras, point_); - } - /// Calculate vector of re-projection errors, before applying noise model Vector reprojectionError(const Values& values) const { - Cameras myCameras; - bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras); + Cameras cameras; + bool nonDegenerate = computeCamerasAndTriangulate(values, cameras); if (nonDegenerate) - return reprojectionError(myCameras); + return cameras.reprojectionErrors(point_); else - return zero(myCameras.size() * 3); + return zero(cameras.size() * 3); } /**