added raw memory access version of hessianDiagonal
Luca
parent
d2b6b12bba
commit
6edd3f10fc
|
|
@ -59,8 +59,7 @@ namespace gtsam {
|
|||
|
||||
/* ************************************************************************* */
|
||||
JacobianFactor::JacobianFactor() :
|
||||
Ab_(cref_list_of<1>(1), 0)
|
||||
{
|
||||
Ab_(cref_list_of<1>(1), 0) {
|
||||
getb().setZero();
|
||||
}
|
||||
|
||||
|
|
@ -72,48 +71,42 @@ namespace gtsam {
|
|||
else if (const HessianFactor* rhs = dynamic_cast<const HessianFactor*>(&gf))
|
||||
*this = JacobianFactor(*rhs);
|
||||
else
|
||||
throw std::invalid_argument("In JacobianFactor(const GaussianFactor& rhs), rhs is neither a JacobianFactor nor a HessianFactor");
|
||||
throw std::invalid_argument(
|
||||
"In JacobianFactor(const GaussianFactor& rhs), rhs is neither a JacobianFactor nor a HessianFactor");
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
JacobianFactor::JacobianFactor(const Vector& b_in) :
|
||||
Ab_(cref_list_of<1>(1), b_in.size())
|
||||
{
|
||||
Ab_(cref_list_of<1>(1), b_in.size()) {
|
||||
getb() = b_in;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
JacobianFactor::JacobianFactor(Key i1, const Matrix& A1,
|
||||
const Vector& b, const SharedDiagonal& model)
|
||||
{
|
||||
JacobianFactor::JacobianFactor(Key i1, const Matrix& A1, const Vector& b,
|
||||
const SharedDiagonal& model) {
|
||||
fillTerms(cref_list_of<1>(make_pair(i1, A1)), b, model);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
JacobianFactor::JacobianFactor(
|
||||
const Key i1, const Matrix& A1, Key i2, const Matrix& A2,
|
||||
const Vector& b, const SharedDiagonal& model)
|
||||
{
|
||||
fillTerms(cref_list_of<2>
|
||||
(make_pair(i1,A1))
|
||||
(make_pair(i2,A2)), b, model);
|
||||
JacobianFactor::JacobianFactor(const Key i1, const Matrix& A1, Key i2,
|
||||
const Matrix& A2, const Vector& b, const SharedDiagonal& model) {
|
||||
fillTerms(cref_list_of<2>(make_pair(i1, A1))(make_pair(i2, A2)), b, model);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
JacobianFactor::JacobianFactor(
|
||||
const Key i1, const Matrix& A1, Key i2, const Matrix& A2,
|
||||
Key i3, const Matrix& A3, const Vector& b, const SharedDiagonal& model)
|
||||
{
|
||||
fillTerms(cref_list_of<3>
|
||||
(make_pair(i1,A1))
|
||||
(make_pair(i2,A2))
|
||||
(make_pair(i3,A3)), b, model);
|
||||
JacobianFactor::JacobianFactor(const Key i1, const Matrix& A1, Key i2,
|
||||
const Matrix& A2, Key i3, const Matrix& A3, const Vector& b,
|
||||
const SharedDiagonal& model) {
|
||||
fillTerms(
|
||||
cref_list_of<3>(make_pair(i1, A1))(make_pair(i2, A2))(make_pair(i3, A3)),
|
||||
b, model);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
JacobianFactor::JacobianFactor(const HessianFactor& factor) :
|
||||
Base(factor), Ab_(VerticalBlockMatrix::LikeActiveViewOf(factor.matrixObject(), factor.rows()))
|
||||
{
|
||||
Base(factor), Ab_(
|
||||
VerticalBlockMatrix::LikeActiveViewOf(factor.matrixObject(),
|
||||
factor.rows())) {
|
||||
// Copy Hessian into our matrix and then do in-place Cholesky
|
||||
Ab_.full() = factor.matrixObject().full();
|
||||
|
||||
|
|
@ -138,31 +131,33 @@ namespace gtsam {
|
|||
// Helper functions for combine constructor
|
||||
namespace {
|
||||
boost::tuple<FastVector<DenseIndex>, DenseIndex, DenseIndex> _countDims(
|
||||
const FastVector<JacobianFactor::shared_ptr>& factors, const FastVector<VariableSlots::const_iterator>& variableSlots)
|
||||
{
|
||||
const FastVector<JacobianFactor::shared_ptr>& factors,
|
||||
const FastVector<VariableSlots::const_iterator>& variableSlots) {
|
||||
gttic(countDims);
|
||||
#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
|
||||
FastVector<DenseIndex> varDims(variableSlots.size(), numeric_limits<DenseIndex>::max());
|
||||
#else
|
||||
FastVector<DenseIndex> varDims(variableSlots.size(), numeric_limits<DenseIndex>::max());
|
||||
FastVector<DenseIndex> varDims(variableSlots.size(),
|
||||
numeric_limits<DenseIndex>::max());
|
||||
#endif
|
||||
DenseIndex m = 0;
|
||||
DenseIndex n = 0;
|
||||
for(size_t jointVarpos = 0; jointVarpos < variableSlots.size(); ++jointVarpos)
|
||||
{
|
||||
for (size_t jointVarpos = 0; jointVarpos < variableSlots.size();
|
||||
++jointVarpos) {
|
||||
const VariableSlots::const_iterator& slots = variableSlots[jointVarpos];
|
||||
|
||||
assert(slots->second.size() == factors.size());
|
||||
|
||||
bool foundVariable = false;
|
||||
for(size_t sourceFactorI = 0; sourceFactorI < slots->second.size(); ++sourceFactorI)
|
||||
{
|
||||
for (size_t sourceFactorI = 0; sourceFactorI < slots->second.size();
|
||||
++sourceFactorI) {
|
||||
const size_t sourceVarpos = slots->second[sourceFactorI];
|
||||
if (sourceVarpos != VariableSlots::Empty) {
|
||||
const JacobianFactor& sourceFactor = *factors[sourceFactorI];
|
||||
if (sourceFactor.cols() > 1) {
|
||||
foundVariable = true;
|
||||
DenseIndex vardim = sourceFactor.getDim(sourceFactor.begin() + sourceVarpos);
|
||||
DenseIndex vardim = sourceFactor.getDim(
|
||||
sourceFactor.begin() + sourceVarpos);
|
||||
|
||||
#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
|
||||
if(varDims[jointVarpos] == numeric_limits<DenseIndex>::max()) {
|
||||
|
|
@ -186,7 +181,8 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
if (!foundVariable)
|
||||
throw std::invalid_argument("Unable to determine dimensionality for all variables");
|
||||
throw std::invalid_argument(
|
||||
"Unable to determine dimensionality for all variables");
|
||||
}
|
||||
|
||||
BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, factors) {
|
||||
|
|
@ -203,15 +199,15 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
FastVector<JacobianFactor::shared_ptr>
|
||||
_convertOrCastToJacobians(const GaussianFactorGraph& factors)
|
||||
{
|
||||
FastVector<JacobianFactor::shared_ptr> _convertOrCastToJacobians(
|
||||
const GaussianFactorGraph& factors) {
|
||||
gttic(Convert_to_Jacobians);
|
||||
FastVector<JacobianFactor::shared_ptr> jacobians;
|
||||
jacobians.reserve(factors.size());
|
||||
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) {
|
||||
if (factor) {
|
||||
if(JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(factor))
|
||||
if (JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<
|
||||
JacobianFactor>(factor))
|
||||
jacobians.push_back(jf);
|
||||
else
|
||||
jacobians.push_back(boost::make_shared<JacobianFactor>(*factor));
|
||||
|
|
@ -222,11 +218,9 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
JacobianFactor::JacobianFactor(
|
||||
const GaussianFactorGraph& graph,
|
||||
JacobianFactor::JacobianFactor(const GaussianFactorGraph& graph,
|
||||
boost::optional<const Ordering&> ordering,
|
||||
boost::optional<const VariableSlots&> variableSlots)
|
||||
{
|
||||
boost::optional<const VariableSlots&> variableSlots) {
|
||||
gttic(JacobianFactor_combine_constructor);
|
||||
|
||||
// Compute VariableSlots if one was not provided
|
||||
|
|
@ -237,7 +231,8 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
// Cast or convert to Jacobians
|
||||
FastVector<JacobianFactor::shared_ptr> jacobians = _convertOrCastToJacobians(graph);
|
||||
FastVector<JacobianFactor::shared_ptr> jacobians = _convertOrCastToJacobians(
|
||||
graph);
|
||||
|
||||
gttic(Order_slots);
|
||||
// Order variable slots - we maintain the vector of ordered slots, as well as keep a list
|
||||
|
|
@ -251,8 +246,10 @@ namespace gtsam {
|
|||
size_t nOrderingSlotsUsed = 0;
|
||||
orderedSlots.resize(ordering->size());
|
||||
FastMap<Key, size_t> inverseOrdering = ordering->invert();
|
||||
for(VariableSlots::const_iterator item = variableSlots->begin(); item != variableSlots->end(); ++item) {
|
||||
FastMap<Key, size_t>::const_iterator orderingPosition = inverseOrdering.find(item->first);
|
||||
for (VariableSlots::const_iterator item = variableSlots->begin();
|
||||
item != variableSlots->end(); ++item) {
|
||||
FastMap<Key, size_t>::const_iterator orderingPosition =
|
||||
inverseOrdering.find(item->first);
|
||||
if (orderingPosition == inverseOrdering.end()) {
|
||||
unorderedSlots.push_back(item);
|
||||
} else {
|
||||
|
|
@ -271,7 +268,8 @@ namespace gtsam {
|
|||
} else {
|
||||
// If no ordering is provided, arrange the slots as they were, which will be sorted
|
||||
// numerically since VariableSlots uses a map sorting on Key.
|
||||
for(VariableSlots::const_iterator item = variableSlots->begin(); item != variableSlots->end(); ++item)
|
||||
for (VariableSlots::const_iterator item = variableSlots->begin();
|
||||
item != variableSlots->end(); ++item)
|
||||
orderedSlots.push_back(item);
|
||||
}
|
||||
gttoc(Order_slots);
|
||||
|
|
@ -285,8 +283,10 @@ namespace gtsam {
|
|||
gttic(allocate);
|
||||
Ab_ = VerticalBlockMatrix(varDims, m, true); // Allocate augmented matrix
|
||||
Base::keys_.resize(orderedSlots.size());
|
||||
boost::range::copy( // Get variable keys
|
||||
orderedSlots | boost::adaptors::indirected | boost::adaptors::map_keys, Base::keys_.begin());
|
||||
boost::range::copy(
|
||||
// Get variable keys
|
||||
orderedSlots | boost::adaptors::indirected | boost::adaptors::map_keys,
|
||||
Base::keys_.begin());
|
||||
gttoc(allocate);
|
||||
|
||||
// Loop over slots in combined factor and copy blocks from source factors
|
||||
|
|
@ -301,10 +301,12 @@ namespace gtsam {
|
|||
const size_t sourceSlot = varslot->second[factorI];
|
||||
const DenseIndex sourceRows = jacobians[factorI]->rows();
|
||||
if (sourceRows > 0) {
|
||||
JacobianFactor::ABlock::RowsBlockXpr destBlock(destSlot.middleRows(nextRow, sourceRows));
|
||||
JacobianFactor::ABlock::RowsBlockXpr destBlock(
|
||||
destSlot.middleRows(nextRow, sourceRows));
|
||||
// Copy if exists in source factor, otherwise set zero
|
||||
if (sourceSlot != VariableSlots::Empty)
|
||||
destBlock = jacobians[factorI]->getA(jacobians[factorI]->begin()+sourceSlot);
|
||||
destBlock = jacobians[factorI]->getA(
|
||||
jacobians[factorI]->begin() + sourceSlot);
|
||||
else
|
||||
destBlock.setZero();
|
||||
nextRow += sourceRows;
|
||||
|
|
@ -328,7 +330,8 @@ namespace gtsam {
|
|||
// If the factor has a noise model and we haven't yet allocated sigmas, allocate it.
|
||||
if (!sigmas)
|
||||
sigmas = Vector::Constant(m, 1.0);
|
||||
sigmas->segment(nextRow, sourceRows) = jacobians[factorI]->get_model()->sigmas();
|
||||
sigmas->segment(nextRow, sourceRows) =
|
||||
jacobians[factorI]->get_model()->sigmas();
|
||||
if (jacobians[factorI]->isConstrained())
|
||||
anyConstrained = true;
|
||||
}
|
||||
|
|
@ -342,13 +345,14 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void JacobianFactor::print(const string& s, const KeyFormatter& formatter) const
|
||||
{
|
||||
void JacobianFactor::print(const string& s,
|
||||
const KeyFormatter& formatter) const {
|
||||
if (!s.empty())
|
||||
cout << s << "\n";
|
||||
for (const_iterator key = begin(); key != end(); ++key) {
|
||||
cout <<
|
||||
formatMatrixIndented((boost::format(" A[%1%] = ") % formatter(*key)).str(), getA(key))
|
||||
cout
|
||||
<< formatMatrixIndented(
|
||||
(boost::format(" A[%1%] = ") % formatter(*key)).str(), getA(key))
|
||||
<< endl;
|
||||
}
|
||||
cout << formatMatrixIndented(" b = ", getb(), true) << "\n";
|
||||
|
|
@ -360,8 +364,7 @@ namespace gtsam {
|
|||
|
||||
/* ************************************************************************* */
|
||||
// Check if two linear factors are equal
|
||||
bool JacobianFactor::equals(const GaussianFactor& f_, double tol) const
|
||||
{
|
||||
bool JacobianFactor::equals(const GaussianFactor& f_, double tol) const {
|
||||
if (!dynamic_cast<const JacobianFactor*>(&f_))
|
||||
return false;
|
||||
else {
|
||||
|
|
@ -385,8 +388,8 @@ namespace gtsam {
|
|||
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))
|
||||
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;
|
||||
|
|
@ -411,7 +414,8 @@ namespace gtsam {
|
|||
|
||||
/* ************************************************************************* */
|
||||
double JacobianFactor::error(const VectorValues& c) const {
|
||||
if (empty()) return 0;
|
||||
if (empty())
|
||||
return 0;
|
||||
Vector weighted = error_vector(c);
|
||||
return 0.5 * weighted.dot(weighted);
|
||||
}
|
||||
|
|
@ -441,14 +445,14 @@ namespace gtsam {
|
|||
/* ************************************************************************* */
|
||||
VectorValues JacobianFactor::hessianDiagonal() const {
|
||||
VectorValues d;
|
||||
for(size_t pos=0; pos<size(); ++pos)
|
||||
{
|
||||
for (size_t pos = 0; pos < size(); ++pos) {
|
||||
Key j = keys_[pos];
|
||||
size_t nj = Ab_(pos).cols();
|
||||
Vector dj(nj);
|
||||
for (size_t k = 0; k < nj; ++k) {
|
||||
Vector column_k = Ab_(pos).col(k);
|
||||
if (model_) column_k = model_->whiten(column_k);
|
||||
if (model_)
|
||||
column_k = model_->whiten(column_k);
|
||||
dj(k) = dot(column_k, column_k);
|
||||
}
|
||||
d.insert(j, dj);
|
||||
|
|
@ -457,18 +461,32 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// TODO: currently assumes all variables of the same size 9 and keys arranged from 0 to n
|
||||
void JacobianFactor::hessianDiagonal(double* d) const {
|
||||
throw std::runtime_error("JacobianFactor::hessianDiagonal non implemented (use VectorValues version)");
|
||||
|
||||
// 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 j = 0; j < (DenseIndex) size(); ++j) {
|
||||
// Get the diagonal block, and insert its diagonal
|
||||
DVector dj;
|
||||
for (size_t k = 0; k < 9; ++k)
|
||||
dj(k) = Ab_(j).col(k).squaredNorm();
|
||||
|
||||
DMap(d + 9 * j) += dj;
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
map<Key, Matrix> JacobianFactor::hessianBlockDiagonal() const {
|
||||
map<Key, Matrix> blocks;
|
||||
for(size_t pos=0; pos<size(); ++pos)
|
||||
{
|
||||
for (size_t pos = 0; pos < size(); ++pos) {
|
||||
Key j = keys_[pos];
|
||||
Matrix Aj = Ab_(pos);
|
||||
if (model_) Aj = model_->Whiten(Aj);
|
||||
if (model_)
|
||||
Aj = model_->Whiten(Aj);
|
||||
blocks.insert(make_pair(j, Aj.transpose() * Aj));
|
||||
}
|
||||
return blocks;
|
||||
|
|
@ -477,7 +495,8 @@ namespace gtsam {
|
|||
/* ************************************************************************* */
|
||||
Vector JacobianFactor::operator*(const VectorValues& x) const {
|
||||
Vector Ax = zero(Ab_.rows());
|
||||
if (empty()) return Ax;
|
||||
if (empty())
|
||||
return Ax;
|
||||
|
||||
// Just iterate over all A matrices and multiply in correct config part
|
||||
for (size_t pos = 0; pos < size(); ++pos)
|
||||
|
|
@ -488,12 +507,10 @@ namespace gtsam {
|
|||
|
||||
/* ************************************************************************* */
|
||||
void JacobianFactor::transposeMultiplyAdd(double alpha, const Vector& e,
|
||||
VectorValues& x) const
|
||||
{
|
||||
VectorValues& x) const {
|
||||
Vector E = alpha * (model_ ? model_->whiten(e) : e);
|
||||
// Just iterate over all A matrices and insert Ai^e into VectorValues
|
||||
for(size_t pos=0; pos<size(); ++pos)
|
||||
{
|
||||
for (size_t pos = 0; pos < size(); ++pos) {
|
||||
Key j = keys_[pos];
|
||||
// Create the value as a zero vector if it does not exist.
|
||||
pair<VectorValues::iterator, bool> xi = x.tryInsert(j, Vector());
|
||||
|
|
@ -511,29 +528,37 @@ namespace gtsam {
|
|||
transposeMultiplyAdd(alpha, Ax, y);
|
||||
}
|
||||
|
||||
void JacobianFactor::multiplyHessianAdd(double alpha, const double* x, double* y, std::vector<size_t> keys) const {
|
||||
void JacobianFactor::multiplyHessianAdd(double alpha, const double* x,
|
||||
double* y, std::vector<size_t> keys) const {
|
||||
|
||||
// Use eigen magic to access raw memory
|
||||
typedef Eigen::Matrix<double, Eigen::Dynamic, 1> DVector;
|
||||
typedef Eigen::Map<DVector> DMap;
|
||||
typedef Eigen::Map<const DVector> ConstDMap;
|
||||
|
||||
if (empty()) return;
|
||||
if (empty())
|
||||
return;
|
||||
Vector Ax = zero(Ab_.rows());
|
||||
|
||||
// Just iterate over all A matrices and multiply in correct config part
|
||||
for (size_t pos = 0; pos < size(); ++pos)
|
||||
Ax += Ab_(pos) * ConstDMap(x + keys[keys_[pos]],keys[keys_[pos]+1]-keys[keys_[pos]]);
|
||||
Ax += Ab_(pos)
|
||||
* ConstDMap(x + keys[keys_[pos]],
|
||||
keys[keys_[pos] + 1] - keys[keys_[pos]]);
|
||||
|
||||
// Deal with noise properly, need to Double* whiten as we are dividing by variance
|
||||
if (model_) { model_->whitenInPlace(Ax); model_->whitenInPlace(Ax); }
|
||||
if (model_) {
|
||||
model_->whitenInPlace(Ax);
|
||||
model_->whitenInPlace(Ax);
|
||||
}
|
||||
|
||||
// multiply with alpha
|
||||
Ax *= alpha;
|
||||
|
||||
// Again iterate over all A matrices and insert Ai^e into y
|
||||
for (size_t pos = 0; pos < size(); ++pos)
|
||||
DMap(y + keys[keys_[pos]],keys[keys_[pos]+1]-keys[keys_[pos]]) += Ab_(pos).transpose() * Ax;
|
||||
DMap(y + keys[keys_[pos]], keys[keys_[pos] + 1] - keys[keys_[pos]]) += Ab_(
|
||||
pos).transpose() * Ax;
|
||||
|
||||
}
|
||||
|
||||
|
|
@ -594,9 +619,9 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
std::pair<boost::shared_ptr<GaussianConditional>, boost::shared_ptr<JacobianFactor> >
|
||||
JacobianFactor::eliminate(const Ordering& keys)
|
||||
{
|
||||
std::pair<boost::shared_ptr<GaussianConditional>,
|
||||
boost::shared_ptr<JacobianFactor> > JacobianFactor::eliminate(
|
||||
const Ordering& keys) {
|
||||
GaussianFactorGraph graph;
|
||||
graph.add(*this);
|
||||
return EliminateQR(graph, keys);
|
||||
|
|
@ -613,9 +638,9 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
std::pair<boost::shared_ptr<GaussianConditional>, boost::shared_ptr<JacobianFactor> >
|
||||
EliminateQR(const GaussianFactorGraph& factors, const Ordering& keys)
|
||||
{
|
||||
std::pair<boost::shared_ptr<GaussianConditional>,
|
||||
boost::shared_ptr<JacobianFactor> > EliminateQR(
|
||||
const GaussianFactorGraph& factors, const Ordering& keys) {
|
||||
gttic(EliminateQR);
|
||||
// Combine and sort variable blocks in elimination order
|
||||
JacobianFactor::shared_ptr jointFactor;
|
||||
|
|
@ -638,18 +663,20 @@ namespace gtsam {
|
|||
jointFactor->Ab_.matrix().triangularView<Eigen::StrictlyLower>().setZero();
|
||||
|
||||
// Split elimination result into conditional and remaining factor
|
||||
GaussianConditional::shared_ptr conditional = jointFactor->splitConditional(keys.size());
|
||||
GaussianConditional::shared_ptr conditional = jointFactor->splitConditional(
|
||||
keys.size());
|
||||
|
||||
return make_pair(conditional, jointFactor);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
GaussianConditional::shared_ptr JacobianFactor::splitConditional(size_t nrFrontals)
|
||||
{
|
||||
GaussianConditional::shared_ptr JacobianFactor::splitConditional(
|
||||
size_t nrFrontals) {
|
||||
gttic(JacobianFactor_splitConditional);
|
||||
|
||||
if (nrFrontals > size())
|
||||
throw std::invalid_argument("Requesting to split more variables than exist using JacobianFactor::splitConditional");
|
||||
throw std::invalid_argument(
|
||||
"Requesting to split more variables than exist using JacobianFactor::splitConditional");
|
||||
|
||||
DenseIndex frontalDim = Ab_.range(0, nrFrontals).cols();
|
||||
|
||||
|
|
@ -661,15 +688,17 @@ namespace gtsam {
|
|||
if (model_) {
|
||||
if ((DenseIndex) model_->dim() < frontalDim)
|
||||
throw IndeterminantLinearSystemException(this->keys().front());
|
||||
conditionalNoiseModel =
|
||||
noiseModel::Diagonal::Sigmas(model_->sigmas().segment(Ab_.rowStart(), Ab_.rows()));
|
||||
conditionalNoiseModel = noiseModel::Diagonal::Sigmas(
|
||||
model_->sigmas().segment(Ab_.rowStart(), Ab_.rows()));
|
||||
}
|
||||
GaussianConditional::shared_ptr conditional = boost::make_shared<GaussianConditional>(
|
||||
Base::keys_, nrFrontals, Ab_, conditionalNoiseModel);
|
||||
const DenseIndex maxRemainingRows = std::min(Ab_.cols() - 1, originalRowEnd) - Ab_.rowStart() - frontalDim;
|
||||
GaussianConditional::shared_ptr conditional = boost::make_shared<
|
||||
GaussianConditional>(Base::keys_, nrFrontals, Ab_, conditionalNoiseModel);
|
||||
const DenseIndex maxRemainingRows = std::min(Ab_.cols() - 1, originalRowEnd)
|
||||
- Ab_.rowStart() - frontalDim;
|
||||
const DenseIndex remainingRows =
|
||||
model_ ? std::min(model_->sigmas().size() - frontalDim, maxRemainingRows)
|
||||
: maxRemainingRows;
|
||||
model_ ?
|
||||
std::min(model_->sigmas().size() - frontalDim, maxRemainingRows) :
|
||||
maxRemainingRows;
|
||||
Ab_.rowStart() += frontalDim;
|
||||
Ab_.rowEnd() = Ab_.rowStart() + remainingRows;
|
||||
Ab_.firstBlock() += nrFrontals;
|
||||
|
|
@ -681,9 +710,11 @@ namespace gtsam {
|
|||
// Set sigmas with the right model
|
||||
if (model_) {
|
||||
if (model_->isConstrained())
|
||||
model_ = noiseModel::Constrained::MixedSigmas(model_->sigmas().tail(remainingRows));
|
||||
model_ = noiseModel::Constrained::MixedSigmas(
|
||||
model_->sigmas().tail(remainingRows));
|
||||
else
|
||||
model_ = noiseModel::Diagonal::Sigmas(model_->sigmas().tail(remainingRows));
|
||||
model_ = noiseModel::Diagonal::Sigmas(
|
||||
model_->sigmas().tail(remainingRows));
|
||||
assert(model_->dim() == (size_t)Ab_.rows());
|
||||
}
|
||||
gttoc(remaining_factor);
|
||||
|
|
|
|||
Loading…
Reference in New Issue