Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Smart Factors for arbitrary cameras #862

Merged
merged 60 commits into from
Nov 8, 2021
Merged
Changes from 1 commit
Commits
Show all changes
60 commits
Select commit Hold shift + click to select a range
be55635
starting to make templates for smart projection factors uniform (all …
Aug 24, 2021
cf3cf39
investigating potential templating alternatives
Aug 25, 2021
db2a915
don't like it - going to create a different class
Aug 25, 2021
8d9ede8
reverted all changes back to master
Aug 26, 2021
432921e
and created a new factor instead - moving to testing now
Aug 26, 2021
37dfb0c
compiles, testing now
Aug 26, 2021
df82ca1
fixed bug
Aug 26, 2021
492c2b8
progress on tests
Aug 26, 2021
7b399a3
removed line
Aug 26, 2021
b523623
still challenging to parse extrinsics
Aug 26, 2021
330a100
Merge branch 'feature/rollingShutterSmartFactors' into feature/camera…
Aug 26, 2021
d004a8c
tests finally passing!
Aug 26, 2021
8af633a
Merge branch 'feature/rollingShutterSmartFactors' into feature/camera…
Aug 26, 2021
fe75554
added capability to use multiple measurements from the same pose. unf…
Aug 26, 2021
050d64b
all tests pass except one on serialization
Aug 26, 2021
9479bdd
passing tough test - nice!
Aug 26, 2021
7d80f52
done with all tests
Aug 26, 2021
e9641ba
Merge branch 'develop' into feature/cameraTemplateForAllSmartFactors
Aug 29, 2021
94727d8
Merge branch 'develop' into feature/cameraTemplateForAllSmartFactors
Aug 29, 2021
0b9c368
Removed types defined in Base class
dellaert Aug 29, 2021
e0946df
Documented linear factors better.
dellaert Aug 29, 2021
4ef234b
Formatting and better documentation
dellaert Aug 29, 2021
372ae27
Added two ReadMe files to document the plethora of smart factors.
dellaert Aug 29, 2021
c0262b0
Address review comments, docs only.
dellaert Aug 29, 2021
f3c65d9
Merge pull request #865 from borglab/feature/readmes
dellaert Aug 29, 2021
c4b6487
Merge branch 'develop' into feature/cameraTemplateForAllSmartFactors
Sep 23, 2021
8c244a2
Merge branch 'develop' into feature/cameraTemplateForAllSmartFactors
Oct 2, 2021
b49bd12
renamed smartProjectionFactorP -> smartProjectionRigFactor
Oct 2, 2021
a5db090
modernized factor
Oct 3, 2021
4c10be9
fixed equal
Oct 3, 2021
117f0d1
fixing tests
Oct 3, 2021
eb82878
halfway there
Oct 3, 2021
fa4de18
working on tests
Oct 3, 2021
3758fda
all tests work except serialization
Oct 3, 2021
4a88574
added extra test with multi cam
Oct 3, 2021
222380c
added more comments here and there
Oct 5, 2021
7988a70
changing API for rolling shutter
Oct 5, 2021
4fd6c2c
bug fix - finalizing last few tests
Oct 6, 2021
0797eae
all tests are passing!
Oct 6, 2021
823a7e7
done with tests
Oct 6, 2021
f0745a9
now I only need to fix comments in rolling shutter factor
Oct 6, 2021
737dcf6
all set here!
Oct 6, 2021
c4cd2b5
fixed formatting (plus small fix: std::vector -> fastVector)
Nov 6, 2021
dfd4a77
formatting + const&
Nov 6, 2021
459c8f9
Merge branch 'develop' into feature/cameraTemplateForAllSmartFactors
Nov 6, 2021
1e38468
more const&
Nov 6, 2021
710a64f
now throwing exception is params are incorrect
Nov 6, 2021
7fa3b5c
added variable in loop
Nov 6, 2021
29f3af5
point2 -> measurement
Nov 6, 2021
dfd86e8
this will need to be applied in #861
Nov 7, 2021
e0af235
disabled timing for test
Nov 7, 2021
5efde52
Merge branch 'develop' into feature/cameraTemplateForAllSmartFactors
Nov 7, 2021
3a4ceda
fixed readme
Nov 7, 2021
2e8d373
serialization is still off
Nov 7, 2021
b1baf6c
final cosmetics
Nov 7, 2021
c105aa4
added intermediate camera variable for clarity
Nov 7, 2021
2c2e43e
got rid of second constructor
Nov 7, 2021
ac58756
further cleanup before moving to sharedPtrs
Nov 7, 2021
4ba9373
moved rig to use shared ptrs
Nov 7, 2021
620f9cb
now using shared ptrs
Nov 7, 2021
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Next Next commit
formatting + const&
lcarlone committed Nov 6, 2021
commit dfd4a774549923eacd2a380ea0e7fd3b876a65b3
240 changes: 126 additions & 114 deletions gtsam/slam/SmartProjectionRigFactor.h
Original file line number Diff line number Diff line change
@@ -14,8 +14,10 @@
* @brief Smart factor on poses, assuming camera calibration is fixed.
* Same as SmartProjectionPoseFactor, except:
* - 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 allows multiple observations from the same pose/key (again, to model a multi-camera system)
* - it admits a different calibration for each measurement (i.e., it
* 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 Frank Dellaert
*/
@@ -30,40 +32,42 @@ namespace gtsam {
* @addtogroup SLAM
*
* If you are using the factor, please cite:
* L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating conditionally
* independent sets in factor graphs: a unifying perspective based on smart factors,
* Int. Conf. on Robotics and Automation (ICRA), 2014.
* L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating
* conditionally independent sets in factor graphs: a unifying perspective based
* on smart factors, Int. Conf. on Robotics and Automation (ICRA), 2014.
*/

/**
* 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
* extrinsic and intrinsic calibration). The factor only constrains poses (variable dimension
* is 6 for each pose). This factor requires that values contains the involved poses (Pose3).
* If all measurements share the same calibration (i.e., are from the same camera), use SmartProjectionPoseFactor instead!
* If the calibration should be optimized, as well, use SmartProjectionFactor instead!
* extrinsic and intrinsic calibration). The factor only constrains poses
* (variable dimension is 6 for each pose). This factor requires that values
* contains the involved poses (Pose3). If all measurements share the same
* calibration (i.e., are from the same camera), use SmartProjectionPoseFactor
* instead! If the calibration should be optimized, as well, use
* SmartProjectionFactor instead!
* @addtogroup SLAM
*/
template<class CAMERA>
template <class CAMERA>
class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {

private:
typedef SmartProjectionFactor<CAMERA> Base;
typedef SmartProjectionRigFactor<CAMERA> This;
typedef typename CAMERA::CalibrationType CALIBRATION;

static const int DimPose = 6; ///< Pose3 dimension
static const int ZDim = 2; ///< Measurement dimension
static const int ZDim = 2; ///< Measurement dimension

protected:

/// vector of keys (one for each observation) with potentially repeated keys
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_;
lucacarlone marked this conversation as resolved.
Show resolved Hide resolved

/// 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_;

public:
@@ -74,36 +78,35 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
typedef boost::shared_ptr<This> shared_ptr;

/// Default constructor, only for serialization
SmartProjectionRigFactor() {
}
SmartProjectionRigFactor() {}

/**
* Constructor
* @param sharedNoiseModel isotropic noise model for the 2D feature measurements
* @param cameraRig set of cameras (fixed poses wrt body and intrinsics) in the camera rig
* @param sharedNoiseModel isotropic noise model for the 2D feature
* measurements
* @param cameraRig set of cameras (fixed poses wrt body and intrinsics) in
* the camera rig
* @param params parameters for the smart projection factors
*/
SmartProjectionRigFactor(const SharedNoiseModel& sharedNoiseModel,
const Cameras& cameraRig,
const SmartProjectionParams& params =
SmartProjectionParams())
: Base(sharedNoiseModel, params),
cameraRig_(cameraRig) {
SmartProjectionRigFactor(
const SharedNoiseModel& sharedNoiseModel, const Cameras& cameraRig,
const SmartProjectionParams& params = SmartProjectionParams())
: Base(sharedNoiseModel, params), cameraRig_(cameraRig) {
// use only configuration that works with this factor
Base::params_.degeneracyMode = gtsam::ZERO_ON_DEGENERACY;
lucacarlone marked this conversation as resolved.
Show resolved Hide resolved
Base::params_.linearizationMode = gtsam::HESSIAN;
}

/**
* 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 params parameters for the smart projection factors
*/
SmartProjectionRigFactor(const SharedNoiseModel& sharedNoiseModel,
const Camera& camera,
const SmartProjectionParams& params =
SmartProjectionParams())
SmartProjectionRigFactor(
const SharedNoiseModel& sharedNoiseModel, const Camera& camera,
const SmartProjectionParams& params = SmartProjectionParams())
: Base(sharedNoiseModel, params) {
// use only configuration that works with this factor
Base::params_.degeneracyMode = gtsam::ZERO_ON_DEGENERACY;
lucacarlone marked this conversation as resolved.
Show resolved Hide resolved
@@ -112,93 +115,99 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
}

/** Virtual destructor */
~SmartProjectionRigFactor() override {
}
~SmartProjectionRigFactor() override {}

/**
* add a new measurement, corresponding to an observation from pose "poseKey"
* and taken from the camera in the rig identified by "cameraId"
* @param measured 2-dimensional location of the projection of a
* single landmark in a single view (the measurement)
* @param poseKey key corresponding to the body pose of the camera taking the measurement
* @param cameraId ID of the camera in the rig taking the measurement (default 0)
* @param poseKey key corresponding to the body pose of the camera taking the
* 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
this->measured_.push_back(measured);
this->nonUniqueKeys_.push_back(poseKey);

// also store keys in the keys_ vector: these keys are assumed to be unique, so we avoid duplicates here
if (std::find(this->keys_.begin(), this->keys_.end(), poseKey) == this->keys_.end())
// also store keys in the keys_ vector: these keys are assumed to be
// 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

// store id of the camera taking the measurement
cameraIds_.push_back(cameraId);
}

/**
* 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
* 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 cameraIds IDs of the cameras in the rig taking each measurement (same order as the measurements)
* @param poseKeys keys corresponding to the body poses of the cameras taking
* 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,
lucacarlone marked this conversation as resolved.
Show resolved Hide resolved
const FastVector<size_t>& cameraIds = FastVector<size_t>()) {
if (poseKeys.size() != measurements.size()
|| (poseKeys.size() != cameraIds.size() && cameraIds.size() != 0)) {
throw std::runtime_error("SmartProjectionRigFactor: "
"trying to add inconsistent inputs");
if (poseKeys.size() != measurements.size() ||
(poseKeys.size() != cameraIds.size() && cameraIds.size() != 0)) {
throw std::runtime_error(
"SmartProjectionRigFactor: "
"trying to add inconsistent inputs");
}
if (cameraIds.size() == 0 && cameraRig_.size() > 1) {
throw std::runtime_error(
"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++) {
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
const KeyVector nonUniqueKeys() const {
return nonUniqueKeys_;
}
/// return (for each observation) the (possibly non unique) keys involved in
/// the measurements
const KeyVector nonUniqueKeys() const { return nonUniqueKeys_; }

/// return the calibration object
inline Cameras cameraRig() const {
return cameraRig_;
}
inline Cameras cameraRig() const { return cameraRig_; }

/// return the calibration object
inline FastVector<size_t> cameraIds() const {
return cameraIds_;
}
inline FastVector<size_t> cameraIds() const { return cameraIds_; }

/**
* 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 override {
void print(
const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
std::cout << s << "SmartProjectionRigFactor: \n ";
for (size_t i = 0; i < nonUniqueKeys_.size(); i++) {
std::cout << "-- Measurement nr " << i << std::endl;
std::cout << "key: " << keyFormatter(nonUniqueKeys_[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);
}

/// equals
bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
const This *e = dynamic_cast<const This*>(&p);
return e && Base::equals(p, tol)
&& nonUniqueKeys_ == e->nonUniqueKeys()
&& cameraRig_.equals(e->cameraRig())
&& std::equal(cameraIds_.begin(), cameraIds_.end(), e->cameraIds().begin());
const This* e = dynamic_cast<const This*>(&p);
return e && Base::equals(p, tol) && nonUniqueKeys_ == e->nonUniqueKeys() &&
cameraRig_.equals(e->cameraRig()) &&
std::equal(cameraIds_.begin(), cameraIds_.end(),
e->cameraIds().begin());
}

/**
@@ -211,12 +220,12 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
typename Base::Cameras cameras;
cameras.reserve(nonUniqueKeys_.size()); // preallocate
for (size_t i = 0; i < nonUniqueKeys_.size(); i++) {
const Pose3 world_P_sensor_i = values.at<Pose3>(nonUniqueKeys_[i]) // = world_P_body
* cameraRig_[cameraIds_[i]].pose(); // = body_P_cam_i
cameras.emplace_back(
world_P_sensor_i,
make_shared<typename CAMERA::CalibrationType>(
cameraRig_[cameraIds_[i]].calibration()));
const Pose3 world_P_sensor_i =
values.at<Pose3>(nonUniqueKeys_[i]) // = world_P_body
* cameraRig_[cameraIds_[i]].pose(); // = body_P_cam_i
cameras.emplace_back(world_P_sensor_i,
make_shared<typename CAMERA::CalibrationType>(
cameraRig_[cameraIds_[i]].calibration()));
}
return cameras;
}
@@ -236,9 +245,10 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
* Compute jacobian F, E and error vector at a given linearization point
* @param values Values structure which must contain camera poses
* corresponding to keys involved in this factor
* @return Return arguments are the camera jacobians Fs (including the jacobian with
* respect to both body poses we interpolate from), the point Jacobian E,
* and the error vector b. Note that the jacobians are computed for a given point.
* @return Return arguments are the camera jacobians Fs (including the
* jacobian with respect to both body poses we interpolate from), the point
* Jacobian E, and the error vector b. Note that the jacobians are computed
* for a given point.
*/
void computeJacobiansWithTriangulatedPoint(typename Base::FBlocks& Fs,
Matrix& E, Vector& b,
@@ -248,7 +258,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
} else { // valid result: compute jacobians
b = -cameras.reprojectionError(*this->result_, this->measured_, Fs, E);
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();
Eigen::Matrix<double, DimPose, DimPose> 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)
boost::shared_ptr<RegularHessianFactor<DimPose> > createHessianFactor(
const Values& values, const double lambda = 0.0, bool diagonalDamping =
false) const {

// we may have multiple observation sharing the same keys (e.g., 2 cameras measuring from the same body pose),
// hence the number of unique keys may be smaller than nrMeasurements
size_t nrUniqueKeys = this->keys_.size(); // note: by construction, keys_ only contains unique keys
const Values& values, const double& lambda = 0.0,
bool diagonalDamping = false) const {
// we may have multiple observation sharing the same keys (e.g., 2 cameras
// measuring from the same body pose), hence the number of unique keys may
// be smaller than nrMeasurements
size_t nrUniqueKeys =
this->keys_
.size(); // note: by construction, keys_ only contains unique keys

Cameras cameras = this->cameras(values);

// Create structures for Hessian Factors
std::vector<size_t> js;
std::vector < Matrix > Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2);
std::vector < Vector > gs(nrUniqueKeys);
std::vector<Matrix> Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2);
std::vector<Vector> gs(nrUniqueKeys);

if (this->measured_.size() != cameras.size()) // 1 observation per camera
throw std::runtime_error("SmartProjectionRigFactor: "
"measured_.size() inconsistent with input");
throw std::runtime_error(
"SmartProjectionRigFactor: "
"measured_.size() inconsistent with input");

// triangulate 3D point at given linearization point
this->triangulateSafe(cameras);

if (!this->result_) { // failed: return "empty/zero" Hessian
if (this->params_.degeneracyMode == ZERO_ON_DEGENERACY) {
for (Matrix& m : Gs)
m = Matrix::Zero(DimPose, DimPose);
for (Vector& v : gs)
v = Vector::Zero(DimPose);
return boost::make_shared < RegularHessianFactor<DimPose>
> (this->keys_, Gs, gs, 0.0);
for (Matrix& m : Gs) m = Matrix::Zero(DimPose, DimPose);
for (Vector& v : gs) v = Vector::Zero(DimPose);
return boost::make_shared<RegularHessianFactor<DimPose> >(this->keys_,
Gs, gs, 0.0);
} else {
throw std::runtime_error(
"SmartProjectionRigFactor: "
@@ -303,30 +314,34 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {

// Whiten using noise model
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]);
}

const Matrix3 P = Base::Cameras::PointCov(E, lambda, diagonalDamping);

// Build augmented Hessian (with last row/column being the information vector)
// Note: we need to get the augumented hessian wrt the unique keys in key_
// Build augmented Hessian (with last row/column being the information
// vector) Note: we need to get the augumented hessian wrt the unique keys
// in key_
SymmetricBlockMatrix augmentedHessianUniqueKeys =
Base::Cameras::template SchurComplementAndRearrangeBlocks<3, 6, 6>(
Fs, E, P, b, nonUniqueKeys_, this->keys_);

return boost::make_shared < RegularHessianFactor<DimPose>
> (this->keys_, augmentedHessianUniqueKeys);
return boost::make_shared<RegularHessianFactor<DimPose> >(
this->keys_, augmentedHessianUniqueKeys);
}

/**
* Linearize to Gaussian Factor (possibly adding a damping factor Lambda for LM)
* @param values Values structure which must contain camera poses and extrinsic pose for this factor
* Linearize to Gaussian Factor (possibly adding a damping factor Lambda for
* LM)
* @param values Values structure which must contain camera poses and
* extrinsic pose for this factor
* @return a Gaussian factor
*/
boost::shared_ptr<GaussianFactor> linearizeDamped(
const Values& values, const double lambda = 0.0) const {
// depending on flag set on construction we may linearize to different linear factors
const Values& values, const double& lambda = 0.0) const {
// depending on flag set on construction we may linearize to different
// linear factors
switch (this->params_.linearizationMode) {
case HESSIAN:
return this->createHessianFactor(values, lambda);
@@ -337,30 +352,27 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
}

/// linearize
boost::shared_ptr<GaussianFactor> linearize(const Values& values) const
override {
boost::shared_ptr<GaussianFactor> linearize(
const Values& values) const override {
return this->linearizeDamped(values);
}

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(nonUniqueKeys_);
ar & BOOST_SERIALIZATION_NVP(cameraRig_);
ar & BOOST_SERIALIZATION_NVP(cameraIds_);
template <class ARCHIVE>
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar& BOOST_SERIALIZATION_NVP(nonUniqueKeys_);
ar& BOOST_SERIALIZATION_NVP(cameraRig_);
ar& BOOST_SERIALIZATION_NVP(cameraIds_);
}

};
// end of class declaration

/// traits
template<class CAMERA>
struct traits<SmartProjectionRigFactor<CAMERA> > : public Testable<
SmartProjectionRigFactor<CAMERA> > {
};
template <class CAMERA>
struct traits<SmartProjectionRigFactor<CAMERA> >
: public Testable<SmartProjectionRigFactor<CAMERA> > {};

} // \ namespace gtsam
} // namespace gtsam
82 changes: 45 additions & 37 deletions gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h
Original file line number Diff line number Diff line change
@@ -11,7 +11,8 @@

/**
* @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
*/

@@ -42,7 +43,6 @@ namespace gtsam {
template <class CAMERA>
class SmartProjectionPoseFactorRollingShutter
: public SmartProjectionFactor<CAMERA> {

private:
typedef SmartProjectionFactor<CAMERA> Base;
typedef SmartProjectionPoseFactorRollingShutter<CAMERA> This;
@@ -57,10 +57,12 @@ class SmartProjectionPoseFactorRollingShutter
/// pair of consecutive poses
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_;
lucacarlone marked this conversation as resolved.
Show resolved Hide resolved

/// 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_;

public:
@@ -72,8 +74,9 @@ class SmartProjectionPoseFactorRollingShutter
/// shorthand for a smart pointer to a factor
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 ZDim = 2; ///< Measurement dimension (Point2)
typedef Eigen::Matrix<double, ZDim, DimBlock>
@@ -84,14 +87,14 @@ class SmartProjectionPoseFactorRollingShutter
/**
* Constructor
* @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
*/
SmartProjectionPoseFactorRollingShutter(
const SharedNoiseModel& sharedNoiseModel, const Cameras& cameraRig,
const SmartProjectionParams& params = SmartProjectionParams())
: Base(sharedNoiseModel, params),
cameraRig_(cameraRig) {
: Base(sharedNoiseModel, params), cameraRig_(cameraRig) {
// use only configuration that works with this factor
Base::params_.degeneracyMode = gtsam::ZERO_ON_DEGENERACY;
lucacarlone marked this conversation as resolved.
Show resolved Hide resolved
Base::params_.linearizationMode = gtsam::HESSIAN;
@@ -130,7 +133,7 @@ class SmartProjectionPoseFactorRollingShutter
*/
void add(const Point2& measured, const Key& world_P_body_key1,
const Key& world_P_body_key2, const double& alpha,
const size_t cameraId = 0) {
const size_t& cameraId = 0) {
// store measurements in base class
this->measured_.push_back(measured);

@@ -164,29 +167,33 @@ class SmartProjectionPoseFactorRollingShutter
* 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 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,
const std::vector<std::pair<Key, Key>>& world_P_body_key_pairs,
const std::vector<double>& alphas,
const FastVector<size_t>& cameraIds = FastVector<size_t>()) {

if (world_P_body_key_pairs.size() != measurements.size()
|| world_P_body_key_pairs.size() != alphas.size()
|| (world_P_body_key_pairs.size() != cameraIds.size()
&& cameraIds.size() != 0)) { // cameraIds.size()=0 is default
throw std::runtime_error("SmartProjectionPoseFactorRollingShutter: "
"trying to add inconsistent inputs");
if (world_P_body_key_pairs.size() != measurements.size() ||
world_P_body_key_pairs.size() != alphas.size() ||
(world_P_body_key_pairs.size() != cameraIds.size() &&
cameraIds.size() != 0)) { // cameraIds.size()=0 is default
throw std::runtime_error(
"SmartProjectionPoseFactorRollingShutter: "
"trying to add inconsistent inputs");
}
if (cameraIds.size() == 0 && cameraRig_.size() > 1) {
throw std::runtime_error(
"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++) {
add(measurements[i], world_P_body_key_pairs[i].first,
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_; }

/// return the calibration object
inline Cameras cameraRig() const {
return cameraRig_;
}
inline Cameras cameraRig() const { return cameraRig_; }

/// return the calibration object
inline FastVector<size_t> cameraIds() const {
return cameraIds_;
}
inline FastVector<size_t> cameraIds() const { return cameraIds_; }

/**
* print
@@ -226,15 +229,16 @@ class SmartProjectionPoseFactorRollingShutter
<< keyFormatter(world_P_body_key_pairs_[i].second) << std::endl;
std::cout << " alpha: " << alphas_[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);
}

/// equals
bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
const SmartProjectionPoseFactorRollingShutter<CAMERA>* e =
dynamic_cast<const SmartProjectionPoseFactorRollingShutter<CAMERA>*>(&p);
dynamic_cast<const SmartProjectionPoseFactorRollingShutter<CAMERA>*>(
&p);

double keyPairsEqual = true;
if (this->world_P_body_key_pairs_.size() ==
@@ -253,9 +257,10 @@ class SmartProjectionPoseFactorRollingShutter
keyPairsEqual = false;
}

return e && Base::equals(p, tol) && alphas_ == e->alphas() && keyPairsEqual
&& cameraRig_.equals(e->cameraRig())
&& std::equal(cameraIds_.begin(), cameraIds_.end(), e->cameraIds().begin());
return e && Base::equals(p, tol) && alphas_ == e->alphas() &&
keyPairsEqual && cameraRig_.equals(e->cameraRig()) &&
std::equal(cameraIds_.begin(), cameraIds_.end(),
e->cameraIds().begin());
}

/**
@@ -292,7 +297,8 @@ class SmartProjectionPoseFactorRollingShutter
dInterpPose_dPoseBody1, dInterpPose_dPoseBody2);
auto body_P_cam = cameraRig_[cameraIds_[i]].pose();
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
Point2 reprojectionError_i =
@@ -317,7 +323,7 @@ class SmartProjectionPoseFactorRollingShutter

/// linearize and return a Hessianfactor that is an approximation of error(p)
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 {
// we may have multiple observation sharing the same keys (due to the
// 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;
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 =
values.at<Pose3>(world_P_body_key_pairs_[i].first);
const Pose3& w_P_body2 =
@@ -415,8 +422,9 @@ class SmartProjectionPoseFactorRollingShutter
interpolate<Pose3>(w_P_body1, w_P_body2, interpolationFactor);
const Pose3& body_P_cam = cameraRig_[cameraIds_[i]].pose();
const Pose3& w_P_cam = w_P_body.compose(body_P_cam);
cameras.emplace_back(w_P_cam, make_shared<typename CAMERA::CalibrationType>(
cameraRig_[cameraIds_[i]].calibration()));
cameras.emplace_back(w_P_cam,
make_shared<typename CAMERA::CalibrationType>(
cameraRig_[cameraIds_[i]].calibration()));
}
return cameras;
}
@@ -429,7 +437,7 @@ class SmartProjectionPoseFactorRollingShutter
* @return a Gaussian factor
*/
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
switch (this->params_.linearizationMode) {