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

Google-style formatting in new files.

release/4.3a0
Frank Dellaert 2021-08-28 17:36:14 -04:00
parent d0505d4bc3
commit bafcde9ee1
6 changed files with 695 additions and 540 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
gtsam/geometry/CameraSet.h
View File

@ -218,48 +218,52 @@ public:
size_t nrNonuniqueKeys = jacobianKeys.size(); size_t nrNonuniqueKeys = jacobianKeys.size();
size_t nrUniqueKeys = hessianKeys.size(); size_t nrUniqueKeys = hessianKeys.size();
// marginalize point: note - we reuse the standard SchurComplement function // Marginalize point: note - we reuse the standard SchurComplement function.
SymmetricBlockMatrix augmentedHessian = SchurComplement<N, ND>(Fs,E,P,b); SymmetricBlockMatrix augmentedHessian = SchurComplement<N, ND>(Fs, E, P, b);
// now pack into an Hessian factor // Pack into an Hessian factor, allow space for b term.
std::vector<DenseIndex> dims(nrUniqueKeys + 1); // this also includes the b term std::vector<DenseIndex> dims(nrUniqueKeys + 1);
std::fill(dims.begin(), dims.end() - 1, NDD); std::fill(dims.begin(), dims.end() - 1, NDD);
dims.back() = 1; dims.back() = 1;
SymmetricBlockMatrix augmentedHessianUniqueKeys; SymmetricBlockMatrix augmentedHessianUniqueKeys;
// here we have to deal with the fact that some blocks may share the same keys // Deal with the fact that some blocks may share the same keys.
if (nrUniqueKeys == nrNonuniqueKeys) { // if there is 1 calibration key per camera if (nrUniqueKeys == nrNonuniqueKeys) {
// Case when 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 {
// the results of the Schur complement matrix // When multiple cameras share a calibration we have to rearrange
std::vector<DenseIndex> nonuniqueDims(nrNonuniqueKeys + 1); // this also includes the b term // the results of the Schur complement matrix.
std::vector<DenseIndex> nonuniqueDims(nrNonuniqueKeys + 1); // includes b
std::fill(nonuniqueDims.begin(), nonuniqueDims.end() - 1, NDD); std::fill(nonuniqueDims.begin(), nonuniqueDims.end() - 1, NDD);
nonuniqueDims.back() = 1; nonuniqueDims.back() = 1;
augmentedHessian = SymmetricBlockMatrix( augmentedHessian = SymmetricBlockMatrix(
nonuniqueDims, Matrix(augmentedHessian.selfadjointView())); nonuniqueDims, Matrix(augmentedHessian.selfadjointView()));
// 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[hessianKeys[k]] = k; keyToSlotMap[hessianKeys[k]] = k;
} }
// initialize matrix to zero // Initialize matrix to zero.
augmentedHessianUniqueKeys = SymmetricBlockMatrix( augmentedHessianUniqueKeys = SymmetricBlockMatrix(
dims, Matrix::Zero(NDD * nrUniqueKeys + 1, NDD * nrUniqueKeys + 1)); dims, Matrix::Zero(NDD * nrUniqueKeys + 1, NDD * 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
// and populates an Hessian that only includes the unique keys (that is what we want to return) // 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 for (size_t i = 0; i < nrNonuniqueKeys; i++) { // rows
Key key_i = jacobianKeys.at(i); Key key_i = jacobianKeys.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 = jacobianKeys.at(j); Key key_j = jacobianKeys.at(j);
if (i == j) { if (i == j) {
@ -273,13 +277,14 @@ public:
} 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));
} }

39
gtsam_unstable/slam/ProjectionFactorRollingShutter.cpp
View File

@ -23,7 +23,6 @@ Vector ProjectionFactorRollingShutter::evaluateError(
const Pose3& pose_a, const Pose3& pose_b, const Point3& point, const Pose3& pose_a, const Pose3& pose_b, const Point3& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2, boost::optional<Matrix&> H1, boost::optional<Matrix&> H2,
boost::optional<Matrix&> H3) const { boost::optional<Matrix&> H3) const {
try { try {
Pose3 pose = interpolate<Pose3>(pose_a, pose_b, alpha_, H1, H2); Pose3 pose = interpolate<Pose3>(pose_a, pose_b, alpha_, H1, H2);
gtsam::Matrix Hprj; gtsam::Matrix Hprj;
@ -32,12 +31,10 @@ Vector ProjectionFactorRollingShutter::evaluateError(
gtsam::Matrix HbodySensor; gtsam::Matrix HbodySensor;
PinholeCamera<Cal3_S2> camera( PinholeCamera<Cal3_S2> camera(
pose.compose(*body_P_sensor_, HbodySensor), *K_); pose.compose(*body_P_sensor_, HbodySensor), *K_);
Point2 reprojectionError( Point2 reprojectionError(camera.project(point, Hprj, H3, boost::none) -
camera.project(point, Hprj, H3, boost::none) - measured_); measured_);
if (H1) if (H1) *H1 = Hprj * HbodySensor * (*H1);
*H1 = Hprj * HbodySensor * (*H1); if (H2) *H2 = Hprj * HbodySensor * (*H2);
if (H2)
*H2 = Hprj * HbodySensor * (*H2);
return reprojectionError; return reprojectionError;
} else { } else {
PinholeCamera<Cal3_S2> camera(pose.compose(*body_P_sensor_), *K_); PinholeCamera<Cal3_S2> camera(pose.compose(*body_P_sensor_), *K_);
@ -45,29 +42,23 @@ Vector ProjectionFactorRollingShutter::evaluateError(
} }
} else { } else {
PinholeCamera<Cal3_S2> camera(pose, *K_); PinholeCamera<Cal3_S2> camera(pose, *K_);
Point2 reprojectionError( Point2 reprojectionError(camera.project(point, Hprj, H3, boost::none) -
camera.project(point, Hprj, H3, boost::none) - measured_); measured_);
if (H1) if (H1) *H1 = Hprj * (*H1);
*H1 = Hprj * (*H1); if (H2) *H2 = Hprj * (*H2);
if (H2)
*H2 = Hprj * (*H2);
return reprojectionError; return reprojectionError;
} }
} catch (CheiralityException& e) { } catch (CheiralityException& e) {
if (H1) if (H1) *H1 = Matrix::Zero(2, 6);
*H1 = Matrix::Zero(2, 6); if (H2) *H2 = Matrix::Zero(2, 6);
if (H2) if (H3) *H3 = Matrix::Zero(2, 3);
*H2 = Matrix::Zero(2, 6);
if (H3)
*H3 = Matrix::Zero(2, 3);
if (verboseCheirality_) if (verboseCheirality_)
std::cout << e.what() << ": Landmark " std::cout << e.what() << ": Landmark "
<< DefaultKeyFormatter(this->key2()) << " moved behind camera " << DefaultKeyFormatter(this->key2()) << " moved behind camera "
<< DefaultKeyFormatter(this->key1()) << std::endl; << DefaultKeyFormatter(this->key1()) << std::endl;
if (throwCheirality_) if (throwCheirality_) throw CheiralityException(this->key2());
throw CheiralityException(this->key2());
} }
return Vector2::Constant(2.0 * K_->fx()); return Vector2::Constant(2.0 * K_->fx());
} }
} //namespace gtsam } // namespace gtsam

173
gtsam_unstable/slam/ProjectionFactorRollingShutter.h
View File

@ -17,41 +17,47 @@
#pragma once #pragma once
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/CalibratedCamera.h>
#include <gtsam/geometry/Cal3_S2.h> #include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/CalibratedCamera.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <boost/optional.hpp> #include <boost/optional.hpp>
namespace gtsam { namespace gtsam {
/** /**
* Non-linear factor for 2D projection measurement obtained using a rolling shutter camera. The calibration is known here. * Non-linear factor for 2D projection measurement obtained using a rolling
* This version takes rolling shutter information into account as follows: consider two consecutive poses A and B, * shutter camera. The calibration is known here. This version takes rolling
* and a Point2 measurement taken starting at time A using a rolling shutter camera. * shutter information into account as follows: consider two consecutive poses A
* Pose A has timestamp t_A, and Pose B has timestamp t_B. The Point2 measurement has timestamp t_p (with t_A <= t_p <= t_B) * and B, and a Point2 measurement taken starting at time A using a rolling
* corresponding to the time of exposure of the row of the image the pixel belongs to. * shutter camera. Pose A has timestamp t_A, and Pose B has timestamp t_B. The
* Let us define the alpha = (t_p - t_A) / (t_B - t_A), we will use the pose interpolated between A and B by * Point2 measurement has timestamp t_p (with t_A <= t_p <= t_B) corresponding
* the alpha to project the corresponding landmark to Point2. * to the time of exposure of the row of the image the pixel belongs to. Let us
* define the alpha = (t_p - t_A) / (t_B - t_A), we will use the pose
* interpolated between A and B by the alpha to project the corresponding
* landmark to Point2.
* @addtogroup SLAM * @addtogroup SLAM
*/ */
class ProjectionFactorRollingShutter : public NoiseModelFactor3<Pose3, Pose3, class ProjectionFactorRollingShutter
Point3> { : public NoiseModelFactor3<Pose3, Pose3, Point3> {
protected: protected:
// Keep a copy of measurement and calibration for I/O // Keep a copy of measurement and calibration for I/O
Point2 measured_; ///< 2D measurement Point2 measured_; ///< 2D measurement
double alpha_; ///< interpolation parameter in [0,1] corresponding to the point2 measurement double alpha_; ///< interpolation parameter in [0,1] corresponding to the
///< point2 measurement
boost::shared_ptr<Cal3_S2> K_; ///< shared pointer to calibration object boost::shared_ptr<Cal3_S2> K_; ///< shared pointer to calibration object
boost::optional<Pose3> body_P_sensor_; ///< The pose of the sensor in the body frame boost::optional<Pose3>
body_P_sensor_; ///< The pose of the sensor in the body frame
// verbosity handling for Cheirality Exceptions // verbosity handling for Cheirality Exceptions
bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false) bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default:
bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions (default: false) ///< false)
bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions
///< (default: false)
public: public:
/// shorthand for base class type /// shorthand for base class type
typedef NoiseModelFactor3<Pose3, Pose3, Point3> Base; typedef NoiseModelFactor3<Pose3, Pose3, Point3> Base;
@ -66,72 +72,72 @@ class ProjectionFactorRollingShutter : public NoiseModelFactor3<Pose3, Pose3,
: measured_(0, 0), : measured_(0, 0),
alpha_(0), alpha_(0),
throwCheirality_(false), throwCheirality_(false),
verboseCheirality_(false) { verboseCheirality_(false) {}
}
/** /**
* Constructor * Constructor
* @param measured is the 2-dimensional pixel location of point in the image (the measurement) * @param measured is the 2-dimensional pixel location of point in the image
* (the measurement)
* @param alpha in [0,1] is the rolling shutter parameter for the measurement * @param alpha in [0,1] is the rolling shutter parameter for the measurement
* @param model is the noise model * @param model is the noise model
* @param poseKey_a is the key of the first camera * @param poseKey_a is the key of the first camera
* @param poseKey_b is the key of the second camera * @param poseKey_b is the key of the second camera
* @param pointKey is the key of the landmark * @param pointKey is the key of the landmark
* @param K shared pointer to the constant calibration * @param K shared pointer to the constant calibration
* @param body_P_sensor is the transform from body to sensor frame (default identity) * @param body_P_sensor is the transform from body to sensor frame (default
* identity)
*/ */
ProjectionFactorRollingShutter(const Point2& measured, double alpha, ProjectionFactorRollingShutter(
const SharedNoiseModel& model, Key poseKey_a, const Point2& measured, double alpha, const SharedNoiseModel& model,
Key poseKey_b, Key pointKey, Key poseKey_a, Key poseKey_b, Key pointKey,
const boost::shared_ptr<Cal3_S2>& K, const boost::shared_ptr<Cal3_S2>& K,
boost::optional<Pose3> body_P_sensor = boost::optional<Pose3> body_P_sensor = boost::none)
boost::none)
: Base(model, poseKey_a, poseKey_b, pointKey), : Base(model, poseKey_a, poseKey_b, pointKey),
measured_(measured), measured_(measured),
alpha_(alpha), alpha_(alpha),
K_(K), K_(K),
body_P_sensor_(body_P_sensor), body_P_sensor_(body_P_sensor),
throwCheirality_(false), throwCheirality_(false),
verboseCheirality_(false) { verboseCheirality_(false) {}
}
/** /**
* Constructor with exception-handling flags * Constructor with exception-handling flags
* @param measured is the 2-dimensional pixel location of point in the image (the measurement) * @param measured is the 2-dimensional pixel location of point in the image
* (the measurement)
* @param alpha in [0,1] is the rolling shutter parameter for the measurement * @param alpha in [0,1] is the rolling shutter parameter for the measurement
* @param model is the noise model * @param model is the noise model
* @param poseKey_a is the key of the first camera * @param poseKey_a is the key of the first camera
* @param poseKey_b is the key of the second camera * @param poseKey_b is the key of the second camera
* @param pointKey is the key of the landmark * @param pointKey is the key of the landmark
* @param K shared pointer to the constant calibration * @param K shared pointer to the constant calibration
* @param throwCheirality determines whether Cheirality exceptions are rethrown * @param throwCheirality determines whether Cheirality exceptions are
* @param verboseCheirality determines whether exceptions are printed for Cheirality * rethrown
* @param body_P_sensor is the transform from body to sensor frame (default identity) * @param verboseCheirality determines whether exceptions are printed for
* Cheirality
* @param body_P_sensor is the transform from body to sensor frame (default
* identity)
*/ */
ProjectionFactorRollingShutter(const Point2& measured, double alpha, ProjectionFactorRollingShutter(
const SharedNoiseModel& model, Key poseKey_a, const Point2& measured, double alpha, const SharedNoiseModel& model,
Key poseKey_b, Key pointKey, Key poseKey_a, Key poseKey_b, Key pointKey,
const boost::shared_ptr<Cal3_S2>& K, const boost::shared_ptr<Cal3_S2>& K, bool throwCheirality,
bool throwCheirality, bool verboseCheirality, bool verboseCheirality,
boost::optional<Pose3> body_P_sensor = boost::optional<Pose3> body_P_sensor = boost::none)
boost::none)
: Base(model, poseKey_a, poseKey_b, pointKey), : Base(model, poseKey_a, poseKey_b, pointKey),
measured_(measured), measured_(measured),
alpha_(alpha), alpha_(alpha),
K_(K), K_(K),
body_P_sensor_(body_P_sensor), body_P_sensor_(body_P_sensor),
throwCheirality_(throwCheirality), throwCheirality_(throwCheirality),
verboseCheirality_(verboseCheirality) { verboseCheirality_(verboseCheirality) {}
}
/** Virtual destructor */ /** Virtual destructor */
virtual ~ProjectionFactorRollingShutter() { virtual ~ProjectionFactorRollingShutter() {}
}
/// @return a deep copy of this factor /// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override { gtsam::NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast < gtsam::NonlinearFactor return boost::static_pointer_cast<gtsam::NonlinearFactor>(
> (gtsam::NonlinearFactor::shared_ptr(new This(*this))); gtsam::NonlinearFactor::shared_ptr(new This(*this)));
} }
/** /**
@ -139,8 +145,9 @@ class ProjectionFactorRollingShutter : public NoiseModelFactor3<Pose3, Pose3,
* @param s optional string naming the factor * @param s optional string naming the factor
* @param keyFormatter optional formatter useful for printing Symbols * @param keyFormatter optional formatter useful for printing Symbols
*/ */
void print(const std::string& s = "", const KeyFormatter& keyFormatter = void print(
DefaultKeyFormatter) const override { const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
std::cout << s << "ProjectionFactorRollingShutter, z = "; std::cout << s << "ProjectionFactorRollingShutter, z = ";
traits<Point2>::Print(measured_); traits<Point2>::Print(measured_);
std::cout << " rolling shutter interpolation param = " << alpha_; std::cout << " rolling shutter interpolation param = " << alpha_;
@ -151,15 +158,15 @@ class ProjectionFactorRollingShutter : public NoiseModelFactor3<Pose3, Pose3,
/// equals /// equals
bool equals(const NonlinearFactor& p, double tol = 1e-9) const override { bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
const This *e = dynamic_cast<const This*>(&p); const This* e = dynamic_cast<const This*>(&p);
return e && Base::equals(p, tol) && (alpha_ == e->alpha()) return e && Base::equals(p, tol) && (alpha_ == e->alpha()) &&
&& traits<Point2>::Equals(this->measured_, e->measured_, tol) traits<Point2>::Equals(this->measured_, e->measured_, tol) &&
&& this->K_->equals(*e->K_, tol) this->K_->equals(*e->K_, tol) &&
&& (this->throwCheirality_ == e->throwCheirality_) (this->throwCheirality_ == e->throwCheirality_) &&
&& (this->verboseCheirality_ == e->verboseCheirality_) (this->verboseCheirality_ == e->verboseCheirality_) &&
&& ((!body_P_sensor_ && !e->body_P_sensor_) ((!body_P_sensor_ && !e->body_P_sensor_) ||
|| (body_P_sensor_ && e->body_P_sensor_ (body_P_sensor_ && e->body_P_sensor_ &&
&& body_P_sensor_->equals(*e->body_P_sensor_))); body_P_sensor_->equals(*e->body_P_sensor_)));
} }
/// Evaluate error h(x)-z and optionally derivatives /// Evaluate error h(x)-z and optionally derivatives
@ -170,51 +177,41 @@ class ProjectionFactorRollingShutter : public NoiseModelFactor3<Pose3, Pose3,
boost::optional<Matrix&> H3 = boost::none) const override; boost::optional<Matrix&> H3 = boost::none) const override;
/** return the measurement */ /** return the measurement */
const Point2& measured() const { const Point2& measured() const { return measured_; }
return measured_;
}
/** return the calibration object */ /** return the calibration object */
inline const boost::shared_ptr<Cal3_S2> calibration() const { inline const boost::shared_ptr<Cal3_S2> calibration() const { return K_; }
return K_;
}
/** returns the rolling shutter interp param*/ /** returns the rolling shutter interp param*/
inline double alpha() const { inline double alpha() const { return alpha_; }
return alpha_;
}
/** return verbosity */ /** return verbosity */
inline bool verboseCheirality() const { inline bool verboseCheirality() const { return verboseCheirality_; }
return verboseCheirality_;
}
/** return flag for throwing cheirality exceptions */ /** return flag for throwing cheirality exceptions */
inline bool throwCheirality() const { inline bool throwCheirality() const { return throwCheirality_; }
return throwCheirality_;
}
private: private:
/// Serialization function /// Serialization function
friend class boost::serialization::access; friend class boost::serialization::access;
template<class ARCHIVE> template <class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) { void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar & BOOST_SERIALIZATION_NVP(measured_); ar& BOOST_SERIALIZATION_NVP(measured_);
ar & BOOST_SERIALIZATION_NVP(alpha_); ar& BOOST_SERIALIZATION_NVP(alpha_);
ar & BOOST_SERIALIZATION_NVP(K_); ar& BOOST_SERIALIZATION_NVP(K_);
ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); ar& BOOST_SERIALIZATION_NVP(body_P_sensor_);
ar & BOOST_SERIALIZATION_NVP(throwCheirality_); ar& BOOST_SERIALIZATION_NVP(throwCheirality_);
ar & BOOST_SERIALIZATION_NVP(verboseCheirality_); ar& BOOST_SERIALIZATION_NVP(verboseCheirality_);
} }
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EIGEN_MAKE_ALIGNED_OPERATOR_NEW
}; };
/// traits /// traits
template<> struct traits<ProjectionFactorRollingShutter> : public Testable< template <>
ProjectionFactorRollingShutter> { struct traits<ProjectionFactorRollingShutter>
}; : public Testable<ProjectionFactorRollingShutter> {};
} //namespace gtsam } // namespace gtsam

230
gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h
View File

@ -11,14 +11,15 @@
/** /**
* @file SmartProjectionPoseFactorRollingShutter.h * @file SmartProjectionPoseFactorRollingShutter.h
* @brief Smart projection factor on poses modeling rolling shutter effect with given readout time * @brief Smart projection factor on poses modeling rolling shutter effect with
* given readout time
* @author Luca Carlone * @author Luca Carlone
*/ */
#pragma once #pragma once
#include <gtsam/slam/SmartProjectionFactor.h>
#include <gtsam/geometry/CameraSet.h> #include <gtsam/geometry/CameraSet.h>
#include <gtsam/slam/SmartProjectionFactor.h>
namespace gtsam { namespace gtsam {
/** /**
@ -33,25 +34,28 @@ namespace gtsam {
*/ */
/** /**
* This factor optimizes two consecutive poses of the body assuming a rolling shutter model of the camera with given readout time. * This factor optimizes two consecutive poses of the body assuming a rolling
* The factor requires that values contain (for each pixel observation) two consecutive camera poses * shutter model of the camera with given readout time. The factor requires that
* from which the pixel observation pose can be interpolated. * values contain (for each pixel observation) two consecutive camera poses from
* which the pixel observation pose can be interpolated.
* @addtogroup SLAM * @addtogroup SLAM
*/ */
template<class CAMERA> template <class CAMERA>
class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAMERA> { class SmartProjectionPoseFactorRollingShutter
: public SmartProjectionFactor<CAMERA> {
public: public:
typedef typename CAMERA::CalibrationType CALIBRATION; typedef typename CAMERA::CalibrationType CALIBRATION;
protected: protected:
/// shared pointer to calibration object (one for each observation) /// shared pointer to calibration object (one for each observation)
std::vector<boost::shared_ptr<CALIBRATION> > K_all_; std::vector<boost::shared_ptr<CALIBRATION>> K_all_;
/// The keys of the pose of the body (with respect to an external world frame): two consecutive poses for each observation /// The keys of the pose of the body (with respect to an external world
/// frame): two consecutive poses for each observation
std::vector<std::pair<Key, Key>> world_P_body_key_pairs_; std::vector<std::pair<Key, Key>> world_P_body_key_pairs_;
/// interpolation factor (one for each observation) to interpolate between pair of consecutive poses /// interpolation factor (one for each observation) to interpolate between
/// pair of consecutive poses
std::vector<double> alphas_; std::vector<double> alphas_;
/// Pose of the camera in the body frame /// Pose of the camera in the body frame
@ -61,7 +65,7 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/// shorthand for base class type /// shorthand for base class type
typedef SmartProjectionFactor<PinholePose<CALIBRATION> > Base; typedef SmartProjectionFactor<PinholePose<CALIBRATION>> Base;
/// shorthand for this class /// shorthand for this class
typedef SmartProjectionPoseFactorRollingShutter This; typedef SmartProjectionPoseFactorRollingShutter This;
@ -69,11 +73,15 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
/// 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 DimBlock = 12; ///< size of the variable stacking 2 poses from which the observation pose is interpolated static const int DimBlock =
12; ///< size of the variable stacking 2 poses from which the observation
///< pose is interpolated
static const int DimPose = 6; ///< Pose3 dimension static const int DimPose = 6; ///< Pose3 dimension
static const int ZDim = 2; ///< Measurement dimension (Point2) static const int ZDim = 2; ///< Measurement dimension (Point2)
typedef Eigen::Matrix<double, ZDim, DimBlock> MatrixZD; // F blocks (derivatives wrt block of 2 poses) typedef Eigen::Matrix<double, ZDim, DimBlock>
typedef std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> > FBlocks; // vector of F blocks MatrixZD; // F blocks (derivatives wrt block of 2 poses)
typedef std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD>>
FBlocks; // vector of F blocks
/** /**
* Constructor * Constructor
@ -83,25 +91,29 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
SmartProjectionPoseFactorRollingShutter( SmartProjectionPoseFactorRollingShutter(
const SharedNoiseModel& sharedNoiseModel, const SharedNoiseModel& sharedNoiseModel,
const SmartProjectionParams& params = SmartProjectionParams()) const SmartProjectionParams& params = SmartProjectionParams())
: Base(sharedNoiseModel, params) { : Base(sharedNoiseModel, params) {}
}
/** Virtual destructor */ /** Virtual destructor */
~SmartProjectionPoseFactorRollingShutter() override = default; ~SmartProjectionPoseFactorRollingShutter() override = default;
/** /**
* add a new measurement, with 2 pose keys, interpolation factor, camera (intrinsic and extrinsic) calibration, and observed pixel. * add a new measurement, with 2 pose keys, interpolation factor, camera
* @param measured 2-dimensional location of the projection of a * (intrinsic and extrinsic) calibration, and observed pixel.
* single landmark in a single view (the measurement), interpolated from the 2 poses * @param measured 2-dimensional location of the projection of a single
* @param world_P_body_key1 key corresponding to the first body poses (time <= time pixel is acquired) * landmark in a single view (the measurement), interpolated from the 2 poses
* @param world_P_body_key2 key corresponding to the second body poses (time >= time pixel is acquired) * @param world_P_body_key1 key corresponding to the first body poses (time <=
* @param alpha interpolation factor in [0,1], such that if alpha = 0 the interpolated pose is the same as world_P_body_key1 * time pixel is acquired)
* @param world_P_body_key2 key corresponding to the second body poses (time
* >= time pixel is acquired)
* @param alpha interpolation factor in [0,1], such that if alpha = 0 the
* interpolated pose is the same as world_P_body_key1
* @param K (fixed) camera intrinsic calibration * @param K (fixed) camera intrinsic calibration
* @param body_P_sensor (fixed) camera extrinsic calibration * @param body_P_sensor (fixed) camera extrinsic calibration
*/ */
void add(const Point2& measured, const Key& world_P_body_key1, void add(const Point2& measured, const Key& world_P_body_key1,
const Key& world_P_body_key2, const double& alpha, const Key& world_P_body_key2, const double& alpha,
const boost::shared_ptr<CALIBRATION>& K, const Pose3 body_P_sensor = Pose3::identity()) { const boost::shared_ptr<CALIBRATION>& K,
const Pose3& body_P_sensor = Pose3::identity()) {
// store measurements in base class // store measurements in base class
this->measured_.push_back(measured); this->measured_.push_back(measured);
@ -109,10 +121,13 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
world_P_body_key_pairs_.push_back( world_P_body_key_pairs_.push_back(
std::make_pair(world_P_body_key1, world_P_body_key2)); std::make_pair(world_P_body_key1, world_P_body_key2));
// also store keys in the keys_ vector: these keys are assumed to be unique, so we avoid duplicates here // also store keys in the keys_ vector: these keys are assumed to be
if (std::find(this->keys_.begin(), this->keys_.end(), world_P_body_key1) == this->keys_.end()) // 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 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()) 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 this->keys_.push_back(world_P_body_key2); // add only unique keys
// store interpolation factor // store interpolation factor
@ -126,12 +141,15 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
} }
/** /**
* Variant of the previous "add" function in which we include multiple measurements * Variant of the previous "add" function in which we include multiple
* measurements
* @param measurements vector of the 2m dimensional location of the projection * @param measurements vector of the 2m dimensional location of the projection
* of a single landmark in the m views (the measurements) * of a single landmark in the m views (the measurements)
* @param world_P_body_key_pairs vector where the i-th element contains a pair of keys corresponding * @param world_P_body_key_pairs vector where the i-th element contains a pair
* to the pair of poses from which the observation pose for the i0-th measurement can be interpolated * of keys corresponding to the pair of poses from which the observation pose
* @param alphas vector of interpolation params (in [0,1]), one for each measurement (in the same order) * for the i0-th measurement can be interpolated
* @param alphas vector of interpolation params (in [0,1]), one for each
* measurement (in the same order)
* @param Ks vector of (fixed) intrinsic calibration objects * @param Ks vector of (fixed) intrinsic calibration objects
* @param body_P_sensors vector of (fixed) extrinsic calibration objects * @param body_P_sensors vector of (fixed) extrinsic calibration objects
*/ */
@ -139,7 +157,7 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
const std::vector<std::pair<Key, Key>>& world_P_body_key_pairs, const std::vector<std::pair<Key, Key>>& world_P_body_key_pairs,
const std::vector<double>& alphas, const std::vector<double>& alphas,
const std::vector<boost::shared_ptr<CALIBRATION>>& Ks, const std::vector<boost::shared_ptr<CALIBRATION>>& Ks,
const std::vector<Pose3> body_P_sensors) { const std::vector<Pose3>& body_P_sensors) {
assert(world_P_body_key_pairs.size() == measurements.size()); assert(world_P_body_key_pairs.size() == measurements.size());
assert(world_P_body_key_pairs.size() == alphas.size()); assert(world_P_body_key_pairs.size() == alphas.size());
assert(world_P_body_key_pairs.size() == Ks.size()); assert(world_P_body_key_pairs.size() == Ks.size());
@ -151,20 +169,24 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
} }
/** /**
* Variant of the previous "add" function in which we include multiple measurements * Variant of the previous "add" function in which we include multiple
* with the same (intrinsic and extrinsic) calibration * measurements with the same (intrinsic and extrinsic) calibration
* @param measurements vector of the 2m dimensional location of the projection * @param measurements vector of the 2m dimensional location of the projection
* of a single landmark in the m views (the measurements) * of a single landmark in the m views (the measurements)
* @param world_P_body_key_pairs vector where the i-th element contains a pair of keys corresponding * @param world_P_body_key_pairs vector where the i-th element contains a pair
* to the pair of poses from which the observation pose for the i0-th measurement can be interpolated * of keys corresponding to the pair of poses from which the observation pose
* @param alphas vector of interpolation params (in [0,1]), one for each measurement (in the same order) * for the i0-th measurement can be interpolated
* @param alphas vector of interpolation params (in [0,1]), one for each
* measurement (in the same order)
* @param K (fixed) camera intrinsic calibration (same for all measurements) * @param K (fixed) camera intrinsic calibration (same for all measurements)
* @param body_P_sensor (fixed) camera extrinsic calibration (same for all measurements) * @param body_P_sensor (fixed) camera extrinsic calibration (same for all
* measurements)
*/ */
void add(const Point2Vector& measurements, void add(const Point2Vector& measurements,
const std::vector<std::pair<Key, Key>>& world_P_body_key_pairs, const std::vector<std::pair<Key, Key>>& world_P_body_key_pairs,
const std::vector<double>& alphas, const std::vector<double>& alphas,
const boost::shared_ptr<CALIBRATION>& K, const Pose3 body_P_sensor = Pose3::identity()) { const boost::shared_ptr<CALIBRATION>& K,
const Pose3& body_P_sensor = Pose3::identity()) {
assert(world_P_body_key_pairs.size() == measurements.size()); assert(world_P_body_key_pairs.size() == measurements.size());
assert(world_P_body_key_pairs.size() == alphas.size()); assert(world_P_body_key_pairs.size() == alphas.size());
for (size_t i = 0; i < measurements.size(); i++) { for (size_t i = 0; i < measurements.size(); i++) {
@ -174,39 +196,37 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
} }
/// return the calibration object /// return the calibration object
inline std::vector<boost::shared_ptr<CALIBRATION>> calibration() const { const std::vector<boost::shared_ptr<CALIBRATION>>& calibration() const {
return K_all_; return K_all_;
} }
/// return (for each observation) the keys of the pair of poses from which we interpolate /// return (for each observation) the keys of the pair of poses from which we
const std::vector<std::pair<Key, Key>> world_P_body_key_pairs() const { /// interpolate
const std::vector<std::pair<Key, Key>>& world_P_body_key_pairs() const {
return world_P_body_key_pairs_; return world_P_body_key_pairs_;
} }
/// return the interpolation factors alphas /// return the interpolation factors alphas
const std::vector<double> alphas() const { const std::vector<double>& alphas() const { return alphas_; }
return alphas_;
}
/// return the extrinsic camera calibration body_P_sensors /// return the extrinsic camera calibration body_P_sensors
const std::vector<Pose3> body_P_sensors() const { const std::vector<Pose3>& body_P_sensors() const { return body_P_sensors_; }
return body_P_sensors_;
}
/** /**
* print * print
* @param s optional string naming the factor * @param s optional string naming the factor
* @param keyFormatter optional formatter useful for printing Symbols * @param keyFormatter optional formatter useful for printing Symbols
*/ */
void print(const std::string& s = "", const KeyFormatter& keyFormatter = void print(
DefaultKeyFormatter) const override { const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
std::cout << s << "SmartProjectionPoseFactorRollingShutter: \n "; std::cout << s << "SmartProjectionPoseFactorRollingShutter: \n ";
for (size_t i = 0; i < K_all_.size(); i++) { for (size_t i = 0; i < K_all_.size(); i++) {
std::cout << "-- Measurement nr " << i << std::endl; std::cout << "-- Measurement nr " << i << std::endl;
std::cout << " pose1 key: " std::cout << " pose1 key: "
<< keyFormatter(world_P_body_key_pairs_[i].first) << std::endl; << keyFormatter(world_P_body_key_pairs_[i].first) << std::endl;
std::cout << " pose2 key: " std::cout << " pose2 key: "
<< keyFormatter(world_P_body_key_pairs_[i].second) << std::endl; << keyFormatter(world_P_body_key_pairs_[i].second) << std::endl;
std::cout << " alpha: " << alphas_[i] << std::endl; std::cout << " alpha: " << alphas_[i] << std::endl;
body_P_sensors_[i].print("extrinsic calibration:\n"); body_P_sensors_[i].print("extrinsic calibration:\n");
K_all_[i]->print("intrinsic calibration = "); K_all_[i]->print("intrinsic calibration = ");
@ -217,17 +237,20 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
/// equals /// equals
bool equals(const NonlinearFactor& p, double tol = 1e-9) const override { bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
const SmartProjectionPoseFactorRollingShutter<CAMERA>* e = const SmartProjectionPoseFactorRollingShutter<CAMERA>* e =
dynamic_cast<const SmartProjectionPoseFactorRollingShutter<CAMERA>*>(&p); dynamic_cast<const SmartProjectionPoseFactorRollingShutter<CAMERA>*>(
&p);
double keyPairsEqual = true; double keyPairsEqual = true;
if(this->world_P_body_key_pairs_.size() == e->world_P_body_key_pairs().size()){ if (this->world_P_body_key_pairs_.size() ==
for(size_t k=0; k< this->world_P_body_key_pairs_.size(); k++){ e->world_P_body_key_pairs().size()) {
for (size_t k = 0; k < this->world_P_body_key_pairs_.size(); k++) {
const Key key1own = world_P_body_key_pairs_[k].first; const Key key1own = world_P_body_key_pairs_[k].first;
const Key key1e = e->world_P_body_key_pairs()[k].first; const Key key1e = e->world_P_body_key_pairs()[k].first;
const Key key2own = world_P_body_key_pairs_[k].second; const Key key2own = world_P_body_key_pairs_[k].second;
const Key key2e = e->world_P_body_key_pairs()[k].second; const Key key2e = e->world_P_body_key_pairs()[k].second;
if ( !(key1own == key1e) || !(key2own == key2e) ){ if (!(key1own == key1e) || !(key2own == key2e)) {
keyPairsEqual = false; break; keyPairsEqual = false;
break;
} }
} }
} else { } else {
@ -235,18 +258,19 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
} }
double extrinsicCalibrationEqual = true; double extrinsicCalibrationEqual = true;
if(this->body_P_sensors_.size() == e->body_P_sensors().size()){ if (this->body_P_sensors_.size() == e->body_P_sensors().size()) {
for(size_t i=0; i< this->body_P_sensors_.size(); i++){ for (size_t i = 0; i < this->body_P_sensors_.size(); i++) {
if (!body_P_sensors_[i].equals(e->body_P_sensors()[i])){ if (!body_P_sensors_[i].equals(e->body_P_sensors()[i])) {
extrinsicCalibrationEqual = false; break; extrinsicCalibrationEqual = false;
break;
} }
} }
} else { } else {
extrinsicCalibrationEqual = false; extrinsicCalibrationEqual = false;
} }
return e && Base::equals(p, tol) && K_all_ == e->calibration() return e && Base::equals(p, tol) && K_all_ == e->calibration() &&
&& alphas_ == e->alphas() && keyPairsEqual && extrinsicCalibrationEqual; alphas_ == e->alphas() && keyPairsEqual && extrinsicCalibrationEqual;
} }
/** /**
@ -264,12 +288,13 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
throw("computeJacobiansWithTriangulatedPoint"); throw("computeJacobiansWithTriangulatedPoint");
} else { // valid result: compute jacobians } else { // valid result: compute jacobians
size_t numViews = this->measured_.size(); size_t numViews = this->measured_.size();
E = Matrix::Zero(2 * numViews, 3); // a Point2 for each view (point jacobian) E = Matrix::Zero(2 * numViews,
3); // a Point2 for each view (point jacobian)
b = Vector::Zero(2 * numViews); // a Point2 for each view b = Vector::Zero(2 * numViews); // a Point2 for each view
// intermediate Jacobians // intermediate Jacobians
Eigen::Matrix<double, ZDim, DimPose> dProject_dPoseCam; Eigen::Matrix<double, ZDim, DimPose> dProject_dPoseCam;
Eigen::Matrix<double, DimPose, DimPose> dInterpPose_dPoseBody1, Eigen::Matrix<double, DimPose, DimPose> dInterpPose_dPoseBody1,
dInterpPose_dPoseBody2, dPoseCam_dInterpPose; dInterpPose_dPoseBody2, dPoseCam_dInterpPose;
Eigen::Matrix<double, ZDim, 3> Ei; Eigen::Matrix<double, ZDim, 3> 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
@ -285,14 +310,16 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
PinholeCamera<CALIBRATION> camera(w_P_cam, *K_all_[i]); PinholeCamera<CALIBRATION> camera(w_P_cam, *K_all_[i]);
// get jacobians and error vector for current measurement // get jacobians and error vector for current measurement
Point2 reprojectionError_i = Point2( Point2 reprojectionError_i =
camera.project(*this->result_, dProject_dPoseCam, Ei) Point2(camera.project(*this->result_, dProject_dPoseCam, Ei) -
- this->measured_.at(i)); this->measured_.at(i));
Eigen::Matrix<double, ZDim, DimBlock> J; // 2 x 12 Eigen::Matrix<double, ZDim, DimBlock> J; // 2 x 12
J.block(0, 0, ZDim, 6) = dProject_dPoseCam * dPoseCam_dInterpPose J.block(0, 0, ZDim, 6) =
* dInterpPose_dPoseBody1; // (2x6) * (6x6) * (6x6) dProject_dPoseCam * dPoseCam_dInterpPose *
J.block(0, 6, ZDim, 6) = dProject_dPoseCam * dPoseCam_dInterpPose dInterpPose_dPoseBody1; // (2x6) * (6x6) * (6x6)
* dInterpPose_dPoseBody2; // (2x6) * (6x6) * (6x6) J.block(0, 6, ZDim, 6) =
dProject_dPoseCam * dPoseCam_dInterpPose *
dInterpPose_dPoseBody2; // (2x6) * (6x6) * (6x6)
// fit into the output structures // fit into the output structures
Fs.push_back(J); Fs.push_back(J);
@ -353,21 +380,23 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
Matrix3 P = Base::Cameras::PointCov(E, lambda, diagonalDamping); Matrix3 P = Base::Cameras::PointCov(E, lambda, diagonalDamping);
// Collect all the key pairs: these are the keys that correspond to the blocks in Fs (on which we apply the Schur Complement) // Collect all the key pairs: these are the keys that correspond to the
// blocks in Fs (on which we apply the Schur Complement)
KeyVector nonuniqueKeys; KeyVector nonuniqueKeys;
for (size_t i = 0; i < world_P_body_key_pairs_.size(); i++) { for (size_t i = 0; i < world_P_body_key_pairs_.size(); i++) {
nonuniqueKeys.push_back(world_P_body_key_pairs_.at(i).first); nonuniqueKeys.push_back(world_P_body_key_pairs_.at(i).first);
nonuniqueKeys.push_back(world_P_body_key_pairs_.at(i).second); nonuniqueKeys.push_back(world_P_body_key_pairs_.at(i).second);
} }
// Build augmented Hessian (with last row/column being the information vector) // Build augmented Hessian (with last row/column being the information
// Note: we need to get the augumented hessian wrt the unique keys in key_ // vector) Note: we need to get the augumented hessian wrt the unique keys
// in key_
SymmetricBlockMatrix augmentedHessianUniqueKeys = SymmetricBlockMatrix augmentedHessianUniqueKeys =
Base::Cameras::template SchurComplementAndRearrangeBlocks<3, 12, 6>( Base::Cameras::template SchurComplementAndRearrangeBlocks<3, 12, 6>(
Fs, E, P, b, nonuniqueKeys, this->keys_); Fs, E, P, b, nonuniqueKeys, this->keys_);
return boost::make_shared < RegularHessianFactor<DimPose> return boost::make_shared<RegularHessianFactor<DimPose>>(
> (this->keys_, augmentedHessianUniqueKeys); this->keys_, augmentedHessianUniqueKeys);
} }
/** /**
@ -376,7 +405,7 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
double error(const Values& values) const override { double error(const Values& values) const override {
if (this->active(values)) { if (this->active(values)) {
return this->totalReprojectionError(this->cameras(values)); return this->totalReprojectionError(this->cameras(values));
} else { // else of active flag } else { // else of active flag
return 0.0; return 0.0;
} }
} }
@ -396,10 +425,13 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
typename Base::Cameras cameras; typename Base::Cameras cameras;
for (size_t i = 0; i < numViews; i++) { // for each measurement for (size_t i = 0; i < numViews; i++) { // for each measurement
const Pose3& w_P_body1 = values.at<Pose3>(world_P_body_key_pairs_[i].first); const Pose3& w_P_body1 =
const Pose3& w_P_body2 = values.at<Pose3>(world_P_body_key_pairs_[i].second); values.at<Pose3>(world_P_body_key_pairs_[i].first);
const Pose3& w_P_body2 =
values.at<Pose3>(world_P_body_key_pairs_[i].second);
double interpolationFactor = alphas_[i]; double interpolationFactor = alphas_[i];
const Pose3& w_P_body = interpolate<Pose3>(w_P_body1, w_P_body2, interpolationFactor); const Pose3& w_P_body =
interpolate<Pose3>(w_P_body1, w_P_body2, interpolationFactor);
const Pose3& body_P_cam = body_P_sensors_[i]; const Pose3& body_P_cam = body_P_sensors_[i];
const Pose3& w_P_cam = w_P_body.compose(body_P_cam); const Pose3& w_P_cam = w_P_body.compose(body_P_cam);
cameras.emplace_back(w_P_cam, K_all_[i]); cameras.emplace_back(w_P_cam, K_all_[i]);
@ -408,44 +440,46 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
} }
/** /**
* Linearize to Gaussian Factor (possibly adding a damping factor Lambda for LM) * Linearize to Gaussian Factor (possibly adding a damping factor Lambda for
* @param values Values structure which must contain camera poses and extrinsic pose for this factor * LM)
* @param values Values structure which must contain camera poses and
* extrinsic pose for this factor
* @return a Gaussian factor * @return a Gaussian factor
*/ */
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 (this->params_.linearizationMode) { switch (this->params_.linearizationMode) {
case HESSIAN: case HESSIAN:
return this->createHessianFactor(values, lambda); return this->createHessianFactor(values, lambda);
default: default:
throw std::runtime_error( throw std::runtime_error(
"SmartProjectionPoseFactorRollingShutter: unknown linearization mode"); "SmartProjectionPoseFactorRollingShutter: unknown linearization "
"mode");
} }
} }
/// linearize /// linearize
boost::shared_ptr<GaussianFactor> linearize(const Values& values) const boost::shared_ptr<GaussianFactor> linearize(
override { const Values& values) const override {
return this->linearizeDamped(values); return this->linearizeDamped(values);
} }
private: private:
/// Serialization function /// Serialization function
friend class boost::serialization::access; friend class boost::serialization::access;
template<class ARCHIVE> template <class ARCHIVE>
void serialize(ARCHIVE& ar, const unsigned int /*version*/) { void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar & BOOST_SERIALIZATION_NVP(K_all_); ar& BOOST_SERIALIZATION_NVP(K_all_);
} }
}; };
// end of class declaration // end of class declaration
/// traits /// traits
template<class CAMERA> template <class CAMERA>
struct traits<SmartProjectionPoseFactorRollingShutter<CAMERA> > : struct traits<SmartProjectionPoseFactorRollingShutter<CAMERA>>
public Testable<SmartProjectionPoseFactorRollingShutter<CAMERA> > { : public Testable<SmartProjectionPoseFactorRollingShutter<CAMERA>> {};
};
} // namespace gtsam } // namespace gtsam

258
gtsam_unstable/slam/tests/testProjectionFactorRollingShutter.cpp
View File

@ -16,34 +16,33 @@
* @date July 2021 * @date July 2021
*/ */
#include <gtsam/base/numericalDerivative.h> #include <CppUnitLite/TestHarness.h>
#include <gtsam/base/TestableAssertions.h> #include <gtsam/base/TestableAssertions.h>
#include <gtsam_unstable/slam/ProjectionFactorRollingShutter.h> #include <gtsam/base/numericalDerivative.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/geometry/Cal3DS2.h> #include <gtsam/geometry/Cal3DS2.h>
#include <gtsam/geometry/Cal3_S2.h> #include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Point2.h> #include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Point3.h>
#include <CppUnitLite/TestHarness.h> #include <gtsam/geometry/Pose3.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam_unstable/slam/ProjectionFactorRollingShutter.h>
using namespace std::placeholders; using namespace std::placeholders;
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
// make a realistic calibration matrix // make a realistic calibration matrix
static double fov = 60; // degrees static double fov = 60; // degrees
static size_t w=640,h=480; static size_t w = 640, h = 480;
static Cal3_S2::shared_ptr K(new Cal3_S2(fov,w,h)); static Cal3_S2::shared_ptr K(new Cal3_S2(fov, w, h));
// Create a noise model for the pixel error // Create a noise model for the pixel error
static SharedNoiseModel model(noiseModel::Unit::Create(2)); static SharedNoiseModel model(noiseModel::Unit::Create(2));
// Convenience for named keys // Convenience for named keys
using symbol_shorthand::X;
using symbol_shorthand::L; using symbol_shorthand::L;
using symbol_shorthand::T; using symbol_shorthand::T;
using symbol_shorthand::X;
// Convenience to define common variables across many tests // Convenience to define common variables across many tests
static Key poseKey1(X(1)); static Key poseKey1(X(1));
@ -51,74 +50,84 @@ static Key poseKey2(X(2));
static Key pointKey(L(1)); static Key pointKey(L(1));
static double interp_params = 0.5; static double interp_params = 0.5;
static Point2 measurement(323.0, 240.0); static Point2 measurement(323.0, 240.0);
static Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); static Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2),
Point3(0.25, -0.10, 1.0));
/* ************************************************************************* */ /* ************************************************************************* */
TEST( ProjectionFactorRollingShutter, Constructor) { TEST(ProjectionFactorRollingShutter, Constructor) {
ProjectionFactorRollingShutter factor(measurement, interp_params, model, poseKey1, poseKey2, pointKey, K);
}
/* ************************************************************************* */
TEST( ProjectionFactorRollingShutter, ConstructorWithTransform) {
ProjectionFactorRollingShutter factor(measurement, interp_params, model, ProjectionFactorRollingShutter factor(measurement, interp_params, model,
poseKey1, poseKey2, pointKey, K, body_P_sensor); poseKey1, poseKey2, pointKey, K);
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( ProjectionFactorRollingShutter, Equals ) { TEST(ProjectionFactorRollingShutter, ConstructorWithTransform) {
{ // factors are equal ProjectionFactorRollingShutter factor(measurement, interp_params, model,
ProjectionFactorRollingShutter factor1(measurement, interp_params, poseKey1, poseKey2, pointKey, K,
model, poseKey1, poseKey2, pointKey, K); body_P_sensor);
ProjectionFactorRollingShutter factor2(measurement, interp_params, }
model, poseKey1, poseKey2, pointKey, K);
/* ************************************************************************* */
TEST(ProjectionFactorRollingShutter, Equals) {
{ // factors are equal
ProjectionFactorRollingShutter factor1(measurement, interp_params, model,
poseKey1, poseKey2, pointKey, K);
ProjectionFactorRollingShutter factor2(measurement, interp_params, model,
poseKey1, poseKey2, pointKey, K);
CHECK(assert_equal(factor1, factor2)); CHECK(assert_equal(factor1, factor2));
} }
{ // factors are NOT equal (keys are different) { // factors are NOT equal (keys are different)
ProjectionFactorRollingShutter factor1(measurement, interp_params, ProjectionFactorRollingShutter factor1(measurement, interp_params, model,
model, poseKey1, poseKey2, pointKey, K); poseKey1, poseKey2, pointKey, K);
ProjectionFactorRollingShutter factor2(measurement, interp_params, ProjectionFactorRollingShutter factor2(measurement, interp_params, model,
model, poseKey1, poseKey1, pointKey, K); poseKey1, poseKey1, pointKey, K);
CHECK(!assert_equal(factor1, factor2)); // not equal CHECK(!assert_equal(factor1, factor2)); // not equal
} }
{ // factors are NOT equal (different interpolation) { // factors are NOT equal (different interpolation)
ProjectionFactorRollingShutter factor1(measurement, 0.1, ProjectionFactorRollingShutter factor1(measurement, 0.1, model, poseKey1,
model, poseKey1, poseKey1, pointKey, K); poseKey1, pointKey, K);
ProjectionFactorRollingShutter factor2(measurement, 0.5, ProjectionFactorRollingShutter factor2(measurement, 0.5, model, poseKey1,
model, poseKey1, poseKey2, pointKey, K); poseKey2, pointKey, K);
CHECK(!assert_equal(factor1, factor2)); // not equal CHECK(!assert_equal(factor1, factor2)); // not equal
} }
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( ProjectionFactorRollingShutter, EqualsWithTransform ) { TEST(ProjectionFactorRollingShutter, EqualsWithTransform) {
{ // factors are equal { // factors are equal
ProjectionFactorRollingShutter factor1(measurement, interp_params, model, ProjectionFactorRollingShutter factor1(measurement, interp_params, model,
poseKey1, poseKey2, pointKey, K, body_P_sensor); poseKey1, poseKey2, pointKey, K,
body_P_sensor);
ProjectionFactorRollingShutter factor2(measurement, interp_params, model, ProjectionFactorRollingShutter factor2(measurement, interp_params, model,
poseKey1, poseKey2, pointKey, K, body_P_sensor); poseKey1, poseKey2, pointKey, K,
body_P_sensor);
CHECK(assert_equal(factor1, factor2)); CHECK(assert_equal(factor1, factor2));
} }
{ // factors are NOT equal { // factors are NOT equal
ProjectionFactorRollingShutter factor1(measurement, interp_params, model, ProjectionFactorRollingShutter factor1(measurement, interp_params, model,
poseKey1, poseKey2, pointKey, K, body_P_sensor); poseKey1, poseKey2, pointKey, K,
Pose3 body_P_sensor2(Rot3::RzRyRx(0.0, 0.0, 0.0), Point3(0.25, -0.10, 1.0)); // rotation different from body_P_sensor body_P_sensor);
Pose3 body_P_sensor2(
Rot3::RzRyRx(0.0, 0.0, 0.0),
Point3(0.25, -0.10, 1.0)); // rotation different from body_P_sensor
ProjectionFactorRollingShutter factor2(measurement, interp_params, model, ProjectionFactorRollingShutter factor2(measurement, interp_params, model,
poseKey1, poseKey2, pointKey, K, body_P_sensor2); poseKey1, poseKey2, pointKey, K,
body_P_sensor2);
CHECK(!assert_equal(factor1, factor2)); CHECK(!assert_equal(factor1, factor2));
} }
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( ProjectionFactorRollingShutter, Error ) { TEST(ProjectionFactorRollingShutter, Error) {
{ {
// Create the factor with a measurement that is 3 pixels off in x // Create the factor with a measurement that is 3 pixels off in x
// Camera pose corresponds to the first camera // Camera pose corresponds to the first camera
double t = 0.0; double t = 0.0;
ProjectionFactorRollingShutter factor(measurement, t, model, poseKey1, poseKey2, pointKey, K); ProjectionFactorRollingShutter factor(measurement, t, model, poseKey1,
poseKey2, pointKey, K);
// Set the linearization point // Set the linearization point
Pose3 pose1(Rot3(), Point3(0,0,-6)); Pose3 pose1(Rot3(), Point3(0, 0, -6));
Pose3 pose2(Rot3(), Point3(0,0,-4)); Pose3 pose2(Rot3(), Point3(0, 0, -4));
Point3 point(0.0, 0.0, 0.0); Point3 point(0.0, 0.0, 0.0);
// Use the factor to calculate the error // Use the factor to calculate the error
@ -134,11 +143,12 @@ TEST( ProjectionFactorRollingShutter, Error ) {
// Create the factor with a measurement that is 3 pixels off in x // Create the factor with a measurement that is 3 pixels off in x
// Camera pose is actually interpolated now // Camera pose is actually interpolated now
double t = 0.5; double t = 0.5;
ProjectionFactorRollingShutter factor(measurement, t, model, poseKey1, poseKey2, pointKey, K); ProjectionFactorRollingShutter factor(measurement, t, model, poseKey1,
poseKey2, pointKey, K);
// Set the linearization point // Set the linearization point
Pose3 pose1(Rot3(), Point3(0,0,-8)); Pose3 pose1(Rot3(), Point3(0, 0, -8));
Pose3 pose2(Rot3(), Point3(0,0,-4)); Pose3 pose2(Rot3(), Point3(0, 0, -4));
Point3 point(0.0, 0.0, 0.0); Point3 point(0.0, 0.0, 0.0);
// Use the factor to calculate the error // Use the factor to calculate the error
@ -153,15 +163,16 @@ TEST( ProjectionFactorRollingShutter, Error ) {
{ {
// Create measurement by projecting 3D landmark // Create measurement by projecting 3D landmark
double t = 0.3; double t = 0.3;
Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0,0,0)); Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0, 0, 0));
Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0,0,1)); Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0, 0, 1));
Pose3 poseInterp = interpolate<Pose3>(pose1, pose2, t); Pose3 poseInterp = interpolate<Pose3>(pose1, pose2, t);
PinholeCamera<Cal3_S2> camera(poseInterp, *K); PinholeCamera<Cal3_S2> camera(poseInterp, *K);
Point3 point(0.0, 0.0, 5.0); // 5 meters in front of the camera Point3 point(0.0, 0.0, 5.0); // 5 meters in front of the camera
Point2 measured = camera.project(point); Point2 measured = camera.project(point);
// create factor // create factor
ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2, pointKey, K); ProjectionFactorRollingShutter factor(measured, t, model, poseKey1,
poseKey2, pointKey, K);
// Use the factor to calculate the error // Use the factor to calculate the error
Vector actualError(factor.evaluateError(pose1, pose2, point)); Vector actualError(factor.evaluateError(pose1, pose2, point));
@ -175,19 +186,20 @@ TEST( ProjectionFactorRollingShutter, Error ) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( ProjectionFactorRollingShutter, ErrorWithTransform ) { TEST(ProjectionFactorRollingShutter, ErrorWithTransform) {
// Create measurement by projecting 3D landmark // Create measurement by projecting 3D landmark
double t = 0.3; double t = 0.3;
Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0,0,0)); Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0, 0, 0));
Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0,0,1)); Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0, 0, 1));
Pose3 poseInterp = interpolate<Pose3>(pose1, pose2, t); Pose3 poseInterp = interpolate<Pose3>(pose1, pose2, t);
Pose3 body_P_sensor3(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0,0.2,0.1)); Pose3 body_P_sensor3(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0, 0.2, 0.1));
PinholeCamera<Cal3_S2> camera(poseInterp*body_P_sensor3, *K); PinholeCamera<Cal3_S2> camera(poseInterp * body_P_sensor3, *K);
Point3 point(0.0, 0.0, 5.0); // 5 meters in front of the camera Point3 point(0.0, 0.0, 5.0); // 5 meters in front of the camera
Point2 measured = camera.project(point); Point2 measured = camera.project(point);
// create factor // create factor
ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2, pointKey, K, body_P_sensor3); ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2,
pointKey, K, body_P_sensor3);
// Use the factor to calculate the error // Use the factor to calculate the error
Vector actualError(factor.evaluateError(pose1, pose2, point)); Vector actualError(factor.evaluateError(pose1, pose2, point));
@ -200,18 +212,19 @@ TEST( ProjectionFactorRollingShutter, ErrorWithTransform ) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( ProjectionFactorRollingShutter, Jacobian ) { TEST(ProjectionFactorRollingShutter, Jacobian) {
// Create measurement by projecting 3D landmark // Create measurement by projecting 3D landmark
double t = 0.3; double t = 0.3;
Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0,0,0)); Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0, 0, 0));
Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0,0,1)); Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0, 0, 1));
Pose3 poseInterp = interpolate<Pose3>(pose1, pose2, t); Pose3 poseInterp = interpolate<Pose3>(pose1, pose2, t);
PinholeCamera<Cal3_S2> camera(poseInterp, *K); PinholeCamera<Cal3_S2> camera(poseInterp, *K);
Point3 point(0.0, 0.0, 5.0); // 5 meters in front of the camera Point3 point(0.0, 0.0, 5.0); // 5 meters in front of the camera
Point2 measured = camera.project(point); Point2 measured = camera.project(point);
// create factor // create factor
ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2, pointKey, K); ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2,
pointKey, K);
// Use the factor to calculate the Jacobians // Use the factor to calculate the Jacobians
Matrix H1Actual, H2Actual, H3Actual; Matrix H1Actual, H2Actual, H3Actual;
@ -222,22 +235,25 @@ TEST( ProjectionFactorRollingShutter, Jacobian ) {
std::function<Vector(const Pose3&, const Pose3&, const Point3&)>( std::function<Vector(const Pose3&, const Pose3&, const Point3&)>(
std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor,
std::placeholders::_1, std::placeholders::_2, std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, boost::none, boost::none, boost::none)), std::placeholders::_3, boost::none, boost::none,
pose1, pose2, point); boost::none)),
pose1, pose2, point);
Matrix H2Expected = numericalDerivative32<Vector, Pose3, Pose3, Point3>( Matrix H2Expected = numericalDerivative32<Vector, Pose3, Pose3, Point3>(
std::function<Vector(const Pose3&, const Pose3&, const Point3&)>( std::function<Vector(const Pose3&, const Pose3&, const Point3&)>(
std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor,
std::placeholders::_1, std::placeholders::_2, std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, boost::none, boost::none, boost::none)), std::placeholders::_3, boost::none, boost::none,
pose1, pose2, point); boost::none)),
pose1, pose2, point);
Matrix H3Expected = numericalDerivative33<Vector, Pose3, Pose3, Point3>( Matrix H3Expected = numericalDerivative33<Vector, Pose3, Pose3, Point3>(
std::function<Vector(const Pose3&, const Pose3&, const Point3&)>( std::function<Vector(const Pose3&, const Pose3&, const Point3&)>(
std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor,
std::placeholders::_1, std::placeholders::_2, std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, boost::none, boost::none, boost::none)), std::placeholders::_3, boost::none, boost::none,
pose1, pose2, point); boost::none)),
pose1, pose2, point);
CHECK(assert_equal(H1Expected, H1Actual, 1e-5)); CHECK(assert_equal(H1Expected, H1Actual, 1e-5));
CHECK(assert_equal(H2Expected, H2Actual, 1e-5)); CHECK(assert_equal(H2Expected, H2Actual, 1e-5));
@ -245,19 +261,20 @@ TEST( ProjectionFactorRollingShutter, Jacobian ) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( ProjectionFactorRollingShutter, JacobianWithTransform ) { TEST(ProjectionFactorRollingShutter, JacobianWithTransform) {
// Create measurement by projecting 3D landmark // Create measurement by projecting 3D landmark
double t = 0.6; double t = 0.6;
Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0,0,0)); Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0, 0, 0));
Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0,0,1)); Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0, 0, 1));
Pose3 poseInterp = interpolate<Pose3>(pose1, pose2, t); Pose3 poseInterp = interpolate<Pose3>(pose1, pose2, t);
Pose3 body_P_sensor3(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0,0.2,0.1)); Pose3 body_P_sensor3(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0, 0.2, 0.1));
PinholeCamera<Cal3_S2> camera(poseInterp*body_P_sensor3, *K); PinholeCamera<Cal3_S2> camera(poseInterp * body_P_sensor3, *K);
Point3 point(0.0, 0.0, 5.0); // 5 meters in front of the camera Point3 point(0.0, 0.0, 5.0); // 5 meters in front of the camera
Point2 measured = camera.project(point); Point2 measured = camera.project(point);
// create factor // create factor
ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2, pointKey, K, body_P_sensor3); ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2,
pointKey, K, body_P_sensor3);
// Use the factor to calculate the Jacobians // Use the factor to calculate the Jacobians
Matrix H1Actual, H2Actual, H3Actual; Matrix H1Actual, H2Actual, H3Actual;
@ -268,22 +285,25 @@ TEST( ProjectionFactorRollingShutter, JacobianWithTransform ) {
std::function<Vector(const Pose3&, const Pose3&, const Point3&)>( std::function<Vector(const Pose3&, const Pose3&, const Point3&)>(
std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor,
std::placeholders::_1, std::placeholders::_2, std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, boost::none, boost::none, boost::none)), std::placeholders::_3, boost::none, boost::none,
pose1, pose2, point); boost::none)),
pose1, pose2, point);
Matrix H2Expected = numericalDerivative32<Vector, Pose3, Pose3, Point3>( Matrix H2Expected = numericalDerivative32<Vector, Pose3, Pose3, Point3>(
std::function<Vector(const Pose3&, const Pose3&, const Point3&)>( std::function<Vector(const Pose3&, const Pose3&, const Point3&)>(
std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor,
std::placeholders::_1, std::placeholders::_2, std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, boost::none, boost::none, boost::none)), std::placeholders::_3, boost::none, boost::none,
pose1, pose2, point); boost::none)),
pose1, pose2, point);
Matrix H3Expected = numericalDerivative33<Vector, Pose3, Pose3, Point3>( Matrix H3Expected = numericalDerivative33<Vector, Pose3, Pose3, Point3>(
std::function<Vector(const Pose3&, const Pose3&, const Point3&)>( std::function<Vector(const Pose3&, const Pose3&, const Point3&)>(
std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor,
std::placeholders::_1, std::placeholders::_2, std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, boost::none, boost::none, boost::none)), std::placeholders::_3, boost::none, boost::none,
pose1, pose2, point); boost::none)),
pose1, pose2, point);
CHECK(assert_equal(H1Expected, H1Actual, 1e-5)); CHECK(assert_equal(H1Expected, H1Actual, 1e-5));
CHECK(assert_equal(H2Expected, H2Actual, 1e-5)); CHECK(assert_equal(H2Expected, H2Actual, 1e-5));
@ -291,41 +311,48 @@ TEST( ProjectionFactorRollingShutter, JacobianWithTransform ) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( ProjectionFactorRollingShutter, cheirality ) { TEST(ProjectionFactorRollingShutter, cheirality) {
// Create measurement by projecting 3D landmark behind camera // Create measurement by projecting 3D landmark behind camera
double t = 0.3; double t = 0.3;
Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0,0,0)); Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0, 0, 0));
Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0,0,1)); Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0, 0, 1));
Pose3 poseInterp = interpolate<Pose3>(pose1, pose2, t); Pose3 poseInterp = interpolate<Pose3>(pose1, pose2, t);
PinholeCamera<Cal3_S2> camera(poseInterp, *K); PinholeCamera<Cal3_S2> camera(poseInterp, *K);
Point3 point(0.0, 0.0, -5.0); // 5 meters behind the camera Point3 point(0.0, 0.0, -5.0); // 5 meters behind the camera
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
Point2 measured = Point2(0.0,0.0); // project would throw an exception Point2 measured = Point2(0.0, 0.0); // project would throw an exception
{ // check that exception is thrown if we set throwCheirality = true { // check that exception is thrown if we set throwCheirality = true
bool throwCheirality = true; bool throwCheirality = true;
bool verboseCheirality = true; bool verboseCheirality = true;
ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2, pointKey, K, throwCheirality, verboseCheirality); ProjectionFactorRollingShutter factor(measured, t, model, poseKey1,
poseKey2, pointKey, K,
throwCheirality, verboseCheirality);
CHECK_EXCEPTION(factor.evaluateError(pose1, pose2, point), CHECK_EXCEPTION(factor.evaluateError(pose1, pose2, point),
CheiralityException); CheiralityException);
} }
{ // check that exception is NOT thrown if we set throwCheirality = false, and outputs are correct { // check that exception is NOT thrown if we set throwCheirality = false,
bool throwCheirality = false; // default // and outputs are correct
bool verboseCheirality = false; // default bool throwCheirality = false; // default
ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2, pointKey, K, throwCheirality, verboseCheirality); bool verboseCheirality = false; // default
ProjectionFactorRollingShutter factor(measured, t, model, poseKey1,
poseKey2, pointKey, K,
throwCheirality, verboseCheirality);
// Use the factor to calculate the error // Use the factor to calculate the error
Matrix H1Actual, H2Actual, H3Actual; Matrix H1Actual, H2Actual, H3Actual;
Vector actualError(factor.evaluateError(pose1, pose2, point, H1Actual, H2Actual, H3Actual)); Vector actualError(factor.evaluateError(pose1, pose2, point, H1Actual,
H2Actual, H3Actual));
// The expected error is zero // The expected error is zero
Vector expectedError = Vector2::Constant(2.0 * K->fx()); // this is what we return when point is behind camera Vector expectedError = Vector2::Constant(
2.0 * K->fx()); // this is what we return when point is behind camera
// Verify we get the expected error // Verify we get the expected error
CHECK(assert_equal(expectedError, actualError, 1e-9)); CHECK(assert_equal(expectedError, actualError, 1e-9));
CHECK(assert_equal(Matrix::Zero(2,6), H1Actual, 1e-5)); CHECK(assert_equal(Matrix::Zero(2, 6), H1Actual, 1e-5));
CHECK(assert_equal(Matrix::Zero(2,6), H2Actual, 1e-5)); CHECK(assert_equal(Matrix::Zero(2, 6), H2Actual, 1e-5));
CHECK(assert_equal(Matrix::Zero(2,3), H3Actual, 1e-5)); CHECK(assert_equal(Matrix::Zero(2, 3), H3Actual, 1e-5));
} }
#else #else
{ {
@ -333,7 +360,8 @@ TEST( ProjectionFactorRollingShutter, cheirality ) {
Point2 measured = camera.project(point); Point2 measured = camera.project(point);
// create factor // create factor
ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2, pointKey, K); ProjectionFactorRollingShutter factor(measured, t, model, poseKey1,
poseKey2, pointKey, K);
// Use the factor to calculate the Jacobians // Use the factor to calculate the Jacobians
Matrix H1Actual, H2Actual, H3Actual; Matrix H1Actual, H2Actual, H3Actual;
@ -344,22 +372,25 @@ TEST( ProjectionFactorRollingShutter, cheirality ) {
std::function<Vector(const Pose3&, const Pose3&, const Point3&)>( std::function<Vector(const Pose3&, const Pose3&, const Point3&)>(
std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor,
std::placeholders::_1, std::placeholders::_2, std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, boost::none, boost::none, boost::none)), std::placeholders::_3, boost::none, boost::none,
pose1, pose2, point); boost::none)),
pose1, pose2, point);
Matrix H2Expected = numericalDerivative32<Vector, Pose3, Pose3, Point3>( Matrix H2Expected = numericalDerivative32<Vector, Pose3, Pose3, Point3>(
std::function<Vector(const Pose3&, const Pose3&, const Point3&)>( std::function<Vector(const Pose3&, const Pose3&, const Point3&)>(
std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor,
std::placeholders::_1, std::placeholders::_2, std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, boost::none, boost::none, boost::none)), std::placeholders::_3, boost::none, boost::none,
pose1, pose2, point); boost::none)),
pose1, pose2, point);
Matrix H3Expected = numericalDerivative33<Vector, Pose3, Pose3, Point3>( Matrix H3Expected = numericalDerivative33<Vector, Pose3, Pose3, Point3>(
std::function<Vector(const Pose3&, const Pose3&, const Point3&)>( std::function<Vector(const Pose3&, const Pose3&, const Point3&)>(
std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor,
std::placeholders::_1, std::placeholders::_2, std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, boost::none, boost::none, boost::none)), std::placeholders::_3, boost::none, boost::none,
pose1, pose2, point); boost::none)),
pose1, pose2, point);
CHECK(assert_equal(H1Expected, H1Actual, 1e-5)); CHECK(assert_equal(H1Expected, H1Actual, 1e-5));
CHECK(assert_equal(H2Expected, H2Actual, 1e-5)); CHECK(assert_equal(H2Expected, H2Actual, 1e-5));
@ -368,8 +399,9 @@ TEST( ProjectionFactorRollingShutter, cheirality ) {
#endif #endif
} }
/* ************************************************************************* */ /* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); } int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */ /* ************************************************************************* */

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

@ -16,17 +16,19 @@
* @date July 2021 * @date July 2021
*/ */
#include "gtsam/slam/tests/smartFactorScenarios.h" #include <CppUnitLite/TestHarness.h>
#include <gtsam/slam/ProjectionFactor.h>
#include <gtsam/slam/PoseTranslationPrior.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h>
#include <gtsam_unstable/slam/ProjectionFactorRollingShutter.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 <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/slam/PoseTranslationPrior.h>
#include <gtsam/slam/ProjectionFactor.h>
#include <gtsam_unstable/slam/ProjectionFactorRollingShutter.h>
#include <gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h>
#include <boost/assign/std/map.hpp> #include <boost/assign/std/map.hpp>
#include <iostream> #include <iostream>
#include "gtsam/slam/tests/smartFactorScenarios.h"
#define DISABLE_TIMING #define DISABLE_TIMING
using namespace gtsam; using namespace gtsam;
@ -39,8 +41,8 @@ static const double sigma = 0.1;
static SharedIsotropic model(noiseModel::Isotropic::Sigma(2, sigma)); static SharedIsotropic model(noiseModel::Isotropic::Sigma(2, sigma));
// Convenience for named keys // Convenience for named keys
using symbol_shorthand::X;
using symbol_shorthand::L; using symbol_shorthand::L;
using symbol_shorthand::X;
// tests data // tests data
static Symbol x1('X', 1); static Symbol x1('X', 1);
@ -48,8 +50,8 @@ static Symbol x2('X', 2);
static Symbol x3('X', 3); static Symbol x3('X', 3);
static Symbol x4('X', 4); static Symbol x4('X', 4);
static Symbol l0('L', 0); static Symbol l0('L', 0);
static Pose3 body_P_sensor = Pose3(Rot3::Ypr(-0.1, 0.2, -0.2), static Pose3 body_P_sensor =
Point3(0.1, 0.0, 0.0)); Pose3(Rot3::Ypr(-0.1, 0.2, -0.2), Point3(0.1, 0.0, 0.0));
static Point2 measurement1(323.0, 240.0); static Point2 measurement1(323.0, 240.0);
static Point2 measurement2(200.0, 220.0); static Point2 measurement2(200.0, 220.0);
@ -64,38 +66,39 @@ static double interp_factor3 = 0.5;
namespace vanillaPoseRS { namespace vanillaPoseRS {
typedef PinholePose<Cal3_S2> Camera; typedef PinholePose<Cal3_S2> Camera;
static Cal3_S2::shared_ptr sharedK(new Cal3_S2(fov, w, h)); static Cal3_S2::shared_ptr sharedK(new Cal3_S2(fov, w, h));
Pose3 interp_pose1 = interpolate<Pose3>(level_pose,pose_right,interp_factor1); Pose3 interp_pose1 = interpolate<Pose3>(level_pose, pose_right, interp_factor1);
Pose3 interp_pose2 = interpolate<Pose3>(pose_right,pose_above,interp_factor2); Pose3 interp_pose2 = interpolate<Pose3>(pose_right, pose_above, interp_factor2);
Pose3 interp_pose3 = interpolate<Pose3>(pose_above,level_pose,interp_factor3); Pose3 interp_pose3 = interpolate<Pose3>(pose_above, level_pose, interp_factor3);
Camera cam1(interp_pose1, sharedK); Camera cam1(interp_pose1, sharedK);
Camera cam2(interp_pose2, sharedK); Camera cam2(interp_pose2, sharedK);
Camera cam3(interp_pose3, sharedK); Camera cam3(interp_pose3, sharedK);
} } // namespace vanillaPoseRS
LevenbergMarquardtParams lmParams; LevenbergMarquardtParams lmParams;
typedef SmartProjectionPoseFactorRollingShutter< PinholePose<Cal3_S2> > SmartFactorRS; typedef SmartProjectionPoseFactorRollingShutter<PinholePose<Cal3_S2>>
SmartFactorRS;
/* ************************************************************************* */ /* ************************************************************************* */
TEST( SmartProjectionPoseFactorRollingShutter, Constructor) { TEST(SmartProjectionPoseFactorRollingShutter, Constructor) {
SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( SmartProjectionPoseFactorRollingShutter, Constructor2) { TEST(SmartProjectionPoseFactorRollingShutter, Constructor2) {
SmartProjectionParams params; SmartProjectionParams params;
params.setRankTolerance(rankTol); params.setRankTolerance(rankTol);
SmartFactorRS factor1(model, params); SmartFactorRS factor1(model, params);
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( SmartProjectionPoseFactorRollingShutter, add) { TEST(SmartProjectionPoseFactorRollingShutter, add) {
using namespace vanillaPose; using namespace vanillaPose;
SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model));
factor1->add(measurement1, x1, x2, interp_factor, sharedK, body_P_sensor); factor1->add(measurement1, x1, x2, interp_factor, sharedK, body_P_sensor);
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( SmartProjectionPoseFactorRollingShutter, Equals ) { TEST(SmartProjectionPoseFactorRollingShutter, Equals) {
using namespace vanillaPose; using namespace vanillaPose;
// create fake measurements // create fake measurements
@ -104,10 +107,10 @@ TEST( SmartProjectionPoseFactorRollingShutter, Equals ) {
measurements.push_back(measurement2); measurements.push_back(measurement2);
measurements.push_back(measurement3); measurements.push_back(measurement3);
std::vector<std::pair<Key,Key>> key_pairs; std::vector<std::pair<Key, Key>> key_pairs;
key_pairs.push_back(std::make_pair(x1,x2)); key_pairs.push_back(std::make_pair(x1, x2));
key_pairs.push_back(std::make_pair(x2,x3)); key_pairs.push_back(std::make_pair(x2, x3));
key_pairs.push_back(std::make_pair(x3,x4)); key_pairs.push_back(std::make_pair(x3, x4));
std::vector<boost::shared_ptr<Cal3_S2>> intrinsicCalibrations; std::vector<boost::shared_ptr<Cal3_S2>> intrinsicCalibrations;
intrinsicCalibrations.push_back(sharedK); intrinsicCalibrations.push_back(sharedK);
@ -126,13 +129,14 @@ TEST( SmartProjectionPoseFactorRollingShutter, Equals ) {
// create by adding a batch of measurements with a bunch of calibrations // create by adding a batch of measurements with a bunch of calibrations
SmartFactorRS::shared_ptr factor2(new SmartFactorRS(model)); SmartFactorRS::shared_ptr factor2(new SmartFactorRS(model));
factor2->add(measurements, key_pairs, interp_factors, intrinsicCalibrations, extrinsicCalibrations); factor2->add(measurements, key_pairs, interp_factors, intrinsicCalibrations,
extrinsicCalibrations);
// create by adding a batch of measurements with a single calibrations // create by adding a batch of measurements with a single calibrations
SmartFactorRS::shared_ptr factor3(new SmartFactorRS(model)); SmartFactorRS::shared_ptr factor3(new SmartFactorRS(model));
factor3->add(measurements, key_pairs, interp_factors, sharedK, body_P_sensor); factor3->add(measurements, key_pairs, interp_factors, sharedK, body_P_sensor);
{ // create equal factors and show equal returns true { // create equal factors and show equal returns true
SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model));
factor1->add(measurement1, x1, x2, interp_factor1, sharedK, body_P_sensor); factor1->add(measurement1, x1, x2, interp_factor1, sharedK, body_P_sensor);
factor1->add(measurement2, x2, x3, interp_factor2, sharedK, body_P_sensor); factor1->add(measurement2, x2, x3, interp_factor2, sharedK, body_P_sensor);
@ -141,28 +145,34 @@ TEST( SmartProjectionPoseFactorRollingShutter, Equals ) {
EXPECT(factor1->equals(*factor2)); EXPECT(factor1->equals(*factor2));
EXPECT(factor1->equals(*factor3)); EXPECT(factor1->equals(*factor3));
} }
{ // create slightly different factors (different keys) and show equal returns false { // create slightly different factors (different keys) and show equal
// returns false
SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model));
factor1->add(measurement1, x1, x2, interp_factor1, sharedK, body_P_sensor); factor1->add(measurement1, x1, x2, interp_factor1, sharedK, body_P_sensor);
factor1->add(measurement2, x2, x2, interp_factor2, sharedK, body_P_sensor); // different! factor1->add(measurement2, x2, x2, interp_factor2, sharedK,
body_P_sensor); // different!
factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor); factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor);
EXPECT(!factor1->equals(*factor2)); EXPECT(!factor1->equals(*factor2));
EXPECT(!factor1->equals(*factor3)); EXPECT(!factor1->equals(*factor3));
} }
{ // create slightly different factors (different extrinsics) and show equal returns false { // create slightly different factors (different extrinsics) and show equal
// returns false
SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model));
factor1->add(measurement1, x1, x2, interp_factor1, sharedK, body_P_sensor); factor1->add(measurement1, x1, x2, interp_factor1, sharedK, body_P_sensor);
factor1->add(measurement2, x2, x3, interp_factor2, sharedK, body_P_sensor*body_P_sensor); // different! factor1->add(measurement2, x2, x3, interp_factor2, sharedK,
body_P_sensor * body_P_sensor); // different!
factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor); factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor);
EXPECT(!factor1->equals(*factor2)); EXPECT(!factor1->equals(*factor2));
EXPECT(!factor1->equals(*factor3)); EXPECT(!factor1->equals(*factor3));
} }
{ // create slightly different factors (different interp factors) and show equal returns false { // create slightly different factors (different interp factors) and show
// equal returns false
SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model));
factor1->add(measurement1, x1, x2, interp_factor1, sharedK, body_P_sensor); factor1->add(measurement1, x1, x2, interp_factor1, sharedK, body_P_sensor);
factor1->add(measurement2, x2, x3, interp_factor1, sharedK, body_P_sensor); // different! factor1->add(measurement2, x2, x3, interp_factor1, sharedK,
body_P_sensor); // different!
factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor); factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor);
EXPECT(!factor1->equals(*factor2)); EXPECT(!factor1->equals(*factor2));
@ -170,13 +180,16 @@ TEST( SmartProjectionPoseFactorRollingShutter, Equals ) {
} }
} }
static const int DimBlock = 12; ///< size of the variable stacking 2 poses from which the observation pose is interpolated static const int DimBlock = 12; ///< size of the variable stacking 2 poses from
static const int ZDim = 2; ///< Measurement dimension (Point2) ///< which the observation pose is interpolated
typedef Eigen::Matrix<double, ZDim, DimBlock> MatrixZD; // F blocks (derivatives wrt camera) static const int ZDim = 2; ///< Measurement dimension (Point2)
typedef std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> > FBlocks; // vector of F blocks typedef Eigen::Matrix<double, ZDim, DimBlock>
MatrixZD; // F blocks (derivatives wrt camera)
typedef std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD>>
FBlocks; // vector of F blocks
/* *************************************************************************/ /* *************************************************************************/
TEST( SmartProjectionPoseFactorRollingShutter, noiselessErrorAndJacobians ) { TEST(SmartProjectionPoseFactorRollingShutter, noiselessErrorAndJacobians) {
using namespace vanillaPoseRS; using namespace vanillaPoseRS;
// Project two landmarks into two cameras // Project two landmarks into two cameras
@ -188,7 +201,7 @@ TEST( SmartProjectionPoseFactorRollingShutter, noiselessErrorAndJacobians ) {
factor.add(level_uv, x1, x2, interp_factor1, sharedK, body_P_sensorId); factor.add(level_uv, x1, x2, interp_factor1, sharedK, body_P_sensorId);
factor.add(level_uv_right, x2, x3, interp_factor2, sharedK, body_P_sensorId); factor.add(level_uv_right, x2, x3, interp_factor2, sharedK, body_P_sensorId);
Values values; // it's a pose factor, hence these are poses Values values; // it's a pose factor, hence these are poses
values.insert(x1, level_pose); values.insert(x1, level_pose);
values.insert(x2, pose_right); values.insert(x2, pose_right);
values.insert(x3, pose_above); values.insert(x3, pose_above);
@ -200,41 +213,56 @@ TEST( SmartProjectionPoseFactorRollingShutter, noiselessErrorAndJacobians ) {
// Check triangulation // Check triangulation
factor.triangulateSafe(factor.cameras(values)); factor.triangulateSafe(factor.cameras(values));
TriangulationResult point = factor.point(); TriangulationResult point = factor.point();
EXPECT(point.valid()); // check triangulated point is valid EXPECT(point.valid()); // check triangulated point is valid
EXPECT(assert_equal(landmark1, *point)); // check triangulation result matches expected 3D landmark EXPECT(assert_equal(
landmark1,
*point)); // check triangulation result matches expected 3D landmark
// Check Jacobians // Check Jacobians
// -- actual Jacobians // -- actual Jacobians
FBlocks actualFs; FBlocks actualFs;
Matrix actualE; Matrix actualE;
Vector actualb; Vector actualb;
factor.computeJacobiansWithTriangulatedPoint(actualFs, actualE, actualb, values); factor.computeJacobiansWithTriangulatedPoint(actualFs, actualE, actualb,
EXPECT(actualE.rows() == 4); EXPECT(actualE.cols() == 3); values);
EXPECT(actualb.rows() == 4); EXPECT(actualb.cols() == 1); EXPECT(actualE.rows() == 4);
EXPECT(actualE.cols() == 3);
EXPECT(actualb.rows() == 4);
EXPECT(actualb.cols() == 1);
EXPECT(actualFs.size() == 2); EXPECT(actualFs.size() == 2);
// -- expected Jacobians from ProjectionFactorsRollingShutter // -- expected Jacobians from ProjectionFactorsRollingShutter
ProjectionFactorRollingShutter factor1(level_uv, interp_factor1, model, x1, x2, l0, sharedK, body_P_sensorId); ProjectionFactorRollingShutter factor1(level_uv, interp_factor1, model, x1,
x2, l0, sharedK, body_P_sensorId);
Matrix expectedF11, expectedF12, expectedE1; Matrix expectedF11, expectedF12, expectedE1;
Vector expectedb1 = factor1.evaluateError(level_pose, pose_right, landmark1, expectedF11, expectedF12, expectedE1); Vector expectedb1 = factor1.evaluateError(
EXPECT(assert_equal( expectedF11, Matrix(actualFs[0].block(0,0,2,6)), 1e-5)); level_pose, pose_right, landmark1, expectedF11, expectedF12, expectedE1);
EXPECT(assert_equal( expectedF12, Matrix(actualFs[0].block(0,6,2,6)), 1e-5)); EXPECT(
EXPECT(assert_equal( expectedE1, Matrix(actualE.block(0,0,2,3)), 1e-5)); assert_equal(expectedF11, Matrix(actualFs[0].block(0, 0, 2, 6)), 1e-5));
// by definition computeJacobiansWithTriangulatedPoint returns minus reprojectionError EXPECT(
EXPECT(assert_equal( expectedb1, -Vector(actualb.segment<2>(0)), 1e-5)); assert_equal(expectedF12, Matrix(actualFs[0].block(0, 6, 2, 6)), 1e-5));
EXPECT(assert_equal(expectedE1, Matrix(actualE.block(0, 0, 2, 3)), 1e-5));
// by definition computeJacobiansWithTriangulatedPoint returns minus
// reprojectionError
EXPECT(assert_equal(expectedb1, -Vector(actualb.segment<2>(0)), 1e-5));
ProjectionFactorRollingShutter factor2(level_uv_right, interp_factor2, model, x2, x3, l0, sharedK, body_P_sensorId); ProjectionFactorRollingShutter factor2(level_uv_right, interp_factor2, model,
x2, x3, l0, sharedK, body_P_sensorId);
Matrix expectedF21, expectedF22, expectedE2; Matrix expectedF21, expectedF22, expectedE2;
Vector expectedb2 = factor2.evaluateError(pose_right, pose_above, landmark1, expectedF21, expectedF22, expectedE2); Vector expectedb2 = factor2.evaluateError(
EXPECT(assert_equal( expectedF21, Matrix(actualFs[1].block(0,0,2,6)), 1e-5)); pose_right, pose_above, landmark1, expectedF21, expectedF22, expectedE2);
EXPECT(assert_equal( expectedF22, Matrix(actualFs[1].block(0,6,2,6)), 1e-5)); EXPECT(
EXPECT(assert_equal( expectedE2, Matrix(actualE.block(2,0,2,3)), 1e-5)); assert_equal(expectedF21, Matrix(actualFs[1].block(0, 0, 2, 6)), 1e-5));
// by definition computeJacobiansWithTriangulatedPoint returns minus reprojectionError EXPECT(
EXPECT(assert_equal( expectedb2, -Vector(actualb.segment<2>(2)), 1e-5)); assert_equal(expectedF22, Matrix(actualFs[1].block(0, 6, 2, 6)), 1e-5));
EXPECT(assert_equal(expectedE2, Matrix(actualE.block(2, 0, 2, 3)), 1e-5));
// by definition computeJacobiansWithTriangulatedPoint returns minus
// reprojectionError
EXPECT(assert_equal(expectedb2, -Vector(actualb.segment<2>(2)), 1e-5));
} }
/* *************************************************************************/ /* *************************************************************************/
TEST( SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians ) { TEST(SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians) {
// also includes non-identical extrinsic calibration // also includes non-identical extrinsic calibration
using namespace vanillaPoseRS; using namespace vanillaPoseRS;
@ -246,9 +274,10 @@ TEST( SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians ) {
SmartFactorRS factor(model); SmartFactorRS factor(model);
factor.add(level_uv, x1, x2, interp_factor1, sharedK, body_P_sensorNonId); factor.add(level_uv, x1, x2, interp_factor1, sharedK, body_P_sensorNonId);
factor.add(level_uv_right, x2, x3, interp_factor2, sharedK, body_P_sensorNonId); factor.add(level_uv_right, x2, x3, interp_factor2, sharedK,
body_P_sensorNonId);
Values values; // it's a pose factor, hence these are poses Values values; // it's a pose factor, hence these are poses
values.insert(x1, level_pose); values.insert(x1, level_pose);
values.insert(x2, pose_right); values.insert(x2, pose_right);
values.insert(x3, pose_above); values.insert(x3, pose_above);
@ -256,7 +285,7 @@ TEST( SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians ) {
// Perform triangulation // Perform triangulation
factor.triangulateSafe(factor.cameras(values)); factor.triangulateSafe(factor.cameras(values));
TriangulationResult point = factor.point(); TriangulationResult point = factor.point();
EXPECT(point.valid()); // check triangulated point is valid EXPECT(point.valid()); // check triangulated point is valid
Point3 landmarkNoisy = *point; Point3 landmarkNoisy = *point;
// Check Jacobians // Check Jacobians
@ -264,32 +293,48 @@ TEST( SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians ) {
FBlocks actualFs; FBlocks actualFs;
Matrix actualE; Matrix actualE;
Vector actualb; Vector actualb;
factor.computeJacobiansWithTriangulatedPoint(actualFs, actualE, actualb, values); factor.computeJacobiansWithTriangulatedPoint(actualFs, actualE, actualb,
EXPECT(actualE.rows() == 4); EXPECT(actualE.cols() == 3); values);
EXPECT(actualb.rows() == 4); EXPECT(actualb.cols() == 1); EXPECT(actualE.rows() == 4);
EXPECT(actualE.cols() == 3);
EXPECT(actualb.rows() == 4);
EXPECT(actualb.cols() == 1);
EXPECT(actualFs.size() == 2); EXPECT(actualFs.size() == 2);
// -- expected Jacobians from ProjectionFactorsRollingShutter // -- expected Jacobians from ProjectionFactorsRollingShutter
ProjectionFactorRollingShutter factor1(level_uv, interp_factor1, model, x1, x2, l0, sharedK, body_P_sensorNonId); ProjectionFactorRollingShutter factor1(level_uv, interp_factor1, model, x1,
x2, l0, sharedK, body_P_sensorNonId);
Matrix expectedF11, expectedF12, expectedE1; Matrix expectedF11, expectedF12, expectedE1;
Vector expectedb1 = factor1.evaluateError(level_pose, pose_right, landmarkNoisy, expectedF11, expectedF12, expectedE1); Vector expectedb1 =
EXPECT(assert_equal( expectedF11, Matrix(actualFs[0].block(0,0,2,6)), 1e-5)); factor1.evaluateError(level_pose, pose_right, landmarkNoisy, expectedF11,
EXPECT(assert_equal( expectedF12, Matrix(actualFs[0].block(0,6,2,6)), 1e-5)); expectedF12, expectedE1);
EXPECT(assert_equal( expectedE1, Matrix(actualE.block(0,0,2,3)), 1e-5)); EXPECT(
// by definition computeJacobiansWithTriangulatedPoint returns minus reprojectionError assert_equal(expectedF11, Matrix(actualFs[0].block(0, 0, 2, 6)), 1e-5));
EXPECT(assert_equal( expectedb1, -Vector(actualb.segment<2>(0)), 1e-5)); EXPECT(
assert_equal(expectedF12, Matrix(actualFs[0].block(0, 6, 2, 6)), 1e-5));
EXPECT(assert_equal(expectedE1, Matrix(actualE.block(0, 0, 2, 3)), 1e-5));
// by definition computeJacobiansWithTriangulatedPoint returns minus
// reprojectionError
EXPECT(assert_equal(expectedb1, -Vector(actualb.segment<2>(0)), 1e-5));
ProjectionFactorRollingShutter factor2(level_uv_right, interp_factor2, model, x2, x3, l0, sharedK, body_P_sensorNonId); ProjectionFactorRollingShutter factor2(level_uv_right, interp_factor2, model,
x2, x3, l0, sharedK,
body_P_sensorNonId);
Matrix expectedF21, expectedF22, expectedE2; Matrix expectedF21, expectedF22, expectedE2;
Vector expectedb2 = factor2.evaluateError(pose_right, pose_above, landmarkNoisy, expectedF21, expectedF22, expectedE2); Vector expectedb2 =
EXPECT(assert_equal( expectedF21, Matrix(actualFs[1].block(0,0,2,6)), 1e-5)); factor2.evaluateError(pose_right, pose_above, landmarkNoisy, expectedF21,
EXPECT(assert_equal( expectedF22, Matrix(actualFs[1].block(0,6,2,6)), 1e-5)); expectedF22, expectedE2);
EXPECT(assert_equal( expectedE2, Matrix(actualE.block(2,0,2,3)), 1e-5)); EXPECT(
// by definition computeJacobiansWithTriangulatedPoint returns minus reprojectionError assert_equal(expectedF21, Matrix(actualFs[1].block(0, 0, 2, 6)), 1e-5));
EXPECT(assert_equal( expectedb2, -Vector(actualb.segment<2>(2)), 1e-5)); EXPECT(
assert_equal(expectedF22, Matrix(actualFs[1].block(0, 6, 2, 6)), 1e-5));
EXPECT(assert_equal(expectedE2, Matrix(actualE.block(2, 0, 2, 3)), 1e-5));
// by definition computeJacobiansWithTriangulatedPoint returns minus
// reprojectionError
EXPECT(assert_equal(expectedb2, -Vector(actualb.segment<2>(2)), 1e-5));
// Check errors // Check errors
double actualError = factor.error(values); // from smart factor double actualError = factor.error(values); // from smart factor
NonlinearFactorGraph nfg; NonlinearFactorGraph nfg;
nfg.add(factor1); nfg.add(factor1);
nfg.add(factor2); nfg.add(factor2);
@ -299,8 +344,7 @@ TEST( SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians ) {
} }
/* *************************************************************************/ /* *************************************************************************/
TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses ) { TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses) {
using namespace vanillaPoseRS; using namespace vanillaPoseRS;
Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3;
@ -310,10 +354,10 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses ) {
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3);
// create inputs // create inputs
std::vector<std::pair<Key,Key>> key_pairs; std::vector<std::pair<Key, Key>> key_pairs;
key_pairs.push_back(std::make_pair(x1,x2)); key_pairs.push_back(std::make_pair(x1, x2));
key_pairs.push_back(std::make_pair(x2,x3)); key_pairs.push_back(std::make_pair(x2, x3));
key_pairs.push_back(std::make_pair(x3,x1)); key_pairs.push_back(std::make_pair(x3, x1));
std::vector<double> interp_factors; std::vector<double> interp_factors;
interp_factors.push_back(interp_factor1); interp_factors.push_back(interp_factor1);
@ -344,20 +388,22 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses ) {
groundTruth.insert(x3, pose_above); groundTruth.insert(x3, pose_above);
DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9); DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9);
// 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/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), Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
Point3(0.1, 0.1, 0.1)); // smaller noise Point3(0.1, 0.1, 0.1)); // smaller noise
Values values; Values values;
values.insert(x1, level_pose); values.insert(x1, level_pose);
values.insert(x2, pose_right); values.insert(x2, pose_right);
// initialize third pose with some noise, we expect it to move back to original pose_above // initialize third pose with some noise, we expect it to move back to
// original pose_above
values.insert(x3, pose_above * noise_pose); values.insert(x3, pose_above * noise_pose);
EXPECT( // check that the pose is actually noisy EXPECT( // check that the pose is actually noisy
assert_equal( assert_equal(Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656,
Pose3( -0.0313952598, -0.000986635786, 0.0314107591,
Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, -0.999013364, -0.0313952598),
-0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), Point3(0.1, -0.1, 1.9)),
Point3(0.1, -0.1, 1.9)), values.at<Pose3>(x3))); values.at<Pose3>(x3)));
Values result; Values result;
LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
@ -366,11 +412,12 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses ) {
} }
/* *************************************************************************/ /* *************************************************************************/
TEST( SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses ) { TEST(SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses) {
// here we replicate a test in SmartProjectionPoseFactor by setting interpolation // here we replicate a test in SmartProjectionPoseFactor by setting
// factors to 0 and 1 (such that the rollingShutter measurements falls back to standard pixel measurements) // interpolation factors to 0 and 1 (such that the rollingShutter measurements
// Note: this is a quite extreme test since in typical camera you would not have more than // falls back to standard pixel measurements) Note: this is a quite extreme
// 1 measurement per landmark at each interpolated pose // test since in typical camera you would not have more than 1 measurement per
// landmark at each interpolated pose
using namespace vanillaPose; using namespace vanillaPose;
// Default cameras for simple derivatives // Default cameras for simple derivatives
@ -423,7 +470,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses ) {
perturbedDelta.insert(x2, delta); perturbedDelta.insert(x2, delta);
double expectedError = 2500; double expectedError = 2500;
// After eliminating the point, A1 and A2 contain 2-rank information on cameras: // After eliminating the point, A1 and A2 contain 2-rank information on
// cameras:
Matrix16 A1, A2; Matrix16 A1, A2;
A1 << -10, 0, 0, 0, 1, 0; A1 << -10, 0, 0, 0, 1, 0;
A2 << 10, 0, 1, 0, -1, 0; A2 << 10, 0, 1, 0, -1, 0;
@ -449,8 +497,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses ) {
Values values; Values values;
values.insert(x1, pose1); values.insert(x1, pose1);
values.insert(x2, pose2); values.insert(x2, pose2);
boost::shared_ptr < RegularHessianFactor<6> > actual = smartFactor1 boost::shared_ptr<RegularHessianFactor<6>> actual =
->createHessianFactor(values); smartFactor1->createHessianFactor(values);
EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6)); EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6));
EXPECT(assert_equal(expected, *actual, 1e-6)); EXPECT(assert_equal(expected, *actual, 1e-6));
EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-6); EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-6);
@ -458,7 +506,7 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses ) {
} }
/* *************************************************************************/ /* *************************************************************************/
TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI ) { TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI) {
using namespace vanillaPoseRS; using namespace vanillaPoseRS;
Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3;
@ -478,7 +526,7 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI ) {
interp_factors.push_back(interp_factor2); interp_factors.push_back(interp_factor2);
interp_factors.push_back(interp_factor3); interp_factors.push_back(interp_factor3);
double excludeLandmarksFutherThanDist = 1e10; //very large double excludeLandmarksFutherThanDist = 1e10; // very large
SmartProjectionParams params; SmartProjectionParams params;
params.setRankTolerance(1.0); params.setRankTolerance(1.0);
params.setLinearizationMode(gtsam::HESSIAN); params.setLinearizationMode(gtsam::HESSIAN);
@ -486,13 +534,13 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI ) {
params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist);
params.setEnableEPI(true); params.setEnableEPI(true);
SmartFactorRS smartFactor1(model,params); SmartFactorRS smartFactor1(model, params);
smartFactor1.add(measurements_lmk1, key_pairs, interp_factors, sharedK); smartFactor1.add(measurements_lmk1, key_pairs, interp_factors, sharedK);
SmartFactorRS smartFactor2(model,params); SmartFactorRS smartFactor2(model, params);
smartFactor2.add(measurements_lmk2, key_pairs, interp_factors, sharedK); smartFactor2.add(measurements_lmk2, key_pairs, interp_factors, sharedK);
SmartFactorRS smartFactor3(model,params); SmartFactorRS smartFactor3(model, params);
smartFactor3.add(measurements_lmk3, key_pairs, interp_factors, sharedK); smartFactor3.add(measurements_lmk3, key_pairs, interp_factors, sharedK);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -509,7 +557,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI ) {
Values values; Values values;
values.insert(x1, level_pose); values.insert(x1, level_pose);
values.insert(x2, pose_right); values.insert(x2, pose_right);
// initialize third pose with some noise, we expect it to move back to original pose_above // initialize third pose with some noise, we expect it to move back to
// original pose_above
values.insert(x3, pose_above * noise_pose); values.insert(x3, pose_above * noise_pose);
// Optimization should correct 3rd pose // Optimization should correct 3rd pose
@ -520,7 +569,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI ) {
} }
/* *************************************************************************/ /* *************************************************************************/
TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_landmarkDistance ) { TEST(SmartProjectionPoseFactorRollingShutter,
optimization_3poses_landmarkDistance) {
using namespace vanillaPoseRS; using namespace vanillaPoseRS;
Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3;
@ -548,13 +598,13 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_landmarkDista
params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist);
params.setEnableEPI(false); params.setEnableEPI(false);
SmartFactorRS smartFactor1(model,params); SmartFactorRS smartFactor1(model, params);
smartFactor1.add(measurements_lmk1, key_pairs, interp_factors, sharedK); smartFactor1.add(measurements_lmk1, key_pairs, interp_factors, sharedK);
SmartFactorRS smartFactor2(model,params); SmartFactorRS smartFactor2(model, params);
smartFactor2.add(measurements_lmk2, key_pairs, interp_factors, sharedK); smartFactor2.add(measurements_lmk2, key_pairs, interp_factors, sharedK);
SmartFactorRS smartFactor3(model,params); SmartFactorRS smartFactor3(model, params);
smartFactor3.add(measurements_lmk3, key_pairs, interp_factors, sharedK); smartFactor3.add(measurements_lmk3, key_pairs, interp_factors, sharedK);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -571,10 +621,12 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_landmarkDista
Values values; Values values;
values.insert(x1, level_pose); values.insert(x1, level_pose);
values.insert(x2, pose_right); values.insert(x2, pose_right);
// initialize third pose with some noise, we expect it to move back to original pose_above // initialize third pose with some noise, we expect it to move back to
// original pose_above
values.insert(x3, pose_above * noise_pose); values.insert(x3, pose_above * noise_pose);
// All factors are disabled (due to the distance threshold) and pose should remain where it is // All factors are disabled (due to the distance threshold) and pose should
// remain where it is
Values result; Values result;
LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
result = optimizer.optimize(); result = optimizer.optimize();
@ -582,7 +634,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_landmarkDista
} }
/* *************************************************************************/ /* *************************************************************************/
TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_dynamicOutlierRejection ) { TEST(SmartProjectionPoseFactorRollingShutter,
optimization_3poses_dynamicOutlierRejection) {
using namespace vanillaPoseRS; using namespace vanillaPoseRS;
// add fourth landmark // add fourth landmark
Point3 landmark4(5, -0.5, 1); Point3 landmark4(5, -0.5, 1);
@ -594,7 +647,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_dynamicOutlie
projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2); projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2);
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3);
projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_lmk4); projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_lmk4);
measurements_lmk4.at(0) = measurements_lmk4.at(0) + Point2(10, 10); // add outlier measurements_lmk4.at(0) =
measurements_lmk4.at(0) + Point2(10, 10); // add outlier
// create inputs // create inputs
std::vector<std::pair<Key, Key>> key_pairs; std::vector<std::pair<Key, Key>> key_pairs;
@ -608,7 +662,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_dynamicOutlie
interp_factors.push_back(interp_factor3); interp_factors.push_back(interp_factor3);
double excludeLandmarksFutherThanDist = 1e10; double excludeLandmarksFutherThanDist = 1e10;
double dynamicOutlierRejectionThreshold = 3; // max 3 pixel of average reprojection error double dynamicOutlierRejectionThreshold =
3; // max 3 pixel of average reprojection error
SmartProjectionParams params; SmartProjectionParams params;
params.setRankTolerance(1.0); params.setRankTolerance(1.0);
@ -640,12 +695,15 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_dynamicOutlie
graph.addPrior(x1, level_pose, noisePrior); graph.addPrior(x1, level_pose, noisePrior);
graph.addPrior(x2, pose_right, noisePrior); graph.addPrior(x2, pose_right, noisePrior);
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Pose3 noise_pose = Pose3(
Point3(0.01, 0.01, 0.01)); // smaller noise, otherwise outlier rejection will kick in Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
Point3(0.01, 0.01,
0.01)); // smaller noise, otherwise outlier rejection will kick in
Values values; Values values;
values.insert(x1, level_pose); values.insert(x1, level_pose);
values.insert(x2, pose_right); values.insert(x2, pose_right);
// initialize third pose with some noise, we expect it to move back to original pose_above // initialize third pose with some noise, we expect it to move back to
// original pose_above
values.insert(x3, pose_above * noise_pose); values.insert(x3, pose_above * noise_pose);
// Optimization should correct 3rd pose // Optimization should correct 3rd pose
@ -656,8 +714,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_dynamicOutlie
} }
/* *************************************************************************/ /* *************************************************************************/
TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRollingShutter) { TEST(SmartProjectionPoseFactorRollingShutter,
hessianComparedToProjFactorsRollingShutter) {
using namespace vanillaPoseRS; using namespace vanillaPoseRS;
Point2Vector measurements_lmk1; Point2Vector measurements_lmk1;
@ -683,10 +741,15 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRolli
Values values; Values values;
values.insert(x1, level_pose); values.insert(x1, level_pose);
values.insert(x2, pose_right); values.insert(x2, pose_right);
// initialize third pose with some noise to get a nontrivial linearization point // initialize third pose with some noise to get a nontrivial linearization
// point
values.insert(x3, pose_above * noise_pose); values.insert(x3, pose_above * noise_pose);
EXPECT( // check that the pose is actually noisy EXPECT( // check that the pose is actually noisy
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))); 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)));
// linearization point for the poses // linearization point for the poses
Pose3 pose1 = level_pose; Pose3 pose1 = level_pose;
@ -695,8 +758,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRolli
// ==== check Hessian of smartFactor1 ===== // ==== check Hessian of smartFactor1 =====
// -- compute actual Hessian // -- compute actual Hessian
boost::shared_ptr<GaussianFactor> linearfactor1 = smartFactor1->linearize( boost::shared_ptr<GaussianFactor> linearfactor1 =
values); smartFactor1->linearize(values);
Matrix actualHessian = linearfactor1->information(); Matrix actualHessian = linearfactor1->information();
// -- compute expected Hessian from manual Schur complement from Jacobians // -- compute expected Hessian from manual Schur complement from Jacobians
@ -714,46 +777,52 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRolli
ProjectionFactorRollingShutter factor11(measurements_lmk1[0], interp_factor1, ProjectionFactorRollingShutter factor11(measurements_lmk1[0], interp_factor1,
model, x1, x2, l0, sharedK); model, x1, x2, l0, sharedK);
Matrix H1Actual, H2Actual, H3Actual; Matrix H1Actual, H2Actual, H3Actual;
// note: b is minus the reprojection error, cf the smart factor jacobian computation // note: b is minus the reprojection error, cf the smart factor jacobian
b.segment<2>(0) = -factor11.evaluateError(pose1, pose2, *point, H1Actual, H2Actual, H3Actual); // computation
b.segment<2>(0) = -factor11.evaluateError(pose1, pose2, *point, H1Actual,
H2Actual, H3Actual);
F.block<2, 6>(0, 0) = H1Actual; F.block<2, 6>(0, 0) = H1Actual;
F.block<2, 6>(0, 6) = H2Actual; F.block<2, 6>(0, 6) = H2Actual;
E.block<2, 3>(0, 0) = H3Actual; E.block<2, 3>(0, 0) = H3Actual;
ProjectionFactorRollingShutter factor12(measurements_lmk1[1], interp_factor2, ProjectionFactorRollingShutter factor12(measurements_lmk1[1], interp_factor2,
model, x2, x3, l0, sharedK); model, x2, x3, l0, sharedK);
b.segment<2>(2) = -factor12.evaluateError(pose2, pose3, *point, H1Actual, H2Actual, H3Actual); b.segment<2>(2) = -factor12.evaluateError(pose2, pose3, *point, H1Actual,
H2Actual, H3Actual);
F.block<2, 6>(2, 6) = H1Actual; F.block<2, 6>(2, 6) = H1Actual;
F.block<2, 6>(2, 12) = H2Actual; F.block<2, 6>(2, 12) = H2Actual;
E.block<2, 3>(2, 0) = H3Actual; E.block<2, 3>(2, 0) = H3Actual;
ProjectionFactorRollingShutter factor13(measurements_lmk1[2], interp_factor3, ProjectionFactorRollingShutter factor13(measurements_lmk1[2], interp_factor3,
model, x3, x1, l0, sharedK); model, x3, x1, l0, sharedK);
b.segment<2>(4) = -factor13.evaluateError(pose3, pose1, *point, H1Actual, H2Actual, H3Actual); b.segment<2>(4) = -factor13.evaluateError(pose3, pose1, *point, H1Actual,
H2Actual, H3Actual);
F.block<2, 6>(4, 12) = H1Actual; F.block<2, 6>(4, 12) = H1Actual;
F.block<2, 6>(4, 0) = H2Actual; F.block<2, 6>(4, 0) = H2Actual;
E.block<2, 3>(4, 0) = H3Actual; E.block<2, 3>(4, 0) = H3Actual;
// whiten // whiten
F = (1/sigma) * F; F = (1 / sigma) * F;
E = (1/sigma) * E; E = (1 / sigma) * E;
b = (1/sigma) * b; b = (1 / sigma) * b;
//* G = F' * F - F' * E * P * E' * F //* G = F' * F - F' * E * P * E' * F
Matrix P = (E.transpose() * E).inverse(); Matrix P = (E.transpose() * E).inverse();
Matrix expectedHessian = F.transpose() * F Matrix expectedHessian =
- (F.transpose() * E * P * E.transpose() * F); F.transpose() * F - (F.transpose() * E * P * E.transpose() * F);
EXPECT(assert_equal(expectedHessian, actualHessian, 1e-6)); EXPECT(assert_equal(expectedHessian, actualHessian, 1e-6));
// ==== check Information vector of smartFactor1 ===== // ==== check Information vector of smartFactor1 =====
GaussianFactorGraph gfg; GaussianFactorGraph gfg;
gfg.add(linearfactor1); gfg.add(linearfactor1);
Matrix actualHessian_v2 = gfg.hessian().first; Matrix actualHessian_v2 = gfg.hessian().first;
EXPECT(assert_equal(actualHessian_v2, actualHessian, 1e-6)); // sanity check on hessian EXPECT(assert_equal(actualHessian_v2, actualHessian,
1e-6)); // sanity check on hessian
// -- compute actual information vector // -- compute actual information vector
Vector actualInfoVector = gfg.hessian().second; Vector actualInfoVector = gfg.hessian().second;
// -- compute expected information vector from manual Schur complement from Jacobians // -- compute expected information vector from manual Schur complement from
// Jacobians
//* g = F' * (b - E * P * E' * b) //* g = F' * (b - E * P * E' * b)
Vector expectedInfoVector = F.transpose() * (b - E * P * E.transpose() * b); Vector expectedInfoVector = F.transpose() * (b - E * P * E.transpose() * b);
EXPECT(assert_equal(expectedInfoVector, actualInfoVector, 1e-6)); EXPECT(assert_equal(expectedInfoVector, actualInfoVector, 1e-6));
@ -771,9 +840,11 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRolli
} }
/* *************************************************************************/ /* *************************************************************************/
TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRollingShutter_measurementsFromSamePose) { TEST(SmartProjectionPoseFactorRollingShutter,
// in this test we make sure the fact works even if we have multiple pixel measurements of the same landmark hessianComparedToProjFactorsRollingShutter_measurementsFromSamePose) {
// at a single pose, a setup that occurs in multi-camera systems // in this test we make sure the fact works even if we have multiple pixel
// measurements of the same landmark at a single pose, a setup that occurs in
// multi-camera systems
using namespace vanillaPoseRS; using namespace vanillaPoseRS;
Point2Vector measurements_lmk1; Point2Vector measurements_lmk1;
@ -783,7 +854,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRolli
// create redundant measurements: // create redundant measurements:
Camera::MeasurementVector measurements_lmk1_redundant = measurements_lmk1; Camera::MeasurementVector measurements_lmk1_redundant = measurements_lmk1;
measurements_lmk1_redundant.push_back(measurements_lmk1.at(0)); // we readd the first measurement measurements_lmk1_redundant.push_back(
measurements_lmk1.at(0)); // we readd the first measurement
// create inputs // create inputs
std::vector<std::pair<Key, Key>> key_pairs; std::vector<std::pair<Key, Key>> key_pairs;
@ -799,17 +871,23 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRolli
interp_factors.push_back(interp_factor1); interp_factors.push_back(interp_factor1);
SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model));
smartFactor1->add(measurements_lmk1_redundant, key_pairs, interp_factors, sharedK); smartFactor1->add(measurements_lmk1_redundant, key_pairs, interp_factors,
sharedK);
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
Point3(0.1, 0.1, 0.1)); // smaller noise Point3(0.1, 0.1, 0.1)); // smaller noise
Values values; Values values;
values.insert(x1, level_pose); values.insert(x1, level_pose);
values.insert(x2, pose_right); values.insert(x2, pose_right);
// initialize third pose with some noise to get a nontrivial linearization point // initialize third pose with some noise to get a nontrivial linearization
// point
values.insert(x3, pose_above * noise_pose); values.insert(x3, pose_above * noise_pose);
EXPECT( // check that the pose is actually noisy EXPECT( // check that the pose is actually noisy
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))); 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)));
// linearization point for the poses // linearization point for the poses
Pose3 pose1 = level_pose; Pose3 pose1 = level_pose;
@ -818,8 +896,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRolli
// ==== check Hessian of smartFactor1 ===== // ==== check Hessian of smartFactor1 =====
// -- compute actual Hessian // -- compute actual Hessian
boost::shared_ptr<GaussianFactor> linearfactor1 = smartFactor1->linearize( boost::shared_ptr<GaussianFactor> linearfactor1 =
values); smartFactor1->linearize(values);
Matrix actualHessian = linearfactor1->information(); Matrix actualHessian = linearfactor1->information();
// -- compute expected Hessian from manual Schur complement from Jacobians // -- compute expected Hessian from manual Schur complement from Jacobians
@ -828,62 +906,74 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRolli
TriangulationResult point = smartFactor1->point(); TriangulationResult point = smartFactor1->point();
EXPECT(point.valid()); // check triangulated point is valid EXPECT(point.valid()); // check triangulated point is valid
// Use standard ProjectionFactorRollingShutter factor to calculate the Jacobians // Use standard ProjectionFactorRollingShutter factor to calculate the
// Jacobians
Matrix F = Matrix::Zero(2 * 4, 6 * 3); Matrix F = Matrix::Zero(2 * 4, 6 * 3);
Matrix E = Matrix::Zero(2 * 4, 3); Matrix E = Matrix::Zero(2 * 4, 3);
Vector b = Vector::Zero(8); Vector b = Vector::Zero(8);
// create projection factors rolling shutter // create projection factors rolling shutter
ProjectionFactorRollingShutter factor11(measurements_lmk1_redundant[0], interp_factor1, ProjectionFactorRollingShutter factor11(measurements_lmk1_redundant[0],
model, x1, x2, l0, sharedK); interp_factor1, model, x1, x2, l0,
sharedK);
Matrix H1Actual, H2Actual, H3Actual; Matrix H1Actual, H2Actual, H3Actual;
// note: b is minus the reprojection error, cf the smart factor jacobian computation // note: b is minus the reprojection error, cf the smart factor jacobian
b.segment<2>(0) = -factor11.evaluateError(pose1, pose2, *point, H1Actual, H2Actual, H3Actual); // computation
b.segment<2>(0) = -factor11.evaluateError(pose1, pose2, *point, H1Actual,
H2Actual, H3Actual);
F.block<2, 6>(0, 0) = H1Actual; F.block<2, 6>(0, 0) = H1Actual;
F.block<2, 6>(0, 6) = H2Actual; F.block<2, 6>(0, 6) = H2Actual;
E.block<2, 3>(0, 0) = H3Actual; E.block<2, 3>(0, 0) = H3Actual;
ProjectionFactorRollingShutter factor12(measurements_lmk1_redundant[1], interp_factor2, ProjectionFactorRollingShutter factor12(measurements_lmk1_redundant[1],
model, x2, x3, l0, sharedK); interp_factor2, model, x2, x3, l0,
b.segment<2>(2) = -factor12.evaluateError(pose2, pose3, *point, H1Actual, H2Actual, H3Actual); sharedK);
b.segment<2>(2) = -factor12.evaluateError(pose2, pose3, *point, H1Actual,
H2Actual, H3Actual);
F.block<2, 6>(2, 6) = H1Actual; F.block<2, 6>(2, 6) = H1Actual;
F.block<2, 6>(2, 12) = H2Actual; F.block<2, 6>(2, 12) = H2Actual;
E.block<2, 3>(2, 0) = H3Actual; E.block<2, 3>(2, 0) = H3Actual;
ProjectionFactorRollingShutter factor13(measurements_lmk1_redundant[2], interp_factor3, ProjectionFactorRollingShutter factor13(measurements_lmk1_redundant[2],
model, x3, x1, l0, sharedK); interp_factor3, model, x3, x1, l0,
b.segment<2>(4) = -factor13.evaluateError(pose3, pose1, *point, H1Actual, H2Actual, H3Actual); sharedK);
b.segment<2>(4) = -factor13.evaluateError(pose3, pose1, *point, H1Actual,
H2Actual, H3Actual);
F.block<2, 6>(4, 12) = H1Actual; F.block<2, 6>(4, 12) = H1Actual;
F.block<2, 6>(4, 0) = H2Actual; F.block<2, 6>(4, 0) = H2Actual;
E.block<2, 3>(4, 0) = H3Actual; E.block<2, 3>(4, 0) = H3Actual;
ProjectionFactorRollingShutter factor14(measurements_lmk1_redundant[3], interp_factor1, ProjectionFactorRollingShutter factor14(measurements_lmk1_redundant[3],
model, x1, x2, l0, sharedK); interp_factor1, model, x1, x2, l0,
b.segment<2>(6) = -factor11.evaluateError(pose1, pose2, *point, H1Actual, H2Actual, H3Actual); sharedK);
b.segment<2>(6) = -factor11.evaluateError(pose1, pose2, *point, H1Actual,
H2Actual, H3Actual);
F.block<2, 6>(6, 0) = H1Actual; F.block<2, 6>(6, 0) = H1Actual;
F.block<2, 6>(6, 6) = H2Actual; F.block<2, 6>(6, 6) = H2Actual;
E.block<2, 3>(6, 0) = H3Actual; E.block<2, 3>(6, 0) = H3Actual;
// whiten // whiten
F = (1/sigma) * F; F = (1 / sigma) * F;
E = (1/sigma) * E; E = (1 / sigma) * E;
b = (1/sigma) * b; b = (1 / sigma) * b;
//* G = F' * F - F' * E * P * E' * F //* G = F' * F - F' * E * P * E' * F
Matrix P = (E.transpose() * E).inverse(); Matrix P = (E.transpose() * E).inverse();
Matrix expectedHessian = F.transpose() * F Matrix expectedHessian =
- (F.transpose() * E * P * E.transpose() * F); F.transpose() * F - (F.transpose() * E * P * E.transpose() * F);
EXPECT(assert_equal(expectedHessian, actualHessian, 1e-6)); EXPECT(assert_equal(expectedHessian, actualHessian, 1e-6));
// ==== check Information vector of smartFactor1 ===== // ==== check Information vector of smartFactor1 =====
GaussianFactorGraph gfg; GaussianFactorGraph gfg;
gfg.add(linearfactor1); gfg.add(linearfactor1);
Matrix actualHessian_v2 = gfg.hessian().first; Matrix actualHessian_v2 = gfg.hessian().first;
EXPECT(assert_equal(actualHessian_v2, actualHessian, 1e-6)); // sanity check on hessian EXPECT(assert_equal(actualHessian_v2, actualHessian,
1e-6)); // sanity check on hessian
// -- compute actual information vector // -- compute actual information vector
Vector actualInfoVector = gfg.hessian().second; Vector actualInfoVector = gfg.hessian().second;
// -- compute expected information vector from manual Schur complement from Jacobians // -- compute expected information vector from manual Schur complement from
// Jacobians
//* g = F' * (b - E * P * E' * b) //* g = F' * (b - E * P * E' * b)
Vector expectedInfoVector = F.transpose() * (b - E * P * E.transpose() * b); Vector expectedInfoVector = F.transpose() * (b - E * P * E.transpose() * b);
EXPECT(assert_equal(expectedInfoVector, actualInfoVector, 1e-6)); EXPECT(assert_equal(expectedInfoVector, actualInfoVector, 1e-6));
@ -902,8 +992,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRolli
} }
/* *************************************************************************/ /* *************************************************************************/
TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_measurementsFromSamePose ) { TEST(SmartProjectionPoseFactorRollingShutter,
optimization_3poses_measurementsFromSamePose) {
using namespace vanillaPoseRS; using namespace vanillaPoseRS;
Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3;
@ -913,27 +1003,32 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_measurementsF
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3);
// create inputs // create inputs
std::vector<std::pair<Key,Key>> key_pairs; std::vector<std::pair<Key, Key>> key_pairs;
key_pairs.push_back(std::make_pair(x1,x2)); key_pairs.push_back(std::make_pair(x1, x2));
key_pairs.push_back(std::make_pair(x2,x3)); key_pairs.push_back(std::make_pair(x2, x3));
key_pairs.push_back(std::make_pair(x3,x1)); key_pairs.push_back(std::make_pair(x3, x1));
std::vector<double> interp_factors; std::vector<double> interp_factors;
interp_factors.push_back(interp_factor1); interp_factors.push_back(interp_factor1);
interp_factors.push_back(interp_factor2); interp_factors.push_back(interp_factor2);
interp_factors.push_back(interp_factor3); interp_factors.push_back(interp_factor3);
// For first factor, we create redundant measurement (taken by the same keys as factor 1, to // For first factor, we create redundant measurement (taken by the same keys
// make sure the redundancy in the keys does not create problems) // as factor 1, to make sure the redundancy in the keys does not create
// problems)
Camera::MeasurementVector& measurements_lmk1_redundant = measurements_lmk1; Camera::MeasurementVector& measurements_lmk1_redundant = measurements_lmk1;
measurements_lmk1_redundant.push_back(measurements_lmk1.at(0)); // we readd the first measurement measurements_lmk1_redundant.push_back(
std::vector<std::pair<Key,Key>> key_pairs_redundant = key_pairs; measurements_lmk1.at(0)); // we readd the first measurement
key_pairs_redundant.push_back(key_pairs.at(0)); // we readd the first pair of keys std::vector<std::pair<Key, Key>> key_pairs_redundant = key_pairs;
key_pairs_redundant.push_back(
key_pairs.at(0)); // we readd the first pair of keys
std::vector<double> interp_factors_redundant = interp_factors; std::vector<double> interp_factors_redundant = interp_factors;
interp_factors_redundant.push_back(interp_factors.at(0));// we readd the first interp factor interp_factors_redundant.push_back(
interp_factors.at(0)); // we readd the first interp factor
SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model));
smartFactor1->add(measurements_lmk1_redundant, key_pairs_redundant, interp_factors_redundant, sharedK); smartFactor1->add(measurements_lmk1_redundant, key_pairs_redundant,
interp_factors_redundant, sharedK);
SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model)); SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model));
smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, sharedK); smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, sharedK);
@ -956,20 +1051,22 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_measurementsF
groundTruth.insert(x3, pose_above); groundTruth.insert(x3, pose_above);
DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9); DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9);
// 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/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), Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
Point3(0.1, 0.1, 0.1)); // smaller noise Point3(0.1, 0.1, 0.1)); // smaller noise
Values values; Values values;
values.insert(x1, level_pose); values.insert(x1, level_pose);
values.insert(x2, pose_right); values.insert(x2, pose_right);
// initialize third pose with some noise, we expect it to move back to original pose_above // initialize third pose with some noise, we expect it to move back to
// original pose_above
values.insert(x3, pose_above * noise_pose); values.insert(x3, pose_above * noise_pose);
EXPECT( // check that the pose is actually noisy EXPECT( // check that the pose is actually noisy
assert_equal( assert_equal(Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656,
Pose3( -0.0313952598, -0.000986635786, 0.0314107591,
Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, -0.999013364, -0.0313952598),
-0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), Point3(0.1, -0.1, 1.9)),
Point3(0.1, -0.1, 1.9)), values.at<Pose3>(x3))); values.at<Pose3>(x3)));
Values result; Values result;
LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
@ -980,11 +1077,11 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_measurementsF
#ifndef DISABLE_TIMING #ifndef DISABLE_TIMING
#include <gtsam/base/timing.h> #include <gtsam/base/timing.h>
// -Total: 0 CPU (0 times, 0 wall, 0.04 children, min: 0 max: 0) // -Total: 0 CPU (0 times, 0 wall, 0.04 children, min: 0 max: 0)
//| -SF RS LINEARIZE: 0.02 CPU (1000 times, 0.017244 wall, 0.02 children, min: 0 max: 0) //| -SF RS LINEARIZE: 0.02 CPU (1000 times, 0.017244 wall, 0.02 children, min:
//| -RS LINEARIZE: 0.02 CPU (1000 times, 0.009035 wall, 0.02 children, min: 0 max: 0) // 0 max: 0) | -RS LINEARIZE: 0.02 CPU (1000 times, 0.009035 wall, 0.02
// children, min: 0 max: 0)
/* *************************************************************************/ /* *************************************************************************/
TEST( SmartProjectionPoseFactorRollingShutter, timing ) { TEST(SmartProjectionPoseFactorRollingShutter, timing) {
using namespace vanillaPose; using namespace vanillaPose;
// Default cameras for simple derivatives // Default cameras for simple derivatives
@ -1007,14 +1104,14 @@ TEST( SmartProjectionPoseFactorRollingShutter, timing ) {
size_t nrTests = 1000; size_t nrTests = 1000;
for(size_t i = 0; i<nrTests; i++){ for (size_t i = 0; i < nrTests; i++) {
SmartFactorRS::shared_ptr smartFactorRS(new SmartFactorRS(model)); SmartFactorRS::shared_ptr smartFactorRS(new SmartFactorRS(model));
double interp_factor = 0; // equivalent to measurement taken at pose 1 double interp_factor = 0; // equivalent to measurement taken at pose 1
smartFactorRS->add(measurements_lmk1[0], x1, x2, interp_factor, sharedKSimple, smartFactorRS->add(measurements_lmk1[0], x1, x2, interp_factor,
body_P_sensorId); sharedKSimple, body_P_sensorId);
interp_factor = 1; // equivalent to measurement taken at pose 2 interp_factor = 1; // equivalent to measurement taken at pose 2
smartFactorRS->add(measurements_lmk1[1], x1, x2, interp_factor, sharedKSimple, smartFactorRS->add(measurements_lmk1[1], x1, x2, interp_factor,
body_P_sensorId); sharedKSimple, body_P_sensorId);
Values values; Values values;
values.insert(x1, pose1); values.insert(x1, pose1);
@ -1024,7 +1121,7 @@ TEST( SmartProjectionPoseFactorRollingShutter, timing ) {
gttoc_(SF_RS_LINEARIZE); gttoc_(SF_RS_LINEARIZE);
} }
for(size_t i = 0; i<nrTests; i++){ for (size_t i = 0; i < nrTests; i++) {
SmartFactor::shared_ptr smartFactor(new SmartFactor(model, sharedKSimple)); SmartFactor::shared_ptr smartFactor(new SmartFactor(model, sharedKSimple));
smartFactor->add(measurements_lmk1[0], x1); smartFactor->add(measurements_lmk1[0], x1);
smartFactor->add(measurements_lmk1[1], x2); smartFactor->add(measurements_lmk1[1], x2);
@ -1046,4 +1143,3 @@ int main() {
return TestRegistry::runAllTests(tr); return TestRegistry::runAllTests(tr);
} }
/* ************************************************************************* */ /* ************************************************************************* */