Formatting and better documentation
Frank Dellaert
parent
e0946dfc86
commit
4ef234bbbb
|
|
@ -11,7 +11,7 @@
|
|||
|
||||
/**
|
||||
* @file ProjectionFactor.h
|
||||
* @brief Basic bearing factor from 2D measurement
|
||||
* @brief Reprojection of a LANDMARK to a 2D point.
|
||||
* @author Chris Beall
|
||||
* @author Richard Roberts
|
||||
* @author Frank Dellaert
|
||||
|
|
@ -22,17 +22,20 @@
|
|||
|
||||
#include <gtsam/nonlinear/NonlinearFactor.h>
|
||||
#include <gtsam/geometry/PinholeCamera.h>
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/geometry/Point3.h>
|
||||
#include <gtsam/geometry/Cal3_S2.h>
|
||||
#include <boost/optional.hpp>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* Non-linear factor for a constraint derived from a 2D measurement. The calibration is known here.
|
||||
* i.e. the main building block for visual SLAM.
|
||||
* Non-linear factor for a constraint derived from a 2D measurement.
|
||||
* The calibration is known here.
|
||||
* The main building block for visual 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> {
|
||||
protected:
|
||||
|
||||
|
|
@ -57,9 +60,9 @@ namespace gtsam {
|
|||
typedef boost::shared_ptr<This> shared_ptr;
|
||||
|
||||
/// Default constructor
|
||||
GenericProjectionFactor() :
|
||||
measured_(0, 0), throwCheirality_(false), verboseCheirality_(false) {
|
||||
}
|
||||
GenericProjectionFactor() :
|
||||
measured_(0, 0), throwCheirality_(false), verboseCheirality_(false) {
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructor
|
||||
|
|
|
|||
|
|
@ -37,12 +37,14 @@
|
|||
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
|
||||
* and an optional sensor pose.
|
||||
* This class mainly computes the derivatives and returns them as a variety of factors.
|
||||
* The methods take a Cameras argument, which should behave like PinholeCamera, and
|
||||
* the value of a point, which is kept in the base class.
|
||||
* This class mainly computes the derivatives and returns them as a variety of
|
||||
* factors. The methods take a CameraSet<CAMERA> argument and the value of a
|
||||
* point, which is kept in the derived class.
|
||||
*
|
||||
* @tparam CAMERA should behave like a set of PinholeCamera.
|
||||
*/
|
||||
template<class CAMERA>
|
||||
class SmartFactorBase: public NonlinearFactor {
|
||||
|
|
@ -64,19 +66,20 @@ protected:
|
|||
/**
|
||||
* 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
|
||||
* 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.
|
||||
*/
|
||||
SharedIsotropic noiseModel_;
|
||||
|
||||
/**
|
||||
* 2D measurement and noise model for each of the m views
|
||||
* We keep a copy of measurements for I/O and computing the error.
|
||||
* Measurements for each of the m views.
|
||||
* 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.
|
||||
*/
|
||||
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
|
||||
mutable FBlocks Fs;
|
||||
|
|
@ -84,16 +87,16 @@ protected:
|
|||
public:
|
||||
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;
|
||||
|
||||
/// 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;
|
||||
|
||||
/// Default Constructor, for serialization
|
||||
/// Default Constructor, for serialization.
|
||||
SmartFactorBase() {}
|
||||
|
||||
/// Constructor
|
||||
/// Construct with given noise model and optional arguments.
|
||||
SmartFactorBase(const SharedNoiseModel& sharedNoiseModel,
|
||||
boost::optional<Pose3> body_P_sensor = boost::none,
|
||||
size_t expectedNumberCameras = 10)
|
||||
|
|
@ -111,12 +114,12 @@ protected:
|
|||
noiseModel_ = sharedIsotropic;
|
||||
}
|
||||
|
||||
/// Virtual destructor, subclasses from NonlinearFactor
|
||||
/// Virtual destructor, subclasses from NonlinearFactor.
|
||||
~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 key is the index corresponding to the camera observing the landmark
|
||||
*/
|
||||
|
|
@ -129,9 +132,7 @@ protected:
|
|||
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) {
|
||||
assert(measurements.size() == cameraKeys.size());
|
||||
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).
|
||||
* The noise is assumed to be the same for all measurements
|
||||
* Add an entire SfM_track (collection of cameras observing a single point).
|
||||
* The noise is assumed to be the same for all measurements.
|
||||
*/
|
||||
template<class SFM_TRACK>
|
||||
void add(const SFM_TRACK& trackToAdd) {
|
||||
|
|
@ -151,17 +152,13 @@ protected:
|
|||
}
|
||||
}
|
||||
|
||||
/// get the dimension (number of rows!) of the factor
|
||||
size_t dim() const override {
|
||||
return ZDim * this->measured_.size();
|
||||
}
|
||||
/// Return the dimension (number of rows!) of the factor.
|
||||
size_t dim() const override { return ZDim * this->measured_.size(); }
|
||||
|
||||
/** return the measurements */
|
||||
const ZVector& measured() const {
|
||||
return measured_;
|
||||
}
|
||||
/// Return the 2D measurements (ZDim, in general).
|
||||
const ZVector& measured() const { 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 {
|
||||
Cameras cameras;
|
||||
for(const Key& k: this->keys_)
|
||||
|
|
@ -188,25 +185,30 @@ protected:
|
|||
|
||||
/// equals
|
||||
bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
|
||||
const This *e = dynamic_cast<const This*>(&p);
|
||||
|
||||
bool areMeasurementsEqual = true;
|
||||
for (size_t i = 0; i < measured_.size(); i++) {
|
||||
if (traits<Z>::Equals(this->measured_.at(i), e->measured_.at(i), tol) == false)
|
||||
areMeasurementsEqual = false;
|
||||
break;
|
||||
if (const This* e = dynamic_cast<const This*>(&p)) {
|
||||
// Check that all measurements are the same.
|
||||
for (size_t i = 0; i < measured_.size(); i++) {
|
||||
if (!traits<Z>::Equals(this->measured_.at(i), e->measured_.at(i), tol))
|
||||
return false;
|
||||
}
|
||||
// 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
|
||||
/// derivatives
|
||||
/// derivatives. This is the error before the noise model is applied.
|
||||
template <class POINT>
|
||||
Vector unwhitenedError(
|
||||
const Cameras& cameras, const POINT& point,
|
||||
boost::optional<typename Cameras::FBlocks&> Fs = boost::none, //
|
||||
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) {
|
||||
const Pose3 sensor_P_body = body_P_sensor_->inverse();
|
||||
constexpr int camera_dim = traits<CAMERA>::dimension;
|
||||
|
|
@ -224,52 +226,60 @@ protected:
|
|||
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)
|
||||
* In practice, this does not do anything in the monocular case, but it is implemented in the stereo version
|
||||
* This corrects the Jacobians for the case in which some 2D measurement is
|
||||
* 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,
|
||||
boost::optional<Matrix&> E = boost::none) const {}
|
||||
virtual void correctForMissingMeasurements(
|
||||
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]
|
||||
* Noise model applied
|
||||
* Calculate vector of re-projection errors [h(x)-z] = [cameras.project(p) -
|
||||
* z], with the noise model applied.
|
||||
*/
|
||||
template<class POINT>
|
||||
Vector whitenedError(const Cameras& cameras, const POINT& point) const {
|
||||
Vector e = cameras.reprojectionError(point, measured_);
|
||||
Vector error = cameras.reprojectionError(point, measured_);
|
||||
if (noiseModel_)
|
||||
noiseModel_->whitenInPlace(e);
|
||||
return e;
|
||||
noiseModel_->whitenInPlace(error);
|
||||
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.
|
||||
* In this class, we take the raw prediction error \f$ h(x)-z \f$, 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
|
||||
/**
|
||||
* 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. In this class, we take the raw prediction error \f$ h(x)-z \f$,
|
||||
* 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>
|
||||
double totalReprojectionError(const Cameras& cameras,
|
||||
const POINT& point) const {
|
||||
Vector e = whitenedError(cameras, point);
|
||||
return 0.5 * e.dot(e);
|
||||
Vector error = whitenedError(cameras, point);
|
||||
return 0.5 * error.dot(error);
|
||||
}
|
||||
|
||||
/// Computes Point Covariance P from E
|
||||
static Matrix PointCov(Matrix& E) {
|
||||
/// Computes Point Covariance P from the "point Jacobian" E.
|
||||
static Matrix PointCov(const Matrix& E) {
|
||||
return (E.transpose() * E).inverse();
|
||||
}
|
||||
|
||||
/**
|
||||
* 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
|
||||
* with respect to the point. The value of cameras/point are passed as parameters.
|
||||
* TODO: Kill this obsolete method
|
||||
* 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 with respect to the point. The value of cameras/point are
|
||||
* passed as parameters.
|
||||
*/
|
||||
template<class POINT>
|
||||
void computeJacobians(FBlocks& Fs, Matrix& E, Vector& b,
|
||||
|
|
@ -281,7 +291,11 @@ protected:
|
|||
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>
|
||||
void computeJacobiansSVD(FBlocks& Fs, Matrix& Enull,
|
||||
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)
|
||||
|
||||
// Do SVD on A
|
||||
// Do SVD on A.
|
||||
Eigen::JacobiSVD<Matrix> svd(E, Eigen::ComputeFullU);
|
||||
Vector s = svd.singularValues();
|
||||
size_t m = this->keys_.size();
|
||||
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(
|
||||
const Cameras& cameras, const Point3& point, const double lambda = 0.0,
|
||||
bool diagonalDamping = false) const {
|
||||
|
|
@ -351,9 +365,7 @@ protected:
|
|||
P, b);
|
||||
}
|
||||
|
||||
/**
|
||||
* Return Jacobians as JacobianFactorQ
|
||||
*/
|
||||
/// Return Jacobians as JacobianFactorQ.
|
||||
boost::shared_ptr<JacobianFactorQ<Dim, ZDim> > createJacobianQFactor(
|
||||
const Cameras& cameras, const Point3& point, double lambda = 0.0,
|
||||
bool diagonalDamping = false) const {
|
||||
|
|
@ -368,8 +380,8 @@ protected:
|
|||
}
|
||||
|
||||
/**
|
||||
* Return Jacobians as JacobianFactorSVD
|
||||
* TODO lambda is currently ignored
|
||||
* Return Jacobians as JacobianFactorSVD.
|
||||
* TODO(dellaert): lambda is currently ignored
|
||||
*/
|
||||
boost::shared_ptr<JacobianFactor> createJacobianSVDFactor(
|
||||
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);
|
||||
}
|
||||
|
||||
// Return sensor pose.
|
||||
Pose3 body_P_sensor() const{
|
||||
if(body_P_sensor_)
|
||||
return *body_P_sensor_;
|
||||
|
|
|
|||
|
|
@ -61,10 +61,11 @@ protected:
|
|||
/// @name Caching triangulation
|
||||
/// @{
|
||||
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
|
||||
typedef boost::shared_ptr<This> shared_ptr;
|
||||
|
|
@ -116,21 +117,31 @@ public:
|
|||
&& 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 {
|
||||
// several calls to linearize will be done from the same linearization point, hence it is not needed to re-triangulate
|
||||
// Note that this is not 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
|
||||
// Several calls to linearize will be done from the same linearization
|
||||
// point, hence it is not needed to re-triangulate. Note that this is not
|
||||
// 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();
|
||||
|
||||
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()
|
||||
|| cameras.size() != cameraPosesTriangulation_.size())
|
||||
retriangulate = true;
|
||||
|
||||
// Otherwise, check poses against cache.
|
||||
if (!retriangulate) {
|
||||
for (size_t i = 0; i < cameras.size(); 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_.reserve(m);
|
||||
for (size_t i = 0; i < m; i++)
|
||||
|
|
@ -149,10 +161,15 @@ public:
|
|||
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 {
|
||||
|
||||
size_t m = cameras.size();
|
||||
|
|
@ -166,17 +183,21 @@ public:
|
|||
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 {
|
||||
triangulateSafe(cameras); // imperative, might reset 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(
|
||||
const Cameras& cameras, const double lambda = 0.0, bool diagonalDamping =
|
||||
false) const {
|
||||
|
||||
const Cameras& cameras, const double lambda = 0.0,
|
||||
bool diagonalDamping = false) const {
|
||||
size_t numKeys = this->keys_.size();
|
||||
// Create structures for Hessian Factors
|
||||
KeyVector js;
|
||||
|
|
@ -184,39 +205,38 @@ public:
|
|||
std::vector<Vector> gs(numKeys);
|
||||
|
||||
if (this->measured_.size() != cameras.size())
|
||||
throw std::runtime_error("SmartProjectionHessianFactor: this->measured_"
|
||||
".size() inconsistent with input");
|
||||
throw std::runtime_error(
|
||||
"SmartProjectionHessianFactor: this->measured_"
|
||||
".size() inconsistent with input");
|
||||
|
||||
triangulateSafe(cameras);
|
||||
|
||||
if (params_.degeneracyMode == ZERO_ON_DEGENERACY && !result_) {
|
||||
// failed: return"empty" Hessian
|
||||
for(Matrix& m: Gs)
|
||||
m = Matrix::Zero(Base::Dim, Base::Dim);
|
||||
for(Vector& v: gs)
|
||||
v = Vector::Zero(Base::Dim);
|
||||
for (Matrix& m : Gs) m = Matrix::Zero(Base::Dim, Base::Dim);
|
||||
for (Vector& v : gs) v = Vector::Zero(Base::Dim);
|
||||
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().
|
||||
std::vector<typename Base::MatrixZD, Eigen::aligned_allocator<typename Base::MatrixZD> > Fblocks;
|
||||
typename Base::FBlocks Fs;
|
||||
Matrix E;
|
||||
Vector b;
|
||||
computeJacobiansWithTriangulatedPoint(Fblocks, E, b, cameras);
|
||||
computeJacobiansWithTriangulatedPoint(Fs, E, b, cameras);
|
||||
|
||||
// Whiten using noise model
|
||||
Base::whitenJacobians(Fblocks, E, b);
|
||||
Base::whitenJacobians(Fs, E, b);
|
||||
|
||||
// build augmented hessian
|
||||
SymmetricBlockMatrix augmentedHessian = //
|
||||
Cameras::SchurComplement(Fblocks, E, b, lambda, diagonalDamping);
|
||||
SymmetricBlockMatrix augmentedHessian = //
|
||||
Cameras::SchurComplement(Fs, E, b, lambda, diagonalDamping);
|
||||
|
||||
return boost::make_shared<RegularHessianFactor<Base::Dim> >(this->keys_,
|
||||
augmentedHessian);
|
||||
return boost::make_shared<RegularHessianFactor<Base::Dim> >(
|
||||
this->keys_, augmentedHessian);
|
||||
}
|
||||
|
||||
// create factor
|
||||
// Create RegularImplicitSchurFactor factor.
|
||||
boost::shared_ptr<RegularImplicitSchurFactor<CAMERA> > createRegularImplicitSchurFactor(
|
||||
const Cameras& cameras, double lambda) const {
|
||||
if (triangulateForLinearize(cameras))
|
||||
|
|
@ -226,7 +246,7 @@ public:
|
|||
return boost::shared_ptr<RegularImplicitSchurFactor<CAMERA> >();
|
||||
}
|
||||
|
||||
/// create factor
|
||||
/// Create JacobianFactorQ factor.
|
||||
boost::shared_ptr<JacobianFactorQ<Base::Dim, 2> > createJacobianQFactor(
|
||||
const Cameras& cameras, double lambda) const {
|
||||
if (triangulateForLinearize(cameras))
|
||||
|
|
@ -236,13 +256,13 @@ public:
|
|||
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(
|
||||
const Values& values, double lambda) const {
|
||||
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(
|
||||
const Cameras& cameras, double lambda) const {
|
||||
if (triangulateForLinearize(cameras))
|
||||
|
|
@ -252,19 +272,19 @@ public:
|
|||
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(
|
||||
const Values& values, double lambda = 0.0) const {
|
||||
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(
|
||||
const Values& values, double lambda = 0.0) const {
|
||||
return createRegularImplicitSchurFactor(this->cameras(values), lambda);
|
||||
}
|
||||
|
||||
/// linearize to a JacobianfactorQ
|
||||
/// Linearize to a JacobianfactorQ.
|
||||
virtual boost::shared_ptr<JacobianFactorQ<Base::Dim, 2> > linearizeToJacobian(
|
||||
const Values& values, double lambda = 0.0) const {
|
||||
return createJacobianQFactor(this->cameras(values), lambda);
|
||||
|
|
@ -334,7 +354,7 @@ public:
|
|||
/// Assumes the point has been computed
|
||||
/// Note E can be 2m*3 or 2m*2, in case point is degenerate
|
||||
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 {
|
||||
|
||||
if (!result_) {
|
||||
|
|
@ -342,32 +362,32 @@ public:
|
|||
// TODO check flag whether we should do this
|
||||
Unit3 backProjected = cameras[0].backprojectPointAtInfinity(
|
||||
this->measured_.at(0));
|
||||
Base::computeJacobians(Fblocks, E, b, cameras, backProjected);
|
||||
Base::computeJacobians(Fs, E, b, cameras, backProjected);
|
||||
} else {
|
||||
// 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
|
||||
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 {
|
||||
Cameras cameras = this->cameras(values);
|
||||
bool nonDegenerate = triangulateForLinearize(cameras);
|
||||
if (nonDegenerate)
|
||||
computeJacobiansWithTriangulatedPoint(Fblocks, E, b, cameras);
|
||||
computeJacobiansWithTriangulatedPoint(Fs, E, b, cameras);
|
||||
return nonDegenerate;
|
||||
}
|
||||
|
||||
/// takes values
|
||||
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 {
|
||||
Cameras cameras = this->cameras(values);
|
||||
bool nonDegenerate = triangulateForLinearize(cameras);
|
||||
if (nonDegenerate)
|
||||
Base::computeJacobiansSVD(Fblocks, Enull, b, cameras, *result_);
|
||||
Base::computeJacobiansSVD(Fs, Enull, b, cameras, *result_);
|
||||
return nonDegenerate;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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,
|
||||
boost::optional<typename Cameras::FBlocks&> Fs = boost::none,
|
||||
boost::optional<Matrix&> E = boost::none) const override
|
||||
{
|
||||
void correctForMissingMeasurements(
|
||||
const Cameras& cameras, Vector& ue,
|
||||
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:
|
||||
for(size_t i=0; i < cameras.size(); i++){
|
||||
for (size_t i = 0; i < cameras.size(); 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);
|
||||
for(size_t ii=0; ii<Dim; ii++)
|
||||
Fi(1,ii) = 0.0;
|
||||
for (size_t ii = 0; ii < Dim; ii++) 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());
|
||||
|
||||
// set the corresponding entry of vector ue to zero
|
||||
|
|
|
|||
|
|
@ -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).
|
||||
* Each camera may have its own extrinsic 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).
|
||||
* This factor optimizes the pose of the body as well as the extrinsic camera
|
||||
* calibration (pose of camera wrt body). Each camera may have its own extrinsic
|
||||
* 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
|
||||
*/
|
||||
class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
|
||||
|
|
|
|||
Loading…
Reference in New Issue