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

Made RegularImplicitSchurFactor fully functional, and whitened again.

release/4.3a0
dellaert 2015-02-25 14:00:21 +01:00
parent d7b5156dcc
commit f7292488c4
2 changed files with 73 additions and 65 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

114
gtsam/slam/RegularImplicitSchurFactor.h
View File

@ -30,23 +30,10 @@ protected:
typedef Eigen::Matrix<double, D, D> MatrixDD; ///< camera hessian typedef Eigen::Matrix<double, D, D> MatrixDD; ///< camera hessian
typedef std::pair<Key, Matrix2D> KeyMatrix2D; ///< named F block typedef std::pair<Key, Matrix2D> KeyMatrix2D; ///< named F block
std::vector<KeyMatrix2D> Fblocks_; ///< All 2*D F blocks (one for each camera) const std::vector<KeyMatrix2D> Fblocks_; ///< All 2*D F blocks (one for each camera)
Matrix3 PointCovariance_; ///< the 3*3 matrix P = inv(E'E) (2*2 if degenerate) const Matrix3 PointCovariance_; ///< the 3*3 matrix P = inv(E'E) (2*2 if degenerate)
Matrix E_; ///< The 2m*3 E Jacobian with respect to the point const Matrix E_; ///< The 2m*3 E Jacobian with respect to the point
Vector b_; ///< 2m-dimensional RHS vector const Vector b_; ///< 2m-dimensional RHS vector
public:
/// Constructor
RegularImplicitSchurFactor() {
}
/// Construct from blcoks of F, E, inv(E'*E), and RHS vector b
RegularImplicitSchurFactor(const std::vector<KeyMatrix2D>& Fblocks, const Matrix& E,
const Matrix3& P, const Vector& b) :
Fblocks_(Fblocks), PointCovariance_(P), E_(E), b_(b) {
initKeys();
}
/// initialize keys from Fblocks /// initialize keys from Fblocks
void initKeys() { void initKeys() {
@ -55,36 +42,42 @@ public:
keys_.push_back(it.first); keys_.push_back(it.first);
} }
public:
/// Constructor
RegularImplicitSchurFactor() {
}
/// Construct from blocks of F, E, inv(E'*E), and RHS vector b
RegularImplicitSchurFactor(const std::vector<KeyMatrix2D>& Fblocks,
const Matrix& E, const Matrix3& P, const Vector& b) :
Fblocks_(Fblocks), PointCovariance_(P), E_(E), b_(b) {
initKeys();
}
/// Destructor /// Destructor
virtual ~RegularImplicitSchurFactor() { virtual ~RegularImplicitSchurFactor() {
} }
// Write access, only use for construction! inline std::vector<KeyMatrix2D>& Fblocks() const {
inline std::vector<KeyMatrix2D>& Fblocks() {
return Fblocks_; return Fblocks_;
} }
inline Matrix3& PointCovariance() { inline const Matrix& E() const {
return PointCovariance_;
}
inline Matrix& E() {
return E_; return E_;
} }
inline Vector& b() { inline const Vector& b() const {
return b_; return b_;
} }
/// Get matrix P
inline const Matrix3& getPointCovariance() const { inline const Matrix3& getPointCovariance() const {
return PointCovariance_; return PointCovariance_;
} }
/// print /// print
void print(const std::string& s = "", void print(const std::string& s = "", const KeyFormatter& keyFormatter =
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { DefaultKeyFormatter) const {
std::cout << " RegularImplicitSchurFactor " << std::endl; std::cout << " RegularImplicitSchurFactor " << std::endl;
Factor::print(s); Factor::print(s);
for (size_t pos = 0; pos < size(); ++pos) { for (size_t pos = 0; pos < size(); ++pos) {
@ -101,9 +94,13 @@ public:
if (!f) if (!f)
return false; return false;
for (size_t pos = 0; pos < size(); ++pos) { for (size_t pos = 0; pos < size(); ++pos) {
if (keys_[pos] != f->keys_[pos]) return false; if (keys_[pos] != f->keys_[pos])
if (Fblocks_[pos].first != f->Fblocks_[pos].first) return false; return false;
if (!equal_with_abs_tol(Fblocks_[pos].second,f->Fblocks_[pos].second,tol)) return false; if (Fblocks_[pos].first != f->Fblocks_[pos].first)
return false;
if (!equal_with_abs_tol(Fblocks_[pos].second, f->Fblocks_[pos].second,
tol))
return false;
} }
return equal_with_abs_tol(PointCovariance_, f->PointCovariance_, tol) return equal_with_abs_tol(PointCovariance_, f->PointCovariance_, tol)
&& equal_with_abs_tol(E_, f->E_, tol) && equal_with_abs_tol(E_, f->E_, tol)
@ -121,7 +118,8 @@ public:
return Matrix(); return Matrix();
} }
virtual std::pair<Matrix, Vector> jacobian() const { virtual std::pair<Matrix, Vector> jacobian() const {
throw std::runtime_error("RegularImplicitSchurFactor::jacobian non implemented"); throw std::runtime_error(
"RegularImplicitSchurFactor::jacobian non implemented");
return std::make_pair(Matrix(), Vector()); return std::make_pair(Matrix(), Vector());
} }
virtual Matrix augmentedInformation() const { virtual Matrix augmentedInformation() const {
@ -146,7 +144,7 @@ public:
// Calculate Fj'*Ej for the current camera (observing a single point) // Calculate Fj'*Ej for the current camera (observing a single point)
// D x 3 = (D x 2) * (2 x 3) // D x 3 = (D x 2) * (2 x 3)
const Matrix2D& Fj = Fblocks_[pos].second; const Matrix2D& Fj = Fblocks_[pos].second;
Eigen::Matrix<double, D, 3> FtE = Fj.transpose() Eigen::Matrix<double, D, 3> FtE = Fj.transpose()
* E_.block<2, 3>(2 * pos, 0); * E_.block<2, 3>(2 * pos, 0);
Eigen::Matrix<double, D, 1> dj; Eigen::Matrix<double, D, 1> dj;
@ -205,7 +203,8 @@ public:
// - FtE * PointCovariance_ * FtE.transpose(); // - FtE * PointCovariance_ * FtE.transpose();
const Matrix23& Ej = E_.block<2, 3>(2 * pos, 0); const Matrix23& Ej = E_.block<2, 3>(2 * pos, 0);
blocks[j] = Fj.transpose() * (Fj - Ej * PointCovariance_ * Ej.transpose() * Fj); blocks[j] = Fj.transpose()
* (Fj - Ej * PointCovariance_ * Ej.transpose() * Fj);
// F'*(I - E*P*E')*F, TODO: this should work, but it does not :-( // F'*(I - E*P*E')*F, TODO: this should work, but it does not :-(
// static const Eigen::Matrix<double, 2, 2> I2 = eye(2); // static const Eigen::Matrix<double, 2, 2> I2 = eye(2);
@ -219,7 +218,8 @@ public:
virtual GaussianFactor::shared_ptr clone() const { virtual GaussianFactor::shared_ptr clone() const {
return boost::make_shared<RegularImplicitSchurFactor<D> >(Fblocks_, return boost::make_shared<RegularImplicitSchurFactor<D> >(Fblocks_,
PointCovariance_, E_, b_); PointCovariance_, E_, b_);
throw std::runtime_error("RegularImplicitSchurFactor::clone non implemented"); throw std::runtime_error(
"RegularImplicitSchurFactor::clone non implemented");
} }
virtual bool empty() const { virtual bool empty() const {
return false; return false;
@ -228,7 +228,8 @@ public:
virtual GaussianFactor::shared_ptr negate() const { virtual GaussianFactor::shared_ptr negate() const {
return boost::make_shared<RegularImplicitSchurFactor<D> >(Fblocks_, return boost::make_shared<RegularImplicitSchurFactor<D> >(Fblocks_,
PointCovariance_, E_, b_); PointCovariance_, E_, b_);
throw std::runtime_error("RegularImplicitSchurFactor::negate non implemented"); throw std::runtime_error(
"RegularImplicitSchurFactor::negate non implemented");
} }
// Raw Vector version of y += F'*alpha*(I - E*P*E')*F*x, for testing // Raw Vector version of y += F'*alpha*(I - E*P*E')*F*x, for testing
@ -254,14 +255,15 @@ public:
Vector3 d1; Vector3 d1;
d1.setZero(); d1.setZero();
for (size_t k = 0; k < size(); k++) for (size_t k = 0; k < size(); k++)
d1 += E_.block < 2, 3 > (2 * k, 0).transpose() * (e1[k] - 2 * b_.segment < 2 > (k * 2)); d1 += E_.block<2, 3>(2 * k, 0).transpose()
* (e1[k] - 2 * b_.segment<2>(k * 2));
// d2 = E.transpose() * e1 = (3*2m)*2m // d2 = E.transpose() * e1 = (3*2m)*2m
Vector3 d2 = PointCovariance_ * d1; Vector3 d2 = PointCovariance_ * d1;
// e3 = alpha*(e1 - E*d2) = 1*[2m-(2m*3)*3] // e3 = alpha*(e1 - E*d2) = 1*[2m-(2m*3)*3]
for (size_t k = 0; k < size(); k++) for (size_t k = 0; k < size(); k++)
e2[k] = e1[k] - 2 * b_.segment < 2 > (k * 2) - E_.block < 2, 3 > (2 * k, 0) * d2; e2[k] = e1[k] - 2 * b_.segment<2>(k * 2) - E_.block<2, 3>(2 * k, 0) * d2;
} }
/* /*
@ -303,7 +305,7 @@ public:
// e1 = F * x - b = (2m*dm)*dm // e1 = F * x - b = (2m*dm)*dm
for (size_t k = 0; k < size(); ++k) for (size_t k = 0; k < size(); ++k)
e1[k] = Fblocks_[k].second * x.at(keys_[k]) - b_.segment < 2 > (k * 2); e1[k] = Fblocks_[k].second * x.at(keys_[k]) - b_.segment<2>(k * 2);
projectError(e1, e2); projectError(e1, e2);
double result = 0; double result = 0;
@ -316,21 +318,21 @@ public:
/** /**
* @brief Calculate corrected error Q*e = (I - E*P*E')*e * @brief Calculate corrected error Q*e = (I - E*P*E')*e
*/ */
void projectError(const Error2s& e1, Error2s& e2) const { void projectError(const Error2s& e1, Error2s& e2) const {
// d1 = E.transpose() * e1 = (3*2m)*2m // d1 = E.transpose() * e1 = (3*2m)*2m
Vector3 d1; Vector3 d1;
d1.setZero(); d1.setZero();
for (size_t k = 0; k < size(); k++) for (size_t k = 0; k < size(); k++)
d1 += E_.block < 2, 3 > (2 * k, 0).transpose() * e1[k]; d1 += E_.block<2, 3>(2 * k, 0).transpose() * e1[k];
// d2 = E.transpose() * e1 = (3*2m)*2m // d2 = E.transpose() * e1 = (3*2m)*2m
Vector3 d2 = PointCovariance_ * d1; Vector3 d2 = PointCovariance_ * d1;
// e3 = alpha*(e1 - E*d2) = 1*[2m-(2m*3)*3] // e3 = alpha*(e1 - E*d2) = 1*[2m-(2m*3)*3]
for (size_t k = 0; k < size(); k++) for (size_t k = 0; k < size(); k++)
e2[k] = e1[k] - E_.block < 2, 3 > (2 * k, 0) * d2; e2[k] = e1[k] - E_.block<2, 3>(2 * k, 0) * d2;
} }
/// Scratch space for multiplyHessianAdd /// Scratch space for multiplyHessianAdd
mutable Error2s e1, e2; mutable Error2s e1, e2;
@ -424,7 +426,7 @@ public:
e1.resize(size()); e1.resize(size());
e2.resize(size()); e2.resize(size());
for (size_t k = 0; k < size(); k++) for (size_t k = 0; k < size(); k++)
e1[k] = b_.segment < 2 > (2 * k); e1[k] = b_.segment<2>(2 * k);
projectError(e1, e2); projectError(e1, e2);
// g = F.transpose()*e2 // g = F.transpose()*e2
@ -451,7 +453,7 @@ public:
e1.resize(size()); e1.resize(size());
e2.resize(size()); e2.resize(size());
for (size_t k = 0; k < size(); k++) for (size_t k = 0; k < size(); k++)
e1[k] = b_.segment < 2 > (2 * k); e1[k] = b_.segment<2>(2 * k);
projectError(e1, e2); projectError(e1, e2);
for (size_t k = 0; k < size(); ++k) { // for each camera in the factor for (size_t k = 0; k < size(); ++k) { // for each camera in the factor
@ -462,10 +464,10 @@ public:
/// Gradient wrt a key at any values /// Gradient wrt a key at any values
Vector gradient(Key key, const VectorValues& x) const { Vector gradient(Key key, const VectorValues& x) const {
throw std::runtime_error("gradient for RegularImplicitSchurFactor is not implemented yet"); throw std::runtime_error(
"gradient for RegularImplicitSchurFactor is not implemented yet");
} }
}; };
// end class RegularImplicitSchurFactor // end class RegularImplicitSchurFactor

24
gtsam/slam/SmartFactorBase.h
View File

@ -657,12 +657,16 @@ public:
boost::shared_ptr<RegularImplicitSchurFactor<Dim> > createRegularImplicitSchurFactor( boost::shared_ptr<RegularImplicitSchurFactor<Dim> > 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 {
typename boost::shared_ptr<RegularImplicitSchurFactor<Dim> > f( std::vector<KeyMatrix2D> F;
new RegularImplicitSchurFactor<Dim>()); Matrix E;
computeJacobians(f->Fblocks(), f->E(), f->b(), cameras, point); Vector b;
f->PointCovariance() = PointCov(f->E(), lambda, diagonalDamping); computeJacobians(F, E, b, cameras, point);
f->initKeys(); noiseModel_->WhitenSystem(E,b);
return f; Matrix3 P = PointCov(E, lambda, diagonalDamping);
// TODO make WhitenInPlace work with any dense matrix type
BOOST_FOREACH(KeyMatrix2D& Fblock,F)
Fblock.second = noiseModel_->Whiten(Fblock.second);
return boost::make_shared<RegularImplicitSchurFactor<Dim> >(F, E, P, b);
} }
/** /**
@ -676,7 +680,8 @@ public:
Vector b; Vector b;
computeJacobians(Fblocks, E, b, cameras, point); computeJacobians(Fblocks, E, b, cameras, point);
Matrix3 P = PointCov(E, lambda, diagonalDamping); Matrix3 P = PointCov(E, lambda, diagonalDamping);
return boost::make_shared<JacobianFactorQ<Dim, ZDim> >(Fblocks, E, P, b); return boost::make_shared<JacobianFactorQ<Dim, ZDim> > //
(Fblocks, E, P, b, noiseModel_);
} }
/** /**
@ -690,12 +695,13 @@ public:
Vector b; Vector b;
Matrix Enull(ZDim * numKeys, ZDim * numKeys - 3); Matrix Enull(ZDim * numKeys, ZDim * numKeys - 3);
computeJacobiansSVD(Fblocks, Enull, b, cameras, point); computeJacobiansSVD(Fblocks, Enull, b, cameras, point);
return boost::make_shared<JacobianFactorSVD<Dim, ZDim> >(Fblocks, Enull, b); return boost::make_shared<JacobianFactorSVD<Dim, ZDim> > //
(Fblocks, Enull, b, noiseModel_);
} }
private: private:
/// Serialization function /// Serialization function
friend class boost::serialization::access; friend class boost::serialization::access;
template<class ARCHIVE> template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) { void serialize(ARCHIVE & ar, const unsigned int version) {