Removed extra code that was sometimes maintaining a staircase pattern in JacobianFactor by sorting rows - a holdover from the staircase optimization that is no longer done now that we are using Eigen's QR
Richard Roberts
parent
7d9139a854
commit
3e93c488e5
|
|
@ -48,10 +48,6 @@ namespace gtsam {
|
|||
* variable index 3), row-block 2 (also meaning component factor 2), comes from
|
||||
* column-block 0 of component factor 2.
|
||||
*
|
||||
* Note that in the case of GaussianFactor, the rows of the combined factor are
|
||||
* further sorted so that the column-block position of the first structural
|
||||
* non-zero increases monotonically through the rows. This additional process
|
||||
* is not performed by this class.
|
||||
* \nosubgrouping
|
||||
*/
|
||||
|
||||
|
|
|
|||
|
|
@ -240,20 +240,6 @@ break;
|
|||
}
|
||||
toc(1, "countDims");
|
||||
|
||||
if (debug) cout << "Sort rows" << endl;
|
||||
tic(2, "sort rows");
|
||||
vector<JacobianFactor::_RowSource> rowSources;
|
||||
rowSources.reserve(m);
|
||||
bool anyConstrained = false;
|
||||
for (size_t sourceFactorI = 0; sourceFactorI < factors.size(); ++sourceFactorI) {
|
||||
const JacobianFactor& sourceFactor(*factors[sourceFactorI]);
|
||||
sourceFactor.collectInfo(sourceFactorI, rowSources);
|
||||
if (sourceFactor.isConstrained()) anyConstrained = true;
|
||||
}
|
||||
assert(rowSources.size() == m);
|
||||
std::sort(rowSources.begin(), rowSources.end());
|
||||
toc(2, "sort rows");
|
||||
|
||||
if (debug) cout << "Allocate new factor" << endl;
|
||||
tic(3, "allocate");
|
||||
JacobianFactor::shared_ptr combined(new JacobianFactor());
|
||||
|
|
@ -261,36 +247,47 @@ break;
|
|||
Vector sigmas(m);
|
||||
toc(3, "allocate");
|
||||
|
||||
if (debug) cout << "Copy rows" << endl;
|
||||
tic(4, "copy rows");
|
||||
if (debug) cout << "Copy blocks" << endl;
|
||||
tic(4, "copy blocks");
|
||||
// Loop over slots in combined factor
|
||||
Index combinedSlot = 0;
|
||||
BOOST_FOREACH(const VariableSlots::value_type& varslot, variableSlots) {
|
||||
for (size_t row = 0; row < m; ++row) {
|
||||
const JacobianFactor::_RowSource& info(rowSources[row]);
|
||||
const JacobianFactor& source(*factors[info.factorI]);
|
||||
size_t sourceRow = info.factorRowI;
|
||||
Index sourceSlot = varslot.second[info.factorI];
|
||||
combined->copyRow(source, sourceRow, sourceSlot, row, combinedSlot);
|
||||
JacobianFactor::ABlock destSlot(combined->getA(combined->begin()+combinedSlot));
|
||||
// Loop over source factors
|
||||
size_t nextRow = 0;
|
||||
for(size_t factorI = 0; factorI < factors.size(); ++factorI) {
|
||||
// Slot in source factor
|
||||
const Index sourceSlot = varslot.second[factorI];
|
||||
const size_t sourceRows = factors[factorI]->rows();
|
||||
JacobianFactor::ABlock::RowsBlockXpr destBlock(destSlot.middleRows(nextRow, sourceRows));
|
||||
// Copy if exists in source factor, otherwise set zero
|
||||
if(sourceSlot != numeric_limits<Index>::max())
|
||||
destBlock = factors[factorI]->getA(factors[factorI]->begin()+sourceSlot);
|
||||
else
|
||||
destBlock.setZero();
|
||||
nextRow += sourceRows;
|
||||
}
|
||||
++combinedSlot;
|
||||
}
|
||||
toc(4, "copy rows");
|
||||
toc(4, "copy blocks");
|
||||
|
||||
if (debug) cout << "Copy rhs (b), sigma, and firstNonzeroBlocks" << endl;
|
||||
if (debug) cout << "Copy rhs (b) and sigma" << endl;
|
||||
tic(5, "copy vectors");
|
||||
for (size_t row = 0; row < m; ++row) {
|
||||
const JacobianFactor::_RowSource& info(rowSources[row]);
|
||||
const JacobianFactor& source(*factors[info.factorI]);
|
||||
const size_t sourceRow = info.factorRowI;
|
||||
combined->getb()(row) = source.getb()(sourceRow);
|
||||
sigmas(row) = source.get_model()->sigmas()(sourceRow);
|
||||
bool anyConstrained = false;
|
||||
// Loop over source factors
|
||||
size_t nextRow = 0;
|
||||
for(size_t factorI = 0; factorI < factors.size(); ++factorI) {
|
||||
const size_t sourceRows = factors[factorI]->rows();
|
||||
combined->getb().segment(nextRow, sourceRows) = factors[factorI]->getb();
|
||||
sigmas.segment(nextRow, sourceRows) = factors[factorI]->get_model()->sigmas();
|
||||
if (factors[factorI]->isConstrained()) anyConstrained = true;
|
||||
nextRow += sourceRows;
|
||||
}
|
||||
combined->copyFNZ(m, variableSlots.size(),rowSources);
|
||||
toc(5, "copy vectors");
|
||||
|
||||
if (debug) cout << "Create noise model from sigmas" << endl;
|
||||
tic(6, "noise model");
|
||||
combined->setModel( anyConstrained,sigmas);
|
||||
combined->setModel(anyConstrained, sigmas);
|
||||
toc(6, "noise model");
|
||||
|
||||
if (debug) cout << "Assert Invariants" << endl;
|
||||
|
|
|
|||
|
|
@ -48,15 +48,12 @@ namespace gtsam {
|
|||
#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());
|
||||
#endif
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
JacobianFactor::JacobianFactor(const JacobianFactor& gf) :
|
||||
GaussianFactor(gf), model_(gf.model_), firstNonzeroBlocks_(gf.firstNonzeroBlocks_), Ab_(matrix_) {
|
||||
GaussianFactor(gf), model_(gf.model_), Ab_(matrix_) {
|
||||
Ab_.assignNoalias(gf.Ab_);
|
||||
assertInvariants();
|
||||
}
|
||||
|
|
@ -77,7 +74,7 @@ namespace gtsam {
|
|||
JacobianFactor::JacobianFactor() : Ab_(matrix_) { assertInvariants(); }
|
||||
|
||||
/* ************************************************************************* */
|
||||
JacobianFactor::JacobianFactor(const Vector& b_in) : firstNonzeroBlocks_(b_in.size(), 0), Ab_(matrix_) {
|
||||
JacobianFactor::JacobianFactor(const Vector& b_in) : Ab_(matrix_) {
|
||||
size_t dims[] = { 1 };
|
||||
Ab_.copyStructureFrom(BlockAb(matrix_, dims, dims+1, b_in.size()));
|
||||
getb() = b_in;
|
||||
|
|
@ -88,7 +85,7 @@ namespace gtsam {
|
|||
/* ************************************************************************* */
|
||||
JacobianFactor::JacobianFactor(Index i1, const Matrix& A1,
|
||||
const Vector& b, const SharedDiagonal& model) :
|
||||
GaussianFactor(i1), model_(model), firstNonzeroBlocks_(b.size(), 0), Ab_(matrix_) {
|
||||
GaussianFactor(i1), model_(model), Ab_(matrix_) {
|
||||
|
||||
if(model->dim() != (size_t) b.size())
|
||||
throw InvalidNoiseModel(b.size(), model->dim());
|
||||
|
|
@ -103,7 +100,7 @@ namespace gtsam {
|
|||
/* ************************************************************************* */
|
||||
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_) {
|
||||
GaussianFactor(i1,i2), model_(model), Ab_(matrix_) {
|
||||
|
||||
if(model->dim() != (size_t) b.size())
|
||||
throw InvalidNoiseModel(b.size(), model->dim());
|
||||
|
|
@ -119,7 +116,7 @@ namespace gtsam {
|
|||
/* ************************************************************************* */
|
||||
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_) {
|
||||
GaussianFactor(i1,i2,i3), model_(model), Ab_(matrix_) {
|
||||
|
||||
if(model->dim() != (size_t) b.size())
|
||||
throw InvalidNoiseModel(b.size(), model->dim());
|
||||
|
|
@ -137,7 +134,7 @@ namespace gtsam {
|
|||
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_)
|
||||
model_(model), Ab_(matrix_)
|
||||
{
|
||||
|
||||
if(model->dim() != (size_t) b.size())
|
||||
|
|
@ -158,7 +155,7 @@ namespace gtsam {
|
|||
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_)
|
||||
model_(model), Ab_(matrix_)
|
||||
{
|
||||
|
||||
if(model->dim() != (size_t) b.size())
|
||||
|
|
@ -179,9 +176,11 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
JacobianFactor::JacobianFactor(const GaussianConditional& cg) : GaussianFactor(cg), model_(noiseModel::Diagonal::Sigmas(cg.get_sigmas(), true)), Ab_(matrix_) {
|
||||
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();
|
||||
}
|
||||
|
||||
|
|
@ -206,8 +205,6 @@ namespace gtsam {
|
|||
Ab_.rowEnd() = maxrank;
|
||||
model_ = noiseModel::Unit::Create(maxrank);
|
||||
|
||||
firstNonzeroBlocks_.resize(this->rows(), 0);
|
||||
|
||||
assertInvariants();
|
||||
}
|
||||
|
||||
|
|
@ -215,7 +212,6 @@ namespace gtsam {
|
|||
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;
|
||||
|
|
@ -417,19 +413,6 @@ namespace gtsam {
|
|||
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();
|
||||
|
|
@ -448,34 +431,6 @@ namespace gtsam {
|
|||
|
||||
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();
|
||||
|
|
@ -503,22 +458,6 @@ namespace gtsam {
|
|||
return splitConditional(nrFrontals);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
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) {
|
||||
|
|
@ -527,34 +466,6 @@ namespace gtsam {
|
|||
boost::bind(&VariableSlots::const_iterator::value_type::first, _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;
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -83,7 +83,6 @@ namespace gtsam {
|
|||
typedef VerticalBlockView<AbMatrix> BlockAb;
|
||||
|
||||
noiseModel::Diagonal::shared_ptr model_; // Gaussian noise model with diagonal covariance matrix
|
||||
std::vector<size_t> firstNonzeroBlocks_;
|
||||
AbMatrix matrix_; // the full matrix corresponding to the factor
|
||||
BlockAb Ab_; // the block view of the full matrix
|
||||
typedef GaussianFactor Base; // typedef to base
|
||||
|
|
@ -274,35 +273,13 @@ namespace gtsam {
|
|||
*/
|
||||
boost::shared_ptr<GaussianConditional> splitConditional(size_t nrFrontals = 1);
|
||||
|
||||
/* Used by ::CombineJacobians for sorting */
|
||||
struct _RowSource {
|
||||
size_t firstNonzeroVar;
|
||||
size_t factorI;
|
||||
size_t factorRowI;
|
||||
_RowSource(size_t _firstNonzeroVar, size_t _factorI, size_t _factorRowI) :
|
||||
firstNonzeroVar(_firstNonzeroVar), factorI(_factorI), factorRowI(_factorRowI) {}
|
||||
bool operator<(const _RowSource& o) const {
|
||||
return firstNonzeroVar < o.firstNonzeroVar;
|
||||
}
|
||||
};
|
||||
|
||||
// following methods all used in CombineJacobians:
|
||||
// Many imperative, perhaps all need to be combined in constructor
|
||||
|
||||
/** Collect information on Jacobian rows */
|
||||
void collectInfo(size_t index, std::vector<_RowSource>& rowSources) const;
|
||||
|
||||
/** allocate space */
|
||||
void allocate(const VariableSlots& variableSlots,
|
||||
std::vector<size_t>& varDims, size_t m);
|
||||
|
||||
/** copy a slot from source */
|
||||
void copyRow(const JacobianFactor& source,
|
||||
Index sourceRow, size_t sourceSlot, size_t row, Index slot);
|
||||
|
||||
/** copy firstNonzeroBlocks structure */
|
||||
void copyFNZ(size_t m, size_t n, std::vector<_RowSource>& rowSources);
|
||||
|
||||
/** set noiseModel correctly */
|
||||
void setModel(bool anyConstrained, const Vector& sigmas);
|
||||
|
||||
|
|
@ -341,7 +318,6 @@ namespace gtsam {
|
|||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
||||
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(GaussianFactor);
|
||||
ar & BOOST_SERIALIZATION_NVP(firstNonzeroBlocks_);
|
||||
ar & BOOST_SERIALIZATION_NVP(Ab_);
|
||||
ar & BOOST_SERIALIZATION_NVP(model_);
|
||||
ar & BOOST_SERIALIZATION_NVP(matrix_);
|
||||
|
|
|
|||
|
|
@ -163,10 +163,10 @@ TEST(GaussianFactorGraph, Combine2)
|
|||
JacobianFactor actual = *CombineJacobians(jacobians, VariableSlots(gfg));
|
||||
|
||||
Matrix zero3x3 = zeros(3,3);
|
||||
Matrix A0 = gtsam::stack(3, &A10, &zero3x3, &zero3x3);
|
||||
Matrix A1 = gtsam::stack(3, &A11, &A01, &A21);
|
||||
Vector b = gtsam::concatVectors(3, &b1, &b0, &b2);
|
||||
Vector sigmas = gtsam::concatVectors(3, &s1, &s0, &s2);
|
||||
Matrix A0 = gtsam::stack(3, &zero3x3, &A10, &zero3x3);
|
||||
Matrix A1 = gtsam::stack(3, &A01, &A11, &A21);
|
||||
Vector b = gtsam::concatVectors(3, &b0, &b1, &b2);
|
||||
Vector sigmas = gtsam::concatVectors(3, &s0, &s1, &s2);
|
||||
|
||||
JacobianFactor expected(0, A0, 1, A1, b, noiseModel::Diagonal::Sigmas(sigmas, true));
|
||||
|
||||
|
|
|
|||
|
|
@ -200,7 +200,7 @@ TEST(DoglegOptimizer, BT_BN_equivalency) {
|
|||
GaussianFactorGraph expected(gbn);
|
||||
GaussianFactorGraph actual(bt);
|
||||
|
||||
EXPECT(assert_equal(expected.denseJacobian(), actual.denseJacobian()));
|
||||
EXPECT(assert_equal(expected.denseHessian(), actual.denseHessian()));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
Loading…
Reference in New Issue