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

SUPERFAST linearize, by pre-allocating the JacobianFactor and writing into it. 

Merging local branch 'feature/BAD_WriteableJacobianFactor' into feature/BAD
release/4.3a0
dellaert 2014-11-02 13:00:08 +01:00
parent a1c6c3cbe5 049631e530
commit d4dcf90bd1
10 changed files with 130 additions and 104 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

106
.cproject
View File

@ -600,7 +600,6 @@
</target>
<target name="tests/testBayesTree.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testBayesTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -608,7 +607,6 @@
</target>
<target name="testBinaryBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testBinaryBayesNet.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -656,7 +654,6 @@
</target>
<target name="testSymbolicBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicBayesNet.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -664,7 +661,6 @@
</target>
<target name="tests/testSymbolicFactor.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testSymbolicFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -672,7 +668,6 @@
</target>
<target name="testSymbolicFactorGraph.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -688,7 +683,6 @@
</target>
<target name="tests/testBayesTree" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testBayesTree</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1120,7 +1114,6 @@
</target>
<target name="testErrors.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testErrors.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1350,46 +1343,6 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testBTree.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testBTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDSF.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDSF.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDSFMap.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDSFMap.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDSFVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDSFVector.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testFixedVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testFixedVector.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -1472,6 +1425,7 @@
</target>
<target name="testSimulated2DOriented.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated2DOriented.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1511,6 +1465,7 @@
</target>
<target name="testSimulated2D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated2D.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1518,6 +1473,7 @@
</target>
<target name="testSimulated3D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated3D.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1531,6 +1487,46 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testBTree.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testBTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDSF.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDSF.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDSFMap.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDSFMap.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDSFVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDSFVector.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testFixedVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testFixedVector.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testEliminationTree.run" path="build/gtsam/inference/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
@ -1788,7 +1784,6 @@
</target>
<target name="Generate DEB Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>-G DEB</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1796,7 +1791,6 @@
</target>
<target name="Generate RPM Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>-G RPM</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1804,7 +1798,6 @@
</target>
<target name="Generate TGZ Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>-G TGZ</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1812,7 +1805,6 @@
</target>
<target name="Generate TGZ Source Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>--config CPackSourceConfig.cmake</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -2441,6 +2433,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testVerticalBlockMatrix.run" path="build/gtsam/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testVerticalBlockMatrix.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check.tests" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
@ -2579,7 +2579,6 @@
</target>
<target name="testGraph.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -2587,7 +2586,6 @@
</target>
<target name="testJunctionTree.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testJunctionTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -2595,7 +2593,6 @@
</target>
<target name="testSymbolicBayesNetB.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicBayesNetB.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -3115,6 +3112,7 @@
</target>
<target name="tests/testGaussianISAM2" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testGaussianISAM2</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>

BIN
gtsam/3rdparty/CCOLAMD/Lib/ccolamd.o vendored
View File

Binary file not shown.

BIN
gtsam/3rdparty/CCOLAMD/Lib/ccolamd_global.o vendored
View File

Binary file not shown.

BIN
gtsam/3rdparty/CCOLAMD/Lib/ccolamd_l.o vendored
View File

Binary file not shown.

39
gtsam/base/VerticalBlockMatrix.h
View File

@ -65,9 +65,10 @@ namespace gtsam {
/** Construct from a container of the sizes of each vertical block. */
template<typename CONTAINER>
VerticalBlockMatrix(const CONTAINER& dimensions, DenseIndex height, bool appendOneDimension = false) :
rowStart_(0), rowEnd_(height), blockStart_(0)
{
VerticalBlockMatrix(const CONTAINER& dimensions, DenseIndex height,
bool appendOneDimension = false) :
variableColOffsets_(dimensions.size() + (appendOneDimension ? 2 : 1)),
rowStart_(0), rowEnd_(height), blockStart_(0) {
fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension);
matrix_.resize(height, variableColOffsets_.back());
assertInvariants();
@ -75,26 +76,28 @@ namespace gtsam {
/** Construct from a container of the sizes of each vertical block and a pre-prepared matrix. */
template<typename CONTAINER, typename DERIVED>
VerticalBlockMatrix(const CONTAINER& dimensions, const Eigen::MatrixBase<DERIVED>& matrix, bool appendOneDimension = false) :
matrix_(matrix), rowStart_(0), rowEnd_(matrix.rows()), blockStart_(0)
{
VerticalBlockMatrix(const CONTAINER& dimensions,
const Eigen::MatrixBase<DERIVED>& matrix, bool appendOneDimension = false) :
matrix_(matrix), variableColOffsets_(dimensions.size() + (appendOneDimension ? 2 : 1)),
rowStart_(0), rowEnd_(matrix.rows()), blockStart_(0) {
fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension);
if(variableColOffsets_.back() != matrix_.cols())
throw std::invalid_argument("Requested to create a VerticalBlockMatrix with dimensions that do not sum to the total columns of the provided matrix.");
if (variableColOffsets_.back() != matrix_.cols())
throw std::invalid_argument(
"Requested to create a VerticalBlockMatrix with dimensions that do not sum to the total columns of the provided matrix.");
assertInvariants();
}
/**
* Construct from iterator over the sizes of each vertical block. */
/** Construct from iterator over the sizes of each vertical block. */
template<typename ITERATOR>
VerticalBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim, DenseIndex height, bool appendOneDimension = false) :
rowStart_(0), rowEnd_(height), blockStart_(0)
{
VerticalBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim,
DenseIndex height, bool appendOneDimension = false) :
variableColOffsets_((lastBlockDim-firstBlockDim) + (appendOneDimension ? 2 : 1)),
rowStart_(0), rowEnd_(height), blockStart_(0) {
fillOffsets(firstBlockDim, lastBlockDim, appendOneDimension);
matrix_.resize(height, variableColOffsets_.back());
assertInvariants();
}
/** Copy the block structure and resize the underlying matrix, but do not copy the matrix data.
* If blockStart(), rowStart(), and/or rowEnd() have been modified, this copies the structure of
* the corresponding matrix view. In the destination VerticalBlockView, blockStart() and
@ -203,18 +206,12 @@ namespace gtsam {
template<typename ITERATOR>
void fillOffsets(ITERATOR firstBlockDim, ITERATOR lastBlockDim, bool appendOneDimension) {
variableColOffsets_.resize((lastBlockDim-firstBlockDim) + 1 + (appendOneDimension ? 1 : 0));
variableColOffsets_[0] = 0;
DenseIndex j=0;
for(ITERATOR dim=firstBlockDim; dim!=lastBlockDim; ++dim) {
for(ITERATOR dim=firstBlockDim; dim!=lastBlockDim; ++dim, ++j)
variableColOffsets_[j+1] = variableColOffsets_[j] + *dim;
++ j;
}
if(appendOneDimension)
{
variableColOffsets_[j+1] = variableColOffsets_[j] + 1;
++ j;
}
}
friend class SymmetricBlockMatrix;

23
gtsam/base/tests/testVerticalBlockMatrix.cpp
View File

@ -24,9 +24,20 @@ using namespace std;
using namespace gtsam;
using boost::assign::list_of;
list<size_t> L = list_of(3)(2)(1);
vector<size_t> dimensions(L.begin(),L.end());
//*****************************************************************************
TEST(VerticalBlockMatrix, constructor) {
VerticalBlockMatrix actual(list_of(3)(2)(1),
TEST(VerticalBlockMatrix, Constructor1) {
VerticalBlockMatrix actual(dimensions,6);
EXPECT_LONGS_EQUAL(6,actual.rows());
EXPECT_LONGS_EQUAL(6,actual.cols());
EXPECT_LONGS_EQUAL(3,actual.nBlocks());
}
//*****************************************************************************
TEST(VerticalBlockMatrix, Constructor2) {
VerticalBlockMatrix actual(dimensions,
(Matrix(6, 6) << 1, 2, 3, 4, 5, 6, //
2, 8, 9, 10, 11, 12, //
3, 9, 15, 16, 17, 18, //
@ -38,6 +49,14 @@ TEST(VerticalBlockMatrix, constructor) {
EXPECT_LONGS_EQUAL(3,actual.nBlocks());
}
//*****************************************************************************
TEST(VerticalBlockMatrix, Constructor3) {
VerticalBlockMatrix actual(dimensions.begin(),dimensions.end(),6);
EXPECT_LONGS_EQUAL(6,actual.rows());
EXPECT_LONGS_EQUAL(6,actual.cols());
EXPECT_LONGS_EQUAL(3,actual.nBlocks());
}
//*****************************************************************************
int main() {
TestResult tr;

27
gtsam/linear/JacobianFactor.h
View File

@ -82,20 +82,22 @@ namespace gtsam {
class GTSAM_EXPORT JacobianFactor : public GaussianFactor
{
public:
typedef JacobianFactor This; ///< Typedef to this class
typedef GaussianFactor Base; ///< Typedef to base class
typedef boost::shared_ptr<This> shared_ptr; ///< shared_ptr to this class
protected:
VerticalBlockMatrix Ab_; // the block view of the full matrix
noiseModel::Diagonal::shared_ptr model_; // Gaussian noise model with diagonal covariance matrix
public:
typedef VerticalBlockMatrix::Block ABlock;
typedef VerticalBlockMatrix::constBlock constABlock;
typedef ABlock::ColXpr BVector;
typedef constABlock::ConstColXpr constBVector;
protected:
VerticalBlockMatrix Ab_; // the block view of the full matrix
noiseModel::Diagonal::shared_ptr model_; // Gaussian noise model with diagonal covariance matrix
public:
/** Convert from other GaussianFactor */
explicit JacobianFactor(const GaussianFactor& gf);
@ -328,6 +330,21 @@ namespace gtsam {
private:
/** Unsafe Constructor that creates an uninitialized Jacobian of right size
* @param keys in some order
* @param diemnsions of the variables in same order
* @param m output dimension
* @param model noise model (default NULL)
*/
template<class KEYS, class DIMENSIONS>
JacobianFactor(const KEYS& keys, const DIMENSIONS& dims, DenseIndex m,
const SharedDiagonal& model = SharedDiagonal()) :
Base(keys), Ab_(dims.begin(), dims.end(), m, true), model_(model) {
}
// be very selective on who can access these private methods:
template<typename T> friend class ExpressionFactor;
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>

34
gtsam_unstable/nonlinear/ExpressionFactor.h
View File

@ -107,23 +107,23 @@ public:
virtual boost::shared_ptr<GaussianFactor> linearize(const Values& x) const {
// This method has been heavily optimized for maximum performance.
// We allocate a VerticalBlockMatrix on the stack first, and then create
// a JacobianMap view onto it, which is then passed
// to [expression_.value] to allow it to write directly into Ab_.
// Check whether noise model is constrained or not
noiseModel::Constrained::shared_ptr constrained = //
boost::dynamic_pointer_cast<noiseModel::Constrained>(this->noiseModel_);
// Another malloc saved by creating a Matrix on the stack
double memory[Dim * augmentedCols_];
Eigen::Map<Eigen::Matrix<double, Dim, Eigen::Dynamic> > //
matrix(memory, Dim, augmentedCols_);
matrix.setZero(); // zero out
// Construct block matrix, is of right size but un-initialized
VerticalBlockMatrix Ab(dimensions_, matrix, true);
// Create a writeable JacobianFactor in advance
boost::shared_ptr<JacobianFactor> factor(
constrained ? new JacobianFactor(keys_, dimensions_, Dim,
constrained->unit()) :
new JacobianFactor(keys_, dimensions_, Dim));
// Wrap keys and VerticalBlockMatrix into structure passed to expression_
VerticalBlockMatrix& Ab = factor->matrixObject();
JacobianMap map(keys_, Ab);
// Zero out Jacobian so we can simply add to it
Ab.matrix().setZero();
// Evaluate error to get Jacobians and RHS vector b
T value = expression_.value(x, map); // <<< Reverse AD happens here !
Ab(size()).col(0) = -measurement_.localCoordinates(value);
@ -131,15 +131,7 @@ public:
// Whiten the corresponding system now
// TODO ! this->noiseModel_->WhitenSystem(Ab);
// TODO pass unwhitened + noise model to Gaussian factor
// For now, only linearized constrained factors have noise model at linear level!!!
noiseModel::Constrained::shared_ptr constrained = //
boost::dynamic_pointer_cast<noiseModel::Constrained>(this->noiseModel_);
if (constrained) {
return boost::make_shared<JacobianFactor>(this->keys(), Ab,
constrained->unit());
} else
return boost::make_shared<JacobianFactor>(this->keys(), Ab);
return factor;
}
};
// ExpressionFactor

3
gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp
View File

@ -28,6 +28,9 @@
#include <CppUnitLite/TestHarness.h>
#include <boost/assign/list_of.hpp>
using boost::assign::list_of;
using namespace std;
using namespace gtsam;

2
gtsam_unstable/timing/timeCameraExpression.cpp
View File

@ -27,7 +27,7 @@
using namespace std;
using namespace gtsam;
#define time timeMultiThreaded
#define time timeSingleThreaded
boost::shared_ptr<Cal3_S2> fixedK(new Cal3_S2());