Moved LinearizedFactor from MastSLAM, started on a test for LinearContainerFactor
Alex Cunningham
parent
2288a6bc1f
commit
111ef8a0f0
|
|
@ -2114,6 +2114,14 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="check.nonlinear_unstable" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>check.nonlinear_unstable</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testRot3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
|
|
|
|||
|
|
@ -5,6 +5,7 @@ set (gtsam_unstable_subdirs
|
|||
discrete
|
||||
dynamics
|
||||
linear
|
||||
nonlinear
|
||||
slam
|
||||
)
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,25 @@
|
|||
# Install headers
|
||||
file(GLOB nonlinear_headers "*.h")
|
||||
install(FILES ${nonlinear_headers} DESTINATION include/gtsam_unstable/nonlinear)
|
||||
|
||||
# Components to link tests in this subfolder against
|
||||
set(nonlinear_local_libs
|
||||
nonlinear_unstable
|
||||
nonlinear
|
||||
linear
|
||||
inference
|
||||
geometry
|
||||
base
|
||||
ccolamd
|
||||
)
|
||||
|
||||
set (nonlinear_full_libs
|
||||
gtsam-static
|
||||
gtsam_unstable-static)
|
||||
|
||||
# Exclude tests that don't work
|
||||
set (nonlinear_excluded_tests "")
|
||||
|
||||
# Add all tests
|
||||
gtsam_add_subdir_tests(nonlinear_unstable "${nonlinear_local_libs}" "${nonlinear_full_libs}" "${nonlinear_excluded_tests}")
|
||||
add_dependencies(check.unstable check.nonlinear_unstable)
|
||||
|
|
@ -0,0 +1,147 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file LinearizedFactor.cpp
|
||||
* @brief A dummy factor that allows a linear factor to act as a nonlinear factor
|
||||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
#include <iostream>
|
||||
#include <boost/foreach.hpp>
|
||||
|
||||
#include <gtsam_unstable/nonlinear/LinearizedFactor.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
using namespace std;
|
||||
|
||||
/* ************************************************************************* */
|
||||
LinearizedFactor::LinearizedFactor(JacobianFactor::shared_ptr lin_factor,
|
||||
const KeyLookup& decoder, const Values& lin_points)
|
||||
: Base(lin_factor->get_model()), b_(lin_factor->getb()), model_(lin_factor->get_model()) {
|
||||
BOOST_FOREACH(const Index& idx, *lin_factor) {
|
||||
// find full symbol
|
||||
KeyLookup::const_iterator decode_it = decoder.find(idx);
|
||||
if (decode_it == decoder.end())
|
||||
throw runtime_error("LinearizedFactor: could not find index in decoder!");
|
||||
Key key(decode_it->second);
|
||||
|
||||
// extract linearization point
|
||||
assert(lin_points.exists(key));
|
||||
this->lin_points_.insert(key, lin_points.at(key)); // NOTE: will not overwrite
|
||||
|
||||
// extract Jacobian
|
||||
Matrix A = lin_factor->getA(lin_factor->find(idx));
|
||||
this->matrices_.insert(std::make_pair(key, A));
|
||||
|
||||
// store keys
|
||||
this->keys_.push_back(key);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
LinearizedFactor::LinearizedFactor(JacobianFactor::shared_ptr lin_factor,
|
||||
const Ordering& ordering, const Values& lin_points)
|
||||
: Base(lin_factor->get_model()), b_(lin_factor->getb()), model_(lin_factor->get_model()) {
|
||||
const KeyLookup decoder = ordering.invert();
|
||||
BOOST_FOREACH(const Index& idx, *lin_factor) {
|
||||
// find full symbol
|
||||
KeyLookup::const_iterator decode_it = decoder.find(idx);
|
||||
if (decode_it == decoder.end())
|
||||
throw runtime_error("LinearizedFactor: could not find index in decoder!");
|
||||
Key key(decode_it->second);
|
||||
|
||||
// extract linearization point
|
||||
assert(lin_points.exists(key));
|
||||
this->lin_points_.insert(key, lin_points.at(key)); // NOTE: will not overwrite
|
||||
|
||||
// extract Jacobian
|
||||
Matrix A = lin_factor->getA(lin_factor->find(idx));
|
||||
this->matrices_.insert(std::make_pair(key, A));
|
||||
|
||||
// store keys
|
||||
this->keys_.push_back(key);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector LinearizedFactor::unwhitenedError(const Values& c,
|
||||
boost::optional<std::vector<Matrix>&> H) const {
|
||||
// extract the points in the new values
|
||||
Vector ret = - b_;
|
||||
|
||||
if (H) H->resize(this->size());
|
||||
size_t i=0;
|
||||
BOOST_FOREACH(Key key, this->keys()) {
|
||||
const Value& newPt = c.at(key);
|
||||
const Value& linPt = lin_points_.at(key);
|
||||
Vector d = linPt.localCoordinates_(newPt);
|
||||
const Matrix& A = matrices_.at(key);
|
||||
ret += A * d;
|
||||
if (H) (*H)[i++] = A;
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
boost::shared_ptr<GaussianFactor>
|
||||
LinearizedFactor::linearize(const Values& c, const Ordering& ordering) const {
|
||||
|
||||
// sort by varid - only known at linearization time
|
||||
typedef std::map<Index, Matrix> VarMatrixMap;
|
||||
VarMatrixMap sorting_terms;
|
||||
BOOST_FOREACH(const KeyMatrixMap::value_type& p, matrices_)
|
||||
sorting_terms.insert(std::make_pair(ordering[p.first], p.second));
|
||||
|
||||
// move into terms
|
||||
std::vector<std::pair<Index, Matrix> > terms;
|
||||
BOOST_FOREACH(const VarMatrixMap::value_type& p, sorting_terms)
|
||||
terms.push_back(p);
|
||||
|
||||
// compute rhs: adjust current by whitened update
|
||||
Vector b = unwhitenedError(c) + b_; // remove original b
|
||||
this->noiseModel_->whitenInPlace(b);
|
||||
|
||||
return boost::shared_ptr<GaussianFactor>(new JacobianFactor(terms, b - b_, model_));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void LinearizedFactor::print(const std::string& s, const KeyFormatter& keyFormatter) const {
|
||||
this->noiseModel_->print(s + std::string(" model"));
|
||||
BOOST_FOREACH(const KeyMatrixMap::value_type& p, matrices_) {
|
||||
gtsam::print(p.second, keyFormatter(p.first));
|
||||
}
|
||||
gtsam::print(b_, std::string("b"));
|
||||
std::cout << " nonlinear keys: ";
|
||||
BOOST_FOREACH(const Key& key, this->keys())
|
||||
cout << keyFormatter(key) << " ";
|
||||
lin_points_.print("Linearization Point");
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool LinearizedFactor::equals(const LinearizedFactor& other, double tol) const {
|
||||
if (!Base::equals(other, tol)
|
||||
|| !lin_points_.equals(other.lin_points_, tol)
|
||||
|| !equal_with_abs_tol(b_, other.b_, tol)
|
||||
|| !model_->equals(*other.model_, tol)
|
||||
|| matrices_.size() != other.matrices_.size())
|
||||
return false;
|
||||
|
||||
KeyMatrixMap::const_iterator map1 = matrices_.begin(), map2 = other.matrices_.begin();
|
||||
for (; map1 != matrices_.end(), map2 != other.matrices_.end(); ++map1, ++map2)
|
||||
if ((map1->first != map2->first) || !equal_with_abs_tol(map1->second, map2->second, tol))
|
||||
return false;
|
||||
return true;
|
||||
}
|
||||
|
||||
} // \namespace gtsam
|
||||
|
|
@ -0,0 +1,122 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file LinearizedFactor.h
|
||||
* @brief A dummy factor that allows a linear factor to act as a nonlinear factor
|
||||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <vector>
|
||||
#include <gtsam/nonlinear/Ordering.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactor.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* A dummy factor that takes a linearized factor and inserts it into
|
||||
* a nonlinear graph.
|
||||
*/
|
||||
class LinearizedFactor : public gtsam::NoiseModelFactor {
|
||||
|
||||
public:
|
||||
/** base type */
|
||||
typedef gtsam::NoiseModelFactor Base;
|
||||
typedef LinearizedFactor This;
|
||||
|
||||
/** shared pointer for convenience */
|
||||
typedef boost::shared_ptr<LinearizedFactor > shared_ptr;
|
||||
|
||||
|
||||
/** decoder for keys - avoids the use of a full ordering */
|
||||
typedef gtsam::Ordering::InvertedMap KeyLookup;
|
||||
|
||||
protected:
|
||||
/** hold onto the factor itself */
|
||||
// store components of a jacobian factor
|
||||
typedef std::map<Key, gtsam::Matrix> KeyMatrixMap;
|
||||
KeyMatrixMap matrices_;
|
||||
gtsam::Vector b_;
|
||||
gtsam::SharedDiagonal model_; /// separate from the noisemodel in NonlinearFactor due to Diagonal/Gaussian
|
||||
|
||||
/** linearization points for error calculation */
|
||||
gtsam::Values lin_points_;
|
||||
|
||||
/** default constructor for serialization */
|
||||
LinearizedFactor() {}
|
||||
|
||||
public:
|
||||
|
||||
virtual ~LinearizedFactor() {}
|
||||
|
||||
ADD_CLONE_NONLINEAR_FACTOR(This);
|
||||
|
||||
/**
|
||||
* Use this constructor with pre-constructed decoders
|
||||
* @param lin_factor is a gaussian factor with linear keys (no labels baked in)
|
||||
* @param decoder is a structure to look up the correct keys
|
||||
* @param values is assumed to have the correct key structure with labels
|
||||
*/
|
||||
LinearizedFactor(gtsam::JacobianFactor::shared_ptr lin_factor,
|
||||
const KeyLookup& decoder, const gtsam::Values& lin_points);
|
||||
|
||||
/**
|
||||
* Use this constructor with the ordering used to linearize the graph
|
||||
* @param lin_factor is a gaussian factor with linear keys (no labels baked in)
|
||||
* @param ordering is the full ordering used to linearize the graph
|
||||
* @param values is assumed to have the correct key structure with labels
|
||||
*/
|
||||
LinearizedFactor(gtsam::JacobianFactor::shared_ptr lin_factor,
|
||||
const gtsam::Ordering& ordering, const gtsam::Values& lin_points);
|
||||
|
||||
/** Vector of errors, unwhitened, with optional derivatives (ordered by key) */
|
||||
virtual gtsam::Vector unwhitenedError(const gtsam::Values& c,
|
||||
boost::optional<std::vector<gtsam::Matrix>&> H = boost::none) const;
|
||||
|
||||
/**
|
||||
* linearize to a GaussianFactor
|
||||
* Reimplemented from NoiseModelFactor to directly copy out the
|
||||
* matrices and only update the RHS b with an updated residual
|
||||
*/
|
||||
virtual boost::shared_ptr<gtsam::GaussianFactor> linearize(
|
||||
const gtsam::Values& c, const gtsam::Ordering& ordering) const;
|
||||
|
||||
// Testable
|
||||
|
||||
/** print function */
|
||||
virtual void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
|
||||
|
||||
/** equals function with optional tolerance */
|
||||
virtual bool equals(const LinearizedFactor& other, double tol = 1e-9) const;
|
||||
|
||||
// access functions
|
||||
|
||||
const gtsam::Vector& b() const { return b_; }
|
||||
inline const gtsam::Values& linearizationPoint() const { return lin_points_; }
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
||||
ar & boost::serialization::make_nvp("NonlinearFactor",
|
||||
boost::serialization::base_object<Base>(*this));
|
||||
ar & BOOST_SERIALIZATION_NVP(matrices_);
|
||||
ar & BOOST_SERIALIZATION_NVP(b_);
|
||||
ar & BOOST_SERIALIZATION_NVP(model_);
|
||||
ar & BOOST_SERIALIZATION_NVP(lin_points_);
|
||||
}
|
||||
};
|
||||
|
||||
} // \namespace gtsam
|
||||
|
||||
|
|
@ -0,0 +1,22 @@
|
|||
/**
|
||||
* @file testLinearContainerFactors.cpp
|
||||
*
|
||||
* @brief Tests the use of nonlinear containers for simple integration of linear factors into nonlinear graphs
|
||||
*
|
||||
* @date Jun 7, 2012
|
||||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( testLinearContainerFactors, jacobian_factor ) {
|
||||
|
||||
// LinearContainerFactor actualLinFactor();
|
||||
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -0,0 +1,68 @@
|
|||
/**
|
||||
* @file testLinearizedFactor
|
||||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
#include <boost/assign/std/vector.hpp>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <gtsam/base/TestableAssertions.h>
|
||||
#include <gtsam/slam/planarSLAM.h>
|
||||
|
||||
#include <gtsam_unstable/nonlinear/LinearizedFactor.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace boost::assign;
|
||||
using namespace gtsam;
|
||||
|
||||
static const double tol = 1e-5;
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( testLinearizedFactor, creation ) {
|
||||
// Create a set of local keys (No robot label)
|
||||
Key l1 = 11, l2 = 12,
|
||||
l3 = 13, l4 = 14,
|
||||
l5 = 15, l6 = 16,
|
||||
l7 = 17, l8 = 18;
|
||||
|
||||
// creating an ordering to decode the linearized factor
|
||||
Ordering ordering;
|
||||
ordering += l1,l2,l3,l4,l5,l6,l7,l8;
|
||||
|
||||
// create a decoder for only the relevant variables
|
||||
LinearizedFactor::KeyLookup decoder;
|
||||
decoder[2] = l3;
|
||||
decoder[4] = l5;
|
||||
|
||||
// create a linear factor
|
||||
SharedDiagonal model = noiseModel::Unit::Create(2);
|
||||
JacobianFactor::shared_ptr linear_factor(new JacobianFactor(
|
||||
ordering[l3], eye(2,2), ordering[l5], 2.0 * eye(2,2), zero(2), model));
|
||||
|
||||
// create a set of values - build with full set of values
|
||||
gtsam::Values full_values, exp_values;
|
||||
full_values.insert(l3, Point2(1.0, 2.0));
|
||||
full_values.insert(l5, Point2(4.0, 3.0));
|
||||
exp_values = full_values;
|
||||
full_values.insert(l1, Point2(3.0, 7.0));
|
||||
|
||||
// create the test LinearizedFactor
|
||||
// This is called in the constructor of DDFSlot for each linear factor
|
||||
LinearizedFactor actual1(linear_factor, decoder, full_values);
|
||||
LinearizedFactor actual2(linear_factor, ordering, full_values);
|
||||
|
||||
EXPECT(assert_equal(actual1, actual2, tol));
|
||||
|
||||
// Verify the keys
|
||||
vector<gtsam::Key> expKeys;
|
||||
expKeys += l3, l5;
|
||||
EXPECT(assert_container_equality(expKeys, actual1.keys()));
|
||||
EXPECT(assert_container_equality(expKeys, actual2.keys()));
|
||||
|
||||
// Verify subset of linearization points
|
||||
EXPECT(assert_equal(exp_values, actual1.linearizationPoint(), tol));
|
||||
EXPECT(assert_equal(exp_values, actual2.linearizationPoint(), tol));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
|
||||
/* ************************************************************************* */
|
||||
Loading…
Reference in New Issue