/* ************************************************************************* */
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));
}
Beispiel #2
0
  /* ************************************************************************* */
  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());
  }