12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Returning GaussianFactor instead of JacobianFactor from NonlinearFactor::linearize and NonlinearFactorGraph::linearize, so that HessianFactors may be returned as well.

release/4.3a0
Richard Roberts 2011-04-12 21:18:10 +00:00
parent 6d5dee0860
commit 5057138ef1
14 changed files with 52 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/nonlinear/NonlinearEquality.h
View File

@ -130,13 +130,13 @@ namespace gtsam {
}
// Linearize is over-written, because base linearization tries to whiten
virtual JacobianFactor::shared_ptr linearize(const VALUES& x, const Ordering& ordering) const {
virtual GaussianFactor::shared_ptr linearize(const VALUES& x, const Ordering& ordering) const {
const T& xj = x[this->key_];
Matrix A;
Vector b = evaluateError(xj, A);
// TODO pass unwhitened + noise model to Gaussian factor
SharedDiagonal model = noiseModel::Constrained::All(b.size());
return JacobianFactor::shared_ptr(new JacobianFactor(ordering[this->key_], A, b, model));
return GaussianFactor::shared_ptr(new JacobianFactor(ordering[this->key_], A, b, model));
}
private:

32
gtsam/nonlinear/NonlinearFactor.h
View File

@ -145,7 +145,7 @@ namespace gtsam {
}
/** linearize to a GaussianFactor */
virtual boost::shared_ptr<JacobianFactor>
virtual boost::shared_ptr<GaussianFactor>
linearize(const VALUES& c, const Ordering& ordering) const = 0;
/**
@ -237,7 +237,7 @@ namespace gtsam {
* Ax-b \approx h(x0+dx)-z = h(x0) + A*dx - z
* Hence b = z - h(x0) = - error_vector(x)
*/
virtual boost::shared_ptr<JacobianFactor> linearize(const VALUES& x, const Ordering& ordering) const {
virtual boost::shared_ptr<GaussianFactor> linearize(const VALUES& x, const Ordering& ordering) const {
const X& xj = x[key_];
Matrix A;
Vector b = - evaluateError(xj, A);
@ -246,10 +246,10 @@ namespace gtsam {
SharedDiagonal constrained =
boost::shared_dynamic_cast<noiseModel::Constrained>(this->noiseModel_);
if (constrained.get() != NULL)
return JacobianFactor::shared_ptr(new JacobianFactor(var, A, b, constrained));
return GaussianFactor::shared_ptr(new JacobianFactor(var, A, b, constrained));
this->noiseModel_->WhitenInPlace(A);
this->noiseModel_->whitenInPlace(b);
return JacobianFactor::shared_ptr(new JacobianFactor(var, A, b,
return GaussianFactor::shared_ptr(new JacobianFactor(var, A, b,
noiseModel::Unit::Create(b.size())));
}
@ -348,7 +348,7 @@ namespace gtsam {
* Ax-b \approx h(x1+dx1,x2+dx2)-z = h(x1,x2) + A2*dx1 + A2*dx2 - z
* Hence b = z - h(x1,x2) = - error_vector(x)
*/
boost::shared_ptr<JacobianFactor> linearize(const VALUES& c, const Ordering& ordering) const {
boost::shared_ptr<GaussianFactor> linearize(const VALUES& c, const Ordering& ordering) const {
const X1& x1 = c[key1_];
const X2& x2 = c[key2_];
Matrix A1, A2;
@ -365,10 +365,10 @@ namespace gtsam {
this->noiseModel_->WhitenInPlace(A2);
this->noiseModel_->whitenInPlace(b);
if(var1 < var2)
return JacobianFactor::shared_ptr(new JacobianFactor(var1, A1, var2,
return GaussianFactor::shared_ptr(new JacobianFactor(var1, A1, var2,
A2, b, noiseModel::Unit::Create(b.size())));
else
return JacobianFactor::shared_ptr(new JacobianFactor(var2, A2, var1,
return GaussianFactor::shared_ptr(new JacobianFactor(var2, A2, var1,
A1, b, noiseModel::Unit::Create(b.size())));
}
@ -492,7 +492,7 @@ namespace gtsam {
* Ax-b \approx h(x1+dx1,x2+dx2,x3+dx3)-z = h(x1,x2,x3) + A2*dx1 + A2*dx2 + A3*dx3 - z
* Hence b = z - h(x1,x2,x3) = - error_vector(x)
*/
boost::shared_ptr<JacobianFactor> linearize(const VALUES& c, const Ordering& ordering) const {
boost::shared_ptr<GaussianFactor> linearize(const VALUES& c, const Ordering& ordering) const {
const X1& x1 = c[key1_];
const X2& x2 = c[key2_];
const X3& x3 = c[key3_];
@ -503,7 +503,7 @@ namespace gtsam {
SharedDiagonal constrained =
boost::shared_dynamic_cast<noiseModel::Constrained>(this->noiseModel_);
if (constrained.get() != NULL) {
return JacobianFactor::shared_ptr(
return GaussianFactor::shared_ptr(
new JacobianFactor(var1, A1, var2, A2, var3, A3, b, constrained));
}
this->noiseModel_->WhitenInPlace(A1);
@ -511,26 +511,26 @@ namespace gtsam {
this->noiseModel_->WhitenInPlace(A3);
this->noiseModel_->whitenInPlace(b);
if(var1 < var2 && var2 < var3)
return JacobianFactor::shared_ptr(
return GaussianFactor::shared_ptr(
new JacobianFactor(var1, A1, var2, A2, var3, A3, b, noiseModel::Unit::Create(b.size())));
else if(var2 < var1 && var1 < var3)
return JacobianFactor::shared_ptr(
return GaussianFactor::shared_ptr(
new JacobianFactor(var2, A2, var1, A1, var3, A3, b, noiseModel::Unit::Create(b.size())));
else if(var1 < var3 && var3 < var2)
return JacobianFactor::shared_ptr(
return GaussianFactor::shared_ptr(
new JacobianFactor(var1, A1, var3, A3, var2, A2, b, noiseModel::Unit::Create(b.size())));
else if(var2 < var3 && var3 < var1)
return JacobianFactor::shared_ptr(
return GaussianFactor::shared_ptr(
new JacobianFactor(var2, A2, var3, A3, var1, A1, b, noiseModel::Unit::Create(b.size())));
else if(var3 < var1 && var1 < var2)
return JacobianFactor::shared_ptr(
return GaussianFactor::shared_ptr(
new JacobianFactor(var3, A3, var1, A1, var2, A2, b, noiseModel::Unit::Create(b.size())));
else if(var3 < var2 && var2 < var1)
return JacobianFactor::shared_ptr(
return GaussianFactor::shared_ptr(
new JacobianFactor(var3, A3, var2, A2, var1, A1, b, noiseModel::Unit::Create(b.size())));
else {
assert(false);
return JacobianFactor::shared_ptr();
return GaussianFactor::shared_ptr();
}
}

9
gtsam/nonlinear/NonlinearFactorGraph-inl.h
View File

@ -139,21 +139,20 @@ namespace gtsam {
/* ************************************************************************* */
template<class VALUES>
typename FactorGraph<JacobianFactor>::shared_ptr NonlinearFactorGraph<VALUES>::linearize(
typename FactorGraph<GaussianFactor>::shared_ptr NonlinearFactorGraph<VALUES>::linearize(
const VALUES& config, const Ordering& ordering) const {
// create an empty linear FG
typename FactorGraph<JacobianFactor>::shared_ptr linearFG(new FactorGraph<
JacobianFactor> );
typename FactorGraph<GaussianFactor>::shared_ptr linearFG(new FactorGraph<GaussianFactor>);
linearFG->reserve(this->size());
// linearize all factors
BOOST_FOREACH(const sharedFactor& factor, this->factors_) {
if(factor) {
JacobianFactor::shared_ptr lf = factor->linearize(config, ordering);
GaussianFactor::shared_ptr lf = factor->linearize(config, ordering);
if (lf) linearFG->push_back(lf);
} else
linearFG->push_back(FactorGraph<JacobianFactor>::sharedFactor());
linearFG->push_back(GaussianFactor::shared_ptr());
}
return linearFG;

2
gtsam/nonlinear/NonlinearFactorGraph.h
View File

@ -89,7 +89,7 @@ namespace gtsam {
/**
* linearize a nonlinear factor graph
*/
boost::shared_ptr<FactorGraph<JacobianFactor> >
boost::shared_ptr<FactorGraph<GaussianFactor> >
linearize(const VALUES& config, const Ordering& ordering) const;
private:

4
gtsam/slam/smallExample.cpp
View File

@ -273,7 +273,7 @@ namespace example {
}
/* ************************************************************************* */
pair<FactorGraph<JacobianFactor>, Ordering> createSmoother(int T, boost::optional<Ordering> ordering) {
pair<FactorGraph<GaussianFactor>, Ordering> createSmoother(int T, boost::optional<Ordering> ordering) {
Graph nlfg;
Values poses;
boost::tie(nlfg, poses) = createNonlinearSmoother(T);
@ -431,7 +431,7 @@ namespace example {
}
/* ************************************************************************* */
boost::tuple<FactorGraph<JacobianFactor>, Ordering, VectorValues> planarGraph(size_t N) {
boost::tuple<FactorGraph<GaussianFactor>, Ordering, VectorValues> planarGraph(size_t N) {
// create empty graph
NonlinearFactorGraph<Values> nlfg;

4
gtsam/slam/smallExample.h
View File

@ -93,7 +93,7 @@ namespace gtsam {
* Create a Kalman smoother by linearizing a non-linear factor graph
* @param T number of time-steps
*/
std::pair<FactorGraph<JacobianFactor>, Ordering> createSmoother(int T, boost::optional<Ordering> ordering = boost::none);
std::pair<FactorGraph<GaussianFactor>, Ordering> createSmoother(int T, boost::optional<Ordering> ordering = boost::none);
/* ******************************************************* */
// Linear Constrained Examples
@ -133,7 +133,7 @@ namespace gtsam {
* -x11-x21-x31
* with x11 clamped at (1,1), and others related by 2D odometry.
*/
boost::tuple<FactorGraph<JacobianFactor>, Ordering, VectorValues> planarGraph(size_t N);
boost::tuple<FactorGraph<GaussianFactor>, Ordering, VectorValues> planarGraph(size_t N);
/*
* Create canonical ordering for planar graph that also works for tree

9
gtsam/slam/tests/testPose2Factor.cpp
View File

@ -52,7 +52,8 @@ TEST( Pose2Factor, error )
// Actual linearization
Ordering ordering(*x0.orderingArbitrary());
boost::shared_ptr<JacobianFactor> linear = factor.linearize(x0, ordering);
boost::shared_ptr<JacobianFactor> linear =
boost::dynamic_pointer_cast<JacobianFactor>(factor.linearize(x0, ordering));
// Check error at x0, i.e. delta = zero !
VectorValues delta(x0.dims(ordering));
@ -88,7 +89,8 @@ TEST( Pose2Factor, rhs )
// Actual linearization
Ordering ordering(*x0.orderingArbitrary());
boost::shared_ptr<JacobianFactor> linear = factor.linearize(x0, ordering);
boost::shared_ptr<JacobianFactor> linear =
boost::dynamic_pointer_cast<JacobianFactor>(factor.linearize(x0, ordering));
// Check RHS
Pose2 hx0 = p1.between(p2);
@ -134,7 +136,8 @@ TEST( Pose2Factor, linearize )
JacobianFactor expected(ordering["x1"], expectedH1, ordering["x2"], expectedH2, expected_b, probModel1);
// Actual linearization
boost::shared_ptr<JacobianFactor> actual = factor.linearize(x0, ordering);
boost::shared_ptr<JacobianFactor> actual =
boost::dynamic_pointer_cast<JacobianFactor>(factor.linearize(x0, ordering));
CHECK(assert_equal(expected,*actual));
// Numerical do not work out because BetweenFactor is approximate ?

6
gtsam/slam/tests/testPose2Prior.cpp
View File

@ -44,7 +44,8 @@ TEST( Pose2Prior, error )
// Actual linearization
Ordering ordering(*x0.orderingArbitrary());
boost::shared_ptr<JacobianFactor> linear = factor.linearize(x0, ordering);
boost::shared_ptr<JacobianFactor> linear =
boost::dynamic_pointer_cast<JacobianFactor>(factor.linearize(x0, ordering));
// Check error at x0, i.e. delta = zero !
VectorValues delta(x0.dims(ordering));
@ -86,7 +87,8 @@ TEST( Pose2Prior, linearize )
// Actual linearization
Ordering ordering(*x0.orderingArbitrary());
boost::shared_ptr<JacobianFactor> actual = factor.linearize(x0, ordering);
boost::shared_ptr<JacobianFactor> actual =
boost::dynamic_pointer_cast<JacobianFactor>(factor.linearize(x0, ordering));
// Test with numerical derivative
Matrix numericalH = numericalDerivative11(h, prior);

4
gtsam/slam/tests/testPose2SLAM.cpp
View File

@ -74,11 +74,11 @@ TEST( Pose2Graph, linearization )
config.insert(2,p2);
// Linearize
Ordering ordering(*config.orderingArbitrary());
boost::shared_ptr<FactorGraph<JacobianFactor> > lfg_linearized = graph.linearize(config, ordering);
boost::shared_ptr<FactorGraph<GaussianFactor> > lfg_linearized = graph.linearize(config, ordering);
//lfg_linearized->print("lfg_actual");
// the expected linear factor
FactorGraph<JacobianFactor> lfg_expected;
FactorGraph<GaussianFactor> lfg_expected;
Matrix A1 = Matrix_(3,3,
0.0,-2.0, -4.2,
2.0, 0.0, -4.2,

7
gtsam/slam/tests/testVSLAMFactor.cpp
View File

@ -67,15 +67,16 @@ TEST( ProjectionFactor, error )
Vector b = Vector_(2,3.,0.);
SharedDiagonal probModel1 = noiseModel::Unit::Create(2);
JacobianFactor expected(ordering["x1"], Ax1, ordering["l1"], Al1, b, probModel1);
JacobianFactor::shared_ptr actual = factor->linearize(config, ordering);
JacobianFactor::shared_ptr actual =
boost::dynamic_pointer_cast<JacobianFactor>(factor->linearize(config, ordering));
CHECK(assert_equal(expected,*actual,1e-3));
// linearize graph
Graph graph;
graph.push_back(factor);
FactorGraph<JacobianFactor> expected_lfg;
FactorGraph<GaussianFactor> expected_lfg;
expected_lfg.push_back(actual);
boost::shared_ptr<FactorGraph<JacobianFactor> > actual_lfg = graph.linearize(config, ordering);
boost::shared_ptr<FactorGraph<GaussianFactor> > actual_lfg = graph.linearize(config, ordering);
CHECK(assert_equal(expected_lfg,*actual_lfg));
// expmap on a config

2
tests/testInference.cpp
View File

@ -85,7 +85,7 @@ TEST( Inference, marginals2)
init.insert(PointKey(0), Point2(1.0,1.0));
Ordering ordering(*fg.orderingCOLAMD(init));
FactorGraph<JacobianFactor>::shared_ptr gfg(fg.linearize(init, ordering));
FactorGraph<GaussianFactor>::shared_ptr gfg(fg.linearize(init, ordering));
GaussianMultifrontalSolver solver(*gfg);
solver.marginalFactor(ordering[PointKey(0)]);
}

4
tests/testNonlinearEquality.cpp
View File

@ -50,8 +50,8 @@ TEST ( NonlinearEquality, linearization ) {
// check linearize
SharedDiagonal constraintModel = noiseModel::Constrained::All(3);
JacobianFactor expLF(0, eye(3), zero(3), constraintModel);
JacobianFactor::shared_ptr actualLF = nle->linearize(linearize, *linearize.orderingArbitrary());
EXPECT(assert_equal(*actualLF, expLF));
GaussianFactor::shared_ptr actualLF = nle->linearize(linearize, *linearize.orderingArbitrary());
EXPECT(assert_equal(*actualLF, (const GaussianFactor&)expLF));
}
/* ********************************************************************** */

8
tests/testNonlinearFactor.cpp
View File

@ -200,13 +200,13 @@ TEST( NonlinearFactor, linearize_constraint1 )
Values config;
config.insert(simulated2D::PoseKey(1), Point2(1.0, 2.0));
JacobianFactor::shared_ptr actual = f0->linearize(config, *config.orderingArbitrary());
GaussianFactor::shared_ptr actual = f0->linearize(config, *config.orderingArbitrary());
// create expected
Ordering ord(*config.orderingArbitrary());
Vector b = Vector_(2, 0., -3.);
JacobianFactor expected(ord["x1"], eye(2), b, constraint);
CHECK(assert_equal(expected, *actual));
CHECK(assert_equal((const GaussianFactor&)expected, *actual));
}
/* ************************************************************************* */
@ -221,13 +221,13 @@ TEST( NonlinearFactor, linearize_constraint2 )
Values config;
config.insert(simulated2D::PoseKey(1), Point2(1.0, 2.0));
config.insert(simulated2D::PointKey(1), Point2(5.0, 4.0));
JacobianFactor::shared_ptr actual = f0.linearize(config, *config.orderingArbitrary());
GaussianFactor::shared_ptr actual = f0.linearize(config, *config.orderingArbitrary());
// create expected
Ordering ord(*config.orderingArbitrary());
Vector b = Vector_(2, -3., -3.);
JacobianFactor expected(ord["x1"], -1*eye(2), ord["l1"], eye(2), b, constraint);
CHECK(assert_equal(expected, *actual));
CHECK(assert_equal((const GaussianFactor&)expected, *actual));
}
/* ************************************************************************* */

4
tests/testNonlinearFactorGraph.cpp
View File

@ -96,8 +96,8 @@ TEST( Graph, linearize )
{
Graph fg = createNonlinearFactorGraph();
Values initial = createNoisyValues();
boost::shared_ptr<FactorGraph<JacobianFactor> > linearized = fg.linearize(initial, *initial.orderingArbitrary());
FactorGraph<JacobianFactor> expected = createGaussianFactorGraph(*initial.orderingArbitrary());
boost::shared_ptr<FactorGraph<GaussianFactor> > linearized = fg.linearize(initial, *initial.orderingArbitrary());
FactorGraph<GaussianFactor> expected = createGaussianFactorGraph(*initial.orderingArbitrary());
CHECK(assert_equal(expected,*linearized)); // Needs correct linearizations
}