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

renamed interp param to alpha, improved comments, added cpp

release/4.3a0
lcarlone 2021-08-13 21:42:09 -04:00
parent a3ee695b85
commit 0a8ebdf8cd
4 changed files with 112 additions and 84 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
gtsam/base/Lie.h
View File

@ -320,13 +320,14 @@ T expm(const Vector& x, int K=7) {
}
/**
* Linear interpolation between X and Y by coefficient t in [0, 1.5] (t>1 implies extrapolation), with optional jacobians.
* Linear interpolation between X and Y by coefficient t (typically t \in [0,1],
* but can also be used to extrapolate before pose X or after pose Y), with optional jacobians.
*/
template <typename T>
T interpolate(const T& X, const T& Y, double t,
typename MakeOptionalJacobian<T, T>::type Hx = boost::none,
typename MakeOptionalJacobian<T, T>::type Hy = boost::none) {
assert(t >= 0.0 && t <= 1.5);
if (Hx || Hy) {
typename traits<T>::TangentVector log_Xinv_Y;
typename MakeJacobian<T, T>::type between_H_x, log_H, exp_H, compose_H_x;

73
gtsam_unstable/slam/ProjectionFactorRollingShutter.cpp Normal file
View File

@ -0,0 +1,73 @@
/* ----------------------------------------------------------------------------
* 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 ProjectionFactorRollingShutter.cpp
* @brief Basic projection factor for rolling shutter cameras
* @author Yotam Stern
*/
#include <gtsam_unstable/slam/ProjectionFactorRollingShutter.h>
namespace gtsam {
Vector ProjectionFactorRollingShutter::evaluateError(
const Pose3& pose_a, const Pose3& pose_b, const Point3& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2,
boost::optional<Matrix&> H3) const {
try {
Pose3 pose = interpolate<Pose3>(pose_a, pose_b, alpha_, H1, H2);
gtsam::Matrix Hprj;
if (body_P_sensor_) {
if (H1 || H2 || H3) {
gtsam::Matrix HbodySensor;
PinholeCamera<Cal3_S2> camera(
pose.compose(*body_P_sensor_, HbodySensor), *K_);
Point2 reprojectionError(
camera.project(point, Hprj, H3, boost::none) - measured_);
if (H1)
*H1 = Hprj * HbodySensor * (*H1);
if (H2)
*H2 = Hprj * HbodySensor * (*H2);
return reprojectionError;
} else {
PinholeCamera<Cal3_S2> camera(pose.compose(*body_P_sensor_), *K_);
return camera.project(point) - 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());
}
} //namespace gtsam

78
gtsam_unstable/slam/ProjectionFactorRollingShutter.h
View File

@ -31,8 +31,8 @@ namespace gtsam {
* and a Point2 measurement taken starting at time A using a rolling shutter camera.
* 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)
* corresponding to the time of exposure of the row of the image the pixel 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.
* 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
*/
@ -42,7 +42,7 @@ class ProjectionFactorRollingShutter : public NoiseModelFactor3<Pose3, Pose3,
// Keep a copy of measurement and calibration for I/O
Point2 measured_; ///< 2D measurement
double interp_param_; ///< 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::optional<Pose3> body_P_sensor_; ///< The pose of the sensor in the body frame
@ -64,7 +64,7 @@ class ProjectionFactorRollingShutter : public NoiseModelFactor3<Pose3, Pose3,
/// Default constructor
ProjectionFactorRollingShutter()
: measured_(0, 0),
interp_param_(0),
alpha_(0),
throwCheirality_(false),
verboseCheirality_(false) {
}
@ -72,7 +72,7 @@ class ProjectionFactorRollingShutter : public NoiseModelFactor3<Pose3, Pose3,
/**
* Constructor
* @param measured is the 2-dimensional pixel location of point in the image (the measurement)
* @param interp_param 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 poseKey_a is the key of the first camera
* @param poseKey_b is the key of the second camera
@ -80,7 +80,7 @@ class ProjectionFactorRollingShutter : public NoiseModelFactor3<Pose3, Pose3,
* @param K shared pointer to the constant calibration
* @param body_P_sensor is the transform from body to sensor frame (default identity)
*/
ProjectionFactorRollingShutter(const Point2& measured, double interp_param,
ProjectionFactorRollingShutter(const Point2& measured, double alpha,
const SharedNoiseModel& model, Key poseKey_a,
Key poseKey_b, Key pointKey,
const boost::shared_ptr<Cal3_S2>& K,
@ -88,7 +88,7 @@ class ProjectionFactorRollingShutter : public NoiseModelFactor3<Pose3, Pose3,
boost::none)
: Base(model, poseKey_a, poseKey_b, pointKey),
measured_(measured),
interp_param_(interp_param),
alpha_(alpha),
K_(K),
body_P_sensor_(body_P_sensor),
throwCheirality_(false),
@ -98,7 +98,7 @@ class ProjectionFactorRollingShutter : public NoiseModelFactor3<Pose3, Pose3,
/**
* Constructor with exception-handling flags
* @param measured is the 2-dimensional pixel location of point in the image (the measurement)
* @param interp_param 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 poseKey_a is the key of the first camera
* @param poseKey_b is the key of the second camera
@ -108,7 +108,7 @@ class ProjectionFactorRollingShutter : public NoiseModelFactor3<Pose3, Pose3,
* @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 interp_param,
ProjectionFactorRollingShutter(const Point2& measured, double alpha,
const SharedNoiseModel& model, Key poseKey_a,
Key poseKey_b, Key pointKey,
const boost::shared_ptr<Cal3_S2>& K,
@ -117,7 +117,7 @@ class ProjectionFactorRollingShutter : public NoiseModelFactor3<Pose3, Pose3,
boost::none)
: Base(model, poseKey_a, poseKey_b, pointKey),
measured_(measured),
interp_param_(interp_param),
alpha_(alpha),
K_(K),
body_P_sensor_(body_P_sensor),
throwCheirality_(throwCheirality),
@ -143,7 +143,7 @@ class ProjectionFactorRollingShutter : public NoiseModelFactor3<Pose3, Pose3,
DefaultKeyFormatter) const {
std::cout << s << "ProjectionFactorRollingShutter, z = ";
traits<Point2>::Print(measured_);
std::cout << " rolling shutter interpolation param = " << interp_param_;
std::cout << " rolling shutter interpolation param = " << alpha_;
if (this->body_P_sensor_)
this->body_P_sensor_->print(" sensor pose in body frame: ");
Base::print("", keyFormatter);
@ -152,7 +152,7 @@ class ProjectionFactorRollingShutter : public NoiseModelFactor3<Pose3, Pose3,
/// 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())
return e && Base::equals(p, tol) && (alpha_ == e->alpha())
&& traits<Point2>::Equals(this->measured_, e->measured_, tol)
&& this->K_->equals(*e->K_, tol)
&& (this->throwCheirality_ == e->throwCheirality_)
@ -167,53 +167,7 @@ class ProjectionFactorRollingShutter : public NoiseModelFactor3<Pose3, Pose3,
const Point3& point, boost::optional<Matrix&> H1 =
boost::none,
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const {
try {
Pose3 pose = interpolate<Pose3>(pose_a, pose_b, interp_param_, H1, H2);
gtsam::Matrix Hprj;
if (body_P_sensor_) {
if (H1 || H2 || H3) {
gtsam::Matrix HbodySensor;
PinholeCamera<Cal3_S2> camera(
pose.compose(*body_P_sensor_, HbodySensor), *K_);
Point2 reprojectionError(
camera.project(point, Hprj, H3, boost::none) - measured_);
if (H1)
*H1 = Hprj * HbodySensor * (*H1);
if (H2)
*H2 = Hprj * HbodySensor * (*H2);
return reprojectionError;
} else {
PinholeCamera<Cal3_S2> camera(pose.compose(*body_P_sensor_), *K_);
return camera.project(point) - 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());
}
boost::optional<Matrix&> H3 = boost::none) const;
/** return the measurement */
const Point2& measured() const {
@ -226,8 +180,8 @@ class ProjectionFactorRollingShutter : public NoiseModelFactor3<Pose3, Pose3,
}
/** returns the rolling shutter interp param*/
inline double interp_param() const {
return interp_param_;
inline double alpha() const {
return alpha_;
}
/** return verbosity */
@ -248,7 +202,7 @@ class ProjectionFactorRollingShutter : public NoiseModelFactor3<Pose3, Pose3,
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(alpha_);
ar & BOOST_SERIALIZATION_NVP(K_);
ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
ar & BOOST_SERIALIZATION_NVP(throwCheirality_);

40
gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h
View File

@ -49,7 +49,7 @@ PinholePose<CALIBRATION> > {
std::vector<std::pair<Key, Key>> world_P_body_key_pairs_;
/// interpolation factor (one for each observation) to interpolate between pair of consecutive poses
std::vector<double> interp_params_;
std::vector<double> alphas_;
/// Pose of the camera in the body frame
std::vector<Pose3> body_P_sensors_;
@ -92,12 +92,12 @@ PinholePose<CALIBRATION> > {
* single landmark in a single view (the measurement), interpolated from the 2 poses
* @param world_P_body_key1 key corresponding to the first body poses (time <= time pixel is acquired)
* @param world_P_body_key2 key corresponding to the second body poses (time >= time pixel is acquired)
* @param interp_param interpolation factor in [0,1], such that if interp_param = 0 the interpolated pose is the same as world_P_body_key1
* @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 body_P_sensor (fixed) camera extrinsic calibration
*/
void add(const Point2& measured, const Key& world_P_body_key1,
const Key& world_P_body_key2, const double& interp_param,
const Key& world_P_body_key2, const double& alpha,
const boost::shared_ptr<CALIBRATION>& K, const Pose3 body_P_sensor = Pose3::identity()) {
// store measurements in base class
this->measured_.push_back(measured);
@ -113,7 +113,7 @@ PinholePose<CALIBRATION> > {
this->keys_.push_back(world_P_body_key2); // add only unique keys
// store interpolation factor
interp_params_.push_back(interp_param);
alphas_.push_back(alpha);
// store fixed intrinsic calibration
K_all_.push_back(K);
@ -128,21 +128,21 @@ PinholePose<CALIBRATION> > {
* 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
* to the pair of poses from which the observation pose for the i0-th measurement can be interpolated
* @param interp_params vector of interpolation params, one for each measurement (in the same order)
* @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 body_P_sensors vector of (fixed) extrinsic calibration objects
*/
void add(const Point2Vector& measurements,
const std::vector<std::pair<Key, Key>>& world_P_body_key_pairs,
const std::vector<double>& interp_params,
const std::vector<double>& alphas,
const std::vector<boost::shared_ptr<CALIBRATION>>& Ks,
const std::vector<Pose3> body_P_sensors) {
assert(world_P_body_key_pairs.size() == measurements.size());
assert(world_P_body_key_pairs.size() == interp_params.size());
assert(world_P_body_key_pairs.size() == alphas.size());
assert(world_P_body_key_pairs.size() == Ks.size());
for (size_t i = 0; i < measurements.size(); i++) {
add(measurements[i], world_P_body_key_pairs[i].first,
world_P_body_key_pairs[i].second, interp_params[i], Ks[i],
world_P_body_key_pairs[i].second, alphas[i], Ks[i],
body_P_sensors[i]);
}
}
@ -154,19 +154,19 @@ PinholePose<CALIBRATION> > {
* 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
* to the pair of poses from which the observation pose for the i0-th measurement can be interpolated
* @param interp_params vector of interpolation params, one for each measurement (in the same order)
* @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 body_P_sensor (fixed) camera extrinsic calibration (same for all measurements)
*/
void add(const Point2Vector& measurements,
const std::vector<std::pair<Key, Key>>& world_P_body_key_pairs,
const std::vector<double>& interp_params,
const std::vector<double>& alphas,
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() == interp_params.size());
assert(world_P_body_key_pairs.size() == alphas.size());
for (size_t i = 0; i < measurements.size(); i++) {
add(measurements[i], world_P_body_key_pairs[i].first,
world_P_body_key_pairs[i].second, interp_params[i], K, body_P_sensor);
world_P_body_key_pairs[i].second, alphas[i], K, body_P_sensor);
}
}
@ -180,9 +180,9 @@ PinholePose<CALIBRATION> > {
return world_P_body_key_pairs_;
}
/// return the interpolation factors interp_params
const std::vector<double> interp_params() const {
return interp_params_;
/// return the interpolation factors alphas
const std::vector<double> alphas() const {
return alphas_;
}
/// return the extrinsic camera calibration body_P_sensors
@ -204,7 +204,7 @@ PinholePose<CALIBRATION> > {
<< keyFormatter(world_P_body_key_pairs_[i].first) << std::endl;
std::cout << " pose2 key: "
<< keyFormatter(world_P_body_key_pairs_[i].second) << std::endl;
std::cout << " interp_param: " << interp_params_[i] << std::endl;
std::cout << " alpha: " << alphas_[i] << std::endl;
body_P_sensors_[i].print("extrinsic calibration:\n");
K_all_[i]->print("intrinsic calibration = ");
}
@ -239,7 +239,7 @@ PinholePose<CALIBRATION> > {
}else{ extrinsicCalibrationEqual = false; }
return e && Base::equals(p, tol) && K_all_ == e->calibration()
&& interp_params_ == e->interp_params() && keyPairsEqual && extrinsicCalibrationEqual;
&& alphas_ == e->alphas() && keyPairsEqual && extrinsicCalibrationEqual;
}
/**
@ -267,7 +267,7 @@ PinholePose<CALIBRATION> > {
for (size_t i = 0; i < numViews; i++) { // for each camera/measurement
const Pose3& w_P_body1 = 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 = interp_params_[i];
double interpolationFactor = alphas_[i];
// get interpolated pose:
const Pose3& w_P_body = interpolate<Pose3>(w_P_body1, w_P_body2,interpolationFactor, dInterpPose_dPoseBody1, dInterpPose_dPoseBody2);
const Pose3& body_P_cam = body_P_sensors_[i];
@ -377,7 +377,7 @@ PinholePose<CALIBRATION> > {
typename Base::Cameras cameras(const Values& values) const override {
size_t numViews = this->measured_.size();
assert(numViews == K_all_.size());
assert(numViews == interp_params_.size());
assert(numViews == alphas_.size());
assert(numViews == body_P_sensors_.size());
assert(numViews == world_P_body_key_pairs_.size());
@ -385,7 +385,7 @@ PinholePose<CALIBRATION> > {
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_body2 = values.at<Pose3>(world_P_body_key_pairs_[i].second);
double interpolationFactor = interp_params_[i];
double interpolationFactor = alphas_[i];
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& w_P_cam = w_P_body.compose(body_P_cam);