/* ************************************************************************* */
TEST( GaussianConditional, isGaussianFactor ) {
// Create R matrix
Matrix R = (Matrix(4,4) <<
1, 2, 3, 4,
0, 5, 6, 7,
0, 0, 8, 9,
0, 0, 0, 10).finished();
// Create a conditional
GaussianConditional conditional(0, Vector::Zero(4), R, Vector::Constant(4, 1.0));
// Expected information matrix computed by conditional
Matrix IExpected = conditional.information();
// Expected information matrix computed by a factor
JacobianFactor jf = *conditional.toFactor();
Matrix IActual = jf.getA(jf.begin()).transpose() * jf.getA(jf.begin());
EXPECT(assert_equal(IExpected, IActual));
}
/* ************************************************************************* */
vector<boost::tuple<size_t, size_t, double> > GaussianFactorGraph::sparseJacobian() const {
// First find dimensions of each variable
vector<size_t> dims;
BOOST_FOREACH(const sharedFactor& factor, *this) {
for (GaussianFactor::const_iterator pos = factor->begin();
pos != factor->end(); ++pos) {
if (dims.size() <= *pos)
dims.resize(*pos + 1, 0);
dims[*pos] = factor->getDim(pos);
}
}
// Compute first scalar column of each variable
vector<size_t> columnIndices(dims.size() + 1, 0);
for (size_t j = 1; j < dims.size() + 1; ++j)
columnIndices[j] = columnIndices[j - 1] + dims[j - 1];
// Iterate over all factors, adding sparse scalar entries
typedef boost::tuple<size_t, size_t, double> triplet;
vector<triplet> entries;
size_t row = 0;
BOOST_FOREACH(const sharedFactor& factor, *this) {
// Convert to JacobianFactor if necessary
JacobianFactor::shared_ptr jacobianFactor(
boost::dynamic_pointer_cast<JacobianFactor>(factor));
if (!jacobianFactor) {
HessianFactor::shared_ptr hessian(
boost::dynamic_pointer_cast<HessianFactor>(factor));
if (hessian)
jacobianFactor.reset(new JacobianFactor(*hessian));
else
throw invalid_argument(
"GaussianFactorGraph contains a factor that is neither a JacobianFactor nor a HessianFactor.");
}
// Whiten the factor and add entries for it
// iterate over all variables in the factor
const JacobianFactor whitened(jacobianFactor->whiten());
for (JacobianFactor::const_iterator pos = whitened.begin();
pos < whitened.end(); ++pos) {
JacobianFactor::constABlock whitenedA = whitened.getA(pos);
// find first column index for this key
size_t column_start = columnIndices[*pos];
for (size_t i = 0; i < (size_t) whitenedA.rows(); i++)
for (size_t j = 0; j < (size_t) whitenedA.cols(); j++) {
double s = whitenedA(i, j);
if (std::abs(s) > 1e-12)
entries.push_back(boost::make_tuple(row + i, column_start + j, s));
}
}
JacobianFactor::constBVector whitenedb(whitened.getb());
size_t bcolumn = columnIndices.back();
for (size_t i = 0; i < (size_t) whitenedb.size(); i++)
entries.push_back(boost::make_tuple(row + i, bcolumn, whitenedb(i)));
// Increment row index
row += jacobianFactor->rows();
}
return vector<triplet>(entries.begin(), entries.end());
}