diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.cpp index 7ab28e614..4e1cbdd46 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.cpp @@ -10,116 +10,95 @@ * -------------------------------------------------------------------------- */ /** - * @file SmartProjectionProjectionPoseFactorRollingShutter.h - * @brief Smart projection factor on poses and extrinsic calibration + * @file SmartProjectionPoseFactorRollingShutter.h + * @brief Smart projection factor on poses modeling rolling shutter effect * @author Luca Carlone - * @author Frank Dellaert */ -#include +//#include namespace gtsam { -SmartStereoProjectionFactorPP::SmartStereoProjectionFactorPP( - const SharedNoiseModel& sharedNoiseModel, - const SmartStereoProjectionParams& params) - : Base(sharedNoiseModel, params) {} // note: no extrinsic specified! -void SmartStereoProjectionFactorPP::add( - const StereoPoint2& measured, - const Key& w_P_body_key, const Key& body_P_cam_key, - const boost::shared_ptr& K) { - // we index by cameras.. - Base::add(measured, w_P_body_key); - // but we also store the extrinsic calibration keys in the same order - world_P_body_keys_.push_back(w_P_body_key); - body_P_cam_keys_.push_back(body_P_cam_key); - - // pose keys are assumed to be unique (1 observation per time stamp), but calibration can be shared - if(std::find(keys_.begin(), keys_.end(), body_P_cam_key) == keys_.end()) - keys_.push_back(body_P_cam_key); // add only unique keys - - K_all_.push_back(K); -} - -void SmartStereoProjectionFactorPP::add( - const std::vector& measurements, - const KeyVector& world_P_body_keys, const KeyVector& body_P_cam_keys, - const std::vector>& Ks) { - assert(world_P_body_keys.size() == measurements.size()); - assert(world_P_body_keys.size() == body_P_cam_keys.size()); - assert(world_P_body_keys.size() == Ks.size()); - for (size_t i = 0; i < measurements.size(); i++) { - Base::add(measurements[i], world_P_body_keys[i]); - // pose keys are assumed to be unique (1 observation per time stamp), but calibration can be shared - if(std::find(keys_.begin(), keys_.end(), body_P_cam_keys[i]) == keys_.end()) - keys_.push_back(body_P_cam_keys[i]); // add only unique keys - - world_P_body_keys_.push_back(world_P_body_keys[i]); - body_P_cam_keys_.push_back(body_P_cam_keys[i]); - - K_all_.push_back(Ks[i]); - } -} - -void SmartStereoProjectionFactorPP::add( - const std::vector& measurements, - const KeyVector& world_P_body_keys, const KeyVector& body_P_cam_keys, - const boost::shared_ptr& K) { - assert(world_P_body_keys.size() == measurements.size()); - assert(world_P_body_keys.size() == body_P_cam_keys.size()); - for (size_t i = 0; i < measurements.size(); i++) { - Base::add(measurements[i], world_P_body_keys[i]); - // pose keys are assumed to be unique (1 observation per time stamp), but calibration can be shared - if(std::find(keys_.begin(), keys_.end(), body_P_cam_keys[i]) == keys_.end()) - keys_.push_back(body_P_cam_keys[i]); // add only unique keys - - world_P_body_keys_.push_back(world_P_body_keys[i]); - body_P_cam_keys_.push_back(body_P_cam_keys[i]); - - K_all_.push_back(K); - } -} - -void SmartStereoProjectionFactorPP::print( - const std::string& s, const KeyFormatter& keyFormatter) const { - std::cout << s << "SmartStereoProjectionFactorPP: \n "; - for (size_t i = 0; i < K_all_.size(); i++) { - K_all_[i]->print("calibration = "); - std::cout << " extrinsic pose key: " << keyFormatter(body_P_cam_keys_[i]) << std::endl; - } - Base::print("", keyFormatter); -} - -bool SmartStereoProjectionFactorPP::equals(const NonlinearFactor& p, - double tol) const { - const SmartStereoProjectionFactorPP* e = - dynamic_cast(&p); - - return e && Base::equals(p, tol) && - body_P_cam_keys_ == e->getExtrinsicPoseKeys(); -} - -double SmartStereoProjectionFactorPP::error(const Values& values) const { - if (this->active(values)) { - return this->totalReprojectionError(cameras(values)); - } else { // else of active flag - return 0.0; - } -} - -SmartStereoProjectionFactorPP::Base::Cameras -SmartStereoProjectionFactorPP::cameras(const Values& values) const { - assert(world_P_body_keys_.size() == K_all_.size()); - assert(world_P_body_keys_.size() == body_P_cam_keys_.size()); - Base::Cameras cameras; - for (size_t i = 0; i < world_P_body_keys_.size(); i++) { - Pose3 w_P_body = values.at(world_P_body_keys_[i]); - Pose3 body_P_cam = values.at(body_P_cam_keys_[i]); - Pose3 w_P_cam = w_P_body.compose(body_P_cam); - cameras.push_back(StereoCamera(w_P_cam, K_all_[i])); - } - return cameras; -} +// +//void SmartProjectionPoseFactorRollingShutter::add( +// const std::vector& measurements, +// const KeyVector& world_P_body_keys, const KeyVector& body_P_cam_keys, +// const std::vector>& Ks) { +// assert(world_P_body_keys.size() == measurements.size()); +// assert(world_P_body_keys.size() == body_P_cam_keys.size()); +// assert(world_P_body_keys.size() == Ks.size()); +// for (size_t i = 0; i < measurements.size(); i++) { +// Base::add(measurements[i], world_P_body_keys[i]); +// // pose keys are assumed to be unique (1 observation per time stamp), but calibration can be shared +// if(std::find(keys_.begin(), keys_.end(), body_P_cam_keys[i]) == keys_.end()) +// keys_.push_back(body_P_cam_keys[i]); // add only unique keys +// +// world_P_body_keys_.push_back(world_P_body_keys[i]); +// body_P_cam_keys_.push_back(body_P_cam_keys[i]); +// +// K_all_.push_back(Ks[i]); +// } +//} +// +//void SmartProjectionPoseFactorRollingShutter::add( +// const std::vector& measurements, +// const KeyVector& world_P_body_keys, const KeyVector& body_P_cam_keys, +// const boost::shared_ptr& K) { +// assert(world_P_body_keys.size() == measurements.size()); +// assert(world_P_body_keys.size() == body_P_cam_keys.size()); +// for (size_t i = 0; i < measurements.size(); i++) { +// Base::add(measurements[i], world_P_body_keys[i]); +// // pose keys are assumed to be unique (1 observation per time stamp), but calibration can be shared +// if(std::find(keys_.begin(), keys_.end(), body_P_cam_keys[i]) == keys_.end()) +// keys_.push_back(body_P_cam_keys[i]); // add only unique keys +// +// world_P_body_keys_.push_back(world_P_body_keys[i]); +// body_P_cam_keys_.push_back(body_P_cam_keys[i]); +// +// K_all_.push_back(K); +// } +//} +// +//void SmartProjectionPoseFactorRollingShutter::print( +// const std::string& s, const KeyFormatter& keyFormatter) const { +// std::cout << s << "SmartProjectionPoseFactorRollingShutter: \n "; +// for (size_t i = 0; i < K_all_.size(); i++) { +// K_all_[i]->print("calibration = "); +// std::cout << " extrinsic pose key: " << keyFormatter(body_P_cam_keys_[i]) << std::endl; +// } +// Base::print("", keyFormatter); +//} +// +//bool SmartProjectionPoseFactorRollingShutter::equals(const NonlinearFactor& p, +// double tol) const { +// const SmartProjectionPoseFactorRollingShutter* e = +// dynamic_cast(&p); +// +// return e && Base::equals(p, tol) && +// body_P_cam_keys_ == e->getExtrinsicPoseKeys(); +//} +// +//double SmartProjectionPoseFactorRollingShutter::error(const Values& values) const { +// if (this->active(values)) { +// return this->totalReprojectionError(cameras(values)); +// } else { // else of active flag +// return 0.0; +// } +//} +// +//SmartProjectionPoseFactorRollingShutter::Base::Cameras +//SmartProjectionPoseFactorRollingShutter::cameras(const Values& values) const { +// assert(world_P_body_keys_.size() == K_all_.size()); +// assert(world_P_body_keys_.size() == body_P_cam_keys_.size()); +// Base::Cameras cameras; +// for (size_t i = 0; i < world_P_body_keys_.size(); i++) { +// Pose3 w_P_body = values.at(world_P_body_keys_[i]); +// Pose3 body_P_cam = values.at(body_P_cam_keys_[i]); +// Pose3 w_P_cam = w_P_body.compose(body_P_cam); +// cameras.push_back(StereoCamera(w_P_cam, K_all_[i])); +// } +// return cameras; +//} } // \ namespace gtsam diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index 40d90d614..fe5ccdf7f 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -10,15 +10,14 @@ * -------------------------------------------------------------------------- */ /** - * @file SmartStereoProjectionFactorPP.h - * @brief Smart stereo factor on poses (P) and camera extrinsic pose (P) calibrations + * @file SmartProjectionPoseFactorRollingShutter.h + * @brief Smart projection factor on poses modeling rolling shutter effect with given readout time * @author Luca Carlone - * @author Frank Dellaert */ #pragma once -#include +#include namespace gtsam { /** @@ -33,37 +32,40 @@ namespace gtsam { */ /** - * This factor optimizes the pose of the body as well as the extrinsic camera calibration (pose of camera wrt body). - * Each camera may have its own extrinsic calibration or the same calibration can be shared by multiple cameras. - * This factor requires that values contain the involved poses and extrinsics (both are Pose3 variables). + * This factor optimizes the pose of the body assuming a rolling shutter model of the camera with given readout time. + * This factor requires that values contain (for each pixel observation) consecutive camera poses + * from which the pixel observation pose can be interpolated. * @addtogroup SLAM */ -class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor { +template +class SmartProjectionPoseFactorRollingShutter: public SmartProjectionFactor< + PinholePose > { + protected: - /// shared pointer to calibration object (one for each camera) - std::vector> K_all_; + /// shared pointer to calibration object (one for each observation) + std::vector > K_all_; - /// The keys corresponding to the pose of the body (with respect to an external world frame) for each view - KeyVector world_P_body_keys_; + // The keys of the pose of the body (with respect to an external world frame): two consecutive poses for each observation + std::vector> world_P_body_key_pairs_; - /// The keys corresponding to the extrinsic pose calibration for each view (pose that transform from camera to body) - KeyVector body_P_cam_keys_; + // interpolation factor (one for each observation) to interpolate between pair of consecutive poses + std::vector gammas_; public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW + typedef PinholePose Camera; /// shorthand for base class type - typedef SmartStereoProjectionFactor Base; + typedef SmartProjectionFactor Base; /// shorthand for this class - typedef SmartStereoProjectionFactorPP This; + typedef SmartProjectionPoseFactorRollingShutter This; /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; - static const int Dim = 12; ///< Camera dimension: 6 for body pose, 6 for extrinsic pose - static const int DimPose = 6; ///< Pose3 dimension - static const int ZDim = 3; ///< Measurement dimension (for a StereoPoint2 measurement) + static const int Dim = 6; ///< Pose3 dimension + static const int ZDim = 2; ///< Measurement dimension (Point2) typedef Eigen::Matrix MatrixZD; // F blocks (derivatives wrt camera) typedef std::vector > FBlocks; // vector of F blocks @@ -72,51 +74,67 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor { * @param Isotropic measurement noise * @param params internal parameters of the smart factors */ - SmartStereoProjectionFactorPP(const SharedNoiseModel& sharedNoiseModel, - const SmartStereoProjectionParams& params = - SmartStereoProjectionParams()); + SmartProjectionPoseFactorRollingShutter( + const SharedNoiseModel& sharedNoiseModel, + const SmartProjectionParams& params = SmartProjectionParams()) + : Base(sharedNoiseModel, params) {} /** Virtual destructor */ - ~SmartStereoProjectionFactorPP() override = default; + ~SmartProjectionPoseFactorRollingShutter() override = default; /** - * add a new measurement, with a pose key, and an extrinsic pose key - * @param measured is the 3-dimensional location of the projection of a - * single landmark in the a single (stereo) view (the measurement) - * @param world_P_body_key is the key corresponding to the body poses observing the same landmark - * @param body_P_cam_key is the key corresponding to the extrinsic camera-to-body pose calibration + * add a new measurement, with 2 pose keys, camera calibration, and observed pixel. + * @param measured is the 2-dimensional location of the projection of a + * single landmark in the a single view (the measurement), interpolated from the 2 poses + * @param world_P_body_key1 is the key corresponding to the first body poses (time <= time pixel is acquired) + * @param world_P_body_key2 is the key corresponding to the second body poses (time >= time pixel is acquired) + * @param gamma in [0,1] is the interpolation factor, such that if gamma = 0 the interpolated pose is the same as world_P_body_key * @param K is the (fixed) camera intrinsic calibration */ - void add(const StereoPoint2& measured, const Key& world_P_body_key, - const Key& body_P_cam_key, - const boost::shared_ptr& K); + void add(const Point2& measured, + const Key& world_P_body_key1, + const Key& world_P_body_key2, + const double& gamma, + const boost::shared_ptr& K){ + // store measurements in base class (note: we only store the first key there) + Base::add(measured, world_P_body_key1); + // but we also store the extrinsic calibration keys in the same order + world_P_body_key_pairs_.push_back(std::make_pair(world_P_body_key1,world_P_body_key2)); + + // pose keys are assumed to be unique, so we avoid duplicates here + if(std::find( this->keys_.begin(), this->keys_.end(), world_P_body_key1) == this->keys_.end()) + this->keys_.push_back(world_P_body_key1); // add only unique keys + if(std::find( this->keys_.begin(), this->keys_.end(), world_P_body_key2) == this->keys_.end()) + this->keys_.push_back(world_P_body_key2); // add only unique keys + + // store fixed calibration + K_all_.push_back(K); + } /** - * Variant of the previous one in which we include a set of measurements - * @param measurements vector of the 3m dimensional location of the projection - * of a single landmark in the m (stereo) view (the measurements) - * @param w_P_body_keys are the ordered keys corresponding to the body poses observing the same landmark - * @param body_P_cam_keys are the ordered keys corresponding to the extrinsic camera-to-body poses calibration - * (note: elements of this vector do not need to be unique: 2 camera views can share the same calibration) + * Variant of the previous one in which we include a set of measurements + * @param measurements vector of the 2m dimensional location of the projection + * of a single landmark in the m views (the measurements) + * @param world_P_body_key_pairs vector of (1 for each view) containing the pair of poses from which each view can be interpolated * @param Ks vector of intrinsic calibration objects */ - void add(const std::vector& measurements, - const KeyVector& w_P_body_keys, const KeyVector& body_P_cam_keys, - const std::vector>& Ks); +// void add(const std::vector& measurements, +// const std::vector>& world_P_body_key_pairs, +// const std::vector& gammas, +// const std::vector>& Ks); /** * Variant of the previous one in which we include a set of measurements with - * the same noise and calibration - * @param measurements vector of the 3m dimensional location of the projection - * of a single landmark in the m (stereo) view (the measurements) - * @param w_P_body_keys are the ordered keys corresponding to the body poses observing the same landmark - * @param body_P_cam_keys are the ordered keys corresponding to the extrinsic camera-to-body poses calibration - * (note: elements of this vector do not need to be unique: 2 camera views can share the same calibration) + * the same calibration + * @param measurements vector of the 2m dimensional location of the projection + * of a single landmark in the m views (the measurements) + * @param world_P_body_key_pairs vector of (1 for each view) containing the pair of poses from which each view can be interpolated * @param K the (known) camera calibration (same for all measurements) */ - void add(const std::vector& measurements, - const KeyVector& w_P_body_keys, const KeyVector& body_P_cam_keys, - const boost::shared_ptr& K); +// void add(const std::vector& measurements, +// const std::vector>& world_P_body_key_pairs, +// const std::vector& gammas, +// const boost::shared_ptr& K); /** * print @@ -130,8 +148,8 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor { bool equals(const NonlinearFactor& p, double tol = 1e-9) const override; /// equals - const KeyVector& getExtrinsicPoseKeys() const { - return body_P_cam_keys_; + const std::vector getGammas() const { + return gammas_; } /** @@ -140,18 +158,26 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor { double error(const Values& values) const override; /** return the calibration object */ - inline std::vector> calibration() const { + inline std::vector> calibration() const { return K_all_; } /** * Collect all cameras involved in this factor * @param values Values structure which must contain camera poses - * corresponding - * to keys involved in this factor - * @return vector of Values + * corresponding to keys involved in this factor + * @return Cameras */ - Base::Cameras cameras(const Values& values) const override; + typename Base::Cameras cameras(const Values& values) const override { + typename Base::Cameras cameras; + for (const Key& k : this->keys_) { +// const Pose3 world_P_sensor_k = +// Base::body_P_sensor_ ? values.at(k) * *Base::body_P_sensor_ +// : values.at(k); +// cameras.emplace_back(world_P_sensor_k, K_); + } + return cameras; + } /** * Compute jacobian F, E and error vector at a given linearization point @@ -161,169 +187,169 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor { * respect to both the body pose and extrinsic pose), the point Jacobian E, * and the error vector b. Note that the jacobians are computed for a given point. */ - void computeJacobiansAndCorrectForMissingMeasurements( - FBlocks& Fs, Matrix& E, Vector& b, const Values& values) const { - if (!result_) { - throw("computeJacobiansWithTriangulatedPoint"); - } else { // valid result: compute jacobians - size_t numViews = measured_.size(); - E = Matrix::Zero(3 * numViews, 3); // a StereoPoint2 for each view (point jacobian) - b = Vector::Zero(3 * numViews); // a StereoPoint2 for each view - Matrix dPoseCam_dPoseBody_i, dPoseCam_dPoseExt_i, dProject_dPoseCam_i, Ei; - - for (size_t i = 0; i < numViews; i++) { // for each camera/measurement - Pose3 w_P_body = values.at(world_P_body_keys_.at(i)); - Pose3 body_P_cam = values.at(body_P_cam_keys_.at(i)); - StereoCamera camera( - w_P_body.compose(body_P_cam, dPoseCam_dPoseBody_i, dPoseCam_dPoseExt_i), - K_all_[i]); - // get jacobians and error vector for current measurement - StereoPoint2 reprojectionError_i = StereoPoint2( - camera.project(*result_, dProject_dPoseCam_i, Ei) - measured_.at(i)); - Eigen::Matrix J; // 3 x 12 - J.block(0, 0) = dProject_dPoseCam_i * dPoseCam_dPoseBody_i; // (3x6) * (6x6) - J.block(0, 6) = dProject_dPoseCam_i * dPoseCam_dPoseExt_i; // (3x6) * (6x6) - // if the right pixel is invalid, fix jacobians - if (std::isnan(measured_.at(i).uR())) - { - J.block<1, 12>(1, 0) = Matrix::Zero(1, 12); - Ei.block<1, 3>(1, 0) = Matrix::Zero(1, 3); - reprojectionError_i = StereoPoint2(reprojectionError_i.uL(), 0.0, - reprojectionError_i.v()); - } - // fit into the output structures - Fs.push_back(J); - size_t row = 3 * i; - b.segment(row) = -reprojectionError_i.vector(); - E.block<3, 3>(row, 0) = Ei; - } - } - } +// void computeJacobiansAndCorrectForMissingMeasurements( +// FBlocks& Fs, Matrix& E, Vector& b, const Values& values) const { +// if (!result_) { +// throw("computeJacobiansWithTriangulatedPoint"); +// } else { // valid result: compute jacobians +// size_t numViews = measured_.size(); +// E = Matrix::Zero(3 * numViews, 3); // a StereoPoint2 for each view (point jacobian) +// b = Vector::Zero(3 * numViews); // a StereoPoint2 for each view +// Matrix dPoseCam_dPoseBody_i, dPoseCam_dPoseExt_i, dProject_dPoseCam_i, Ei; +// +// for (size_t i = 0; i < numViews; i++) { // for each camera/measurement +// Pose3 w_P_body = values.at(world_P_body_key_pairs_.at(i)); +// Pose3 body_P_cam = values.at(body_P_cam_ this->keys_.at(i)); +// StereoCamera camera( +// w_P_body.compose(body_P_cam, dPoseCam_dPoseBody_i, dPoseCam_dPoseExt_i), +// K_all_[i]); +// // get jacobians and error vector for current measurement +// StereoPoint2 reprojectionError_i = StereoPoint2( +// camera.project(*result_, dProject_dPoseCam_i, Ei) - measured_.at(i)); +// Eigen::Matrix J; // 3 x 12 +// J.block(0, 0) = dProject_dPoseCam_i * dPoseCam_dPoseBody_i; // (3x6) * (6x6) +// J.block(0, 6) = dProject_dPoseCam_i * dPoseCam_dPoseExt_i; // (3x6) * (6x6) +// // if the right pixel is invalid, fix jacobians +// if (std::isnan(measured_.at(i).uR())) +// { +// J.block<1, 12>(1, 0) = Matrix::Zero(1, 12); +// Ei.block<1, 3>(1, 0) = Matrix::Zero(1, 3); +// reprojectionError_i = StereoPoint2(reprojectionError_i.uL(), 0.0, +// reprojectionError_i.v()); +// } +// // fit into the output structures +// Fs.push_back(J); +// size_t row = 3 * i; +// b.segment(row) = -reprojectionError_i.vector(); +// E.block<3, 3>(row, 0) = Ei; +// } +// } +// } /// linearize and return a Hessianfactor that is an approximation of error(p) - boost::shared_ptr > createHessianFactor( - const Values& values, const double lambda = 0.0, bool diagonalDamping = - false) const { - - // we may have multiple cameras sharing the same extrinsic cals, hence the number - // of keys may be smaller than 2 * nrMeasurements (which is the upper bound where we - // have a body key and an extrinsic calibration key for each measurement) - size_t nrUniqueKeys = keys_.size(); - size_t nrNonuniqueKeys = world_P_body_keys_.size() - + body_P_cam_keys_.size(); - - // Create structures for Hessian Factors - KeyVector js; - std::vector < Matrix > Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2); - std::vector gs(nrUniqueKeys); - - if (this->measured_.size() != cameras(values).size()) - throw std::runtime_error("SmartStereoProjectionHessianFactor: this->" - "measured_.size() inconsistent with input"); - - // triangulate 3D point at given linearization point - triangulateSafe(cameras(values)); - - if (!result_) { // failed: return "empty/zero" Hessian - for (Matrix& m : Gs) - m = Matrix::Zero(DimPose, DimPose); - for (Vector& v : gs) - v = Vector::Zero(DimPose); - return boost::make_shared < RegularHessianFactor - > (keys_, Gs, gs, 0.0); - } - - // compute Jacobian given triangulated 3D Point - FBlocks Fs; - Matrix F, E; - Vector b; - computeJacobiansAndCorrectForMissingMeasurements(Fs, E, b, values); - - // Whiten using noise model - noiseModel_->WhitenSystem(E, b); - for (size_t i = 0; i < Fs.size(); i++) - Fs[i] = noiseModel_->Whiten(Fs[i]); - - // build augmented Hessian (with last row/column being the information vector) - Matrix3 P; - Cameras::ComputePointCovariance<3>(P, E, lambda, diagonalDamping); - - // marginalize point: note - we reuse the standard SchurComplement function - SymmetricBlockMatrix augmentedHessian = - Cameras::SchurComplement<3, Dim>(Fs, E, P, b); - - // now pack into an Hessian factor - std::vector dims(nrUniqueKeys + 1); // this also includes the b term - std::fill(dims.begin(), dims.end() - 1, 6); - dims.back() = 1; - SymmetricBlockMatrix augmentedHessianUniqueKeys; - - // here we have to deal with the fact that some cameras may share the same extrinsic key - if (nrUniqueKeys == nrNonuniqueKeys) { // if there is 1 calibration key per camera - augmentedHessianUniqueKeys = SymmetricBlockMatrix( - dims, Matrix(augmentedHessian.selfadjointView())); - } else { // if multiple cameras share a calibration we have to rearrange - // the results of the Schur complement matrix - std::vector nonuniqueDims(nrNonuniqueKeys + 1); // this also includes the b term - std::fill(nonuniqueDims.begin(), nonuniqueDims.end() - 1, 6); - nonuniqueDims.back() = 1; - augmentedHessian = SymmetricBlockMatrix( - nonuniqueDims, Matrix(augmentedHessian.selfadjointView())); - - // these are the keys that correspond to the blocks in augmentedHessian (output of SchurComplement) - KeyVector nonuniqueKeys; - for (size_t i = 0; i < world_P_body_keys_.size(); i++) { - nonuniqueKeys.push_back(world_P_body_keys_.at(i)); - nonuniqueKeys.push_back(body_P_cam_keys_.at(i)); - } - - // get map from key to location in the new augmented Hessian matrix (the one including only unique keys) - std::map keyToSlotMap; - for (size_t k = 0; k < nrUniqueKeys; k++) { - keyToSlotMap[keys_[k]] = k; - } - - // initialize matrix to zero - augmentedHessianUniqueKeys = SymmetricBlockMatrix( - dims, Matrix::Zero(6 * nrUniqueKeys + 1, 6 * nrUniqueKeys + 1)); - - // add contributions for each key: note this loops over the hessian with nonUnique keys (augmentedHessian) - // and populates an Hessian that only includes the unique keys (that is what we want to return) - for (size_t i = 0; i < nrNonuniqueKeys; i++) { // rows - Key key_i = nonuniqueKeys.at(i); - - // update information vector - augmentedHessianUniqueKeys.updateOffDiagonalBlock( - keyToSlotMap[key_i], nrUniqueKeys, - augmentedHessian.aboveDiagonalBlock(i, nrNonuniqueKeys)); - - // update blocks - for (size_t j = i; j < nrNonuniqueKeys; j++) { // cols - Key key_j = nonuniqueKeys.at(j); - if (i == j) { - augmentedHessianUniqueKeys.updateDiagonalBlock( - keyToSlotMap[key_i], augmentedHessian.diagonalBlock(i)); - } else { // (i < j) - if (keyToSlotMap[key_i] != keyToSlotMap[key_j]) { - augmentedHessianUniqueKeys.updateOffDiagonalBlock( - keyToSlotMap[key_i], keyToSlotMap[key_j], - augmentedHessian.aboveDiagonalBlock(i, j)); - } else { - augmentedHessianUniqueKeys.updateDiagonalBlock( - keyToSlotMap[key_i], - augmentedHessian.aboveDiagonalBlock(i, j) - + augmentedHessian.aboveDiagonalBlock(i, j).transpose()); - } - } - } - } - // update bottom right element of the matrix - augmentedHessianUniqueKeys.updateDiagonalBlock( - nrUniqueKeys, augmentedHessian.diagonalBlock(nrNonuniqueKeys)); - } - return boost::make_shared < RegularHessianFactor - > (keys_, augmentedHessianUniqueKeys); - } +// boost::shared_ptr > createHessianFactor( +// const Values& values, const double lambda = 0.0, bool diagonalDamping = +// false) const { +// +// // we may have multiple cameras sharing the same extrinsic cals, hence the number +// // of keys may be smaller than 2 * nrMeasurements (which is the upper bound where we +// // have a body key and an extrinsic calibration key for each measurement) +// size_t nrUniqueKeys = this->keys_.size(); +// size_t nrNonuniqueKeys = world_P_body_key_pairs_.size() +// + body_P_cam_ this->keys_.size(); +// +// // Create structures for Hessian Factors +// KeyVector js; +// std::vector < Matrix > Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2); +// std::vector gs(nrUniqueKeys); +// +// if (this->measured_.size() != cameras(values).size()) +// throw std::runtime_error("SmartStereoProjectionHessianFactor: this->" +// "measured_.size() inconsistent with input"); +// +// // triangulate 3D point at given linearization point +// triangulateSafe(cameras(values)); +// +// if (!result_) { // failed: return "empty/zero" Hessian +// for (Matrix& m : Gs) +// m = Matrix::Zero(DimPose, DimPose); +// for (Vector& v : gs) +// v = Vector::Zero(DimPose); +// return boost::make_shared < RegularHessianFactor +// > ( this->keys_, Gs, gs, 0.0); +// } +// +// // compute Jacobian given triangulated 3D Point +// FBlocks Fs; +// Matrix F, E; +// Vector b; +// computeJacobiansAndCorrectForMissingMeasurements(Fs, E, b, values); +// +// // Whiten using noise model +// noiseModel_->WhitenSystem(E, b); +// for (size_t i = 0; i < Fs.size(); i++) +// Fs[i] = noiseModel_->Whiten(Fs[i]); +// +// // build augmented Hessian (with last row/column being the information vector) +// Matrix3 P; +// Cameras::ComputePointCovariance<3>(P, E, lambda, diagonalDamping); +// +// // marginalize point: note - we reuse the standard SchurComplement function +// SymmetricBlockMatrix augmentedHessian = +// Cameras::SchurComplement<3, Dim>(Fs, E, P, b); +// +// // now pack into an Hessian factor +// std::vector dims(nrUniqueKeys + 1); // this also includes the b term +// std::fill(dims.begin(), dims.end() - 1, 6); +// dims.back() = 1; +// SymmetricBlockMatrix augmentedHessianUniqueKeys; +// +// // here we have to deal with the fact that some cameras may share the same extrinsic key +// if (nrUniqueKeys == nrNonuniqueKeys) { // if there is 1 calibration key per camera +// augmentedHessianUniqueKeys = SymmetricBlockMatrix( +// dims, Matrix(augmentedHessian.selfadjointView())); +// } else { // if multiple cameras share a calibration we have to rearrange +// // the results of the Schur complement matrix +// std::vector nonuniqueDims(nrNonuniqueKeys + 1); // this also includes the b term +// std::fill(nonuniqueDims.begin(), nonuniqueDims.end() - 1, 6); +// nonuniqueDims.back() = 1; +// augmentedHessian = SymmetricBlockMatrix( +// nonuniqueDims, Matrix(augmentedHessian.selfadjointView())); +// +// // these are the keys that correspond to the blocks in augmentedHessian (output of SchurComplement) +// KeyVector nonuniqueKeys; +// for (size_t i = 0; i < world_P_body_key_pairs_.size(); i++) { +// nonuniqueKeys.push_back(world_P_body_key_pairs_.at(i)); +// nonuniqueKeys.push_back(body_P_cam_ this->keys_.at(i)); +// } +// +// // get map from key to location in the new augmented Hessian matrix (the one including only unique keys) +// std::map keyToSlotMap; +// for (size_t k = 0; k < nrUniqueKeys; k++) { +// keyToSlotMap[ this->keys_[k]] = k; +// } +// +// // initialize matrix to zero +// augmentedHessianUniqueKeys = SymmetricBlockMatrix( +// dims, Matrix::Zero(6 * nrUniqueKeys + 1, 6 * nrUniqueKeys + 1)); +// +// // add contributions for each key: note this loops over the hessian with nonUnique keys (augmentedHessian) +// // and populates an Hessian that only includes the unique keys (that is what we want to return) +// for (size_t i = 0; i < nrNonuniqueKeys; i++) { // rows +// Key key_i = nonuniqueKeys.at(i); +// +// // update information vector +// augmentedHessianUniqueKeys.updateOffDiagonalBlock( +// keyToSlotMap[key_i], nrUniqueKeys, +// augmentedHessian.aboveDiagonalBlock(i, nrNonuniqueKeys)); +// +// // update blocks +// for (size_t j = i; j < nrNonuniqueKeys; j++) { // cols +// Key key_j = nonuniqueKeys.at(j); +// if (i == j) { +// augmentedHessianUniqueKeys.updateDiagonalBlock( +// keyToSlotMap[key_i], augmentedHessian.diagonalBlock(i)); +// } else { // (i < j) +// if (keyToSlotMap[key_i] != keyToSlotMap[key_j]) { +// augmentedHessianUniqueKeys.updateOffDiagonalBlock( +// keyToSlotMap[key_i], keyToSlotMap[key_j], +// augmentedHessian.aboveDiagonalBlock(i, j)); +// } else { +// augmentedHessianUniqueKeys.updateDiagonalBlock( +// keyToSlotMap[key_i], +// augmentedHessian.aboveDiagonalBlock(i, j) +// + augmentedHessian.aboveDiagonalBlock(i, j).transpose()); +// } +// } +// } +// } +// // update bottom right element of the matrix +// augmentedHessianUniqueKeys.updateDiagonalBlock( +// nrUniqueKeys, augmentedHessian.diagonalBlock(nrNonuniqueKeys)); +// } +// return boost::make_shared < RegularHessianFactor +// > ( this->keys_, augmentedHessianUniqueKeys); +// } /** * Linearize to Gaussian Factor (possibly adding a damping factor Lambda for LM) @@ -333,12 +359,12 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor { boost::shared_ptr linearizeDamped( const Values& values, const double lambda = 0.0) const { // depending on flag set on construction we may linearize to different linear factors - switch (params_.linearizationMode) { - case HESSIAN: - return createHessianFactor(values, lambda); + switch (this->params_.linearizationMode) { +// case HESSIAN: +// return createHessianFactor(values, lambda); default: throw std::runtime_error( - "SmartStereoProjectionFactorPP: unknown linearization mode"); + "SmartProjectionPoseFactorRollingShutter: unknown linearization mode"); } } @@ -361,9 +387,9 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor { // end of class declaration /// traits -template<> -struct traits : public Testable< - SmartStereoProjectionFactorPP> { +template +struct traits > : public Testable< + SmartProjectionPoseFactorRollingShutter > { }; } // namespace gtsam diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index c7f220c10..d4c268b3c 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -10,19 +10,17 @@ * -------------------------------------------------------------------------- */ /** - * @file testSmartProjectionPoseFactor.cpp - * @brief Unit tests for ProjectionFactor Class - * @author Chris Beall + * @file testSmartProjectionPoseFactorRollingShutter.cpp + * @brief Unit tests for SmartProjectionPoseFactorRollingShutter Class * @author Luca Carlone - * @author Zsolt Kira - * @author Frank Dellaert - * @date Sept 2013 + * @date July 2021 */ -#include "smartFactorScenarios.h" +#include "gtsam/slam/tests/smartFactorScenarios.h" #include #include #include +#include #include #include #include @@ -52,13 +50,13 @@ LevenbergMarquardtParams lmParams; // Make more verbose like so (in tests): // params.verbosityLM = LevenbergMarquardtParams::SUMMARY; -/* ************************************************************************* */ +/* ************************************************************************* * TEST( SmartProjectionPoseFactor, Constructor) { using namespace vanillaPose; SmartFactor::shared_ptr factor1(new SmartFactor(model, sharedK)); } -/* ************************************************************************* */ +/* ************************************************************************* * TEST( SmartProjectionPoseFactor, Constructor2) { using namespace vanillaPose; SmartProjectionParams params; @@ -66,14 +64,14 @@ TEST( SmartProjectionPoseFactor, Constructor2) { SmartFactor factor1(model, sharedK, params); } -/* ************************************************************************* */ +/* ************************************************************************* * TEST( SmartProjectionPoseFactor, Constructor3) { using namespace vanillaPose; SmartFactor::shared_ptr factor1(new SmartFactor(model, sharedK)); factor1->add(measurement1, x1); } -/* ************************************************************************* */ +/* ************************************************************************* * TEST( SmartProjectionPoseFactor, Constructor4) { using namespace vanillaPose; SmartProjectionParams params; @@ -82,7 +80,7 @@ TEST( SmartProjectionPoseFactor, Constructor4) { factor1.add(measurement1, x1); } -/* ************************************************************************* */ +/* ************************************************************************* * TEST( SmartProjectionPoseFactor, params) { using namespace vanillaPose; SmartProjectionParams params; @@ -93,7 +91,7 @@ TEST( SmartProjectionPoseFactor, params) { EXPECT_DOUBLES_EQUAL(1e-3, rt, 1e-7); } -/* ************************************************************************* */ +/* ************************************************************************* * TEST( SmartProjectionPoseFactor, Equals ) { using namespace vanillaPose; SmartFactor::shared_ptr factor1(new SmartFactor(model, sharedK)); @@ -105,7 +103,7 @@ TEST( SmartProjectionPoseFactor, Equals ) { CHECK(assert_equal(*factor1, *factor2)); } -/* *************************************************************************/ +/* ************************************************************************* TEST( SmartProjectionPoseFactor, noiseless ) { using namespace vanillaPose; @@ -163,7 +161,7 @@ TEST( SmartProjectionPoseFactor, noiseless ) { EXPECT_DOUBLES_EQUAL(expectedError, actualError3, 1e-6); } -/* *************************************************************************/ +/* ************************************************************************* TEST( SmartProjectionPoseFactor, noisy ) { using namespace vanillaPose; @@ -197,7 +195,7 @@ TEST( SmartProjectionPoseFactor, noisy ) { DOUBLES_EQUAL(actualError1, actualError2, 1e-7); } -/* *************************************************************************/ +/* ************************************************************************* TEST(SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform) { using namespace vanillaPose; @@ -272,7 +270,7 @@ TEST(SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform) { EXPECT(assert_equal(wTb3, result.at(x3))); } -/* *************************************************************************/ +/* ************************************************************************* TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) { using namespace vanillaPose2; @@ -333,7 +331,7 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) { EXPECT(assert_equal(pose_above, result.at(x3), 1e-6)); } -/* *************************************************************************/ +/* ************************************************************************* TEST( SmartProjectionPoseFactor, Factors ) { using namespace vanillaPose; @@ -497,7 +495,7 @@ TEST( SmartProjectionPoseFactor, Factors ) { } } -/* *************************************************************************/ +/* ************************************************************************* TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) { using namespace vanillaPose; @@ -551,7 +549,7 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) { EXPECT(assert_equal(pose_above, result.at(x3), 1e-7)); } -/* *************************************************************************/ +/* ************************************************************************* TEST( SmartProjectionPoseFactor, jacobianSVD ) { using namespace vanillaPose; @@ -607,7 +605,7 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ) { EXPECT(assert_equal(pose_above, result.at(x3), 1e-6)); } -/* *************************************************************************/ +/* ************************************************************************* TEST( SmartProjectionPoseFactor, landmarkDistance ) { using namespace vanillaPose; @@ -666,7 +664,7 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ) { EXPECT(assert_equal(values.at(x3), result.at(x3))); } -/* *************************************************************************/ +/* ************************************************************************* TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) { using namespace vanillaPose; @@ -732,58 +730,7 @@ TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) { EXPECT(assert_equal(cam3.pose(), result.at(x3))); } -/* *************************************************************************/ -TEST( SmartProjectionPoseFactor, jacobianQ ) { - - using namespace vanillaPose; - - KeyVector views {x1, x2, x3}; - - 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); - - SmartProjectionParams params; - params.setLinearizationMode(gtsam::JACOBIAN_Q); - - SmartFactor::shared_ptr smartFactor1( - new SmartFactor(model, sharedK, params)); - smartFactor1->add(measurements_cam1, views); - - SmartFactor::shared_ptr smartFactor2( - new SmartFactor(model, sharedK, params)); - smartFactor2->add(measurements_cam2, views); - - SmartFactor::shared_ptr smartFactor3( - new SmartFactor(model, sharedK, params)); - smartFactor3->add(measurements_cam3, views); - - const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - - NonlinearFactorGraph graph; - graph.push_back(smartFactor1); - graph.push_back(smartFactor2); - graph.push_back(smartFactor3); - graph.addPrior(x1, cam1.pose(), noisePrior); - graph.addPrior(x2, cam2.pose(), noisePrior); - - Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), - Point3(0.1, 0.1, 0.1)); // smaller noise - Values values; - values.insert(x1, cam1.pose()); - values.insert(x2, cam2.pose()); - values.insert(x3, pose_above * noise_pose); - - Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); - result = optimizer.optimize(); - EXPECT(assert_equal(pose_above, result.at(x3), 1e-6)); -} - -/* *************************************************************************/ +/* ************************************************************************* TEST( SmartProjectionPoseFactor, 3poses_projection_factor ) { using namespace vanillaPose2; @@ -830,7 +777,7 @@ TEST( SmartProjectionPoseFactor, 3poses_projection_factor ) { EXPECT(assert_equal(pose_above, result.at(x3), 1e-7)); } -/* *************************************************************************/ +/* ************************************************************************* TEST( SmartProjectionPoseFactor, CheckHessian) { KeyVector views {x1, x2, x3}; @@ -912,144 +859,7 @@ TEST( SmartProjectionPoseFactor, CheckHessian) { EXPECT(assert_equal(InfoVector, GaussianGraph->hessian().second, 1e-6)); } -/* *************************************************************************/ -TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_factor ) { - using namespace vanillaPose2; - - KeyVector views {x1, x2, x3}; - - // Two different cameras, at the same position, but different rotations - Pose3 pose2 = level_pose * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); - Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); - Camera cam2(pose2, sharedK2); - Camera cam3(pose3, sharedK2); - - Point2Vector measurements_cam1, measurements_cam2; - - // Project three landmarks into three cameras - projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); - projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); - - SmartProjectionParams params; - params.setRankTolerance(50); - params.setDegeneracyMode(gtsam::HANDLE_INFINITY); - - SmartFactor::shared_ptr smartFactor1( - new SmartFactor(model, sharedK2, params)); - smartFactor1->add(measurements_cam1, views); - - SmartFactor::shared_ptr smartFactor2( - new SmartFactor(model, sharedK2, params)); - smartFactor2->add(measurements_cam2, views); - - const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, 0.10); - Point3 positionPrior = Point3(0, 0, 1); - - NonlinearFactorGraph graph; - graph.push_back(smartFactor1); - graph.push_back(smartFactor2); - graph.addPrior(x1, cam1.pose(), noisePrior); - graph.emplace_shared >(x2, positionPrior, noisePriorTranslation); - graph.emplace_shared >(x3, positionPrior, noisePriorTranslation); - - Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), - Point3(0.1, 0.1, 0.1)); // smaller noise - Values values; - values.insert(x1, cam1.pose()); - values.insert(x2, pose2 * noise_pose); - values.insert(x3, pose3 * noise_pose); - - // params.verbosityLM = LevenbergMarquardtParams::SUMMARY; - LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); - Values result = optimizer.optimize(); - EXPECT(assert_equal(pose3, result.at(x3))); -} - -/* *************************************************************************/ -TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ) { - - // this test considers a condition in which the cheirality constraint is triggered - using namespace vanillaPose; - - KeyVector views {x1, x2, x3}; - - // Two different cameras, at the same position, but different rotations - Pose3 pose2 = level_pose - * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); - Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); - Camera cam2(pose2, sharedK); - Camera cam3(pose3, sharedK); - - 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); - - SmartProjectionParams params; - params.setRankTolerance(10); - params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); - - SmartFactor::shared_ptr smartFactor1( - new SmartFactor(model, sharedK, params)); - smartFactor1->add(measurements_cam1, views); - - SmartFactor::shared_ptr smartFactor2( - new SmartFactor(model, sharedK, params)); - smartFactor2->add(measurements_cam2, views); - - SmartFactor::shared_ptr smartFactor3( - new SmartFactor(model, sharedK, params)); - smartFactor3->add(measurements_cam3, views); - - const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, - 0.10); - Point3 positionPrior = Point3(0, 0, 1); - - NonlinearFactorGraph graph; - graph.push_back(smartFactor1); - graph.push_back(smartFactor2); - graph.push_back(smartFactor3); - graph.addPrior(x1, cam1.pose(), noisePrior); - graph.emplace_shared >(x2, positionPrior, noisePriorTranslation); - graph.emplace_shared >(x3, positionPrior, noisePriorTranslation); - - // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), - Point3(0.1, 0.1, 0.1)); // smaller noise - Values values; - values.insert(x1, cam1.pose()); - values.insert(x2, cam2.pose()); - values.insert(x3, pose3 * noise_pose); - EXPECT( - assert_equal( - Pose3( - Rot3(0.00563056869, -0.130848107, 0.991386438, -0.991390265, - -0.130426831, -0.0115837907, 0.130819108, -0.98278564, - -0.130455917), - Point3(0.0897734171, -0.110201006, 0.901022872)), - values.at(x3))); - - Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); - result = optimizer.optimize(); - - // Since we do not do anything on degenerate instances (ZERO_ON_DEGENERACY) - // rotation remains the same as the initial guess, but position is fixed by PoseTranslationPrior -#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION - EXPECT(assert_equal(Pose3(values.at(x3).rotation(), - Point3(0,0,1)), result.at(x3))); -#else - // if the check is disabled, no cheirality exception if thrown and the pose converges to the right rotation - // with modest accuracy since the configuration is essentially degenerate without the translation due to noise (noise_pose) - EXPECT(assert_equal(pose3, result.at(x3),1e-3)); -#endif -} - -/* *************************************************************************/ +/* ************************************************************************* TEST( SmartProjectionPoseFactor, Hessian ) { using namespace vanillaPose2; @@ -1080,299 +890,6 @@ TEST( SmartProjectionPoseFactor, Hessian ) { // check that it is correctly scaled when using noiseProjection = [1/4 0; 0 1/4] } -/* *************************************************************************/ -TEST( SmartProjectionPoseFactor, HessianWithRotation ) { - // cout << " ************************ SmartProjectionPoseFactor: rotated Hessian **********************" << endl; - - using namespace vanillaPose; - - KeyVector views {x1, x2, x3}; - - Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; - - projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); - - SmartFactor::shared_ptr smartFactorInstance(new SmartFactor(model, sharedK)); - smartFactorInstance->add(measurements_cam1, views); - - Values values; - values.insert(x1, cam1.pose()); - values.insert(x2, cam2.pose()); - values.insert(x3, cam3.pose()); - - boost::shared_ptr factor = smartFactorInstance->linearize( - values); - - Pose3 poseDrift = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0)); - - Values rotValues; - rotValues.insert(x1, poseDrift.compose(level_pose)); - rotValues.insert(x2, poseDrift.compose(pose_right)); - rotValues.insert(x3, poseDrift.compose(pose_above)); - - boost::shared_ptr factorRot = smartFactorInstance->linearize( - rotValues); - - // Hessian is invariant to rotations in the nondegenerate case - EXPECT(assert_equal(factor->information(), factorRot->information(), 1e-7)); - - Pose3 poseDrift2 = Pose3(Rot3::Ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2), - Point3(10, -4, 5)); - - Values tranValues; - tranValues.insert(x1, poseDrift2.compose(level_pose)); - tranValues.insert(x2, poseDrift2.compose(pose_right)); - tranValues.insert(x3, poseDrift2.compose(pose_above)); - - boost::shared_ptr factorRotTran = - smartFactorInstance->linearize(tranValues); - - // Hessian is invariant to rotations and translations in the nondegenerate case - EXPECT(assert_equal(factor->information(), factorRotTran->information(), 1e-7)); -} - -/* *************************************************************************/ -TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ) { - - using namespace vanillaPose2; - - KeyVector views {x1, x2, x3}; - - // All cameras have the same pose so will be degenerate ! - Camera cam2(level_pose, sharedK2); - Camera cam3(level_pose, sharedK2); - - Point2Vector measurements_cam1; - projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); - - SmartFactor::shared_ptr smartFactor(new SmartFactor(model, sharedK2)); - smartFactor->add(measurements_cam1, views); - - Values values; - values.insert(x1, cam1.pose()); - values.insert(x2, cam2.pose()); - values.insert(x3, cam3.pose()); - - boost::shared_ptr factor = smartFactor->linearize(values); - - Pose3 poseDrift = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0)); - - Values rotValues; - rotValues.insert(x1, poseDrift.compose(level_pose)); - rotValues.insert(x2, poseDrift.compose(level_pose)); - rotValues.insert(x3, poseDrift.compose(level_pose)); - - boost::shared_ptr factorRot = // - smartFactor->linearize(rotValues); - - // Hessian is invariant to rotations in the nondegenerate case - EXPECT(assert_equal(factor->information(), factorRot->information(), 1e-7)); - - Pose3 poseDrift2 = Pose3(Rot3::Ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2), - Point3(10, -4, 5)); - - Values tranValues; - tranValues.insert(x1, poseDrift2.compose(level_pose)); - tranValues.insert(x2, poseDrift2.compose(level_pose)); - tranValues.insert(x3, poseDrift2.compose(level_pose)); - - boost::shared_ptr factorRotTran = smartFactor->linearize( - tranValues); - - // Hessian is invariant to rotations and translations in the nondegenerate case - EXPECT(assert_equal(factor->information(), factorRotTran->information(), 1e-7)); -} - -/* ************************************************************************* */ -TEST( SmartProjectionPoseFactor, ConstructorWithCal3Bundler) { - using namespace bundlerPose; - SmartProjectionParams params; - params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); - SmartFactor factor(model, sharedBundlerK, params); - factor.add(measurement1, x1); -} - -/* *************************************************************************/ -TEST( SmartProjectionPoseFactor, Cal3Bundler ) { - - using namespace bundlerPose; - - // three landmarks ~5 meters in front of camera - Point3 landmark3(3, 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); - - KeyVector views {x1, x2, x3}; - - SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedBundlerK)); - smartFactor1->add(measurements_cam1, views); - - SmartFactor::shared_ptr smartFactor2(new SmartFactor(model, sharedBundlerK)); - smartFactor2->add(measurements_cam2, views); - - SmartFactor::shared_ptr smartFactor3(new SmartFactor(model, sharedBundlerK)); - smartFactor3->add(measurements_cam3, views); - - const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - - NonlinearFactorGraph graph; - graph.push_back(smartFactor1); - graph.push_back(smartFactor2); - graph.push_back(smartFactor3); - graph.addPrior(x1, cam1.pose(), noisePrior); - graph.addPrior(x2, cam2.pose(), noisePrior); - - // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), - Point3(0.1, 0.1, 0.1)); // smaller noise - Values values; - values.insert(x1, cam1.pose()); - values.insert(x2, cam2.pose()); - // initialize third pose with some noise, we expect it to move back to original pose_above - values.insert(x3, pose_above * noise_pose); - EXPECT( - assert_equal( - Pose3( - Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, - -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), - Point3(0.1, -0.1, 1.9)), values.at(x3))); - - Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); - result = optimizer.optimize(); - EXPECT(assert_equal(cam3.pose(), result.at(x3), 1e-6)); -} - -/* *************************************************************************/ -TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) { - - using namespace bundlerPose; - - KeyVector views {x1, x2, x3}; - - // Two different cameras - Pose3 pose2 = level_pose - * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); - Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); - Camera cam2(pose2, sharedBundlerK); - Camera cam3(pose3, sharedBundlerK); - - // landmark3 at 3 meters now - Point3 landmark3(3, 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); - - SmartProjectionParams params; - params.setRankTolerance(10); - params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); - - SmartFactor::shared_ptr smartFactor1( - new SmartFactor(model, sharedBundlerK, params)); - smartFactor1->add(measurements_cam1, views); - - SmartFactor::shared_ptr smartFactor2( - new SmartFactor(model, sharedBundlerK, params)); - smartFactor2->add(measurements_cam2, views); - - SmartFactor::shared_ptr smartFactor3( - new SmartFactor(model, sharedBundlerK, params)); - smartFactor3->add(measurements_cam3, views); - - const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, - 0.10); - Point3 positionPrior = Point3(0, 0, 1); - - NonlinearFactorGraph graph; - graph.push_back(smartFactor1); - graph.push_back(smartFactor2); - graph.push_back(smartFactor3); - graph.addPrior(x1, cam1.pose(), noisePrior); - graph.emplace_shared >(x2, positionPrior, noisePriorTranslation); - graph.emplace_shared >(x3, positionPrior, noisePriorTranslation); - - // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), - Point3(0.1, 0.1, 0.1)); // smaller noise - Values values; - values.insert(x1, cam1.pose()); - values.insert(x2, cam2.pose()); - // initialize third pose with some noise, we expect it to move back to original pose_above - values.insert(x3, pose3 * noise_pose); - EXPECT( - assert_equal( - Pose3( - Rot3(0.00563056869, -0.130848107, 0.991386438, -0.991390265, - -0.130426831, -0.0115837907, 0.130819108, -0.98278564, - -0.130455917), - Point3(0.0897734171, -0.110201006, 0.901022872)), - values.at(x3))); - - Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); - result = optimizer.optimize(); - - EXPECT( - assert_equal( - Pose3( - Rot3(0.00563056869, -0.130848107, 0.991386438, -0.991390265, - -0.130426831, -0.0115837907, 0.130819108, -0.98278564, - -0.130455917), - Point3(0.0897734171, -0.110201006, 0.901022872)), - values.at(x3))); -} - -/* ************************************************************************* */ -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic"); -BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel"); -BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); - -TEST(SmartProjectionPoseFactor, serialize) { - using namespace vanillaPose; - using namespace gtsam::serializationTestHelpers; - SmartProjectionParams params; - params.setRankTolerance(rankTol); - SmartFactor factor(model, sharedK, params); - - EXPECT(equalsObj(factor)); - EXPECT(equalsXML(factor)); - EXPECT(equalsBinary(factor)); -} - -TEST(SmartProjectionPoseFactor, serialize2) { - using namespace vanillaPose; - using namespace gtsam::serializationTestHelpers; - SmartProjectionParams params; - params.setRankTolerance(rankTol); - Pose3 bts; - SmartFactor factor(model, sharedK, bts, params); - - // insert some measurments - KeyVector key_view; - Point2Vector meas_view; - key_view.push_back(Symbol('x', 1)); - meas_view.push_back(Point2(10, 10)); - factor.add(meas_view, key_view); - - EXPECT(equalsObj(factor)); - EXPECT(equalsXML(factor)); - EXPECT(equalsBinary(factor)); -} - /* ************************************************************************* */ int main() { TestResult tr;