rolling shutter projection factor

release/4.3a0
yotams 2021-04-22 08:52:28 +03:00
parent 301f7fe1a3
commit d4eaa15280
4 changed files with 250 additions and 4 deletions

View File

@ -319,11 +319,30 @@ T expm(const Vector& x, int K=7) {
}
/**
* Linear interpolation between X and Y by coefficient t in [0, 1].
* Linear interpolation and some extrapolation between X and Y by coefficient t in [0, 1.5], optinal jacobians calculations.
*/
template <typename T>
T interpolate(const T& X, const T& Y, double t) {
assert(t >= 0 && t <= 1);
template <typename T, int N>
T interpolate(const T& X, const T& Y, double t, OptionalJacobian<N, N> Hx = boost::none, OptionalJacobian<N, N> Hy = boost::none) {
assert(t >= 0 && t <= 1.5);
if (Hx && Hy) {
typedef Eigen::Matrix<double, N, N> Jacobian;
typename traits<T>::TangentVector tres;
T tres1;
Jacobian d1;
Jacobian d2;
tres1 = traits<T>::Between(X, Y, Hx, Hy);
tres = traits<T>::Logmap(tres1, d1);
*Hx = d1 * (*Hx);
*Hy = d1 * (*Hy);
tres1 = traits<T>::Expmap(t * tres, d1);
*Hx = t * d1 * (*Hx);
*Hy = t * d1 * (*Hy);
tres1 = traits<T>::Compose(X, tres1, d1, d2);
*Hx = d1 + d2 * (*Hx);
*Hy = d2 * (*Hy);
return tres1;
}
return traits<T>::Compose(X, traits<T>::Expmap(t * traits<T>::Logmap(traits<T>::Between(X, Y))));
}

View File

@ -423,4 +423,8 @@ std::ostream &operator<<(std::ostream &os, const Pose3& pose) {
return os;
}
Pose3 pose3_interp(const Pose3& X, const Pose3& Y, double t, Matrix& Hx, Matrix& Hy) {
return X.interp(t, Y, Hx, Hy);
}
} // namespace gtsam

View File

@ -353,6 +353,15 @@ public:
return std::make_pair(0, 2);
}
/**
* @brief Spherical Linear interpolation between *this and other
* @param s a value between 0 and 1.5
* @param other final point of iterpolation geodesic on manifold
* @param Hx jacobian of the interpolation on this
& @param Hy jacobian of the interpolation on other
*/
Pose3 interp(double t, const Pose3& other, OptionalJacobian<6, 6> Hx = boost::none, OptionalJacobian<6, 6> Hy = boost::none) const;
/// Output stream operator
GTSAM_EXPORT
friend std::ostream &operator<<(std::ostream &os, const Pose3& p);

View File

@ -0,0 +1,214 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file RollingShutterProjectionFactor.h
* @brief Basic bearing factor from 2D measurement for rolling shutter cameras
* @author Yotam Stern
*/
#pragma once
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/geometry/SimpleCamera.h>
#include <boost/optional.hpp>
namespace gtsam {
/**
* Non-linear factor for a constraint derived from a 2D measurement. The calibration is known here.
* i.e. the main building block for visual SLAM.
* this version takes rolling shutter information into account like so: consider camera A (pose A) and camera B, and Point2 from camera A.
* camera A has timestamp t_A for the exposure time of its first row, and so does camera B t_B, Point2 has timestamp t_p according to the timestamp
* corresponding to the time of exposure of the row in the camera it belongs to.
* let us define the interp_param = (t_p - t_A) / (t_B - t_A), we will use the pose interpolated between A and B by the interp_param to project
* the corresponding landmark to Point2.
* @addtogroup SLAM
*/
class RollingShutterProjectionFactor: public NoiseModelFactor3<Pose3, Pose3, Point3> {
protected:
// Keep a copy of measurement and calibration for I/O
Point2 measured_; ///< 2D measurement
double interp_param_; ///< interpolation parameter corresponding to the point2 measured
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
// verbosity handling for Cheirality Exceptions
bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false)
bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions (default: false)
public:
/// shorthand for base class type
typedef NoiseModelFactor3<Pose3, Pose3, Point3> Base;
/// shorthand for this class
typedef RollingShutterProjectionFactor This;
/// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<This> shared_ptr;
/// Default constructor
RollingShutterProjectionFactor() :
measured_(0, 0), interp_param_(0), throwCheirality_(false), verboseCheirality_(false) {
}
/**
* Constructor
* @param measured is the 2 dimensional location of point in image (the measurement)
* @param interp_param is the rolling shutter parameter for the measurement
* @param model is the standard deviation
* @param poseKey_a is the index of the first camera
* @param poseKey_b is the index of the second camera
* @param pointKey is the index of the landmark
* @param K shared pointer to the constant calibration
* @param body_P_sensor is the transform from body to sensor frame (default identity)
*/
RollingShutterProjectionFactor(const Point2& measured, double interp_param, const SharedNoiseModel& model,
Key poseKey_a, Key poseKey_b, Key pointKey, const boost::shared_ptr<Cal3_S2>& K,
boost::optional<Pose3> body_P_sensor = boost::none) :
Base(model, poseKey_a, poseKey_b, pointKey), measured_(measured), interp_param_(interp_param), K_(K), body_P_sensor_(body_P_sensor),
throwCheirality_(false), verboseCheirality_(false) {}
/**
* Constructor with exception-handling flags
* @param measured is the 2 dimensional location of point in image (the measurement)
* @param interp_param is the rolling shutter parameter for the measurement
* @param model is the standard deviation
* @param poseKey_a is the index of the first camera
* @param poseKey_b is the index of the second camera
* @param pointKey is the index of the landmark
* @param K shared pointer to the constant calibration
* @param throwCheirality determines whether Cheirality exceptions are rethrown
* @param verboseCheirality determines whether exceptions are printed for Cheirality
* @param body_P_sensor is the transform from body to sensor frame (default identity)
*/
RollingShutterProjectionFactor(const Point2& measured, double interp_param, const SharedNoiseModel& model,
Key poseKey_a, Key poseKey_b, Key pointKey, const boost::shared_ptr<Cal3_S2>& K,
bool throwCheirality, bool verboseCheirality,
boost::optional<Pose3> body_P_sensor = boost::none) :
Base(model, poseKey_a, poseKey_b, pointKey), measured_(measured), interp_param_(interp_param), K_(K), body_P_sensor_(body_P_sensor),
throwCheirality_(throwCheirality), verboseCheirality_(verboseCheirality) {}
/** Virtual destructor */
virtual ~RollingShutterProjectionFactor() {}
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/**
* print
* @param s optional string naming the factor
* @param keyFormatter optional formatter useful for printing Symbols
*/
void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
std::cout << s << "RollingShutterProjectionFactor, z = ";
traits<Point2>::Print(measured_);
std::cout << " rolling shutter interpolation param = " << interp_param_;
if(this->body_P_sensor_)
this->body_P_sensor_->print(" sensor pose in body frame: ");
Base::print("", keyFormatter);
}
/// equals
virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
const This *e = dynamic_cast<const This*>(&p);
return e
&& Base::equals(p, tol)
&& (interp_param_ == e->interp_param())
&& traits<Point2>::Equals(this->measured_, e->measured_, tol)
&& this->K_->equals(*e->K_, tol)
&& ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_)));
}
/// Evaluate error h(x)-z and optionally derivatives
Vector evaluateError(const Pose3& pose_a, const Pose3& pose_b, const Point3& point,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H3 = boost::none) const {
Pose3 pose;
gtsam::Matrix Hprj;
//pose = interpolate(pose_a, pose_b, interp_param_, H1, H2);
pose = pose_a.interp(interp_param_, pose_b, H1, H2);
try {
if(body_P_sensor_) {
if(H1 && H2) {
gtsam::Matrix H0;
PinholeCamera<Cal3_S2> camera(pose.compose(*body_P_sensor_, H0), *K_);
Point2 reprojectionError(camera.project(point, Hprj, H3, boost::none) - measured_);
*H1 = Hprj * H0 * (*H1);
*H2 = Hprj * H0 * (*H2);
return reprojectionError;
} else {
PinholeCamera<Cal3_S2> camera(pose.compose(*body_P_sensor_), *K_);
return camera.project(point, Hprj, H3, boost::none) - measured_;
}
} else {
PinholeCamera<Cal3_S2> camera(pose, *K_);
Point2 reprojectionError(camera.project(point, Hprj, H3, boost::none) - measured_);
if (H1) *H1 = Hprj * (*H1);
if (H2) *H2 = Hprj * (*H2);
return reprojectionError;
}
} catch( CheiralityException& e) {
if (H1) *H1 = Matrix::Zero(2,6);
if (H2) *H2 = Matrix::Zero(2,6);
if (H3) *H3 = Matrix::Zero(2,3);
if (verboseCheirality_)
std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) <<
" moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl;
if (throwCheirality_)
throw CheiralityException(this->key2());
}
return Vector2::Constant(2.0 * K_->fx());
}
/** return the measurement */
const Point2& measured() const {
return measured_;
}
/** return the calibration object */
inline const boost::shared_ptr<Cal3_S2> calibration() const {
return K_;
}
/** returns the rolling shutter interp param*/
inline double interp_param() const {return interp_param_; }
/** return verbosity */
inline bool verboseCheirality() const { return verboseCheirality_; }
/** return flag for throwing cheirality exceptions */
inline bool throwCheirality() const { return throwCheirality_; }
private:
/// Serialization function
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar & BOOST_SERIALIZATION_NVP(measured_);
ar & BOOST_SERIALIZATION_NVP(interp_param_);
ar & BOOST_SERIALIZATION_NVP(K_);
ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
ar & BOOST_SERIALIZATION_NVP(throwCheirality_);
ar & BOOST_SERIALIZATION_NVP(verboseCheirality_);
}
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
}; // rolling shutter projection factor
} //namespace gtsam