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

Making some tests compile

release/4.3a0
Richard Roberts 2013-08-05 22:31:26 +00:00
parent 90b1349f23
commit 09643929fd
11 changed files with 136 additions and 143 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
gtsam/inference/FactorGraph.h
View File

@ -178,6 +178,12 @@ namespace gtsam {
bayesTree.addFactorsToGraph(*this); bayesTree.addFactorsToGraph(*this);
} }
/** Add a factor or a shared_ptr to a factor, synonym for push_back() */
template<class FACTOR>
void add(const FACTOR& factor) {
push_back(factor);
}
/** += syntax for push_back, e.g. graph += f1, f2, f3 */ /** += syntax for push_back, e.g. graph += f1, f2, f3 */
//template<class T> //template<class T>
//boost::assign::list_inserter<boost::function<void(const T&)> > //boost::assign::list_inserter<boost::function<void(const T&)> >

6
gtsam/linear/tests/testGaussianBayesTreeUnordered.cpp
View File

@ -78,7 +78,7 @@ namespace {
* *
* 1 0 0 1 * 1 0 0 1
*/ */
TEST( GaussianBayesTreeOrdered, eliminate ) TEST( GaussianBayesTree, eliminate )
{ {
GaussianBayesTree bt = *chain.eliminateMultifrontal(chainOrdering); GaussianBayesTree bt = *chain.eliminateMultifrontal(chainOrdering);
@ -94,7 +94,7 @@ TEST( GaussianBayesTreeOrdered, eliminate )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( GaussianBayesTreeOrdered, optimizeMultiFrontal ) TEST( GaussianBayesTree, optimizeMultiFrontal )
{ {
VectorValues expected = pair_list_of VectorValues expected = pair_list_of
(x1, Vector_(1, 0.)) (x1, Vector_(1, 0.))
@ -107,7 +107,7 @@ TEST( GaussianBayesTreeOrdered, optimizeMultiFrontal )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(GaussianBayesTreeOrdered, complicatedMarginal) { TEST(GaussianBayesTree, complicatedMarginal) {
// Create the conditionals to go in the BayesTree // Create the conditionals to go in the BayesTree
GaussianBayesTree bt; GaussianBayesTree bt;

2
gtsam/linear/tests/testJacobianFactorUnordered.cpp
View File

@ -397,7 +397,7 @@ TEST(JacobianFactor, eliminate2 )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(JacobianFactor, EliminateQROrdered) TEST(JacobianFactor, EliminateQR)
{ {
// Augmented Ab test case for whole factor graph // Augmented Ab test case for whole factor graph
Matrix Ab = Matrix_(14,11, Matrix Ab = Matrix_(14,11,

64
gtsam/nonlinear/tests/testLinearContainerFactor.cpp
View File

@ -28,7 +28,7 @@ Pose2 poseA1(0.0, 0.0, 0.0), poseA2(2.0, 0.0, 0.0);
/* ************************************************************************* */ /* ************************************************************************* */
TEST( testLinearContainerFactor, generic_jacobian_factor ) { TEST( testLinearContainerFactor, generic_jacobian_factor ) {
OrderingOrdered initOrdering; initOrdering += x1, x2, l1, l2; Ordering initOrdering; initOrdering += x1, x2, l1, l2;
Matrix A1 = Matrix_(2,2, Matrix A1 = Matrix_(2,2,
2.74222, -0.0067457, 2.74222, -0.0067457,
@ -39,7 +39,7 @@ TEST( testLinearContainerFactor, generic_jacobian_factor ) {
Vector b = Vector_(2, 0.0277052, Vector b = Vector_(2, 0.0277052,
-0.0533393); -0.0533393);
JacobianFactorOrdered expLinFactor(initOrdering[l1], A1, initOrdering[l2], A2, b, diag_model2); JacobianFactor expLinFactor(initOrdering[l1], A1, initOrdering[l2], A2, b, diag_model2);
LinearContainerFactor actFactor(expLinFactor, initOrdering); LinearContainerFactor actFactor(expLinFactor, initOrdering);
EXPECT_LONGS_EQUAL(2, actFactor.size()); EXPECT_LONGS_EQUAL(2, actFactor.size());
@ -57,20 +57,20 @@ TEST( testLinearContainerFactor, generic_jacobian_factor ) {
values.insert(x2, poseA2); values.insert(x2, poseA2);
// Check reconstruction from same ordering // Check reconstruction from same ordering
GaussianFactorOrdered::shared_ptr actLinearizationA = actFactor.linearize(values, initOrdering); GaussianFactor::shared_ptr actLinearizationA = actFactor.linearize(values, initOrdering);
EXPECT(assert_equal(*expLinFactor.clone(), *actLinearizationA, tol)); EXPECT(assert_equal(*expLinFactor.clone(), *actLinearizationA, tol));
// Check reconstruction from new ordering // Check reconstruction from new ordering
OrderingOrdered newOrdering; newOrdering += x1, l1, x2, l2; Ordering newOrdering; newOrdering += x1, l1, x2, l2;
GaussianFactorOrdered::shared_ptr actLinearizationB = actFactor.linearize(values, newOrdering); GaussianFactor::shared_ptr actLinearizationB = actFactor.linearize(values, newOrdering);
JacobianFactorOrdered expLinFactor2(newOrdering[l1], A1, newOrdering[l2], A2, b, diag_model2); JacobianFactor expLinFactor2(newOrdering[l1], A1, newOrdering[l2], A2, b, diag_model2);
EXPECT(assert_equal(*expLinFactor2.clone(), *actLinearizationB, tol)); EXPECT(assert_equal(*expLinFactor2.clone(), *actLinearizationB, tol));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( testLinearContainerFactor, jacobian_factor_withlinpoints ) { TEST( testLinearContainerFactor, jacobian_factor_withlinpoints ) {
OrderingOrdered ordering; ordering += x1, x2, l1, l2; Ordering ordering; ordering += x1, x2, l1, l2;
Matrix A1 = Matrix_(2,2, Matrix A1 = Matrix_(2,2,
2.74222, -0.0067457, 2.74222, -0.0067457,
@ -81,7 +81,7 @@ TEST( testLinearContainerFactor, jacobian_factor_withlinpoints ) {
Vector b = Vector_(2, 0.0277052, Vector b = Vector_(2, 0.0277052,
-0.0533393); -0.0533393);
JacobianFactorOrdered expLinFactor(ordering[l1], A1, ordering[l2], A2, b, diag_model2); JacobianFactor expLinFactor(ordering[l1], A1, ordering[l2], A2, b, diag_model2);
Values values; Values values;
values.insert(l1, landmark1); values.insert(l1, landmark1);
@ -108,7 +108,7 @@ TEST( testLinearContainerFactor, jacobian_factor_withlinpoints ) {
Vector delta_l1 = Vector_(2, 1.0, 2.0); Vector delta_l1 = Vector_(2, 1.0, 2.0);
Vector delta_l2 = Vector_(2, 3.0, 4.0); Vector delta_l2 = Vector_(2, 3.0, 4.0);
VectorValuesOrdered delta = values.zeroVectors(ordering); VectorValues delta = values.zeroVectors(ordering);
delta.at(ordering[l1]) = delta_l1; delta.at(ordering[l1]) = delta_l1;
delta.at(ordering[l2]) = delta_l2; delta.at(ordering[l2]) = delta_l2;
Values noisyValues = values.retract(delta, ordering); Values noisyValues = values.retract(delta, ordering);
@ -117,10 +117,10 @@ TEST( testLinearContainerFactor, jacobian_factor_withlinpoints ) {
EXPECT_DOUBLES_EQUAL(expLinFactor.error(values.zeroVectors(ordering)), actFactor.error(values), tol); EXPECT_DOUBLES_EQUAL(expLinFactor.error(values.zeroVectors(ordering)), actFactor.error(values), tol);
// Check linearization with corrections for updated linearization point // Check linearization with corrections for updated linearization point
OrderingOrdered newOrdering; newOrdering += x1, l1, x2, l2; Ordering newOrdering; newOrdering += x1, l1, x2, l2;
GaussianFactorOrdered::shared_ptr actLinearizationB = actFactor.linearize(noisyValues, newOrdering); GaussianFactor::shared_ptr actLinearizationB = actFactor.linearize(noisyValues, newOrdering);
Vector bprime = b - A1 * delta_l1 - A2 * delta_l2; Vector bprime = b - A1 * delta_l1 - A2 * delta_l2;
JacobianFactorOrdered expLinFactor2(newOrdering[l1], A1, newOrdering[l2], A2, bprime, diag_model2); JacobianFactor expLinFactor2(newOrdering[l1], A1, newOrdering[l2], A2, bprime, diag_model2);
EXPECT(assert_equal(*expLinFactor2.clone(), *actLinearizationB, tol)); EXPECT(assert_equal(*expLinFactor2.clone(), *actLinearizationB, tol));
} }
@ -145,8 +145,8 @@ TEST( testLinearContainerFactor, generic_hessian_factor ) {
double f = 10.0; double f = 10.0;
OrderingOrdered initOrdering; initOrdering += x1, x2, l1, l2; Ordering initOrdering; initOrdering += x1, x2, l1, l2;
HessianFactorOrdered initFactor(initOrdering[x1], initOrdering[x2], initOrdering[l1], HessianFactor initFactor(initOrdering[x1], initOrdering[x2], initOrdering[l1],
G11, G12, G13, g1, G22, G23, g2, G33, g3, f); G11, G12, G13, g1, G22, G23, g2, G33, g3, f);
Values values; Values values;
@ -159,13 +159,13 @@ TEST( testLinearContainerFactor, generic_hessian_factor ) {
EXPECT(!actFactor.isJacobian()); EXPECT(!actFactor.isJacobian());
EXPECT(actFactor.isHessian()); EXPECT(actFactor.isHessian());
GaussianFactorOrdered::shared_ptr actLinearization1 = actFactor.linearize(values, initOrdering); GaussianFactor::shared_ptr actLinearization1 = actFactor.linearize(values, initOrdering);
EXPECT(assert_equal(*initFactor.clone(), *actLinearization1, tol)); EXPECT(assert_equal(*initFactor.clone(), *actLinearization1, tol));
OrderingOrdered newOrdering; newOrdering += l1, x1, x2, l2; Ordering newOrdering; newOrdering += l1, x1, x2, l2;
HessianFactorOrdered expLinFactor(newOrdering[x1], newOrdering[x2], newOrdering[l1], HessianFactor expLinFactor(newOrdering[x1], newOrdering[x2], newOrdering[l1],
G11, G12, G13, g1, G22, G23, g2, G33, g3, f); G11, G12, G13, g1, G22, G23, g2, G33, g3, f);
GaussianFactorOrdered::shared_ptr actLinearization2 = actFactor.linearize(values, newOrdering); GaussianFactor::shared_ptr actLinearization2 = actFactor.linearize(values, newOrdering);
EXPECT(assert_equal(*expLinFactor.clone(), *actLinearization2, tol)); EXPECT(assert_equal(*expLinFactor.clone(), *actLinearization2, tol));
} }
@ -196,8 +196,8 @@ TEST( testLinearContainerFactor, hessian_factor_withlinpoints ) {
Matrix G(5,5); Matrix G(5,5);
G << G11, G12, Matrix::Zero(2,3), G22; G << G11, G12, Matrix::Zero(2,3), G22;
OrderingOrdered ordering; ordering += x1, x2, l1; Ordering ordering; ordering += x1, x2, l1;
HessianFactorOrdered initFactor(ordering[x1], ordering[l1], G11, G12, g1, G22, g2, f); HessianFactor initFactor(ordering[x1], ordering[l1], G11, G12, g1, G22, g2, f);
Values linearizationPoint, expLinPoints; Values linearizationPoint, expLinPoints;
linearizationPoint.insert(l1, landmark1); linearizationPoint.insert(l1, landmark1);
@ -219,7 +219,7 @@ TEST( testLinearContainerFactor, hessian_factor_withlinpoints ) {
Vector delta_x2 = Vector_(3, 6.0, 7.0, 0.3); Vector delta_x2 = Vector_(3, 6.0, 7.0, 0.3);
// Check error calculation // Check error calculation
VectorValuesOrdered delta = linearizationPoint.zeroVectors(ordering); VectorValues delta = linearizationPoint.zeroVectors(ordering);
delta.at(ordering[l1]) = delta_l1; delta.at(ordering[l1]) = delta_l1;
delta.at(ordering[x1]) = delta_x1; delta.at(ordering[x1]) = delta_x1;
delta.at(ordering[x2]) = delta_x2; delta.at(ordering[x2]) = delta_x2;
@ -239,7 +239,7 @@ TEST( testLinearContainerFactor, hessian_factor_withlinpoints ) {
Vector g1_prime = g_prime.head(3); Vector g1_prime = g_prime.head(3);
Vector g2_prime = g_prime.tail(2); Vector g2_prime = g_prime.tail(2);
double f_prime = f + dv.transpose() * G.selfadjointView<Eigen::Upper>() * dv - 2.0 * dv.transpose() * g; double f_prime = f + dv.transpose() * G.selfadjointView<Eigen::Upper>() * dv - 2.0 * dv.transpose() * g;
HessianFactorOrdered expNewFactor(ordering[x1], ordering[l1], G11, G12, g1_prime, G22, g2_prime, f_prime); HessianFactor expNewFactor(ordering[x1], ordering[l1], G11, G12, g1_prime, G22, g2_prime, f_prime);
EXPECT(assert_equal(*expNewFactor.clone(), *actFactor.linearize(noisyValues, ordering), tol)); EXPECT(assert_equal(*expNewFactor.clone(), *actFactor.linearize(noisyValues, ordering), tol));
} }
@ -252,12 +252,12 @@ TEST( testLinearContainerFactor, creation ) {
l7 = 17, l8 = 18; l7 = 17, l8 = 18;
// creating an ordering to decode the linearized factor // creating an ordering to decode the linearized factor
OrderingOrdered ordering; Ordering ordering;
ordering += l1,l2,l3,l4,l5,l6,l7,l8; ordering += l1,l2,l3,l4,l5,l6,l7,l8;
// create a linear factor // create a linear factor
SharedDiagonal model = noiseModel::Unit::Create(2); SharedDiagonal model = noiseModel::Unit::Create(2);
JacobianFactorOrdered::shared_ptr linear_factor(new JacobianFactorOrdered( JacobianFactor::shared_ptr linear_factor(new JacobianFactor(
ordering[l3], eye(2,2), ordering[l5], 2.0 * eye(2,2), zero(2), model)); 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 // create a set of values - build with full set of values
@ -284,7 +284,7 @@ TEST( testLinearContainerFactor, jacobian_relinearize )
// Create a Between Factor from a Point3. This is actually a linear factor. // Create a Between Factor from a Point3. This is actually a linear factor.
gtsam::Key key1(1); gtsam::Key key1(1);
gtsam::Key key2(2); gtsam::Key key2(2);
gtsam::OrderingOrdered ordering; gtsam::Ordering ordering;
ordering.push_back(key1); ordering.push_back(key1);
ordering.push_back(key2); ordering.push_back(key2);
gtsam::Values linpoint1; gtsam::Values linpoint1;
@ -296,7 +296,7 @@ TEST( testLinearContainerFactor, jacobian_relinearize )
gtsam::BetweenFactor<gtsam::Point3> betweenFactor(key1, key2, measured, model); gtsam::BetweenFactor<gtsam::Point3> betweenFactor(key1, key2, measured, model);
// Create a jacobian container factor at linpoint 1 // Create a jacobian container factor at linpoint 1
gtsam::JacobianFactorOrdered::shared_ptr jacobian(new gtsam::JacobianFactorOrdered(*betweenFactor.linearize(linpoint1, ordering))); gtsam::JacobianFactor::shared_ptr jacobian(new gtsam::JacobianFactor(*betweenFactor.linearize(linpoint1, ordering)));
gtsam::LinearContainerFactor jacobianContainer(jacobian, ordering, linpoint1); gtsam::LinearContainerFactor jacobianContainer(jacobian, ordering, linpoint1);
// Create a second linearization point // Create a second linearization point
@ -310,8 +310,8 @@ TEST( testLinearContainerFactor, jacobian_relinearize )
EXPECT_DOUBLES_EQUAL(expected_error, actual_error, 1e-9 ); EXPECT_DOUBLES_EQUAL(expected_error, actual_error, 1e-9 );
// Re-linearize around the new point and check the factors // Re-linearize around the new point and check the factors
gtsam::GaussianFactorOrdered::shared_ptr expected_factor = betweenFactor.linearize(linpoint2, ordering); gtsam::GaussianFactor::shared_ptr expected_factor = betweenFactor.linearize(linpoint2, ordering);
gtsam::GaussianFactorOrdered::shared_ptr actual_factor = jacobianContainer.linearize(linpoint2, ordering); gtsam::GaussianFactor::shared_ptr actual_factor = jacobianContainer.linearize(linpoint2, ordering);
CHECK(gtsam::assert_equal(*expected_factor, *actual_factor)); CHECK(gtsam::assert_equal(*expected_factor, *actual_factor));
} }
@ -321,7 +321,7 @@ TEST( testLinearContainerFactor, hessian_relinearize )
// Create a Between Factor from a Point3. This is actually a linear factor. // Create a Between Factor from a Point3. This is actually a linear factor.
gtsam::Key key1(1); gtsam::Key key1(1);
gtsam::Key key2(2); gtsam::Key key2(2);
gtsam::OrderingOrdered ordering; gtsam::Ordering ordering;
ordering.push_back(key1); ordering.push_back(key1);
ordering.push_back(key2); ordering.push_back(key2);
gtsam::Values linpoint1; gtsam::Values linpoint1;
@ -333,7 +333,7 @@ TEST( testLinearContainerFactor, hessian_relinearize )
gtsam::BetweenFactor<gtsam::Point3> betweenFactor(key1, key2, measured, model); gtsam::BetweenFactor<gtsam::Point3> betweenFactor(key1, key2, measured, model);
// Create a hessian container factor at linpoint 1 // Create a hessian container factor at linpoint 1
gtsam::HessianFactorOrdered::shared_ptr hessian(new gtsam::HessianFactorOrdered(*betweenFactor.linearize(linpoint1, ordering))); gtsam::HessianFactor::shared_ptr hessian(new gtsam::HessianFactor(*betweenFactor.linearize(linpoint1, ordering)));
gtsam::LinearContainerFactor hessianContainer(hessian, ordering, linpoint1); gtsam::LinearContainerFactor hessianContainer(hessian, ordering, linpoint1);
// Create a second linearization point // Create a second linearization point
@ -347,8 +347,8 @@ TEST( testLinearContainerFactor, hessian_relinearize )
EXPECT_DOUBLES_EQUAL(expected_error, actual_error, 1e-9 ); EXPECT_DOUBLES_EQUAL(expected_error, actual_error, 1e-9 );
// Re-linearize around the new point and check the factors // Re-linearize around the new point and check the factors
gtsam::GaussianFactorOrdered::shared_ptr expected_factor = gtsam::HessianFactorOrdered::shared_ptr(new gtsam::HessianFactorOrdered(*betweenFactor.linearize(linpoint2, ordering))); gtsam::GaussianFactor::shared_ptr expected_factor = gtsam::HessianFactor::shared_ptr(new gtsam::HessianFactor(*betweenFactor.linearize(linpoint2, ordering)));
gtsam::GaussianFactorOrdered::shared_ptr actual_factor = hessianContainer.linearize(linpoint2, ordering); gtsam::GaussianFactor::shared_ptr actual_factor = hessianContainer.linearize(linpoint2, ordering);
CHECK(gtsam::assert_equal(*expected_factor, *actual_factor)); CHECK(gtsam::assert_equal(*expected_factor, *actual_factor));
} }

32
gtsam/nonlinear/tests/testValues.cpp
View File

@ -16,6 +16,7 @@
#include <gtsam/nonlinear/Values.h> #include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/Symbol.h> #include <gtsam/nonlinear/Symbol.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/geometry/Pose2.h> #include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Pose3.h> #include <gtsam/geometry/Pose3.h>
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
@ -24,6 +25,7 @@
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <boost/assign/std/list.hpp> // for operator += #include <boost/assign/std/list.hpp> // for operator +=
#include <boost/assign/list_of.hpp>
using namespace boost::assign; using namespace boost::assign;
#include <stdexcept> #include <stdexcept>
#include <limits> #include <limits>
@ -155,7 +157,7 @@ TEST( Values, update_element )
// config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0)); // config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0));
// LONGS_EQUAL(5, config0.dim()); // LONGS_EQUAL(5, config0.dim());
// //
// VectorValuesOrdered expected; // VectorValues expected;
// expected.insert(key1, zero(2)); // expected.insert(key1, zero(2));
// expected.insert(key2, zero(3)); // expected.insert(key2, zero(3));
// CHECK(assert_equal(expected, config0.zero())); // CHECK(assert_equal(expected, config0.zero()));
@ -168,16 +170,15 @@ TEST(Values, expmap_a)
config0.insert(key1, LieVector(3, 1.0, 2.0, 3.0)); config0.insert(key1, LieVector(3, 1.0, 2.0, 3.0));
config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0)); config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0));
OrderingOrdered ordering(*config0.orderingArbitrary()); VectorValues increment = pair_list_of
VectorValuesOrdered increment(config0.dims(ordering)); (key1, Vector_(3, 1.0, 1.1, 1.2))
increment[ordering[key1]] = Vector_(3, 1.0, 1.1, 1.2); (key2, Vector_(3, 1.3, 1.4, 1.5));
increment[ordering[key2]] = Vector_(3, 1.3, 1.4, 1.5);
Values expected; Values expected;
expected.insert(key1, LieVector(3, 2.0, 3.1, 4.2)); expected.insert(key1, LieVector(3, 2.0, 3.1, 4.2));
expected.insert(key2, LieVector(3, 6.3, 7.4, 8.5)); expected.insert(key2, LieVector(3, 6.3, 7.4, 8.5));
CHECK(assert_equal(expected, config0.retract(increment, ordering))); CHECK(assert_equal(expected, config0.retract(increment)));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -187,15 +188,14 @@ TEST(Values, expmap_b)
config0.insert(key1, LieVector(3, 1.0, 2.0, 3.0)); config0.insert(key1, LieVector(3, 1.0, 2.0, 3.0));
config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0)); config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0));
OrderingOrdered ordering(*config0.orderingArbitrary()); VectorValues increment = pair_list_of
VectorValuesOrdered increment(VectorValuesOrdered::Zero(config0.dims(ordering))); (key2, LieVector(3, 1.3, 1.4, 1.5));
increment[ordering[key2]] = LieVector(3, 1.3, 1.4, 1.5);
Values expected; Values expected;
expected.insert(key1, LieVector(3, 1.0, 2.0, 3.0)); expected.insert(key1, LieVector(3, 1.0, 2.0, 3.0));
expected.insert(key2, LieVector(3, 6.3, 7.4, 8.5)); expected.insert(key2, LieVector(3, 6.3, 7.4, 8.5));
CHECK(assert_equal(expected, config0.retract(increment, ordering))); CHECK(assert_equal(expected, config0.retract(increment)));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -241,15 +241,13 @@ TEST(Values, localCoordinates)
valuesA.insert(key1, LieVector(3, 1.0, 2.0, 3.0)); valuesA.insert(key1, LieVector(3, 1.0, 2.0, 3.0));
valuesA.insert(key2, LieVector(3, 5.0, 6.0, 7.0)); valuesA.insert(key2, LieVector(3, 5.0, 6.0, 7.0));
OrderingOrdered ordering = *valuesA.orderingArbitrary(); VectorValues expDelta = pair_list_of
(key1, Vector_(3, 0.1, 0.2, 0.3))
(key2, Vector_(3, 0.4, 0.5, 0.6));
VectorValuesOrdered expDelta = valuesA.zeroVectors(ordering); Values valuesB = valuesA.retract(expDelta);
// expDelta.at(ordering[key1]) = Vector_(3, 0.1, 0.2, 0.3);
// expDelta.at(ordering[key2]) = Vector_(3, 0.4, 0.5, 0.6);
Values valuesB = valuesA.retract(expDelta, ordering); EXPECT(assert_equal(expDelta, valuesA.localCoordinates(valuesB)));
EXPECT(assert_equal(expDelta, valuesA.localCoordinates(valuesB, ordering)));
} }
/* ************************************************************************* */ /* ************************************************************************* */

33
gtsam_unstable/linear/tests/testBayesTreeOperations.cpp
View File

@ -10,7 +10,6 @@
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <gtsam_unstable/linear/bayesTreeOperations.h> #include <gtsam_unstable/linear/bayesTreeOperations.h>
#include <gtsam/linear/GaussianMultifrontalSolver.h>
#include <tests/smallExample.h> #include <tests/smallExample.h>
@ -31,7 +30,7 @@ static const double tol = 1e-4;
TEST( testBayesTreeOperations, splitFactor1 ) { TEST( testBayesTreeOperations, splitFactor1 ) {
// Build upper-triangular system // Build upper-triangular system
JacobianFactorOrdered initFactor( JacobianFactor initFactor(
0,Matrix_(4, 2, 0,Matrix_(4, 2,
1.0, 2.0, 1.0, 2.0,
0.0, 3.0, 0.0, 3.0,
@ -45,8 +44,8 @@ TEST( testBayesTreeOperations, splitFactor1 ) {
Vector_(4, 0.1, 0.2, 0.3, 0.4), Vector_(4, 0.1, 0.2, 0.3, 0.4),
model4); model4);
GaussianFactorGraphOrdered actSplit = splitFactor(initFactor.clone()); GaussianFactorGraph actSplit = splitFactor(initFactor.clone());
GaussianFactorGraphOrdered expSplit; GaussianFactorGraph expSplit;
expSplit.add( expSplit.add(
0,Matrix_(2, 2, 0,Matrix_(2, 2,
@ -71,7 +70,7 @@ TEST( testBayesTreeOperations, splitFactor1 ) {
TEST( testBayesTreeOperations, splitFactor2 ) { TEST( testBayesTreeOperations, splitFactor2 ) {
// Build upper-triangular system // Build upper-triangular system
JacobianFactorOrdered initFactor( JacobianFactor initFactor(
0,Matrix_(6, 2, 0,Matrix_(6, 2,
1.0, 2.0, 1.0, 2.0,
0.0, 3.0, 0.0, 3.0,
@ -96,8 +95,8 @@ TEST( testBayesTreeOperations, splitFactor2 ) {
Vector_(6, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6), Vector_(6, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6),
model6); model6);
GaussianFactorGraphOrdered actSplit = splitFactor(initFactor.clone()); GaussianFactorGraph actSplit = splitFactor(initFactor.clone());
GaussianFactorGraphOrdered expSplit; GaussianFactorGraph expSplit;
expSplit.add( expSplit.add(
0,Matrix_(2, 2, 0,Matrix_(2, 2,
@ -134,7 +133,7 @@ TEST( testBayesTreeOperations, splitFactor2 ) {
TEST( testBayesTreeOperations, splitFactor3 ) { TEST( testBayesTreeOperations, splitFactor3 ) {
// Build upper-triangular system // Build upper-triangular system
JacobianFactorOrdered initFactor( JacobianFactor initFactor(
0,Matrix_(4, 2, 0,Matrix_(4, 2,
1.0, 2.0, 1.0, 2.0,
0.0, 3.0, 0.0, 3.0,
@ -153,8 +152,8 @@ TEST( testBayesTreeOperations, splitFactor3 ) {
Vector_(4, 0.1, 0.2, 0.3, 0.4), Vector_(4, 0.1, 0.2, 0.3, 0.4),
model4); model4);
GaussianFactorGraphOrdered actSplit = splitFactor(initFactor.clone()); GaussianFactorGraph actSplit = splitFactor(initFactor.clone());
GaussianFactorGraphOrdered expSplit; GaussianFactorGraph expSplit;
expSplit.add( expSplit.add(
0,Matrix_(2, 2, 0,Matrix_(2, 2,
@ -192,12 +191,12 @@ TEST( testBayesTreeOperations, liquefy ) {
using namespace example; using namespace example;
// Create smoother with 7 nodes // Create smoother with 7 nodes
OrderingOrdered ordering; Ordering ordering;
ordering += X(1),X(3),X(5),X(7),X(2),X(6),X(4); ordering += X(1),X(3),X(5),X(7),X(2),X(6),X(4);
GaussianFactorGraphOrdered smoother = createSmoother(7, ordering).first; GaussianFactorGraph smoother = createSmoother(7, ordering).first;
// Create the Bayes tree // Create the Bayes tree
GaussianBayesTreeOrdered bayesTree = *GaussianMultifrontalSolver(smoother).eliminate(); GaussianBayesTree bayesTree = *GaussianMultifrontalSolver(smoother).eliminate();
// bayesTree.print("Full tree"); // bayesTree.print("Full tree");
SharedDiagonal unit6 = noiseModel::Diagonal::Sigmas(Vector_(ones(6))); SharedDiagonal unit6 = noiseModel::Diagonal::Sigmas(Vector_(ones(6)));
@ -206,8 +205,8 @@ TEST( testBayesTreeOperations, liquefy ) {
// Liquefy the tree back into a graph // Liquefy the tree back into a graph
{ {
GaussianFactorGraphOrdered actGraph = liquefy(bayesTree, false); // Doesn't split conditionals GaussianFactorGraph actGraph = liquefy(bayesTree, false); // Doesn't split conditionals
GaussianFactorGraphOrdered expGraph; GaussianFactorGraph expGraph;
Matrix A12 = Matrix_(6, 2, Matrix A12 = Matrix_(6, 2,
1.73205081, 0.0, 1.73205081, 0.0,
@ -278,8 +277,8 @@ TEST( testBayesTreeOperations, liquefy ) {
// Liquefy the tree back into a graph, splitting factors // Liquefy the tree back into a graph, splitting factors
{ {
GaussianFactorGraphOrdered actGraph = liquefy(bayesTree, true); GaussianFactorGraph actGraph = liquefy(bayesTree, true);
GaussianFactorGraphOrdered expGraph; GaussianFactorGraph expGraph;
// Conditional 1 // Conditional 1
{ {

58
gtsam_unstable/linear/tests/testConditioning.cpp
View File

@ -65,10 +65,10 @@ TEST( testConditioning, directed_elimination_singlefrontal ) {
Index root_key = 0, removed_key = 1, remaining_parent = 2; Index root_key = 0, removed_key = 1, remaining_parent = 2;
Matrix R11 = Matrix_(1,1, 1.0), R22 = Matrix_(1,1, 3.0), S = Matrix_(1,1,-2.0), T = Matrix_(1,1,-3.0); Matrix R11 = Matrix_(1,1, 1.0), R22 = Matrix_(1,1, 3.0), S = Matrix_(1,1,-2.0), T = Matrix_(1,1,-3.0);
Vector d0 = d.segment(0,1), d1 = d.segment(1,1), sigmas = Vector_(1, 1.0); Vector d0 = d.segment(0,1), d1 = d.segment(1,1), sigmas = Vector_(1, 1.0);
GaussianConditionalOrdered::shared_ptr initConditional(new GaussianConditional::shared_ptr initConditional(new
GaussianConditionalOrdered(root_key, d0, R11, removed_key, S, remaining_parent, T, sigmas)); GaussianConditional(root_key, d0, R11, removed_key, S, remaining_parent, T, sigmas));
VectorValuesOrdered solution; VectorValues solution;
solution.insert(0, x.segment(0,1)); solution.insert(0, x.segment(0,1));
solution.insert(1, x.segment(1,1)); solution.insert(1, x.segment(1,1));
solution.insert(2, x.segment(2,1)); solution.insert(2, x.segment(2,1));
@ -76,22 +76,22 @@ TEST( testConditioning, directed_elimination_singlefrontal ) {
std::set<Index> saved_indices; std::set<Index> saved_indices;
saved_indices += root_key, remaining_parent; saved_indices += root_key, remaining_parent;
GaussianConditionalOrdered::shared_ptr actSummarized = conditionDensity(initConditional, saved_indices, solution); GaussianConditional::shared_ptr actSummarized = conditionDensity(initConditional, saved_indices, solution);
GaussianConditionalOrdered::shared_ptr expSummarized(new GaussianConditional::shared_ptr expSummarized(new
GaussianConditionalOrdered(root_key, d0 - S*x(1), R11, remaining_parent, T, sigmas)); GaussianConditional(root_key, d0 - S*x(1), R11, remaining_parent, T, sigmas));
CHECK(actSummarized); CHECK(actSummarized);
EXPECT(assert_equal(*expSummarized, *actSummarized, tol)); EXPECT(assert_equal(*expSummarized, *actSummarized, tol));
// Simple test of base case: if target index isn't present, return clone // Simple test of base case: if target index isn't present, return clone
GaussianConditionalOrdered::shared_ptr actSummarizedSimple = conditionDensity(expSummarized, saved_indices, solution); GaussianConditional::shared_ptr actSummarizedSimple = conditionDensity(expSummarized, saved_indices, solution);
CHECK(actSummarizedSimple); CHECK(actSummarizedSimple);
EXPECT(assert_equal(*expSummarized, *actSummarizedSimple, tol)); EXPECT(assert_equal(*expSummarized, *actSummarizedSimple, tol));
// case where frontal variable is to be eliminated - return null // case where frontal variable is to be eliminated - return null
GaussianConditionalOrdered::shared_ptr removeFrontalInit(new GaussianConditional::shared_ptr removeFrontalInit(new
GaussianConditionalOrdered(removed_key, d1, R22, remaining_parent, T, sigmas)); GaussianConditional(removed_key, d1, R22, remaining_parent, T, sigmas));
GaussianConditionalOrdered::shared_ptr actRemoveFrontal = conditionDensity(removeFrontalInit, saved_indices, solution); GaussianConditional::shared_ptr actRemoveFrontal = conditionDensity(removeFrontalInit, saved_indices, solution);
EXPECT(!actRemoveFrontal); EXPECT(!actRemoveFrontal);
} }
@ -108,9 +108,9 @@ TEST( testConditioning, directed_elimination_multifrontal ) {
terms += make_pair(root_key, Matrix(R11.col(0))); terms += make_pair(root_key, Matrix(R11.col(0)));
terms += make_pair(removed_key, Matrix(R11.col(1))); terms += make_pair(removed_key, Matrix(R11.col(1)));
terms += make_pair(remaining_parent, S); terms += make_pair(remaining_parent, S);
GaussianConditionalOrdered::shared_ptr initConditional(new GaussianConditionalOrdered(terms, 2, d1, sigmas2)); GaussianConditional::shared_ptr initConditional(new GaussianConditional(terms, 2, d1, sigmas2));
VectorValuesOrdered solution; VectorValues solution;
solution.insert(0, x.segment(0,1)); solution.insert(0, x.segment(0,1));
solution.insert(1, x.segment(1,1)); solution.insert(1, x.segment(1,1));
solution.insert(2, x.segment(2,1)); solution.insert(2, x.segment(2,1));
@ -118,9 +118,9 @@ TEST( testConditioning, directed_elimination_multifrontal ) {
std::set<Index> saved_indices; std::set<Index> saved_indices;
saved_indices += root_key, remaining_parent; saved_indices += root_key, remaining_parent;
GaussianConditionalOrdered::shared_ptr actSummarized = conditionDensity(initConditional, saved_indices, solution); GaussianConditional::shared_ptr actSummarized = conditionDensity(initConditional, saved_indices, solution);
GaussianConditionalOrdered::shared_ptr expSummarized(new GaussianConditional::shared_ptr expSummarized(new
GaussianConditionalOrdered(root_key, d.segment(0,1) - Sprime*x(1), R11prime, remaining_parent, R.block(0,2,1,1), sigmas1)); GaussianConditional(root_key, d.segment(0,1) - Sprime*x(1), R11prime, remaining_parent, R.block(0,2,1,1), sigmas1));
CHECK(actSummarized); CHECK(actSummarized);
EXPECT(assert_equal(*expSummarized, *actSummarized, tol)); EXPECT(assert_equal(*expSummarized, *actSummarized, tol));
@ -140,14 +140,14 @@ TEST( testConditioning, directed_elimination_multifrontal_multidim ) {
0.0, 0.0, 0.0, 0.0, 0.0, 4.0, 0.0, 8.0, 0.0, 6.0, 0.6); 0.0, 0.0, 0.0, 0.0, 0.0, 4.0, 0.0, 8.0, 0.0, 6.0, 0.6);
vector<size_t> init_dims; init_dims += 2, 2, 2, 2, 2, 1; vector<size_t> init_dims; init_dims += 2, 2, 2, 2, 2, 1;
GaussianConditionalOrdered::rsd_type init_matrices(Rinit, init_dims.begin(), init_dims.end()); GaussianConditional::rsd_type init_matrices(Rinit, init_dims.begin(), init_dims.end());
Vector sigmas = ones(6); Vector sigmas = ones(6);
vector<size_t> init_keys; init_keys += 0, 1, 2, 3, 4; vector<size_t> init_keys; init_keys += 0, 1, 2, 3, 4;
GaussianConditionalOrdered::shared_ptr initConditional(new GaussianConditional::shared_ptr initConditional(new
GaussianConditionalOrdered(init_keys.begin(), init_keys.end(), 3, init_matrices, sigmas)); GaussianConditional(init_keys.begin(), init_keys.end(), 3, init_matrices, sigmas));
// Construct a solution vector // Construct a solution vector
VectorValuesOrdered solution; VectorValues solution;
solution.insert(0, zero(2)); solution.insert(0, zero(2));
solution.insert(1, zero(2)); solution.insert(1, zero(2));
solution.insert(2, zero(2)); solution.insert(2, zero(2));
@ -159,7 +159,7 @@ TEST( testConditioning, directed_elimination_multifrontal_multidim ) {
std::set<Index> saved_indices; std::set<Index> saved_indices;
saved_indices += 0, 2, 4; saved_indices += 0, 2, 4;
GaussianConditionalOrdered::shared_ptr actSummarized = conditionDensity(initConditional, saved_indices, solution); GaussianConditional::shared_ptr actSummarized = conditionDensity(initConditional, saved_indices, solution);
CHECK(actSummarized); CHECK(actSummarized);
Matrix Rexp = Matrix_(4, 7, Matrix Rexp = Matrix_(4, 7,
@ -173,10 +173,10 @@ TEST( testConditioning, directed_elimination_multifrontal_multidim ) {
Rexp.block(2, 6, 2, 1) -= Rinit.block(4, 6, 2, 2) * solution.at(3); Rexp.block(2, 6, 2, 1) -= Rinit.block(4, 6, 2, 2) * solution.at(3);
vector<size_t> exp_dims; exp_dims += 2, 2, 2, 1; vector<size_t> exp_dims; exp_dims += 2, 2, 2, 1;
GaussianConditionalOrdered::rsd_type exp_matrices(Rexp, exp_dims.begin(), exp_dims.end()); GaussianConditional::rsd_type exp_matrices(Rexp, exp_dims.begin(), exp_dims.end());
Vector exp_sigmas = ones(4); Vector exp_sigmas = ones(4);
vector<size_t> exp_keys; exp_keys += 0, 2, 4; vector<size_t> exp_keys; exp_keys += 0, 2, 4;
GaussianConditionalOrdered expSummarized(exp_keys.begin(), exp_keys.end(), 2, exp_matrices, exp_sigmas); GaussianConditional expSummarized(exp_keys.begin(), exp_keys.end(), 2, exp_matrices, exp_sigmas);
EXPECT(assert_equal(expSummarized, *actSummarized, tol)); EXPECT(assert_equal(expSummarized, *actSummarized, tol));
} }
@ -205,13 +205,13 @@ TEST( testConditioning, directed_elimination_multifrontal_multidim2 ) {
Rinit.rightCols(1) = dinit; Rinit.rightCols(1) = dinit;
Vector sigmas = ones(10); Vector sigmas = ones(10);
GaussianConditionalOrdered::rsd_type init_matrices(Rinit, init_dims.begin(), init_dims.end()); GaussianConditional::rsd_type init_matrices(Rinit, init_dims.begin(), init_dims.end());
vector<size_t> init_keys; init_keys += 3, 4, 5, 6; vector<size_t> init_keys; init_keys += 3, 4, 5, 6;
GaussianConditionalOrdered::shared_ptr initConditional(new GaussianConditional::shared_ptr initConditional(new
GaussianConditionalOrdered(init_keys.begin(), init_keys.end(), 4, init_matrices, sigmas)); GaussianConditional(init_keys.begin(), init_keys.end(), 4, init_matrices, sigmas));
// Calculate a solution // Calculate a solution
VectorValuesOrdered solution; VectorValues solution;
solution.insert(0, zero(3)); solution.insert(0, zero(3));
solution.insert(1, zero(3)); solution.insert(1, zero(3));
solution.insert(2, zero(3)); solution.insert(2, zero(3));
@ -226,7 +226,7 @@ TEST( testConditioning, directed_elimination_multifrontal_multidim2 ) {
std::set<Index> saved_indices; std::set<Index> saved_indices;
saved_indices += 5, 6; saved_indices += 5, 6;
GaussianConditionalOrdered::shared_ptr actSummarized = conditionDensity(initConditional, saved_indices, solution); GaussianConditional::shared_ptr actSummarized = conditionDensity(initConditional, saved_indices, solution);
CHECK(actSummarized); CHECK(actSummarized);
// Create expected value on [5], [6] // Create expected value on [5], [6]
@ -238,9 +238,9 @@ TEST( testConditioning, directed_elimination_multifrontal_multidim2 ) {
Vector expsigmas = ones(4); Vector expsigmas = ones(4);
vector<size_t> exp_dims; exp_dims += 2, 2, 1; vector<size_t> exp_dims; exp_dims += 2, 2, 1;
GaussianConditionalOrdered::rsd_type exp_matrices(Rexp, exp_dims.begin(), exp_dims.end()); GaussianConditional::rsd_type exp_matrices(Rexp, exp_dims.begin(), exp_dims.end());
vector<size_t> exp_keys; exp_keys += 5, 6; vector<size_t> exp_keys; exp_keys += 5, 6;
GaussianConditionalOrdered expConditional(exp_keys.begin(), exp_keys.end(), 2, exp_matrices, expsigmas); GaussianConditional expConditional(exp_keys.begin(), exp_keys.end(), 2, exp_matrices, expsigmas);
EXPECT(assert_equal(expConditional, *actSummarized, tol)); EXPECT(assert_equal(expConditional, *actSummarized, tol));
} }

14
gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp
View File

@ -20,10 +20,10 @@
#include <gtsam_unstable/nonlinear/BatchFixedLagSmoother.h> #include <gtsam_unstable/nonlinear/BatchFixedLagSmoother.h>
#include <gtsam/base/debug.h> #include <gtsam/base/debug.h>
#include <gtsam/inference/Key.h> #include <gtsam/inference/Key.h>
#include <gtsam/inference/Ordering.h>
#include <gtsam/geometry/Point2.h> #include <gtsam/geometry/Point2.h>
#include <gtsam/linear/GaussianBayesNetOrdered.h> #include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianSequentialSolver.h> #include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/nonlinear/OrderingOrdered.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h> #include <gtsam/nonlinear/Values.h>
#include <gtsam/slam/PriorFactor.h> #include <gtsam/slam/PriorFactor.h>
@ -35,11 +35,9 @@ using namespace gtsam;
/* ************************************************************************* */ /* ************************************************************************* */
bool check_smoother(const NonlinearFactorGraph& fullgraph, const Values& fullinit, const BatchFixedLagSmoother& smoother, const Key& key) { bool check_smoother(const NonlinearFactorGraph& fullgraph, const Values& fullinit, const BatchFixedLagSmoother& smoother, const Key& key) {
OrderingOrdered ordering = *fullgraph.orderingCOLAMD(fullinit); GaussianFactorGraph linearized = *fullgraph.linearize(fullinit);
GaussianFactorGraphOrdered linearized = *fullgraph.linearize(fullinit, ordering); VectorValues delta = linearized.optimize();
GaussianBayesNetOrdered gbn = *GaussianSequentialSolver(linearized).eliminate(); Values fullfinal = fullinit.retract(delta);
VectorValuesOrdered delta = optimize(gbn);
Values fullfinal = fullinit.retract(delta, ordering);
Point2 expected = fullfinal.at<Point2>(key); Point2 expected = fullfinal.at<Point2>(key);
Point2 actual = smoother.calculateEstimate<Point2>(key); Point2 actual = smoother.calculateEstimate<Point2>(key);

24
gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp
View File

@ -23,12 +23,12 @@
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h> #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LinearContainerFactor.h> #include <gtsam/nonlinear/LinearContainerFactor.h>
#include <gtsam/nonlinear/OrderingOrdered.h>
#include <gtsam/nonlinear/Values.h> #include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/Symbol.h> #include <gtsam/nonlinear/Symbol.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/inference/Ordering.h>
#include <gtsam/inference/Key.h> #include <gtsam/inference/Key.h>
#include <gtsam/linear/GaussianSequentialSolver.h> #include <gtsam/inference/JunctionTree.h>
#include <gtsam/inference/JunctionTreeOrdered.h>
#include <gtsam/geometry/Pose3.h> #include <gtsam/geometry/Pose3.h>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
@ -86,22 +86,16 @@ bool hessian_equal(const NonlinearFactorGraph& expected, const NonlinearFactorGr
return false; return false;
} }
// Create an ordering
OrderingOrdered ordering;
BOOST_FOREACH(Key key, expectedKeys) {
ordering.push_back(key);
}
// Linearize each factor graph // Linearize each factor graph
GaussianFactorGraphOrdered expectedGaussian; GaussianFactorGraph expectedGaussian;
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, expected) { BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, expected) {
if(factor) if(factor)
expectedGaussian.push_back( factor->linearize(theta, ordering) ); expectedGaussian.push_back( factor->linearize(theta) );
} }
GaussianFactorGraphOrdered actualGaussian; GaussianFactorGraph actualGaussian;
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, actual) { BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, actual) {
if(factor) if(factor)
actualGaussian.push_back( factor->linearize(theta, ordering) ); actualGaussian.push_back( factor->linearize(theta) );
} }
// Convert linear factor graph into a dense Hessian // Convert linear factor graph into a dense Hessian
@ -384,7 +378,7 @@ NonlinearFactorGraph MarginalFactors(const NonlinearFactorGraph& factors, const
BOOST_FOREACH(Key key, remainingKeys) { BOOST_FOREACH(Key key, remainingKeys) {
constraints[key] = 1; constraints[key] = 1;
} }
OrderingOrdered ordering = *factors.orderingCOLAMDConstrained(values, constraints); Ordering ordering = *factors.orderingCOLAMDConstrained(values, constraints);
// Convert the remaining keys into indices // Convert the remaining keys into indices
std::vector<Index> remainingIndices; std::vector<Index> remainingIndices;
@ -394,7 +388,7 @@ NonlinearFactorGraph MarginalFactors(const NonlinearFactorGraph& factors, const
// Solve for the Gaussian marginal factors // Solve for the Gaussian marginal factors
GaussianSequentialSolver gss(*factors.linearize(values, ordering), true); GaussianSequentialSolver gss(*factors.linearize(values, ordering), true);
GaussianFactorGraphOrdered linearMarginals = *gss.jointFactorGraph(remainingIndices); GaussianFactorGraph linearMarginals = *gss.jointFactorGraph(remainingIndices);
// Convert to LinearContainFactors // Convert to LinearContainFactors
return LinearContainerFactor::convertLinearGraph(linearMarginals, ordering, values); return LinearContainerFactor::convertLinearGraph(linearMarginals, ordering, values);

26
gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp
View File

@ -23,11 +23,11 @@
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h> #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LinearContainerFactor.h> #include <gtsam/nonlinear/LinearContainerFactor.h>
#include <gtsam/nonlinear/OrderingOrdered.h>
#include <gtsam/nonlinear/Values.h> #include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/Symbol.h> #include <gtsam/nonlinear/Symbol.h>
#include <gtsam/inference/Ordering.h>
#include <gtsam/inference/Key.h> #include <gtsam/inference/Key.h>
#include <gtsam/inference/JunctionTreeOrdered.h> #include <gtsam/inference/JunctionTree.h>
#include <gtsam/geometry/Pose3.h> #include <gtsam/geometry/Pose3.h>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
@ -78,18 +78,18 @@ bool hessian_equal(const NonlinearFactorGraph& expected, const NonlinearFactorGr
return false; return false;
// Create an ordering // Create an ordering
OrderingOrdered ordering; Ordering ordering;
BOOST_FOREACH(Key key, expectedKeys) { BOOST_FOREACH(Key key, expectedKeys) {
ordering.push_back(key); ordering.push_back(key);
} }
// Linearize each factor graph // Linearize each factor graph
GaussianFactorGraphOrdered expectedGaussian; GaussianFactorGraph expectedGaussian;
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, expected) { BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, expected) {
if(factor) if(factor)
expectedGaussian.push_back( factor->linearize(theta, ordering) ); expectedGaussian.push_back( factor->linearize(theta, ordering) );
} }
GaussianFactorGraphOrdered actualGaussian; GaussianFactorGraph actualGaussian;
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, actual) { BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, actual) {
if(factor) if(factor)
actualGaussian.push_back( factor->linearize(theta, ordering) ); actualGaussian.push_back( factor->linearize(theta, ordering) );
@ -511,7 +511,7 @@ TEST_UNSAFE( ConcurrentBatchSmoother, synchronize )
Values optimalTheta = BatchOptimize(fullGraph, fullTheta); Values optimalTheta = BatchOptimize(fullGraph, fullTheta);
// Re-eliminate to create the Bayes Tree // Re-eliminate to create the Bayes Tree
OrderingOrdered ordering; Ordering ordering;
ordering.push_back(Symbol('X', 2)); ordering.push_back(Symbol('X', 2));
ordering.push_back(Symbol('X', 0)); ordering.push_back(Symbol('X', 0));
ordering.push_back(Symbol('X', 1)); ordering.push_back(Symbol('X', 1));
@ -527,10 +527,10 @@ TEST_UNSAFE( ConcurrentBatchSmoother, synchronize )
ordering.push_back(Symbol('X', 12)); ordering.push_back(Symbol('X', 12));
Values linpoint; Values linpoint;
linpoint.insert(optimalTheta); linpoint.insert(optimalTheta);
GaussianFactorGraphOrdered linearGraph = *fullGraph.linearize(linpoint, ordering); GaussianFactorGraph linearGraph = *fullGraph.linearize(linpoint, ordering);
JunctionTreeOrdered<GaussianFactorGraphOrdered, ISAM2Clique> jt(linearGraph); JunctionTree<GaussianFactorGraph, ISAM2Clique> jt(linearGraph);
ISAM2Clique::shared_ptr root = jt.eliminate(EliminateQROrdered); ISAM2Clique::shared_ptr root = jt.eliminate(EliminateQR);
BayesTreeOrdered<GaussianConditionalOrdered, ISAM2Clique> bayesTree; BayesTree<GaussianConditional, ISAM2Clique> bayesTree;
bayesTree.insert(root); bayesTree.insert(root);
// Extract the values for the smoother keys. This consists of the branches: X4 and X6 // Extract the values for the smoother keys. This consists of the branches: X4 and X6
@ -637,9 +637,9 @@ TEST_UNSAFE( ConcurrentBatchSmoother, synchronize )
linpoint.insert(optimalTheta); linpoint.insert(optimalTheta);
linpoint.update(rootValues); linpoint.update(rootValues);
linearGraph = *fullGraph.linearize(linpoint, ordering); linearGraph = *fullGraph.linearize(linpoint, ordering);
jt = JunctionTreeOrdered<GaussianFactorGraphOrdered, ISAM2Clique>(linearGraph); jt = JunctionTree<GaussianFactorGraph, ISAM2Clique>(linearGraph);
root = jt.eliminate(EliminateQROrdered); root = jt.eliminate(EliminateQR);
bayesTree = BayesTreeOrdered<GaussianConditionalOrdered, ISAM2Clique>(); bayesTree = BayesTree<GaussianConditional, ISAM2Clique>();
bayesTree.insert(root); bayesTree.insert(root);
// Add the loop closure to the smoother // Add the loop closure to the smoother

14
gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp
View File

@ -22,11 +22,11 @@
#include <gtsam/base/debug.h> #include <gtsam/base/debug.h>
#include <gtsam/geometry/Point2.h> #include <gtsam/geometry/Point2.h>
#include <gtsam/inference/Key.h> #include <gtsam/inference/Key.h>
#include <gtsam/linear/GaussianBayesNetOrdered.h> #include <gtsam/inference/Ordering.h>
#include <gtsam/linear/GaussianSequentialSolver.h> #include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/slam/PriorFactor.h> #include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h> #include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/OrderingOrdered.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h> #include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/Symbol.h> #include <gtsam/nonlinear/Symbol.h>
@ -39,11 +39,9 @@ Key MakeKey(size_t index) { return Symbol('x', index); }
/* ************************************************************************* */ /* ************************************************************************* */
bool check_smoother(const NonlinearFactorGraph& fullgraph, const Values& fullinit, const IncrementalFixedLagSmoother& smoother, const Key& key) { bool check_smoother(const NonlinearFactorGraph& fullgraph, const Values& fullinit, const IncrementalFixedLagSmoother& smoother, const Key& key) {
OrderingOrdered ordering = *fullgraph.orderingCOLAMD(fullinit); GaussianFactorGraph linearized = *fullgraph.linearize(fullinit);
GaussianFactorGraphOrdered linearized = *fullgraph.linearize(fullinit, ordering); VectorValues delta = linearized.optimize();
GaussianBayesNetOrdered gbn = *GaussianSequentialSolver(linearized).eliminate(); Values fullfinal = fullinit.retract(delta);
VectorValuesOrdered delta = optimize(gbn);
Values fullfinal = fullinit.retract(delta, ordering);
Point2 expected = fullfinal.at<Point2>(key); Point2 expected = fullfinal.at<Point2>(key);
Point2 actual = smoother.calculateEstimate<Point2>(key); Point2 actual = smoother.calculateEstimate<Point2>(key);