12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

setting up .h and tests - compiles and tests pass.

release/4.3a0
lcarlone 2021-07-19 15:30:53 -04:00
parent cd1d4b4df5
commit 00387b32cd
3 changed files with 356 additions and 834 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

187
gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.cpp
View File

@ -10,116 +10,95 @@
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/** /**
* @file SmartProjectionProjectionPoseFactorRollingShutter.h * @file SmartProjectionPoseFactorRollingShutter.h
* @brief Smart projection factor on poses and extrinsic calibration * @brief Smart projection factor on poses modeling rolling shutter effect
* @author Luca Carlone * @author Luca Carlone
* @author Frank Dellaert
*/ */
#include <gtsam_unstable/slam/SmartStereoProjectionFactorPP.h> //#include <gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h>
namespace gtsam { namespace gtsam {
SmartStereoProjectionFactorPP::SmartStereoProjectionFactorPP(
const SharedNoiseModel& sharedNoiseModel,
const SmartStereoProjectionParams& params)
: Base(sharedNoiseModel, params) {} // note: no extrinsic specified!
void SmartStereoProjectionFactorPP::add( //
const StereoPoint2& measured, //void SmartProjectionPoseFactorRollingShutter::add(
const Key& w_P_body_key, const Key& body_P_cam_key, // const std::vector<StereoPoint2>& measurements,
const boost::shared_ptr<Cal3_S2Stereo>& K) { // const KeyVector& world_P_body_keys, const KeyVector& body_P_cam_keys,
// we index by cameras.. // const std::vector<boost::shared_ptr<Cal3_S2Stereo>>& Ks) {
Base::add(measured, w_P_body_key); // assert(world_P_body_keys.size() == measurements.size());
// but we also store the extrinsic calibration keys in the same order // assert(world_P_body_keys.size() == body_P_cam_keys.size());
world_P_body_keys_.push_back(w_P_body_key); // assert(world_P_body_keys.size() == Ks.size());
body_P_cam_keys_.push_back(body_P_cam_key); // 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 // // 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()) // if(std::find(keys_.begin(), keys_.end(), body_P_cam_keys[i]) == keys_.end())
keys_.push_back(body_P_cam_key); // add only unique keys // keys_.push_back(body_P_cam_keys[i]); // add only unique keys
//
K_all_.push_back(K); // world_P_body_keys_.push_back(world_P_body_keys[i]);
} // body_P_cam_keys_.push_back(body_P_cam_keys[i]);
//
void SmartStereoProjectionFactorPP::add( // K_all_.push_back(Ks[i]);
const std::vector<StereoPoint2>& measurements, // }
const KeyVector& world_P_body_keys, const KeyVector& body_P_cam_keys, //}
const std::vector<boost::shared_ptr<Cal3_S2Stereo>>& Ks) { //
assert(world_P_body_keys.size() == measurements.size()); //void SmartProjectionPoseFactorRollingShutter::add(
assert(world_P_body_keys.size() == body_P_cam_keys.size()); // const std::vector<StereoPoint2>& measurements,
assert(world_P_body_keys.size() == Ks.size()); // const KeyVector& world_P_body_keys, const KeyVector& body_P_cam_keys,
for (size_t i = 0; i < measurements.size(); i++) { // const boost::shared_ptr<Cal3_S2Stereo>& K) {
Base::add(measurements[i], world_P_body_keys[i]); // assert(world_P_body_keys.size() == measurements.size());
// pose keys are assumed to be unique (1 observation per time stamp), but calibration can be shared // assert(world_P_body_keys.size() == body_P_cam_keys.size());
if(std::find(keys_.begin(), keys_.end(), body_P_cam_keys[i]) == keys_.end()) // for (size_t i = 0; i < measurements.size(); i++) {
keys_.push_back(body_P_cam_keys[i]); // add only unique keys // 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
world_P_body_keys_.push_back(world_P_body_keys[i]); // if(std::find(keys_.begin(), keys_.end(), body_P_cam_keys[i]) == keys_.end())
body_P_cam_keys_.push_back(body_P_cam_keys[i]); // keys_.push_back(body_P_cam_keys[i]); // add only unique keys
//
K_all_.push_back(Ks[i]); // 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::add( // }
const std::vector<StereoPoint2>& measurements, //}
const KeyVector& world_P_body_keys, const KeyVector& body_P_cam_keys, //
const boost::shared_ptr<Cal3_S2Stereo>& K) { //void SmartProjectionPoseFactorRollingShutter::print(
assert(world_P_body_keys.size() == measurements.size()); // const std::string& s, const KeyFormatter& keyFormatter) const {
assert(world_P_body_keys.size() == body_P_cam_keys.size()); // std::cout << s << "SmartProjectionPoseFactorRollingShutter: \n ";
for (size_t i = 0; i < measurements.size(); i++) { // for (size_t i = 0; i < K_all_.size(); i++) {
Base::add(measurements[i], world_P_body_keys[i]); // K_all_[i]->print("calibration = ");
// pose keys are assumed to be unique (1 observation per time stamp), but calibration can be shared // std::cout << " extrinsic pose key: " << keyFormatter(body_P_cam_keys_[i]) << std::endl;
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 // Base::print("", keyFormatter);
//}
world_P_body_keys_.push_back(world_P_body_keys[i]); //
body_P_cam_keys_.push_back(body_P_cam_keys[i]); //bool SmartProjectionPoseFactorRollingShutter::equals(const NonlinearFactor& p,
// double tol) const {
K_all_.push_back(K); // const SmartProjectionPoseFactorRollingShutter* e =
} // dynamic_cast<const SmartProjectionPoseFactorRollingShutter*>(&p);
} //
// return e && Base::equals(p, tol) &&
void SmartStereoProjectionFactorPP::print( // body_P_cam_keys_ == e->getExtrinsicPoseKeys();
const std::string& s, const KeyFormatter& keyFormatter) const { //}
std::cout << s << "SmartStereoProjectionFactorPP: \n "; //
for (size_t i = 0; i < K_all_.size(); i++) { //double SmartProjectionPoseFactorRollingShutter::error(const Values& values) const {
K_all_[i]->print("calibration = "); // if (this->active(values)) {
std::cout << " extrinsic pose key: " << keyFormatter(body_P_cam_keys_[i]) << std::endl; // return this->totalReprojectionError(cameras(values));
} // } else { // else of active flag
Base::print("", keyFormatter); // return 0.0;
} // }
//}
bool SmartStereoProjectionFactorPP::equals(const NonlinearFactor& p, //
double tol) const { //SmartProjectionPoseFactorRollingShutter::Base::Cameras
const SmartStereoProjectionFactorPP* e = //SmartProjectionPoseFactorRollingShutter::cameras(const Values& values) const {
dynamic_cast<const SmartStereoProjectionFactorPP*>(&p); // assert(world_P_body_keys_.size() == K_all_.size());
// assert(world_P_body_keys_.size() == body_P_cam_keys_.size());
return e && Base::equals(p, tol) && // Base::Cameras cameras;
body_P_cam_keys_ == e->getExtrinsicPoseKeys(); // for (size_t i = 0; i < world_P_body_keys_.size(); i++) {
} // Pose3 w_P_body = values.at<Pose3>(world_P_body_keys_[i]);
// Pose3 body_P_cam = values.at<Pose3>(body_P_cam_keys_[i]);
double SmartStereoProjectionFactorPP::error(const Values& values) const { // Pose3 w_P_cam = w_P_body.compose(body_P_cam);
if (this->active(values)) { // cameras.push_back(StereoCamera(w_P_cam, K_all_[i]));
return this->totalReprojectionError(cameras(values)); // }
} else { // else of active flag // return cameras;
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<Pose3>(world_P_body_keys_[i]);
Pose3 body_P_cam = values.at<Pose3>(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 } // \ namespace gtsam

474
gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h
View File

@ -10,15 +10,14 @@
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/** /**
* @file SmartStereoProjectionFactorPP.h * @file SmartProjectionPoseFactorRollingShutter.h
* @brief Smart stereo factor on poses (P) and camera extrinsic pose (P) calibrations * @brief Smart projection factor on poses modeling rolling shutter effect with given readout time
* @author Luca Carlone * @author Luca Carlone
* @author Frank Dellaert
*/ */
#pragma once #pragma once
#include <gtsam_unstable/slam/SmartStereoProjectionFactor.h> #include <gtsam/slam/SmartProjectionFactor.h>
namespace gtsam { 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). * This factor optimizes the pose of the body assuming a rolling shutter model of the camera with given readout time.
* Each camera may have its own extrinsic calibration or the same calibration can be shared by multiple cameras. * This factor requires that values contain (for each pixel observation) consecutive camera poses
* This factor requires that values contain the involved poses and extrinsics (both are Pose3 variables). * from which the pixel observation pose can be interpolated.
* @addtogroup SLAM * @addtogroup SLAM
*/ */
class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor { template<class CALIBRATION>
class SmartProjectionPoseFactorRollingShutter: public SmartProjectionFactor<
PinholePose<CALIBRATION> > {
protected: protected:
/// shared pointer to calibration object (one for each camera) /// shared pointer to calibration object (one for each observation)
std::vector<boost::shared_ptr<Cal3_S2Stereo>> K_all_; std::vector<boost::shared_ptr<CALIBRATION> > K_all_;
/// The keys corresponding to the pose of the body (with respect to an external world frame) for each view // The keys of the pose of the body (with respect to an external world frame): two consecutive poses for each observation
KeyVector world_P_body_keys_; std::vector<std::pair<Key,Key>> world_P_body_key_pairs_;
/// The keys corresponding to the extrinsic pose calibration for each view (pose that transform from camera to body) // interpolation factor (one for each observation) to interpolate between pair of consecutive poses
KeyVector body_P_cam_keys_; std::vector<double> gammas_;
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef PinholePose<CALIBRATION> Camera;
/// shorthand for base class type /// shorthand for base class type
typedef SmartStereoProjectionFactor Base; typedef SmartProjectionFactor<Camera> Base;
/// shorthand for this class /// shorthand for this class
typedef SmartStereoProjectionFactorPP This; typedef SmartProjectionPoseFactorRollingShutter This;
/// shorthand for a smart pointer to a factor /// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<This> shared_ptr; typedef boost::shared_ptr<This> shared_ptr;
static const int Dim = 12; ///< Camera dimension: 6 for body pose, 6 for extrinsic pose static const int Dim = 6; ///< Pose3 dimension
static const int DimPose = 6; ///< Pose3 dimension static const int ZDim = 2; ///< Measurement dimension (Point2)
static const int ZDim = 3; ///< Measurement dimension (for a StereoPoint2 measurement)
typedef Eigen::Matrix<double, ZDim, Dim> MatrixZD; // F blocks (derivatives wrt camera) typedef Eigen::Matrix<double, ZDim, Dim> MatrixZD; // F blocks (derivatives wrt camera)
typedef std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> > FBlocks; // vector of F blocks typedef std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> > FBlocks; // vector of F blocks
@ -72,51 +74,67 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
* @param Isotropic measurement noise * @param Isotropic measurement noise
* @param params internal parameters of the smart factors * @param params internal parameters of the smart factors
*/ */
SmartStereoProjectionFactorPP(const SharedNoiseModel& sharedNoiseModel, SmartProjectionPoseFactorRollingShutter(
const SmartStereoProjectionParams& params = const SharedNoiseModel& sharedNoiseModel,
SmartStereoProjectionParams()); const SmartProjectionParams& params = SmartProjectionParams())
: Base(sharedNoiseModel, params) {}
/** Virtual destructor */ /** Virtual destructor */
~SmartStereoProjectionFactorPP() override = default; ~SmartProjectionPoseFactorRollingShutter() override = default;
/** /**
* add a new measurement, with a pose key, and an extrinsic pose key * add a new measurement, with 2 pose keys, camera calibration, and observed pixel.
* @param measured is the 3-dimensional location of the projection of a * @param measured is the 2-dimensional location of the projection of a
* single landmark in the a single (stereo) view (the measurement) * single landmark in the a single view (the measurement), interpolated from the 2 poses
* @param world_P_body_key is the key corresponding to the body poses observing the same landmark * @param world_P_body_key1 is the key corresponding to the first body poses (time <= time pixel is acquired)
* @param body_P_cam_key is the key corresponding to the extrinsic camera-to-body pose calibration * @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 * @param K is the (fixed) camera intrinsic calibration
*/ */
void add(const StereoPoint2& measured, const Key& world_P_body_key, void add(const Point2& measured,
const Key& body_P_cam_key, const Key& world_P_body_key1,
const boost::shared_ptr<Cal3_S2Stereo>& K); const Key& world_P_body_key2,
const double& gamma,
const boost::shared_ptr<CALIBRATION>& 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 * Variant of the previous one in which we include a set of measurements
* @param measurements vector of the 3m dimensional location of the projection * @param measurements vector of the 2m dimensional location of the projection
* of a single landmark in the m (stereo) view (the measurements) * of a single landmark in the m views (the measurements)
* @param w_P_body_keys are the ordered keys corresponding to the body poses observing the same landmark * @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 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)
* @param Ks vector of intrinsic calibration objects * @param Ks vector of intrinsic calibration objects
*/ */
void add(const std::vector<StereoPoint2>& measurements, // void add(const std::vector<Point2>& measurements,
const KeyVector& w_P_body_keys, const KeyVector& body_P_cam_keys, // const std::vector<std::pair<Key,Key>>& world_P_body_key_pairs,
const std::vector<boost::shared_ptr<Cal3_S2Stereo>>& Ks); // const std::vector<double>& gammas,
// const std::vector<boost::shared_ptr<CALIBRATION>>& Ks);
/** /**
* Variant of the previous one in which we include a set of measurements with * Variant of the previous one in which we include a set of measurements with
* the same noise and calibration * the same calibration
* @param measurements vector of the 3m dimensional location of the projection * @param measurements vector of the 2m dimensional location of the projection
* of a single landmark in the m (stereo) view (the measurements) * of a single landmark in the m views (the measurements)
* @param w_P_body_keys are the ordered keys corresponding to the body poses observing the same landmark * @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 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)
* @param K the (known) camera calibration (same for all measurements) * @param K the (known) camera calibration (same for all measurements)
*/ */
void add(const std::vector<StereoPoint2>& measurements, // void add(const std::vector<Point2>& measurements,
const KeyVector& w_P_body_keys, const KeyVector& body_P_cam_keys, // const std::vector<std::pair<Key,Key>>& world_P_body_key_pairs,
const boost::shared_ptr<Cal3_S2Stereo>& K); // const std::vector<double>& gammas,
// const boost::shared_ptr<CALIBRATION>& K);
/** /**
* print * print
@ -130,8 +148,8 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
bool equals(const NonlinearFactor& p, double tol = 1e-9) const override; bool equals(const NonlinearFactor& p, double tol = 1e-9) const override;
/// equals /// equals
const KeyVector& getExtrinsicPoseKeys() const { const std::vector<double> getGammas() const {
return body_P_cam_keys_; return gammas_;
} }
/** /**
@ -140,18 +158,26 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
double error(const Values& values) const override; double error(const Values& values) const override;
/** return the calibration object */ /** return the calibration object */
inline std::vector<boost::shared_ptr<Cal3_S2Stereo>> calibration() const { inline std::vector<boost::shared_ptr<CALIBRATION>> calibration() const {
return K_all_; return K_all_;
} }
/** /**
* Collect all cameras involved in this factor * Collect all cameras involved in this factor
* @param values Values structure which must contain camera poses * @param values Values structure which must contain camera poses
* corresponding * corresponding to keys involved in this factor
* to keys involved in this factor * @return Cameras
* @return vector of Values
*/ */
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<Pose3>(k) * *Base::body_P_sensor_
// : values.at<Pose3>(k);
// cameras.emplace_back(world_P_sensor_k, K_);
}
return cameras;
}
/** /**
* Compute jacobian F, E and error vector at a given linearization point * 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, * 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. * and the error vector b. Note that the jacobians are computed for a given point.
*/ */
void computeJacobiansAndCorrectForMissingMeasurements( // void computeJacobiansAndCorrectForMissingMeasurements(
FBlocks& Fs, Matrix& E, Vector& b, const Values& values) const { // FBlocks& Fs, Matrix& E, Vector& b, const Values& values) const {
if (!result_) { // if (!result_) {
throw("computeJacobiansWithTriangulatedPoint"); // throw("computeJacobiansWithTriangulatedPoint");
} else { // valid result: compute jacobians // } else { // valid result: compute jacobians
size_t numViews = measured_.size(); // size_t numViews = measured_.size();
E = Matrix::Zero(3 * numViews, 3); // a StereoPoint2 for each view (point jacobian) // E = Matrix::Zero(3 * numViews, 3); // a StereoPoint2 for each view (point jacobian)
b = Vector::Zero(3 * numViews); // a StereoPoint2 for each view // b = Vector::Zero(3 * numViews); // a StereoPoint2 for each view
Matrix dPoseCam_dPoseBody_i, dPoseCam_dPoseExt_i, dProject_dPoseCam_i, Ei; // Matrix dPoseCam_dPoseBody_i, dPoseCam_dPoseExt_i, dProject_dPoseCam_i, Ei;
//
for (size_t i = 0; i < numViews; i++) { // for each camera/measurement // for (size_t i = 0; i < numViews; i++) { // for each camera/measurement
Pose3 w_P_body = values.at<Pose3>(world_P_body_keys_.at(i)); // Pose3 w_P_body = values.at<Pose3>(world_P_body_key_pairs_.at(i));
Pose3 body_P_cam = values.at<Pose3>(body_P_cam_keys_.at(i)); // Pose3 body_P_cam = values.at<Pose3>(body_P_cam_ this->keys_.at(i));
StereoCamera camera( // StereoCamera camera(
w_P_body.compose(body_P_cam, dPoseCam_dPoseBody_i, dPoseCam_dPoseExt_i), // w_P_body.compose(body_P_cam, dPoseCam_dPoseBody_i, dPoseCam_dPoseExt_i),
K_all_[i]); // K_all_[i]);
// get jacobians and error vector for current measurement // // get jacobians and error vector for current measurement
StereoPoint2 reprojectionError_i = StereoPoint2( // StereoPoint2 reprojectionError_i = StereoPoint2(
camera.project(*result_, dProject_dPoseCam_i, Ei) - measured_.at(i)); // camera.project(*result_, dProject_dPoseCam_i, Ei) - measured_.at(i));
Eigen::Matrix<double, ZDim, Dim> J; // 3 x 12 // Eigen::Matrix<double, ZDim, Dim> J; // 3 x 12
J.block<ZDim, 6>(0, 0) = dProject_dPoseCam_i * dPoseCam_dPoseBody_i; // (3x6) * (6x6) // J.block<ZDim, 6>(0, 0) = dProject_dPoseCam_i * dPoseCam_dPoseBody_i; // (3x6) * (6x6)
J.block<ZDim, 6>(0, 6) = dProject_dPoseCam_i * dPoseCam_dPoseExt_i; // (3x6) * (6x6) // J.block<ZDim, 6>(0, 6) = dProject_dPoseCam_i * dPoseCam_dPoseExt_i; // (3x6) * (6x6)
// if the right pixel is invalid, fix jacobians // // if the right pixel is invalid, fix jacobians
if (std::isnan(measured_.at(i).uR())) // if (std::isnan(measured_.at(i).uR()))
{ // {
J.block<1, 12>(1, 0) = Matrix::Zero(1, 12); // J.block<1, 12>(1, 0) = Matrix::Zero(1, 12);
Ei.block<1, 3>(1, 0) = Matrix::Zero(1, 3); // Ei.block<1, 3>(1, 0) = Matrix::Zero(1, 3);
reprojectionError_i = StereoPoint2(reprojectionError_i.uL(), 0.0, // reprojectionError_i = StereoPoint2(reprojectionError_i.uL(), 0.0,
reprojectionError_i.v()); // reprojectionError_i.v());
} // }
// fit into the output structures // // fit into the output structures
Fs.push_back(J); // Fs.push_back(J);
size_t row = 3 * i; // size_t row = 3 * i;
b.segment<ZDim>(row) = -reprojectionError_i.vector(); // b.segment<ZDim>(row) = -reprojectionError_i.vector();
E.block<3, 3>(row, 0) = Ei; // E.block<3, 3>(row, 0) = Ei;
} // }
} // }
} // }
/// linearize and return a Hessianfactor that is an approximation of error(p) /// linearize and return a Hessianfactor that is an approximation of error(p)
boost::shared_ptr<RegularHessianFactor<DimPose> > createHessianFactor( // boost::shared_ptr<RegularHessianFactor<DimPose> > createHessianFactor(
const Values& values, const double lambda = 0.0, bool diagonalDamping = // const Values& values, const double lambda = 0.0, bool diagonalDamping =
false) const { // false) const {
//
// we may have multiple cameras sharing the same extrinsic cals, hence the number // // 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 // // 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) // // have a body key and an extrinsic calibration key for each measurement)
size_t nrUniqueKeys = keys_.size(); // size_t nrUniqueKeys = this->keys_.size();
size_t nrNonuniqueKeys = world_P_body_keys_.size() // size_t nrNonuniqueKeys = world_P_body_key_pairs_.size()
+ body_P_cam_keys_.size(); // + body_P_cam_ this->keys_.size();
//
// Create structures for Hessian Factors // // Create structures for Hessian Factors
KeyVector js; // KeyVector js;
std::vector < Matrix > Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2); // std::vector < Matrix > Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2);
std::vector<Vector> gs(nrUniqueKeys); // std::vector<Vector> gs(nrUniqueKeys);
//
if (this->measured_.size() != cameras(values).size()) // if (this->measured_.size() != cameras(values).size())
throw std::runtime_error("SmartStereoProjectionHessianFactor: this->" // throw std::runtime_error("SmartStereoProjectionHessianFactor: this->"
"measured_.size() inconsistent with input"); // "measured_.size() inconsistent with input");
//
// triangulate 3D point at given linearization point // // triangulate 3D point at given linearization point
triangulateSafe(cameras(values)); // triangulateSafe(cameras(values));
//
if (!result_) { // failed: return "empty/zero" Hessian // if (!result_) { // failed: return "empty/zero" Hessian
for (Matrix& m : Gs) // for (Matrix& m : Gs)
m = Matrix::Zero(DimPose, DimPose); // m = Matrix::Zero(DimPose, DimPose);
for (Vector& v : gs) // for (Vector& v : gs)
v = Vector::Zero(DimPose); // v = Vector::Zero(DimPose);
return boost::make_shared < RegularHessianFactor<DimPose> // return boost::make_shared < RegularHessianFactor<DimPose>
> (keys_, Gs, gs, 0.0); // > ( this->keys_, Gs, gs, 0.0);
} // }
//
// compute Jacobian given triangulated 3D Point // // compute Jacobian given triangulated 3D Point
FBlocks Fs; // FBlocks Fs;
Matrix F, E; // Matrix F, E;
Vector b; // Vector b;
computeJacobiansAndCorrectForMissingMeasurements(Fs, E, b, values); // computeJacobiansAndCorrectForMissingMeasurements(Fs, E, b, values);
//
// Whiten using noise model // // Whiten using noise model
noiseModel_->WhitenSystem(E, b); // noiseModel_->WhitenSystem(E, b);
for (size_t i = 0; i < Fs.size(); i++) // for (size_t i = 0; i < Fs.size(); i++)
Fs[i] = noiseModel_->Whiten(Fs[i]); // Fs[i] = noiseModel_->Whiten(Fs[i]);
//
// build augmented Hessian (with last row/column being the information vector) // // build augmented Hessian (with last row/column being the information vector)
Matrix3 P; // Matrix3 P;
Cameras::ComputePointCovariance<3>(P, E, lambda, diagonalDamping); // Cameras::ComputePointCovariance<3>(P, E, lambda, diagonalDamping);
//
// marginalize point: note - we reuse the standard SchurComplement function // // marginalize point: note - we reuse the standard SchurComplement function
SymmetricBlockMatrix augmentedHessian = // SymmetricBlockMatrix augmentedHessian =
Cameras::SchurComplement<3, Dim>(Fs, E, P, b); // Cameras::SchurComplement<3, Dim>(Fs, E, P, b);
//
// now pack into an Hessian factor // // now pack into an Hessian factor
std::vector<DenseIndex> dims(nrUniqueKeys + 1); // this also includes the b term // std::vector<DenseIndex> dims(nrUniqueKeys + 1); // this also includes the b term
std::fill(dims.begin(), dims.end() - 1, 6); // std::fill(dims.begin(), dims.end() - 1, 6);
dims.back() = 1; // dims.back() = 1;
SymmetricBlockMatrix augmentedHessianUniqueKeys; // SymmetricBlockMatrix augmentedHessianUniqueKeys;
//
// here we have to deal with the fact that some cameras may share the same extrinsic key // // 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 // if (nrUniqueKeys == nrNonuniqueKeys) { // if there is 1 calibration key per camera
augmentedHessianUniqueKeys = SymmetricBlockMatrix( // augmentedHessianUniqueKeys = SymmetricBlockMatrix(
dims, Matrix(augmentedHessian.selfadjointView())); // dims, Matrix(augmentedHessian.selfadjointView()));
} else { // if multiple cameras share a calibration we have to rearrange // } else { // if multiple cameras share a calibration we have to rearrange
// the results of the Schur complement matrix // // the results of the Schur complement matrix
std::vector<DenseIndex> nonuniqueDims(nrNonuniqueKeys + 1); // this also includes the b term // std::vector<DenseIndex> nonuniqueDims(nrNonuniqueKeys + 1); // this also includes the b term
std::fill(nonuniqueDims.begin(), nonuniqueDims.end() - 1, 6); // std::fill(nonuniqueDims.begin(), nonuniqueDims.end() - 1, 6);
nonuniqueDims.back() = 1; // nonuniqueDims.back() = 1;
augmentedHessian = SymmetricBlockMatrix( // augmentedHessian = SymmetricBlockMatrix(
nonuniqueDims, Matrix(augmentedHessian.selfadjointView())); // nonuniqueDims, Matrix(augmentedHessian.selfadjointView()));
//
// these are the keys that correspond to the blocks in augmentedHessian (output of SchurComplement) // // these are the keys that correspond to the blocks in augmentedHessian (output of SchurComplement)
KeyVector nonuniqueKeys; // KeyVector nonuniqueKeys;
for (size_t i = 0; i < world_P_body_keys_.size(); i++) { // for (size_t i = 0; i < world_P_body_key_pairs_.size(); i++) {
nonuniqueKeys.push_back(world_P_body_keys_.at(i)); // nonuniqueKeys.push_back(world_P_body_key_pairs_.at(i));
nonuniqueKeys.push_back(body_P_cam_keys_.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) // // get map from key to location in the new augmented Hessian matrix (the one including only unique keys)
std::map<Key, size_t> keyToSlotMap; // std::map<Key, size_t> keyToSlotMap;
for (size_t k = 0; k < nrUniqueKeys; k++) { // for (size_t k = 0; k < nrUniqueKeys; k++) {
keyToSlotMap[keys_[k]] = k; // keyToSlotMap[ this->keys_[k]] = k;
} // }
//
// initialize matrix to zero // // initialize matrix to zero
augmentedHessianUniqueKeys = SymmetricBlockMatrix( // augmentedHessianUniqueKeys = SymmetricBlockMatrix(
dims, Matrix::Zero(6 * nrUniqueKeys + 1, 6 * nrUniqueKeys + 1)); // dims, Matrix::Zero(6 * nrUniqueKeys + 1, 6 * nrUniqueKeys + 1));
//
// add contributions for each key: note this loops over the hessian with nonUnique keys (augmentedHessian) // // 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) // // 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 // for (size_t i = 0; i < nrNonuniqueKeys; i++) { // rows
Key key_i = nonuniqueKeys.at(i); // Key key_i = nonuniqueKeys.at(i);
//
// update information vector // // update information vector
augmentedHessianUniqueKeys.updateOffDiagonalBlock( // augmentedHessianUniqueKeys.updateOffDiagonalBlock(
keyToSlotMap[key_i], nrUniqueKeys, // keyToSlotMap[key_i], nrUniqueKeys,
augmentedHessian.aboveDiagonalBlock(i, nrNonuniqueKeys)); // augmentedHessian.aboveDiagonalBlock(i, nrNonuniqueKeys));
//
// update blocks // // update blocks
for (size_t j = i; j < nrNonuniqueKeys; j++) { // cols // for (size_t j = i; j < nrNonuniqueKeys; j++) { // cols
Key key_j = nonuniqueKeys.at(j); // Key key_j = nonuniqueKeys.at(j);
if (i == j) { // if (i == j) {
augmentedHessianUniqueKeys.updateDiagonalBlock( // augmentedHessianUniqueKeys.updateDiagonalBlock(
keyToSlotMap[key_i], augmentedHessian.diagonalBlock(i)); // keyToSlotMap[key_i], augmentedHessian.diagonalBlock(i));
} else { // (i < j) // } else { // (i < j)
if (keyToSlotMap[key_i] != keyToSlotMap[key_j]) { // if (keyToSlotMap[key_i] != keyToSlotMap[key_j]) {
augmentedHessianUniqueKeys.updateOffDiagonalBlock( // augmentedHessianUniqueKeys.updateOffDiagonalBlock(
keyToSlotMap[key_i], keyToSlotMap[key_j], // keyToSlotMap[key_i], keyToSlotMap[key_j],
augmentedHessian.aboveDiagonalBlock(i, j)); // augmentedHessian.aboveDiagonalBlock(i, j));
} else { // } else {
augmentedHessianUniqueKeys.updateDiagonalBlock( // augmentedHessianUniqueKeys.updateDiagonalBlock(
keyToSlotMap[key_i], // keyToSlotMap[key_i],
augmentedHessian.aboveDiagonalBlock(i, j) // augmentedHessian.aboveDiagonalBlock(i, j)
+ augmentedHessian.aboveDiagonalBlock(i, j).transpose()); // + augmentedHessian.aboveDiagonalBlock(i, j).transpose());
} // }
} // }
} // }
} // }
// update bottom right element of the matrix // // update bottom right element of the matrix
augmentedHessianUniqueKeys.updateDiagonalBlock( // augmentedHessianUniqueKeys.updateDiagonalBlock(
nrUniqueKeys, augmentedHessian.diagonalBlock(nrNonuniqueKeys)); // nrUniqueKeys, augmentedHessian.diagonalBlock(nrNonuniqueKeys));
} // }
return boost::make_shared < RegularHessianFactor<DimPose> // return boost::make_shared < RegularHessianFactor<DimPose>
> (keys_, augmentedHessianUniqueKeys); // > ( this->keys_, augmentedHessianUniqueKeys);
} // }
/** /**
* Linearize to Gaussian Factor (possibly adding a damping factor Lambda for LM) * Linearize to Gaussian Factor (possibly adding a damping factor Lambda for LM)
@ -333,12 +359,12 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
boost::shared_ptr<GaussianFactor> linearizeDamped( boost::shared_ptr<GaussianFactor> linearizeDamped(
const Values& values, const double lambda = 0.0) const { const Values& values, const double lambda = 0.0) const {
// depending on flag set on construction we may linearize to different linear factors // depending on flag set on construction we may linearize to different linear factors
switch (params_.linearizationMode) { switch (this->params_.linearizationMode) {
case HESSIAN: // case HESSIAN:
return createHessianFactor(values, lambda); // return createHessianFactor(values, lambda);
default: default:
throw std::runtime_error( throw std::runtime_error(
"SmartStereoProjectionFactorPP: unknown linearization mode"); "SmartProjectionPoseFactorRollingShutter: unknown linearization mode");
} }
} }
@ -361,9 +387,9 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
// end of class declaration // end of class declaration
/// traits /// traits
template<> template<class CALIBRATION>
struct traits<SmartStereoProjectionFactorPP> : public Testable< struct traits<SmartProjectionPoseFactorRollingShutter<CALIBRATION> > : public Testable<
SmartStereoProjectionFactorPP> { SmartProjectionPoseFactorRollingShutter<CALIBRATION> > {
}; };
} // namespace gtsam } // namespace gtsam

529
gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp
View File

@ -10,19 +10,17 @@
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/** /**
* @file testSmartProjectionPoseFactor.cpp * @file testSmartProjectionPoseFactorRollingShutter.cpp
* @brief Unit tests for ProjectionFactor Class * @brief Unit tests for SmartProjectionPoseFactorRollingShutter Class
* @author Chris Beall
* @author Luca Carlone * @author Luca Carlone
* @author Zsolt Kira * @date July 2021
* @author Frank Dellaert
* @date Sept 2013
*/ */
#include "smartFactorScenarios.h" #include "gtsam/slam/tests/smartFactorScenarios.h"
#include <gtsam/slam/ProjectionFactor.h> #include <gtsam/slam/ProjectionFactor.h>
#include <gtsam/slam/PoseTranslationPrior.h> #include <gtsam/slam/PoseTranslationPrior.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h> #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h>
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/serializationTestHelpers.h> #include <gtsam/base/serializationTestHelpers.h>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
@ -52,13 +50,13 @@ LevenbergMarquardtParams lmParams;
// Make more verbose like so (in tests): // Make more verbose like so (in tests):
// params.verbosityLM = LevenbergMarquardtParams::SUMMARY; // params.verbosityLM = LevenbergMarquardtParams::SUMMARY;
/* ************************************************************************* */ /* ************************************************************************* *
TEST( SmartProjectionPoseFactor, Constructor) { TEST( SmartProjectionPoseFactor, Constructor) {
using namespace vanillaPose; using namespace vanillaPose;
SmartFactor::shared_ptr factor1(new SmartFactor(model, sharedK)); SmartFactor::shared_ptr factor1(new SmartFactor(model, sharedK));
} }
/* ************************************************************************* */ /* ************************************************************************* *
TEST( SmartProjectionPoseFactor, Constructor2) { TEST( SmartProjectionPoseFactor, Constructor2) {
using namespace vanillaPose; using namespace vanillaPose;
SmartProjectionParams params; SmartProjectionParams params;
@ -66,14 +64,14 @@ TEST( SmartProjectionPoseFactor, Constructor2) {
SmartFactor factor1(model, sharedK, params); SmartFactor factor1(model, sharedK, params);
} }
/* ************************************************************************* */ /* ************************************************************************* *
TEST( SmartProjectionPoseFactor, Constructor3) { TEST( SmartProjectionPoseFactor, Constructor3) {
using namespace vanillaPose; using namespace vanillaPose;
SmartFactor::shared_ptr factor1(new SmartFactor(model, sharedK)); SmartFactor::shared_ptr factor1(new SmartFactor(model, sharedK));
factor1->add(measurement1, x1); factor1->add(measurement1, x1);
} }
/* ************************************************************************* */ /* ************************************************************************* *
TEST( SmartProjectionPoseFactor, Constructor4) { TEST( SmartProjectionPoseFactor, Constructor4) {
using namespace vanillaPose; using namespace vanillaPose;
SmartProjectionParams params; SmartProjectionParams params;
@ -82,7 +80,7 @@ TEST( SmartProjectionPoseFactor, Constructor4) {
factor1.add(measurement1, x1); factor1.add(measurement1, x1);
} }
/* ************************************************************************* */ /* ************************************************************************* *
TEST( SmartProjectionPoseFactor, params) { TEST( SmartProjectionPoseFactor, params) {
using namespace vanillaPose; using namespace vanillaPose;
SmartProjectionParams params; SmartProjectionParams params;
@ -93,7 +91,7 @@ TEST( SmartProjectionPoseFactor, params) {
EXPECT_DOUBLES_EQUAL(1e-3, rt, 1e-7); EXPECT_DOUBLES_EQUAL(1e-3, rt, 1e-7);
} }
/* ************************************************************************* */ /* ************************************************************************* *
TEST( SmartProjectionPoseFactor, Equals ) { TEST( SmartProjectionPoseFactor, Equals ) {
using namespace vanillaPose; using namespace vanillaPose;
SmartFactor::shared_ptr factor1(new SmartFactor(model, sharedK)); SmartFactor::shared_ptr factor1(new SmartFactor(model, sharedK));
@ -105,7 +103,7 @@ TEST( SmartProjectionPoseFactor, Equals ) {
CHECK(assert_equal(*factor1, *factor2)); CHECK(assert_equal(*factor1, *factor2));
} }
/* *************************************************************************/ /* *************************************************************************
TEST( SmartProjectionPoseFactor, noiseless ) { TEST( SmartProjectionPoseFactor, noiseless ) {
using namespace vanillaPose; using namespace vanillaPose;
@ -163,7 +161,7 @@ TEST( SmartProjectionPoseFactor, noiseless ) {
EXPECT_DOUBLES_EQUAL(expectedError, actualError3, 1e-6); EXPECT_DOUBLES_EQUAL(expectedError, actualError3, 1e-6);
} }
/* *************************************************************************/ /* *************************************************************************
TEST( SmartProjectionPoseFactor, noisy ) { TEST( SmartProjectionPoseFactor, noisy ) {
using namespace vanillaPose; using namespace vanillaPose;
@ -197,7 +195,7 @@ TEST( SmartProjectionPoseFactor, noisy ) {
DOUBLES_EQUAL(actualError1, actualError2, 1e-7); DOUBLES_EQUAL(actualError1, actualError2, 1e-7);
} }
/* *************************************************************************/ /* *************************************************************************
TEST(SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform) { TEST(SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform) {
using namespace vanillaPose; using namespace vanillaPose;
@ -272,7 +270,7 @@ TEST(SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform) {
EXPECT(assert_equal(wTb3, result.at<Pose3>(x3))); EXPECT(assert_equal(wTb3, result.at<Pose3>(x3)));
} }
/* *************************************************************************/ /* *************************************************************************
TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) { TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) {
using namespace vanillaPose2; using namespace vanillaPose2;
@ -333,7 +331,7 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) {
EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-6)); EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-6));
} }
/* *************************************************************************/ /* *************************************************************************
TEST( SmartProjectionPoseFactor, Factors ) { TEST( SmartProjectionPoseFactor, Factors ) {
using namespace vanillaPose; using namespace vanillaPose;
@ -497,7 +495,7 @@ TEST( SmartProjectionPoseFactor, Factors ) {
} }
} }
/* *************************************************************************/ /* *************************************************************************
TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) { TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) {
using namespace vanillaPose; using namespace vanillaPose;
@ -551,7 +549,7 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) {
EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-7)); EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-7));
} }
/* *************************************************************************/ /* *************************************************************************
TEST( SmartProjectionPoseFactor, jacobianSVD ) { TEST( SmartProjectionPoseFactor, jacobianSVD ) {
using namespace vanillaPose; using namespace vanillaPose;
@ -607,7 +605,7 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ) {
EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-6)); EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-6));
} }
/* *************************************************************************/ /* *************************************************************************
TEST( SmartProjectionPoseFactor, landmarkDistance ) { TEST( SmartProjectionPoseFactor, landmarkDistance ) {
using namespace vanillaPose; using namespace vanillaPose;
@ -666,7 +664,7 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ) {
EXPECT(assert_equal(values.at<Pose3>(x3), result.at<Pose3>(x3))); EXPECT(assert_equal(values.at<Pose3>(x3), result.at<Pose3>(x3)));
} }
/* *************************************************************************/ /* *************************************************************************
TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) { TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) {
using namespace vanillaPose; using namespace vanillaPose;
@ -732,58 +730,7 @@ TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) {
EXPECT(assert_equal(cam3.pose(), result.at<Pose3>(x3))); EXPECT(assert_equal(cam3.pose(), result.at<Pose3>(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<Pose3>(x3), 1e-6));
}
/* *************************************************************************/
TEST( SmartProjectionPoseFactor, 3poses_projection_factor ) { TEST( SmartProjectionPoseFactor, 3poses_projection_factor ) {
using namespace vanillaPose2; using namespace vanillaPose2;
@ -830,7 +777,7 @@ TEST( SmartProjectionPoseFactor, 3poses_projection_factor ) {
EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-7)); EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-7));
} }
/* *************************************************************************/ /* *************************************************************************
TEST( SmartProjectionPoseFactor, CheckHessian) { TEST( SmartProjectionPoseFactor, CheckHessian) {
KeyVector views {x1, x2, x3}; KeyVector views {x1, x2, x3};
@ -912,144 +859,7 @@ TEST( SmartProjectionPoseFactor, CheckHessian) {
EXPECT(assert_equal(InfoVector, GaussianGraph->hessian().second, 1e-6)); 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<PoseTranslationPrior<Pose3> >(x2, positionPrior, noisePriorTranslation);
graph.emplace_shared<PoseTranslationPrior<Pose3> >(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<Pose3>(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<PoseTranslationPrior<Pose3> >(x2, positionPrior, noisePriorTranslation);
graph.emplace_shared<PoseTranslationPrior<Pose3> >(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<Pose3>(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<Pose3>(x3).rotation(),
Point3(0,0,1)), result.at<Pose3>(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<Pose3>(x3),1e-3));
#endif
}
/* *************************************************************************/
TEST( SmartProjectionPoseFactor, Hessian ) { TEST( SmartProjectionPoseFactor, Hessian ) {
using namespace vanillaPose2; 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] // 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<GaussianFactor> 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<GaussianFactor> 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<GaussianFactor> 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<GaussianFactor> 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<GaussianFactor> 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<GaussianFactor> 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<Pose3>(x3)));
Values result;
LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
result = optimizer.optimize();
EXPECT(assert_equal(cam3.pose(), result.at<Pose3>(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<PoseTranslationPrior<Pose3> >(x2, positionPrior, noisePriorTranslation);
graph.emplace_shared<PoseTranslationPrior<Pose3> >(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<Pose3>(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<Pose3>(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() { int main() {
TestResult tr; TestResult tr;