Moved LinearContainerFactor to gtsam/nonlinear and removed the now-redundant LinearizedFactor from gtsam_unstable
Alex Cunningham
parent
cba120c96d
commit
237da8b3a7
24
.cproject
24
.cproject
|
|
@ -597,6 +597,14 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testLinearContainerFactor.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testLinearContainerFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testGeneralSFMFactor.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
|
|
@ -1345,14 +1353,6 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testLinearContainerFactor.run" path="build/gtsam_unstable/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testLinearContainerFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="check" path="build/inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
|
|
@ -2356,14 +2356,6 @@
|
|||
<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="wrap_gtsam_distclean" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
|
|
|
|||
36
gtsam.h
36
gtsam.h
|
|
@ -1590,6 +1590,42 @@ class JointMarginal {
|
|||
void print() const;
|
||||
};
|
||||
|
||||
#include <gtsam/nonlinear/LinearContainerFactor.h>
|
||||
virtual class LinearContainerFactor : gtsam::NonlinearFactor {
|
||||
|
||||
LinearContainerFactor(gtsam::GaussianFactor* factor, const gtsam::Ordering& ordering,
|
||||
const gtsam::Values& linearizationPoint);
|
||||
LinearContainerFactor(gtsam::GaussianFactor* factor, const gtsam::Values& linearizationPoint);
|
||||
LinearContainerFactor(gtsam::GaussianFactor* factor, const gtsam::InvertedOrdering& ordering,
|
||||
const gtsam::Values& linearizationPoint);
|
||||
|
||||
LinearContainerFactor(gtsam::GaussianFactor* factor, const gtsam::Ordering& ordering);
|
||||
LinearContainerFactor(gtsam::GaussianFactor* factor);
|
||||
LinearContainerFactor(gtsam::GaussianFactor* factor, const gtsam::InvertedOrdering& ordering);
|
||||
|
||||
gtsam::GaussianFactor* factor() const;
|
||||
// const boost::optional<Values>& linearizationPoint() const;
|
||||
|
||||
gtsam::GaussianFactor* order(const gtsam::Ordering& ordering) const;
|
||||
gtsam::GaussianFactor* negate(const gtsam::Ordering& ordering) const;
|
||||
gtsam::NonlinearFactor* negate() const;
|
||||
|
||||
bool isJacobian() const;
|
||||
gtsam::JacobianFactor* toJacobian() const;
|
||||
gtsam::HessianFactor* toHessian() const;
|
||||
|
||||
static gtsam::NonlinearFactorGraph convertLinearGraph(const gtsam::GaussianFactorGraph& linear_graph,
|
||||
const gtsam::Ordering& ordering, const gtsam::Values& linearizationPoint);
|
||||
static gtsam::NonlinearFactorGraph convertLinearGraph(const gtsam::GaussianFactorGraph& linear_graph,
|
||||
const gtsam::InvertedOrdering& invOrdering, const gtsam::Values& linearizationPoint);
|
||||
|
||||
static gtsam::NonlinearFactorGraph convertLinearGraph(const gtsam::GaussianFactorGraph& linear_graph,
|
||||
const gtsam::Ordering& ordering);
|
||||
static gtsam::NonlinearFactorGraph convertLinearGraph(const gtsam::GaussianFactorGraph& linear_graph,
|
||||
const gtsam::InvertedOrdering& invOrdering);
|
||||
|
||||
}; // \class LinearContainerFactor
|
||||
|
||||
//*************************************************************************
|
||||
// Nonlinear optimizers
|
||||
//*************************************************************************
|
||||
|
|
|
|||
|
|
@ -5,7 +5,7 @@
|
|||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
#include <gtsam_unstable/nonlinear/LinearContainerFactor.h>
|
||||
#include <gtsam/nonlinear/LinearContainerFactor.h>
|
||||
|
||||
#include <boost/foreach.hpp>
|
||||
|
||||
|
|
@ -9,7 +9,7 @@
|
|||
|
||||
#include <boost/assign/std/vector.hpp>
|
||||
|
||||
#include <gtsam_unstable/nonlinear/LinearContainerFactor.h>
|
||||
#include <gtsam/nonlinear/LinearContainerFactor.h>
|
||||
|
||||
#include <gtsam/base/TestableAssertions.h>
|
||||
#include <gtsam/geometry/Pose2.h>
|
||||
|
|
@ -6,7 +6,7 @@ set (gtsam_unstable_subdirs
|
|||
discrete
|
||||
# linear
|
||||
dynamics
|
||||
nonlinear
|
||||
# nonlinear
|
||||
slam
|
||||
)
|
||||
|
||||
|
|
@ -36,7 +36,7 @@ set(gtsam_unstable_srcs
|
|||
${discrete_srcs}
|
||||
${dynamics_srcs}
|
||||
#${linear_srcs}
|
||||
${nonlinear_srcs}
|
||||
#${nonlinear_srcs}
|
||||
${slam_srcs}
|
||||
)
|
||||
|
||||
|
|
|
|||
|
|
@ -177,41 +177,7 @@ virtual class DGroundConstraint : gtsam::NonlinearFactor {
|
|||
//*************************************************************************
|
||||
// nonlinear
|
||||
//*************************************************************************
|
||||
#include <gtsam_unstable/nonlinear/LinearContainerFactor.h>
|
||||
virtual class LinearContainerFactor : gtsam::NonlinearFactor {
|
||||
|
||||
LinearContainerFactor(gtsam::GaussianFactor* factor, const gtsam::Ordering& ordering,
|
||||
const gtsam::Values& linearizationPoint);
|
||||
LinearContainerFactor(gtsam::GaussianFactor* factor, const gtsam::Values& linearizationPoint);
|
||||
LinearContainerFactor(gtsam::GaussianFactor* factor, const gtsam::InvertedOrdering& ordering,
|
||||
const gtsam::Values& linearizationPoint);
|
||||
|
||||
LinearContainerFactor(gtsam::GaussianFactor* factor, const gtsam::Ordering& ordering);
|
||||
LinearContainerFactor(gtsam::GaussianFactor* factor);
|
||||
LinearContainerFactor(gtsam::GaussianFactor* factor, const gtsam::InvertedOrdering& ordering);
|
||||
|
||||
gtsam::GaussianFactor* factor() const;
|
||||
// const boost::optional<Values>& linearizationPoint() const;
|
||||
|
||||
gtsam::GaussianFactor* order(const gtsam::Ordering& ordering) const;
|
||||
gtsam::GaussianFactor* negate(const gtsam::Ordering& ordering) const;
|
||||
gtsam::NonlinearFactor* negate() const;
|
||||
|
||||
bool isJacobian() const;
|
||||
gtsam::JacobianFactor* toJacobian() const;
|
||||
gtsam::HessianFactor* toHessian() const;
|
||||
|
||||
static gtsam::NonlinearFactorGraph convertLinearGraph(const gtsam::GaussianFactorGraph& linear_graph,
|
||||
const gtsam::Ordering& ordering, const gtsam::Values& linearizationPoint);
|
||||
static gtsam::NonlinearFactorGraph convertLinearGraph(const gtsam::GaussianFactorGraph& linear_graph,
|
||||
const gtsam::InvertedOrdering& invOrdering, const gtsam::Values& linearizationPoint);
|
||||
|
||||
static gtsam::NonlinearFactorGraph convertLinearGraph(const gtsam::GaussianFactorGraph& linear_graph,
|
||||
const gtsam::Ordering& ordering);
|
||||
static gtsam::NonlinearFactorGraph convertLinearGraph(const gtsam::GaussianFactorGraph& linear_graph,
|
||||
const gtsam::InvertedOrdering& invOrdering);
|
||||
|
||||
}; // \class LinearContainerFactor
|
||||
|
||||
//*************************************************************************
|
||||
// slam
|
||||
|
|
|
|||
|
|
@ -1,148 +0,0 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 NonlinearFactor& other, double tol) const {
|
||||
const LinearizedFactor* e = dynamic_cast<const LinearizedFactor*>(&other);
|
||||
if (!e || !Base::equals(other, tol)
|
||||
|| !lin_points_.equals(e->lin_points_, tol)
|
||||
|| !equal_with_abs_tol(b_, e->b_, tol)
|
||||
|| !model_->equals(*e->model_, tol)
|
||||
|| matrices_.size() != e->matrices_.size())
|
||||
return false;
|
||||
|
||||
KeyMatrixMap::const_iterator map1 = matrices_.begin(), map2 = e->matrices_.begin();
|
||||
for (; map1 != matrices_.end() && map2 != e->matrices_.end(); ++map1, ++map2)
|
||||
if ((map1->first != map2->first) || !equal_with_abs_tol(map1->second, map2->second, tol))
|
||||
return false;
|
||||
return true;
|
||||
}
|
||||
|
||||
} // \namespace gtsam
|
||||
|
|
@ -1,125 +0,0 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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() {}
|
||||
|
||||
/// @return a deep copy of this factor
|
||||
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
|
||||
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
|
||||
gtsam::NonlinearFactor::shared_ptr(new This(*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 NonlinearFactor& 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
|
||||
|
||||
|
|
@ -1,69 +0,0 @@
|
|||
/**
|
||||
* @file testLinearizedFactor
|
||||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
#include <boost/assign/std/vector.hpp>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <gtsam/base/TestableAssertions.h>
|
||||
#include <gtsam/geometry/Point2.h>
|
||||
#include <gtsam/nonlinear/Values.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