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

634 lines
24 KiB
C++
Raw Blame History

/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file JacobianFactor.cpp
* @author Richard Roberts
* @date Dec 8, 2010
*/
#include <gtsam/linear/GaussianConditional.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/HessianFactor.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/inference/VariableSlots.h>
#include <gtsam/base/debug.h>
#include <gtsam/base/timing.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/FastMap.h>
#include <gtsam/base/cholesky.h>
#include <boost/foreach.hpp>
#include <boost/format.hpp>
#include <boost/make_shared.hpp>
#include <boost/lambda/bind.hpp>
#include <boost/lambda/lambda.hpp>
#include <cmath>
#include <sstream>
#include <stdexcept>
using namespace std;
using namespace boost::lambda;
namespace gtsam {
/* ************************************************************************* */
void JacobianFactor::assertInvariants() const {
#ifndef NDEBUG
GaussianFactor::assertInvariants(); // The base class checks for unique keys
assert((size() == 0 && Ab_.rows() == 0 && Ab_.nBlocks() == 0) || size()+1 == Ab_.nBlocks());
assert(firstNonzeroBlocks_.size() == Ab_.rows());
for(size_t i=0; i<firstNonzeroBlocks_.size(); ++i)
assert(firstNonzeroBlocks_[i] < Ab_.nBlocks());
// Check for non-finite values
for(size_t i=0; i<Ab_.rows(); ++i)
for(size_t j=0; j<Ab_.cols(); ++j)
if(isnan(matrix_(i,j)))
throw invalid_argument("JacobianFactor contains NaN matrix entries.");
#endif
}
/* ************************************************************************* */
JacobianFactor::JacobianFactor(const JacobianFactor& gf) :
GaussianFactor(gf), model_(gf.model_), firstNonzeroBlocks_(gf.firstNonzeroBlocks_), Ab_(matrix_) {
Ab_.assignNoalias(gf.Ab_);
assertInvariants();
}
/* ************************************************************************* */
JacobianFactor::JacobianFactor() : Ab_(matrix_) { assertInvariants(); }
/* ************************************************************************* */
JacobianFactor::JacobianFactor(const Vector& b_in) : firstNonzeroBlocks_(b_in.size(), 0), Ab_(matrix_) {
size_t dims[] = { 1 };
Ab_.copyStructureFrom(BlockAb(matrix_, dims, dims+1, b_in.size()));
getb() = b_in;
assertInvariants();
}
/* ************************************************************************* */
JacobianFactor::JacobianFactor(Index i1, const Matrix& A1,
const Vector& b, const SharedDiagonal& model) :
GaussianFactor(i1), model_(model), firstNonzeroBlocks_(b.size(), 0), Ab_(matrix_) {
size_t dims[] = { A1.cols(), 1};
Ab_.copyStructureFrom(BlockAb(matrix_, dims, dims+2, b.size()));
Ab_(0) = A1;
getb() = b;
assertInvariants();
}
/* ************************************************************************* */
JacobianFactor::JacobianFactor(Index i1, const Matrix& A1, Index i2, const Matrix& A2,
const Vector& b, const SharedDiagonal& model) :
GaussianFactor(i1,i2), model_(model), firstNonzeroBlocks_(b.size(), 0), Ab_(matrix_) {
size_t dims[] = { A1.cols(), A2.cols(), 1};
Ab_.copyStructureFrom(BlockAb(matrix_, dims, dims+3, b.size()));
Ab_(0) = A1;
Ab_(1) = A2;
getb() = b;
assertInvariants();
}
/* ************************************************************************* */
JacobianFactor::JacobianFactor(Index i1, const Matrix& A1, Index i2, const Matrix& A2,
Index i3, const Matrix& A3, const Vector& b, const SharedDiagonal& model) :
GaussianFactor(i1,i2,i3), model_(model), firstNonzeroBlocks_(b.size(), 0), Ab_(matrix_) {
size_t dims[] = { A1.cols(), A2.cols(), A3.cols(), 1};
Ab_.copyStructureFrom(BlockAb(matrix_, dims, dims+4, b.size()));
Ab_(0) = A1;
Ab_(1) = A2;
Ab_(2) = A3;
getb() = b;
assertInvariants();
}
/* ************************************************************************* */
JacobianFactor::JacobianFactor(const std::vector<std::pair<Index, Matrix> > &terms,
const Vector &b, const SharedDiagonal& model) :
GaussianFactor(GetKeys(terms.size(), terms.begin(), terms.end())),
model_(model), firstNonzeroBlocks_(b.size(), 0), Ab_(matrix_)
{
size_t* dims = (size_t*)alloca(sizeof(size_t)*(terms.size()+1)); // FIXME: alloca is bad, just ask Google.
for(size_t j=0; j<terms.size(); ++j)
dims[j] = terms[j].second.cols();
dims[terms.size()] = 1;
Ab_.copyStructureFrom(BlockAb(matrix_, dims, dims+terms.size()+1, b.size()));
for(size_t j=0; j<terms.size(); ++j)
Ab_(j) = terms[j].second;
getb() = b;
assertInvariants();
}
/* ************************************************************************* */
JacobianFactor::JacobianFactor(const std::list<std::pair<Index, Matrix> > &terms,
const Vector &b, const SharedDiagonal& model) :
GaussianFactor(GetKeys(terms.size(), terms.begin(), terms.end())),
model_(model), firstNonzeroBlocks_(b.size(), 0), Ab_(matrix_)
{
size_t* dims=(size_t*)alloca(sizeof(size_t)*(terms.size()+1)); // FIXME: alloca is bad, just ask Google.
size_t j=0;
std::list<std::pair<Index, Matrix> >::const_iterator term=terms.begin();
for(; term!=terms.end(); ++term,++j)
dims[j] = term->second.cols();
dims[j] = 1;
Ab_.copyStructureFrom(BlockAb(matrix_, dims, dims+terms.size()+1, b.size()));
j = 0;
for(term=terms.begin(); term!=terms.end(); ++term,++j)
Ab_(j) = term->second;
getb() = b;
assertInvariants();
}
/* ************************************************************************* */
JacobianFactor::JacobianFactor(const GaussianConditional& cg) : GaussianFactor(cg), model_(noiseModel::Diagonal::Sigmas(cg.get_sigmas(), true)), Ab_(matrix_) {
Ab_.assignNoalias(cg.rsd_);
firstNonzeroBlocks_.resize(cg.get_d().size(), 0); // set sigmas from precisions
assertInvariants();
}
/* ************************************************************************* */
JacobianFactor::JacobianFactor(const HessianFactor& factor) : Ab_(matrix_) {
keys_ = factor.keys_;
Ab_.assignNoalias(factor.info_);
size_t maxrank;
try {
maxrank = choleskyCareful(matrix_).first;
} catch(const CarefulCholeskyNegativeMatrixException& e) {
cout <<
"Attempting to convert a HessianFactor to a JacobianFactor, but for this\n"
"HessianFactor it is not possible because either the Hessian is negative or\n"
"indefinite, or the quadratic error function it describes becomes negative for\n"
"some values. Here is the HessianFactor on which this conversion was attempted:\n";
factor.print("");
throw;
}
// Zero out lower triangle
matrix_.topRows(maxrank).triangularView<Eigen::StrictlyLower>() =
Matrix::Zero(maxrank, matrix_.cols());
// FIXME: replace with triangular system
Ab_.rowEnd() = maxrank;
model_ = noiseModel::Unit::Create(maxrank);
firstNonzeroBlocks_.resize(this->rows(), 0);
// Sort keys
set<Index> vars;
for(size_t j=0; j<size(); ++j)
vars.insert(keys_[j]);
Permutation permutation(Permutation::Identity(*vars.rbegin() + 1));
size_t jNew = 0;
BOOST_FOREACH(const Index& var, vars) {
permutation[var] = jNew++;
}
permuteWithInverse(permutation);
jNew = 0;
BOOST_FOREACH(const Index& var, vars) {
keys_[jNew++] = var;
}
assertInvariants();
}
/* ************************************************************************* */
JacobianFactor& JacobianFactor::operator=(const JacobianFactor& rhs) {
this->Base::operator=(rhs); // Copy keys
model_ = rhs.model_; // Copy noise model
firstNonzeroBlocks_ = rhs.firstNonzeroBlocks_; // Copy staircase pattern
Ab_.assignNoalias(rhs.Ab_); // Copy matrix and block structure
assertInvariants();
return *this;
}
/* ************************************************************************* */
void JacobianFactor::print(const string& s) const {
cout << s << "\n";
if (empty()) {
cout << " empty, keys: ";
BOOST_FOREACH(const Index& key, keys()) { cout << key << " "; }
cout << endl;
} else {
for(const_iterator key=begin(); key!=end(); ++key)
cout << boost::format("A[%1%]=\n")%*key << getA(key) << endl;
cout << "b=" << getb() << endl;
model_->print("model");
}
}
/* ************************************************************************* */
// Check if two linear factors are equal
bool JacobianFactor::equals(const GaussianFactor& f_, double tol) const {
if(!dynamic_cast<const JacobianFactor*>(&f_))
return false;
else {
const JacobianFactor& f(static_cast<const JacobianFactor&>(f_));
if (empty()) return (f.empty());
if(keys()!=f.keys() /*|| !model_->equals(lf->model_, tol)*/)
return false;
if (!(Ab_.rows() == f.Ab_.rows() && Ab_.cols() == f.Ab_.cols()))
return false;
constABlock Ab1(Ab_.range(0, Ab_.nBlocks()));
constABlock Ab2(f.Ab_.range(0, f.Ab_.nBlocks()));
for(size_t row=0; row< (size_t) Ab1.rows(); ++row)
if(!equal_with_abs_tol(Ab1.row(row), Ab2.row(row), tol) &&
!equal_with_abs_tol(-Ab1.row(row), Ab2.row(row), tol))
return false;
return true;
}
}
/* ************************************************************************* */
Vector JacobianFactor::unweighted_error(const VectorValues& c) const {
Vector e = -getb();
if (empty()) return e;
for(size_t pos=0; pos<size(); ++pos)
e += Ab_(pos) * c[keys_[pos]];
return e;
}
/* ************************************************************************* */
Vector JacobianFactor::error_vector(const VectorValues& c) const {
if (empty()) return model_->whiten(-getb());
return model_->whiten(unweighted_error(c));
}
/* ************************************************************************* */
double JacobianFactor::error(const VectorValues& c) const {
if (empty()) return 0;
Vector weighted = error_vector(c);
return 0.5 * weighted.dot(weighted);
}
/* ************************************************************************* */
Matrix JacobianFactor::computeInformation() const {
Matrix AbWhitened = Ab_.full();
model_->WhitenInPlace(AbWhitened);
return AbWhitened.transpose() * AbWhitened;
}
/* ************************************************************************* */
Vector JacobianFactor::operator*(const VectorValues& x) const {
Vector Ax = zero(Ab_.rows());
if (empty()) return Ax;
// Just iterate over all A matrices and multiply in correct config part
for(size_t pos=0; pos<size(); ++pos)
Ax += Ab_(pos) * x[keys_[pos]];
return model_->whiten(Ax);
}
/* ************************************************************************* */
void JacobianFactor::transposeMultiplyAdd(double alpha, const Vector& e,
VectorValues& x) const {
Vector E = alpha * model_->whiten(e);
// Just iterate over all A matrices and insert Ai^e into VectorValues
for(size_t pos=0; pos<size(); ++pos)
gtsam::transposeMultiplyAdd(1.0, Ab_(pos), E, x[keys_[pos]]);
}
/* ************************************************************************* */
pair<Matrix,Vector> JacobianFactor::matrix(bool weight) const {
Matrix A(Ab_.range(0, size()));
Vector b(getb());
// divide in sigma so error is indeed 0.5*|Ax-b|
if (weight) model_->WhitenSystem(A,b);
return make_pair(A, b);
}
/* ************************************************************************* */
Matrix JacobianFactor::matrix_augmented(bool weight) const {
if (weight) { Matrix Ab(Ab_.range(0,Ab_.nBlocks())); model_->WhitenInPlace(Ab); return Ab; }
else return Ab_.range(0, Ab_.nBlocks());
}
/* ************************************************************************* */
std::vector<boost::tuple<size_t, size_t, double> >
JacobianFactor::sparse(const std::vector<size_t>& columnIndices) const {
std::vector<boost::tuple<size_t, size_t, double> > entries;
// iterate over all variables in the factor
for(const_iterator var=begin(); var<end(); ++var) {
Matrix whitenedA(model_->Whiten(getA(var)));
// find first column index for this key
size_t column_start = columnIndices[*var];
for (size_t i = 0; i < (size_t) whitenedA.rows(); i++)
for (size_t j = 0; j < (size_t) whitenedA.cols(); j++) {
double s = whitenedA(i,j);
if (std::abs(s) > 1e-12) entries.push_back(
boost::make_tuple(i, column_start + j, s));
}
}
Vector whitenedb(model_->whiten(getb()));
size_t bcolumn = columnIndices.back();
for (size_t i = 0; i < (size_t) whitenedb.size(); i++)
entries.push_back(boost::make_tuple(i, bcolumn, whitenedb(i)));
// return the result
return entries;
}
/* ************************************************************************* */
JacobianFactor JacobianFactor::whiten() const {
JacobianFactor result(*this);
result.model_->WhitenInPlace(result.matrix_);
result.model_ = noiseModel::Unit::Create(result.model_->dim());
return result;
}
/* ************************************************************************* */
GaussianConditional::shared_ptr JacobianFactor::eliminateFirst() {
return this->eliminate(1);
}
/* ************************************************************************* */
GaussianConditional::shared_ptr JacobianFactor::eliminate(size_t nrFrontals) {
assert(Ab_.rowStart() == 0 && Ab_.rowEnd() == (size_t) matrix_.rows() && Ab_.firstBlock() == 0);
assert(size() >= nrFrontals);
assertInvariants();
const bool debug = ISDEBUG("JacobianFactor::eliminate");
if(debug) cout << "Eliminating " << nrFrontals << " frontal variables" << endl;
if(debug) this->print("Eliminating JacobianFactor: ");
// NOTE: stairs are not currently used in the Eigen QR implementation
// add this back if DenseQR is ever reimplemented
// tic(1, "stairs");
// // Translate the left-most nonzero column indices into top-most zero row indices
// vector<int> firstZeroRows(Ab_.cols());
// {
// size_t lastNonzeroRow = 0;
// vector<int>::iterator firstZeroRowsIt = firstZeroRows.begin();
// for(size_t var=0; var<keys().size(); ++var) {
// while(lastNonzeroRow < this->rows() && firstNonzeroBlocks_[lastNonzeroRow] <= var)
// ++ lastNonzeroRow;
// fill(firstZeroRowsIt, firstZeroRowsIt+Ab_(var).cols(), lastNonzeroRow);
// firstZeroRowsIt += Ab_(var).cols();
// }
// assert(firstZeroRowsIt+1 == firstZeroRows.end());
// *firstZeroRowsIt = this->rows();
// }
// toc(1, "stairs");
// #ifndef NDEBUG
// for(size_t col=0; col<Ab_.cols(); ++col) {
// if(debug) cout << "Staircase[" << col << "] = " << firstZeroRows[col] << endl;
// if(col != 0) assert(firstZeroRows[col] >= firstZeroRows[col-1]);
// assert(firstZeroRows[col] <= (long)this->rows());
// }
// #endif
if(debug) gtsam::print(matrix_, "Augmented Ab: ");
size_t frontalDim = Ab_.range(0,nrFrontals).cols();
if(debug) cout << "frontalDim = " << frontalDim << endl;
// Use in-place QR dense Ab appropriate to NoiseModel
tic(2, "QR");
SharedDiagonal noiseModel = model_->QR(matrix_);
toc(2, "QR");
// Zero the lower-left triangle. todo: not all of these entries actually
// need to be zeroed if we are careful to start copying rows after the last
// structural zero.
if(matrix_.rows() > 0)
for(size_t j=0; j<(size_t) matrix_.cols(); ++j)
for(size_t i=j+1; i<noiseModel->dim(); ++i)
matrix_(i,j) = 0.0;
if(debug) gtsam::print(matrix_, "QR result: ");
if(debug) noiseModel->print("QR result noise model: ");
// Check for singular factor
if(noiseModel->dim() < frontalDim) {
throw domain_error((boost::format(
"JacobianFactor is singular in variable %1%, discovered while attempting\n"
"to eliminate this variable.") % front()).str());
}
// Extract conditionals
tic(3, "cond Rd");
GaussianConditional::shared_ptr conditionals(new GaussianConditional());
// Restrict the matrix to be in the first nrFrontals variables
Ab_.rowEnd() = Ab_.rowStart() + frontalDim;
const Eigen::VectorBlock<const Vector> sigmas = noiseModel->sigmas().segment(Ab_.rowStart(), Ab_.rowEnd()-Ab_.rowStart());
conditionals = boost::make_shared<ConditionalType>(begin(), end(), nrFrontals, Ab_, sigmas);
if(debug) conditionals->print("Extracted conditional: ");
Ab_.rowStart() += frontalDim;
Ab_.firstBlock() += nrFrontals;
toc(3, "cond Rd");
if(debug) conditionals->print("Extracted conditionals: ");
tic(4, "remaining factor");
// Take lower-right block of Ab to get the new factor
Ab_.rowEnd() = noiseModel->dim();
keys_.erase(begin(), begin() + nrFrontals);
// Set sigmas with the right model
if (noiseModel->isConstrained())
model_ = noiseModel::Constrained::MixedSigmas(sub(noiseModel->sigmas(), frontalDim, noiseModel->dim()));
else
model_ = noiseModel::Diagonal::Sigmas(sub(noiseModel->sigmas(), frontalDim, noiseModel->dim()));
if(debug) this->print("Eliminated factor: ");
assert(Ab_.rows() <= Ab_.cols()-1);
toc(4, "remaining factor");
// todo SL: deal with "dead" pivot columns!!!
tic(5, "rowstarts");
size_t varpos = 0;
firstNonzeroBlocks_.resize(this->rows());
for(size_t row=0; row<rows(); ++row) {
if(debug) cout << "row " << row << " varpos " << varpos << " Ab_.offset(varpos)=" << Ab_.offset(varpos) << " Ab_.offset(varpos+1)=" << Ab_.offset(varpos+1) << endl;
while(varpos < this->size() && Ab_.offset(varpos+1)-Ab_.offset(0) <= row)
++ varpos;
firstNonzeroBlocks_[row] = varpos;
if(debug) cout << "firstNonzeroVars_[" << row << "] = " << firstNonzeroBlocks_[row] << endl;
}
toc(5, "rowstarts");
if(debug) print("Eliminated factor: ");
assertInvariants();
return conditionals;
}
/* ************************************************************************* */
void JacobianFactor::collectInfo(size_t index, vector<_RowSource>& rowSources) const {
assertInvariants();
for (size_t row = 0; row < rows(); ++row) {
Index firstNonzeroVar;
if (firstNonzeroBlocks_[row] < size()) {
firstNonzeroVar = keys_[firstNonzeroBlocks_[row]];
} else if (firstNonzeroBlocks_[row] == size()) {
firstNonzeroVar = back() + 1;
} else {
assert(false);
}
rowSources.push_back(_RowSource(firstNonzeroVar, index, row));
}
}
/* ************************************************************************* */
void JacobianFactor::allocate(const VariableSlots& variableSlots, vector<
size_t>& varDims, size_t m) {
keys_.resize(variableSlots.size());
std::transform(variableSlots.begin(), variableSlots.end(), begin(),
boost::bind(&VariableSlots::const_iterator::value_type::first,
boost::lambda::_1));
varDims.push_back(1);
Ab_.copyStructureFrom(BlockAb(matrix_, varDims.begin(), varDims.end(), m));
firstNonzeroBlocks_.resize(m);
}
/* ************************************************************************* */
void JacobianFactor::copyRow(const JacobianFactor& source, Index sourceRow,
size_t sourceSlot, size_t row, Index slot) {
ABlock combinedBlock = Ab_(slot);
if (sourceSlot != numeric_limits<Index>::max()) {
if (source.firstNonzeroBlocks_[sourceRow] <= sourceSlot) {
const constABlock sourceBlock(source.Ab_(sourceSlot));
combinedBlock.row(row).noalias() = sourceBlock.row(sourceRow);
} else {
combinedBlock.row(row).setZero();
}
} else {
combinedBlock.row(row).setZero();
}
}
/* ************************************************************************* */
void JacobianFactor::copyFNZ(size_t m, size_t n,
vector<_RowSource>& rowSources) {
Index i = 0;
for (size_t row = 0; row < m; ++row) {
while (i < n && rowSources[row].firstNonzeroVar > keys_[i])
++i;
firstNonzeroBlocks_[row] = i;
}
}
/* ************************************************************************* */
void JacobianFactor::setModel(bool anyConstrained, const Vector& sigmas) {
if (anyConstrained)
model_ = noiseModel::Constrained::MixedSigmas(sigmas);
else
model_ = noiseModel::Diagonal::Sigmas(sigmas);
}
/* ************************************************************************* */
Errors operator*(const FactorGraph<JacobianFactor>& fg, const VectorValues& x) {
Errors e;
BOOST_FOREACH(const JacobianFactor::shared_ptr& Ai, fg) {
e.push_back((*Ai)*x);
}
return e;
}
/* ************************************************************************* */
void multiplyInPlace(const FactorGraph<JacobianFactor>& fg, const VectorValues& x, Errors& e) {
multiplyInPlace(fg,x,e.begin());
}
/* ************************************************************************* */
void multiplyInPlace(const FactorGraph<JacobianFactor>& fg, const VectorValues& x, const Errors::iterator& e) {
Errors::iterator ei = e;
BOOST_FOREACH(const JacobianFactor::shared_ptr& Ai, fg) {
*ei = (*Ai)*x;
ei++;
}
}
/* ************************************************************************* */
// x += alpha*A'*e
void transposeMultiplyAdd(const FactorGraph<JacobianFactor>& fg, double alpha, const Errors& e, VectorValues& x) {
// For each factor add the gradient contribution
Errors::const_iterator ei = e.begin();
BOOST_FOREACH(const JacobianFactor::shared_ptr& Ai, fg) {
Ai->transposeMultiplyAdd(alpha,*(ei++),x);
}
}
/* ************************************************************************* */
VectorValues gradient(const FactorGraph<JacobianFactor>& fg, const VectorValues& x0) {
// It is crucial for performance to make a zero-valued clone of x
VectorValues g = VectorValues::Zero(x0);
Errors e;
BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) {
e.push_back(factor->error_vector(x0));
}
transposeMultiplyAdd(fg, 1.0, e, g);
return g;
}
/* ************************************************************************* */
void gradientAtZero(const FactorGraph<JacobianFactor>& fg, VectorValues& g) {
// Zero-out the gradient
g.setZero();
Errors e;
BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) {
e.push_back(-factor->getb());
}
transposeMultiplyAdd(fg, 1.0, e, g);
}
/* ************************************************************************* */
void residual(const FactorGraph<JacobianFactor>& fg, const VectorValues &x, VectorValues &r) {
Index i = 0 ;
BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) {
r[i] = factor->getb();
i++;
}
VectorValues Ax = VectorValues::SameStructure(r);
multiply(fg,x,Ax);
axpy(-1.0,Ax,r);
}
/* ************************************************************************* */
void multiply(const FactorGraph<JacobianFactor>& fg, const VectorValues &x, VectorValues &r) {
r.vector() = Vector::Zero(r.dim());
Index i = 0;
BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) {
SubVector &y = r[i];
for(JacobianFactor::const_iterator j = factor->begin(); j != factor->end(); ++j) {
y += factor->getA(j) * x[*j];
}
++i;
}
}
/* ************************************************************************* */
void transposeMultiply(const FactorGraph<JacobianFactor>& fg, const VectorValues &r, VectorValues &x) {
x.vector() = Vector::Zero(x.dim());
Index i = 0;
BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) {
for(JacobianFactor::const_iterator j = factor->begin(); j != factor->end(); ++j) {
x[*j] += factor->getA(j).transpose() * r[i];
}
++i;
}
}
}
Reference in New Issue View Git Blame Copy Permalink