BORG Formatting
dellaert
parent
2c99f68ed7
commit
850501ed52
|
|
@ -49,183 +49,185 @@
|
|||
|
||||
using namespace std;
|
||||
using namespace boost::assign;
|
||||
namespace br { using namespace boost::range; using namespace boost::adaptors; }
|
||||
namespace br {
|
||||
using namespace boost::range;
|
||||
using namespace boost::adaptors;
|
||||
}
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
HessianFactor::HessianFactor() :
|
||||
info_(cref_list_of<1>(1))
|
||||
{
|
||||
info_(cref_list_of<1>(1)) {
|
||||
linearTerm().setZero();
|
||||
constantTerm() = 0.0;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
HessianFactor::HessianFactor(Key j, const Matrix& G, const Vector& g, double f) :
|
||||
GaussianFactor(cref_list_of<1>(j)), info_(cref_list_of<2>(G.cols())(1))
|
||||
{
|
||||
if(G.rows() != G.cols() || G.rows() != g.size()) throw invalid_argument(
|
||||
"Attempting to construct HessianFactor with inconsistent matrix and/or vector dimensions");
|
||||
info_(0,0) = G;
|
||||
info_(0,1) = g;
|
||||
info_(1,1)(0,0) = f;
|
||||
GaussianFactor(cref_list_of<1>(j)), info_(cref_list_of<2>(G.cols())(1)) {
|
||||
if (G.rows() != G.cols() || G.rows() != g.size())
|
||||
throw invalid_argument(
|
||||
"Attempting to construct HessianFactor with inconsistent matrix and/or vector dimensions");
|
||||
info_(0, 0) = G;
|
||||
info_(0, 1) = g;
|
||||
info_(1, 1)(0, 0) = f;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// error is 0.5*(x-mu)'*inv(Sigma)*(x-mu) = 0.5*(x'*G*x - 2*x'*G*mu + mu'*G*mu)
|
||||
// where G = inv(Sigma), g = G*mu, f = mu'*G*mu = mu'*g
|
||||
HessianFactor::HessianFactor(Key j, const Vector& mu, const Matrix& Sigma) :
|
||||
GaussianFactor(cref_list_of<1>(j)),
|
||||
info_(cref_list_of<2> (Sigma.cols()) (1) )
|
||||
{
|
||||
if (Sigma.rows() != Sigma.cols() || Sigma.rows() != mu.size()) throw invalid_argument(
|
||||
"Attempting to construct HessianFactor with inconsistent matrix and/or vector dimensions");
|
||||
info_(0,0) = Sigma.inverse(); // G
|
||||
info_(0,1) = info_(0,0).selfadjointView() * mu; // g
|
||||
info_(1,1)(0,0) = mu.dot(info_(0,1).knownOffDiagonal().col(0)); // f
|
||||
GaussianFactor(cref_list_of<1>(j)), info_(cref_list_of<2>(Sigma.cols())(1)) {
|
||||
if (Sigma.rows() != Sigma.cols() || Sigma.rows() != mu.size())
|
||||
throw invalid_argument(
|
||||
"Attempting to construct HessianFactor with inconsistent matrix and/or vector dimensions");
|
||||
info_(0, 0) = Sigma.inverse(); // G
|
||||
info_(0, 1) = info_(0, 0).selfadjointView() * mu; // g
|
||||
info_(1, 1)(0, 0) = mu.dot(info_(0, 1).knownOffDiagonal().col(0)); // f
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
HessianFactor::HessianFactor(Key j1, Key j2,
|
||||
const Matrix& G11, const Matrix& G12, const Vector& g1,
|
||||
const Matrix& G22, const Vector& g2, double f) :
|
||||
GaussianFactor(cref_list_of<2>(j1)(j2)),
|
||||
info_(cref_list_of<3> (G11.cols()) (G22.cols()) (1) )
|
||||
{
|
||||
info_(0,0) = G11;
|
||||
info_(0,1) = G12;
|
||||
info_(0,2) = g1;
|
||||
info_(1,1) = G22;
|
||||
info_(1,2) = g2;
|
||||
info_(2,2)(0,0) = f;
|
||||
HessianFactor::HessianFactor(Key j1, Key j2, const Matrix& G11,
|
||||
const Matrix& G12, const Vector& g1, const Matrix& G22, const Vector& g2,
|
||||
double f) :
|
||||
GaussianFactor(cref_list_of<2>(j1)(j2)), info_(
|
||||
cref_list_of<3>(G11.cols())(G22.cols())(1)) {
|
||||
info_(0, 0) = G11;
|
||||
info_(0, 1) = G12;
|
||||
info_(0, 2) = g1;
|
||||
info_(1, 1) = G22;
|
||||
info_(1, 2) = g2;
|
||||
info_(2, 2)(0, 0) = f;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
HessianFactor::HessianFactor(Key j1, Key j2, Key j3,
|
||||
const Matrix& G11, const Matrix& G12, const Matrix& G13, const Vector& g1,
|
||||
const Matrix& G22, const Matrix& G23, const Vector& g2,
|
||||
const Matrix& G33, const Vector& g3, double f) :
|
||||
GaussianFactor(cref_list_of<3>(j1)(j2)(j3)),
|
||||
info_(cref_list_of<4> (G11.cols()) (G22.cols()) (G33.cols()) (1) )
|
||||
{
|
||||
if(G11.rows() != G11.cols() || G11.rows() != G12.rows() || G11.rows() != G13.rows() || G11.rows() != g1.size() ||
|
||||
G22.cols() != G12.cols() || G33.cols() != G13.cols() || G22.cols() != g2.size() || G33.cols() != g3.size())
|
||||
throw invalid_argument("Inconsistent matrix and/or vector dimensions in HessianFactor constructor");
|
||||
info_(0,0) = G11;
|
||||
info_(0,1) = G12;
|
||||
info_(0,2) = G13;
|
||||
info_(0,3) = g1;
|
||||
info_(1,1) = G22;
|
||||
info_(1,2) = G23;
|
||||
info_(1,3) = g2;
|
||||
info_(2,2) = G33;
|
||||
info_(2,3) = g3;
|
||||
info_(3,3)(0,0) = f;
|
||||
HessianFactor::HessianFactor(Key j1, Key j2, Key j3, const Matrix& G11,
|
||||
const Matrix& G12, const Matrix& G13, const Vector& g1, const Matrix& G22,
|
||||
const Matrix& G23, const Vector& g2, const Matrix& G33, const Vector& g3,
|
||||
double f) :
|
||||
GaussianFactor(cref_list_of<3>(j1)(j2)(j3)), info_(
|
||||
cref_list_of<4>(G11.cols())(G22.cols())(G33.cols())(1)) {
|
||||
if (G11.rows() != G11.cols() || G11.rows() != G12.rows()
|
||||
|| G11.rows() != G13.rows() || G11.rows() != g1.size()
|
||||
|| G22.cols() != G12.cols() || G33.cols() != G13.cols()
|
||||
|| G22.cols() != g2.size() || G33.cols() != g3.size())
|
||||
throw invalid_argument(
|
||||
"Inconsistent matrix and/or vector dimensions in HessianFactor constructor");
|
||||
info_(0, 0) = G11;
|
||||
info_(0, 1) = G12;
|
||||
info_(0, 2) = G13;
|
||||
info_(0, 3) = g1;
|
||||
info_(1, 1) = G22;
|
||||
info_(1, 2) = G23;
|
||||
info_(1, 3) = g2;
|
||||
info_(2, 2) = G33;
|
||||
info_(2, 3) = g3;
|
||||
info_(3, 3)(0, 0) = f;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
namespace {
|
||||
DenseIndex _getSizeHF(const Vector& m) { return m.size(); }
|
||||
DenseIndex _getSizeHF(const Vector& m) {
|
||||
return m.size();
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
HessianFactor::HessianFactor(const std::vector<Key>& js, const std::vector<Matrix>& Gs,
|
||||
const std::vector<Vector>& gs, double f) :
|
||||
GaussianFactor(js), info_(gs | br::transformed(&_getSizeHF), true)
|
||||
{
|
||||
HessianFactor::HessianFactor(const std::vector<Key>& js,
|
||||
const std::vector<Matrix>& Gs, const std::vector<Vector>& gs, double f) :
|
||||
GaussianFactor(js), info_(gs | br::transformed(&_getSizeHF), true) {
|
||||
// Get the number of variables
|
||||
size_t variable_count = js.size();
|
||||
|
||||
// Verify the provided number of entries in the vectors are consistent
|
||||
if(gs.size() != variable_count || Gs.size() != (variable_count*(variable_count+1))/2)
|
||||
throw invalid_argument("Inconsistent number of entries between js, Gs, and gs in HessianFactor constructor.\nThe number of keys provided \
|
||||
if (gs.size() != variable_count
|
||||
|| Gs.size() != (variable_count * (variable_count + 1)) / 2)
|
||||
throw invalid_argument(
|
||||
"Inconsistent number of entries between js, Gs, and gs in HessianFactor constructor.\nThe number of keys provided \
|
||||
in js must match the number of linear vector pieces in gs. The number of upper-diagonal blocks in Gs must be n*(n+1)/2");
|
||||
|
||||
// Verify the dimensions of each provided matrix are consistent
|
||||
// Note: equations for calculating the indices derived from the "sum of an arithmetic sequence" formula
|
||||
for(size_t i = 0; i < variable_count; ++i){
|
||||
for (size_t i = 0; i < variable_count; ++i) {
|
||||
DenseIndex block_size = gs[i].size();
|
||||
// Check rows
|
||||
for(size_t j = 0; j < variable_count-i; ++j){
|
||||
size_t index = i*(2*variable_count - i + 1)/2 + j;
|
||||
if(Gs[index].rows() != block_size){
|
||||
throw invalid_argument("Inconsistent matrix and/or vector dimensions in HessianFactor constructor");
|
||||
for (size_t j = 0; j < variable_count - i; ++j) {
|
||||
size_t index = i * (2 * variable_count - i + 1) / 2 + j;
|
||||
if (Gs[index].rows() != block_size) {
|
||||
throw invalid_argument(
|
||||
"Inconsistent matrix and/or vector dimensions in HessianFactor constructor");
|
||||
}
|
||||
}
|
||||
// Check cols
|
||||
for(size_t j = 0; j <= i; ++j){
|
||||
size_t index = j*(2*variable_count - j + 1)/2 + (i-j);
|
||||
if(Gs[index].cols() != block_size){
|
||||
throw invalid_argument("Inconsistent matrix and/or vector dimensions in HessianFactor constructor");
|
||||
for (size_t j = 0; j <= i; ++j) {
|
||||
size_t index = j * (2 * variable_count - j + 1) / 2 + (i - j);
|
||||
if (Gs[index].cols() != block_size) {
|
||||
throw invalid_argument(
|
||||
"Inconsistent matrix and/or vector dimensions in HessianFactor constructor");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Fill in the blocks
|
||||
size_t index = 0;
|
||||
for(size_t i = 0; i < variable_count; ++i){
|
||||
for(size_t j = i; j < variable_count; ++j){
|
||||
for (size_t i = 0; i < variable_count; ++i) {
|
||||
for (size_t j = i; j < variable_count; ++j) {
|
||||
info_(i, j) = Gs[index++];
|
||||
}
|
||||
info_(i, variable_count) = gs[i];
|
||||
}
|
||||
info_(variable_count, variable_count)(0,0) = f;
|
||||
info_(variable_count, variable_count)(0, 0) = f;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
namespace {
|
||||
void _FromJacobianHelper(const JacobianFactor& jf, SymmetricBlockMatrix& info)
|
||||
{
|
||||
void _FromJacobianHelper(const JacobianFactor& jf, SymmetricBlockMatrix& info) {
|
||||
gttic(HessianFactor_fromJacobian);
|
||||
const SharedDiagonal& jfModel = jf.get_model();
|
||||
if(jfModel)
|
||||
{
|
||||
if(jf.get_model()->isConstrained())
|
||||
throw invalid_argument("Cannot construct HessianFactor from JacobianFactor with constrained noise model");
|
||||
info.full().triangularView() = jf.matrixObject().full().transpose() *
|
||||
(jfModel->invsigmas().array() * jfModel->invsigmas().array()).matrix().asDiagonal() *
|
||||
jf.matrixObject().full();
|
||||
if (jfModel) {
|
||||
if (jf.get_model()->isConstrained())
|
||||
throw invalid_argument(
|
||||
"Cannot construct HessianFactor from JacobianFactor with constrained noise model");
|
||||
info.full().triangularView() =
|
||||
jf.matrixObject().full().transpose()
|
||||
* (jfModel->invsigmas().array() * jfModel->invsigmas().array()).matrix().asDiagonal()
|
||||
* jf.matrixObject().full();
|
||||
} else {
|
||||
info.full().triangularView() = jf.matrixObject().full().transpose() * jf.matrixObject().full();
|
||||
info.full().triangularView() = jf.matrixObject().full().transpose()
|
||||
* jf.matrixObject().full();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
HessianFactor::HessianFactor(const JacobianFactor& jf) :
|
||||
GaussianFactor(jf), info_(SymmetricBlockMatrix::LikeActiveViewOf(jf.matrixObject()))
|
||||
{
|
||||
GaussianFactor(jf), info_(
|
||||
SymmetricBlockMatrix::LikeActiveViewOf(jf.matrixObject())) {
|
||||
_FromJacobianHelper(jf, info_);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
HessianFactor::HessianFactor(const GaussianFactor& gf) :
|
||||
GaussianFactor(gf)
|
||||
{
|
||||
GaussianFactor(gf) {
|
||||
// Copy the matrix data depending on what type of factor we're copying from
|
||||
if(const JacobianFactor* jf = dynamic_cast<const JacobianFactor*>(&gf))
|
||||
{
|
||||
if (const JacobianFactor* jf = dynamic_cast<const JacobianFactor*>(&gf)) {
|
||||
info_ = SymmetricBlockMatrix::LikeActiveViewOf(jf->matrixObject());
|
||||
_FromJacobianHelper(*jf, info_);
|
||||
}
|
||||
else if(const HessianFactor* hf = dynamic_cast<const HessianFactor*>(&gf))
|
||||
{
|
||||
} else if (const HessianFactor* hf = dynamic_cast<const HessianFactor*>(&gf)) {
|
||||
info_ = hf->info_;
|
||||
}
|
||||
else
|
||||
{
|
||||
throw std::invalid_argument("In HessianFactor(const GaussianFactor& gf), gf is neither a JacobianFactor nor a HessianFactor");
|
||||
} else {
|
||||
throw std::invalid_argument(
|
||||
"In HessianFactor(const GaussianFactor& gf), gf is neither a JacobianFactor nor a HessianFactor");
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
HessianFactor::HessianFactor(const GaussianFactorGraph& factors,
|
||||
boost::optional<const Scatter&> scatter)
|
||||
{
|
||||
boost::optional<const Scatter&> scatter) {
|
||||
gttic(HessianFactor_MergeConstructor);
|
||||
boost::optional<Scatter> computedScatter;
|
||||
if(!scatter) {
|
||||
if (!scatter) {
|
||||
computedScatter = Scatter(factors);
|
||||
scatter = computedScatter;
|
||||
}
|
||||
|
|
@ -247,45 +249,46 @@ HessianFactor::HessianFactor(const GaussianFactorGraph& factors,
|
|||
// Form A' * A
|
||||
gttic(update);
|
||||
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors)
|
||||
if(factor)
|
||||
if (factor)
|
||||
factor->updateHessian(keys_, &info_);
|
||||
gttoc(update);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void HessianFactor::print(const std::string& s, const KeyFormatter& formatter) const {
|
||||
void HessianFactor::print(const std::string& s,
|
||||
const KeyFormatter& formatter) const {
|
||||
cout << s << "\n";
|
||||
cout << " keys: ";
|
||||
for(const_iterator key=begin(); key!=end(); ++key)
|
||||
for (const_iterator key = begin(); key != end(); ++key)
|
||||
cout << formatter(*key) << "(" << getDim(key) << ") ";
|
||||
cout << "\n";
|
||||
gtsam::print(Matrix(info_.full().selfadjointView()), "Augmented information matrix: ");
|
||||
gtsam::print(Matrix(info_.full().selfadjointView()),
|
||||
"Augmented information matrix: ");
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool HessianFactor::equals(const GaussianFactor& lf, double tol) const {
|
||||
if(!dynamic_cast<const HessianFactor*>(&lf))
|
||||
if (!dynamic_cast<const HessianFactor*>(&lf))
|
||||
return false;
|
||||
else {
|
||||
if(!Factor::equals(lf, tol))
|
||||
if (!Factor::equals(lf, tol))
|
||||
return false;
|
||||
Matrix thisMatrix = info_.full().selfadjointView();
|
||||
thisMatrix(thisMatrix.rows()-1, thisMatrix.cols()-1) = 0.0;
|
||||
Matrix rhsMatrix = static_cast<const HessianFactor&>(lf).info_.full().selfadjointView();
|
||||
rhsMatrix(rhsMatrix.rows()-1, rhsMatrix.cols()-1) = 0.0;
|
||||
thisMatrix(thisMatrix.rows() - 1, thisMatrix.cols() - 1) = 0.0;
|
||||
Matrix rhsMatrix =
|
||||
static_cast<const HessianFactor&>(lf).info_.full().selfadjointView();
|
||||
rhsMatrix(rhsMatrix.rows() - 1, rhsMatrix.cols() - 1) = 0.0;
|
||||
return equal_with_abs_tol(thisMatrix, rhsMatrix, tol);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix HessianFactor::augmentedInformation() const
|
||||
{
|
||||
Matrix HessianFactor::augmentedInformation() const {
|
||||
return info_.full().selfadjointView();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix HessianFactor::information() const
|
||||
{
|
||||
Matrix HessianFactor::information() const {
|
||||
return info_.range(0, size(), 0, size()).selfadjointView();
|
||||
}
|
||||
|
||||
|
|
@ -293,10 +296,10 @@ Matrix HessianFactor::information() const
|
|||
VectorValues HessianFactor::hessianDiagonal() const {
|
||||
VectorValues d;
|
||||
// Loop over all variables
|
||||
for (DenseIndex j = 0; j < (DenseIndex)size(); ++j) {
|
||||
for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) {
|
||||
// Get the diagonal block, and insert its diagonal
|
||||
Matrix B = info_(j, j).selfadjointView();
|
||||
d.insert(keys_[j],B.diagonal());
|
||||
d.insert(keys_[j], B.diagonal());
|
||||
}
|
||||
return d;
|
||||
}
|
||||
|
|
@ -309,26 +312,24 @@ void HessianFactor::hessianDiagonal(double* d) const {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
map<Key,Matrix> HessianFactor::hessianBlockDiagonal() const {
|
||||
map<Key,Matrix> blocks;
|
||||
map<Key, Matrix> HessianFactor::hessianBlockDiagonal() const {
|
||||
map<Key, Matrix> blocks;
|
||||
// Loop over all variables
|
||||
for (DenseIndex j = 0; j < (DenseIndex)size(); ++j) {
|
||||
for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) {
|
||||
// Get the diagonal block, and insert it
|
||||
Matrix B = info_(j, j).selfadjointView();
|
||||
blocks.insert(make_pair(keys_[j],B));
|
||||
blocks.insert(make_pair(keys_[j], B));
|
||||
}
|
||||
return blocks;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix HessianFactor::augmentedJacobian() const
|
||||
{
|
||||
Matrix HessianFactor::augmentedJacobian() const {
|
||||
return JacobianFactor(*this).augmentedJacobian();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
std::pair<Matrix, Vector> HessianFactor::jacobian() const
|
||||
{
|
||||
std::pair<Matrix, Vector> HessianFactor::jacobian() const {
|
||||
return JacobianFactor(*this).jacobian();
|
||||
}
|
||||
|
||||
|
|
@ -341,13 +342,13 @@ double HessianFactor::error(const VectorValues& c) const {
|
|||
// NOTE may not be as efficient
|
||||
const Vector x = c.vector(keys());
|
||||
xtg = x.dot(linearTerm());
|
||||
xGx = x.transpose() * info_.range(0, size(), 0, size()).selfadjointView() * x;
|
||||
return 0.5 * (f - 2.0 * xtg + xGx);
|
||||
xGx = x.transpose() * info_.range(0, size(), 0, size()).selfadjointView() * x;
|
||||
return 0.5 * (f - 2.0 * xtg + xGx);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void HessianFactor::updateHessian(const FastVector<Key>& infoKeys,
|
||||
SymmetricBlockMatrix* info) const {
|
||||
SymmetricBlockMatrix* info) const {
|
||||
gttic(updateHessian_HessianFactor);
|
||||
// Apply updates to the upper triangle
|
||||
DenseIndex n = size(), N = info->nBlocks() - 1;
|
||||
|
|
@ -356,17 +357,17 @@ void HessianFactor::updateHessian(const FastVector<Key>& infoKeys,
|
|||
const DenseIndex J = (j == n) ? N : Slot(infoKeys, keys_[j]);
|
||||
slots[j] = J;
|
||||
for (DenseIndex i = 0; i <= j; ++i) {
|
||||
const DenseIndex I = slots[i]; // because i<=j, slots[i] is valid.
|
||||
const DenseIndex I = slots[i]; // because i<=j, slots[i] is valid.
|
||||
(*info)(I, J) += info_(i, j);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
GaussianFactor::shared_ptr HessianFactor::negate() const
|
||||
{
|
||||
GaussianFactor::shared_ptr HessianFactor::negate() const {
|
||||
shared_ptr result = boost::make_shared<This>(*this);
|
||||
result->info_.full().triangularView() = -result->info_.full().triangularView().nestedExpression(); // Negate the information matrix of the result
|
||||
result->info_.full().triangularView() =
|
||||
-result->info_.full().triangularView().nestedExpression(); // Negate the information matrix of the result
|
||||
return result;
|
||||
}
|
||||
|
||||
|
|
@ -383,7 +384,7 @@ void HessianFactor::multiplyHessianAdd(double alpha, const VectorValues& x,
|
|||
// Accessing the VectorValues one by one is expensive
|
||||
// So we will loop over columns to access x only once per column
|
||||
// And fill the above temporary y values, to be added into yvalues after
|
||||
for (DenseIndex j = 0; j < (DenseIndex)size(); ++j) {
|
||||
for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) {
|
||||
// xj is the input vector
|
||||
Vector xj = x.at(keys_[j]);
|
||||
DenseIndex i = 0;
|
||||
|
|
@ -392,13 +393,13 @@ void HessianFactor::multiplyHessianAdd(double alpha, const VectorValues& x,
|
|||
// blocks on the diagonal are only half
|
||||
y[i] += info_(j, j).selfadjointView() * xj;
|
||||
// for below diagonal, we take transpose block from upper triangular part
|
||||
for (i = j + 1; i < (DenseIndex)size(); ++i)
|
||||
for (i = j + 1; i < (DenseIndex) size(); ++i)
|
||||
y[i] += info_(i, j).knownOffDiagonal() * xj;
|
||||
}
|
||||
|
||||
// copy to yvalues
|
||||
for(DenseIndex i = 0; i < (DenseIndex)size(); ++i) {
|
||||
bool didNotExist;
|
||||
for (DenseIndex i = 0; i < (DenseIndex) size(); ++i) {
|
||||
bool didNotExist;
|
||||
VectorValues::iterator it;
|
||||
boost::tie(it, didNotExist) = yvalues.tryInsert(keys_[i], Vector());
|
||||
if (didNotExist)
|
||||
|
|
@ -413,7 +414,7 @@ VectorValues HessianFactor::gradientAtZero() const {
|
|||
VectorValues g;
|
||||
size_t n = size();
|
||||
for (size_t j = 0; j < n; ++j)
|
||||
g.insert(keys_[j], -info_(j,n).knownOffDiagonal());
|
||||
g.insert(keys_[j], -info_(j, n).knownOffDiagonal());
|
||||
return g;
|
||||
}
|
||||
|
||||
|
|
@ -436,8 +437,7 @@ Vector HessianFactor::gradient(Key key, const VectorValues& x) const {
|
|||
if (i > j) {
|
||||
Matrix Gji = info(j, i);
|
||||
Gij = Gji.transpose();
|
||||
}
|
||||
else {
|
||||
} else {
|
||||
Gij = info(i, j);
|
||||
}
|
||||
// Accumulate Gij*xj to gradf
|
||||
|
|
@ -449,30 +449,34 @@ Vector HessianFactor::gradient(Key key, const VectorValues& x) const {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
std::pair<boost::shared_ptr<GaussianConditional>, boost::shared_ptr<HessianFactor> >
|
||||
EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys)
|
||||
{
|
||||
std::pair<boost::shared_ptr<GaussianConditional>,
|
||||
boost::shared_ptr<HessianFactor> > EliminateCholesky(
|
||||
const GaussianFactorGraph& factors, const Ordering& keys) {
|
||||
gttic(EliminateCholesky);
|
||||
|
||||
// Build joint factor
|
||||
HessianFactor::shared_ptr jointFactor;
|
||||
try {
|
||||
jointFactor = boost::make_shared<HessianFactor>(factors, Scatter(factors, keys));
|
||||
} catch(std::invalid_argument&) {
|
||||
jointFactor = boost::make_shared<HessianFactor>(factors,
|
||||
Scatter(factors, keys));
|
||||
} catch (std::invalid_argument&) {
|
||||
throw InvalidDenseElimination(
|
||||
"EliminateCholesky was called with a request to eliminate variables that are not\n"
|
||||
"involved in the provided factors.");
|
||||
"involved in the provided factors.");
|
||||
}
|
||||
|
||||
// Do dense elimination
|
||||
GaussianConditional::shared_ptr conditional;
|
||||
try {
|
||||
size_t numberOfKeysToEliminate = keys.size();
|
||||
VerticalBlockMatrix Ab = jointFactor->info_.choleskyPartial(numberOfKeysToEliminate);
|
||||
conditional = boost::make_shared<GaussianConditional>(jointFactor->keys(), numberOfKeysToEliminate, Ab);
|
||||
VerticalBlockMatrix Ab = jointFactor->info_.choleskyPartial(
|
||||
numberOfKeysToEliminate);
|
||||
conditional = boost::make_shared<GaussianConditional>(jointFactor->keys(),
|
||||
numberOfKeysToEliminate, Ab);
|
||||
// Erase the eliminated keys in the remaining factor
|
||||
jointFactor->keys_.erase(jointFactor->begin(), jointFactor->begin() + numberOfKeysToEliminate);
|
||||
} catch(CholeskyFailed&) {
|
||||
jointFactor->keys_.erase(jointFactor->begin(),
|
||||
jointFactor->begin() + numberOfKeysToEliminate);
|
||||
} catch (CholeskyFailed&) {
|
||||
throw IndeterminantLinearSystemException(keys.front());
|
||||
}
|
||||
|
||||
|
|
@ -481,9 +485,9 @@ EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys)
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
std::pair<boost::shared_ptr<GaussianConditional>, boost::shared_ptr<GaussianFactor> >
|
||||
EliminatePreferCholesky(const GaussianFactorGraph& factors, const Ordering& keys)
|
||||
{
|
||||
std::pair<boost::shared_ptr<GaussianConditional>,
|
||||
boost::shared_ptr<GaussianFactor> > EliminatePreferCholesky(
|
||||
const GaussianFactorGraph& factors, const Ordering& keys) {
|
||||
gttic(EliminatePreferCholesky);
|
||||
|
||||
// If any JacobianFactors have constrained noise models, we have to convert
|
||||
|
|
|
|||
Loading…
Reference in New Issue