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

80
gtsam/slam/RegularImplicitSchurFactor.h
View File

@ -30,23 +30,10 @@ protected:
typedef Eigen::Matrix<double, D, D> MatrixDD; ///< camera hessian
typedef std::pair<Key, Matrix2D> KeyMatrix2D; ///< named F block
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)
Matrix E_; ///< The 2m*3 E Jacobian with respect to the point
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();
}
const std::vector<KeyMatrix2D> Fblocks_; ///< All 2*D F blocks (one for each camera)
const Matrix3 PointCovariance_; ///< the 3*3 matrix P = inv(E'E) (2*2 if degenerate)
const Matrix E_; ///< The 2m*3 E Jacobian with respect to the point
const Vector b_; ///< 2m-dimensional RHS vector
/// initialize keys from Fblocks
void initKeys() {
@ -55,36 +42,42 @@ public:
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
virtual ~RegularImplicitSchurFactor() {
}
// Write access, only use for construction!
inline std::vector<KeyMatrix2D>& Fblocks() {
inline std::vector<KeyMatrix2D>& Fblocks() const {
return Fblocks_;
}
inline Matrix3& PointCovariance() {
return PointCovariance_;
}
inline Matrix& E() {
inline const Matrix& E() const {
return E_;
}
inline Vector& b() {
inline const Vector& b() const {
return b_;
}
/// Get matrix P
inline const Matrix3& getPointCovariance() const {
return PointCovariance_;
}
/// print
void print(const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
void print(const std::string& s = "", const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const {
std::cout << " RegularImplicitSchurFactor " << std::endl;
Factor::print(s);
for (size_t pos = 0; pos < size(); ++pos) {
@ -101,9 +94,13 @@ public:
if (!f)
return false;
for (size_t pos = 0; pos < size(); ++pos) {
if (keys_[pos] != f->keys_[pos]) 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;
if (keys_[pos] != f->keys_[pos])
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)
&& equal_with_abs_tol(E_, f->E_, tol)
@ -121,7 +118,8 @@ public:
return Matrix();
}
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());
}
virtual Matrix augmentedInformation() const {
@ -205,7 +203,8 @@ public:
// - FtE * PointCovariance_ * FtE.transpose();
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 :-(
// static const Eigen::Matrix<double, 2, 2> I2 = eye(2);
@ -219,7 +218,8 @@ public:
virtual GaussianFactor::shared_ptr clone() const {
return boost::make_shared<RegularImplicitSchurFactor<D> >(Fblocks_,
PointCovariance_, E_, b_);
throw std::runtime_error("RegularImplicitSchurFactor::clone non implemented");
throw std::runtime_error(
"RegularImplicitSchurFactor::clone non implemented");
}
virtual bool empty() const {
return false;
@ -228,7 +228,8 @@ public:
virtual GaussianFactor::shared_ptr negate() const {
return boost::make_shared<RegularImplicitSchurFactor<D> >(Fblocks_,
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
@ -254,7 +255,8 @@ public:
Vector3 d1;
d1.setZero();
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
Vector3 d2 = PointCovariance_ * d1;
@ -462,10 +464,10 @@ public:
/// Gradient wrt a key at any values
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

22
gtsam/slam/SmartFactorBase.h
View File

@ -657,12 +657,16 @@ public:
boost::shared_ptr<RegularImplicitSchurFactor<Dim> > createRegularImplicitSchurFactor(
const Cameras& cameras, const Point3& point, double lambda = 0.0,
bool diagonalDamping = false) const {
typename boost::shared_ptr<RegularImplicitSchurFactor<Dim> > f(
new RegularImplicitSchurFactor<Dim>());
computeJacobians(f->Fblocks(), f->E(), f->b(), cameras, point);
f->PointCovariance() = PointCov(f->E(), lambda, diagonalDamping);
f->initKeys();
return f;
std::vector<KeyMatrix2D> F;
Matrix E;
Vector b;
computeJacobians(F, E, b, cameras, point);
noiseModel_->WhitenSystem(E,b);
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;
computeJacobians(Fblocks, E, b, cameras, point);
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,7 +695,8 @@ public:
Vector b;
Matrix Enull(ZDim * numKeys, ZDim * numKeys - 3);
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: