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

Removed dividers, added empty comments

release/4.3a0
dellaert 2015-02-19 01:19:14 +01:00
parent fd7411c68c
commit 3f437c448b
1 changed files with 66 additions and 49 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

115
gtsam/slam/SmartFactorBase.h
View File

@ -35,20 +35,26 @@
#include <vector> #include <vector>
namespace gtsam { namespace gtsam {
/// Base class with no internal point, completely functional
/**
* @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.
*/
template<class POSE, class Z, class CAMERA, size_t D> template<class POSE, class Z, class CAMERA, size_t D>
class SmartFactorBase: public NonlinearFactor { class SmartFactorBase: public NonlinearFactor {
protected: protected:
// Keep a copy of measurement and calibration for I/O // Keep a copy of measurement for I/O
std::vector<Z> measured_; ///< 2D measurement for each of the m views std::vector<Z> measured_; ///< 2D measurement for each of the m views
std::vector<SharedNoiseModel> noise_; ///< noise model used
///< (important that the order is the same as the keys that we use to create the factor) ///< (important that the order is the same as the keys that we use to create the factor)
std::vector<SharedNoiseModel> noise_; ///< noise model used
boost::optional<POSE> body_P_sensor_; ///< The pose of the sensor in the body frame (one for all cameras) boost::optional<POSE> body_P_sensor_; ///< The pose of the sensor in the body frame (one for all cameras)
static const int ZDim = traits<Z>::dimension; ///< Measurement dimension static const int ZDim = traits<Z>::dimension; ///< Measurement dimension
/// Definitions for blocks of F /// Definitions for blocks of F
typedef Eigen::Matrix<double, ZDim, D> Matrix2D; // F typedef Eigen::Matrix<double, ZDim, D> Matrix2D; // F
@ -65,11 +71,8 @@ protected:
/// shorthand for this class /// shorthand for this class
typedef SmartFactorBase<POSE, Z, CAMERA, D> This; typedef SmartFactorBase<POSE, Z, CAMERA, D> This;
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/// shorthand for a smart pointer to a factor /// shorthand for a smart pointer to a factor
@ -107,7 +110,6 @@ public:
/** /**
* variant of the previous add: adds a bunch of measurements, together with the camera keys and noises * variant of the previous add: adds a bunch of measurements, together with the camera keys and noises
*/ */
// ****************************************************************************************************
void add(std::vector<Z>& measurements, std::vector<Key>& poseKeys, void add(std::vector<Z>& measurements, std::vector<Key>& poseKeys,
std::vector<SharedNoiseModel>& noises) { std::vector<SharedNoiseModel>& noises) {
for (size_t i = 0; i < measurements.size(); i++) { for (size_t i = 0; i < measurements.size(); i++) {
@ -120,7 +122,6 @@ public:
/** /**
* variant of the previous add: adds a bunch of measurements and uses the same noise model for all of them * variant of the previous add: adds a bunch of measurements and uses the same noise model for all of them
*/ */
// ****************************************************************************************************
void add(std::vector<Z>& measurements, std::vector<Key>& poseKeys, void add(std::vector<Z>& measurements, std::vector<Key>& poseKeys,
const SharedNoiseModel& noise) { const SharedNoiseModel& noise) {
for (size_t i = 0; i < measurements.size(); i++) { for (size_t i = 0; i < measurements.size(); i++) {
@ -134,7 +135,6 @@ public:
* Adds an entire SfM_track (collection of cameras observing a single point). * Adds 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, const SharedNoiseModel& noise) { void add(const SFM_TRACK& trackToAdd, const SharedNoiseModel& noise) {
for (size_t k = 0; k < trackToAdd.number_measurements(); k++) { for (size_t k = 0; k < trackToAdd.number_measurements(); k++) {
@ -187,8 +187,7 @@ public:
&& body_P_sensor_->equals(*e->body_P_sensor_))); && body_P_sensor_->equals(*e->body_P_sensor_)));
} }
// **************************************************************************************************** /// Calculate vector of re-projection errors, before applying noise model
// /// Calculate vector of re-projection errors, before applying noise model
Vector reprojectionError(const Cameras& cameras, const Point3& point) const { Vector reprojectionError(const Cameras& cameras, const Point3& point) const {
Vector b = zero(ZDim * cameras.size()); Vector b = zero(ZDim * cameras.size());
@ -210,7 +209,6 @@ public:
return b; return b;
} }
// ****************************************************************************************************
/** /**
* Calculate the error of the factor. * 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. * This is the log-likelihood, e.g. \f$ 0.5(h(x)-z)^2/\sigma^2 \f$ in case of Gaussian.
@ -229,7 +227,7 @@ public:
try { try {
Z reprojectionError(camera.project(point) - zi); Z reprojectionError(camera.project(point) - zi);
overallError += 0.5 overallError += 0.5
* this->noise_.at(i)->distance(reprojectionError.vector()); * this->noise_.at(i)->distance(reprojectionError.vector());
} catch (CheiralityException&) { } catch (CheiralityException&) {
std::cout << "Cheirality exception " << std::endl; std::cout << "Cheirality exception " << std::endl;
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
@ -239,7 +237,6 @@ public:
return overallError; return overallError;
} }
// ****************************************************************************************************
/// Assumes non-degenerate ! /// Assumes non-degenerate !
void computeEP(Matrix& E, Matrix& PointCov, const Cameras& cameras, void computeEP(Matrix& E, Matrix& PointCov, const Cameras& cameras,
const Point3& point) const { const Point3& point) const {
@ -264,9 +261,10 @@ public:
PointCov.noalias() = (E.transpose() * E).inverse(); PointCov.noalias() = (E.transpose() * E).inverse();
} }
// **************************************************************************************************** /**
/// Compute F, E only (called below in both vanilla and SVD versions) * Compute F, E only (called below in both vanilla and SVD versions)
/// Given a Point3, assumes dimensionality is 3 * Given a Point3, assumes dimensionality is 3
*/
double computeJacobians(std::vector<KeyMatrix2D>& Fblocks, Matrix& E, double computeJacobians(std::vector<KeyMatrix2D>& Fblocks, Matrix& E,
Vector& b, const Cameras& cameras, const Point3& point) const { Vector& b, const Cameras& cameras, const Point3& point) const {
@ -280,9 +278,11 @@ public:
Vector bi; Vector bi;
try { try {
bi = -(cameras[i].project(point, Fi, Ei, Hcali) - this->measured_.at(i)).vector(); bi =
if(body_P_sensor_){ -(cameras[i].project(point, Fi, Ei, Hcali) - this->measured_.at(i)).vector();
Pose3 w_Pose_body = (cameras[i].pose()).compose(body_P_sensor_->inverse()); if (body_P_sensor_) {
Pose3 w_Pose_body = (cameras[i].pose()).compose(
body_P_sensor_->inverse());
Matrix J(6, 6); Matrix J(6, 6);
Pose3 world_P_body = w_Pose_body.compose(*body_P_sensor_, J); Pose3 world_P_body = w_Pose_body.compose(*body_P_sensor_, J);
Fi = Fi * J; Fi = Fi * J;
@ -307,7 +307,6 @@ public:
return f; return f;
} }
// ****************************************************************************************************
/// Version that computes PointCov, with optional lambda parameter /// Version that computes PointCov, with optional lambda parameter
double computeJacobians(std::vector<KeyMatrix2D>& Fblocks, Matrix& E, double computeJacobians(std::vector<KeyMatrix2D>& Fblocks, Matrix& E,
Matrix3& PointCov, Vector& b, const Cameras& cameras, const Point3& point, Matrix3& PointCov, Vector& b, const Cameras& cameras, const Point3& point,
@ -322,7 +321,7 @@ public:
EtE(0, 0) += lambda * EtE(0, 0); EtE(0, 0) += lambda * EtE(0, 0);
EtE(1, 1) += lambda * EtE(1, 1); EtE(1, 1) += lambda * EtE(1, 1);
EtE(2, 2) += lambda * EtE(2, 2); EtE(2, 2) += lambda * EtE(2, 2);
}else{ } else {
EtE(0, 0) += lambda; EtE(0, 0) += lambda;
EtE(1, 1) += lambda; EtE(1, 1) += lambda;
EtE(2, 2) += lambda; EtE(2, 2) += lambda;
@ -333,7 +332,6 @@ public:
return f; return f;
} }
// ****************************************************************************************************
// TODO, there should not be a Matrix version, really // TODO, there should not be a Matrix version, really
double computeJacobians(Matrix& F, Matrix& E, Matrix3& PointCov, Vector& b, double computeJacobians(Matrix& F, Matrix& E, Matrix3& PointCov, Vector& b,
const Cameras& cameras, const Point3& point, const Cameras& cameras, const Point3& point,
@ -351,7 +349,6 @@ public:
return f; return f;
} }
// ****************************************************************************************************
/// SVD version /// SVD version
double computeJacobiansSVD(std::vector<KeyMatrix2D>& Fblocks, Matrix& Enull, double computeJacobiansSVD(std::vector<KeyMatrix2D>& Fblocks, Matrix& Enull,
Vector& b, const Cameras& cameras, const Point3& point, double lambda = Vector& b, const Cameras& cameras, const Point3& point, double lambda =
@ -372,7 +369,6 @@ public:
return f; return f;
} }
// ****************************************************************************************************
/// Matrix version of SVD /// Matrix version of SVD
// TODO, there should not be a Matrix version, really // TODO, there should not be a Matrix version, really
double computeJacobiansSVD(Matrix& F, Matrix& Enull, Vector& b, double computeJacobiansSVD(Matrix& F, Matrix& Enull, Vector& b,
@ -390,7 +386,6 @@ public:
return f; return f;
} }
// ****************************************************************************************************
/// linearize returns a Hessianfactor that is an approximation of error(p) /// linearize returns a Hessianfactor that is an approximation of error(p)
boost::shared_ptr<RegularHessianFactor<D> > createHessianFactor( boost::shared_ptr<RegularHessianFactor<D> > createHessianFactor(
const Cameras& cameras, const Point3& point, const double lambda = 0.0, const Cameras& cameras, const Point3& point, const double lambda = 0.0,
@ -433,8 +428,9 @@ public:
#endif #endif
} }
// **************************************************************************************************** /**
// slow version - works on full (sparse) matrices * slow version - works on full (sparse) matrices
*/
void schurComplement(const std::vector<KeyMatrix2D>& Fblocks, const Matrix& E, void schurComplement(const std::vector<KeyMatrix2D>& Fblocks, const Matrix& E,
const Matrix& PointCov, const Vector& b, const Matrix& PointCov, const Vector& b,
/*output ->*/std::vector<Matrix>& Gs, std::vector<Vector>& gs) const { /*output ->*/std::vector<Matrix>& Gs, std::vector<Vector>& gs) const {
@ -490,7 +486,8 @@ public:
// D = (Dx2) * (2) // D = (Dx2) * (2)
// (augmentedHessian.matrix()).block<D,1> (i1,numKeys+1) = Fi1.transpose() * b.segment < 2 > (2 * i1); // F' * b // (augmentedHessian.matrix()).block<D,1> (i1,numKeys+1) = Fi1.transpose() * b.segment < 2 > (2 * i1); // F' * b
augmentedHessian(i1, numKeys) = Fi1.transpose() * b.segment<ZDim>(ZDim * i1) // F' * b augmentedHessian(i1, numKeys) = Fi1.transpose()
* b.segment<ZDim>(ZDim * i1) // F' * b
- Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (DxZDim) * (ZDimx3) * (3*ZDimm) * (ZDimm x 1) - Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (DxZDim) * (ZDimx3) * (3*ZDimm) * (ZDimm x 1)
// (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) ) // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) )
@ -508,7 +505,9 @@ public:
} // end of for over cameras } // end of for over cameras
} }
// **************************************************************************************************** /**
*
*/
void sparseSchurComplement(const std::vector<KeyMatrix2D>& Fblocks, void sparseSchurComplement(const std::vector<KeyMatrix2D>& Fblocks,
const Matrix& E, const Matrix& P /*Point Covariance*/, const Vector& b, const Matrix& E, const Matrix& P /*Point Covariance*/, const Vector& b,
/*output ->*/std::vector<Matrix>& Gs, std::vector<Vector>& gs) const { /*output ->*/std::vector<Matrix>& Gs, std::vector<Vector>& gs) const {
@ -535,7 +534,7 @@ public:
{ // for i1 = i2 { // for i1 = i2
// D = (Dx2) * (2) // D = (Dx2) * (2)
gs.at(i1) = Fi1.transpose() * b.segment<ZDim>(ZDim * i1) // F' * b gs.at(i1) = Fi1.transpose() * b.segment<ZDim>(ZDim * i1) // F' * b
-Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (DxZDim) * (ZDimx3) * (3*ZDimm) * (ZDimm x 1) - Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (DxZDim) * (ZDimx3) * (3*ZDimm) * (ZDimm x 1)
// (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) ) // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) )
Gs.at(GsIndex) = Fi1.transpose() Gs.at(GsIndex) = Fi1.transpose()
@ -554,7 +553,9 @@ public:
} // end of for over cameras } // end of for over cameras
} }
// **************************************************************************************************** /**
*
*/
void updateAugmentedHessian(const Cameras& cameras, const Point3& point, void updateAugmentedHessian(const Cameras& cameras, const Point3& point,
const double lambda, bool diagonalDamping, const double lambda, bool diagonalDamping,
SymmetricBlockMatrix& augmentedHessian, SymmetricBlockMatrix& augmentedHessian,
@ -569,10 +570,13 @@ public:
double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda, double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda,
diagonalDamping); diagonalDamping);
updateSparseSchurComplement(Fblocks, E, PointCov, b, f, allKeys, augmentedHessian); // augmentedHessian.matrix().block<D,D> (i1,i2) = ... updateSparseSchurComplement(Fblocks, E, PointCov, b, f, allKeys,
augmentedHessian); // augmentedHessian.matrix().block<D,D> (i1,i2) = ...
} }
// **************************************************************************************************** /**
*
*/
void updateSparseSchurComplement(const std::vector<KeyMatrix2D>& Fblocks, void updateSparseSchurComplement(const std::vector<KeyMatrix2D>& Fblocks,
const Matrix& E, const Matrix& P /*Point Covariance*/, const Vector& b, const Matrix& E, const Matrix& P /*Point Covariance*/, const Vector& b,
const double f, const FastVector<Key> allKeys, const double f, const FastVector<Key> allKeys,
@ -584,9 +588,9 @@ public:
MatrixDD matrixBlock; MatrixDD matrixBlock;
typedef SymmetricBlockMatrix::Block Block; ///< A block from the Hessian matrix typedef SymmetricBlockMatrix::Block Block; ///< A block from the Hessian matrix
FastMap<Key,size_t> KeySlotMap; FastMap<Key, size_t> KeySlotMap;
for (size_t slot=0; slot < allKeys.size(); slot++) for (size_t slot = 0; slot < allKeys.size(); slot++)
KeySlotMap.insert(std::make_pair(allKeys[slot],slot)); KeySlotMap.insert(std::make_pair(allKeys[slot], slot));
// a single point is observed in numKeys cameras // a single point is observed in numKeys cameras
size_t numKeys = this->keys_.size(); // cameras observing current point size_t numKeys = this->keys_.size(); // cameras observing current point
@ -607,7 +611,8 @@ public:
// information vector - store previous vector // information vector - store previous vector
// vectorBlock = augmentedHessian(aug_i1, aug_numKeys).knownOffDiagonal(); // vectorBlock = augmentedHessian(aug_i1, aug_numKeys).knownOffDiagonal();
// add contribution of current factor // add contribution of current factor
augmentedHessian(aug_i1, aug_numKeys) = augmentedHessian(aug_i1, aug_numKeys).knownOffDiagonal() augmentedHessian(aug_i1, aug_numKeys) = augmentedHessian(aug_i1,
aug_numKeys).knownOffDiagonal()
+ Fi1.transpose() * b.segment<ZDim>(ZDim * i1) // F' * b + Fi1.transpose() * b.segment<ZDim>(ZDim * i1) // F' * b
- Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (DxZDim) * (ZDimx3) * (3*ZDimm) * (ZDimm x 1) - Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (DxZDim) * (ZDimx3) * (3*ZDimm) * (ZDimm x 1)
@ -615,8 +620,11 @@ public:
// main block diagonal - store previous block // main block diagonal - store previous block
matrixBlock = augmentedHessian(aug_i1, aug_i1); matrixBlock = augmentedHessian(aug_i1, aug_i1);
// add contribution of current factor // add contribution of current factor
augmentedHessian(aug_i1, aug_i1) = matrixBlock + augmentedHessian(aug_i1, aug_i1) =
( Fi1.transpose() * (Fi1 - Ei1_P * E.block<ZDim, 3>(ZDim * i1, 0).transpose() * Fi1) ); matrixBlock
+ (Fi1.transpose()
* (Fi1
- Ei1_P * E.block<ZDim, 3>(ZDim * i1, 0).transpose() * Fi1));
// upper triangular part of the hessian // upper triangular part of the hessian
for (size_t i2 = i1 + 1; i2 < numKeys; i2++) { // for each camera for (size_t i2 = i1 + 1; i2 < numKeys; i2++) { // for each camera
@ -629,15 +637,19 @@ public:
// off diagonal block - store previous block // off diagonal block - store previous block
// matrixBlock = augmentedHessian(aug_i1, aug_i2).knownOffDiagonal(); // matrixBlock = augmentedHessian(aug_i1, aug_i2).knownOffDiagonal();
// add contribution of current factor // add contribution of current factor
augmentedHessian(aug_i1, aug_i2) = augmentedHessian(aug_i1, aug_i2).knownOffDiagonal() augmentedHessian(aug_i1, aug_i2) =
- Fi1.transpose() * (Ei1_P * E.block<ZDim, 3>(ZDim * i2, 0).transpose() * Fi2); augmentedHessian(aug_i1, aug_i2).knownOffDiagonal()
- Fi1.transpose()
* (Ei1_P * E.block<ZDim, 3>(ZDim * i2, 0).transpose() * Fi2);
} }
} // end of for over cameras } // end of for over cameras
augmentedHessian(aug_numKeys, aug_numKeys)(0, 0) += f; augmentedHessian(aug_numKeys, aug_numKeys)(0, 0) += f;
} }
// **************************************************************************************************** /**
*
*/
boost::shared_ptr<RegularImplicitSchurFactor<D> > createRegularImplicitSchurFactor( boost::shared_ptr<RegularImplicitSchurFactor<D> > createRegularImplicitSchurFactor(
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 {
@ -649,7 +661,9 @@ public:
return f; return f;
} }
// **************************************************************************************************** /**
*
*/
boost::shared_ptr<JacobianFactorQ<D, ZDim> > createJacobianQFactor( boost::shared_ptr<JacobianFactorQ<D, 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 {
@ -659,18 +673,21 @@ public:
Vector b; Vector b;
computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda, computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda,
diagonalDamping); diagonalDamping);
return boost::make_shared<JacobianFactorQ<D, ZDim> >(Fblocks, E, PointCov, b); return boost::make_shared<JacobianFactorQ<D, ZDim> >(Fblocks, E, PointCov,
b);
} }
// **************************************************************************************************** /**
*
*/
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 {
size_t numKeys = this->keys_.size(); size_t numKeys = this->keys_.size();
std::vector<KeyMatrix2D> Fblocks; std::vector<KeyMatrix2D> Fblocks;
Vector b; Vector b;
Matrix Enull(ZDim*numKeys, ZDim*numKeys-3); Matrix Enull(ZDim * numKeys, ZDim * numKeys - 3);
computeJacobiansSVD(Fblocks, Enull, b, cameras, point, lambda); computeJacobiansSVD(Fblocks, Enull, b, cameras, point, lambda);
return boost::make_shared< JacobianFactorSVD<6, ZDim> >(Fblocks, Enull, b); return boost::make_shared<JacobianFactorSVD<6, ZDim> >(Fblocks, Enull, b);
} }
private: private: