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

Migrated to non-keyed Fblocks

release/4.3a0
dellaert 2015-03-04 23:01:47 -08:00
parent d0e0754668
commit 26f2b33c47
6 changed files with 44 additions and 43 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
gtsam/slam/JacobianFactorSVD.h
View File

@ -16,7 +16,6 @@ class JacobianFactorSVD: public RegularJacobianFactor<D> {
typedef RegularJacobianFactor<D> Base;
typedef Eigen::Matrix<double, ZDim, D> MatrixZD; // e.g 2 x 6 with Z=Point2
typedef std::pair<Key, MatrixZD> KeyMatrixZD;
typedef std::pair<Key, Matrix> KeyMatrix;
public:
@ -46,12 +45,13 @@ public:
*
* @Fblocks:
*/
JacobianFactorSVD(const std::vector<KeyMatrixZD>& Fblocks,
const Matrix& Enull, const Vector& b, //
JacobianFactorSVD(const FastVector<Key>& keys,
const std::vector<MatrixZD>& Fblocks, const Matrix& Enull,
const Vector& b, //
const SharedDiagonal& model = SharedDiagonal()) :
Base() {
size_t numKeys = Enull.rows() / ZDim;
size_t j = 0, m2 = ZDim * numKeys - 3;
size_t m2 = ZDim * numKeys - 3;
// PLAIN NULL SPACE TRICK
// Matrix Q = Enull * Enull.transpose();
// BOOST_FOREACH(const KeyMatrixZD& it, Fblocks)
@ -59,11 +59,13 @@ public:
// JacobianFactor factor(QF, Q * b);
std::vector<KeyMatrix> QF;
QF.reserve(numKeys);
BOOST_FOREACH(const KeyMatrixZD& it, Fblocks)
for (size_t k = 0; k < Fblocks.size(); ++k) {
Key key = keys[k];
QF.push_back(
KeyMatrix(it.first,
(Enull.transpose()).block(0, ZDim * j++, m2, ZDim) * it.second));
JacobianFactor::fillTerms(QF, - Enull.transpose() * b, model);
KeyMatrix(key,
(Enull.transpose()).block(0, ZDim * k, m2, ZDim) * Fblocks[k]));
}
JacobianFactor::fillTerms(QF, -Enull.transpose() * b, model);
}
};

14
gtsam/slam/SmartFactorBase.h
View File

@ -281,12 +281,12 @@ public:
* 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
*/
double computeJacobians(std::vector<MatrixZD>& Fblocks, Matrix& E, Vector& b,
const Cameras& cameras, const Point3& point) const {
// Project into Camera set and calculate derivatives
typename Cameras::FBlocks F;
b = reprojectionError(cameras, point, F, E);
b = reprojectionError(cameras, point, Fblocks, E);
return b.squaredNorm();
}
@ -357,8 +357,8 @@ public:
void whitenJacobians(std::vector<MatrixZD>& F, Matrix& E, Vector& b) const {
noiseModel_->WhitenSystem(E, b);
// TODO make WhitenInPlace work with any dense matrix type
BOOST_FOREACH(MatrixZD& Fblock,F)
Fblock.second = noiseModel_->Whiten(Fblock.second);
for (size_t i = 0; i < F.size(); i++)
F[i] = noiseModel_->Whiten(F[i]);
}
/**
@ -390,7 +390,7 @@ public:
const size_t M = b.size();
Matrix3 P = PointCov(E, lambda, diagonalDamping);
SharedIsotropic n = noiseModel::Isotropic::Sigma(M, noiseModel_->sigma());
return boost::make_shared<JacobianFactorQ<Dim, ZDim> >(F, E, P, b, n);
return boost::make_shared<JacobianFactorQ<Dim, ZDim> >(keys_, F, E, P, b, n);
}
/**
@ -407,7 +407,7 @@ public:
computeJacobiansSVD(F, E0, b, cameras, point);
SharedIsotropic n = noiseModel::Isotropic::Sigma(M - 3,
noiseModel_->sigma());
return boost::make_shared<JacobianFactorSVD<Dim, ZDim> >(F, E0, b, n);
return boost::make_shared<JacobianFactorSVD<Dim, ZDim> >(keys_, F, E0, b, n);
}
/// Create BIG block-diagonal matrix F from Fblocks
@ -416,7 +416,7 @@ public:
F.resize(ZDim * m, Dim * m);
F.setZero();
for (size_t i = 0; i < m; ++i)
F.block<This::ZDim, Dim>(This::ZDim * i, Dim * i) = Fblocks.at(i).second;
F.block<ZDim, Dim>(ZDim * i, Dim * i) = Fblocks.at(i);
}
private:

2
gtsam/slam/SmartProjectionCameraFactor.h
View File

@ -112,7 +112,7 @@ public:
}
/// linearize returns a Hessianfactor that is an approximation of error(p)
virtual boost::shared_ptr<RegularImplicitSchurFactor<Dim> > linearizeToImplicit(
virtual boost::shared_ptr<RegularImplicitSchurFactor<Camera> > linearizeToImplicit(
const Values& values, double lambda=0.0) const {
return Base::createRegularImplicitSchurFactor(Base::cameras(values),lambda);
}

21
gtsam/slam/SmartProjectionFactor.h
View File

@ -278,7 +278,7 @@ public:
Vector b;
double f;
{
std::vector<typename Base::KeyMatrix2D> Fblocks;
std::vector<typename Base::MatrixZD> Fblocks;
f = computeJacobiansWithTriangulatedPoint(Fblocks, E, b, cameras);
Base::whitenJacobians(Fblocks, E, b);
Base::FillDiagonalF(Fblocks, F); // expensive !
@ -326,12 +326,12 @@ public:
}
// create factor
boost::shared_ptr<RegularImplicitSchurFactor<Base::Dim> > createRegularImplicitSchurFactor(
boost::shared_ptr<RegularImplicitSchurFactor<CAMERA> > createRegularImplicitSchurFactor(
const Cameras& cameras, double lambda) const {
if (triangulateForLinearize(cameras))
return Base::createRegularImplicitSchurFactor(cameras, *result_, lambda);
else
return boost::shared_ptr<RegularImplicitSchurFactor<Base::Dim> >();
return boost::shared_ptr<RegularImplicitSchurFactor<CAMERA> >();
}
/// create factor
@ -374,7 +374,7 @@ public:
/// Assumes the point has been computed
/// Note E can be 2m*3 or 2m*2, in case point is degenerate
double computeJacobiansWithTriangulatedPoint(
std::vector<typename Base::KeyMatrix2D>& Fblocks, Matrix& E, Vector& b,
std::vector<typename Base::MatrixZD>& Fblocks, Matrix& E, Vector& b,
const Cameras& cameras) const {
if (!result_) {
@ -385,18 +385,15 @@ public:
int m = this->keys_.size();
E = zeros(2 * m, 2);
b = zero(2 * m);
double f = 0;
for (size_t i = 0; i < this->measured_.size(); i++) {
Matrix Fi, Ei;
Vector bi = -(cameras[i].projectPointAtInfinity(*result_, Fi, Ei)
- this->measured_.at(i)).vector();
f += bi.squaredNorm();
Fblocks.push_back(typename Base::KeyMatrix2D(this->keys_[i], Fi));
Fblocks.push_back(Fi);
E.block<2, 2>(2 * i, 0) = Ei;
subInsert(b, bi, 2 * i);
}
return f;
return b.squaredNorm();
} else {
// valid result: just return Base version
return Base::computeJacobians(Fblocks, E, b, cameras, *result_);
@ -405,7 +402,7 @@ public:
/// Version that takes values, and creates the point
bool triangulateAndComputeJacobians(
std::vector<typename Base::KeyMatrix2D>& Fblocks, Matrix& E, Vector& b,
std::vector<typename Base::MatrixZD>& Fblocks, Matrix& E, Vector& b,
const Values& values) const {
Cameras cameras = this->cameras(values);
bool nonDegenerate = triangulateForLinearize(cameras);
@ -416,8 +413,8 @@ public:
/// takes values
bool triangulateAndComputeJacobiansSVD(
std::vector<typename Base::KeyMatrix2D>& Fblocks, Matrix& Enull,
Vector& b, const Values& values) const {
std::vector<typename Base::MatrixZD>& Fblocks, Matrix& Enull, Vector& b,
const Values& values) const {
Cameras cameras = this->cameras(values);
bool nonDegenerate = triangulateForLinearize(cameras);
if (nonDegenerate)

4
gtsam/slam/tests/testSmartProjectionCameraFactor.cpp
View File

@ -772,7 +772,7 @@ TEST( SmartProjectionCameraFactor, computeImplicitJacobian ) {
Point3 point;
if (factor1->point())
point = *(factor1->point());
vector<SmartFactorBundler::KeyMatrix2D> Fblocks;
vector<Matrix29> Fblocks;
factor1->computeJacobians(Fblocks, expectedE, expectedb, cameras, point);
NonlinearFactorGraph generalGraph;
@ -823,7 +823,7 @@ TEST( SmartProjectionCameraFactor, implicitJacobianFactor ) {
implicitFactor->add(level_uv_right, c2, unit2);
GaussianFactor::shared_ptr gaussianImplicitSchurFactor =
implicitFactor->linearize(values);
typedef RegularImplicitSchurFactor<9> Implicit9;
typedef RegularImplicitSchurFactor<Camera> Implicit9;
Implicit9& implicitSchurFactor =
dynamic_cast<Implicit9&>(*gaussianImplicitSchurFactor);

28
gtsam/slam/tests/testSmartProjectionPoseFactor.cpp
View File

@ -142,7 +142,7 @@ TEST_UNSAFE( SmartProjectionPoseFactor, noiseless ) {
// Calculate using computeJacobians
Vector b;
vector<SmartFactor::KeyMatrix2D> Fblocks;
vector<SmartFactor::MatrixZD> Fblocks;
double actualError3 = factor.computeJacobians(Fblocks, E, b, cameras, *point);
EXPECT(assert_equal(expectedE, E, 1e-7));
EXPECT_DOUBLES_EQUAL(expectedError, actualError3, 1e-8);
@ -264,10 +264,12 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) {
/* *************************************************************************/
TEST( SmartProjectionPoseFactor, Factors ) {
typedef PinholePose<Cal3_S2> Camera;
// Default cameras for simple derivatives
Rot3 R;
static Cal3_S2::shared_ptr sharedK(new Cal3_S2(100, 100, 0, 0, 0));
PinholePose<Cal3_S2> cam1(Pose3(R, Point3(0, 0, 0)), sharedK), cam2(
Camera cam1(Pose3(R, Point3(0, 0, 0)), sharedK), cam2(
Pose3(R, Point3(1, 0, 0)), sharedK);
// one landmarks 1m in front of camera
@ -350,15 +352,19 @@ TEST( SmartProjectionPoseFactor, Factors ) {
E(2, 0) = 10;
E(2, 2) = 1;
E(3, 1) = 10;
vector<pair<Key, Matrix26> > Fblocks = list_of<pair<Key, Matrix> > //
(make_pair(x1, F1))(make_pair(x2, F2));
vector<Matrix26> Fblocks = list_of<Matrix>(F1)(F2);
Vector b(4);
b.setZero();
// Create smart factors
FastVector<Key> keys;
keys.push_back(x1);
keys.push_back(x2);
// createJacobianQFactor
SharedIsotropic n = noiseModel::Isotropic::Sigma(4, sigma);
Matrix3 P = (E.transpose() * E).inverse();
JacobianFactorQ<6, 2> expectedQ(Fblocks, E, P, b, n);
JacobianFactorQ<6, 2> expectedQ(keys, Fblocks, E, P, b, n);
EXPECT(assert_equal(expectedInformation, expectedQ.information(), 1e-8));
boost::shared_ptr<JacobianFactorQ<6, 2> > actualQ =
@ -370,13 +376,13 @@ TEST( SmartProjectionPoseFactor, Factors ) {
// Whiten for RegularImplicitSchurFactor (does not have noise model)
model->WhitenSystem(E, b);
Matrix3 whiteP = (E.transpose() * E).inverse();
BOOST_FOREACH(SmartFactor::KeyMatrix2D& Fblock,Fblocks)
Fblock.second = model->Whiten(Fblock.second);
Fblocks[0] = model->Whiten(Fblocks[0]);
Fblocks[1] = model->Whiten(Fblocks[1]);
// createRegularImplicitSchurFactor
RegularImplicitSchurFactor<6> expected(Fblocks, E, whiteP, b);
RegularImplicitSchurFactor<Camera> expected(keys, Fblocks, E, whiteP, b);
boost::shared_ptr<RegularImplicitSchurFactor<6> > actual =
boost::shared_ptr<RegularImplicitSchurFactor<Camera> > actual =
smartFactor1->createRegularImplicitSchurFactor(cameras, 0.0);
CHECK(actual);
EXPECT(assert_equal(expectedInformation, expected.information(), 1e-8));
@ -764,8 +770,6 @@ TEST( SmartProjectionPoseFactor, 3poses_projection_factor ) {
values.insert(L(1), landmark1);
values.insert(L(2), landmark2);
values.insert(L(3), landmark3);
if (isDebugTest)
values.at<Pose3>(x3).print("Pose3 before optimization: ");
DOUBLES_EQUAL(48406055, graph.error(values), 1);
@ -779,8 +783,6 @@ TEST( SmartProjectionPoseFactor, 3poses_projection_factor ) {
DOUBLES_EQUAL(0, graph.error(result), 1e-9);
if (isDebugTest)
result.at<Pose3>(x3).print("Pose3 after optimization: ");
EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-7));
}