Matrix version of sparseJacobian for MATLAB wrapping, unit-tested in linear now
Frank Dellaert
parent
58d83f704f
commit
65616dbde5
84
.cproject
84
.cproject
|
|
@ -769,6 +769,14 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</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">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
|
|
@ -905,10 +913,10 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</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>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testTupleConfig.run</buildTarget>
|
||||
<buildTarget>testTupleValues.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
|
|
@ -1664,10 +1672,10 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</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>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>tests/testHessianFactor.run</buildTarget>
|
||||
<buildTarget>tests/testJacobianFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
|
|
@ -1704,6 +1712,14 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</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">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
|
|
@ -1712,10 +1728,10 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</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>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>CameraResectioning</buildTarget>
|
||||
<buildTarget>SLAMSelfContained.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
|
|
@ -1736,54 +1752,6 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</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">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
|
|
@ -1960,6 +1928,14 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</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">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
|
|
|
|||
|
|
@ -82,7 +82,8 @@ namespace gtsam {
|
|||
/* ************************************************************************* */
|
||||
std::vector<boost::tuple<size_t, size_t, double> > GaussianFactorGraph::sparseJacobian(
|
||||
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;
|
||||
BOOST_FOREACH(const sharedFactor& factor, *this) {
|
||||
// Convert to JacobianFactor if necessary
|
||||
|
|
@ -99,13 +100,13 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
// Add entries, adjusting the row index i
|
||||
std::vector<boost::tuple<size_t, size_t, double> > factorEntries(
|
||||
std::vector<triplet> factorEntries(
|
||||
jacobianFactor->sparse(columnIndices));
|
||||
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(
|
||||
factorEntries[entry].get<0> () + i, factorEntries[entry].get<
|
||||
1> (), factorEntries[entry].get<2> ()));
|
||||
factorEntries[k].get<0> () + i, factorEntries[k].get<
|
||||
1> (), factorEntries[k].get<2> ()));
|
||||
|
||||
// Increment row index
|
||||
i += jacobianFactor->rows();
|
||||
|
|
@ -113,6 +114,31 @@ namespace gtsam {
|
|||
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 {
|
||||
// combine all factors
|
||||
|
|
|
|||
|
|
@ -149,12 +149,21 @@ namespace gtsam {
|
|||
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,
|
||||
* 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
|
||||
* @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;
|
||||
|
||||
/**
|
||||
* 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 first column index for each variable
|
||||
* @param columnIndices First column index for each variable, base 1, in vector format.
|
||||
*/
|
||||
std::vector<boost::tuple<size_t,size_t,double> > sparseJacobian(const std::vector<size_t>& columnIndices) const;
|
||||
Matrix sparse(const Vector& columnIndices) const;
|
||||
|
||||
/**
|
||||
* Return a dense \f$ m \times n \f$ Jacobian matrix, augmented with b
|
||||
|
|
|
|||
|
|
@ -39,6 +39,33 @@ static SharedDiagonal
|
|||
sigma0_1 = sharedSigma(2,0.1), sigma_02 = sharedSigma(2,0.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
|
||||
TEST(GaussianFactor, Combine)
|
||||
|
|
@ -545,24 +572,6 @@ TEST(GaussianFactor, permuteWithInverse)
|
|||
#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);}
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -413,25 +413,6 @@ TEST( GaussianFactorGraph, copying )
|
|||
// 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 )
|
||||
{
|
||||
|
|
|
|||
Loading…
Reference in New Issue