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

Merge branch 'feature/smartFactors'

release/4.3a0
Luca 2014-05-28 18:19:35 -04:00
parent 2699605943 386ed2e510
commit bc13ec9f92
17 changed files with 349 additions and 169 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
gtsam/linear/GaussianFactor.h
View File

@ -130,6 +130,9 @@ namespace gtsam {
/// A'*b for Jacobian, eta for Hessian
virtual VectorValues gradientAtZero() const = 0;
/// A'*b for Jacobian, eta for Hessian (raw memory version)
virtual void gradientAtZero(double* d) const = 0;
private:
/** Serialization function */
friend class boost::serialization::access;

7
gtsam/linear/GaussianFactorGraph.cpp
View File

@ -75,6 +75,13 @@ namespace gtsam {
return dims_accumulated;
}
/* ************************************************************************* */
GaussianFactorGraph::shared_ptr GaussianFactorGraph::cloneToPtr() const {
gtsam::GaussianFactorGraph::shared_ptr result(new GaussianFactorGraph());
*result = *this;
return result;
}
/* ************************************************************************* */
GaussianFactorGraph GaussianFactorGraph::clone() const {
GaussianFactorGraph result;

10
gtsam/linear/GaussianFactorGraph.h
View File

@ -160,7 +160,13 @@ namespace gtsam {
* Cloning preserves null factors so indices for the original graph are still
* valid for the cloned graph.
*/
GaussianFactorGraph clone() const;
virtual GaussianFactorGraph clone() const;
/**
* CloneToPtr() performs a simple assignment to a new graph and returns it.
* There is no preservation of null factors!
*/
virtual GaussianFactorGraph::shared_ptr cloneToPtr() const;
/**
* Returns the negation of all factors in this graph - corresponds to antifactors.
@ -257,7 +263,7 @@ namespace gtsam {
* @param [output] g A VectorValues to store the gradient, which must be preallocated,
* see allocateVectorValues
* @return The gradient as a VectorValues */
VectorValues gradientAtZero() const;
virtual VectorValues gradientAtZero() const;
/** Optimize along the gradient direction, with a closed-form computation to perform the line
* search. The gradient is computed about \f$ \delta x=0 \f$.

17
gtsam/linear/HessianFactor.cpp
View File

@ -599,6 +599,23 @@ VectorValues HessianFactor::gradientAtZero() const {
return g;
}
/* ************************************************************************* */
// TODO: currently assumes all variables of the same size 9 and keys arranged from 0 to n
void HessianFactor::gradientAtZero(double* d) const {
// Use eigen magic to access raw memory
typedef Eigen::Matrix<double, 9, 1> DVector;
typedef Eigen::Map<DVector> DMap;
// Loop over all variables in the factor
for (DenseIndex pos = 0; pos < (DenseIndex)size(); ++pos) {
Key j = keys_[pos];
// Get the diagonal block, and insert its diagonal
DVector dj = -info_(pos,size()).knownOffDiagonal();
DMap(d + 9 * j) += dj;
}
}
/* ************************************************************************* */
std::pair<boost::shared_ptr<GaussianConditional>, boost::shared_ptr<HessianFactor> >
EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys)

2
gtsam/linear/HessianFactor.h
View File

@ -387,6 +387,8 @@ namespace gtsam {
/// eta for Hessian
VectorValues gradientAtZero() const;
virtual void gradientAtZero(double* d) const;
/**
* Densely partially eliminate with Cholesky factorization. JacobianFactors are
* left-multiplied with their transpose to form the Hessian using the conversion constructor

5
gtsam/linear/JacobianFactor.cpp
View File

@ -573,6 +573,11 @@ VectorValues JacobianFactor::gradientAtZero() const {
return g;
}
/* ************************************************************************* */
void JacobianFactor::gradientAtZero(double* d) const {
//throw std::runtime_error("gradientAtZero not implemented for Jacobian factor");
}
/* ************************************************************************* */
pair<Matrix, Vector> JacobianFactor::jacobian() const {
pair<Matrix, Vector> result = jacobianUnweighted();

3
gtsam/linear/JacobianFactor.h
View File

@ -286,6 +286,9 @@ namespace gtsam {
/// A'*b for Jacobian
VectorValues gradientAtZero() const;
/* ************************************************************************* */
virtual void gradientAtZero(double* d) const;
/** Return a whitened version of the factor, i.e. with unit diagonal noise model. */
JacobianFactor whiten() const;

5
gtsam/navigation/CombinedImuFactor.h
View File

@ -34,6 +34,11 @@ namespace gtsam {
*
* @addtogroup SLAM
*
* If you are using the factor, please cite:
* L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating conditionally
* independent sets in factor graphs: a unifying perspective based on smart factors,
* Int. Conf. on Robotics and Automation (ICRA), 2014.
*
* REFERENCES:
* [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008.
* [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built

6
gtsam/navigation/ImuFactor.h
View File

@ -33,6 +33,12 @@ namespace gtsam {
/**
*
* @addtogroup SLAM
*
* If you are using the factor, please cite:
* L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating conditionally
* independent sets in factor graphs: a unifying perspective based on smart factors,
* Int. Conf. on Robotics and Automation (ICRA), 2014.
*
** REFERENCES:
* [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008.
* [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built

81
gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
View File

@ -138,7 +138,7 @@ void LevenbergMarquardtOptimizer::decreaseLambda(double stepQuality) {
}
/* ************************************************************************* */
GaussianFactorGraph LevenbergMarquardtOptimizer::buildDampedSystem(
GaussianFactorGraph::shared_ptr LevenbergMarquardtOptimizer::buildDampedSystem(
const GaussianFactorGraph& linear) {
gttic(damp);
@ -159,7 +159,8 @@ GaussianFactorGraph LevenbergMarquardtOptimizer::buildDampedSystem(
// for each of the variables, add a prior
double sigma = 1.0 / std::sqrt(state_.lambda);
GaussianFactorGraph damped = linear;
GaussianFactorGraph::shared_ptr dampedPtr = linear.cloneToPtr();
GaussianFactorGraph &damped = (*dampedPtr);
damped.reserve(damped.size() + state_.values.size());
if (params_.diagonalDamping) {
BOOST_FOREACH(const VectorValues::KeyValuePair& key_vector, state_.hessianDiagonal) {
@ -188,7 +189,20 @@ GaussianFactorGraph LevenbergMarquardtOptimizer::buildDampedSystem(
}
}
gttoc(damp);
return damped;
return dampedPtr;
}
/* ************************************************************************* */
// Log current error/lambda to file
inline void LevenbergMarquardtOptimizer::writeLogFile(double currentError){
if (!params_.logFile.empty()) {
ofstream os(params_.logFile.c_str(), ios::app);
boost::posix_time::ptime currentTime = boost::posix_time::microsec_clock::universal_time();
os << /*inner iterations*/ state_.totalNumberInnerIterations << ","
<< 1e-6 * (currentTime - state_.startTime).total_microseconds() << ","
<< /*current error*/ currentError << "," << state_.lambda << ","
<< /*outer iterations*/ state_.iterations << endl;
}
}
/* ************************************************************************* */
@ -205,6 +219,9 @@ void LevenbergMarquardtOptimizer::iterate() {
cout << "linearizing = " << endl;
GaussianFactorGraph::shared_ptr linear = linearize();
if(state_.totalNumberInnerIterations==0) // write initial error
writeLogFile(state_.error);
// Keep increasing lambda until we make make progress
while (true) {
@ -212,21 +229,8 @@ void LevenbergMarquardtOptimizer::iterate() {
cout << "trying lambda = " << state_.lambda << endl;
// Build damped system for this lambda (adds prior factors that make it like gradient descent)
GaussianFactorGraph dampedSystem = buildDampedSystem(*linear);
// Log current error/lambda to file
if (!params_.logFile.empty()) {
ofstream os(params_.logFile.c_str(), ios::app);
boost::posix_time::ptime currentTime =
boost::posix_time::microsec_clock::universal_time();
os << state_.totalNumberInnerIterations << ","
<< 1e-6 * (currentTime - state_.startTime).total_microseconds() << ","
<< state_.error << "," << state_.lambda << endl;
}
++state_.totalNumberInnerIterations;
GaussianFactorGraph::shared_ptr dampedSystemPtr = buildDampedSystem(*linear);
GaussianFactorGraph &dampedSystem = (*dampedSystemPtr);
// Try solving
double modelFidelity = 0.0;
@ -256,6 +260,9 @@ void LevenbergMarquardtOptimizer::iterate() {
double newlinearizedError = linear->error(delta);
double linearizedCostChange = state_.error - newlinearizedError;
if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA)
cout << "newlinearizedError = " << newlinearizedError <<
" linearizedCostChange = " << linearizedCostChange << endl;
if (linearizedCostChange >= 0) { // step is valid
// update values
@ -266,50 +273,62 @@ void LevenbergMarquardtOptimizer::iterate() {
// compute new error
gttic(compute_error);
if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA)
cout << "calculating error" << endl;
cout << "calculating error:" << endl;
newError = graph_.error(newValues);
gttoc(compute_error);
if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA)
cout << "old error (" << state_.error
<< ") new (tentative) error (" << newError << ")" << endl;
// cost change in the original, nonlinear system (old - new)
double costChange = state_.error - newError;
if (linearizedCostChange > 1e-15) { // the error has to decrease to satify this condition
if (linearizedCostChange > 1e-20) { // the (linear) error has to decrease to satisfy this condition
// fidelity of linearized model VS original system between
modelFidelity = costChange / linearizedCostChange;
// if we decrease the error in the nonlinear system and modelFidelity is above threshold
step_is_successful = modelFidelity > params_.minModelFidelity;
} else {
step_is_successful = true; // linearizedCostChange close to zero
}
if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA)
cout << "modelFidelity: " << modelFidelity << endl;
} // else we consider the step non successful and we either increase lambda or stop if error change is small
double minAbsoluteTolerance = params_.relativeErrorTol * state_.error;
// if the change is small we terminate
if (fabs(costChange) < minAbsoluteTolerance)
if (fabs(costChange) < minAbsoluteTolerance){
if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA)
cout << "fabs(costChange)="<<fabs(costChange) << " minAbsoluteTolerance="<< minAbsoluteTolerance
<< " (relativeErrorTol=" << params_.relativeErrorTol << ")" << endl;
stopSearchingLambda = true;
}
}
}
}
}
++state_.totalNumberInnerIterations;
if (step_is_successful) { // we have successfully decreased the cost and we have good modelFidelity
state_.values.swap(newValues);
state_.error = newError;
decreaseLambda(modelFidelity);
writeLogFile(state_.error);
break;
} else if (!stopSearchingLambda) { // we failed to solved the system or we had no decrease in cost
if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA)
cout << "increasing lambda: old error (" << state_.error
<< ") new error (" << newError << ")" << endl;
cout << "increasing lambda" << endl;
increaseLambda();
writeLogFile(state_.error);
// check if lambda is too big
if (state_.lambda >= params_.lambdaUpperBound) {
if (nloVerbosity >= NonlinearOptimizerParams::TERMINATION)
cout
<< "Warning: Levenberg-Marquardt giving up because cannot decrease error with maximum lambda"
<< endl;
cout << "Warning: Levenberg-Marquardt giving up because "
"cannot decrease error with maximum lambda" << endl;
break;
}
} else { // the change in the cost is very small and it is not worth trying bigger lambdas
writeLogFile(state_.error);
if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA)
cout << "Levenberg-Marquardt: stopping as relative cost reduction is small" << endl;
break;
}
} // end while

5
gtsam/nonlinear/LevenbergMarquardtOptimizer.h
View File

@ -113,7 +113,6 @@ public:
inline void setDiagonalDamping(bool flag) {
diagonalDamping = flag;
}
inline void setUseFixedLambdaFactor(bool flag) {
useFixedLambdaFactor_ = flag;
}
@ -255,9 +254,11 @@ public:
}
/** Build a damped system for a specific lambda */
GaussianFactorGraph buildDampedSystem(const GaussianFactorGraph& linear);
GaussianFactorGraph::shared_ptr buildDampedSystem(const GaussianFactorGraph& linear);
friend class ::NonlinearOptimizerMoreOptimizationTest;
void writeLogFile(double currentError);
/// @}
protected:

2
gtsam/slam/dataset.cpp
View File

@ -713,7 +713,7 @@ bool writeBALfromValues(const string& filename, const SfM_data &data, Values& va
}
for (size_t j = 0; j < dataValues.number_tracks(); j++){ // for each point
Key pointKey = symbol('l',j);
Key pointKey = P(j);
if(values.exists(pointKey)){
Point3 point = values.at<Point3>(pointKey);
dataValues.tracks[j].p = point;

6
gtsam/slam/tests/testDataset.cpp
View File

@ -19,10 +19,12 @@
#include <boost/algorithm/string.hpp>
#include <gtsam/inference/Symbol.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/slam/dataset.h>
using namespace gtsam::symbol_shorthand;
using namespace std;
using namespace gtsam;
@ -176,7 +178,7 @@ TEST( dataSet, writeBALfromValues_Dubrovnik){
value.insert(poseKey, pose);
}
for(size_t j=0; j < readData.number_tracks(); j++){ // for each point
Key pointKey = symbol('l',j);
Key pointKey = P(j);
Point3 point = poseChange.transform_from( readData.tracks[j].p );
value.insert(pointKey, point);
}
@ -208,7 +210,7 @@ TEST( dataSet, writeBALfromValues_Dubrovnik){
EXPECT(assert_equal(expectedPose,actualPose, 1e-7));
Point3 expectedPoint = track0.p;
Key pointKey = symbol('l',0);
Key pointKey = P(0);
Point3 actualPoint = value.at<Point3>(pointKey);
EXPECT(assert_equal(expectedPoint,actualPoint, 1e-6));
}

126
gtsam_unstable/slam/ImplicitSchurFactor.h
View File

@ -24,7 +24,6 @@ class ImplicitSchurFactor: public GaussianFactor {
public:
typedef ImplicitSchurFactor This; ///< Typedef to this class
typedef JacobianFactor Base; ///< Typedef to base class
typedef boost::shared_ptr<This> shared_ptr; ///< shared_ptr to this class
protected:
@ -87,7 +86,8 @@ public:
}
/// print
void print(const std::string& s, const KeyFormatter& formatter) const {
void print(const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
std::cout << " ImplicitSchurFactor " << std::endl;
Factor::print(s);
std::cout << " PointCovariance_ \n" << PointCovariance_ << std::endl;
@ -188,14 +188,19 @@ public:
/// Return the block diagonal of the Hessian for this factor
virtual std::map<Key, Matrix> hessianBlockDiagonal() const {
std::map<Key, Matrix> blocks;
// F'*(I - E*P*E')*F
for (size_t pos = 0; pos < size(); ++pos) {
Key j = keys_[pos];
const Matrix2D& Fj = Fblocks_[pos].second;
// F'*F - F'*E*P*E'*F (9*2)*(2*9) - (9*2)*(2*3)*(3*3)*(3*2)*(2*9)
Eigen::Matrix<double, D, 3> FtE = Fj.transpose()
* E_.block<2, 3>(2 * pos, 0);
blocks[j] = Fj.transpose() * Fj
- FtE * PointCovariance_ * FtE.transpose();
const Matrix2D& Fj = Fblocks_[pos].second;
// Eigen::Matrix<double, D, 3> FtE = Fj.transpose()
// * E_.block<2, 3>(2 * pos, 0);
// blocks[j] = Fj.transpose() * Fj
// - FtE * PointCovariance_ * FtE.transpose();
const Matrix23& Ej = E_.block<2, 3>(2 * pos, 0);
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);
// Eigen::Matrix<double, 2, 2> Q = //
@ -234,6 +239,73 @@ public:
typedef std::vector<Vector2> Error2s;
/**
* @brief Calculate corrected error Q*(e-2*b) = (I - E*P*E')*(e-2*b)
*/
void projectError2(const Error2s& e1, Error2s& e2) const {
// d1 = E.transpose() * (e1-2*b) = (3*2m)*2m
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));
// d2 = E.transpose() * e1 = (3*2m)*2m
Vector3 d2 = PointCovariance_ * d1;
// e3 = alpha*(e1 - E*d2) = 1*[2m-(2m*3)*3]
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;
}
/*
* This definition matches the linearized error in the Hessian Factor:
* LinError(x) = x'*H*x - 2*x'*eta + f
* with:
* H = F' * (I-E'*P*E) * F = F' * Q * F
* eta = F' * (I-E'*P*E) * b = F' * Q * b
* f = nonlinear error
* (x'*H*x - 2*x'*eta + f) = x'*F'*Q*F*x - 2*x'*F'*Q *b + f = x'*F'*Q*(F*x - 2*b) + f
*/
virtual double error(const VectorValues& x) const {
// resize does not do malloc if correct size
e1.resize(size());
e2.resize(size());
// e1 = F * x - b = (2m*dm)*dm
for (size_t k = 0; k < size(); ++k)
e1[k] = Fblocks_[k].second * x.at(keys_[k]);
projectError2(e1, e2);
double result = 0;
for (size_t k = 0; k < size(); ++k)
result += dot(e1[k], e2[k]);
double f = b_.squaredNorm();
return 0.5 * (result + f);
}
/// needed to be GaussianFactor - (I - E*P*E')*(F*x - b)
// This is wrong and does not match the definition in Hessian
// virtual double error(const VectorValues& x) const {
//
// // resize does not do malloc if correct size
// e1.resize(size());
// e2.resize(size());
//
// // e1 = F * x - b = (2m*dm)*dm
// for (size_t k = 0; k < size(); ++k)
// e1[k] = Fblocks_[k].second * x.at(keys_[k]) - b_.segment < 2 > (k * 2);
// projectError(e1, e2);
//
// double result = 0;
// for (size_t k = 0; k < size(); ++k)
// result += dot(e2[k], e2[k]);
//
// std::cout << "implicitFactor::error result " << result << std::endl;
// return 0.5 * result;
// }
/**
* @brief Calculate corrected error Q*e = (I - E*P*E')*e
*/
@ -253,24 +325,6 @@ public:
e2[k] = e1[k] - E_.block < 2, 3 > (2 * k, 0) * d2;
}
/// needed to be GaussianFactor - (I - E*P*E')*(F*x - b)
virtual double error(const VectorValues& x) const {
// resize does not do malloc if correct size
e1.resize(size());
e2.resize(size());
// e1 = F * x - b = (2m*dm)*dm
for (size_t k = 0; k < size(); ++k)
e1[k] = Fblocks_[k].second * x.at(keys_[k]) - b_.segment < 2 > (k * 2);
projectError(e1, e2);
double result = 0;
for (size_t k = 0; k < size(); ++k)
result += dot(e2[k], e2[k]);
return 0.5 * result;
}
/// Scratch space for multiplyHessianAdd
mutable Error2s e1, e2;
@ -377,6 +431,28 @@ public:
return g;
}
/**
* Calculate gradient, which is -F'Q*b, see paper - RAW MEMORY ACCESS
*/
void gradientAtZero(double* d) const {
// Use eigen magic to access raw memory
typedef Eigen::Matrix<double, D, 1> DVector;
typedef Eigen::Map<DVector> DMap;
// calculate Q*b
e1.resize(size());
e2.resize(size());
for (size_t k = 0; k < size(); k++)
e1[k] = b_.segment < 2 > (2 * k);
projectError(e1, e2);
for (size_t k = 0; k < size(); ++k) { // for each camera in the factor
Key j = keys_[k];
DMap(d + D * j) += -Fblocks_[k].second.transpose() * e2[k];
}
}
};
// ImplicitSchurFactor

198
gtsam_unstable/slam/SmartFactorBase.h
View File

@ -303,15 +303,18 @@ public:
// Point covariance inv(E'*E)
Matrix3 EtE = E.transpose() * E;
Matrix3 DMatrix = eye(E.cols()); // damping matrix
if (diagonalDamping) { // diagonal of the hessian
DMatrix(0, 0) = EtE(0, 0);
DMatrix(1, 1) = EtE(1, 1);
DMatrix(2, 2) = EtE(2, 2);
EtE(0, 0) += lambda * EtE(0, 0);
EtE(1, 1) += lambda * EtE(1, 1);
EtE(2, 2) += lambda * EtE(2, 2);
}else{
EtE(0, 0) += lambda;
EtE(1, 1) += lambda;
EtE(2, 2) += lambda;
}
PointCov.noalias() = (EtE + lambda * DMatrix).inverse();
PointCov.noalias() = (EtE).inverse();
return f;
}
@ -322,7 +325,7 @@ public:
const Cameras& cameras, const Point3& point,
const double lambda = 0.0) const {
size_t numKeys = this->keys_.size();
int numKeys = this->keys_.size();
std::vector<KeyMatrix2D> Fblocks;
double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point,
lambda);
@ -388,7 +391,7 @@ public:
double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda,
diagonalDamping);
//#define HESSIAN_BLOCKS
//#define HESSIAN_BLOCKS // slower, as internally the Hessian factor will transform the blocks into SymmetricBlockMatrix
#ifdef HESSIAN_BLOCKS
// Create structures for Hessian Factors
std::vector < Matrix > Gs(numKeys * (numKeys + 1) / 2);
@ -402,7 +405,7 @@ public:
return boost::make_shared < RegularHessianFactor<D>
> (this->keys_, Gs, gs, f);
#else
#else // we create directly a SymmetricBlockMatrix
size_t n1 = D * numKeys + 1;
std::vector<DenseIndex> dims(numKeys + 1); // this also includes the b term
std::fill(dims.begin(), dims.end() - 1, D);
@ -411,36 +414,13 @@ public:
SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(n1, n1)); // for 10 cameras, size should be (10*D+1 x 10*D+1)
sparseSchurComplement(Fblocks, E, PointCov, b, augmentedHessian); // augmentedHessian.matrix().block<D,D> (i1,i2) = ...
augmentedHessian(numKeys, numKeys)(0, 0) = f;
return boost::make_shared<RegularHessianFactor<D> >(
this->keys_, augmentedHessian);
return boost::make_shared<RegularHessianFactor<D> >(this->keys_,
augmentedHessian);
#endif
}
// ****************************************************************************************************
boost::shared_ptr<RegularHessianFactor<D> > updateAugmentedHessian(
const Cameras& cameras, const Point3& point, const double lambda,
bool diagonalDamping, SymmetricBlockMatrix& augmentedHessian) const {
int numKeys = this->keys_.size();
std::vector < KeyMatrix2D > Fblocks;
Matrix E;
Matrix3 PointCov;
Vector b;
double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda,
diagonalDamping);
std::vector<DenseIndex> dims(numKeys + 1); // this also includes the b term
std::fill(dims.begin(), dims.end() - 1, D);
dims.back() = 1;
updateSparseSchurComplement(Fblocks, E, PointCov, b, augmentedHessian); // augmentedHessian.matrix().block<D,D> (i1,i2) = ...
std::cout << "f "<< f <<std::endl;
augmentedHessian(numKeys,numKeys)(0,0) += f;
}
// ****************************************************************************************************
// slow version - works on full (sparse) matrices
void schurComplement(const std::vector<KeyMatrix2D>& Fblocks, const Matrix& E,
const Matrix& PointCov, const Vector& b,
/*output ->*/std::vector<Matrix>& Gs, std::vector<Vector>& gs) const {
@ -477,53 +457,6 @@ public:
}
}
// ****************************************************************************************************
void updateSparseSchurComplement(const std::vector<KeyMatrix2D>& Fblocks,
const Matrix& E, const Matrix& P /*Point Covariance*/, const Vector& b,
/*output ->*/SymmetricBlockMatrix& augmentedHessian) const {
// Schur complement trick
// Gs = F' * F - F' * E * P * E' * F
// gs = F' * (b - E * P * E' * b)
// a single point is observed in numKeys cameras
size_t numKeys = this->keys_.size(); // cameras observing current point
size_t aug_numKeys = augmentedHessian.rows() - 1; // all cameras in the group
// Blockwise Schur complement
for (size_t i1 = 0; i1 < numKeys; i1++) { // for each camera
const Matrix2D& Fi1 = Fblocks.at(i1).second;
const Matrix23 Ei1_P = E.block<2, 3>(2 * i1, 0) * P;
// D = (Dx2) * (2)
// (augmentedHessian.matrix()).block<D,1> (i1,numKeys+1) = Fi1.transpose() * b.segment < 2 > (2 * i1); // F' * b
size_t aug_i1 = this->keys_[i1];
std::cout << "i1 "<< i1 <<std::endl;
std::cout << "aug_i1 "<< aug_i1 <<std::endl;
std::cout << "aug_numKeys "<< aug_numKeys <<std::endl;
augmentedHessian(aug_i1,aug_numKeys) = //augmentedHessian(aug_i1,aug_numKeys) +
Fi1.transpose() * b.segment < 2 > (2 * i1) // F' * b
- Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (Dx2) * (2x3) * (3*2m) * (2m x 1)
// (DxD) = (Dx2) * ( (2xD) - (2x3) * (3x2) * (2xD) )
std::cout << "filled 1 " <<std::endl;
augmentedHessian(aug_i1,aug_i1) = //augmentedHessian(aug_i1,aug_i1) +
Fi1.transpose() * (Fi1 - Ei1_P * E.block<2, 3>(2 * i1, 0).transpose() * Fi1);
// upper triangular part of the hessian
for (size_t i2 = i1+1; i2 < numKeys; i2++) { // for each camera
const Matrix2D& Fi2 = Fblocks.at(i2).second;
size_t aug_i2 = this->keys_[i2];
std::cout << "i2 "<< i2 <<std::endl;
std::cout << "aug_i2 "<< aug_i2 <<std::endl;
// (DxD) = (Dx2) * ( (2x2) * (2xD) )
augmentedHessian(aug_i1, aug_i2) = //augmentedHessian(aug_i1, aug_i2)
- Fi1.transpose() * (Ei1_P * E.block<2, 3>(2 * i2, 0).transpose() * Fi2);
}
} // end of for over cameras
}
// ****************************************************************************************************
void sparseSchurComplement(const std::vector<KeyMatrix2D>& Fblocks,
const Matrix& E, const Matrix& P /*Point Covariance*/, const Vector& b,
@ -547,16 +480,16 @@ public:
- Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (Dx2) * (2x3) * (3*2m) * (2m x 1)
// (DxD) = (Dx2) * ( (2xD) - (2x3) * (3x2) * (2xD) )
augmentedHessian(i1,i1) =
Fi1.transpose() * (Fi1 - Ei1_P * E.block<2, 3>(2 * i1, 0).transpose() * Fi1);
augmentedHessian(i1, i1) = Fi1.transpose()
* (Fi1 - Ei1_P * E.block<2, 3>(2 * i1, 0).transpose() * Fi1);
// upper triangular part of the hessian
for (size_t i2 = i1 + 1; i2 < numKeys; i2++) { // for each camera
const Matrix2D& Fi2 = Fblocks.at(i2).second;
// (DxD) = (Dx2) * ( (2x2) * (2xD) )
augmentedHessian(i1,i2) =
-Fi1.transpose() * (Ei1_P * E.block<2, 3>(2 * i2, 0).transpose() * Fi2);
augmentedHessian(i1, i2) = -Fi1.transpose()
* (Ei1_P * E.block<2, 3>(2 * i2, 0).transpose() * Fi2);
}
} // end of for over cameras
}
@ -587,12 +520,12 @@ public:
{ // for i1 = i2
// D = (Dx2) * (2)
gs.at(i1) = Fi1.transpose() * b.segment < 2 > (2 * i1); // F' * b
// D = (Dx2) * (2x3) * (3*2m) * (2m x 1)
gs.at(i1) -= Fi1.transpose() * (Ei1_P * (E.transpose() * b));
gs.at(i1) = Fi1.transpose() * b.segment<2>(2 * i1) // F' * b
-Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (Dx2) * (2x3) * (3*2m) * (2m x 1)
// (DxD) = (Dx2) * ( (2xD) - (2x3) * (3x2) * (2xD) )
Gs.at(GsIndex) = Fi1.transpose() * (Fi1 - Ei1_P * E.block<2, 3>(2 * i1, 0).transpose() * Fi1);
Gs.at(GsIndex) = Fi1.transpose()
* (Fi1 - Ei1_P * E.block<2, 3>(2 * i1, 0).transpose() * Fi1);
GsIndex++;
}
// upper triangular part of the hessian
@ -600,11 +533,96 @@ public:
const Matrix2D& Fi2 = Fblocks.at(i2).second;
// (DxD) = (Dx2) * ( (2x2) * (2xD) )
Gs.at(GsIndex) = -Fi1.transpose() * (Ei1_P * E.block<2, 3>(2 * i2, 0).transpose() * Fi2);
Gs.at(GsIndex) = -Fi1.transpose()
* (Ei1_P * E.block<2, 3>(2 * i2, 0).transpose() * Fi2);
GsIndex++;
}
} // end of for over cameras
}
// ****************************************************************************************************
void updateAugmentedHessian(const Cameras& cameras, const Point3& point,
const double lambda, bool diagonalDamping,
SymmetricBlockMatrix& augmentedHessian,
const FastVector<Key> allKeys) const {
// int numKeys = this->keys_.size();
std::vector<KeyMatrix2D> Fblocks;
Matrix E;
Matrix3 PointCov;
Vector b;
double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda,
diagonalDamping);
updateSparseSchurComplement(Fblocks, E, PointCov, b, f, allKeys, augmentedHessian); // augmentedHessian.matrix().block<D,D> (i1,i2) = ...
}
// ****************************************************************************************************
void updateSparseSchurComplement(const std::vector<KeyMatrix2D>& Fblocks,
const Matrix& E, const Matrix& P /*Point Covariance*/, const Vector& b,
const double f, const FastVector<Key> allKeys,
/*output ->*/SymmetricBlockMatrix& augmentedHessian) const {
// Schur complement trick
// Gs = F' * F - F' * E * P * E' * F
// gs = F' * (b - E * P * E' * b)
MatrixDD matrixBlock;
typedef SymmetricBlockMatrix::Block Block; ///< A block from the Hessian matrix
FastMap<Key,size_t> KeySlotMap;
for (size_t slot=0; slot < allKeys.size(); slot++)
KeySlotMap.insert(std::make_pair<Key,size_t>(allKeys[slot],slot));
// a single point is observed in numKeys cameras
size_t numKeys = this->keys_.size(); // cameras observing current point
size_t aug_numKeys = (augmentedHessian.rows() - 1) / D; // all cameras in the group
// Blockwise Schur complement
for (size_t i1 = 0; i1 < numKeys; i1++) { // for each camera in the current factor
const Matrix2D& Fi1 = Fblocks.at(i1).second;
const Matrix23 Ei1_P = E.block<2, 3>(2 * i1, 0) * P;
// D = (Dx2) * (2)
// allKeys are the list of all camera keys in the group, e.g, (1,3,4,5,7)
// we should map those to a slot in the local (grouped) hessian (0,1,2,3,4)
// Key cameraKey_i1 = this->keys_[i1];
DenseIndex aug_i1 = KeySlotMap[this->keys_[i1]];
// information vector - store previous vector
// vectorBlock = augmentedHessian(aug_i1, aug_numKeys).knownOffDiagonal();
// add contribution of current factor
augmentedHessian(aug_i1, aug_numKeys) = augmentedHessian(aug_i1, aug_numKeys).knownOffDiagonal()
+ Fi1.transpose() * b.segment<2>(2 * i1) // F' * b
- Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (Dx2) * (2x3) * (3*2m) * (2m x 1)
// (DxD) = (Dx2) * ( (2xD) - (2x3) * (3x2) * (2xD) )
// main block diagonal - store previous block
matrixBlock = augmentedHessian(aug_i1, aug_i1);
// add contribution of current factor
augmentedHessian(aug_i1, aug_i1) = matrixBlock +
( Fi1.transpose() * (Fi1 - Ei1_P * E.block<2, 3>(2 * i1, 0).transpose() * Fi1) );
// upper triangular part of the hessian
for (size_t i2 = i1 + 1; i2 < numKeys; i2++) { // for each camera
const Matrix2D& Fi2 = Fblocks.at(i2).second;
//Key cameraKey_i2 = this->keys_[i2];
DenseIndex aug_i2 = KeySlotMap[this->keys_[i2]];
// (DxD) = (Dx2) * ( (2x2) * (2xD) )
// off diagonal block - store previous block
// matrixBlock = augmentedHessian(aug_i1, aug_i2).knownOffDiagonal();
// add contribution of current factor
augmentedHessian(aug_i1, aug_i2) = augmentedHessian(aug_i1, aug_i2).knownOffDiagonal()
- Fi1.transpose() * (Ei1_P * E.block<2, 3>(2 * i2, 0).transpose() * Fi2);
}
} // end of for over cameras
augmentedHessian(aug_numKeys, aug_numKeys)(0, 0) += f;
}
// ****************************************************************************************************
boost::shared_ptr<ImplicitSchurFactor<D> > createImplicitSchurFactor(
const Cameras& cameras, const Point3& point, double lambda = 0.0,

10
gtsam_unstable/slam/SmartProjectionPoseFactor.h
View File

@ -22,6 +22,16 @@
#include "SmartProjectionFactor.h"
namespace gtsam {
/**
*
* @addtogroup SLAM
*
* If you are using the factor, please cite:
* L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating conditionally
* independent sets in factor graphs: a unifying perspective based on smart factors,
* Int. Conf. on Robotics and Automation (ICRA), 2014.
*
*/
/**
* The calibration is known here. The factor only constraints poses (variable dimension is 6)

4
tests/testNonlinearOptimizer.cpp
View File

@ -295,7 +295,7 @@ TEST_UNSAFE(NonlinearOptimizer, MoreOptimization) {
// test the diagonal
GaussianFactorGraph::shared_ptr linear = optimizer.linearize();
GaussianFactorGraph damped = optimizer.buildDampedSystem(*linear);
GaussianFactorGraph damped = *optimizer.buildDampedSystem(*linear);
VectorValues d = linear->hessianDiagonal(), //
expectedDiagonal = d + params.lambdaInitial * d;
EXPECT(assert_equal(expectedDiagonal, damped.hessianDiagonal()));
@ -309,7 +309,7 @@ TEST_UNSAFE(NonlinearOptimizer, MoreOptimization) {
EXPECT(assert_equal(expectedGradient,linear->gradientAtZero()));
// Check that the gradient is zero for damped system (it is not!)
damped = optimizer.buildDampedSystem(*linear);
damped = *optimizer.buildDampedSystem(*linear);
VectorValues actualGradient = damped.gradientAtZero();
EXPECT(assert_equal(expectedGradient,actualGradient));