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.
* 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_;
/// 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,21 +78,20 @@ 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;
Base::params_.linearizationMode = gtsam::HESSIAN;
@ -96,14 +99,14 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
/**
* 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;
@ -112,24 +115,28 @@ 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
@ -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
* 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,
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
gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h
View File

@ -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_;
/// 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;
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,7 +229,7 @@ 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);
}
@ -234,7 +237,8 @@ class SmartProjectionPoseFactorRollingShutter
/// 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) {