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

Formatting and better documentation

release/4.3a0
Frank Dellaert 2021-08-29 16:46:53 -04:00
parent e0946dfc86
commit 4ef234bbbb
5 changed files with 173 additions and 135 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
gtsam/slam/ProjectionFactor.h
View File

@ -11,7 +11,7 @@
/** /**
* @file ProjectionFactor.h * @file ProjectionFactor.h
* @brief Basic bearing factor from 2D measurement * @brief Reprojection of a LANDMARK to a 2D point.
* @author Chris Beall * @author Chris Beall
* @author Richard Roberts * @author Richard Roberts
* @author Frank Dellaert * @author Frank Dellaert
@ -22,17 +22,20 @@
#include <gtsam/nonlinear/NonlinearFactor.h> #include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/geometry/PinholeCamera.h> #include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Cal3_S2.h> #include <gtsam/geometry/Cal3_S2.h>
#include <boost/optional.hpp> #include <boost/optional.hpp>
namespace gtsam { namespace gtsam {
/** /**
* Non-linear factor for a constraint derived from a 2D measurement. The calibration is known here. * Non-linear factor for a constraint derived from a 2D measurement.
* i.e. the main building block for visual SLAM. * The calibration is known here.
* The main building block for visual SLAM.
* @addtogroup SLAM * @addtogroup SLAM
*/ */
template<class POSE, class LANDMARK, class CALIBRATION = Cal3_S2> template<class POSE=Pose3, class LANDMARK=Point3, class CALIBRATION = Cal3_S2>
class GenericProjectionFactor: public NoiseModelFactor2<POSE, LANDMARK> { class GenericProjectionFactor: public NoiseModelFactor2<POSE, LANDMARK> {
protected: protected:
@ -57,9 +60,9 @@ namespace gtsam {
typedef boost::shared_ptr<This> shared_ptr; typedef boost::shared_ptr<This> shared_ptr;
/// Default constructor /// Default constructor
GenericProjectionFactor() : GenericProjectionFactor() :
measured_(0, 0), throwCheirality_(false), verboseCheirality_(false) { measured_(0, 0), throwCheirality_(false), verboseCheirality_(false) {
} }
/** /**
* Constructor * Constructor

155
gtsam/slam/SmartFactorBase.h
View File

@ -37,12 +37,14 @@
namespace gtsam { namespace gtsam {
/** /**
* @brief Base class for smart factors * @brief Base class for smart factors.
* This base class has no internal point, but it has a measurement, noise model * This base class has no internal point, but it has a measurement, noise model
* and an optional sensor pose. * and an optional sensor pose.
* This class mainly computes the derivatives and returns them as a variety of factors. * This class mainly computes the derivatives and returns them as a variety of
* The methods take a Cameras argument, which should behave like PinholeCamera, and * factors. The methods take a CameraSet<CAMERA> argument and the value of a
* the value of a point, which is kept in the base class. * point, which is kept in the derived class.
*
* @tparam CAMERA should behave like a set of PinholeCamera.
*/ */
template<class CAMERA> template<class CAMERA>
class SmartFactorBase: public NonlinearFactor { class SmartFactorBase: public NonlinearFactor {
@ -64,19 +66,20 @@ protected:
/** /**
* As of Feb 22, 2015, the noise model is the same for all measurements and * As of Feb 22, 2015, the noise model is the same for all measurements and
* is isotropic. This allows for moving most calculations of Schur complement * is isotropic. This allows for moving most calculations of Schur complement
* etc to be moved to CameraSet very easily, and also agrees pragmatically * etc. to be easily moved to CameraSet, and also agrees pragmatically
* with what is normally done. * with what is normally done.
*/ */
SharedIsotropic noiseModel_; SharedIsotropic noiseModel_;
/** /**
* 2D measurement and noise model for each of the m views * Measurements for each of the m views.
* We keep a copy of measurements for I/O and computing the error. * We keep a copy of the measurements for I/O and computing the error.
* The order is kept the same as the keys that we use to create the factor. * The order is kept the same as the keys that we use to create the factor.
*/ */
ZVector measured_; ZVector measured_;
boost::optional<Pose3> body_P_sensor_; ///< Pose of the camera in the body frame boost::optional<Pose3>
body_P_sensor_; ///< Pose of the camera in the body frame
// Cache for Fblocks, to avoid a malloc ever time we re-linearize // Cache for Fblocks, to avoid a malloc ever time we re-linearize
mutable FBlocks Fs; mutable FBlocks Fs;
@ -84,16 +87,16 @@ protected:
public: public:
GTSAM_MAKE_ALIGNED_OPERATOR_NEW GTSAM_MAKE_ALIGNED_OPERATOR_NEW
/// 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;
/// We use the new CameraSte data structure to refer to a set of cameras /// The CameraSet data structure is used to refer to a set of cameras.
typedef CameraSet<CAMERA> Cameras; typedef CameraSet<CAMERA> Cameras;
/// Default Constructor, for serialization /// Default Constructor, for serialization.
SmartFactorBase() {} SmartFactorBase() {}
/// Constructor /// Construct with given noise model and optional arguments.
SmartFactorBase(const SharedNoiseModel& sharedNoiseModel, SmartFactorBase(const SharedNoiseModel& sharedNoiseModel,
boost::optional<Pose3> body_P_sensor = boost::none, boost::optional<Pose3> body_P_sensor = boost::none,
size_t expectedNumberCameras = 10) size_t expectedNumberCameras = 10)
@ -111,12 +114,12 @@ protected:
noiseModel_ = sharedIsotropic; noiseModel_ = sharedIsotropic;
} }
/// Virtual destructor, subclasses from NonlinearFactor /// Virtual destructor, subclasses from NonlinearFactor.
~SmartFactorBase() override { ~SmartFactorBase() override {
} }
/** /**
* Add a new measurement and pose/camera key * Add a new measurement and pose/camera key.
* @param measured is the 2m dimensional projection of a single landmark * @param measured is the 2m dimensional projection of a single landmark
* @param key is the index corresponding to the camera observing the landmark * @param key is the index corresponding to the camera observing the landmark
*/ */
@ -129,9 +132,7 @@ protected:
this->keys_.push_back(key); this->keys_.push_back(key);
} }
/** /// Add a bunch of measurements, together with the camera keys.
* Add a bunch of measurements, together with the camera keys
*/
void add(const ZVector& measurements, const KeyVector& cameraKeys) { void add(const ZVector& measurements, const KeyVector& cameraKeys) {
assert(measurements.size() == cameraKeys.size()); assert(measurements.size() == cameraKeys.size());
for (size_t i = 0; i < measurements.size(); i++) { for (size_t i = 0; i < measurements.size(); i++) {
@ -140,8 +141,8 @@ protected:
} }
/** /**
* Adds an entire SfM_track (collection of cameras observing a single point). * Add an entire SfM_track (collection of cameras observing a single point).
* The noise is assumed to be the same for all measurements * The noise is assumed to be the same for all measurements.
*/ */
template<class SFM_TRACK> template<class SFM_TRACK>
void add(const SFM_TRACK& trackToAdd) { void add(const SFM_TRACK& trackToAdd) {
@ -151,17 +152,13 @@ protected:
} }
} }
/// get the dimension (number of rows!) of the factor /// Return the dimension (number of rows!) of the factor.
size_t dim() const override { size_t dim() const override { return ZDim * this->measured_.size(); }
return ZDim * this->measured_.size();
}
/** return the measurements */ /// Return the 2D measurements (ZDim, in general).
const ZVector& measured() const { const ZVector& measured() const { return measured_; }
return measured_;
}
/// Collect all cameras: important that in key order /// Collect all cameras: important that in key order.
virtual Cameras cameras(const Values& values) const { virtual Cameras cameras(const Values& values) const {
Cameras cameras; Cameras cameras;
for(const Key& k: this->keys_) for(const Key& k: this->keys_)
@ -188,25 +185,30 @@ protected:
/// 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); if (const This* e = dynamic_cast<const This*>(&p)) {
// Check that all measurements are the same.
bool areMeasurementsEqual = true; for (size_t i = 0; i < measured_.size(); i++) {
for (size_t i = 0; i < measured_.size(); i++) { if (!traits<Z>::Equals(this->measured_.at(i), e->measured_.at(i), tol))
if (traits<Z>::Equals(this->measured_.at(i), e->measured_.at(i), tol) == false) return false;
areMeasurementsEqual = false; }
break; // If so, check base class.
return Base::equals(p, tol);
} else {
return false;
} }
return e && Base::equals(p, tol) && areMeasurementsEqual;
} }
/// Compute reprojection errors [h(x)-z] = [cameras.project(p)-z] and /// Compute reprojection errors [h(x)-z] = [cameras.project(p)-z] and
/// derivatives /// derivatives. This is the error before the noise model is applied.
template <class POINT> template <class POINT>
Vector unwhitenedError( Vector unwhitenedError(
const Cameras& cameras, const POINT& point, const Cameras& cameras, const POINT& point,
boost::optional<typename Cameras::FBlocks&> Fs = boost::none, // boost::optional<typename Cameras::FBlocks&> Fs = boost::none, //
boost::optional<Matrix&> E = boost::none) const { boost::optional<Matrix&> E = boost::none) const {
Vector ue = cameras.reprojectionError(point, measured_, Fs, E); // Reproject, with optional derivatives.
Vector error = cameras.reprojectionError(point, measured_, Fs, E);
// Apply chain rule if body_P_sensor_ is given.
if (body_P_sensor_ && Fs) { if (body_P_sensor_ && Fs) {
const Pose3 sensor_P_body = body_P_sensor_->inverse(); const Pose3 sensor_P_body = body_P_sensor_->inverse();
constexpr int camera_dim = traits<CAMERA>::dimension; constexpr int camera_dim = traits<CAMERA>::dimension;
@ -224,52 +226,60 @@ protected:
Fs->at(i) = Fs->at(i) * J; Fs->at(i) = Fs->at(i) * J;
} }
} }
correctForMissingMeasurements(cameras, ue, Fs, E);
return ue; // Correct the Jacobians in case some measurements are missing.
correctForMissingMeasurements(cameras, error, Fs, E);
return error;
} }
/** /**
* This corrects the Jacobians for the case in which some pixel measurement is missing (nan) * This corrects the Jacobians for the case in which some 2D measurement is
* In practice, this does not do anything in the monocular case, but it is implemented in the stereo version * missing (nan). In practice, this does not do anything in the monocular
* case, but it is implemented in the stereo version.
*/ */
virtual void correctForMissingMeasurements(const Cameras& cameras, Vector& ue, boost::optional<typename Cameras::FBlocks&> Fs = boost::none, virtual void correctForMissingMeasurements(
boost::optional<Matrix&> E = boost::none) const {} const Cameras& cameras, Vector& ue,
boost::optional<typename Cameras::FBlocks&> Fs = boost::none,
boost::optional<Matrix&> E = boost::none) const {}
/** /**
* Calculate vector of re-projection errors [h(x)-z] = [cameras.project(p) - z] * Calculate vector of re-projection errors [h(x)-z] = [cameras.project(p) -
* Noise model applied * z], with the noise model applied.
*/ */
template<class POINT> template<class POINT>
Vector whitenedError(const Cameras& cameras, const POINT& point) const { Vector whitenedError(const Cameras& cameras, const POINT& point) const {
Vector e = cameras.reprojectionError(point, measured_); Vector error = cameras.reprojectionError(point, measured_);
if (noiseModel_) if (noiseModel_)
noiseModel_->whitenInPlace(e); noiseModel_->whitenInPlace(error);
return e; return error;
} }
/** Calculate the error of the factor. /**
* This is the log-likelihood, e.g. \f$ 0.5(h(x)-z)^2/\sigma^2 \f$ in case of Gaussian. * Calculate the error of the factor.
* In this class, we take the raw prediction error \f$ h(x)-z \f$, ask the noise model * This is the log-likelihood, e.g. \f$ 0.5(h(x)-z)^2/\sigma^2 \f$ in case of
* to transform it to \f$ (h(x)-z)^2/\sigma^2 \f$, and then multiply by 0.5. * Gaussian. In this class, we take the raw prediction error \f$ h(x)-z \f$,
* Will be used in "error(Values)" function required by NonlinearFactor base class * ask the noise model to transform it to \f$ (h(x)-z)^2/\sigma^2 \f$, and
* then multiply by 0.5. Will be used in "error(Values)" function required by
* NonlinearFactor base class
*/ */
template<class POINT> template<class POINT>
double totalReprojectionError(const Cameras& cameras, double totalReprojectionError(const Cameras& cameras,
const POINT& point) const { const POINT& point) const {
Vector e = whitenedError(cameras, point); Vector error = whitenedError(cameras, point);
return 0.5 * e.dot(e); return 0.5 * error.dot(error);
} }
/// Computes Point Covariance P from E /// Computes Point Covariance P from the "point Jacobian" E.
static Matrix PointCov(Matrix& E) { static Matrix PointCov(const Matrix& E) {
return (E.transpose() * E).inverse(); return (E.transpose() * E).inverse();
} }
/** /**
* Compute F, E, and b (called below in both vanilla and SVD versions), where * Compute F, E, and b (called below in both vanilla and SVD versions), where
* F is a vector of derivatives wrpt the cameras, and E the stacked derivatives * F is a vector of derivatives wrpt the cameras, and E the stacked
* with respect to the point. The value of cameras/point are passed as parameters. * derivatives with respect to the point. The value of cameras/point are
* TODO: Kill this obsolete method * passed as parameters.
*/ */
template<class POINT> template<class POINT>
void computeJacobians(FBlocks& Fs, Matrix& E, Vector& b, void computeJacobians(FBlocks& Fs, Matrix& E, Vector& b,
@ -281,7 +291,11 @@ protected:
b = -unwhitenedError(cameras, point, Fs, E); b = -unwhitenedError(cameras, point, Fs, E);
} }
/// SVD version /**
* SVD version that produces smaller Jacobian matrices by doing an SVD
* decomposition on E, and returning the left nulkl-space of E.
* See JacobianFactorSVD for more documentation.
* */
template<class POINT> template<class POINT>
void computeJacobiansSVD(FBlocks& Fs, Matrix& Enull, void computeJacobiansSVD(FBlocks& Fs, Matrix& Enull,
Vector& b, const Cameras& cameras, const POINT& point) const { Vector& b, const Cameras& cameras, const POINT& point) const {
@ -291,14 +305,14 @@ protected:
static const int N = FixedDimension<POINT>::value; // 2 (Unit3) or 3 (Point3) static const int N = FixedDimension<POINT>::value; // 2 (Unit3) or 3 (Point3)
// Do SVD on A // Do SVD on A.
Eigen::JacobiSVD<Matrix> svd(E, Eigen::ComputeFullU); Eigen::JacobiSVD<Matrix> svd(E, Eigen::ComputeFullU);
Vector s = svd.singularValues();
size_t m = this->keys_.size(); size_t m = this->keys_.size();
Enull = svd.matrixU().block(0, N, ZDim * m, ZDim * m - N); // last ZDim*m-N columns Enull = svd.matrixU().block(0, N, ZDim * m, ZDim * m - N); // last ZDim*m-N columns
} }
/// Linearize to a Hessianfactor /// Linearize to a Hessianfactor.
// TODO(dellaert): Not used/tested anywhere and not properly whitened.
boost::shared_ptr<RegularHessianFactor<Dim> > createHessianFactor( boost::shared_ptr<RegularHessianFactor<Dim> > createHessianFactor(
const Cameras& cameras, const Point3& point, const double lambda = 0.0, const Cameras& cameras, const Point3& point, const double lambda = 0.0,
bool diagonalDamping = false) const { bool diagonalDamping = false) const {
@ -351,9 +365,7 @@ protected:
P, b); P, b);
} }
/** /// Return Jacobians as JacobianFactorQ.
* Return Jacobians as JacobianFactorQ
*/
boost::shared_ptr<JacobianFactorQ<Dim, ZDim> > createJacobianQFactor( boost::shared_ptr<JacobianFactorQ<Dim, ZDim> > createJacobianQFactor(
const Cameras& cameras, const Point3& point, double lambda = 0.0, const Cameras& cameras, const Point3& point, double lambda = 0.0,
bool diagonalDamping = false) const { bool diagonalDamping = false) const {
@ -368,8 +380,8 @@ protected:
} }
/** /**
* Return Jacobians as JacobianFactorSVD * Return Jacobians as JacobianFactorSVD.
* TODO lambda is currently ignored * TODO(dellaert): lambda is currently ignored
*/ */
boost::shared_ptr<JacobianFactor> createJacobianSVDFactor( boost::shared_ptr<JacobianFactor> createJacobianSVDFactor(
const Cameras& cameras, const Point3& point, double lambda = 0.0) const { const Cameras& cameras, const Point3& point, double lambda = 0.0) const {
@ -393,6 +405,7 @@ protected:
F.block<ZDim, Dim>(ZDim * i, Dim * i) = Fs.at(i); F.block<ZDim, Dim>(ZDim * i, Dim * i) = Fs.at(i);
} }
// Return sensor pose.
Pose3 body_P_sensor() const{ Pose3 body_P_sensor() const{
if(body_P_sensor_) if(body_P_sensor_)
return *body_P_sensor_; return *body_P_sensor_;

106
gtsam/slam/SmartProjectionFactor.h
View File

@ -61,10 +61,11 @@ protected:
/// @name Caching triangulation /// @name Caching triangulation
/// @{ /// @{
mutable TriangulationResult result_; ///< result from triangulateSafe mutable TriangulationResult result_; ///< result from triangulateSafe
mutable std::vector<Pose3, Eigen::aligned_allocator<Pose3> > cameraPosesTriangulation_; ///< current triangulation poses mutable std::vector<Pose3, Eigen::aligned_allocator<Pose3> >
cameraPosesTriangulation_; ///< current triangulation poses
/// @} /// @}
public: public:
/// 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;
@ -116,21 +117,31 @@ public:
&& Base::equals(p, tol); && Base::equals(p, tol);
} }
/// Check if the new linearization point is the same as the one used for previous triangulation /**
* @brief Check if the new linearization point is the same as the one used for
* previous triangulation.
*
* @param cameras
* @return true if we need to re-triangulate.
*/
bool decideIfTriangulate(const Cameras& cameras) const { bool decideIfTriangulate(const Cameras& cameras) const {
// several calls to linearize will be done from the same linearization point, hence it is not needed to re-triangulate // Several calls to linearize will be done from the same linearization
// Note that this is not yet "selecting linearization", that will come later, and we only check if the // point, hence it is not needed to re-triangulate. Note that this is not
// current linearization is the "same" (up to tolerance) w.r.t. the last time we triangulated the point // yet "selecting linearization", that will come later, and we only check if
// the current linearization is the "same" (up to tolerance) w.r.t. the last
// time we triangulated the point.
size_t m = cameras.size(); size_t m = cameras.size();
bool retriangulate = false; bool retriangulate = false;
// if we do not have a previous linearization point or the new linearization point includes more poses // Definitely true if we do not have a previous linearization point or the
// new linearization point includes more poses.
if (cameraPosesTriangulation_.empty() if (cameraPosesTriangulation_.empty()
|| cameras.size() != cameraPosesTriangulation_.size()) || cameras.size() != cameraPosesTriangulation_.size())
retriangulate = true; retriangulate = true;
// Otherwise, check poses against cache.
if (!retriangulate) { if (!retriangulate) {
for (size_t i = 0; i < cameras.size(); i++) { for (size_t i = 0; i < cameras.size(); i++) {
if (!cameras[i].pose().equals(cameraPosesTriangulation_[i], if (!cameras[i].pose().equals(cameraPosesTriangulation_[i],
@ -141,7 +152,8 @@ public:
} }
} }
if (retriangulate) { // we store the current poses used for triangulation // Store the current poses used for triangulation if we will re-triangulate.
if (retriangulate) {
cameraPosesTriangulation_.clear(); cameraPosesTriangulation_.clear();
cameraPosesTriangulation_.reserve(m); cameraPosesTriangulation_.reserve(m);
for (size_t i = 0; i < m; i++) for (size_t i = 0; i < m; i++)
@ -149,10 +161,15 @@ public:
cameraPosesTriangulation_.push_back(cameras[i].pose()); cameraPosesTriangulation_.push_back(cameras[i].pose());
} }
return retriangulate; // if we arrive to this point all poses are the same and we don't need re-triangulation return retriangulate;
} }
/// triangulateSafe /**
* @brief Call gtsam::triangulateSafe iff we need to re-triangulate.
*
* @param cameras
* @return TriangulationResult
*/
TriangulationResult triangulateSafe(const Cameras& cameras) const { TriangulationResult triangulateSafe(const Cameras& cameras) const {
size_t m = cameras.size(); size_t m = cameras.size();
@ -166,17 +183,21 @@ public:
return result_; return result_;
} }
/// triangulate /**
* @brief Possibly re-triangulate before calculating Jacobians.
*
* @param cameras
* @return true if we could safely triangulate
*/
bool triangulateForLinearize(const Cameras& cameras) const { bool triangulateForLinearize(const Cameras& cameras) const {
triangulateSafe(cameras); // imperative, might reset result_ triangulateSafe(cameras); // imperative, might reset result_
return bool(result_); return bool(result_);
} }
/// linearize returns a Hessianfactor that is an approximation of error(p) /// Create a Hessianfactor that is an approximation of error(p).
boost::shared_ptr<RegularHessianFactor<Base::Dim> > createHessianFactor( boost::shared_ptr<RegularHessianFactor<Base::Dim> > createHessianFactor(
const Cameras& cameras, const double lambda = 0.0, bool diagonalDamping = const Cameras& cameras, const double lambda = 0.0,
false) const { bool diagonalDamping = false) const {
size_t numKeys = this->keys_.size(); size_t numKeys = this->keys_.size();
// Create structures for Hessian Factors // Create structures for Hessian Factors
KeyVector js; KeyVector js;
@ -184,39 +205,38 @@ public:
std::vector<Vector> gs(numKeys); std::vector<Vector> gs(numKeys);
if (this->measured_.size() != cameras.size()) if (this->measured_.size() != cameras.size())
throw std::runtime_error("SmartProjectionHessianFactor: this->measured_" throw std::runtime_error(
".size() inconsistent with input"); "SmartProjectionHessianFactor: this->measured_"
".size() inconsistent with input");
triangulateSafe(cameras); triangulateSafe(cameras);
if (params_.degeneracyMode == ZERO_ON_DEGENERACY && !result_) { if (params_.degeneracyMode == ZERO_ON_DEGENERACY && !result_) {
// failed: return"empty" Hessian // failed: return"empty" Hessian
for(Matrix& m: Gs) for (Matrix& m : Gs) m = Matrix::Zero(Base::Dim, Base::Dim);
m = Matrix::Zero(Base::Dim, Base::Dim); for (Vector& v : gs) v = Vector::Zero(Base::Dim);
for(Vector& v: gs)
v = Vector::Zero(Base::Dim);
return boost::make_shared<RegularHessianFactor<Base::Dim> >(this->keys_, return boost::make_shared<RegularHessianFactor<Base::Dim> >(this->keys_,
Gs, gs, 0.0); Gs, gs, 0.0);
} }
// Jacobian could be 3D Point3 OR 2D Unit3, difference is E.cols(). // Jacobian could be 3D Point3 OR 2D Unit3, difference is E.cols().
std::vector<typename Base::MatrixZD, Eigen::aligned_allocator<typename Base::MatrixZD> > Fblocks; typename Base::FBlocks Fs;
Matrix E; Matrix E;
Vector b; Vector b;
computeJacobiansWithTriangulatedPoint(Fblocks, E, b, cameras); computeJacobiansWithTriangulatedPoint(Fs, E, b, cameras);
// Whiten using noise model // Whiten using noise model
Base::whitenJacobians(Fblocks, E, b); Base::whitenJacobians(Fs, E, b);
// build augmented hessian // build augmented hessian
SymmetricBlockMatrix augmentedHessian = // SymmetricBlockMatrix augmentedHessian = //
Cameras::SchurComplement(Fblocks, E, b, lambda, diagonalDamping); Cameras::SchurComplement(Fs, E, b, lambda, diagonalDamping);
return boost::make_shared<RegularHessianFactor<Base::Dim> >(this->keys_, return boost::make_shared<RegularHessianFactor<Base::Dim> >(
augmentedHessian); this->keys_, augmentedHessian);
} }
// create factor // Create RegularImplicitSchurFactor factor.
boost::shared_ptr<RegularImplicitSchurFactor<CAMERA> > createRegularImplicitSchurFactor( boost::shared_ptr<RegularImplicitSchurFactor<CAMERA> > createRegularImplicitSchurFactor(
const Cameras& cameras, double lambda) const { const Cameras& cameras, double lambda) const {
if (triangulateForLinearize(cameras)) if (triangulateForLinearize(cameras))
@ -226,7 +246,7 @@ public:
return boost::shared_ptr<RegularImplicitSchurFactor<CAMERA> >(); return boost::shared_ptr<RegularImplicitSchurFactor<CAMERA> >();
} }
/// create factor /// Create JacobianFactorQ factor.
boost::shared_ptr<JacobianFactorQ<Base::Dim, 2> > createJacobianQFactor( boost::shared_ptr<JacobianFactorQ<Base::Dim, 2> > createJacobianQFactor(
const Cameras& cameras, double lambda) const { const Cameras& cameras, double lambda) const {
if (triangulateForLinearize(cameras)) if (triangulateForLinearize(cameras))
@ -236,13 +256,13 @@ public:
return boost::make_shared<JacobianFactorQ<Base::Dim, 2> >(this->keys_); return boost::make_shared<JacobianFactorQ<Base::Dim, 2> >(this->keys_);
} }
/// Create a factor, takes values /// Create JacobianFactorQ factor, takes values.
boost::shared_ptr<JacobianFactorQ<Base::Dim, 2> > createJacobianQFactor( boost::shared_ptr<JacobianFactorQ<Base::Dim, 2> > createJacobianQFactor(
const Values& values, double lambda) const { const Values& values, double lambda) const {
return createJacobianQFactor(this->cameras(values), lambda); return createJacobianQFactor(this->cameras(values), lambda);
} }
/// different (faster) way to compute Jacobian factor /// Different (faster) way to compute a JacobianFactorSVD factor.
boost::shared_ptr<JacobianFactor> createJacobianSVDFactor( boost::shared_ptr<JacobianFactor> createJacobianSVDFactor(
const Cameras& cameras, double lambda) const { const Cameras& cameras, double lambda) const {
if (triangulateForLinearize(cameras)) if (triangulateForLinearize(cameras))
@ -252,19 +272,19 @@ public:
return boost::make_shared<JacobianFactorSVD<Base::Dim, 2> >(this->keys_); return boost::make_shared<JacobianFactorSVD<Base::Dim, 2> >(this->keys_);
} }
/// linearize to a Hessianfactor /// Linearize to a Hessianfactor.
virtual boost::shared_ptr<RegularHessianFactor<Base::Dim> > linearizeToHessian( virtual boost::shared_ptr<RegularHessianFactor<Base::Dim> > linearizeToHessian(
const Values& values, double lambda = 0.0) const { const Values& values, double lambda = 0.0) const {
return createHessianFactor(this->cameras(values), lambda); return createHessianFactor(this->cameras(values), lambda);
} }
/// linearize to an Implicit Schur factor /// Linearize to an Implicit Schur factor.
virtual boost::shared_ptr<RegularImplicitSchurFactor<CAMERA> > linearizeToImplicit( virtual boost::shared_ptr<RegularImplicitSchurFactor<CAMERA> > linearizeToImplicit(
const Values& values, double lambda = 0.0) const { const Values& values, double lambda = 0.0) const {
return createRegularImplicitSchurFactor(this->cameras(values), lambda); return createRegularImplicitSchurFactor(this->cameras(values), lambda);
} }
/// linearize to a JacobianfactorQ /// Linearize to a JacobianfactorQ.
virtual boost::shared_ptr<JacobianFactorQ<Base::Dim, 2> > linearizeToJacobian( virtual boost::shared_ptr<JacobianFactorQ<Base::Dim, 2> > linearizeToJacobian(
const Values& values, double lambda = 0.0) const { const Values& values, double lambda = 0.0) const {
return createJacobianQFactor(this->cameras(values), lambda); return createJacobianQFactor(this->cameras(values), lambda);
@ -334,7 +354,7 @@ public:
/// Assumes the point has been computed /// Assumes the point has been computed
/// Note E can be 2m*3 or 2m*2, in case point is degenerate /// Note E can be 2m*3 or 2m*2, in case point is degenerate
void computeJacobiansWithTriangulatedPoint( void computeJacobiansWithTriangulatedPoint(
std::vector<typename Base::MatrixZD, Eigen::aligned_allocator<typename Base::MatrixZD> >& Fblocks, Matrix& E, Vector& b, typename Base::FBlocks& Fs, Matrix& E, Vector& b,
const Cameras& cameras) const { const Cameras& cameras) const {
if (!result_) { if (!result_) {
@ -342,32 +362,32 @@ public:
// TODO check flag whether we should do this // TODO check flag whether we should do this
Unit3 backProjected = cameras[0].backprojectPointAtInfinity( Unit3 backProjected = cameras[0].backprojectPointAtInfinity(
this->measured_.at(0)); this->measured_.at(0));
Base::computeJacobians(Fblocks, E, b, cameras, backProjected); Base::computeJacobians(Fs, E, b, cameras, backProjected);
} else { } else {
// valid result: just return Base version // valid result: just return Base version
Base::computeJacobians(Fblocks, E, b, cameras, *result_); Base::computeJacobians(Fs, E, b, cameras, *result_);
} }
} }
/// Version that takes values, and creates the point /// Version that takes values, and creates the point
bool triangulateAndComputeJacobians( bool triangulateAndComputeJacobians(
std::vector<typename Base::MatrixZD, Eigen::aligned_allocator<typename Base::MatrixZD> >& Fblocks, Matrix& E, Vector& b, typename Base::FBlocks& Fs, Matrix& E, Vector& b,
const Values& values) const { const Values& values) const {
Cameras cameras = this->cameras(values); Cameras cameras = this->cameras(values);
bool nonDegenerate = triangulateForLinearize(cameras); bool nonDegenerate = triangulateForLinearize(cameras);
if (nonDegenerate) if (nonDegenerate)
computeJacobiansWithTriangulatedPoint(Fblocks, E, b, cameras); computeJacobiansWithTriangulatedPoint(Fs, E, b, cameras);
return nonDegenerate; return nonDegenerate;
} }
/// takes values /// takes values
bool triangulateAndComputeJacobiansSVD( bool triangulateAndComputeJacobiansSVD(
std::vector<typename Base::MatrixZD, Eigen::aligned_allocator<typename Base::MatrixZD> >& Fblocks, Matrix& Enull, Vector& b, typename Base::FBlocks& Fs, Matrix& Enull, Vector& b,
const Values& values) const { const Values& values) const {
Cameras cameras = this->cameras(values); Cameras cameras = this->cameras(values);
bool nonDegenerate = triangulateForLinearize(cameras); bool nonDegenerate = triangulateForLinearize(cameras);
if (nonDegenerate) if (nonDegenerate)
Base::computeJacobiansSVD(Fblocks, Enull, b, cameras, *result_); Base::computeJacobiansSVD(Fs, Enull, b, cameras, *result_);
return nonDegenerate; return nonDegenerate;
} }

22
gtsam_unstable/slam/SmartStereoProjectionFactor.h
View File

@ -447,23 +447,23 @@ public:
} }
/** /**
* This corrects the Jacobians and error vector for the case in which the right pixel in the monocular camera is missing (nan) * This corrects the Jacobians and error vector for the case in which the
* right 2D measurement in the monocular camera is missing (nan).
*/ */
void correctForMissingMeasurements(const Cameras& cameras, Vector& ue, void correctForMissingMeasurements(
boost::optional<typename Cameras::FBlocks&> Fs = boost::none, const Cameras& cameras, Vector& ue,
boost::optional<Matrix&> E = boost::none) const override boost::optional<typename Cameras::FBlocks&> Fs = boost::none,
{ boost::optional<Matrix&> E = boost::none) const override {
// when using stereo cameras, some of the measurements might be missing: // when using stereo cameras, some of the measurements might be missing:
for(size_t i=0; i < cameras.size(); i++){ for (size_t i = 0; i < cameras.size(); i++) {
const StereoPoint2& z = measured_.at(i); const StereoPoint2& z = measured_.at(i);
if(std::isnan(z.uR())) // if the right pixel is invalid if (std::isnan(z.uR())) // if the right 2D measurement is invalid
{ {
if(Fs){ // delete influence of right point on jacobian Fs if (Fs) { // delete influence of right point on jacobian Fs
MatrixZD& Fi = Fs->at(i); MatrixZD& Fi = Fs->at(i);
for(size_t ii=0; ii<Dim; ii++) for (size_t ii = 0; ii < Dim; ii++) Fi(1, ii) = 0.0;
Fi(1,ii) = 0.0;
} }
if(E) // delete influence of right point on jacobian E if (E) // delete influence of right point on jacobian E
E->row(ZDim * i + 1) = Matrix::Zero(1, E->cols()); E->row(ZDim * i + 1) = Matrix::Zero(1, E->cols());
// set the corresponding entry of vector ue to zero // set the corresponding entry of vector ue to zero

8
gtsam_unstable/slam/SmartStereoProjectionFactorPP.h
View File

@ -33,9 +33,11 @@ namespace gtsam {
*/ */
/** /**
* This factor optimizes the pose of the body as well as the extrinsic camera calibration (pose of camera wrt body). * This factor optimizes the pose of the body as well as the extrinsic camera
* Each camera may have its own extrinsic calibration or the same calibration can be shared by multiple cameras. * calibration (pose of camera wrt body). Each camera may have its own extrinsic
* This factor requires that values contain the involved poses and extrinsics (both are Pose3 variables). * calibration or the same calibration can be shared by multiple cameras. This
* factor requires that values contain the involved poses and extrinsics (both
* are Pose3 variables).
* @addtogroup SLAM * @addtogroup SLAM
*/ */
class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor { class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {