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

Merged from branch 'branches/unordered'

release/4.3a0
Richard Roberts 2013-08-09 21:35:30 +00:00
parent a98180f84f 5b7d7b3793
commit 4ed047f8a6
42 changed files with 480 additions and 620 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

104
.cproject
View File

@ -613,6 +613,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testWhiteNoiseFactor.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j6 -j8</buildArguments>
<buildTarget>testWhiteNoiseFactor.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>
@ -621,54 +629,6 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testPlanarSLAM.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testPlanarSLAM.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testPose2SLAM.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testPose2SLAM.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testPose3SLAM.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testPose3SLAM.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testSimulated2D.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testSimulated2D.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testSimulated2DOriented.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testSimulated2DOriented.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testVisualSLAM.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testVisualSLAM.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testProjectionFactor.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
@ -677,14 +637,6 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testSerializationSLAM.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testSerializationSLAM.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGeneralSFMFactor_Cal3Bundler.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
@ -693,6 +645,22 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testAntiFactor.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j6 -j8</buildArguments>
<buildTarget>testAntiFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testBetweenFactor.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j6 -j8</buildArguments>
<buildTarget>testBetweenFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -2468,6 +2436,30 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="gtsam_unstable-shared" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j6 -j8</buildArguments>
<buildTarget>gtsam_unstable-shared</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="gtsam_unstable-static" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j6 -j8</buildArguments>
<buildTarget>gtsam_unstable-static</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check.nonlinear_unstable" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j6 -j8</buildArguments>
<buildTarget>check.nonlinear_unstable</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testSpirit.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>

3
gtsam/navigation/tests/testInertialNavFactor_GlobalVelocity.cpp
View File

@ -24,6 +24,7 @@
#include <gtsam/inference/Key.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/base/TestableAssertions.h>
using namespace std;
using namespace gtsam;
@ -737,7 +738,7 @@ TEST( InertialNavFactor_GlobalVelocity, LinearizeTiming)
GaussianFactorGraph graph;
gttic_(LinearizeTiming);
for(size_t i = 0; i < 100000; ++i) {
GaussianFactor::shared_ptr g = f.linearize(values, ordering);
GaussianFactor::shared_ptr g = f.linearize(values);
graph.push_back(g);
}
gttoc_(LinearizeTiming);

4
gtsam/nonlinear/CMakeLists.txt
View File

@ -15,9 +15,7 @@ set(nonlinear_local_libs
# Files to exclude from compilation of tests and timing scripts
set(nonlinear_excluded_files
# "${CMAKE_CURRENT_SOURCE_DIR}/tests/testTypedDiscreteFactor.cpp" # Example of excluding a test
#"" # Add to this list, with full path, to exclude
"${CMAKE_CURRENT_SOURCE_DIR}/tests/testLinearContainerFactor.cpp"
"${CMAKE_CURRENT_SOURCE_DIR}/tests/testWhiteNoiseFactor.cpp"
"" # Add to this list, with full path, to exclude
)
# Build tests

22
gtsam/nonlinear/LinearContainerFactor.cpp
View File

@ -9,11 +9,10 @@
#include <gtsam/linear/HessianFactor.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <boost/foreach.hpp>
#if 0
namespace gtsam {
/* ************************************************************************* */
@ -31,30 +30,27 @@ void LinearContainerFactor::initializeLinearizationPoint(const Values& lineariza
/* ************************************************************************* */
LinearContainerFactor::LinearContainerFactor(const GaussianFactor::shared_ptr& factor,
const boost::optional<Values>& linearizationPoint)
: factor_(factor), linearizationPoint_(linearizationPoint) {
// Extract keys stashed in linear factor
BOOST_FOREACH(const Index& idx, factor_->keys())
keys_.push_back(idx);
: NonlinearFactor(factor->keys()), factor_(factor), linearizationPoint_(linearizationPoint) {
}
/* ************************************************************************* */
LinearContainerFactor::LinearContainerFactor(
const JacobianFactor& factor, const Values& linearizationPoint)
: factor_(factor.clone()) {
: NonlinearFactor(factor.keys()), factor_(factor.clone()) {
initializeLinearizationPoint(linearizationPoint);
}
/* ************************************************************************* */
LinearContainerFactor::LinearContainerFactor(
const HessianFactor& factor, const Values& linearizationPoint)
: factor_(factor.clone()) {
: NonlinearFactor(factor.keys()), factor_(factor.clone()) {
initializeLinearizationPoint(linearizationPoint);
}
/* ************************************************************************* */
LinearContainerFactor::LinearContainerFactor(
const GaussianFactor::shared_ptr& factor, const Values& linearizationPoint)
: factor_(factor->clone()) {
: NonlinearFactor(factor->keys()), factor_(factor->clone()) {
initializeLinearizationPoint(linearizationPoint);
}
@ -139,12 +135,12 @@ GaussianFactor::shared_ptr LinearContainerFactor::linearize(const Values& c) con
/* ************************************************************************* */
bool LinearContainerFactor::isJacobian() const {
return boost::dynamic_pointer_cast<JacobianFactor>(factor_);
return boost::dynamic_pointer_cast<JacobianFactor>(factor_).get();
}
/* ************************************************************************* */
bool LinearContainerFactor::isHessian() const {
return boost::dynamic_pointer_cast<HessianFactor>(factor_);
return boost::dynamic_pointer_cast<HessianFactor>(factor_).get();
}
/* ************************************************************************* */
@ -160,12 +156,13 @@ HessianFactor::shared_ptr LinearContainerFactor::toHessian() const {
/* ************************************************************************* */
GaussianFactor::shared_ptr LinearContainerFactor::negateToGaussian() const {
GaussianFactor::shared_ptr result = factor_->negate();
return result;
}
/* ************************************************************************* */
NonlinearFactor::shared_ptr LinearContainerFactor::negateToNonlinear() const {
GaussianFactor::shared_ptr antifactor = factor_->negate(); // already has keys in place
return boost::make_shared<LinearContainerFactor>(antifactor, linearizationPoint_);
return NonlinearFactor::shared_ptr(new LinearContainerFactor(antifactor, linearizationPoint_));
}
/* ************************************************************************* */
@ -183,4 +180,3 @@ NonlinearFactorGraph LinearContainerFactor::convertLinearGraph(
/* ************************************************************************* */
} // \namespace gtsam
#endif

4
gtsam/nonlinear/LinearContainerFactor.h
View File

@ -38,10 +38,10 @@ protected:
public:
/** Primary constructor: store a linear factor and decode the ordering */
/** Primary constructor: store a linear factor with optional linearization point */
LinearContainerFactor(const JacobianFactor& factor, const Values& linearizationPoint = Values());
/** Primary constructor: store a linear factor and decode the ordering */
/** Primary constructor: store a linear factor with optional linearization point */
LinearContainerFactor(const HessianFactor& factor, const Values& linearizationPoint = Values());
/** Constructor from shared_ptr */

15
gtsam/nonlinear/WhiteNoiseFactor.h
View File

@ -144,22 +144,21 @@ namespace gtsam {
* Create a symbolic factor using the given ordering to determine the
* variable indices.
*/
virtual IndexFactor::shared_ptr symbolic(const Ordering& ordering) const {
const Index j1 = ordering[meanKey_], j2 = ordering[precisionKey_];
return IndexFactor::shared_ptr(new IndexFactor(j1, j2));
}
// virtual IndexFactor::shared_ptr symbolic(const Ordering& ordering) const {
// const Index j1 = ordering[meanKey_], j2 = ordering[precisionKey_];
// return IndexFactor::shared_ptr(new IndexFactor(j1, j2));
// }
/// @}
/// @name Advanced Interface
/// @{
/// linearize returns a Hessianfactor that is an approximation of error(p)
virtual boost::shared_ptr<GaussianFactor> linearize(const Values& x,
const Ordering& ordering) const {
virtual boost::shared_ptr<GaussianFactor> linearize(const Values& x) const {
double u = x.at<LieScalar>(meanKey_);
double p = x.at<LieScalar>(precisionKey_);
Index j1 = ordering[meanKey_];
Index j2 = ordering[precisionKey_];
Index j1 = meanKey_;
Index j2 = precisionKey_;
return linearize(z_, u, p, j1, j2);
}

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

@ -8,6 +8,8 @@
#include <CppUnitLite/TestHarness.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/LinearContainerFactor.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/linear/HessianFactor.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/base/TestableAssertions.h>
@ -28,8 +30,6 @@ Pose2 poseA1(0.0, 0.0, 0.0), poseA2(2.0, 0.0, 0.0);
/* ************************************************************************* */
TEST( testLinearContainerFactor, generic_jacobian_factor ) {
Ordering initOrdering; initOrdering += x1, x2, l1, l2;
Matrix A1 = Matrix_(2,2,
2.74222, -0.0067457,
0.0, 2.63624);
@ -39,9 +39,9 @@ TEST( testLinearContainerFactor, generic_jacobian_factor ) {
Vector b = Vector_(2, 0.0277052,
-0.0533393);
JacobianFactor expLinFactor(initOrdering[l1], A1, initOrdering[l2], A2, b, diag_model2);
JacobianFactor expLinFactor(l1, A1, l2, A2, b, diag_model2);
LinearContainerFactor actFactor(expLinFactor, initOrdering);
LinearContainerFactor actFactor(expLinFactor);
EXPECT_LONGS_EQUAL(2, actFactor.size());
EXPECT(actFactor.isJacobian());
EXPECT(!actFactor.isHessian());
@ -56,22 +56,14 @@ TEST( testLinearContainerFactor, generic_jacobian_factor ) {
values.insert(x1, poseA1);
values.insert(x2, poseA2);
// Check reconstruction from same ordering
GaussianFactor::shared_ptr actLinearizationA = actFactor.linearize(values, initOrdering);
// Check reconstruction
GaussianFactor::shared_ptr actLinearizationA = actFactor.linearize(values);
EXPECT(assert_equal(*expLinFactor.clone(), *actLinearizationA, tol));
// Check reconstruction from new ordering
Ordering newOrdering; newOrdering += x1, l1, x2, l2;
GaussianFactor::shared_ptr actLinearizationB = actFactor.linearize(values, newOrdering);
JacobianFactor expLinFactor2(newOrdering[l1], A1, newOrdering[l2], A2, b, diag_model2);
EXPECT(assert_equal(*expLinFactor2.clone(), *actLinearizationB, tol));
}
/* ************************************************************************* */
TEST( testLinearContainerFactor, jacobian_factor_withlinpoints ) {
Ordering ordering; ordering += x1, x2, l1, l2;
Matrix A1 = Matrix_(2,2,
2.74222, -0.0067457,
0.0, 2.63624);
@ -81,7 +73,7 @@ TEST( testLinearContainerFactor, jacobian_factor_withlinpoints ) {
Vector b = Vector_(2, 0.0277052,
-0.0533393);
JacobianFactor expLinFactor(ordering[l1], A1, ordering[l2], A2, b, diag_model2);
JacobianFactor expLinFactor(l1, A1, l2, A2, b, diag_model2);
Values values;
values.insert(l1, landmark1);
@ -89,8 +81,8 @@ TEST( testLinearContainerFactor, jacobian_factor_withlinpoints ) {
values.insert(x1, poseA1);
values.insert(x2, poseA2);
LinearContainerFactor actFactor(expLinFactor, ordering, values);
LinearContainerFactor actFactorNolin(expLinFactor, ordering);
LinearContainerFactor actFactor(expLinFactor, values);
LinearContainerFactor actFactorNolin(expLinFactor);
EXPECT(assert_equal(actFactor, actFactor, tol));
EXPECT(assert_inequal(actFactor, actFactorNolin, tol));
@ -108,19 +100,18 @@ TEST( testLinearContainerFactor, jacobian_factor_withlinpoints ) {
Vector delta_l1 = Vector_(2, 1.0, 2.0);
Vector delta_l2 = Vector_(2, 3.0, 4.0);
VectorValues delta = values.zeroVectors(ordering);
delta.at(ordering[l1]) = delta_l1;
delta.at(ordering[l2]) = delta_l2;
Values noisyValues = values.retract(delta, ordering);
VectorValues delta = values.zeroVectors();
delta.at(l1) = delta_l1;
delta.at(l2) = delta_l2;
Values noisyValues = values.retract(delta);
double expError = expLinFactor.error(delta);
EXPECT_DOUBLES_EQUAL(expError, actFactor.error(noisyValues), tol);
EXPECT_DOUBLES_EQUAL(expLinFactor.error(values.zeroVectors(ordering)), actFactor.error(values), tol);
EXPECT_DOUBLES_EQUAL(expLinFactor.error(values.zeroVectors()), actFactor.error(values), tol);
// Check linearization with corrections for updated linearization point
Ordering newOrdering; newOrdering += x1, l1, x2, l2;
GaussianFactor::shared_ptr actLinearizationB = actFactor.linearize(noisyValues, newOrdering);
GaussianFactor::shared_ptr actLinearizationB = actFactor.linearize(noisyValues);
Vector bprime = b - A1 * delta_l1 - A2 * delta_l2;
JacobianFactor expLinFactor2(newOrdering[l1], A1, newOrdering[l2], A2, bprime, diag_model2);
JacobianFactor expLinFactor2(l1, A1, l2, A2, bprime, diag_model2);
EXPECT(assert_equal(*expLinFactor2.clone(), *actLinearizationB, tol));
}
@ -145,8 +136,7 @@ TEST( testLinearContainerFactor, generic_hessian_factor ) {
double f = 10.0;
Ordering initOrdering; initOrdering += x1, x2, l1, l2;
HessianFactor initFactor(initOrdering[x1], initOrdering[x2], initOrdering[l1],
HessianFactor initFactor(x1, x2, l1,
G11, G12, G13, g1, G22, G23, g2, G33, g3, f);
Values values;
@ -155,18 +145,12 @@ TEST( testLinearContainerFactor, generic_hessian_factor ) {
values.insert(x1, poseA1);
values.insert(x2, poseA2);
LinearContainerFactor actFactor(initFactor, initOrdering);
LinearContainerFactor actFactor(initFactor);
EXPECT(!actFactor.isJacobian());
EXPECT(actFactor.isHessian());
GaussianFactor::shared_ptr actLinearization1 = actFactor.linearize(values, initOrdering);
GaussianFactor::shared_ptr actLinearization1 = actFactor.linearize(values);
EXPECT(assert_equal(*initFactor.clone(), *actLinearization1, tol));
Ordering newOrdering; newOrdering += l1, x1, x2, l2;
HessianFactor expLinFactor(newOrdering[x1], newOrdering[x2], newOrdering[l1],
G11, G12, G13, g1, G22, G23, g2, G33, g3, f);
GaussianFactor::shared_ptr actLinearization2 = actFactor.linearize(values, newOrdering);
EXPECT(assert_equal(*expLinFactor.clone(), *actLinearization2, tol));
}
/* ************************************************************************* */
@ -196,8 +180,7 @@ TEST( testLinearContainerFactor, hessian_factor_withlinpoints ) {
Matrix G(5,5);
G << G11, G12, Matrix::Zero(2,3), G22;
Ordering ordering; ordering += x1, x2, l1;
HessianFactor initFactor(ordering[x1], ordering[l1], G11, G12, g1, G22, g2, f);
HessianFactor initFactor(x1, l1, G11, G12, g1, G22, g2, f);
Values linearizationPoint, expLinPoints;
linearizationPoint.insert(l1, landmark1);
@ -205,7 +188,7 @@ TEST( testLinearContainerFactor, hessian_factor_withlinpoints ) {
expLinPoints = linearizationPoint;
linearizationPoint.insert(x2, poseA2);
LinearContainerFactor actFactor(initFactor, ordering, linearizationPoint);
LinearContainerFactor actFactor(initFactor, linearizationPoint);
EXPECT(!actFactor.isJacobian());
EXPECT(actFactor.isHessian());
@ -219,16 +202,16 @@ TEST( testLinearContainerFactor, hessian_factor_withlinpoints ) {
Vector delta_x2 = Vector_(3, 6.0, 7.0, 0.3);
// Check error calculation
VectorValues delta = linearizationPoint.zeroVectors(ordering);
delta.at(ordering[l1]) = delta_l1;
delta.at(ordering[x1]) = delta_x1;
delta.at(ordering[x2]) = delta_x2;
VectorValues delta = linearizationPoint.zeroVectors();
delta.at(l1) = delta_l1;
delta.at(x1) = delta_x1;
delta.at(x2) = delta_x2;
EXPECT(assert_equal(Vector_(5, 3.0, 4.0, 0.5, 1.0, 2.0), delta.vector(initFactor.keys())));
Values noisyValues = linearizationPoint.retract(delta, ordering);
Values noisyValues = linearizationPoint.retract(delta);
double expError = initFactor.error(delta);
EXPECT_DOUBLES_EQUAL(expError, actFactor.error(noisyValues), tol);
EXPECT_DOUBLES_EQUAL(initFactor.error(linearizationPoint.zeroVectors(ordering)), actFactor.error(linearizationPoint), tol);
EXPECT_DOUBLES_EQUAL(initFactor.error(linearizationPoint.zeroVectors()), actFactor.error(linearizationPoint), tol);
// Compute updated versions
Vector dv = Vector_(5, 3.0, 4.0, 0.5, 1.0, 2.0);
@ -239,8 +222,8 @@ TEST( testLinearContainerFactor, hessian_factor_withlinpoints ) {
Vector g1_prime = g_prime.head(3);
Vector g2_prime = g_prime.tail(2);
double f_prime = f + dv.transpose() * G.selfadjointView<Eigen::Upper>() * dv - 2.0 * dv.transpose() * g;
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));
HessianFactor expNewFactor(x1, l1, G11, G12, g1_prime, G22, g2_prime, f_prime);
EXPECT(assert_equal(*expNewFactor.clone(), *actFactor.linearize(noisyValues), tol));
}
/* ************************************************************************* */
@ -251,14 +234,10 @@ TEST( testLinearContainerFactor, creation ) {
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 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));
l3, eye(2,2), 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;
@ -267,7 +246,7 @@ TEST( testLinearContainerFactor, creation ) {
exp_values = full_values;
full_values.insert(l1, Point2(3.0, 7.0));
LinearContainerFactor actual(linear_factor, ordering, full_values);
LinearContainerFactor actual(linear_factor, full_values);
// Verify the keys
std::vector<gtsam::Key> expKeys;
@ -284,9 +263,6 @@ TEST( testLinearContainerFactor, jacobian_relinearize )
// Create a Between Factor from a Point3. This is actually a linear factor.
gtsam::Key key1(1);
gtsam::Key key2(2);
gtsam::Ordering ordering;
ordering.push_back(key1);
ordering.push_back(key2);
gtsam::Values linpoint1;
linpoint1.insert(key1, gtsam::Point3(-22.4, +8.5, +2.4));
linpoint1.insert(key2, gtsam::Point3(-21.0, +5.0, +21.0));
@ -296,8 +272,8 @@ TEST( testLinearContainerFactor, jacobian_relinearize )
gtsam::BetweenFactor<gtsam::Point3> betweenFactor(key1, key2, measured, model);
// Create a jacobian container factor at linpoint 1
gtsam::JacobianFactor::shared_ptr jacobian(new gtsam::JacobianFactor(*betweenFactor.linearize(linpoint1, ordering)));
gtsam::LinearContainerFactor jacobianContainer(jacobian, ordering, linpoint1);
gtsam::JacobianFactor::shared_ptr jacobian(new gtsam::JacobianFactor(*betweenFactor.linearize(linpoint1)));
gtsam::LinearContainerFactor jacobianContainer(jacobian, linpoint1);
// Create a second linearization point
gtsam::Values linpoint2;
@ -310,8 +286,8 @@ TEST( testLinearContainerFactor, jacobian_relinearize )
EXPECT_DOUBLES_EQUAL(expected_error, actual_error, 1e-9 );
// Re-linearize around the new point and check the factors
gtsam::GaussianFactor::shared_ptr expected_factor = betweenFactor.linearize(linpoint2, ordering);
gtsam::GaussianFactor::shared_ptr actual_factor = jacobianContainer.linearize(linpoint2, ordering);
gtsam::GaussianFactor::shared_ptr expected_factor = betweenFactor.linearize(linpoint2);
gtsam::GaussianFactor::shared_ptr actual_factor = jacobianContainer.linearize(linpoint2);
CHECK(gtsam::assert_equal(*expected_factor, *actual_factor));
}
@ -321,9 +297,6 @@ TEST( testLinearContainerFactor, hessian_relinearize )
// Create a Between Factor from a Point3. This is actually a linear factor.
gtsam::Key key1(1);
gtsam::Key key2(2);
gtsam::Ordering ordering;
ordering.push_back(key1);
ordering.push_back(key2);
gtsam::Values linpoint1;
linpoint1.insert(key1, gtsam::Point3(-22.4, +8.5, +2.4));
linpoint1.insert(key2, gtsam::Point3(-21.0, +5.0, +21.0));
@ -333,8 +306,8 @@ TEST( testLinearContainerFactor, hessian_relinearize )
gtsam::BetweenFactor<gtsam::Point3> betweenFactor(key1, key2, measured, model);
// Create a hessian container factor at linpoint 1
gtsam::HessianFactor::shared_ptr hessian(new gtsam::HessianFactor(*betweenFactor.linearize(linpoint1, ordering)));
gtsam::LinearContainerFactor hessianContainer(hessian, ordering, linpoint1);
gtsam::HessianFactor::shared_ptr hessian(new gtsam::HessianFactor(*betweenFactor.linearize(linpoint1)));
gtsam::LinearContainerFactor hessianContainer(hessian, linpoint1);
// Create a second linearization point
gtsam::Values linpoint2;
@ -347,8 +320,8 @@ TEST( testLinearContainerFactor, hessian_relinearize )
EXPECT_DOUBLES_EQUAL(expected_error, actual_error, 1e-9 );
// Re-linearize around the new point and check the factors
gtsam::GaussianFactor::shared_ptr expected_factor = gtsam::HessianFactor::shared_ptr(new gtsam::HessianFactor(*betweenFactor.linearize(linpoint2, ordering)));
gtsam::GaussianFactor::shared_ptr actual_factor = hessianContainer.linearize(linpoint2, ordering);
gtsam::GaussianFactor::shared_ptr expected_factor = gtsam::HessianFactor::shared_ptr(new gtsam::HessianFactor(*betweenFactor.linearize(linpoint2)));
gtsam::GaussianFactor::shared_ptr actual_factor = hessianContainer.linearize(linpoint2);
CHECK(gtsam::assert_equal(*expected_factor, *actual_factor));
}

7
gtsam/slam/AntiFactor.h
View File

@ -47,7 +47,7 @@ namespace gtsam {
AntiFactor() {}
/** constructor - Creates the equivalent AntiFactor from an existing factor */
AntiFactor(NonlinearFactor::shared_ptr factor) : Base(factor->begin(), factor->end()), factor_(factor) {}
AntiFactor(NonlinearFactor::shared_ptr factor) : Base(factor->keys()), factor_(factor) {}
virtual ~AntiFactor() {}
@ -94,11 +94,10 @@ namespace gtsam {
* returns a Jacobian instead of a full Hessian), but with the opposite sign. This
* effectively cancels the effect of the original factor on the factor graph.
*/
boost::shared_ptr<GaussianFactor>
linearize(const Values& c, const Ordering& ordering) const {
boost::shared_ptr<GaussianFactor> linearize(const Values& c) const {
// Generate the linearized factor from the contained nonlinear factor
GaussianFactor::shared_ptr gaussianFactor = factor_->linearize(c, ordering);
GaussianFactor::shared_ptr gaussianFactor = factor_->linearize(c);
// return the negated version of the factor
return gaussianFactor->negate();

50
gtsam/slam/tests/testAntiFactor.cpp
View File

@ -23,6 +23,10 @@
#include <gtsam/nonlinear/NonlinearOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/linear/HessianFactor.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/inference/Ordering.h>
#include <gtsam/geometry/Pose3.h>
using namespace std;
@ -40,21 +44,15 @@ TEST( AntiFactor, NegativeHessian)
SharedNoiseModel sigma(noiseModel::Unit::Create(Pose3::Dim()));
// Create a configuration corresponding to the ground truth
boost::shared_ptr<Values> values(new Values());
values->insert(1, pose1);
values->insert(2, pose2);
// Define an elimination ordering
Ordering::shared_ptr ordering(new Ordering());
ordering->insert(1, 0);
ordering->insert(2, 1);
Values values;
values.insert(1, pose1);
values.insert(2, pose2);
// Create a "standard" factor
BetweenFactor<Pose3>::shared_ptr originalFactor(new BetweenFactor<Pose3>(1, 2, z, sigma));
// Linearize it into a Jacobian Factor
GaussianFactor::shared_ptr originalJacobian = originalFactor->linearize(*values, *ordering);
GaussianFactor::shared_ptr originalJacobian = originalFactor->linearize(values);
// Convert it to a Hessian Factor
HessianFactor::shared_ptr originalHessian = HessianFactor::shared_ptr(new HessianFactor(*originalJacobian));
@ -63,7 +61,7 @@ TEST( AntiFactor, NegativeHessian)
AntiFactor::shared_ptr antiFactor(new AntiFactor(originalFactor));
// Linearize the AntiFactor into a Hessian Factor
GaussianFactor::shared_ptr antiGaussian = antiFactor->linearize(*values, *ordering);
GaussianFactor::shared_ptr antiGaussian = antiFactor->linearize(values);
HessianFactor::shared_ptr antiHessian = boost::dynamic_pointer_cast<HessianFactor>(antiGaussian);
@ -96,39 +94,39 @@ TEST( AntiFactor, EquivalentBayesNet)
Pose3 z(Rot3(), Point3(1, 1, 1));
SharedNoiseModel sigma(noiseModel::Unit::Create(Pose3::Dim()));
NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph());
graph->add(PriorFactor<Pose3>(1, pose1, sigma));
graph->add(BetweenFactor<Pose3>(1, 2, pose1.between(pose2), sigma));
NonlinearFactorGraph graph;
graph.push_back(PriorFactor<Pose3>(1, pose1, sigma));
graph.push_back(BetweenFactor<Pose3>(1, 2, pose1.between(pose2), sigma));
// Create a configuration corresponding to the ground truth
Values::shared_ptr values(new Values());
values->insert(1, pose1);
values->insert(2, pose2);
Values values;
values.insert(1, pose1);
values.insert(2, pose2);
// Define an elimination ordering
Ordering::shared_ptr ordering = graph->orderingCOLAMD(*values);
Ordering ordering = graph.orderingCOLAMD();
// Eliminate into a BayesNet
GaussianSequentialSolver solver1(*graph->linearize(*values, *ordering));
GaussianBayesNet::shared_ptr expectedBayesNet = solver1.eliminate();
GaussianFactorGraph lin_graph = *graph.linearize(values);
GaussianBayesNet::shared_ptr expectedBayesNet = lin_graph.eliminateSequential(ordering);
// Back-substitute to find the optimal deltas
VectorValues expectedDeltas = optimize(*expectedBayesNet);
VectorValues expectedDeltas = expectedBayesNet->optimize();
// Add an additional factor between Pose1 and Pose2
BetweenFactor<Pose3>::shared_ptr f1(new BetweenFactor<Pose3>(1, 2, z, sigma));
graph->push_back(f1);
graph.push_back(f1);
// Add the corresponding AntiFactor between Pose1 and Pose2
AntiFactor::shared_ptr f2(new AntiFactor(f1));
graph->push_back(f2);
graph.push_back(f2);
// Again, Eliminate into a BayesNet
GaussianSequentialSolver solver2(*graph->linearize(*values, *ordering));
GaussianBayesNet::shared_ptr actualBayesNet = solver2.eliminate();
GaussianFactorGraph lin_graph1 = *graph.linearize(values);
GaussianBayesNet::shared_ptr actualBayesNet = lin_graph1.eliminateSequential(ordering);
// Back-substitute to find the optimal deltas
VectorValues actualDeltas = optimize(*actualBayesNet);
VectorValues actualDeltas = actualBayesNet->optimize();
// Verify the BayesNets are identical
CHECK(assert_equal(*expectedBayesNet, *actualBayesNet, 1e-5));

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

@ -15,6 +15,7 @@ using namespace gtsam;
using namespace gtsam::symbol_shorthand;
using namespace gtsam::noiseModel;
/* ************************************************************************* */
TEST(BetweenFactor, Rot3) {
Rot3 R1 = Rot3::rodriguez(0.1, 0.2, 0.3);
Rot3 R2 = Rot3::rodriguez(0.4, 0.5, 0.6);
@ -26,19 +27,19 @@ TEST(BetweenFactor, Rot3) {
Vector actual = factor.evaluateError(R1, R2, actualH1, actualH2);
Vector expected = Rot3::Logmap(measured.inverse() * R1.between(R2));
CHECK(assert_equal(expected,actual, 1e-100));
EXPECT(assert_equal(expected,actual, 1e-100));
Matrix numericalH1 = numericalDerivative21(
boost::function<Vector(const Rot3&, const Rot3&)>(boost::bind(
&BetweenFactor<Rot3>::evaluateError, factor, _1, _2, boost::none,
boost::none)), R1, R2, 1e-5);
CHECK(assert_equal(numericalH1,actualH1));
EXPECT(assert_equal(numericalH1,actualH1, 1e-5));
Matrix numericalH2 = numericalDerivative22(
boost::function<Vector(const Rot3&, const Rot3&)>(boost::bind(
&BetweenFactor<Rot3>::evaluateError, factor, _1, _2, boost::none,
boost::none)), R1, R2, 1e-5);
CHECK(assert_equal(numericalH2,actualH2));
EXPECT(assert_equal(numericalH2,actualH2, 1e-5));
}
/* ************************************************************************* */

12
gtsam/slam/tests/testGeneralSFMFactor.cpp
View File

@ -371,7 +371,7 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) {
graph.addCameraConstraint(0, cameras[0]);
// Constrain the scale of the problem with a soft range factor of 1m between the cameras
graph.add(RangeFactor<GeneralCamera,GeneralCamera>(X(0), X(1), 2.0, noiseModel::Isotropic::Sigma(1, 10.0)));
graph.push_back(RangeFactor<GeneralCamera,GeneralCamera>(X(0), X(1), 2.0, noiseModel::Isotropic::Sigma(1, 10.0)));
const double reproj_error = 1e-5 ;
@ -386,8 +386,8 @@ TEST(GeneralSFMFactor, GeneralCameraPoseRange) {
// Tests range factor between a GeneralCamera and a Pose3
Graph graph;
graph.addCameraConstraint(0, GeneralCamera());
graph.add(RangeFactor<GeneralCamera, Pose3>(X(0), X(1), 2.0, noiseModel::Isotropic::Sigma(1, 1.0)));
graph.add(PriorFactor<Pose3>(X(1), Pose3(Rot3(), Point3(1.0, 0.0, 0.0)), noiseModel::Isotropic::Sigma(6, 1.0)));
graph.push_back(RangeFactor<GeneralCamera, Pose3>(X(0), X(1), 2.0, noiseModel::Isotropic::Sigma(1, 1.0)));
graph.push_back(PriorFactor<Pose3>(X(1), Pose3(Rot3(), Point3(1.0, 0.0, 0.0)), noiseModel::Isotropic::Sigma(6, 1.0)));
Values init;
init.insert(X(0), GeneralCamera());
@ -410,9 +410,9 @@ TEST(GeneralSFMFactor, GeneralCameraPoseRange) {
TEST(GeneralSFMFactor, CalibratedCameraPoseRange) {
// Tests range factor between a CalibratedCamera and a Pose3
NonlinearFactorGraph graph;
graph.add(PriorFactor<CalibratedCamera>(X(0), CalibratedCamera(), noiseModel::Isotropic::Sigma(6, 1.0)));
graph.add(RangeFactor<CalibratedCamera, Pose3>(X(0), X(1), 2.0, noiseModel::Isotropic::Sigma(1, 1.0)));
graph.add(PriorFactor<Pose3>(X(1), Pose3(Rot3(), Point3(1.0, 0.0, 0.0)), noiseModel::Isotropic::Sigma(6, 1.0)));
graph.push_back(PriorFactor<CalibratedCamera>(X(0), CalibratedCamera(), noiseModel::Isotropic::Sigma(6, 1.0)));
graph.push_back(RangeFactor<CalibratedCamera, Pose3>(X(0), X(1), 2.0, noiseModel::Isotropic::Sigma(1, 1.0)));
graph.push_back(PriorFactor<Pose3>(X(1), Pose3(Rot3(), Point3(1.0, 0.0, 0.0)), noiseModel::Isotropic::Sigma(6, 1.0)));
Values init;
init.insert(X(0), CalibratedCamera());

3
gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp
View File

@ -20,7 +20,6 @@
#include <gtsam/slam/RangeFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/nonlinear/Ordering.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/linear/VectorValues.h>
@ -369,7 +368,7 @@ TEST( GeneralSFMFactor_Cal3Bundler, optimize_varK_BA ) {
graph.addCameraConstraint(0, cameras[0]);
// Constrain the scale of the problem with a soft range factor of 1m between the cameras
graph.add(RangeFactor<GeneralCamera,GeneralCamera>(X(0), X(1), 2.0, noiseModel::Isotropic::Sigma(1, 10.0)));
graph.push_back(RangeFactor<GeneralCamera,GeneralCamera>(X(0), X(1), 2.0, noiseModel::Isotropic::Sigma(1, 10.0)));
const double reproj_error = 1e-5 ;

1
gtsam/slam/tests/testProjectionFactor.cpp
View File

@ -23,6 +23,7 @@
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/base/TestableAssertions.h>
#include <CppUnitLite/TestHarness.h>
using namespace std;

1
gtsam/slam/tests/testRangeFactor.cpp
View File

@ -24,6 +24,7 @@
#include <gtsam/geometry/Point2.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/base/TestableAssertions.h>
#include <boost/bind.hpp>
using namespace std;

24
gtsam/slam/tests/testReferenceFrameFactor.cpp
View File

@ -136,15 +136,15 @@ TEST( ReferenceFrameFactor, converge_trans ) {
// Pose2 trans = transIdeal * Pose2(-200.0, 100.0, 2.0); // beyond pi/2 - fails
NonlinearFactorGraph graph;
graph.add(PointReferenceFrameFactor(lB1, tA1, lA1));
graph.add(PointReferenceFrameFactor(lB2, tA1, lA2));
graph.push_back(PointReferenceFrameFactor(lB1, tA1, lA1));
graph.push_back(PointReferenceFrameFactor(lB2, tA1, lA2));
// hard constraints on points
double error_gain = 1000.0;
graph.add(NonlinearEquality<gtsam::Point2>(lA1, local1, error_gain));
graph.add(NonlinearEquality<gtsam::Point2>(lA2, local2, error_gain));
graph.add(NonlinearEquality<gtsam::Point2>(lB1, global1, error_gain));
graph.add(NonlinearEquality<gtsam::Point2>(lB2, global2, error_gain));
graph.push_back(NonlinearEquality<gtsam::Point2>(lA1, local1, error_gain));
graph.push_back(NonlinearEquality<gtsam::Point2>(lA2, local2, error_gain));
graph.push_back(NonlinearEquality<gtsam::Point2>(lB1, global1, error_gain));
graph.push_back(NonlinearEquality<gtsam::Point2>(lB2, global2, error_gain));
// create initial estimate
Values init;
@ -186,9 +186,9 @@ TEST( ReferenceFrameFactor, converge_local ) {
NonlinearFactorGraph graph;
double error_gain = 1000.0;
graph.add(PointReferenceFrameFactor(lB1, tA1, lA1));
graph.add(NonlinearEquality<gtsam::Point2>(lB1, global, error_gain));
graph.add(NonlinearEquality<gtsam::Pose2>(tA1, trans, error_gain));
graph.push_back(PointReferenceFrameFactor(lB1, tA1, lA1));
graph.push_back(NonlinearEquality<gtsam::Point2>(lB1, global, error_gain));
graph.push_back(NonlinearEquality<gtsam::Pose2>(tA1, trans, error_gain));
// create initial estimate
Values init;
@ -222,9 +222,9 @@ TEST( ReferenceFrameFactor, converge_global ) {
NonlinearFactorGraph graph;
double error_gain = 1000.0;
graph.add(PointReferenceFrameFactor(lB1, tA1, lA1));
graph.add(NonlinearEquality<gtsam::Point2>(lA1, local, error_gain));
graph.add(NonlinearEquality<gtsam::Pose2>(tA1, trans, error_gain));
graph.push_back(PointReferenceFrameFactor(lB1, tA1, lA1));
graph.push_back(NonlinearEquality<gtsam::Point2>(lA1, local, error_gain));
graph.push_back(NonlinearEquality<gtsam::Pose2>(tA1, trans, error_gain));
// create initial estimate
Values init;

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

@ -189,11 +189,11 @@ TEST( StereoFactor, singlePoint)
{
NonlinearFactorGraph graph;
graph.add(NonlinearEquality<Pose3>(X(1), camera1));
graph.push_back(NonlinearEquality<Pose3>(X(1), camera1));
StereoPoint2 measurement(320, 320.0-50, 240);
// arguments: measurement, sigma, cam#, measurement #, K, baseline (m)
graph.add(GenericStereoFactor<Pose3, Point3>(measurement, model, X(1), L(1), K));
graph.push_back(GenericStereoFactor<Pose3, Point3>(measurement, model, X(1), L(1), K));
// Create a configuration corresponding to the ground truth
Values values;

9
gtsam_unstable/CMakeLists.txt
View File

@ -18,11 +18,18 @@ add_custom_target(check.unstable COMMAND ${CMAKE_CTEST_COMMAND} --output-on-fail
# Add the full name to this list, as in the following example
# Sources to remove from builds
set (excluded_sources # "")
"${CMAKE_CURRENT_SOURCE_DIR}/nonlinear/BatchFixedLagSmoother.cpp"
"${CMAKE_CURRENT_SOURCE_DIR}/nonlinear/ConcurrentBatchFilter.cpp"
"${CMAKE_CURRENT_SOURCE_DIR}/nonlinear/ConcurrentBatchSmoother.cpp"
# "${CMAKE_CURRENT_SOURCE_DIR}/nonlinear/ConcurrentFilteringAndSmoothing.cpp"
# "${CMAKE_CURRENT_SOURCE_DIR}/nonlinear/FixedLagSmoother.cpp"
"${CMAKE_CURRENT_SOURCE_DIR}/nonlinear/IncrementalFixedLagSmoother.cpp"
"${CMAKE_CURRENT_SOURCE_DIR}/slam/serialization.cpp"
)
set (excluded_headers # "")
"${CMAKE_CURRENT_SOURCE_DIR}/slam/serialization.h"
"${CMAKE_CURRENT_SOURCE_DIR}/slam/serialization.h"
)
# assemble core libaries

18
gtsam_unstable/linear/bayesTreeOperations.cpp
View File

@ -13,14 +13,6 @@ using namespace std;
namespace gtsam {
/* ************************************************************************* */
std::set<Index> keysToIndices(const KeySet& keys, const Ordering& ordering) {
std::set<Index> result;
BOOST_FOREACH(const Key& key, keys)
result.insert(ordering[key]);
return result;
}
/* ************************************************************************* */
GaussianFactorGraph splitFactors(const GaussianFactorGraph& fullgraph) {
GaussianFactorGraph result;
@ -55,9 +47,9 @@ GaussianFactorGraph splitFactor(const GaussianFactor::shared_ptr& factor) {
size_t nrRows = std::min(rowsRemaining, varDim);
// Extract submatrices
std::vector<std::pair<Index, Matrix> > matrices;
std::vector<std::pair<Key, Matrix> > matrices;
for (colIt = rowIt; colIt != jf->end(); ++colIt) {
Index idx = *colIt;
Key idx = *colIt;
const Matrix subA = jf->getA(colIt).middleRows(startRow, nrRows);
matrices.push_back(make_pair(idx, subA));
}
@ -103,11 +95,11 @@ void findCliques(const GaussianBayesTree::sharedClique& current_clique,
/* ************************************************************************* */
std::set<GaussianConditional::shared_ptr> findAffectedCliqueConditionals(
const GaussianBayesTree& bayesTree, const std::set<Index>& savedIndices) {
const GaussianBayesTree& bayesTree, const std::set<Key>& savedIndices) {
std::set<GaussianConditional::shared_ptr> affected_cliques;
// FIXME: track previously found keys more efficiently
BOOST_FOREACH(const Index& index, savedIndices) {
GaussianBayesTree::sharedClique clique = bayesTree.nodes()[index];
BOOST_FOREACH(const Key& index, savedIndices) {
GaussianBayesTree::sharedClique clique = bayesTree[index];
// add path back to root to affected set
findCliques(clique, affected_cliques);

19
gtsam_unstable/linear/bayesTreeOperations.h
View File

@ -12,15 +12,9 @@
#include <gtsam_unstable/base/dllexport.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/GaussianBayesTree.h>
#include <gtsam/nonlinear/Ordering.h>
namespace gtsam {
// Managing orderings
/** Converts sets of keys to indices by way of orderings */
GTSAM_UNSTABLE_EXPORT std::set<Index> keysToIndices(const KeySet& keys, const Ordering& ordering);
// Linear Graph Operations
/**
@ -47,7 +41,7 @@ GTSAM_UNSTABLE_EXPORT GaussianFactorGraph removePriors(const GaussianFactorGraph
* @return the set of conditionals extracted from cliques.
*/
GTSAM_UNSTABLE_EXPORT std::set<GaussianConditional::shared_ptr> findAffectedCliqueConditionals(
const GaussianBayesTree& bayesTree, const std::set<Index>& savedIndices);
const GaussianBayesTree& bayesTree, const std::set<Key>& savedIndices);
/**
* Recursively traverses from a given clique in a Bayes Tree and collects all of the conditionals
@ -78,11 +72,11 @@ GaussianFactorGraph liquefy(const typename BAYESTREE::sharedClique& root, bool s
if (root && root->conditional()) {
GaussianConditional::shared_ptr conditional = root->conditional();
if (!splitConditionals)
result.push_back(conditional->toFactor());
result.push_back(conditional);
else
result.push_back(splitFactor(conditional->toFactor()));
result.push_back(splitFactor(conditional));
}
BOOST_FOREACH(const typename BAYESTREE::sharedClique& child, root->children())
BOOST_FOREACH(typename BAYESTREE::sharedClique child, root->children)
result.push_back(liquefy<BAYESTREE>(child, splitConditionals));
return result;
}
@ -94,7 +88,10 @@ GaussianFactorGraph liquefy(const typename BAYESTREE::sharedClique& root, bool s
*/
template <class BAYESTREE>
GaussianFactorGraph liquefy(const BAYESTREE& bayesTree, bool splitConditionals = false) {
return liquefy<BAYESTREE>(bayesTree.root(), splitConditionals);
GaussianFactorGraph result;
BOOST_FOREACH(const typename BAYESTREE::sharedClique& root, bayesTree.roots())
result.push_back(liquefy<BAYESTREE>(root, splitConditionals));
return result;
}
} // \namespace gtsam

40
gtsam_unstable/linear/conditioning.cpp
View File

@ -8,13 +8,16 @@
#include <gtsam_unstable/linear/conditioning.h>
#include <gtsam_unstable/linear/bayesTreeOperations.h>
#include <boost/assign/std/vector.hpp>
using namespace std;
using namespace boost::assign;
namespace gtsam {
/* ************************************************************************* */
GaussianConditional::shared_ptr conditionDensity(const GaussianConditional::shared_ptr& initConditional,
const std::set<Index>& saved_indices, const VectorValues& solution) {
const std::set<Key>& saved_indices, const VectorValues& solution) {
const bool verbose = false;
if (!initConditional)
@ -26,12 +29,12 @@ GaussianConditional::shared_ptr conditionDensity(const GaussianConditional::shar
}
// Check for presence of variables to remove
std::set<Index> frontalsToRemove, parentsToRemove;
BOOST_FOREACH(const Index& frontal, initConditional->frontals())
std::set<Key> frontalsToRemove, parentsToRemove;
BOOST_FOREACH(const Key& frontal, initConditional->frontals())
if (!saved_indices.count(frontal))
frontalsToRemove.insert(frontal);
BOOST_FOREACH(const Index& parent, initConditional->parents())
BOOST_FOREACH(const Key& parent, initConditional->parents())
if (!saved_indices.count(parent))
parentsToRemove.insert(parent);
@ -50,14 +53,14 @@ GaussianConditional::shared_ptr conditionDensity(const GaussianConditional::shar
size_t oldOffset = 0;
vector<size_t> newDims, oldDims;
vector<size_t> oldColOffsets;
vector<Index> newIndices;
vector<Key> newIndices;
vector<size_t> newIdxToOldIdx; // Access to arrays, maps from new var to old var
const vector<Index>& oldIndices = initConditional->keys();
const vector<Key>& oldIndices = initConditional->keys();
const size_t oldNrFrontals = initConditional->nrFrontals();
GaussianConditional::const_iterator varIt = initConditional->beginFrontals();
size_t oldIdx = 0;
for (; varIt != initConditional->endFrontals(); ++varIt) {
size_t varDim = initConditional->dim(varIt);
size_t varDim = initConditional->getDim(varIt);
oldDims += varDim;
if (!frontalsToRemove.count(*varIt)) {
newTotalCols += varDim;
@ -73,7 +76,7 @@ GaussianConditional::shared_ptr conditionDensity(const GaussianConditional::shar
}
varIt = initConditional->beginParents();
for (; varIt != initConditional->endParents(); ++varIt) {
size_t varDim = initConditional->dim(varIt);
size_t varDim = initConditional->getDim(varIt);
oldDims += varDim;
if (!parentsToRemove.count(*varIt)) {
newTotalCols += varDim;
@ -99,7 +102,7 @@ GaussianConditional::shared_ptr conditionDensity(const GaussianConditional::shar
size_t oldColOffset = oldColOffsets.at(newfrontalIdx);
// get R block, sliced by row
Eigen::Block<GaussianConditional::rsd_type::constBlock> rblock =
Eigen::Block<GaussianConditional::constABlock> rblock =
initConditional->get_R().block(oldColOffset, oldColOffset, dim, oldRNrCols-oldColOffset);
if (verbose) cout << " rblock\n" << rblock << endl;
@ -113,14 +116,14 @@ GaussianConditional::shared_ptr conditionDensity(const GaussianConditional::shar
if (verbose) cout << " full_matrix: set rhs\n" << full_matrix << endl;
// set sigmas
sigmas.segment(newColOffset, dim) = initConditional->get_sigmas().segment(oldColOffset, dim);
sigmas.segment(newColOffset, dim) = initConditional->get_model()->sigmas().segment(oldColOffset, dim);
// add parents in R block, while updating rhs
// Loop through old variable list
size_t newParentStartCol = newColOffset + dim;
size_t oldParentStartCol = dim; // Copying from Rblock - offset already accounted for
for (size_t oldIdx = newIdxToOldIdx[newfrontalIdx]+1; oldIdx<oldNrFrontals; ++oldIdx) {
Index parentKey = oldIndices[oldIdx];
Key parentKey = oldIndices[oldIdx];
size_t parentDim = oldDims[oldIdx];
if (verbose) cout << " Adding parents from R: parentKey: " << parentKey << " parentDim: " << parentDim << endl;
if (!frontalsToRemove.count(parentKey)) {
@ -144,8 +147,8 @@ GaussianConditional::shared_ptr conditionDensity(const GaussianConditional::shar
// add parents (looping over original block structure), while updating rhs
GaussianConditional::const_iterator oldParent = initConditional->beginParents();
for (; oldParent != initConditional->endParents(); ++oldParent) {
Index parentKey = *oldParent;
size_t parentDim = initConditional->dim(oldParent);
Key parentKey = *oldParent;
size_t parentDim = initConditional->getDim(oldParent);
if (verbose) cout << " Adding parents from S: parentKey: " << parentKey << " parentDim: " << parentDim << endl;
if (parentsToRemove.count(parentKey)) {
// Solve out the variable
@ -169,17 +172,18 @@ GaussianConditional::shared_ptr conditionDensity(const GaussianConditional::shar
// Construct a new conditional
if (verbose) cout << "backsubSummarize() Complete!" << endl;
GaussianConditional::rsd_type matrices(full_matrix, newDims.begin(), newDims.end());
// GaussianConditional::rsd_type matrices(full_matrix, newDims.begin(), newDims.end());
VerticalBlockMatrix matrices(newDims, full_matrix);
return GaussianConditional::shared_ptr(new
GaussianConditional(newIndices.begin(), newIndices.end(), newNrFrontals, matrices, sigmas));
GaussianConditional(newIndices, newNrFrontals, matrices, noiseModel::Diagonal::Sigmas(sigmas)));
}
/* ************************************************************************* */
GaussianFactorGraph conditionDensity(const GaussianBayesTree& bayesTree,
const std::set<Index>& saved_indices) {
const std::set<Key>& saved_indices) {
const bool verbose = false;
VectorValues solution = optimize(bayesTree);
VectorValues solution = bayesTree.optimize();
// FIXME: set of conditionals does not manage possibility of solving out whole separators
std::set<GaussianConditional::shared_ptr> affected_cliques = findAffectedCliqueConditionals(bayesTree, saved_indices);
@ -191,7 +195,7 @@ GaussianFactorGraph conditionDensity(const GaussianBayesTree& bayesTree,
GaussianConditional::shared_ptr reducedConditional = conditionDensity(conditional, saved_indices, solution);
if (reducedConditional) {
if (verbose) reducedConditional->print("Final conditional");
summarized_graph.push_back(reducedConditional->toFactor());
summarized_graph.push_back(reducedConditional);
} else if (verbose) {
cout << "Conditional removed after summarization!" << endl;
}

11
gtsam_unstable/linear/conditioning.h
View File

@ -12,7 +12,6 @@
#include <gtsam_unstable/base/dllexport.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/GaussianBayesTree.h>
#include <gtsam/nonlinear/Ordering.h>
namespace gtsam {
@ -27,16 +26,18 @@ namespace gtsam {
* @param saved_indices is the set of indices that should appear in the result
* @param solution is a full solution for the system
*/
GTSAM_UNSTABLE_EXPORT gtsam::GaussianConditional::shared_ptr conditionDensity(const gtsam::GaussianConditional::shared_ptr& initConditional,
const std::set<gtsam::Index>& saved_indices, const gtsam::VectorValues& solution);
GTSAM_UNSTABLE_EXPORT GaussianConditional::shared_ptr conditionDensity(
const GaussianConditional::shared_ptr& initConditional,
const std::set<Key>& saved_indices, const VectorValues& solution);
/**
* Backsubstitution-based conditioning for a complete Bayes Tree - reduces
* conditionals by solving out variables to eliminate. Traverses the tree to
* add the correct dummy factors whenever a separator is eliminated.
*/
GTSAM_UNSTABLE_EXPORT gtsam::GaussianFactorGraph conditionDensity(const gtsam::GaussianBayesTree& bayesTree,
const std::set<gtsam::Index>& saved_indices);
GTSAM_UNSTABLE_EXPORT GaussianFactorGraph conditionDensity(
const GaussianBayesTree& bayesTree,
const std::set<Key>& saved_indices);
} // \namespace gtsam

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

@ -193,10 +193,10 @@ TEST( testBayesTreeOperations, liquefy ) {
// Create smoother with 7 nodes
Ordering ordering;
ordering += X(1),X(3),X(5),X(7),X(2),X(6),X(4);
GaussianFactorGraph smoother = createSmoother(7, ordering).first;
GaussianFactorGraph smoother = createSmoother(7);
// Create the Bayes tree
GaussianBayesTree bayesTree = *GaussianMultifrontalSolver(smoother).eliminate();
GaussianBayesTree bayesTree = *smoother.eliminateMultifrontal(ordering);
// bayesTree.print("Full tree");
SharedDiagonal unit6 = noiseModel::Diagonal::Sigmas(Vector_(ones(6)));

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

@ -12,6 +12,8 @@
#include <gtsam/base/TestableAssertions.h>
#include <boost/assign/std/set.hpp>
#include <boost/assign/std/list.hpp>
#include <boost/assign/std/vector.hpp>
#include <gtsam_unstable/linear/conditioning.h>
@ -64,7 +66,8 @@ TEST( testConditioning, directed_elimination_singlefrontal ) {
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);
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);
SharedDiagonal sigmas = noiseModel::Unit::Create(1);
GaussianConditional::shared_ptr initConditional(new
GaussianConditional(root_key, d0, R11, removed_key, S, remaining_parent, T, sigmas));
@ -95,155 +98,156 @@ TEST( testConditioning, directed_elimination_singlefrontal ) {
EXPECT(!actRemoveFrontal);
}
/* ************************************************************************* */
TEST( testConditioning, directed_elimination_multifrontal ) {
// Use top two rows from the previous example
Index root_key = 0, removed_key = 1, remaining_parent = 2;
Matrix R11 = R.topLeftCorner(2,2), S = R.block(0,2,2,1),
Sprime = Matrix_(1,1,-2.0), R11prime = Matrix_(1,1, 1.0);
Vector d1 = d.segment(0,2), sigmas1 = Vector_(1, 1.0), sigmas2 = Vector_(2, 1.0, 1.0);
std::list<std::pair<Index, Matrix> > terms;
terms += make_pair(root_key, Matrix(R11.col(0)));
terms += make_pair(removed_key, Matrix(R11.col(1)));
terms += make_pair(remaining_parent, S);
GaussianConditional::shared_ptr initConditional(new GaussianConditional(terms, 2, d1, sigmas2));
VectorValues solution;
solution.insert(0, x.segment(0,1));
solution.insert(1, x.segment(1,1));
solution.insert(2, x.segment(2,1));
std::set<Index> saved_indices;
saved_indices += root_key, remaining_parent;
GaussianConditional::shared_ptr actSummarized = conditionDensity(initConditional, saved_indices, solution);
GaussianConditional::shared_ptr expSummarized(new
GaussianConditional(root_key, d.segment(0,1) - Sprime*x(1), R11prime, remaining_parent, R.block(0,2,1,1), sigmas1));
CHECK(actSummarized);
EXPECT(assert_equal(*expSummarized, *actSummarized, tol));
}
/* ************************************************************************* */
TEST( testConditioning, directed_elimination_multifrontal_multidim ) {
// use larger example, three frontal variables, dim = 2 each, two parents (one removed)
// Vars: 0, 1, 2, 3, 4; frontal: 0, 1, 2. parents: 3, 4;
// Remove 1, 3
Matrix Rinit = Matrix_(6, 11,
1.0, 0.0, 2.0, 0.0, 3.0, 0.0, 1.0, 0.0, -1.0, 0.0, 0.1,
0.0, 1.0, 0.0, 2.0, 0.0, 3.0, 0.0, 1.0, 0.0, 1.0, 0.2,
0.0, 0.0, 3.0, 0.0, 4.0, 0.0, 0.0,-1.0, 1.0, 0.0, 0.3,
0.0, 0.0, 0.0, 4.0, 0.0, 4.0, 3.0, 2.0, 0.0, 9.0, 0.4,
0.0, 0.0, 0.0, 0.0, 5.0, 0.0, 7.0, 0.0, 3.0, 0.0, 0.5,
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;
GaussianConditional::rsd_type init_matrices(Rinit, init_dims.begin(), init_dims.end());
Vector sigmas = ones(6);
vector<size_t> init_keys; init_keys += 0, 1, 2, 3, 4;
GaussianConditional::shared_ptr initConditional(new
GaussianConditional(init_keys.begin(), init_keys.end(), 3, init_matrices, sigmas));
// Construct a solution vector
VectorValues solution;
solution.insert(0, zero(2));
solution.insert(1, zero(2));
solution.insert(2, zero(2));
solution.insert(3, Vector_(2, 1.0, 2.0));
solution.insert(4, Vector_(2, 3.0, 4.0));
initConditional->solveInPlace(solution);
std::set<Index> saved_indices;
saved_indices += 0, 2, 4;
GaussianConditional::shared_ptr actSummarized = conditionDensity(initConditional, saved_indices, solution);
CHECK(actSummarized);
Matrix Rexp = Matrix_(4, 7,
1.0, 0.0, 3.0, 0.0, -1.0, 0.0, 0.1,
0.0, 1.0, 0.0, 3.0, 0.0, 1.0, 0.2,
0.0, 0.0, 5.0, 0.0, 3.0, 0.0, 0.5,
0.0, 0.0, 0.0, 4.0, 0.0, 6.0, 0.6);
// Update rhs
Rexp.block(0, 6, 2, 1) -= Rinit.block(0, 2, 2, 2) * solution.at(1) + Rinit.block(0, 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;
GaussianConditional::rsd_type exp_matrices(Rexp, exp_dims.begin(), exp_dims.end());
Vector exp_sigmas = ones(4);
vector<size_t> exp_keys; exp_keys += 0, 2, 4;
GaussianConditional expSummarized(exp_keys.begin(), exp_keys.end(), 2, exp_matrices, exp_sigmas);
EXPECT(assert_equal(expSummarized, *actSummarized, tol));
}
/* ************************************************************************* */
TEST( testConditioning, directed_elimination_multifrontal_multidim2 ) {
// Example from LinearAugmentedSystem
// 4 variables, last two in ordering kept - should be able to do this with no computation.
vector<size_t> init_dims; init_dims += 3, 3, 2, 2, 1;
//Full initial conditional: density on [3] [4] [5] [6]
Matrix Rinit = Matrix_(10, 11,
8.78422312, -0.0375455118, -0.0387376278, -5.059576, 0.0, 0.0, -0.0887200041, 0.00429643583, -0.130078263, 0.0193260727, 0.0,
0.0, 8.46951839, 9.51456887, -0.0224291821, -5.24757636, 0.0, 0.0586258904, -0.173455825, 0.11090295, -0.330696013, 0.0,
0.0, 0.0, 16.5539485, 0.00105159359, -2.35354497, -6.04085484, -0.0212095105, 0.0978729072, 0.00471054272, 0.0694956367, 0.0,
0.0, 0.0, 0.0, 10.9015885, -0.0105694572, 0.000582715469, -0.0410535006, 0.00162772139, -0.0601433772, 0.0082824087,0.0,
0.0, 0.0, 0.0, 0.0, 10.5531086, -1.34722553, 0.02438072, -0.0644224578, 0.0561372492, -0.148932792, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 21.4870439, -0.00443305851, 0.0234766354, 0.00484572411, 0.0101997356, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.73892865, 0.0242046766, -0.0459727048, 0.0445071938, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.61246954, 0.02287419, -0.102870789, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.04823446, -0.302033014, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.24068986, 0.0);
Vector dinit = Vector_(10,
-0.00186915, 0.00318554, 0.000592421, -0.000861, 0.00171528, 0.000274123, -0.0284011, 0.0275465, 0.0439795, -0.0222134);
Rinit.rightCols(1) = dinit;
Vector sigmas = ones(10);
GaussianConditional::rsd_type init_matrices(Rinit, init_dims.begin(), init_dims.end());
vector<size_t> init_keys; init_keys += 3, 4, 5, 6;
GaussianConditional::shared_ptr initConditional(new
GaussianConditional(init_keys.begin(), init_keys.end(), 4, init_matrices, sigmas));
// Calculate a solution
VectorValues solution;
solution.insert(0, zero(3));
solution.insert(1, zero(3));
solution.insert(2, zero(3));
solution.insert(3, zero(3));
solution.insert(4, zero(3));
solution.insert(5, zero(2));
solution.insert(6, zero(2));
initConditional->solveInPlace(solution);
// Perform summarization
std::set<Index> saved_indices;
saved_indices += 5, 6;
GaussianConditional::shared_ptr actSummarized = conditionDensity(initConditional, saved_indices, solution);
CHECK(actSummarized);
// Create expected value on [5], [6]
Matrix Rexp = Matrix_(4, 5,
2.73892865, 0.0242046766, -0.0459727048, 0.0445071938, -0.0284011,
0.0, 2.61246954, 0.02287419, -0.102870789, 0.0275465,
0.0, 0.0, 2.04823446, -0.302033014, 0.0439795,
0.0, 0.0, 0.0, 2.24068986, -0.0222134);
Vector expsigmas = ones(4);
vector<size_t> exp_dims; exp_dims += 2, 2, 1;
GaussianConditional::rsd_type exp_matrices(Rexp, exp_dims.begin(), exp_dims.end());
vector<size_t> exp_keys; exp_keys += 5, 6;
GaussianConditional expConditional(exp_keys.begin(), exp_keys.end(), 2, exp_matrices, expsigmas);
EXPECT(assert_equal(expConditional, *actSummarized, tol));
}
///* ************************************************************************* */
//TEST( testConditioning, directed_elimination_multifrontal ) {
// // Use top two rows from the previous example
// Index root_key = 0, removed_key = 1, remaining_parent = 2;
// Matrix R11 = R.topLeftCorner(2,2), S = R.block(0,2,2,1),
// Sprime = Matrix_(1,1,-2.0), R11prime = Matrix_(1,1, 1.0);
// Vector d1 = d.segment(0,2);
// SharedDiagonal sigmas1 = noiseModel::Unit::Create(1), sigmas2 = noiseModel::Unit::Create(2);
//
//
// std::list<std::pair<Index, Matrix> > terms;
// terms += make_pair(root_key, Matrix(R11.col(0)));
// terms += make_pair(removed_key, Matrix(R11.col(1)));
// terms += make_pair(remaining_parent, S);
// GaussianConditional::shared_ptr initConditional(new GaussianConditional(terms, 2, d1, sigmas2));
//
// VectorValues solution;
// solution.insert(0, x.segment(0,1));
// solution.insert(1, x.segment(1,1));
// solution.insert(2, x.segment(2,1));
//
// std::set<Index> saved_indices;
// saved_indices += root_key, remaining_parent;
//
// GaussianConditional::shared_ptr actSummarized = conditionDensity(initConditional, saved_indices, solution);
// GaussianConditional::shared_ptr expSummarized(new
// GaussianConditional(root_key, d.segment(0,1) - Sprime*x(1), R11prime, remaining_parent, R.block(0,2,1,1), sigmas1));
//
// CHECK(actSummarized);
// EXPECT(assert_equal(*expSummarized, *actSummarized, tol));
//}
//
///* ************************************************************************* */
//TEST( testConditioning, directed_elimination_multifrontal_multidim ) {
// // use larger example, three frontal variables, dim = 2 each, two parents (one removed)
// // Vars: 0, 1, 2, 3, 4; frontal: 0, 1, 2. parents: 3, 4;
// // Remove 1, 3
// Matrix Rinit = Matrix_(6, 11,
// 1.0, 0.0, 2.0, 0.0, 3.0, 0.0, 1.0, 0.0, -1.0, 0.0, 0.1,
// 0.0, 1.0, 0.0, 2.0, 0.0, 3.0, 0.0, 1.0, 0.0, 1.0, 0.2,
// 0.0, 0.0, 3.0, 0.0, 4.0, 0.0, 0.0,-1.0, 1.0, 0.0, 0.3,
// 0.0, 0.0, 0.0, 4.0, 0.0, 4.0, 3.0, 2.0, 0.0, 9.0, 0.4,
// 0.0, 0.0, 0.0, 0.0, 5.0, 0.0, 7.0, 0.0, 3.0, 0.0, 0.5,
// 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;
// VerticalBlockMatrix init_matrices(init_dims, Rinit);
// SharedDiagonal sigmas = noiseModel::Unit::Create(6);
// vector<size_t> init_keys; init_keys += 0, 1, 2, 3, 4;
// GaussianConditional::shared_ptr initConditional(new
// GaussianConditional(init_keys, 3, init_matrices, sigmas));
//
// // Construct a solution vector
// VectorValues solution;
// solution.insert(0, zero(2));
// solution.insert(1, zero(2));
// solution.insert(2, zero(2));
// solution.insert(3, Vector_(2, 1.0, 2.0));
// solution.insert(4, Vector_(2, 3.0, 4.0));
//
// solution = initConditional->solve(solution);
//
// std::set<Index> saved_indices;
// saved_indices += 0, 2, 4;
//
// GaussianConditional::shared_ptr actSummarized = conditionDensity(initConditional, saved_indices, solution);
// CHECK(actSummarized);
//
// Matrix Rexp = Matrix_(4, 7,
// 1.0, 0.0, 3.0, 0.0, -1.0, 0.0, 0.1,
// 0.0, 1.0, 0.0, 3.0, 0.0, 1.0, 0.2,
// 0.0, 0.0, 5.0, 0.0, 3.0, 0.0, 0.5,
// 0.0, 0.0, 0.0, 4.0, 0.0, 6.0, 0.6);
//
// // Update rhs
// Rexp.block(0, 6, 2, 1) -= Rinit.block(0, 2, 2, 2) * solution.at(1) + Rinit.block(0, 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;
// VerticalBlockMatrix exp_matrices(exp_dims, Rexp);
// SharedDiagonal exp_sigmas = noiseModel::Unit::Create(4);
// vector<size_t> exp_keys; exp_keys += 0, 2, 4;
// GaussianConditional expSummarized(exp_keys, 2, exp_matrices, exp_sigmas);
//
// EXPECT(assert_equal(expSummarized, *actSummarized, tol));
//}
//
///* ************************************************************************* */
//TEST( testConditioning, directed_elimination_multifrontal_multidim2 ) {
// // Example from LinearAugmentedSystem
// // 4 variables, last two in ordering kept - should be able to do this with no computation.
//
// vector<size_t> init_dims; init_dims += 3, 3, 2, 2, 1;
//
// //Full initial conditional: density on [3] [4] [5] [6]
// Matrix Rinit = Matrix_(10, 11,
// 8.78422312, -0.0375455118, -0.0387376278, -5.059576, 0.0, 0.0, -0.0887200041, 0.00429643583, -0.130078263, 0.0193260727, 0.0,
// 0.0, 8.46951839, 9.51456887, -0.0224291821, -5.24757636, 0.0, 0.0586258904, -0.173455825, 0.11090295, -0.330696013, 0.0,
// 0.0, 0.0, 16.5539485, 0.00105159359, -2.35354497, -6.04085484, -0.0212095105, 0.0978729072, 0.00471054272, 0.0694956367, 0.0,
// 0.0, 0.0, 0.0, 10.9015885, -0.0105694572, 0.000582715469, -0.0410535006, 0.00162772139, -0.0601433772, 0.0082824087,0.0,
// 0.0, 0.0, 0.0, 0.0, 10.5531086, -1.34722553, 0.02438072, -0.0644224578, 0.0561372492, -0.148932792, 0.0,
// 0.0, 0.0, 0.0, 0.0, 0.0, 21.4870439, -0.00443305851, 0.0234766354, 0.00484572411, 0.0101997356, 0.0,
// 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.73892865, 0.0242046766, -0.0459727048, 0.0445071938, 0.0,
// 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.61246954, 0.02287419, -0.102870789, 0.0,
// 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.04823446, -0.302033014, 0.0,
// 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.24068986, 0.0);
// Vector dinit = Vector_(10,
// -0.00186915, 0.00318554, 0.000592421, -0.000861, 0.00171528, 0.000274123, -0.0284011, 0.0275465, 0.0439795, -0.0222134);
// Rinit.rightCols(1) = dinit;
// SharedDiagonal sigmas = noiseModel::Unit::Create(10);
//
// VerticalBlockMatrix init_matrices(init_dims, Rinit);
// vector<size_t> init_keys; init_keys += 3, 4, 5, 6;
// GaussianConditional::shared_ptr initConditional(new
// GaussianConditional(init_keys, 4, init_matrices, sigmas));
//
// // Calculate a solution
// VectorValues solution;
// solution.insert(0, zero(3));
// solution.insert(1, zero(3));
// solution.insert(2, zero(3));
// solution.insert(3, zero(3));
// solution.insert(4, zero(3));
// solution.insert(5, zero(2));
// solution.insert(6, zero(2));
//
// solution = initConditional->solve(solution);
//
// // Perform summarization
// std::set<Index> saved_indices;
// saved_indices += 5, 6;
//
// GaussianConditional::shared_ptr actSummarized = conditionDensity(initConditional, saved_indices, solution);
// CHECK(actSummarized);
//
// // Create expected value on [5], [6]
// Matrix Rexp = Matrix_(4, 5,
// 2.73892865, 0.0242046766, -0.0459727048, 0.0445071938, -0.0284011,
// 0.0, 2.61246954, 0.02287419, -0.102870789, 0.0275465,
// 0.0, 0.0, 2.04823446, -0.302033014, 0.0439795,
// 0.0, 0.0, 0.0, 2.24068986, -0.0222134);
// SharedDiagonal expsigmas = noiseModel::Unit::Create(4);
//
// vector<size_t> exp_dims; exp_dims += 2, 2, 1;
// VerticalBlockMatrix exp_matrices(exp_dims, Rexp);
// vector<size_t> exp_keys; exp_keys += 5, 6;
// GaussianConditional expConditional(exp_keys, 2, exp_matrices, expsigmas);
//
// EXPECT(assert_equal(expConditional, *actSummarized, tol));
//}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }

2
gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp
View File

@ -22,7 +22,7 @@
#include <gtsam/linear/GaussianJunctionTree.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/GaussianFactor.h>
#include <gtsam/inference/inference.h>
//#include <gtsam/inference/inference.h>
#include <gtsam/base/debug.h>
namespace gtsam {

5
gtsam_unstable/nonlinear/BatchFixedLagSmoother.h
View File

@ -55,7 +55,7 @@ public:
* a single variable is needed, it is faster to call calculateEstimate(const KEY&).
*/
Values calculateEstimate() const {
return theta_.retract(delta_, ordering_);
return theta_.retract(delta_);
}
/** Compute an estimate for a single variable using its incomplete linear delta computed
@ -66,8 +66,7 @@ public:
*/
template<class VALUE>
VALUE calculateEstimate(Key key) const {
const Index index = ordering_.at(key);
const Vector delta = delta_.at(index);
const Vector delta = delta_.at(key);
return theta_.at<VALUE>(key).retract(delta);
}

8
gtsam_unstable/nonlinear/CMakeLists.txt
View File

@ -19,7 +19,13 @@ set (nonlinear_full_libs
${gtsam_unstable-default})
# Exclude tests that don't work
set (nonlinear_excluded_tests "")
set (nonlinear_excluded_tests #"")
"${CMAKE_CURRENT_SOURCE_DIR}/tests/testBatchFixedLagSmoother.cpp"
"${CMAKE_CURRENT_SOURCE_DIR}/tests/testConcurrentBatchFilter.cpp"
"${CMAKE_CURRENT_SOURCE_DIR}/tests/testConcurrentBatchSmoother.cpp"
"${CMAKE_CURRENT_SOURCE_DIR}/tests/testIncrementalFixedLagSmoother.cpp"
)
# Add all tests
gtsam_add_subdir_tests(nonlinear_unstable "${nonlinear_local_libs}" "${nonlinear_full_libs}" "${nonlinear_excluded_tests}")

28
gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp
View File

@ -128,8 +128,8 @@ void ConcurrentBatchFilter::synchronize(const NonlinearFactorGraph& summarizedFa
// Perform an optional optimization on the to-be-sent-to-the-smoother factors
if(relin_) {
// Create ordering and delta
Ordering ordering = *graph.orderingCOLAMD(values);
VectorValues delta = values.zeroVectors(ordering);
Ordering ordering = graph.orderingCOLAMD();
VectorValues delta = values.zeroVectors();
// Optimize this graph using a modified version of L-M
optimize(graph, values, ordering, delta, separatorValues, parameters_);
// Update filter theta and delta
@ -162,8 +162,8 @@ void ConcurrentBatchFilter::synchronize(const NonlinearFactorGraph& summarizedFa
// Generate separate orderings that place the filter keys or the smoother keys first
// TODO: This is convenient, but it recalculates the variable index each time
Ordering filterOrdering = *graph.orderingCOLAMDConstrained(values, filterConstraints);
Ordering smootherOrdering = *graph.orderingCOLAMDConstrained(values, smootherConstraints);
Ordering filterOrdering = graph.orderingCOLAMDConstrained(filterConstraints);
Ordering smootherOrdering = graph.orderingCOLAMDConstrained(smootherConstraints);
// Extract the set of filter keys and smoother keys
std::set<Key> filterKeys;
@ -339,7 +339,7 @@ ConcurrentBatchFilter::Result ConcurrentBatchFilter::optimize(const NonlinearFac
double errorTol = parameters.errorTol;
// Create a Values that holds the current evaluation point
Values evalpoint = theta.retract(delta, ordering);
Values evalpoint = theta.retract(delta);
result.error = factors.error(evalpoint);
// Use a custom optimization loop so the linearization points can be controlled
@ -352,7 +352,7 @@ ConcurrentBatchFilter::Result ConcurrentBatchFilter::optimize(const NonlinearFac
gttic(optimizer_iteration);
{
// Linearize graph around the linearization point
GaussianFactorGraph linearFactorGraph = *factors.linearize(theta, ordering);
GaussianFactorGraph linearFactorGraph = *factors.linearize(theta);
// Keep increasing lambda until we make make progress
while(true) {
@ -377,7 +377,7 @@ ConcurrentBatchFilter::Result ConcurrentBatchFilter::optimize(const NonlinearFac
// Solve Damped Gaussian Factor Graph
newDelta = GaussianJunctionTree(dampedFactorGraph).optimize(parameters.getEliminationFunction());
// update the evalpoint with the new delta
evalpoint = theta.retract(newDelta, ordering);
evalpoint = theta.retract(newDelta);
gttoc(solve);
// Evaluate the new nonlinear error
@ -442,7 +442,7 @@ void ConcurrentBatchFilter::marginalize(const FastList<Key>& keysToMove) {
// Note: It is assumed the ordering already has these keys first
// Create the linear factor graph
GaussianFactorGraph linearFactorGraph = *factors_.linearize(theta_, ordering_);
GaussianFactorGraph linearFactorGraph = *factors_.linearize(theta_);
// Calculate the variable index
VariableIndex variableIndex(linearFactorGraph, ordering_.size());
@ -474,7 +474,7 @@ void ConcurrentBatchFilter::marginalize(const FastList<Key>& keysToMove) {
BOOST_FOREACH(Index index, indicesToEliminate) {
GaussianFactor::shared_ptr gaussianFactor = forest.at(index)->eliminateRecursive(parameters_.getEliminationFunction());
if(gaussianFactor->size() > 0) {
LinearContainerFactor::shared_ptr marginalFactor(new LinearContainerFactor(gaussianFactor, ordering_, theta_));
LinearContainerFactor::shared_ptr marginalFactor(new LinearContainerFactor(gaussianFactor, theta_));
marginalFactors.push_back(marginalFactor);
// Add the keys associated with the marginal factor to the separator values
BOOST_FOREACH(Key key, *marginalFactor) {
@ -551,7 +551,7 @@ NonlinearFactorGraph ConcurrentBatchFilter::marginalize(const NonlinearFactorGra
// Note: It is assumed the ordering already has these keys first
// Create the linear factor graph
GaussianFactorGraph linearFactorGraph = *graph.linearize(values, ordering);
GaussianFactorGraph linearFactorGraph = *graph.linearize(values);
// Construct a variable index
VariableIndex variableIndex(linearFactorGraph, ordering.size());
@ -576,7 +576,7 @@ NonlinearFactorGraph ConcurrentBatchFilter::marginalize(const NonlinearFactorGra
BOOST_FOREACH(Index index, indicesToEliminate) {
GaussianFactor::shared_ptr gaussianFactor = forest.at(index)->eliminateRecursive(function);
if(gaussianFactor->size() > 0) {
LinearContainerFactor::shared_ptr marginalFactor(new LinearContainerFactor(gaussianFactor, ordering, values));
LinearContainerFactor::shared_ptr marginalFactor(new LinearContainerFactor(gaussianFactor, values));
marginalFactors.push_back(marginalFactor);
}
}
@ -604,13 +604,13 @@ void ConcurrentBatchFilter::PrintNonlinearFactor(const NonlinearFactor::shared_p
}
/* ************************************************************************* */
void ConcurrentBatchFilter::PrintLinearFactor(const GaussianFactor::shared_ptr& factor, const Ordering& ordering,
void ConcurrentBatchFilter::PrintLinearFactor(const GaussianFactor::shared_ptr& factor,
const std::string& indent, const KeyFormatter& keyFormatter) {
std::cout << indent;
if(factor) {
std::cout << "g( ";
BOOST_FOREACH(Index index, *factor) {
std::cout << keyFormatter(ordering.key(index)) << " ";
BOOST_FOREACH(Index key, *factor) {
std::cout << keyFormatter(key) << " ";
}
std::cout << ")" << std::endl;
} else {

7
gtsam_unstable/nonlinear/ConcurrentBatchFilter.h
View File

@ -89,7 +89,7 @@ public:
* If only a single variable is needed, it may be faster to call calculateEstimate(const KEY&).
*/
Values calculateEstimate() const {
return theta_.retract(delta_, ordering_);
return theta_.retract(delta_);
}
/** Compute the current best estimate of a single variable. This is generally faster than
@ -99,8 +99,7 @@ public:
*/
template<class VALUE>
VALUE calculateEstimate(Key key) const {
const Index index = ordering_.at(key);
const Vector delta = delta_.at(index);
const Vector delta = delta_.at(key);
return theta_.at<VALUE>(key).retract(delta);
}
@ -213,7 +212,7 @@ private:
const std::string& indent = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter);
/** Print just the nonlinear keys in a linear factor */
static void PrintLinearFactor(const GaussianFactor::shared_ptr& factor, const Ordering& ordering,
static void PrintLinearFactor(const GaussianFactor::shared_ptr& factor,
const std::string& indent = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter);
// A custom elimination tree that supports forests and partial elimination

2
gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h
View File

@ -89,7 +89,7 @@ public:
* If only a single variable is needed, it may be faster to call calculateEstimate(const KEY&).
*/
Values calculateEstimate() const {
return theta_.retract(delta_, ordering_);
return theta_.retract(delta_);
}
/** Compute the current best estimate of a single variable. This is generally faster than

62
gtsam_unstable/nonlinear/LinearizedFactor.cpp
View File

@ -22,38 +22,31 @@
namespace gtsam {
/* ************************************************************************* */
LinearizedGaussianFactor::LinearizedGaussianFactor(const GaussianFactor::shared_ptr& gaussian, const Ordering& ordering, const Values& lin_points) {
LinearizedGaussianFactor::LinearizedGaussianFactor(
const GaussianFactor::shared_ptr& gaussian, const Values& lin_points)
: NonlinearFactor(gaussian->keys())
{
// Extract the keys and linearization points
BOOST_FOREACH(const Index& idx, gaussian->keys()) {
// find full symbol
if (idx < ordering.size()) {
Key key = ordering.key(idx);
// extract linearization point
assert(lin_points.exists(key));
this->lin_points_.insert(key, lin_points.at(key));
// store keys
this->keys_.push_back(key);
} else {
throw std::runtime_error("LinearizedGaussianFactor: could not find index in decoder!");
}
BOOST_FOREACH(const Key& key, gaussian->keys()) {
// extract linearization point
assert(lin_points.exists(key));
this->lin_points_.insert(key, lin_points.at(key));
}
}
/* ************************************************************************* */
// LinearizedJacobianFactor
/* ************************************************************************* */
LinearizedJacobianFactor::LinearizedJacobianFactor() : Ab_(matrix_) {
}
/* ************************************************************************* */
LinearizedJacobianFactor::LinearizedJacobianFactor(const JacobianFactor::shared_ptr& jacobian,
const Ordering& ordering, const Values& lin_points)
: Base(jacobian, ordering, lin_points), Ab_(matrix_) {
LinearizedJacobianFactor::LinearizedJacobianFactor(
const JacobianFactor::shared_ptr& jacobian, const Values& lin_points)
: Base(jacobian, lin_points), Ab_(matrix_) {
// Get the Ab matrix from the Jacobian factor, with any covariance baked in
AbMatrix fullMatrix = jacobian->matrix_augmented(true);
AbMatrix fullMatrix = jacobian->augmentedJacobian();
// Create the dims array
size_t *dims = (size_t *)alloca(sizeof(size_t) * (jacobian->size() + 1));
@ -110,12 +103,12 @@ double LinearizedJacobianFactor::error(const Values& c) const {
/* ************************************************************************* */
boost::shared_ptr<GaussianFactor>
LinearizedJacobianFactor::linearize(const Values& c, const Ordering& ordering) const {
LinearizedJacobianFactor::linearize(const Values& c) const {
// Create the 'terms' data structure for the Jacobian constructor
std::vector<std::pair<Index, Matrix> > terms;
BOOST_FOREACH(Key key, keys()) {
terms.push_back(std::make_pair(ordering[key], this->A(key)));
terms.push_back(std::make_pair(key, this->A(key)));
}
// compute rhs
@ -140,19 +133,16 @@ Vector LinearizedJacobianFactor::error_vector(const Values& c) const {
return errorVector;
}
/* ************************************************************************* */
// LinearizedHessianFactor
/* ************************************************************************* */
LinearizedHessianFactor::LinearizedHessianFactor() : info_(matrix_) {
}
/* ************************************************************************* */
LinearizedHessianFactor::LinearizedHessianFactor(const HessianFactor::shared_ptr& hessian,
const Ordering& ordering, const Values& lin_points)
: Base(hessian, ordering, lin_points), info_(matrix_) {
LinearizedHessianFactor::LinearizedHessianFactor(
const HessianFactor::shared_ptr& hessian, const Values& lin_points)
: Base(hessian, lin_points), info_(matrix_) {
// Copy the augmented matrix holding G, g, and f
Matrix fullMatrix = hessian->info();
@ -228,13 +218,7 @@ double LinearizedHessianFactor::error(const Values& c) const {
/* ************************************************************************* */
boost::shared_ptr<GaussianFactor>
LinearizedHessianFactor::linearize(const Values& c, const Ordering& ordering) const {
// Use the ordering to convert the keys into indices;
std::vector<Index> js;
BOOST_FOREACH(Key key, this->keys()){
js.push_back(ordering.at(key));
}
LinearizedHessianFactor::linearize(const Values& c) const {
// Make a copy of the info matrix
Matrix newMatrix;
@ -275,7 +259,7 @@ LinearizedHessianFactor::linearize(const Values& c, const Ordering& ordering) co
// Create a Hessian Factor from the modified info matrix
//return boost::shared_ptr<GaussianFactor>(new HessianFactor(js, newInfo));
return boost::shared_ptr<GaussianFactor>(new HessianFactor(js, Gs, gs, f));
return boost::shared_ptr<GaussianFactor>(new HessianFactor(keys(), Gs, gs, f));
}
} // \namespace aspn

18
gtsam_unstable/nonlinear/LinearizedFactor.h
View File

@ -19,7 +19,6 @@
#include <vector>
#include <gtsam_unstable/base/dllexport.h>
#include <gtsam/nonlinear/Ordering.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/HessianFactor.h>
@ -51,10 +50,9 @@ public:
/**
* @param gaussian: A jacobian or hessian factor
* @param ordering: The full ordering used to linearize this factor
* @param lin_points: The linearization points for, at least, the variables used by this factor
*/
LinearizedGaussianFactor(const GaussianFactor::shared_ptr& gaussian, const Ordering& ordering, const Values& lin_points);
LinearizedGaussianFactor(const GaussianFactor::shared_ptr& gaussian, const Values& lin_points);
virtual ~LinearizedGaussianFactor() {};
@ -111,11 +109,9 @@ public:
/**
* @param jacobian: A jacobian factor
* @param ordering: The ordering used to linearize this factor
* @param lin_points: The linearization points for, at least, the variables used by this factor
*/
LinearizedJacobianFactor(const JacobianFactor::shared_ptr& jacobian,
const Ordering& ordering, const Values& lin_points);
LinearizedJacobianFactor(const JacobianFactor::shared_ptr& jacobian, const Values& lin_points);
virtual ~LinearizedJacobianFactor() {}
@ -148,8 +144,7 @@ public:
* Reimplemented from NoiseModelFactor to directly copy out the
* matrices and only update the RHS b with an updated residual
*/
boost::shared_ptr<GaussianFactor> linearize(
const Values& c, const Ordering& ordering) const;
boost::shared_ptr<GaussianFactor> linearize(const Values& c) const;
/** (A*x-b) */
Vector error_vector(const Values& c) const;
@ -204,11 +199,9 @@ public:
/**
* Use this constructor with the ordering used to linearize the graph
* @param hessian: A hessian factor
* @param ordering: The ordering used to linearize this factor
* @param lin_points: The linearization points for, at least, the variables used by this factor
*/
LinearizedHessianFactor(const HessianFactor::shared_ptr& hessian,
const Ordering& ordering, const Values& lin_points);
LinearizedHessianFactor(const HessianFactor::shared_ptr& hessian, const Values& lin_points);
virtual ~LinearizedHessianFactor() {}
@ -270,8 +263,7 @@ public:
* Reimplemented from NoiseModelFactor to directly copy out the
* matrices and only update the RHS b with an updated residual
*/
boost::shared_ptr<GaussianFactor> linearize(
const Values& c, const Ordering& ordering) const;
boost::shared_ptr<GaussianFactor> linearize(const Values& c) const;
private:
/** Serialization function */

34
gtsam_unstable/nonlinear/sequentialSummarization.cpp
View File

@ -1,34 +0,0 @@
/**
* @file summarization.cpp
*
* @date Jun 22, 2012
* @author Alex Cunningham
*/
#include <gtsam_unstable/nonlinear/sequentialSummarization.h>
#include <boost/foreach.hpp>
using namespace std;
namespace gtsam {
/* ************************************************************************* */
GaussianFactorGraph::shared_ptr summarizeGraphSequential(
const GaussianFactorGraph& full_graph, const std::vector<Index>& indices, bool useQR) {
GaussianSequentialSolver solver(full_graph, useQR);
return solver.jointFactorGraph(indices);
}
/* ************************************************************************* */
GaussianFactorGraph::shared_ptr summarizeGraphSequential(
const GaussianFactorGraph& full_graph, const Ordering& ordering, const KeySet& saved_keys, bool useQR) {
std::vector<Index> indices;
BOOST_FOREACH(const Key& k, saved_keys)
indices.push_back(ordering[k]);
return summarizeGraphSequential(full_graph, indices, useQR);
}
/* ************************************************************************* */
} // \namespace gtsam

34
gtsam_unstable/nonlinear/sequentialSummarization.h
View File

@ -1,34 +0,0 @@
/**
* @file summarization.h
*
* @brief Types and utility functions for summarization
*
* @date Jun 22, 2012
* @author Alex Cunningham
*/
#pragma once
#include <gtsam_unstable/base/dllexport.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/nonlinear/Ordering.h>
namespace gtsam {
/**
* Summarization function that eliminates a set of variables (does not convert to Jacobians)
* NOTE: uses sequential solver - requires fully constrained system
*/
GaussianFactorGraph::shared_ptr GTSAM_UNSTABLE_EXPORT summarizeGraphSequential(
const GaussianFactorGraph& full_graph, const std::vector<Index>& indices, bool useQR = false);
/** Summarization that also converts keys to indices */
GaussianFactorGraph::shared_ptr GTSAM_UNSTABLE_EXPORT summarizeGraphSequential(
const GaussianFactorGraph& full_graph, const Ordering& ordering,
const KeySet& saved_keys, bool useQR = false);
} // \namespace gtsam

70
gtsam_unstable/nonlinear/tests/testLinearizedFactor.cpp
View File

@ -33,9 +33,6 @@ TEST( LinearizedFactor, equals_jacobian )
// Create a Between Factor from a Point3. This is actually a linear factor.
Key x1(1);
Key x2(2);
Ordering ordering;
ordering.push_back(x1);
ordering.push_back(x2);
Values values;
values.insert(x1, Point3(-22.4, +8.5, +2.4));
values.insert(x2, Point3(-21.0, +5.0, +21.0));
@ -46,9 +43,9 @@ TEST( LinearizedFactor, equals_jacobian )
// Create two identical factors and make sure they're equal
JacobianFactor::shared_ptr jf = boost::static_pointer_cast<JacobianFactor>(betweenFactor.linearize(values, ordering));
LinearizedJacobianFactor jacobian1(jf, ordering, values);
LinearizedJacobianFactor jacobian2(jf, ordering, values);
JacobianFactor::shared_ptr jf = boost::static_pointer_cast<JacobianFactor>(betweenFactor.linearize(values));
LinearizedJacobianFactor jacobian1(jf, values);
LinearizedJacobianFactor jacobian2(jf, values);
CHECK(assert_equal(jacobian1, jacobian2));
}
@ -59,9 +56,6 @@ TEST( LinearizedFactor, clone_jacobian )
// Create a Between Factor from a Point3. This is actually a linear factor.
Key x1(1);
Key x2(2);
Ordering ordering;
ordering.push_back(x1);
ordering.push_back(x2);
Values values;
values.insert(x1, Point3(-22.4, +8.5, +2.4));
values.insert(x2, Point3(-21.0, +5.0, +21.0));
@ -71,13 +65,13 @@ TEST( LinearizedFactor, clone_jacobian )
BetweenFactor<Point3> betweenFactor(x1, x2, measured, model);
// Create one factor that is a clone of the other and make sure they're equal
JacobianFactor::shared_ptr jf = boost::static_pointer_cast<JacobianFactor>(betweenFactor.linearize(values, ordering));
LinearizedJacobianFactor jacobian1(jf, ordering, values);
JacobianFactor::shared_ptr jf = boost::static_pointer_cast<JacobianFactor>(betweenFactor.linearize(values));
LinearizedJacobianFactor jacobian1(jf, values);
LinearizedJacobianFactor::shared_ptr jacobian2 = boost::static_pointer_cast<LinearizedJacobianFactor>(jacobian1.clone());
CHECK(assert_equal(jacobian1, *jacobian2));
JacobianFactor::shared_ptr jf1 = boost::static_pointer_cast<JacobianFactor>(jacobian1.linearize(values, ordering));
JacobianFactor::shared_ptr jf2 = boost::static_pointer_cast<JacobianFactor>(jacobian2->linearize(values, ordering));
JacobianFactor::shared_ptr jf1 = boost::static_pointer_cast<JacobianFactor>(jacobian1.linearize(values));
JacobianFactor::shared_ptr jf2 = boost::static_pointer_cast<JacobianFactor>(jacobian2->linearize(values));
CHECK(assert_equal(*jf1, *jf2));
Matrix information1 = jf1->augmentedInformation();
@ -91,9 +85,6 @@ TEST( LinearizedFactor, add_jacobian )
// Create a Between Factor from a Point3. This is actually a linear factor.
Key x1(1);
Key x2(2);
Ordering ordering;
ordering.push_back(x1);
ordering.push_back(x2);
Values values;
values.insert(x1, Point3(-22.4, +8.5, +2.4));
values.insert(x2, Point3(-21.0, +5.0, +21.0));
@ -103,10 +94,10 @@ TEST( LinearizedFactor, add_jacobian )
BetweenFactor<Point3> betweenFactor(x1, x2, measured, model);
// Create two factor graphs, one using 'push_back' and one using 'add' and make sure they're equal
JacobianFactor::shared_ptr jf = boost::static_pointer_cast<JacobianFactor>(betweenFactor.linearize(values, ordering));
LinearizedJacobianFactor::shared_ptr jacobian(new LinearizedJacobianFactor(jf, ordering, values));
JacobianFactor::shared_ptr jf = boost::static_pointer_cast<JacobianFactor>(betweenFactor.linearize(values));
LinearizedJacobianFactor::shared_ptr jacobian(new LinearizedJacobianFactor(jf, values));
NonlinearFactorGraph graph1; graph1.push_back(jacobian);
NonlinearFactorGraph graph2; graph2.add(*jacobian);
NonlinearFactorGraph graph2; graph2.push_back(*jacobian);
// TODO: When creating a Jacobian from a cached factor, I experienced a problem in the 'add' version
// However, I am currently unable to reproduce the error in this unit test.
@ -133,8 +124,8 @@ TEST( LinearizedFactor, add_jacobian )
//
//
// // Create a linearized jacobian factors
// JacobianFactor::shared_ptr jf = boost::static_pointer_cast<JacobianFactor>(betweenFactor.linearize(values, ordering));
// LinearizedJacobianFactor jacobian(jf, ordering, values);
// JacobianFactor::shared_ptr jf = boost::static_pointer_cast<JacobianFactor>(betweenFactor.linearize(values));
// LinearizedJacobianFactor jacobian(jf, values);
//
//
// for(double x1 = -10; x1 < 10; x1 += 2.0) {
@ -156,8 +147,8 @@ TEST( LinearizedFactor, add_jacobian )
// EXPECT_DOUBLES_EQUAL(expected_error, actual_error, 1e-9 );
//
// // Check that the linearized factors are identical
// GaussianFactor::shared_ptr expected_factor = betweenFactor.linearize(linpoint, ordering);
// GaussianFactor::shared_ptr actual_factor = jacobian.linearize(linpoint, ordering);
// GaussianFactor::shared_ptr expected_factor = betweenFactor.linearize(linpoint);
// GaussianFactor::shared_ptr actual_factor = jacobian.linearize(linpoint);
// CHECK(assert_equal(*expected_factor, *actual_factor));
// }
// }
@ -175,9 +166,6 @@ TEST( LinearizedFactor, equals_hessian )
// Create a Between Factor from a Point3. This is actually a linear factor.
Key x1(1);
Key x2(2);
Ordering ordering;
ordering.push_back(x1);
ordering.push_back(x2);
Values values;
values.insert(x1, Point3(-22.4, +8.5, +2.4));
values.insert(x2, Point3(-21.0, +5.0, +21.0));
@ -188,10 +176,10 @@ TEST( LinearizedFactor, equals_hessian )
// Create two identical factors and make sure they're equal
JacobianFactor::shared_ptr jf = boost::static_pointer_cast<JacobianFactor>(betweenFactor.linearize(values, ordering));
JacobianFactor::shared_ptr jf = boost::static_pointer_cast<JacobianFactor>(betweenFactor.linearize(values));
HessianFactor::shared_ptr hf(new HessianFactor(*jf));
LinearizedHessianFactor hessian1(hf, ordering, values);
LinearizedHessianFactor hessian2(hf, ordering, values);
LinearizedHessianFactor hessian1(hf, values);
LinearizedHessianFactor hessian2(hf, values);
CHECK(assert_equal(hessian1, hessian2));
}
@ -202,9 +190,6 @@ TEST( LinearizedFactor, clone_hessian )
// Create a Between Factor from a Point3. This is actually a linear factor.
Key x1(1);
Key x2(2);
Ordering ordering;
ordering.push_back(x1);
ordering.push_back(x2);
Values values;
values.insert(x1, Point3(-22.4, +8.5, +2.4));
values.insert(x2, Point3(-21.0, +5.0, +21.0));
@ -215,9 +200,9 @@ TEST( LinearizedFactor, clone_hessian )
// Create two identical factors and make sure they're equal
JacobianFactor::shared_ptr jf = boost::static_pointer_cast<JacobianFactor>(betweenFactor.linearize(values, ordering));
JacobianFactor::shared_ptr jf = boost::static_pointer_cast<JacobianFactor>(betweenFactor.linearize(values));
HessianFactor::shared_ptr hf(new HessianFactor(*jf));
LinearizedHessianFactor hessian1(hf, ordering, values);
LinearizedHessianFactor hessian1(hf, values);
LinearizedHessianFactor::shared_ptr hessian2 = boost::static_pointer_cast<LinearizedHessianFactor>(hessian1.clone());
CHECK(assert_equal(hessian1, *hessian2));
@ -229,9 +214,6 @@ TEST( LinearizedFactor, add_hessian )
// Create a Between Factor from a Point3. This is actually a linear factor.
Key x1(1);
Key x2(2);
Ordering ordering;
ordering.push_back(x1);
ordering.push_back(x2);
Values values;
values.insert(x1, Point3(-22.4, +8.5, +2.4));
values.insert(x2, Point3(-21.0, +5.0, +21.0));
@ -242,11 +224,11 @@ TEST( LinearizedFactor, add_hessian )
// Create two identical factors and make sure they're equal
JacobianFactor::shared_ptr jf = boost::static_pointer_cast<JacobianFactor>(betweenFactor.linearize(values, ordering));
JacobianFactor::shared_ptr jf = boost::static_pointer_cast<JacobianFactor>(betweenFactor.linearize(values));
HessianFactor::shared_ptr hf(new HessianFactor(*jf));
LinearizedHessianFactor::shared_ptr hessian(new LinearizedHessianFactor(hf, ordering, values));
LinearizedHessianFactor::shared_ptr hessian(new LinearizedHessianFactor(hf, values));
NonlinearFactorGraph graph1; graph1.push_back(hessian);
NonlinearFactorGraph graph2; graph2.add(*hessian);
NonlinearFactorGraph graph2; graph2.push_back(*hessian);
CHECK(assert_equal(graph1, graph2));
}
@ -270,9 +252,9 @@ TEST( LinearizedFactor, add_hessian )
//
//
// // Create a linearized hessian factor
// JacobianFactor::shared_ptr jf = boost::static_pointer_cast<JacobianFactor>(betweenFactor.linearize(values, ordering));
// JacobianFactor::shared_ptr jf = boost::static_pointer_cast<JacobianFactor>(betweenFactor.linearize(values));
// HessianFactor::shared_ptr hf(new HessianFactor(*jf));
// LinearizedHessianFactor hessian(hf, ordering, values);
// LinearizedHessianFactor hessian(hf, values);
//
//
// for(double x1 = -10; x1 < 10; x1 += 2.0) {
@ -294,8 +276,8 @@ TEST( LinearizedFactor, add_hessian )
// EXPECT_DOUBLES_EQUAL(expected_error, actual_error, 1e-9 );
//
// // Check that the linearized factors are identical
// GaussianFactor::shared_ptr expected_factor = HessianFactor::shared_ptr(new HessianFactor(*betweenFactor.linearize(linpoint, ordering)));
// GaussianFactor::shared_ptr actual_factor = hessian.linearize(linpoint, ordering);
// GaussianFactor::shared_ptr expected_factor = HessianFactor::shared_ptr(new HessianFactor(*betweenFactor.linearize(linpoint)));
// GaussianFactor::shared_ptr actual_factor = hessian.linearize(linpoint);
// CHECK(assert_equal(*expected_factor, *actual_factor));
// }
// }

10
gtsam_unstable/slam/CMakeLists.txt
View File

@ -25,7 +25,15 @@ set (slam_full_libs
# Exclude tests that don't work
set (slam_excluded_tests
"${CMAKE_CURRENT_SOURCE_DIR}/tests/testSerialization.cpp"
"${CMAKE_CURRENT_SOURCE_DIR}/tests/testAHRS.cpp"
"${CMAKE_CURRENT_SOURCE_DIR}/tests/testCombinedImuFactor.cpp"
"${CMAKE_CURRENT_SOURCE_DIR}/tests/testImuFactor.cpp"
"${CMAKE_CURRENT_SOURCE_DIR}/tests/testInvDepthFactor3.cpp"
"${CMAKE_CURRENT_SOURCE_DIR}/tests/testInvDepthFactorVariant1.cpp"
"${CMAKE_CURRENT_SOURCE_DIR}/tests/testInvDepthFactorVariant2.cpp"
"${CMAKE_CURRENT_SOURCE_DIR}/tests/testInvDepthFactorVariant3.cpp"
"${CMAKE_CURRENT_SOURCE_DIR}/tests/testMultiProjectionFactor.cpp"
"${CMAKE_CURRENT_SOURCE_DIR}/tests/testSerialization.cpp"
# "" # Add to this list, with full path, to exclude
)
# Add all tests

9
gtsam_unstable/slam/DummyFactor.cpp
View File

@ -7,13 +7,16 @@
#include <gtsam_unstable/slam/DummyFactor.h>
#include <boost/assign/list_of.hpp>
#include <boost/foreach.hpp>
using namespace boost::assign;
namespace gtsam {
/* ************************************************************************* */
DummyFactor::DummyFactor(const Key& key1, size_t dim1, const Key& key2, size_t dim2)
: NonlinearFactor(key1, key2)
: NonlinearFactor(cref_list_of<2>(key1)(key2))
{
dims_.push_back(dim1);
dims_.push_back(dim2);
@ -38,7 +41,7 @@ bool DummyFactor::equals(const NonlinearFactor& f, double tol) const {
/* ************************************************************************* */
boost::shared_ptr<GaussianFactor>
DummyFactor::linearize(const Values& c, const Ordering& ordering) const {
DummyFactor::linearize(const Values& c) const {
// Only linearize if the factor is active
if (!this->active(c))
return boost::shared_ptr<JacobianFactor>();
@ -46,7 +49,7 @@ DummyFactor::linearize(const Values& c, const Ordering& ordering) const {
// Fill in terms with zero matrices
std::vector<std::pair<Index, Matrix> > terms(this->size());
for(size_t j=0; j<this->size(); ++j) {
terms[j].first = ordering[this->keys()[j]];
terms[j].first = keys()[j];
terms[j].second = zeros(rowDim_, dims_[j]);
}

3
gtsam_unstable/slam/DummyFactor.h
View File

@ -54,8 +54,7 @@ public:
virtual size_t dim() const { return rowDim_; }
/** linearize to a GaussianFactor */
virtual boost::shared_ptr<GaussianFactor>
linearize(const Values& c, const Ordering& ordering) const;
virtual boost::shared_ptr<GaussianFactor> linearize(const Values& c) const;
/**
* Creates a shared_ptr clone of the factor - needs to be specialized to allow

9
gtsam_unstable/slam/SmartProjectionFactor.h
View File

@ -147,7 +147,7 @@ namespace gtsam {
}
/// linearize returns a Hessianfactor that is an approximation of error(p)
virtual boost::shared_ptr<GaussianFactor> linearize(const Values& values, const Ordering& ordering) const {
virtual boost::shared_ptr<GaussianFactor> linearize(const Values& values) const {
// std::cout.precision(20);
@ -171,11 +171,6 @@ namespace gtsam {
std::vector<Matrix> Gs(keys_.size()*(keys_.size()+1)/2);
std::vector<Vector> gs(keys_.size());
double f = 0;
// fill in the keys
std::vector<Index> js;
BOOST_FOREACH(const Key& k, keys_) {
js += ordering[k];
}
bool blockwise = false;
@ -336,7 +331,7 @@ namespace gtsam {
// ==========================================================================================================
return HessianFactor::shared_ptr(new HessianFactor(js, Gs, gs, f));
return HessianFactor::shared_ptr(new HessianFactor(keys_, Gs, gs, f));
}
/**

1
gtsam_unstable/slam/SmartRangeFactor.h
View File

@ -13,6 +13,7 @@
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/inference/Key.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/base/timing.h>
#include <boost/foreach.hpp>
#include <map>

12
gtsam_unstable/slam/tests/testDummyFactor.cpp
View File

@ -10,10 +10,11 @@
#include <CppUnitLite/TestHarness.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam_unstable/slam/DummyFactor.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/base/TestableAssertions.h>
using namespace gtsam;
const double tol = 1e-9;
@ -41,15 +42,12 @@ TEST( testDummyFactor, basics ) {
// errors - all zeros
DOUBLES_EQUAL(0.0, dummyfactor.error(c), tol);
Ordering ordering;
ordering += key1, key2;
// linearization: all zeros
GaussianFactor::shared_ptr actLinearization = dummyfactor.linearize(c, ordering);
GaussianFactor::shared_ptr actLinearization = dummyfactor.linearize(c);
CHECK(actLinearization);
noiseModel::Diagonal::shared_ptr model3 = noiseModel::Unit::Create(3);
GaussianFactor::shared_ptr expLinearization(new JacobianFactor(
ordering[key1], zeros(3,3), ordering[key2], zeros(3,3), zero(3), model3));
key1, zeros(3,3), key2, zeros(3,3), zero(3), model3));
EXPECT(assert_equal(*expLinearization, *actLinearization, tol));
}

9
gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp
View File

@ -25,11 +25,8 @@
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LinearContainerFactor.h>
#include <gtsam/nonlinear/Ordering.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/nonlinear/Key.h>
#include <gtsam/linear/GaussianSequentialSolver.h>
#include <gtsam/inference/JunctionTree.h>
#include <gtsam_unstable/geometry/triangulation.h>
#include <gtsam/geometry/Pose3.h>
@ -40,8 +37,10 @@
#include <gtsam/geometry/SimpleCamera.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/assign/std/vector.hpp>
using namespace std;
using namespace boost::assign;
using namespace gtsam;
// make a realistic calibration matrix
@ -223,8 +222,8 @@ TEST( MultiProjectionFactor, 3poses ){
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
graph.add(PriorFactor<Pose3>(x1, pose1, noisePrior));
graph.add(PriorFactor<Pose3>(x2, pose2, noisePrior));
graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
graph.push_back(PriorFactor<Pose3>(x2, pose2, noisePrior));
// smartFactor1->print("smartFactor1");

6
gtsam_unstable/slam/tests/testSmartRangeFactor.cpp
View File

@ -113,11 +113,11 @@ TEST( SmartRangeFactor, optimization ) {
// Create Factor graph
NonlinearFactorGraph graph;
graph.add(f);
graph.push_back(f);
const noiseModel::Base::shared_ptr //
priorNoise = noiseModel::Diagonal::Sigmas(Vector3(1, 1, M_PI));
graph.add(PriorFactor<Pose2>(1, pose1, priorNoise));
graph.add(PriorFactor<Pose2>(2, pose2, priorNoise));
graph.push_back(PriorFactor<Pose2>(1, pose1, priorNoise));
graph.push_back(PriorFactor<Pose2>(2, pose2, priorNoise));
// Try optimizing
LevenbergMarquardtParams params;