Working on HessianFactor
Richard Roberts
parent
af8f302402
commit
4ea9fda03b
|
|
@ -40,14 +40,15 @@
|
|||
#endif
|
||||
#include <boost/assign/list_of.hpp>
|
||||
#include <boost/range/adaptor/transformed.hpp>
|
||||
#include <boost/range/adaptor/map.hpp>
|
||||
#include <boost/range/join.hpp>
|
||||
#include <boost/range/algorithm/copy.hpp>
|
||||
|
||||
#include <sstream>
|
||||
|
||||
using namespace std;
|
||||
using namespace boost::assign;
|
||||
using boost::adaptors::transformed;
|
||||
namespace br = boost::range;
|
||||
namespace br { using namespace boost::range; using namespace boost::adaptors; }
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
|
@ -154,7 +155,7 @@ namespace { DenseIndex _getColsHF(const Matrix& m) { return m.cols(); } }
|
|||
/* ************************************************************************* */
|
||||
HessianFactor::HessianFactor(const std::vector<Key>& js, const std::vector<Matrix>& Gs,
|
||||
const std::vector<Vector>& gs, double f) :
|
||||
GaussianFactor(js), info_(br::join(Gs | transformed(&_getColsHF), cref_list_of<1,DenseIndex>(1)))
|
||||
GaussianFactor(js), info_(br::join(Gs | br::transformed(&_getColsHF), cref_list_of<1,DenseIndex>(1)))
|
||||
{
|
||||
// Get the number of variables
|
||||
size_t variable_count = js.size();
|
||||
|
|
@ -196,82 +197,86 @@ GaussianFactor(js), info_(br::join(Gs | transformed(&_getColsHF), cref_list_of<1
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
HessianFactor::HessianFactor(const JacobianFactor& jf)
|
||||
namespace {
|
||||
void _FromJacobianHelper(const JacobianFactor& jf, SymmetricBlockMatrix& info)
|
||||
{
|
||||
throw std::runtime_error("Not implemented");
|
||||
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() = jf.matrixObject().full().transpose() * jfModel->invsigmas().asDiagonal() *
|
||||
jfModel->invsigmas().asDiagonal() * jf.matrixObject().full();
|
||||
} else {
|
||||
info.full() = jf.matrixObject().full().transpose() * jf.matrixObject().full();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
HessianFactor::HessianFactor(const GaussianFactor& gf)
|
||||
HessianFactor::HessianFactor(const JacobianFactor& jf) :
|
||||
GaussianFactor(jf), info_(SymmetricBlockMatrix::LikeActiveViewOf(jf.matrixObject()))
|
||||
{
|
||||
throw std::runtime_error("Not implemented");
|
||||
//// Copy the variable indices
|
||||
//(GaussianFactorOrdered&)(*this) = gf;
|
||||
//// Copy the matrix data depending on what type of factor we're copying from
|
||||
//if(dynamic_cast<const JacobianFactorOrdered*>(&gf)) {
|
||||
// const JacobianFactorOrdered& jf(static_cast<const JacobianFactorOrdered&>(gf));
|
||||
// if(jf.model_->isConstrained())
|
||||
// throw invalid_argument("Cannot construct HessianFactor from JacobianFactor with constrained noise model");
|
||||
// else {
|
||||
// Vector invsigmas = jf.model_->invsigmas().cwiseProduct(jf.model_->invsigmas());
|
||||
// info_.copyStructureFrom(jf.Ab_);
|
||||
// BlockInfo::constBlock A = jf.Ab_.full();
|
||||
// matrix_.noalias() = A.transpose() * invsigmas.asDiagonal() * A;
|
||||
// }
|
||||
//} else if(dynamic_cast<const HessianFactorOrdered*>(&gf)) {
|
||||
// const HessianFactorOrdered& hf(static_cast<const HessianFactorOrdered&>(gf));
|
||||
// info_.assignNoalias(hf.info_);
|
||||
//} else
|
||||
// throw std::invalid_argument("In HessianFactor(const GaussianFactor& gf), gf is neither a JacobianFactor nor a HessianFactor");
|
||||
//assertInvariants();
|
||||
_FromJacobianHelper(jf, info_);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
HessianFactor::HessianFactor(const 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))
|
||||
{
|
||||
info_ = SymmetricBlockMatrix::LikeActiveViewOf(jf->matrixObject());
|
||||
_FromJacobianHelper(*jf, info_);
|
||||
}
|
||||
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");
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
HessianFactor::HessianFactor(const GaussianFactorGraph& factors)
|
||||
{
|
||||
throw std::runtime_error("Not implemented");
|
||||
Scatter scatter(factors);
|
||||
|
||||
//Scatter scatter(factors);
|
||||
// Pull out keys and dimensions
|
||||
gttic(keys);
|
||||
vector<size_t> dimensions(scatter.size() + 1);
|
||||
BOOST_FOREACH(const Scatter::value_type& var_slot, scatter) {
|
||||
dimensions[var_slot.second.slot] = var_slot.second.dimension;
|
||||
}
|
||||
// This is for the r.h.s. vector
|
||||
dimensions.back() = 1;
|
||||
gttoc(keys);
|
||||
|
||||
//// Pull out keys and dimensions
|
||||
//gttic(keys);
|
||||
//vector<size_t> dimensions(scatter.size() + 1);
|
||||
//BOOST_FOREACH(const Scatter::value_type& var_slot, scatter) {
|
||||
// dimensions[var_slot.second.slot] = var_slot.second.dimension;
|
||||
//}
|
||||
//// This is for the r.h.s. vector
|
||||
//dimensions.back() = 1;
|
||||
//gttoc(keys);
|
||||
// Allocate and copy keys
|
||||
gttic(allocate);
|
||||
info_ = SymmetricBlockMatrix(dimensions);
|
||||
info_.full().setZero();
|
||||
keys_.resize(scatter.size());
|
||||
br::copy(scatter | br::map_keys, keys_.begin());
|
||||
gttoc(allocate);
|
||||
|
||||
//const bool debug = ISDEBUG("EliminateCholesky");
|
||||
//// Form Ab' * Ab
|
||||
//gttic(allocate);
|
||||
//info_.resize(dimensions.begin(), dimensions.end(), false);
|
||||
//// Fill in keys
|
||||
//keys_.resize(scatter.size());
|
||||
//std::transform(scatter.begin(), scatter.end(), keys_.begin(), boost::bind(&Scatter::value_type::first, ::_1));
|
||||
//gttoc(allocate);
|
||||
//gttic(zero);
|
||||
//matrix_.noalias() = Matrix::Zero(matrix_.rows(),matrix_.cols());
|
||||
//gttoc(zero);
|
||||
//gttic(update);
|
||||
//if (debug) cout << "Combining " << factors.size() << " factors" << endl;
|
||||
//BOOST_FOREACH(const GaussianFactorOrdered::shared_ptr& factor, factors)
|
||||
//{
|
||||
// if(factor) {
|
||||
// if(shared_ptr hessian = boost::dynamic_pointer_cast<HessianFactorOrdered>(factor))
|
||||
// updateATA(*hessian, scatter);
|
||||
// else if(JacobianFactorOrdered::shared_ptr jacobianFactor = boost::dynamic_pointer_cast<JacobianFactorOrdered>(factor))
|
||||
// updateATA(*jacobianFactor, scatter);
|
||||
// else
|
||||
// throw invalid_argument("GaussianFactor is neither Hessian nor Jacobian");
|
||||
// }
|
||||
//}
|
||||
//gttoc(update);
|
||||
|
||||
//if (debug) gtsam::print(matrix_, "Ab' * Ab: ");
|
||||
|
||||
//assertInvariants();
|
||||
// Form A' * A
|
||||
gttic(update);
|
||||
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors)
|
||||
{
|
||||
if(factor) {
|
||||
if(const HessianFactor* hessian = dynamic_cast<const HessianFactor*>(factor.get()))
|
||||
updateATA(*hessian, scatter);
|
||||
else if(const JacobianFactor* jacobian = dynamic_cast<const JacobianFactor*>(factor.get()))
|
||||
updateATA(*jacobian, scatter);
|
||||
else
|
||||
throw invalid_argument("GaussianFactor is neither Hessian nor Jacobian");
|
||||
}
|
||||
}
|
||||
gttoc(update);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -298,25 +303,27 @@ bool HessianFactor::equals(const GaussianFactor& lf, double tol) const {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix HessianFactor::augmentedInformation() const {
|
||||
Matrix HessianFactor::augmentedInformation() const
|
||||
{
|
||||
return info_.full().selfadjointView<Eigen::Upper>();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix HessianFactor::information() const {
|
||||
Matrix HessianFactor::information() const
|
||||
{
|
||||
return info_.range(0, this->size(), 0, this->size()).selfadjointView<Eigen::Upper>();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix HessianFactor::augmentedJacobian() const
|
||||
{
|
||||
throw std::runtime_error("Not implemented");
|
||||
return JacobianFactor(*this).augmentedJacobian();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
std::pair<Matrix, Vector> HessianFactor::jacobian() const
|
||||
{
|
||||
throw std::runtime_error("Not implemented");
|
||||
return JacobianFactor(*this).jacobian();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -335,136 +342,41 @@ double HessianFactor::error(const VectorValues& c) const {
|
|||
/* ************************************************************************* */
|
||||
void HessianFactor::updateATA(const HessianFactor& update, const Scatter& scatter)
|
||||
{
|
||||
// This function updates 'combined' with the information in 'update'. 'scatter' maps variables in
|
||||
// the update factor to slots in the combined factor.
|
||||
|
||||
throw std::runtime_error("Not implemented");
|
||||
|
||||
//// This function updates 'combined' with the information in 'update'.
|
||||
//// 'scatter' maps variables in the update factor to slots in the combined
|
||||
//// factor.
|
||||
|
||||
//const bool debug = ISDEBUG("updateATA");
|
||||
|
||||
//// First build an array of slots
|
||||
//gttic(slots);
|
||||
// First build an array of slots
|
||||
gttic(slots);
|
||||
//size_t* slots = (size_t*)alloca(sizeof(size_t)*update.size()); // FIXME: alloca is bad, just ask Google.
|
||||
//size_t slot = 0;
|
||||
//BOOST_FOREACH(Index j, update) {
|
||||
// slots[slot] = scatter.find(j)->second.slot;
|
||||
// ++ slot;
|
||||
//}
|
||||
//gttoc(slots);
|
||||
vector<size_t> slots(update.size());
|
||||
size_t slot = 0;
|
||||
BOOST_FOREACH(Index j, update) {
|
||||
slots[slot] = scatter.find(j)->second.slot;
|
||||
++ slot;
|
||||
}
|
||||
gttoc(slots);
|
||||
|
||||
//if(debug) {
|
||||
// this->print("Updating this: ");
|
||||
// update.print("with (Hessian): ");
|
||||
//}
|
||||
|
||||
//// Apply updates to the upper triangle
|
||||
//gttic(update);
|
||||
//for(size_t j2=0; j2<update.info_.nBlocks(); ++j2) {
|
||||
// size_t slot2 = (j2 == update.size()) ? this->info_.nBlocks()-1 : slots[j2];
|
||||
// for(size_t j1=0; j1<=j2; ++j1) {
|
||||
// size_t slot1 = (j1 == update.size()) ? this->info_.nBlocks()-1 : slots[j1];
|
||||
// if(slot2 > slot1) {
|
||||
// if(debug)
|
||||
// cout << "Updating (" << slot1 << "," << slot2 << ") from (" << j1 << "," << j2 << ")" << endl;
|
||||
// matrix_.block(info_.offset(slot1), info_.offset(slot2), info_(slot1,slot2).rows(), info_(slot1,slot2).cols()).noalias() +=
|
||||
// update.matrix_.block(update.info_.offset(j1), update.info_.offset(j2), update.info_(j1,j2).rows(), update.info_(j1,j2).cols());
|
||||
// } else if(slot1 > slot2) {
|
||||
// if(debug)
|
||||
// cout << "Updating (" << slot2 << "," << slot1 << ") from (" << j1 << "," << j2 << ")" << endl;
|
||||
// matrix_.block(info_.offset(slot2), info_.offset(slot1), info_(slot2,slot1).rows(), info_(slot2,slot1).cols()).noalias() +=
|
||||
// update.matrix_.block(update.info_.offset(j1), update.info_.offset(j2), update.info_(j1,j2).rows(), update.info_(j1,j2).cols()).transpose();
|
||||
// } else {
|
||||
// if(debug)
|
||||
// cout << "Updating (" << slot1 << "," << slot2 << ") from (" << j1 << "," << j2 << ")" << endl;
|
||||
// matrix_.block(info_.offset(slot1), info_.offset(slot2), info_(slot1,slot2).rows(), info_(slot1,slot2).cols()).triangularView<Eigen::Upper>() +=
|
||||
// update.matrix_.block(update.info_.offset(j1), update.info_.offset(j2), update.info_(j1,j2).rows(), update.info_(j1,j2).cols());
|
||||
// }
|
||||
// if(debug) cout << "Updating block " << slot1 << "," << slot2 << " from block " << j1 << "," << j2 << "\n";
|
||||
// if(debug) this->print();
|
||||
// }
|
||||
//}
|
||||
//gttoc(update);
|
||||
// Apply updates to the upper triangle
|
||||
gttic(update);
|
||||
for(size_t j2=0; j2<update.info_.nBlocks(); ++j2) {
|
||||
size_t slot2 = (j2 == update.size()) ? this->info_.nBlocks()-1 : slots[j2];
|
||||
for(size_t j1=0; j1<=j2; ++j1) {
|
||||
size_t slot1 = (j1 == update.size()) ? this->info_.nBlocks()-1 : slots[j1];
|
||||
if(slot2 > slot1)
|
||||
info_(slot1, slot2).noalias() += update.info_(j1, j2);
|
||||
else if(slot1 > slot2)
|
||||
info_(slot2, slot1).noalias() += update.info_(j1, j2).transpose();
|
||||
else
|
||||
info_(slot1, slot2).triangularView<Eigen::Upper>() += update.info_(j1, j2);
|
||||
}
|
||||
}
|
||||
gttoc(update);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void HessianFactor::updateATA(const JacobianFactor& update, const Scatter& scatter)
|
||||
{
|
||||
throw std::runtime_error("Not implemented");
|
||||
|
||||
//// This function updates 'combined' with the information in 'update'.
|
||||
//// 'scatter' maps variables in the update factor to slots in the combined
|
||||
//// factor.
|
||||
|
||||
//const bool debug = ISDEBUG("updateATA");
|
||||
|
||||
//// First build an array of slots
|
||||
//gttic(slots);
|
||||
//size_t* slots = (size_t*)alloca(sizeof(size_t)*update.size()); // FIXME: alloca is bad, just ask Google.
|
||||
//size_t slot = 0;
|
||||
//BOOST_FOREACH(Index j, update) {
|
||||
// slots[slot] = scatter.find(j)->second.slot;
|
||||
// ++ slot;
|
||||
//}
|
||||
//gttoc(slots);
|
||||
|
||||
//gttic(form_ATA);
|
||||
//if(update.model_->isConstrained())
|
||||
// throw invalid_argument("Cannot update HessianFactor from JacobianFactor with constrained noise model");
|
||||
|
||||
//if(debug) {
|
||||
// this->print("Updating this: ");
|
||||
// update.print("with (Jacobian): ");
|
||||
//}
|
||||
|
||||
//typedef Eigen::Block<const JacobianFactorOrdered::AbMatrix> BlockUpdateMatrix;
|
||||
//BlockUpdateMatrix updateA(update.matrix_.block(
|
||||
// update.Ab_.rowStart(),update.Ab_.offset(0), update.Ab_.full().rows(), update.Ab_.full().cols()));
|
||||
//if (debug) cout << "updateA: \n" << updateA << endl;
|
||||
|
||||
//Matrix updateInform;
|
||||
//if(boost::dynamic_pointer_cast<noiseModel::Unit>(update.model_)) {
|
||||
// updateInform.noalias() = updateA.transpose() * updateA;
|
||||
//} else {
|
||||
// noiseModel::Diagonal::shared_ptr diagonal(boost::dynamic_pointer_cast<noiseModel::Diagonal>(update.model_));
|
||||
// if(diagonal) {
|
||||
// Vector invsigmas2 = update.model_->invsigmas().cwiseProduct(update.model_->invsigmas());
|
||||
// updateInform.noalias() = updateA.transpose() * invsigmas2.asDiagonal() * updateA;
|
||||
// } else
|
||||
// throw invalid_argument("In HessianFactor::updateATA, JacobianFactor noise model is neither Unit nor Diagonal");
|
||||
//}
|
||||
//if (debug) cout << "updateInform: \n" << updateInform << endl;
|
||||
// gttoc(form_ATA);
|
||||
|
||||
//// Apply updates to the upper triangle
|
||||
//gttic(update);
|
||||
//for(size_t j2=0; j2<update.Ab_.nBlocks(); ++j2) {
|
||||
// size_t slot2 = (j2 == update.size()) ? this->info_.nBlocks()-1 : slots[j2];
|
||||
// for(size_t j1=0; j1<=j2; ++j1) {
|
||||
// size_t slot1 = (j1 == update.size()) ? this->info_.nBlocks()-1 : slots[j1];
|
||||
// size_t off0 = update.Ab_.offset(0);
|
||||
// if(slot2 > slot1) {
|
||||
// if(debug)
|
||||
// cout << "Updating (" << slot1 << "," << slot2 << ") from (" << j1 << "," << j2 << ")" << endl;
|
||||
// matrix_.block(info_.offset(slot1), info_.offset(slot2), info_(slot1,slot2).rows(), info_(slot1,slot2).cols()).noalias() +=
|
||||
// updateInform.block(update.Ab_.offset(j1)-off0, update.Ab_.offset(j2)-off0, update.Ab_(j1).cols(), update.Ab_(j2).cols());
|
||||
// } else if(slot1 > slot2) {
|
||||
// if(debug)
|
||||
// cout << "Updating (" << slot2 << "," << slot1 << ") from (" << j1 << "," << j2 << ")" << endl;
|
||||
// matrix_.block(info_.offset(slot2), info_.offset(slot1), info_(slot2,slot1).rows(), info_(slot2,slot1).cols()).noalias() +=
|
||||
// updateInform.block(update.Ab_.offset(j1)-off0, update.Ab_.offset(j2)-off0, update.Ab_(j1).cols(), update.Ab_(j2).cols()).transpose();
|
||||
// } else {
|
||||
// if(debug)
|
||||
// cout << "Updating (" << slot1 << "," << slot2 << ") from (" << j1 << "," << j2 << ")" << endl;
|
||||
// matrix_.block(info_.offset(slot1), info_.offset(slot2), info_(slot1,slot2).rows(), info_(slot1,slot2).cols()).triangularView<Eigen::Upper>() +=
|
||||
// updateInform.block(update.Ab_.offset(j1)-off0, update.Ab_.offset(j2)-off0, update.Ab_(j1).cols(), update.Ab_(j2).cols());
|
||||
// }
|
||||
// if(debug) cout << "Updating block " << slot1 << "," << slot2 << " from block " << j1 << "," << j2 << "\n";
|
||||
// if(debug) this->print();
|
||||
// }
|
||||
//}
|
||||
//gttoc(update);
|
||||
updateATA(HessianFactor(update), scatter);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -479,38 +391,36 @@ void HessianFactor::partialCholesky(size_t nrFrontals)
|
|||
/* ************************************************************************* */
|
||||
GaussianConditional::shared_ptr HessianFactor::splitEliminatedFactor(size_t nrFrontals)
|
||||
{
|
||||
throw std::runtime_error("Not implemented");
|
||||
static const bool debug = false;
|
||||
|
||||
//static const bool debug = false;
|
||||
// Extract conditionals
|
||||
gttic(extract_conditionals);
|
||||
typedef VerticalBlockView<Matrix> BlockAb;
|
||||
BlockAb Ab(matrix_, info_);
|
||||
|
||||
//// Extract conditionals
|
||||
//gttic(extract_conditionals);
|
||||
//GaussianConditionalOrdered::shared_ptr conditional(new GaussianConditionalOrdered());
|
||||
//typedef VerticalBlockView<Matrix> BlockAb;
|
||||
//BlockAb Ab(matrix_, info_);
|
||||
size_t varDim = info_.offset(nrFrontals);
|
||||
Ab.rowEnd() = Ab.rowStart() + varDim;
|
||||
|
||||
//size_t varDim = info_.offset(nrFrontals);
|
||||
//Ab.rowEnd() = Ab.rowStart() + varDim;
|
||||
// Create one big conditionals with many frontal variables.
|
||||
gttic(construct_cond);
|
||||
|
||||
//// Create one big conditionals with many frontal variables.
|
||||
//gttic(construct_cond);
|
||||
//Vector sigmas = Vector::Ones(varDim);
|
||||
//conditional = boost::make_shared<ConditionalType>(keys_.begin(), keys_.end(), nrFrontals, Ab, sigmas);
|
||||
//gttoc(construct_cond);
|
||||
//if(debug) conditional->print("Extracted conditional: ");
|
||||
VerticalBlockMatrix Ab()
|
||||
GaussianConditional::shared_ptr conditional = boost::make_shared<GaussianConditional>(
|
||||
keys_, nrFrontals, Ab);
|
||||
gttoc(construct_cond);
|
||||
|
||||
//gttoc(extract_conditionals);
|
||||
gttoc(extract_conditionals);
|
||||
|
||||
//// Take lower-right block of Ab_ to get the new factor
|
||||
//gttic(remaining_factor);
|
||||
//info_.blockStart() = nrFrontals;
|
||||
//// Assign the keys
|
||||
//vector<Index> remainingKeys(keys_.size() - nrFrontals);
|
||||
//remainingKeys.assign(keys_.begin() + nrFrontals, keys_.end());
|
||||
//keys_.swap(remainingKeys);
|
||||
//gttoc(remaining_factor);
|
||||
// Take lower-right block of Ab_ to get the new factor
|
||||
gttic(remaining_factor);
|
||||
info_.blockStart() = nrFrontals;
|
||||
// Assign the keys
|
||||
vector<Index> remainingKeys(keys_.size() - nrFrontals);
|
||||
remainingKeys.assign(keys_.begin() + nrFrontals, keys_.end());
|
||||
keys_.swap(remainingKeys);
|
||||
gttoc(remaining_factor);
|
||||
|
||||
//return conditional;
|
||||
return conditional;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -187,20 +187,19 @@ namespace gtsam {
|
|||
*/
|
||||
std::pair<Matrix, Vector> jacobianUnweighted() const;
|
||||
|
||||
/**
|
||||
* Return (dense) matrix associated with factor
|
||||
* The returned system is an augmented matrix: [A b]
|
||||
* @param set weight to use whitening to bake in weights
|
||||
*/
|
||||
/** Return (dense) matrix associated with factor. The returned system is an augmented matrix:
|
||||
* [A b]
|
||||
* @param set weight to use whitening to bake in weights*/
|
||||
virtual Matrix augmentedJacobian() const;
|
||||
|
||||
/**
|
||||
* Return (dense) matrix associated with factor
|
||||
* The returned system is an augmented matrix: [A b]
|
||||
* @param set weight to use whitening to bake in weights
|
||||
*/
|
||||
/** Return (dense) matrix associated with factor. The returned system is an augmented matrix:
|
||||
* [A b]
|
||||
* @param set weight to use whitening to bake in weights */
|
||||
Matrix augmentedJacobianUnweighted() const;
|
||||
|
||||
/** Return the full augmented Jacobian matrix of this factor as a VerticalBlockMatrix object. */
|
||||
const VerticalBlockMatrix& matrixObject() const { return Ab_; }
|
||||
|
||||
/**
|
||||
* Construct the corresponding anti-factor to negate information
|
||||
* stored stored in this factor.
|
||||
|
|
|
|||
|
|
@ -25,6 +25,7 @@
|
|||
#include <gtsam/linear/HessianFactor.h>
|
||||
#include <gtsam/linear/JacobianFactor.h>
|
||||
#include <gtsam/linear/GaussianFactorGraph.h>
|
||||
#include <gtsam/linear/VectorValues.h>
|
||||
|
||||
#include <gtsam/base/TestableAssertions.h>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
|
@ -35,20 +36,19 @@ using namespace gtsam;
|
|||
|
||||
const double tol = 1e-5;
|
||||
|
||||
#if 0
|
||||
/* ************************************************************************* */
|
||||
TEST(HessianFactor, emptyConstructor) {
|
||||
HessianFactor f;
|
||||
DOUBLES_EQUAL(0.0, f.constantTerm(), 1e-9); // Constant term should be zero
|
||||
EXPECT(assert_equal(Vector(), f.linearTerm())); // Linear term should be empty
|
||||
EXPECT(assert_equal(zeros(1,1), f.info())); // Full matrix should be 1-by-1 zero matrix
|
||||
DOUBLES_EQUAL(0.0, f.error(VectorValuesOrdered()), 1e-9); // Should have zero error
|
||||
DOUBLES_EQUAL(0.0, f.error(VectorValues()), 1e-9); // Should have zero error
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(HessianFactor, ConversionConstructor) {
|
||||
|
||||
HessianFactorOrdered expected;
|
||||
HessianFactor expected;
|
||||
expected.keys_.push_back(0);
|
||||
expected.keys_.push_back(1);
|
||||
size_t dims[] = { 2, 4, 1 };
|
||||
|
|
@ -94,11 +94,11 @@ TEST(HessianFactor, ConversionConstructor) {
|
|||
vector<pair<Index, Matrix> > meas;
|
||||
meas.push_back(make_pair(0, Ax2));
|
||||
meas.push_back(make_pair(1, Al1x1));
|
||||
JacobianFactorOrdered combined(meas, b2, noiseModel::Diagonal::Sigmas(sigmas));
|
||||
JacobianFactor combined(meas, b2, noiseModel::Diagonal::Sigmas(sigmas));
|
||||
|
||||
HessianFactorOrdered actual(combined);
|
||||
HessianFactor actual(combined);
|
||||
|
||||
VectorValuesOrdered values(std::vector<size_t>(dims, dims+2));
|
||||
VectorValues values(std::vector<size_t>(dims, dims+2));
|
||||
values[0] = Vector_(2, 1.0, 2.0);
|
||||
values[1] = Vector_(4, 3.0, 4.0, 5.0, 6.0);
|
||||
|
||||
|
|
@ -111,7 +111,7 @@ TEST(HessianFactor, ConversionConstructor) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(HessianFactorOrdered, Constructor1)
|
||||
TEST(HessianFactor, Constructor1)
|
||||
{
|
||||
Matrix G = Matrix_(2,2, 3.0, 5.0, 0.0, 6.0);
|
||||
Vector g = Vector_(2, -8.0, -9.0);
|
||||
|
|
@ -121,11 +121,11 @@ TEST(HessianFactorOrdered, Constructor1)
|
|||
|
||||
vector<size_t> dims;
|
||||
dims.push_back(2);
|
||||
VectorValuesOrdered dx(dims);
|
||||
VectorValues dx(dims);
|
||||
|
||||
dx[0] = dxv;
|
||||
|
||||
HessianFactorOrdered factor(0, G, g, f);
|
||||
HessianFactor factor(0, G, g, f);
|
||||
|
||||
// extract underlying parts
|
||||
Matrix info_matrix = factor.info_.range(0, 1, 0, 1);
|
||||
|
|
@ -143,12 +143,12 @@ TEST(HessianFactorOrdered, Constructor1)
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(HessianFactorOrdered, Constructor1b)
|
||||
TEST(HessianFactor, Constructor1b)
|
||||
{
|
||||
Vector mu = Vector_(2,1.0,2.0);
|
||||
Matrix Sigma = eye(2,2);
|
||||
|
||||
HessianFactorOrdered factor(0, mu, Sigma);
|
||||
HessianFactor factor(0, mu, Sigma);
|
||||
|
||||
Matrix G = eye(2,2);
|
||||
Vector g = G*mu;
|
||||
|
|
@ -163,7 +163,7 @@ TEST(HessianFactorOrdered, Constructor1b)
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(HessianFactorOrdered, Constructor2)
|
||||
TEST(HessianFactor, Constructor2)
|
||||
{
|
||||
Matrix G11 = Matrix_(1,1, 1.0);
|
||||
Matrix G12 = Matrix_(1,2, 2.0, 4.0);
|
||||
|
|
@ -178,12 +178,12 @@ TEST(HessianFactorOrdered, Constructor2)
|
|||
vector<size_t> dims;
|
||||
dims.push_back(1);
|
||||
dims.push_back(2);
|
||||
VectorValuesOrdered dx(dims);
|
||||
VectorValues dx(dims);
|
||||
|
||||
dx[0] = dx0;
|
||||
dx[1] = dx1;
|
||||
|
||||
HessianFactorOrdered factor(0, 1, G11, G12, g1, G22, g2, f);
|
||||
HessianFactor factor(0, 1, G11, G12, g1, G22, g2, f);
|
||||
|
||||
double expected = 90.5;
|
||||
double actual = factor.error(dx);
|
||||
|
|
@ -201,7 +201,7 @@ TEST(HessianFactorOrdered, Constructor2)
|
|||
|
||||
// Check case when vector values is larger than factor
|
||||
dims.push_back(2);
|
||||
VectorValuesOrdered dxLarge(dims);
|
||||
VectorValues dxLarge(dims);
|
||||
dxLarge[0] = dx0;
|
||||
dxLarge[1] = dx1;
|
||||
dxLarge[2] = Vector_(2, 0.1, 0.2);
|
||||
|
|
@ -209,7 +209,7 @@ TEST(HessianFactorOrdered, Constructor2)
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(HessianFactorOrdered, Constructor3)
|
||||
TEST(HessianFactor, Constructor3)
|
||||
{
|
||||
Matrix G11 = Matrix_(1,1, 1.0);
|
||||
Matrix G12 = Matrix_(1,2, 2.0, 4.0);
|
||||
|
|
@ -234,13 +234,13 @@ TEST(HessianFactorOrdered, Constructor3)
|
|||
dims.push_back(1);
|
||||
dims.push_back(2);
|
||||
dims.push_back(3);
|
||||
VectorValuesOrdered dx(dims);
|
||||
VectorValues dx(dims);
|
||||
|
||||
dx[0] = dx0;
|
||||
dx[1] = dx1;
|
||||
dx[2] = dx2;
|
||||
|
||||
HessianFactorOrdered factor(0, 1, 2, G11, G12, G13, g1, G22, G23, g2, G33, g3, f);
|
||||
HessianFactor factor(0, 1, 2, G11, G12, G13, g1, G22, G23, g2, G33, g3, f);
|
||||
|
||||
double expected = 371.3750;
|
||||
double actual = factor.error(dx);
|
||||
|
|
@ -261,7 +261,7 @@ TEST(HessianFactorOrdered, Constructor3)
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(HessianFactorOrdered, ConstructorNWay)
|
||||
TEST(HessianFactor, ConstructorNWay)
|
||||
{
|
||||
Matrix G11 = Matrix_(1,1, 1.0);
|
||||
Matrix G12 = Matrix_(1,2, 2.0, 4.0);
|
||||
|
|
@ -286,7 +286,7 @@ TEST(HessianFactorOrdered, ConstructorNWay)
|
|||
dims.push_back(1);
|
||||
dims.push_back(2);
|
||||
dims.push_back(3);
|
||||
VectorValuesOrdered dx(dims);
|
||||
VectorValues dx(dims);
|
||||
|
||||
dx[0] = dx0;
|
||||
dx[1] = dx1;
|
||||
|
|
@ -299,7 +299,7 @@ TEST(HessianFactorOrdered, ConstructorNWay)
|
|||
Gs.push_back(G11); Gs.push_back(G12); Gs.push_back(G13); Gs.push_back(G22); Gs.push_back(G23); Gs.push_back(G33);
|
||||
std::vector<Vector> gs;
|
||||
gs.push_back(g1); gs.push_back(g2); gs.push_back(g3);
|
||||
HessianFactorOrdered factor(js, Gs, gs, f);
|
||||
HessianFactor factor(js, Gs, gs, f);
|
||||
|
||||
double expected = 371.3750;
|
||||
double actual = factor.error(dx);
|
||||
|
|
@ -320,7 +320,7 @@ TEST(HessianFactorOrdered, ConstructorNWay)
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST_UNSAFE(HessianFactorOrdered, CopyConstructor_and_assignment)
|
||||
TEST_UNSAFE(HessianFactor, CopyConstructor_and_assignment)
|
||||
{
|
||||
Matrix G11 = Matrix_(1,1, 1.0);
|
||||
Matrix G12 = Matrix_(1,2, 2.0, 4.0);
|
||||
|
|
@ -335,15 +335,15 @@ TEST_UNSAFE(HessianFactorOrdered, CopyConstructor_and_assignment)
|
|||
vector<size_t> dims;
|
||||
dims.push_back(1);
|
||||
dims.push_back(2);
|
||||
VectorValuesOrdered dx(dims);
|
||||
VectorValues dx(dims);
|
||||
|
||||
dx[0] = dx0;
|
||||
dx[1] = dx1;
|
||||
|
||||
HessianFactorOrdered originalFactor(0, 1, G11, G12, g1, G22, g2, f);
|
||||
HessianFactor originalFactor(0, 1, G11, G12, g1, G22, g2, f);
|
||||
|
||||
// Make a copy
|
||||
HessianFactorOrdered copy1(originalFactor);
|
||||
HessianFactor copy1(originalFactor);
|
||||
|
||||
double expected = 90.5;
|
||||
double actual = copy1.error(dx);
|
||||
|
|
@ -360,8 +360,8 @@ TEST_UNSAFE(HessianFactorOrdered, CopyConstructor_and_assignment)
|
|||
EXPECT(assert_equal(G22, copy1.info(copy1.begin()+1, copy1.begin()+1)));
|
||||
|
||||
// Make a copy using the assignment operator
|
||||
HessianFactorOrdered copy2;
|
||||
copy2 = HessianFactorOrdered(originalFactor); // Make a temporary to make sure copying does not shared references
|
||||
HessianFactor copy2;
|
||||
copy2 = HessianFactor(originalFactor); // Make a temporary to make sure copying does not shared references
|
||||
|
||||
actual = copy2.error(dx);
|
||||
DOUBLES_EQUAL(expected, actual, 1e-10);
|
||||
|
|
@ -376,7 +376,7 @@ TEST_UNSAFE(HessianFactorOrdered, CopyConstructor_and_assignment)
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST_UNSAFE(HessianFactorOrdered, CombineAndEliminate)
|
||||
TEST_UNSAFE(HessianFactor, CombineAndEliminate)
|
||||
{
|
||||
Matrix A01 = Matrix_(3,3,
|
||||
1.0, 0.0, 0.0,
|
||||
|
|
@ -403,7 +403,7 @@ TEST_UNSAFE(HessianFactorOrdered, CombineAndEliminate)
|
|||
Vector b2 = Vector_(3, 3.5, 3.5, 3.5);
|
||||
Vector s2 = Vector_(3, 3.6, 3.6, 3.6);
|
||||
|
||||
GaussianFactorGraphOrdered gfg;
|
||||
GaussianFactorGraph gfg;
|
||||
gfg.add(1, A01, b0, noiseModel::Diagonal::Sigmas(s0, true));
|
||||
gfg.add(0, A10, 1, A11, b1, noiseModel::Diagonal::Sigmas(s1, true));
|
||||
gfg.add(1, A21, b2, noiseModel::Diagonal::Sigmas(s2, true));
|
||||
|
|
@ -415,35 +415,35 @@ TEST_UNSAFE(HessianFactorOrdered, CombineAndEliminate)
|
|||
Vector sigmas = gtsam::concatVectors(3, &s1, &s0, &s2);
|
||||
|
||||
// create a full, uneliminated version of the factor
|
||||
JacobianFactorOrdered expectedFactor(0, A0, 1, A1, b, noiseModel::Diagonal::Sigmas(sigmas, true));
|
||||
JacobianFactor expectedFactor(0, A0, 1, A1, b, noiseModel::Diagonal::Sigmas(sigmas, true));
|
||||
|
||||
// perform elimination
|
||||
GaussianConditionalOrdered::shared_ptr expectedBN = expectedFactor.eliminate(1);
|
||||
GaussianConditional::shared_ptr expectedBN = expectedFactor.eliminate(1);
|
||||
|
||||
// create expected Hessian after elimination
|
||||
HessianFactorOrdered expectedCholeskyFactor(expectedFactor);
|
||||
HessianFactor expectedCholeskyFactor(expectedFactor);
|
||||
|
||||
// Convert all factors to hessians
|
||||
FactorGraphOrdered<HessianFactorOrdered> hessians;
|
||||
BOOST_FOREACH(const GaussianFactorGraphOrdered::sharedFactor& factor, gfg) {
|
||||
if(boost::shared_ptr<HessianFactorOrdered> hf = boost::dynamic_pointer_cast<HessianFactorOrdered>(factor))
|
||||
FactorGraph<HessianFactor> hessians;
|
||||
BOOST_FOREACH(const GaussianFactorGraph::sharedFactor& factor, gfg) {
|
||||
if(boost::shared_ptr<HessianFactor> hf = boost::dynamic_pointer_cast<HessianFactor>(factor))
|
||||
hessians.push_back(hf);
|
||||
else if(boost::shared_ptr<JacobianFactorOrdered> jf = boost::dynamic_pointer_cast<JacobianFactorOrdered>(factor))
|
||||
hessians.push_back(boost::make_shared<HessianFactorOrdered>(*jf));
|
||||
else if(boost::shared_ptr<JacobianFactor> jf = boost::dynamic_pointer_cast<JacobianFactor>(factor))
|
||||
hessians.push_back(boost::make_shared<HessianFactor>(*jf));
|
||||
else
|
||||
CHECK(false);
|
||||
}
|
||||
|
||||
// Eliminate
|
||||
GaussianFactorGraphOrdered::EliminationResult actualCholesky = EliminateCholeskyOrdered(gfg, 1);
|
||||
HessianFactorOrdered::shared_ptr actualFactor = boost::dynamic_pointer_cast<HessianFactorOrdered>(actualCholesky.second);
|
||||
GaussianFactorGraph::EliminationResult actualCholesky = EliminateCholesky(gfg, 1);
|
||||
HessianFactor::shared_ptr actualFactor = boost::dynamic_pointer_cast<HessianFactor>(actualCholesky.second);
|
||||
|
||||
EXPECT(assert_equal(*expectedBN, *actualCholesky.first, 1e-6));
|
||||
EXPECT(assert_equal(expectedCholeskyFactor, *actualFactor, 1e-6));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(HessianFactorOrdered, eliminate2 )
|
||||
TEST(HessianFactor, eliminate2 )
|
||||
{
|
||||
// sigmas
|
||||
double sigma1 = 0.2;
|
||||
|
|
@ -477,17 +477,17 @@ TEST(HessianFactorOrdered, eliminate2 )
|
|||
vector<pair<Index, Matrix> > meas;
|
||||
meas.push_back(make_pair(0, Ax2));
|
||||
meas.push_back(make_pair(1, Al1x1));
|
||||
JacobianFactorOrdered combined(meas, b2, noiseModel::Diagonal::Sigmas(sigmas));
|
||||
JacobianFactor combined(meas, b2, noiseModel::Diagonal::Sigmas(sigmas));
|
||||
|
||||
// eliminate the combined factor
|
||||
HessianFactorOrdered::shared_ptr combinedLF_Chol(new HessianFactorOrdered(combined));
|
||||
FactorGraphOrdered<HessianFactorOrdered> combinedLFG_Chol;
|
||||
HessianFactor::shared_ptr combinedLF_Chol(new HessianFactor(combined));
|
||||
FactorGraph<HessianFactor> combinedLFG_Chol;
|
||||
combinedLFG_Chol.push_back(combinedLF_Chol);
|
||||
|
||||
GaussianFactorGraphOrdered::EliminationResult actual_Chol = EliminateCholeskyOrdered(
|
||||
GaussianFactorGraph::EliminationResult actual_Chol = EliminateCholesky(
|
||||
combinedLFG_Chol, 1);
|
||||
HessianFactorOrdered::shared_ptr actualFactor = boost::dynamic_pointer_cast<
|
||||
HessianFactorOrdered>(actual_Chol.second);
|
||||
HessianFactor::shared_ptr actualFactor = boost::dynamic_pointer_cast<
|
||||
HessianFactor>(actual_Chol.second);
|
||||
|
||||
// create expected Conditional Gaussian
|
||||
double oldSigma = 0.0894427; // from when R was made unit
|
||||
|
|
@ -500,7 +500,7 @@ TEST(HessianFactorOrdered, eliminate2 )
|
|||
+0.00,-0.20,+0.00,-0.80
|
||||
)/oldSigma;
|
||||
Vector d = Vector_(2,0.2,-0.14)/oldSigma;
|
||||
GaussianConditionalOrdered expectedCG(0,d,R11,1,S12,ones(2));
|
||||
GaussianConditional expectedCG(0,d,R11,1,S12,ones(2));
|
||||
EXPECT(assert_equal(expectedCG,*actual_Chol.first,1e-4));
|
||||
|
||||
// the expected linear factor
|
||||
|
|
@ -511,15 +511,15 @@ TEST(HessianFactorOrdered, eliminate2 )
|
|||
0.00, 1.00, +0.00, -1.00
|
||||
)/sigma;
|
||||
Vector b1 = Vector_(2,0.0,0.894427);
|
||||
JacobianFactorOrdered expectedLF(1, Bl1x1, b1, noiseModel::Isotropic::Sigma(2,1.0));
|
||||
EXPECT(assert_equal(HessianFactorOrdered(expectedLF), *actualFactor, 1.5e-3));
|
||||
JacobianFactor expectedLF(1, Bl1x1, b1, noiseModel::Isotropic::Sigma(2,1.0));
|
||||
EXPECT(assert_equal(HessianFactor(expectedLF), *actualFactor, 1.5e-3));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(HessianFactorOrdered, eliminateUnsorted) {
|
||||
TEST(HessianFactor, eliminateUnsorted) {
|
||||
|
||||
JacobianFactorOrdered::shared_ptr factor1(
|
||||
new JacobianFactorOrdered(0,
|
||||
JacobianFactor::shared_ptr factor1(
|
||||
new JacobianFactor(0,
|
||||
Matrix_(3,3,
|
||||
44.7214, 0.0, 0.0,
|
||||
0.0, 44.7214, 0.0,
|
||||
|
|
@ -531,8 +531,8 @@ TEST(HessianFactorOrdered, eliminateUnsorted) {
|
|||
0.0, 0.0, -44.7214),
|
||||
Vector_(3, 1.98916e-17, -4.96503e-15, -7.75792e-17),
|
||||
noiseModel::Unit::Create(3)));
|
||||
HessianFactorOrdered::shared_ptr unsorted_factor2(
|
||||
new HessianFactorOrdered(JacobianFactorOrdered(0,
|
||||
HessianFactor::shared_ptr unsorted_factor2(
|
||||
new HessianFactor(JacobianFactor(0,
|
||||
Matrix_(6,3,
|
||||
25.8367, 0.1211, 0.0593,
|
||||
0.0, 23.4099, 30.8733,
|
||||
|
|
@ -555,8 +555,8 @@ TEST(HessianFactorOrdered, eliminateUnsorted) {
|
|||
permutation[1] = 0;
|
||||
unsorted_factor2->permuteWithInverse(permutation);
|
||||
|
||||
HessianFactorOrdered::shared_ptr sorted_factor2(
|
||||
new HessianFactorOrdered(JacobianFactorOrdered(0,
|
||||
HessianFactor::shared_ptr sorted_factor2(
|
||||
new HessianFactor(JacobianFactor(0,
|
||||
Matrix_(6,3,
|
||||
25.7429, -1.6897, 0.4587,
|
||||
1.6400, 23.3095, -8.4816,
|
||||
|
|
@ -575,30 +575,30 @@ TEST(HessianFactorOrdered, eliminateUnsorted) {
|
|||
Vector_(6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
|
||||
noiseModel::Unit::Create(6))));
|
||||
|
||||
GaussianFactorGraphOrdered sortedGraph;
|
||||
GaussianFactorGraph sortedGraph;
|
||||
// sortedGraph.push_back(factor1);
|
||||
sortedGraph.push_back(sorted_factor2);
|
||||
|
||||
GaussianFactorGraphOrdered unsortedGraph;
|
||||
GaussianFactorGraph unsortedGraph;
|
||||
// unsortedGraph.push_back(factor1);
|
||||
unsortedGraph.push_back(unsorted_factor2);
|
||||
|
||||
GaussianConditionalOrdered::shared_ptr expected_bn;
|
||||
GaussianFactorOrdered::shared_ptr expected_factor;
|
||||
GaussianConditional::shared_ptr expected_bn;
|
||||
GaussianFactor::shared_ptr expected_factor;
|
||||
boost::tie(expected_bn, expected_factor) =
|
||||
EliminatePreferCholeskyOrdered(sortedGraph, 1);
|
||||
EliminatePreferCholesky(sortedGraph, 1);
|
||||
|
||||
GaussianConditionalOrdered::shared_ptr actual_bn;
|
||||
GaussianFactorOrdered::shared_ptr actual_factor;
|
||||
GaussianConditional::shared_ptr actual_bn;
|
||||
GaussianFactor::shared_ptr actual_factor;
|
||||
boost::tie(actual_bn, actual_factor) =
|
||||
EliminatePreferCholeskyOrdered(unsortedGraph, 1);
|
||||
EliminatePreferCholesky(unsortedGraph, 1);
|
||||
|
||||
EXPECT(assert_equal(*expected_bn, *actual_bn, 1e-10));
|
||||
EXPECT(assert_equal(*expected_factor, *actual_factor, 1e-10));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(HessianFactorOrdered, combine) {
|
||||
TEST(HessianFactor, combine) {
|
||||
|
||||
// update the information matrix with a single jacobian factor
|
||||
Matrix A0 = Matrix_(2, 2,
|
||||
|
|
@ -612,12 +612,12 @@ TEST(HessianFactorOrdered, combine) {
|
|||
0.0, -8.94427191);
|
||||
Vector b = Vector_(2, 2.23606798,-1.56524758);
|
||||
SharedDiagonal model = noiseModel::Diagonal::Sigmas(ones(2));
|
||||
GaussianFactorOrdered::shared_ptr f(new JacobianFactorOrdered(0, A0, 1, A1, 2, A2, b, model));
|
||||
FactorGraphOrdered<GaussianFactorOrdered> factors;
|
||||
GaussianFactor::shared_ptr f(new JacobianFactor(0, A0, 1, A1, 2, A2, b, model));
|
||||
FactorGraph<GaussianFactor> factors;
|
||||
factors.push_back(f);
|
||||
|
||||
// Form Ab' * Ab
|
||||
HessianFactorOrdered actual(factors);
|
||||
HessianFactor actual(factors);
|
||||
|
||||
Matrix expected = Matrix_(7, 7,
|
||||
125.0000, 0.0, -25.0000, 0.0, -100.0000, 0.0, 25.0000,
|
||||
|
|
@ -631,7 +631,6 @@ TEST(HessianFactorOrdered, combine) {
|
|||
Matrix(actual.matrix_.triangularView<Eigen::Upper>()), tol));
|
||||
|
||||
}
|
||||
#endif
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
|
||||
|
|
|
|||
Loading…
Reference in New Issue