Added LinearContainerFactor to add linear factors to nonlinear graphs
Alex Cunningham
parent
59c6234d4f
commit
e6a294d188
|
|
@ -0,0 +1,148 @@
|
||||||
|
/**
|
||||||
|
* @file LinearContainerFactor.cpp
|
||||||
|
*
|
||||||
|
* @date Jul 6, 2012
|
||||||
|
* @author Alex Cunningham
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <gtsam_unstable/nonlinear/LinearContainerFactor.h>
|
||||||
|
|
||||||
|
#include <boost/foreach.hpp>
|
||||||
|
|
||||||
|
namespace gtsam {
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
void LinearContainerFactor::rekeyFactor(const Ordering& ordering) {
|
||||||
|
Ordering::InvertedMap invOrdering = ordering.invert(); // TODO: inefficient - make more selective ordering invert
|
||||||
|
rekeyFactor(invOrdering);
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
void LinearContainerFactor::rekeyFactor(const Ordering::InvertedMap& invOrdering) {
|
||||||
|
BOOST_FOREACH(Index& idx, factor_->keys()) {
|
||||||
|
Key fullKey = invOrdering.find(idx)->second;
|
||||||
|
idx = fullKey;
|
||||||
|
keys_.push_back(fullKey);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
LinearContainerFactor::LinearContainerFactor(
|
||||||
|
const JacobianFactor& factor, const Ordering& ordering)
|
||||||
|
: factor_(factor.clone()) {
|
||||||
|
rekeyFactor(ordering);
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
LinearContainerFactor::LinearContainerFactor(
|
||||||
|
const HessianFactor& factor, const Ordering& ordering)
|
||||||
|
: factor_(factor.clone()) {
|
||||||
|
rekeyFactor(ordering);
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
LinearContainerFactor::LinearContainerFactor(
|
||||||
|
const GaussianFactor::shared_ptr& factor, const Ordering& ordering)
|
||||||
|
: factor_(factor->clone()) {
|
||||||
|
rekeyFactor(ordering);
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
LinearContainerFactor::LinearContainerFactor(
|
||||||
|
const GaussianFactor::shared_ptr& factor)
|
||||||
|
: factor_(factor->clone())
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
LinearContainerFactor::LinearContainerFactor(const JacobianFactor& factor,
|
||||||
|
const Ordering::InvertedMap& inverted_ordering)
|
||||||
|
: factor_(factor.clone()) {
|
||||||
|
rekeyFactor(inverted_ordering);
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
LinearContainerFactor::LinearContainerFactor(const HessianFactor& factor,
|
||||||
|
const Ordering::InvertedMap& inverted_ordering)
|
||||||
|
: factor_(factor.clone()) {
|
||||||
|
rekeyFactor(inverted_ordering);
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
LinearContainerFactor::LinearContainerFactor(
|
||||||
|
const GaussianFactor::shared_ptr& factor,
|
||||||
|
const Ordering::InvertedMap& ordering)
|
||||||
|
: factor_(factor->clone()) {
|
||||||
|
rekeyFactor(ordering);
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
void LinearContainerFactor::print(const std::string& s, const KeyFormatter& keyFormatter) const {
|
||||||
|
Base::print(s+"LinearContainerFactor", keyFormatter);
|
||||||
|
if (factor_)
|
||||||
|
factor_->print(" Stored Factor", keyFormatter);
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
bool LinearContainerFactor::equals(const NonlinearFactor& f, double tol) const {
|
||||||
|
const LinearContainerFactor* jcf = dynamic_cast<const LinearContainerFactor*>(&f);
|
||||||
|
return jcf && factor_->equals(*jcf->factor_, tol) && NonlinearFactor::equals(f);
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
double LinearContainerFactor::error(const Values& c) const {
|
||||||
|
// VectorValues vecvalues;
|
||||||
|
// // FIXME: add values correctly here
|
||||||
|
// return factor_.error(vecvalues);
|
||||||
|
return 0; // FIXME: placeholder
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
size_t LinearContainerFactor::dim() const {
|
||||||
|
if (isJacobian())
|
||||||
|
return toJacobian()->get_model()->dim();
|
||||||
|
else
|
||||||
|
return 1; // Hessians don't have true dimension
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
boost::shared_ptr<GaussianFactor>
|
||||||
|
LinearContainerFactor::linearize(const Values& c, const Ordering& ordering) const {
|
||||||
|
// clone factor
|
||||||
|
boost::shared_ptr<GaussianFactor> result = factor_->clone();
|
||||||
|
|
||||||
|
// rekey
|
||||||
|
BOOST_FOREACH(Index& key, result->keys())
|
||||||
|
key = ordering[key];
|
||||||
|
|
||||||
|
return result;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
bool LinearContainerFactor::isJacobian() const {
|
||||||
|
return boost::shared_dynamic_cast<JacobianFactor>(factor_);
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
JacobianFactor::shared_ptr LinearContainerFactor::toJacobian() const {
|
||||||
|
return boost::shared_dynamic_cast<JacobianFactor>(factor_);
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
HessianFactor::shared_ptr LinearContainerFactor::toHessian() const {
|
||||||
|
return boost::shared_dynamic_cast<HessianFactor>(factor_);
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
GaussianFactor::shared_ptr LinearContainerFactor::negate(const Ordering& ordering) const {
|
||||||
|
GaussianFactor::shared_ptr result = factor_->negate();
|
||||||
|
BOOST_FOREACH(Key& key, result->keys())
|
||||||
|
key = ordering[key];
|
||||||
|
return result;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
} // \namespace gtsam
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
@ -0,0 +1,113 @@
|
||||||
|
/**
|
||||||
|
* @file LinearContainerFactor.h
|
||||||
|
*
|
||||||
|
* @brief Wrap Jacobian and Hessian linear factors to allow simple injection into a nonlinear graph
|
||||||
|
*
|
||||||
|
* @date Jul 6, 2012
|
||||||
|
* @author Alex Cunningham
|
||||||
|
*/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <gtsam/linear/HessianFactor.h>
|
||||||
|
#include <gtsam/nonlinear/NonlinearFactor.h>
|
||||||
|
|
||||||
|
namespace gtsam {
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Dummy version of a generic linear factor to be injected into a nonlinear factor graph
|
||||||
|
*/
|
||||||
|
class LinearContainerFactor : public NonlinearFactor {
|
||||||
|
protected:
|
||||||
|
|
||||||
|
GaussianFactor::shared_ptr factor_;
|
||||||
|
|
||||||
|
public:
|
||||||
|
|
||||||
|
/** Primary constructor: store a linear factor and decode the ordering */
|
||||||
|
LinearContainerFactor(const JacobianFactor& factor, const Ordering& ordering);
|
||||||
|
|
||||||
|
/** Primary constructor: store a linear factor and decode the ordering */
|
||||||
|
LinearContainerFactor(const HessianFactor& factor, const Ordering& ordering);
|
||||||
|
|
||||||
|
/** Constructor from shared_ptr */
|
||||||
|
LinearContainerFactor(const GaussianFactor::shared_ptr& factor, const Ordering& ordering);
|
||||||
|
|
||||||
|
/** Constructor from re-keyed factor: all indices assumed replaced with Key */
|
||||||
|
LinearContainerFactor(const GaussianFactor::shared_ptr& factor);
|
||||||
|
|
||||||
|
/** Alternate constructor: store a linear factor and decode keys with inverted ordering*/
|
||||||
|
LinearContainerFactor(const JacobianFactor& factor,
|
||||||
|
const Ordering::InvertedMap& inverted_ordering);
|
||||||
|
|
||||||
|
/** Alternate constructor: store a linear factor and decode keys with inverted ordering*/
|
||||||
|
LinearContainerFactor(const HessianFactor& factor,
|
||||||
|
const Ordering::InvertedMap& inverted_ordering);
|
||||||
|
|
||||||
|
/** Constructor from shared_ptr with inverted ordering*/
|
||||||
|
LinearContainerFactor(const GaussianFactor::shared_ptr& factor,
|
||||||
|
const Ordering::InvertedMap& ordering);
|
||||||
|
|
||||||
|
// Access
|
||||||
|
|
||||||
|
const GaussianFactor::shared_ptr& factor() const { return factor_; }
|
||||||
|
|
||||||
|
// Testable
|
||||||
|
|
||||||
|
/** print */
|
||||||
|
void print(const std::string& s = "", const KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const;
|
||||||
|
|
||||||
|
/** Check if two factors are equal */
|
||||||
|
bool equals(const NonlinearFactor& f, double tol = 1e-9) const;
|
||||||
|
|
||||||
|
// NonlinearFactor
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Calculate the error of the factor: uses the underlying linear factor to compute ordering
|
||||||
|
*/
|
||||||
|
double error(const Values& c) const;
|
||||||
|
|
||||||
|
/** get the dimension of the factor: rows of linear factor */
|
||||||
|
size_t dim() const;
|
||||||
|
|
||||||
|
/** linearize to a GaussianFactor: values has no effect, just clones/rekeys underlying factor */
|
||||||
|
boost::shared_ptr<GaussianFactor>
|
||||||
|
linearize(const Values& c, const Ordering& ordering) const;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Creates an anti-factor directly and performs rekeying due to ordering
|
||||||
|
*/
|
||||||
|
GaussianFactor::shared_ptr negate(const Ordering& ordering) const;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Creates a shared_ptr clone of the factor - needs to be specialized to allow
|
||||||
|
* for subclasses
|
||||||
|
*
|
||||||
|
* Clones the underlying linear factor
|
||||||
|
*/
|
||||||
|
NonlinearFactor::shared_ptr clone() const {
|
||||||
|
return NonlinearFactor::shared_ptr(new LinearContainerFactor(factor_));
|
||||||
|
}
|
||||||
|
|
||||||
|
// casting syntactic sugar
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Simple check whether this is a Jacobian or Hessian factor
|
||||||
|
*/
|
||||||
|
bool isJacobian() const;
|
||||||
|
|
||||||
|
/** Casts to JacobianFactor */
|
||||||
|
JacobianFactor::shared_ptr toJacobian() const;
|
||||||
|
|
||||||
|
/** Casts to HessianFactor */
|
||||||
|
HessianFactor::shared_ptr toHessian() const;
|
||||||
|
|
||||||
|
protected:
|
||||||
|
void rekeyFactor(const Ordering& ordering);
|
||||||
|
void rekeyFactor(const Ordering::InvertedMap& invOrdering);
|
||||||
|
|
||||||
|
}; // \class LinearContainerFactor
|
||||||
|
|
||||||
|
} // \namespace gtsam
|
||||||
|
|
||||||
|
|
||||||
|
|
@ -0,0 +1,113 @@
|
||||||
|
/**
|
||||||
|
* @file testLinearContainerFactor.cpp
|
||||||
|
*
|
||||||
|
* @date Jul 6, 2012
|
||||||
|
* @author Alex Cunningham
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <CppUnitLite/TestHarness.h>
|
||||||
|
|
||||||
|
#include <gtsam_unstable/nonlinear/LinearContainerFactor.h>
|
||||||
|
|
||||||
|
#include <gtsam/geometry/Pose2.h>
|
||||||
|
|
||||||
|
using namespace gtsam;
|
||||||
|
|
||||||
|
const gtsam::noiseModel::Diagonal::shared_ptr diag_model2 = noiseModel::Diagonal::Sigmas(Vector_(2, 1.0, 1.0));
|
||||||
|
const double tol = 1e-5;
|
||||||
|
|
||||||
|
gtsam::Key l1 = 101, l2 = 102, x1 = 1, x2 = 2;
|
||||||
|
|
||||||
|
Point2 landmark1(5.0, 1.5), landmark2(7.0, 1.5);
|
||||||
|
Pose2 poseA1(0.0, 0.0, 0.0), poseA2(2.0, 0.0, 0.0);
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
TEST( testLinearContainerFactor, generic_jacobian_factor ) {
|
||||||
|
|
||||||
|
Ordering initOrdering; initOrdering += x1, x2, l1, l2;
|
||||||
|
|
||||||
|
JacobianFactor expLinFactor1(
|
||||||
|
initOrdering[l1],
|
||||||
|
Matrix_(2,2,
|
||||||
|
2.74222, -0.0067457,
|
||||||
|
0.0, 2.63624),
|
||||||
|
initOrdering[l2],
|
||||||
|
Matrix_(2,2,
|
||||||
|
-0.0455167, -0.0443573,
|
||||||
|
-0.0222154, -0.102489),
|
||||||
|
Vector_(2, 0.0277052,
|
||||||
|
-0.0533393),
|
||||||
|
diag_model2);
|
||||||
|
|
||||||
|
LinearContainerFactor actFactor1(expLinFactor1, initOrdering);
|
||||||
|
Values values;
|
||||||
|
values.insert(l1, landmark1);
|
||||||
|
values.insert(l2, landmark2);
|
||||||
|
values.insert(x1, poseA1);
|
||||||
|
values.insert(x2, poseA2);
|
||||||
|
|
||||||
|
// Check reconstruction from same ordering
|
||||||
|
GaussianFactor::shared_ptr actLinearizationA = actFactor1.linearize(values, initOrdering);
|
||||||
|
EXPECT(assert_equal(*expLinFactor1.clone(), *actLinearizationA, tol));
|
||||||
|
|
||||||
|
// Check reconstruction from new ordering
|
||||||
|
Ordering newOrdering; newOrdering += x1, l1, x2, l2;
|
||||||
|
GaussianFactor::shared_ptr actLinearizationB = actFactor1.linearize(values, newOrdering);
|
||||||
|
|
||||||
|
JacobianFactor expLinFactor2(
|
||||||
|
newOrdering[l1],
|
||||||
|
Matrix_(2,2,
|
||||||
|
2.74222, -0.0067457,
|
||||||
|
0.0, 2.63624),
|
||||||
|
newOrdering[l2],
|
||||||
|
Matrix_(2,2,
|
||||||
|
-0.0455167, -0.0443573,
|
||||||
|
-0.0222154, -0.102489),
|
||||||
|
Vector_(2, 0.0277052,
|
||||||
|
-0.0533393),
|
||||||
|
diag_model2);
|
||||||
|
|
||||||
|
EXPECT(assert_equal(*expLinFactor2.clone(), *actLinearizationB, tol));
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
TEST( testLinearContainerFactor, generic_hessian_factor ) {
|
||||||
|
Matrix G11 = Matrix_(1,1, 1.0);
|
||||||
|
Matrix G12 = Matrix_(1,2, 2.0, 4.0);
|
||||||
|
Matrix G13 = Matrix_(1,3, 3.0, 6.0, 9.0);
|
||||||
|
|
||||||
|
Matrix G22 = Matrix_(2,2, 3.0, 5.0, 0.0, 6.0);
|
||||||
|
Matrix G23 = Matrix_(2,3, 4.0, 6.0, 8.0, 1.0, 2.0, 4.0);
|
||||||
|
|
||||||
|
Matrix G33 = Matrix_(3,3, 1.0, 2.0, 3.0, 0.0, 5.0, 6.0, 0.0, 0.0, 9.0);
|
||||||
|
|
||||||
|
Vector g1 = Vector_(1, -7.0);
|
||||||
|
Vector g2 = Vector_(2, -8.0, -9.0);
|
||||||
|
Vector g3 = Vector_(3, 1.0, 2.0, 3.0);
|
||||||
|
|
||||||
|
double f = 10.0;
|
||||||
|
|
||||||
|
Ordering initOrdering; initOrdering += x1, x2, l1, l2;
|
||||||
|
HessianFactor initFactor(initOrdering[x1], initOrdering[x2], initOrdering[l1],
|
||||||
|
G11, G12, G13, g1, G22, G23, g2, G33, g3, f);
|
||||||
|
|
||||||
|
Values values;
|
||||||
|
values.insert(l1, landmark1);
|
||||||
|
values.insert(l2, landmark2);
|
||||||
|
values.insert(x1, poseA1);
|
||||||
|
values.insert(x2, poseA2);
|
||||||
|
|
||||||
|
LinearContainerFactor actFactor(initFactor, initOrdering);
|
||||||
|
GaussianFactor::shared_ptr actLinearization1 = actFactor.linearize(values, initOrdering);
|
||||||
|
EXPECT(assert_equal(*initFactor.clone(), *actLinearization1, tol));
|
||||||
|
|
||||||
|
Ordering newOrdering; newOrdering += l1, x1, x2, l2;
|
||||||
|
HessianFactor expLinFactor(newOrdering[x1], newOrdering[x2], newOrdering[l1],
|
||||||
|
G11, G12, G13, g1, G22, G23, g2, G33, g3, f);
|
||||||
|
GaussianFactor::shared_ptr actLinearization2 = actFactor.linearize(values, newOrdering);
|
||||||
|
EXPECT(assert_equal(*expLinFactor.clone(), *actLinearization2, tol));
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
|
||||||
|
/* ************************************************************************* */
|
||||||
Loading…
Reference in New Issue