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

Matrix version of sparseJacobian for MATLAB wrapping, unit-tested in linear now

release/4.3a0
Frank Dellaert 2011-10-29 04:01:03 +00:00
parent 58d83f704f
commit 65616dbde5
5 changed files with 103 additions and 102 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

84
.cproject
View File

@ -769,6 +769,14 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="tests/testSimulated2D.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>tests/testSimulated2D.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="all" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j2</buildArguments>
@ -905,10 +913,10 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testTupleConfig.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testTupleValues.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j2</buildArguments>
<buildTarget>testTupleConfig.run</buildTarget> <buildTarget>testTupleValues.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
@ -1664,10 +1672,10 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="tests/testHessianFactor.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="tests/testJacobianFactor.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j2</buildArguments>
<buildTarget>tests/testHessianFactor.run</buildTarget> <buildTarget>tests/testJacobianFactor.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
@ -1704,6 +1712,14 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="tests/testHessianFactor.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>tests/testHessianFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="SimpleRotation.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="SimpleRotation.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j2</buildArguments>
@ -1712,10 +1728,10 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="CameraResectioning" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="SLAMSelfContained.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j2</buildArguments>
<buildTarget>CameraResectioning</buildTarget> <buildTarget>SLAMSelfContained.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
@ -1736,54 +1752,6 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="easyPoint2KalmanFilter.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>easyPoint2KalmanFilter.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="elaboratePoint2KalmanFilter.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>elaboratePoint2KalmanFilter.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="PlanarSLAMSelfContained_advanced.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>PlanarSLAMSelfContained_advanced.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="Pose2SLAMExample_advanced.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>Pose2SLAMExample_advanced.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="Pose2SLAMExample_easy.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>Pose2SLAMExample_easy.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="Pose2SLAMwSPCG_easy.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>Pose2SLAMwSPCG_easy.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="check" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j2</buildArguments>
@ -1960,6 +1928,14 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="install" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>install</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="check" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j2</buildArguments>

36
gtsam/linear/GaussianFactorGraph.cpp
View File

@ -82,7 +82,8 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
std::vector<boost::tuple<size_t, size_t, double> > GaussianFactorGraph::sparseJacobian( std::vector<boost::tuple<size_t, size_t, double> > GaussianFactorGraph::sparseJacobian(
const std::vector<size_t>& columnIndices) const { const std::vector<size_t>& columnIndices) const {
std::vector<boost::tuple<size_t, size_t, double> > entries; typedef boost::tuple<size_t, size_t, double> triplet;
std::vector<triplet> entries;
size_t i = 0; size_t i = 0;
BOOST_FOREACH(const sharedFactor& factor, *this) { BOOST_FOREACH(const sharedFactor& factor, *this) {
// Convert to JacobianFactor if necessary // Convert to JacobianFactor if necessary
@ -99,13 +100,13 @@ namespace gtsam {
} }
// Add entries, adjusting the row index i // Add entries, adjusting the row index i
std::vector<boost::tuple<size_t, size_t, double> > factorEntries( std::vector<triplet> factorEntries(
jacobianFactor->sparse(columnIndices)); jacobianFactor->sparse(columnIndices));
entries.reserve(entries.size() + factorEntries.size()); entries.reserve(entries.size() + factorEntries.size());
for (size_t entry = 0; entry < factorEntries.size(); ++entry) for (size_t k = 0; k < factorEntries.size(); ++k)
entries.push_back(boost::make_tuple( entries.push_back(boost::make_tuple(
factorEntries[entry].get<0> () + i, factorEntries[entry].get< factorEntries[k].get<0> () + i, factorEntries[k].get<
1> (), factorEntries[entry].get<2> ())); 1> (), factorEntries[k].get<2> ()));
// Increment row index // Increment row index
i += jacobianFactor->rows(); i += jacobianFactor->rows();
@ -113,6 +114,31 @@ namespace gtsam {
return entries; return entries;
} }
/* ************************************************************************* */
Matrix GaussianFactorGraph::sparse(const Vector& columnIndices) const {
// translate from base 1 Vector to vector of base 0 indices
std::vector<size_t> indices;
for (int i = 0; i < columnIndices.size(); i++)
indices.push_back(columnIndices[i] - 1);
// call sparseJacobian
typedef boost::tuple<size_t, size_t, double> triplet;
std::vector < boost::tuple<size_t, size_t, double> > result =
sparseJacobian(indices);
// translate to base 1 matrix
size_t nzmax = result.size();
Matrix IJS(3,nzmax);
for (size_t k = 0; k < result.size(); k++) {
const triplet& entry = result[k];
IJS(0,k) = entry.get<0>() + 1;
IJS(1,k) = entry.get<1>() + 1;
IJS(2,k) = entry.get<2>();
}
return IJS;
}
/* ************************************************************************* */ /* ************************************************************************* */
Matrix GaussianFactorGraph::denseJacobian() const { Matrix GaussianFactorGraph::denseJacobian() const {
// combine all factors // combine all factors

21
gtsam/linear/GaussianFactorGraph.h
View File

@ -149,12 +149,21 @@ namespace gtsam {
void combine(const GaussianFactorGraph &lfg); void combine(const GaussianFactorGraph &lfg);
/** /**
* Return vector of i, j, and s to generate an m-by-n sparse Jacobian matrix * Return vector of i, j, and s to generate an m-by-n sparse Jacobian matrix,
* such that S(i(k),j(k)) = s(k), which can be given to MATLAB's sparse. * where i(k) and j(k) are the base 0 row and column indices, s(k) a double.
* The standard deviations are baked into A and b * The standard deviations are baked into A and b
* @param first column index for each variable * @param columnIndices First column index for each variable.
*/ */
std::vector<boost::tuple<size_t,size_t,double> > sparseJacobian(const std::vector<size_t>& columnIndices) const; std::vector<boost::tuple<size_t, size_t, double> > sparseJacobian(
const std::vector<size_t>& columnIndices) const;
/**
* Matrix version of sparseJacobian: generates a 3*m matrix with [i,j,s] entries
* such that S(i(k),j(k)) = s(k), which can be given to MATLAB's sparse.
* The standard deviations are baked into A and b
* @param columnIndices First column index for each variable, base 1, in vector format.
*/
Matrix sparse(const Vector& columnIndices) const;
/** /**
* Return a dense \f$ m \times n \f$ Jacobian matrix, augmented with b * Return a dense \f$ m \times n \f$ Jacobian matrix, augmented with b

45
gtsam/linear/tests/testGaussianFactorGraph.cpp
View File

@ -39,6 +39,33 @@ static SharedDiagonal
sigma0_1 = sharedSigma(2,0.1), sigma_02 = sharedSigma(2,0.2), sigma0_1 = sharedSigma(2,0.1), sigma_02 = sharedSigma(2,0.2),
constraintModel = noiseModel::Constrained::All(2); constraintModel = noiseModel::Constrained::All(2);
/* ************************************************************************* */
TEST(GaussianFactorGraph, initialization) {
// Create empty graph
GaussianFactorGraph fg;
SharedDiagonal unit2 = noiseModel::Unit::Create(2);
fg.add(0, 10*eye(2), -1.0*ones(2), unit2);
fg.add(0, -10*eye(2),1, 10*eye(2), Vector_(2, 2.0, -1.0), unit2);
fg.add(0, -5*eye(2), 2, 5*eye(2), Vector_(2, 0.0, 1.0), unit2);
fg.add(1, -5*eye(2), 2, 5*eye(2), Vector_(2, -1.0, 1.5), unit2);
FactorGraph<JacobianFactor> graph = *fg.dynamicCastFactors<FactorGraph<JacobianFactor> >();
EXPECT_LONGS_EQUAL(4, graph.size());
JacobianFactor factor = *graph[0];
// Test sparse, which takes a vector and returns a matrix, used in MATLAB
Matrix expectedIJS = Matrix_(3,22,
1., 2., 1., 2., 3., 4., 3., 4., 3., 4., 5., 6., 5., 6., 5., 6., 7., 8., 7., 8., 7., 8.,
1., 2., 5., 5., 1., 2., 3., 4., 5., 5., 1., 2., 5., 6., 5., 5., 3., 4., 5., 6., 5., 5.,
10., 10., -1., -1., -10., -10., 10., 10., 2., -1., -5., -5., 5., 5., 0., 1., -5., -5., 5., 5., -1., 1.5
);
Vector columnIndices = Vector_(3,1.0,3.0,5.0);
Matrix actualIJS = fg.sparse(columnIndices);
EQUALITY(expectedIJS, actualIJS);
}
/* ************************************************************************* */ /* ************************************************************************* */
#ifdef BROKEN #ifdef BROKEN
TEST(GaussianFactor, Combine) TEST(GaussianFactor, Combine)
@ -545,24 +572,6 @@ TEST(GaussianFactor, permuteWithInverse)
#endif #endif
} }
/* ************************************************************************* */
TEST(GaussianFactorGraph, initialization) {
// Create empty graph
GaussianFactorGraph fg;
SharedDiagonal unit2 = noiseModel::Unit::Create(2);
// ordering x1, x2, l1 - build system in smallExample.cpp: createGaussianFactorGraph()
fg.add(0, 10*eye(2), -1.0*ones(2), unit2);
fg.add(0, -10*eye(2),1, 10*eye(2), Vector_(2, 2.0, -1.0), unit2);
fg.add(0, -5*eye(2), 2, 5*eye(2), Vector_(2, 0.0, 1.0), unit2);
fg.add(1, -5*eye(2), 2, 5*eye(2), Vector_(2, -1.0, 1.5), unit2);
FactorGraph<JacobianFactor> graph = *fg.dynamicCastFactors<FactorGraph<JacobianFactor> >();
EXPECT_LONGS_EQUAL(4, graph.size());
JacobianFactor factor = *graph[0];
}
/* ************************************************************************* */ /* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);} int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */ /* ************************************************************************* */

19
tests/testGaussianFactorGraph.cpp
View File

@ -413,25 +413,6 @@ TEST( GaussianFactorGraph, copying )
// EXPECT(6 == mn.second); // EXPECT(6 == mn.second);
//} //}
///* ************************************************************************* */
//SL-FIX TEST( GaussianFactorGraph, sparse )
//{
// // create a small linear factor graph
// GaussianFactorGraph fg = createGaussianFactorGraph();
//
// // render with a given ordering
// Ordering ord;
// ord += "x2","l1","x1";
//
// Matrix ijs = fg.sparse(ord);
//
// EQUALITY(Matrix_(3, 14,
// // f(x1) f(x2,x1) f(l1,x1) f(x2,l1)
// +1., 2., 3., 4., 3., 4., 5.,6., 5., 6., 7., 8., 7., 8.,
// +5., 6., 5., 6., 1., 2., 3.,4., 5., 6., 3., 4., 1., 2.,
// 10.,10., -10.,-10., 10., 10., 5.,5.,-5.,-5., 5., 5.,-5.,-5.), ijs);
//}
/* ************************************************************************* */ /* ************************************************************************* */
TEST( GaussianFactorGraph, CONSTRUCTOR_GaussianBayesNet ) TEST( GaussianFactorGraph, CONSTRUCTOR_GaussianBayesNet )
{ {