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

formatting + const&

release/4.3a0
lcarlone 2021-11-06 18:11:46 -04:00
parent c4cd2b5080
commit dfd4a77454
2 changed files with 171 additions and 151 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

240
gtsam/slam/SmartProjectionRigFactor.h
View File

@ -14,8 +14,10 @@
* @brief Smart factor on poses, assuming camera calibration is fixed. * @brief Smart factor on poses, assuming camera calibration is fixed.
* Same as SmartProjectionPoseFactor, except: * Same as SmartProjectionPoseFactor, except:
* - it is templated on CAMERA (i.e., it allows cameras beyond pinhole) * - it is templated on CAMERA (i.e., it allows cameras beyond pinhole)
* - it admits a different calibration for each measurement (i.e., it can model a multi-camera rig system) * - it admits a different calibration for each measurement (i.e., it
* - it allows multiple observations from the same pose/key (again, to model a multi-camera system) * can model a multi-camera rig system)
* - it allows multiple observations from the same pose/key (again, to
* model a multi-camera system)
* @author Luca Carlone * @author Luca Carlone
* @author Frank Dellaert * @author Frank Dellaert
*/ */
@ -30,40 +32,42 @@ namespace gtsam {
* @addtogroup SLAM * @addtogroup SLAM
* *
* If you are using the factor, please cite: * If you are using the factor, please cite:
* L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating conditionally * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating
* independent sets in factor graphs: a unifying perspective based on smart factors, * conditionally independent sets in factor graphs: a unifying perspective based
* Int. Conf. on Robotics and Automation (ICRA), 2014. * on smart factors, Int. Conf. on Robotics and Automation (ICRA), 2014.
*/ */
/** /**
* This factor assumes that camera calibration is fixed (but each measurement * This factor assumes that camera calibration is fixed (but each measurement
* can be taken by a different camera in the rig, hence can have a different * can be taken by a different camera in the rig, hence can have a different
* extrinsic and intrinsic calibration). The factor only constrains poses (variable dimension * extrinsic and intrinsic calibration). The factor only constrains poses
* is 6 for each pose). This factor requires that values contains the involved poses (Pose3). * (variable dimension is 6 for each pose). This factor requires that values
* If all measurements share the same calibration (i.e., are from the same camera), use SmartProjectionPoseFactor instead! * contains the involved poses (Pose3). If all measurements share the same
* If the calibration should be optimized, as well, use SmartProjectionFactor instead! * calibration (i.e., are from the same camera), use SmartProjectionPoseFactor
* instead! If the calibration should be optimized, as well, use
* SmartProjectionFactor instead!
* @addtogroup SLAM * @addtogroup SLAM
*/ */
template<class CAMERA> template <class CAMERA>
class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> { class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
private: private:
typedef SmartProjectionFactor<CAMERA> Base; typedef SmartProjectionFactor<CAMERA> Base;
typedef SmartProjectionRigFactor<CAMERA> This; typedef SmartProjectionRigFactor<CAMERA> This;
typedef typename CAMERA::CalibrationType CALIBRATION; typedef typename CAMERA::CalibrationType CALIBRATION;
static const int DimPose = 6; ///< Pose3 dimension static const int DimPose = 6; ///< Pose3 dimension
static const int ZDim = 2; ///< Measurement dimension static const int ZDim = 2; ///< Measurement dimension
protected: protected:
/// vector of keys (one for each observation) with potentially repeated keys /// vector of keys (one for each observation) with potentially repeated keys
KeyVector nonUniqueKeys_; KeyVector nonUniqueKeys_;
/// cameras in the rig (fixed poses wrt body + fixed intrinsics, for each camera) /// cameras in the rig (fixed poses wrt body + fixed intrinsics, for each
/// camera)
typename Base::Cameras cameraRig_; typename Base::Cameras cameraRig_;
/// vector of camera Ids (one for each observation, in the same order), identifying which camera took the measurement /// vector of camera Ids (one for each observation, in the same order),
/// identifying which camera took the measurement
FastVector<size_t> cameraIds_; FastVector<size_t> cameraIds_;
public: public:
@ -74,21 +78,20 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
typedef boost::shared_ptr<This> shared_ptr; typedef boost::shared_ptr<This> shared_ptr;
/// Default constructor, only for serialization /// Default constructor, only for serialization
SmartProjectionRigFactor() { SmartProjectionRigFactor() {}
}
/** /**
* Constructor * Constructor
* @param sharedNoiseModel isotropic noise model for the 2D feature measurements * @param sharedNoiseModel isotropic noise model for the 2D feature
* @param cameraRig set of cameras (fixed poses wrt body and intrinsics) in the camera rig * measurements
* @param cameraRig set of cameras (fixed poses wrt body and intrinsics) in
* the camera rig
* @param params parameters for the smart projection factors * @param params parameters for the smart projection factors
*/ */
SmartProjectionRigFactor(const SharedNoiseModel& sharedNoiseModel, SmartProjectionRigFactor(
const Cameras& cameraRig, const SharedNoiseModel& sharedNoiseModel, const Cameras& cameraRig,
const SmartProjectionParams& params = const SmartProjectionParams& params = SmartProjectionParams())
SmartProjectionParams()) : Base(sharedNoiseModel, params), cameraRig_(cameraRig) {
: Base(sharedNoiseModel, params),
cameraRig_(cameraRig) {
// use only configuration that works with this factor // use only configuration that works with this factor
Base::params_.degeneracyMode = gtsam::ZERO_ON_DEGENERACY; Base::params_.degeneracyMode = gtsam::ZERO_ON_DEGENERACY;
Base::params_.linearizationMode = gtsam::HESSIAN; Base::params_.linearizationMode = gtsam::HESSIAN;
@ -96,14 +99,14 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
/** /**
* Constructor * Constructor
* @param sharedNoiseModel isotropic noise model for the 2D feature measurements * @param sharedNoiseModel isotropic noise model for the 2D feature
* measurements
* @param camera single camera (fixed poses wrt body and intrinsics) * @param camera single camera (fixed poses wrt body and intrinsics)
* @param params parameters for the smart projection factors * @param params parameters for the smart projection factors
*/ */
SmartProjectionRigFactor(const SharedNoiseModel& sharedNoiseModel, SmartProjectionRigFactor(
const Camera& camera, const SharedNoiseModel& sharedNoiseModel, const Camera& camera,
const SmartProjectionParams& params = const SmartProjectionParams& params = SmartProjectionParams())
SmartProjectionParams())
: Base(sharedNoiseModel, params) { : Base(sharedNoiseModel, params) {
// use only configuration that works with this factor // use only configuration that works with this factor
Base::params_.degeneracyMode = gtsam::ZERO_ON_DEGENERACY; Base::params_.degeneracyMode = gtsam::ZERO_ON_DEGENERACY;
@ -112,24 +115,28 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
} }
/** Virtual destructor */ /** Virtual destructor */
~SmartProjectionRigFactor() override { ~SmartProjectionRigFactor() override {}
}
/** /**
* add a new measurement, corresponding to an observation from pose "poseKey" * add a new measurement, corresponding to an observation from pose "poseKey"
* and taken from the camera in the rig identified by "cameraId" * and taken from the camera in the rig identified by "cameraId"
* @param measured 2-dimensional location of the projection of a * @param measured 2-dimensional location of the projection of a
* single landmark in a single view (the measurement) * single landmark in a single view (the measurement)
* @param poseKey key corresponding to the body pose of the camera taking the measurement * @param poseKey key corresponding to the body pose of the camera taking the
* @param cameraId ID of the camera in the rig taking the measurement (default 0) * measurement
* @param cameraId ID of the camera in the rig taking the measurement (default
* 0)
*/ */
void add(const Point2& measured, const Key& poseKey, const size_t cameraId = 0) { void add(const Point2& measured, const Key& poseKey,
const size_t& cameraId = 0) {
// store measurement and key // store measurement and key
this->measured_.push_back(measured); this->measured_.push_back(measured);
this->nonUniqueKeys_.push_back(poseKey); this->nonUniqueKeys_.push_back(poseKey);
// 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(), poseKey) == this->keys_.end()) // unique, so we avoid duplicates here
if (std::find(this->keys_.begin(), this->keys_.end(), poseKey) ==
this->keys_.end())
this->keys_.push_back(poseKey); // add only unique keys this->keys_.push_back(poseKey); // add only unique keys
// store id of the camera taking the measurement // store id of the camera taking the measurement
@ -137,68 +144,70 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
} }
/** /**
* 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 poseKeys keys corresponding to the body poses of the cameras taking the measurements * @param poseKeys keys corresponding to the body poses of the cameras taking
* @param cameraIds IDs of the cameras in the rig taking each measurement (same order as the measurements) * the measurements
* @param cameraIds IDs of the cameras in the rig taking each measurement
* (same order as the measurements)
*/ */
void add(const Point2Vector& measurements, const KeyVector& poseKeys, void add(const Point2Vector& measurements, const KeyVector& poseKeys,
const FastVector<size_t>& cameraIds = FastVector<size_t>()) { const FastVector<size_t>& cameraIds = FastVector<size_t>()) {
if (poseKeys.size() != measurements.size() if (poseKeys.size() != measurements.size() ||
|| (poseKeys.size() != cameraIds.size() && cameraIds.size() != 0)) { (poseKeys.size() != cameraIds.size() && cameraIds.size() != 0)) {
throw std::runtime_error("SmartProjectionRigFactor: " throw std::runtime_error(
"trying to add inconsistent inputs"); "SmartProjectionRigFactor: "
"trying to add inconsistent inputs");
} }
if (cameraIds.size() == 0 && cameraRig_.size() > 1) { if (cameraIds.size() == 0 && cameraRig_.size() > 1) {
throw std::runtime_error( throw std::runtime_error(
"SmartProjectionRigFactor: " "SmartProjectionRigFactor: "
"camera rig includes multiple camera but add did not input cameraIds"); "camera rig includes multiple camera but add did not input "
"cameraIds");
} }
for (size_t i = 0; i < measurements.size(); i++) { for (size_t i = 0; i < measurements.size(); i++) {
add(measurements[i], poseKeys[i], cameraIds.size() == 0 ? 0 : cameraIds[i]); add(measurements[i], poseKeys[i],
cameraIds.size() == 0 ? 0 : cameraIds[i]);
} }
} }
/// return (for each observation) the (possibly non unique) keys involved in the measurements /// return (for each observation) the (possibly non unique) keys involved in
const KeyVector nonUniqueKeys() const { /// the measurements
return nonUniqueKeys_; const KeyVector nonUniqueKeys() const { return nonUniqueKeys_; }
}
/// return the calibration object /// return the calibration object
inline Cameras cameraRig() const { inline Cameras cameraRig() const { return cameraRig_; }
return cameraRig_;
}
/// return the calibration object /// return the calibration object
inline FastVector<size_t> cameraIds() const { inline FastVector<size_t> cameraIds() const { return cameraIds_; }
return cameraIds_;
}
/** /**
* 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 << "SmartProjectionRigFactor: \n "; std::cout << s << "SmartProjectionRigFactor: \n ";
for (size_t i = 0; i < nonUniqueKeys_.size(); i++) { for (size_t i = 0; i < nonUniqueKeys_.size(); i++) {
std::cout << "-- Measurement nr " << i << std::endl; std::cout << "-- Measurement nr " << i << std::endl;
std::cout << "key: " << keyFormatter(nonUniqueKeys_[i]) << std::endl; std::cout << "key: " << keyFormatter(nonUniqueKeys_[i]) << std::endl;
std::cout << "cameraId: " << cameraIds_[i] << std::endl; std::cout << "cameraId: " << cameraIds_[i] << std::endl;
cameraRig_[ cameraIds_[i] ].print("camera in rig:\n"); cameraRig_[cameraIds_[i]].print("camera in rig:\n");
} }
Base::print("", keyFormatter); Base::print("", keyFormatter);
} }
/// 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) return e && Base::equals(p, tol) && nonUniqueKeys_ == e->nonUniqueKeys() &&
&& nonUniqueKeys_ == e->nonUniqueKeys() cameraRig_.equals(e->cameraRig()) &&
&& cameraRig_.equals(e->cameraRig()) std::equal(cameraIds_.begin(), cameraIds_.end(),
&& std::equal(cameraIds_.begin(), cameraIds_.end(), e->cameraIds().begin()); e->cameraIds().begin());
} }
/** /**
@ -211,12 +220,12 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
typename Base::Cameras cameras; typename Base::Cameras cameras;
cameras.reserve(nonUniqueKeys_.size()); // preallocate cameras.reserve(nonUniqueKeys_.size()); // preallocate
for (size_t i = 0; i < nonUniqueKeys_.size(); i++) { for (size_t i = 0; i < nonUniqueKeys_.size(); i++) {
const Pose3 world_P_sensor_i = values.at<Pose3>(nonUniqueKeys_[i]) // = world_P_body const Pose3 world_P_sensor_i =
* cameraRig_[cameraIds_[i]].pose(); // = body_P_cam_i values.at<Pose3>(nonUniqueKeys_[i]) // = world_P_body
cameras.emplace_back( * cameraRig_[cameraIds_[i]].pose(); // = body_P_cam_i
world_P_sensor_i, cameras.emplace_back(world_P_sensor_i,
make_shared<typename CAMERA::CalibrationType>( make_shared<typename CAMERA::CalibrationType>(
cameraRig_[cameraIds_[i]].calibration())); cameraRig_[cameraIds_[i]].calibration()));
} }
return cameras; return cameras;
} }
@ -236,9 +245,10 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
* Compute jacobian F, E and error vector at a given linearization point * Compute jacobian F, E and error vector at a given linearization point
* @param values Values structure which must contain camera poses * @param values Values structure which must contain camera poses
* corresponding to keys involved in this factor * corresponding to keys involved in this factor
* @return Return arguments are the camera jacobians Fs (including the jacobian with * @return Return arguments are the camera jacobians Fs (including the
* respect to both body poses we interpolate from), the point Jacobian E, * jacobian with respect to both body poses we interpolate from), the point
* and the error vector b. Note that the jacobians are computed for a given point. * Jacobian E, and the error vector b. Note that the jacobians are computed
* for a given point.
*/ */
void computeJacobiansWithTriangulatedPoint(typename Base::FBlocks& Fs, void computeJacobiansWithTriangulatedPoint(typename Base::FBlocks& Fs,
Matrix& E, Vector& b, Matrix& E, Vector& b,
@ -248,7 +258,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
} else { // valid result: compute jacobians } else { // valid result: compute jacobians
b = -cameras.reprojectionError(*this->result_, this->measured_, Fs, E); b = -cameras.reprojectionError(*this->result_, this->measured_, Fs, E);
for (size_t i = 0; i < Fs.size(); i++) { for (size_t i = 0; i < Fs.size(); i++) {
const Pose3& body_P_sensor = cameraRig_[ cameraIds_[i] ].pose(); const Pose3& body_P_sensor = cameraRig_[cameraIds_[i]].pose();
const Pose3 world_P_body = cameras[i].pose() * body_P_sensor.inverse(); const Pose3 world_P_body = cameras[i].pose() * body_P_sensor.inverse();
Eigen::Matrix<double, DimPose, DimPose> H; Eigen::Matrix<double, DimPose, DimPose> H;
world_P_body.compose(body_P_sensor, H); world_P_body.compose(body_P_sensor, H);
@ -259,35 +269,36 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
/// linearize and return a Hessianfactor that is an approximation of error(p) /// linearize and return a Hessianfactor that is an approximation of error(p)
boost::shared_ptr<RegularHessianFactor<DimPose> > createHessianFactor( boost::shared_ptr<RegularHessianFactor<DimPose> > createHessianFactor(
const Values& values, const double lambda = 0.0, bool diagonalDamping = const Values& values, const double& lambda = 0.0,
false) const { bool diagonalDamping = false) const {
// we may have multiple observation sharing the same keys (e.g., 2 cameras
// we may have multiple observation sharing the same keys (e.g., 2 cameras measuring from the same body pose), // measuring from the same body pose), hence the number of unique keys may
// hence the number of unique keys may be smaller than nrMeasurements // be smaller than nrMeasurements
size_t nrUniqueKeys = this->keys_.size(); // note: by construction, keys_ only contains unique keys size_t nrUniqueKeys =
this->keys_
.size(); // note: by construction, keys_ only contains unique keys
Cameras cameras = this->cameras(values); Cameras cameras = this->cameras(values);
// Create structures for Hessian Factors // Create structures for Hessian Factors
std::vector<size_t> js; std::vector<size_t> js;
std::vector < Matrix > Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2); std::vector<Matrix> Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2);
std::vector < Vector > gs(nrUniqueKeys); std::vector<Vector> gs(nrUniqueKeys);
if (this->measured_.size() != cameras.size()) // 1 observation per camera if (this->measured_.size() != cameras.size()) // 1 observation per camera
throw std::runtime_error("SmartProjectionRigFactor: " throw std::runtime_error(
"measured_.size() inconsistent with input"); "SmartProjectionRigFactor: "
"measured_.size() inconsistent with input");
// triangulate 3D point at given linearization point // triangulate 3D point at given linearization point
this->triangulateSafe(cameras); this->triangulateSafe(cameras);
if (!this->result_) { // failed: return "empty/zero" Hessian if (!this->result_) { // failed: return "empty/zero" Hessian
if (this->params_.degeneracyMode == ZERO_ON_DEGENERACY) { if (this->params_.degeneracyMode == ZERO_ON_DEGENERACY) {
for (Matrix& m : Gs) for (Matrix& m : Gs) m = Matrix::Zero(DimPose, DimPose);
m = Matrix::Zero(DimPose, DimPose); for (Vector& v : gs) v = Vector::Zero(DimPose);
for (Vector& v : gs) return boost::make_shared<RegularHessianFactor<DimPose> >(this->keys_,
v = Vector::Zero(DimPose); Gs, gs, 0.0);
return boost::make_shared < RegularHessianFactor<DimPose>
> (this->keys_, Gs, gs, 0.0);
} else { } else {
throw std::runtime_error( throw std::runtime_error(
"SmartProjectionRigFactor: " "SmartProjectionRigFactor: "
@ -303,30 +314,34 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
// Whiten using noise model // Whiten using noise model
this->noiseModel_->WhitenSystem(E, b); this->noiseModel_->WhitenSystem(E, b);
for (size_t i = 0; i < Fs.size(); i++){ for (size_t i = 0; i < Fs.size(); i++) {
Fs[i] = this->noiseModel_->Whiten(Fs[i]); Fs[i] = this->noiseModel_->Whiten(Fs[i]);
} }
const Matrix3 P = Base::Cameras::PointCov(E, lambda, diagonalDamping); const Matrix3 P = Base::Cameras::PointCov(E, lambda, diagonalDamping);
// 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, 6, 6>( Base::Cameras::template SchurComplementAndRearrangeBlocks<3, 6, 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);
} }
/** /**
* 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);
@ -337,30 +352,27 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
} }
/// 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(nonUniqueKeys_); ar& BOOST_SERIALIZATION_NVP(nonUniqueKeys_);
ar & BOOST_SERIALIZATION_NVP(cameraRig_); ar& BOOST_SERIALIZATION_NVP(cameraRig_);
ar & BOOST_SERIALIZATION_NVP(cameraIds_); ar& BOOST_SERIALIZATION_NVP(cameraIds_);
} }
}; };
// end of class declaration // end of class declaration
/// traits /// traits
template<class CAMERA> template <class CAMERA>
struct traits<SmartProjectionRigFactor<CAMERA> > : public Testable< struct traits<SmartProjectionRigFactor<CAMERA> >
SmartProjectionRigFactor<CAMERA> > { : public Testable<SmartProjectionRigFactor<CAMERA> > {};
};
} // \ namespace gtsam } // namespace gtsam

82
gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h
View File

@ -11,7 +11,8 @@
/** /**
* @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
*/ */
@ -42,7 +43,6 @@ namespace gtsam {
template <class CAMERA> template <class CAMERA>
class SmartProjectionPoseFactorRollingShutter class SmartProjectionPoseFactorRollingShutter
: public SmartProjectionFactor<CAMERA> { : public SmartProjectionFactor<CAMERA> {
private: private:
typedef SmartProjectionFactor<CAMERA> Base; typedef SmartProjectionFactor<CAMERA> Base;
typedef SmartProjectionPoseFactorRollingShutter<CAMERA> This; typedef SmartProjectionPoseFactorRollingShutter<CAMERA> This;
@ -57,10 +57,12 @@ class SmartProjectionPoseFactorRollingShutter
/// pair of consecutive poses /// pair of consecutive poses
std::vector<double> alphas_; std::vector<double> alphas_;
/// one or more cameras taking observations (fixed poses wrt body + fixed intrinsics) /// one or more cameras taking observations (fixed poses wrt body + fixed
/// intrinsics)
typename Base::Cameras cameraRig_; typename Base::Cameras cameraRig_;
/// vector of camera Ids (one for each observation, in the same order), identifying which camera took the measurement /// vector of camera Ids (one for each observation, in the same order),
/// identifying which camera took the measurement
FastVector<size_t> cameraIds_; FastVector<size_t> cameraIds_;
public: public:
@ -72,8 +74,9 @@ class SmartProjectionPoseFactorRollingShutter
/// 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 static const int DimBlock =
///< pose is interpolated 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> typedef Eigen::Matrix<double, ZDim, DimBlock>
@ -84,14 +87,14 @@ class SmartProjectionPoseFactorRollingShutter
/** /**
* Constructor * Constructor
* @param Isotropic measurement noise * @param Isotropic measurement noise
* @param cameraRig set of cameras (fixed poses wrt body and intrinsics) taking the measurements * @param cameraRig set of cameras (fixed poses wrt body and intrinsics)
* taking the measurements
* @param params internal parameters of the smart factors * @param params internal parameters of the smart factors
*/ */
SmartProjectionPoseFactorRollingShutter( SmartProjectionPoseFactorRollingShutter(
const SharedNoiseModel& sharedNoiseModel, const Cameras& cameraRig, const SharedNoiseModel& sharedNoiseModel, const Cameras& cameraRig,
const SmartProjectionParams& params = SmartProjectionParams()) const SmartProjectionParams& params = SmartProjectionParams())
: Base(sharedNoiseModel, params), : Base(sharedNoiseModel, params), cameraRig_(cameraRig) {
cameraRig_(cameraRig) {
// use only configuration that works with this factor // use only configuration that works with this factor
Base::params_.degeneracyMode = gtsam::ZERO_ON_DEGENERACY; Base::params_.degeneracyMode = gtsam::ZERO_ON_DEGENERACY;
Base::params_.linearizationMode = gtsam::HESSIAN; Base::params_.linearizationMode = gtsam::HESSIAN;
@ -130,7 +133,7 @@ class SmartProjectionPoseFactorRollingShutter
*/ */
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 size_t cameraId = 0) { const size_t& cameraId = 0) {
// store measurements in base class // store measurements in base class
this->measured_.push_back(measured); this->measured_.push_back(measured);
@ -164,29 +167,33 @@ class SmartProjectionPoseFactorRollingShutter
* for the i0-th measurement can be interpolated * for the i0-th measurement can be interpolated
* @param alphas vector of interpolation params (in [0,1]), one for each * @param alphas vector of interpolation params (in [0,1]), one for each
* measurement (in the same order) * measurement (in the same order)
* @param cameraIds IDs of the cameras taking each measurement (same order as the measurements) * @param cameraIds IDs of the cameras taking each measurement (same order as
* the 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 FastVector<size_t>& cameraIds = FastVector<size_t>()) { const FastVector<size_t>& cameraIds = FastVector<size_t>()) {
if (world_P_body_key_pairs.size() != measurements.size() ||
if (world_P_body_key_pairs.size() != measurements.size() world_P_body_key_pairs.size() != alphas.size() ||
|| world_P_body_key_pairs.size() != alphas.size() (world_P_body_key_pairs.size() != cameraIds.size() &&
|| (world_P_body_key_pairs.size() != cameraIds.size() cameraIds.size() != 0)) { // cameraIds.size()=0 is default
&& cameraIds.size() != 0)) { // cameraIds.size()=0 is default throw std::runtime_error(
throw std::runtime_error("SmartProjectionPoseFactorRollingShutter: " "SmartProjectionPoseFactorRollingShutter: "
"trying to add inconsistent inputs"); "trying to add inconsistent inputs");
} }
if (cameraIds.size() == 0 && cameraRig_.size() > 1) { if (cameraIds.size() == 0 && cameraRig_.size() > 1) {
throw std::runtime_error( throw std::runtime_error(
"SmartProjectionPoseFactorRollingShutter: " "SmartProjectionPoseFactorRollingShutter: "
"camera rig includes multiple camera but add did not input cameraIds"); "camera rig includes multiple camera but add did not input "
"cameraIds");
} }
for (size_t i = 0; i < measurements.size(); i++) { for (size_t i = 0; i < measurements.size(); i++) {
add(measurements[i], world_P_body_key_pairs[i].first, add(measurements[i], world_P_body_key_pairs[i].first,
world_P_body_key_pairs[i].second, alphas[i], world_P_body_key_pairs[i].second, alphas[i],
cameraIds.size() == 0 ? 0 : cameraIds[i]); // use 0 as default if cameraIds was not specified cameraIds.size() == 0 ? 0
: cameraIds[i]); // use 0 as default if
// cameraIds was not specified
} }
} }
@ -200,14 +207,10 @@ class SmartProjectionPoseFactorRollingShutter
const std::vector<double> alphas() const { return alphas_; } const std::vector<double> alphas() const { return alphas_; }
/// return the calibration object /// return the calibration object
inline Cameras cameraRig() const { inline Cameras cameraRig() const { return cameraRig_; }
return cameraRig_;
}
/// return the calibration object /// return the calibration object
inline FastVector<size_t> cameraIds() const { inline FastVector<size_t> cameraIds() const { return cameraIds_; }
return cameraIds_;
}
/** /**
* print * print
@ -226,7 +229,7 @@ class SmartProjectionPoseFactorRollingShutter
<< 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;
std::cout << "cameraId: " << cameraIds_[i] << std::endl; std::cout << "cameraId: " << cameraIds_[i] << std::endl;
cameraRig_[ cameraIds_[i] ].print("camera in rig:\n"); cameraRig_[cameraIds_[i]].print("camera in rig:\n");
} }
Base::print("", keyFormatter); Base::print("", keyFormatter);
} }
@ -234,7 +237,8 @@ class SmartProjectionPoseFactorRollingShutter
/// 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() == if (this->world_P_body_key_pairs_.size() ==
@ -253,9 +257,10 @@ class SmartProjectionPoseFactorRollingShutter
keyPairsEqual = false; keyPairsEqual = false;
} }
return e && Base::equals(p, tol) && alphas_ == e->alphas() && keyPairsEqual return e && Base::equals(p, tol) && alphas_ == e->alphas() &&
&& cameraRig_.equals(e->cameraRig()) keyPairsEqual && cameraRig_.equals(e->cameraRig()) &&
&& std::equal(cameraIds_.begin(), cameraIds_.end(), e->cameraIds().begin()); std::equal(cameraIds_.begin(), cameraIds_.end(),
e->cameraIds().begin());
} }
/** /**
@ -292,7 +297,8 @@ class SmartProjectionPoseFactorRollingShutter
dInterpPose_dPoseBody1, dInterpPose_dPoseBody2); dInterpPose_dPoseBody1, dInterpPose_dPoseBody2);
auto body_P_cam = cameraRig_[cameraIds_[i]].pose(); auto body_P_cam = cameraRig_[cameraIds_[i]].pose();
auto w_P_cam = w_P_body.compose(body_P_cam, dPoseCam_dInterpPose); auto w_P_cam = w_P_body.compose(body_P_cam, dPoseCam_dInterpPose);
PinholeCamera<CALIBRATION> camera(w_P_cam, cameraRig_[cameraIds_[i]].calibration()); PinholeCamera<CALIBRATION> camera(
w_P_cam, cameraRig_[cameraIds_[i]].calibration());
// get jacobians and error vector for current measurement // get jacobians and error vector for current measurement
Point2 reprojectionError_i = Point2 reprojectionError_i =
@ -317,7 +323,7 @@ class SmartProjectionPoseFactorRollingShutter
/// linearize and return a Hessianfactor that is an approximation of error(p) /// linearize and return a Hessianfactor that is an approximation of error(p)
boost::shared_ptr<RegularHessianFactor<DimPose>> createHessianFactor( boost::shared_ptr<RegularHessianFactor<DimPose>> createHessianFactor(
const Values& values, const double lambda = 0.0, const Values& values, const double& lambda = 0.0,
bool diagonalDamping = false) const { bool diagonalDamping = false) const {
// we may have multiple observation sharing the same keys (due to the // we may have multiple observation sharing the same keys (due to the
// rolling shutter interpolation), hence the number of unique keys may be // rolling shutter interpolation), hence the number of unique keys may be
@ -405,7 +411,8 @@ class SmartProjectionPoseFactorRollingShutter
*/ */
typename Base::Cameras cameras(const Values& values) const override { typename Base::Cameras cameras(const Values& values) const override {
typename Base::Cameras cameras; typename Base::Cameras cameras;
for (size_t i = 0; i < this->measured_.size(); i++) { // for each measurement for (size_t i = 0; i < this->measured_.size();
i++) { // for each measurement
const Pose3& w_P_body1 = const Pose3& w_P_body1 =
values.at<Pose3>(world_P_body_key_pairs_[i].first); values.at<Pose3>(world_P_body_key_pairs_[i].first);
const Pose3& w_P_body2 = const Pose3& w_P_body2 =
@ -415,8 +422,9 @@ class SmartProjectionPoseFactorRollingShutter
interpolate<Pose3>(w_P_body1, w_P_body2, interpolationFactor); interpolate<Pose3>(w_P_body1, w_P_body2, interpolationFactor);
const Pose3& body_P_cam = cameraRig_[cameraIds_[i]].pose(); const Pose3& body_P_cam = cameraRig_[cameraIds_[i]].pose();
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, make_shared<typename CAMERA::CalibrationType>( cameras.emplace_back(w_P_cam,
cameraRig_[cameraIds_[i]].calibration())); make_shared<typename CAMERA::CalibrationType>(
cameraRig_[cameraIds_[i]].calibration()));
} }
return cameras; return cameras;
} }
@ -429,7 +437,7 @@ class SmartProjectionPoseFactorRollingShutter
* @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 // depending on flag set on construction we may linearize to different
// linear factors // linear factors
switch (this->params_.linearizationMode) { switch (this->params_.linearizationMode) {