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

Merge branch 'feature/BAD' into feature/BAD_generic_value_traits 

Conflicts:
	gtsam_unstable/nonlinear/tests/testAdaptAutoDiff.cpp
	gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp
release/4.3a0
Mike Bosse 2014-11-02 19:00:30 +01:00
parent 95e59d7c5d 17d352bab4
commit 11416cac65
20 changed files with 262 additions and 231 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>
<target name="tests/testBayesTree.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="tests/testBayesTree.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testBayesTree.run</buildTarget> <buildTarget>tests/testBayesTree.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -608,7 +607,6 @@
</target> </target>
<target name="testBinaryBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testBinaryBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testBinaryBayesNet.run</buildTarget> <buildTarget>testBinaryBayesNet.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -656,7 +654,6 @@
</target> </target>
<target name="testSymbolicBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testSymbolicBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicBayesNet.run</buildTarget> <buildTarget>testSymbolicBayesNet.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -664,7 +661,6 @@
</target> </target>
<target name="tests/testSymbolicFactor.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="tests/testSymbolicFactor.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testSymbolicFactor.run</buildTarget> <buildTarget>tests/testSymbolicFactor.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -672,7 +668,6 @@
</target> </target>
<target name="testSymbolicFactorGraph.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testSymbolicFactorGraph.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicFactorGraph.run</buildTarget> <buildTarget>testSymbolicFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -688,7 +683,6 @@
</target> </target>
<target name="tests/testBayesTree" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="tests/testBayesTree" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testBayesTree</buildTarget> <buildTarget>tests/testBayesTree</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -1120,7 +1114,6 @@
</target> </target>
<target name="testErrors.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testErrors.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testErrors.run</buildTarget> <buildTarget>testErrors.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -1350,46 +1343,6 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </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"> <target name="all" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j2</buildArguments>
@ -1472,6 +1425,7 @@
</target> </target>
<target name="testSimulated2DOriented.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testSimulated2DOriented.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated2DOriented.run</buildTarget> <buildTarget>testSimulated2DOriented.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -1511,6 +1465,7 @@
</target> </target>
<target name="testSimulated2D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testSimulated2D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated2D.run</buildTarget> <buildTarget>testSimulated2D.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -1518,6 +1473,7 @@
</target> </target>
<target name="testSimulated3D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testSimulated3D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated3D.run</buildTarget> <buildTarget>testSimulated3D.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -1531,6 +1487,46 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </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"> <target name="testEliminationTree.run" path="build/gtsam/inference/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j5</buildArguments>
@ -1788,7 +1784,6 @@
</target> </target>
<target name="Generate DEB Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="Generate DEB Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand> <buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>-G DEB</buildTarget> <buildTarget>-G DEB</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -1796,7 +1791,6 @@
</target> </target>
<target name="Generate RPM Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="Generate RPM Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand> <buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>-G RPM</buildTarget> <buildTarget>-G RPM</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -1804,7 +1798,6 @@
</target> </target>
<target name="Generate TGZ Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="Generate TGZ Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand> <buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>-G TGZ</buildTarget> <buildTarget>-G TGZ</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -1812,7 +1805,6 @@
</target> </target>
<target name="Generate TGZ Source Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="Generate TGZ Source Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand> <buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>--config CPackSourceConfig.cmake</buildTarget> <buildTarget>--config CPackSourceConfig.cmake</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -2441,6 +2433,14 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </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"> <target name="check.tests" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j5</buildArguments>
@ -2579,7 +2579,6 @@
</target> </target>
<target name="testGraph.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testGraph.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testGraph.run</buildTarget> <buildTarget>testGraph.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -2587,7 +2586,6 @@
</target> </target>
<target name="testJunctionTree.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testJunctionTree.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testJunctionTree.run</buildTarget> <buildTarget>testJunctionTree.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -2595,7 +2593,6 @@
</target> </target>
<target name="testSymbolicBayesNetB.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testSymbolicBayesNetB.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicBayesNetB.run</buildTarget> <buildTarget>testSymbolicBayesNetB.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -3115,6 +3112,7 @@
</target> </target>
<target name="tests/testGaussianISAM2" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="tests/testGaussianISAM2" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testGaussianISAM2</buildTarget> <buildTarget>tests/testGaussianISAM2</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <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.

3
gtsam/base/Matrix.cpp
View File

@ -543,8 +543,7 @@ Matrix collect(size_t nrMatrices, ...)
void vector_scale_inplace(const Vector& v, Matrix& A, bool inf_mask) { void vector_scale_inplace(const Vector& v, Matrix& A, bool inf_mask) {
const DenseIndex m = A.rows(); const DenseIndex m = A.rows();
if (inf_mask) { if (inf_mask) {
// only scale the first v.size() rows of A to support augmented Matrix for (DenseIndex i=0; i<m; ++i) {
for (DenseIndex i=0; i<v.size(); ++i) {
const double& vi = v(i); const double& vi = v(i);
if (std::isfinite(vi)) if (std::isfinite(vi))
A.row(i) *= vi; A.row(i) *= vi;

1
gtsam/base/Matrix.h
View File

@ -416,7 +416,6 @@ GTSAM_EXPORT Matrix collect(size_t nrMatrices, ...);
* Arguments (Matrix, Vector) scales the columns, * Arguments (Matrix, Vector) scales the columns,
* (Vector, Matrix) scales the rows * (Vector, Matrix) scales the rows
* @param inf_mask when true, will not scale with a NaN or inf value. * @param inf_mask when true, will not scale with a NaN or inf value.
* The inplace version also allows v.size()<A.rows() and only scales the first v.size() rows of A.
*/ */
GTSAM_EXPORT void vector_scale_inplace(const Vector& v, Matrix& A, bool inf_mask = false); // row GTSAM_EXPORT void vector_scale_inplace(const Vector& v, Matrix& A, bool inf_mask = false); // row
GTSAM_EXPORT Matrix vector_scale(const Vector& v, const Matrix& A, bool inf_mask = false); // row GTSAM_EXPORT Matrix vector_scale(const Vector& v, const Matrix& A, bool inf_mask = false); // row

37
gtsam/base/VerticalBlockMatrix.h
View File

@ -65,9 +65,10 @@ namespace gtsam {
/** Construct from a container of the sizes of each vertical block. */ /** Construct from a container of the sizes of each vertical block. */
template<typename CONTAINER> template<typename CONTAINER>
VerticalBlockMatrix(const CONTAINER& dimensions, DenseIndex height, bool appendOneDimension = false) : VerticalBlockMatrix(const CONTAINER& dimensions, DenseIndex height,
rowStart_(0), rowEnd_(height), blockStart_(0) bool appendOneDimension = false) :
{ variableColOffsets_(dimensions.size() + (appendOneDimension ? 2 : 1)),
rowStart_(0), rowEnd_(height), blockStart_(0) {
fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension); fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension);
matrix_.resize(height, variableColOffsets_.back()); matrix_.resize(height, variableColOffsets_.back());
assertInvariants(); assertInvariants();
@ -75,21 +76,23 @@ namespace gtsam {
/** Construct from a container of the sizes of each vertical block and a pre-prepared matrix. */ /** Construct from a container of the sizes of each vertical block and a pre-prepared matrix. */
template<typename CONTAINER, typename DERIVED> template<typename CONTAINER, typename DERIVED>
VerticalBlockMatrix(const CONTAINER& dimensions, const Eigen::MatrixBase<DERIVED>& matrix, bool appendOneDimension = false) : VerticalBlockMatrix(const CONTAINER& dimensions,
matrix_(matrix), rowStart_(0), rowEnd_(matrix.rows()), blockStart_(0) 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); fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension);
if(variableColOffsets_.back() != matrix_.cols()) 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."); throw std::invalid_argument(
"Requested to create a VerticalBlockMatrix with dimensions that do not sum to the total columns of the provided matrix.");
assertInvariants(); assertInvariants();
} }
/** /** Construct from iterator over the sizes of each vertical block. */
* Construct from iterator over the sizes of each vertical block. */
template<typename ITERATOR> template<typename ITERATOR>
VerticalBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim, DenseIndex height, bool appendOneDimension = false) : VerticalBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim,
rowStart_(0), rowEnd_(height), blockStart_(0) DenseIndex height, bool appendOneDimension = false) :
{ variableColOffsets_((lastBlockDim-firstBlockDim) + (appendOneDimension ? 2 : 1)),
rowStart_(0), rowEnd_(height), blockStart_(0) {
fillOffsets(firstBlockDim, lastBlockDim, appendOneDimension); fillOffsets(firstBlockDim, lastBlockDim, appendOneDimension);
matrix_.resize(height, variableColOffsets_.back()); matrix_.resize(height, variableColOffsets_.back());
assertInvariants(); assertInvariants();
@ -203,18 +206,12 @@ namespace gtsam {
template<typename ITERATOR> template<typename ITERATOR>
void fillOffsets(ITERATOR firstBlockDim, ITERATOR lastBlockDim, bool appendOneDimension) { void fillOffsets(ITERATOR firstBlockDim, ITERATOR lastBlockDim, bool appendOneDimension) {
variableColOffsets_.resize((lastBlockDim-firstBlockDim) + 1 + (appendOneDimension ? 1 : 0));
variableColOffsets_[0] = 0; variableColOffsets_[0] = 0;
DenseIndex j=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; variableColOffsets_[j+1] = variableColOffsets_[j] + *dim;
++ j;
}
if(appendOneDimension) if(appendOneDimension)
{
variableColOffsets_[j+1] = variableColOffsets_[j] + 1; variableColOffsets_[j+1] = variableColOffsets_[j] + 1;
++ j;
}
} }
friend class SymmetricBlockMatrix; friend class SymmetricBlockMatrix;

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

@ -24,9 +24,20 @@ using namespace std;
using namespace gtsam; using namespace gtsam;
using boost::assign::list_of; 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) { TEST(VerticalBlockMatrix, Constructor1) {
VerticalBlockMatrix actual(list_of(3)(2)(1), 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, // (Matrix(6, 6) << 1, 2, 3, 4, 5, 6, //
2, 8, 9, 10, 11, 12, // 2, 8, 9, 10, 11, 12, //
3, 9, 15, 16, 17, 18, // 3, 9, 15, 16, 17, 18, //
@ -38,6 +49,14 @@ TEST(VerticalBlockMatrix, constructor) {
EXPECT_LONGS_EQUAL(3,actual.nBlocks()); 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() { int main() {
TestResult tr; TestResult tr;

8
gtsam/geometry/Pose3.cpp
View File

@ -243,8 +243,8 @@ Pose3 Pose3::transform_to(const Pose3& pose) const {
Point3 Pose3::transform_from(const Point3& p, boost::optional<Matrix&> Dpose, Point3 Pose3::transform_from(const Point3& p, boost::optional<Matrix&> Dpose,
boost::optional<Matrix&> Dpoint) const { boost::optional<Matrix&> Dpoint) const {
if (Dpose) { if (Dpose) {
const Matrix R = R_.matrix(); const Matrix3 R = R_.matrix();
Matrix DR = R * skewSymmetric(-p.x(), -p.y(), -p.z()); Matrix3 DR = R * skewSymmetric(-p.x(), -p.y(), -p.z());
Dpose->resize(3, 6); Dpose->resize(3, 6);
(*Dpose) << DR, R; (*Dpose) << DR, R;
} }
@ -263,7 +263,7 @@ Point3 Pose3::transform_to(const Point3& p, boost::optional<Matrix36&> Dpose,
boost::optional<Matrix3&> Dpoint) const { boost::optional<Matrix3&> Dpoint) const {
// Only get transpose once, to avoid multiple allocations, // Only get transpose once, to avoid multiple allocations,
// as well as multiple conversions in the Quaternion case // as well as multiple conversions in the Quaternion case
const Matrix3& Rt = R_.transpose(); const Matrix3 Rt = R_.transpose();
const Point3 q(Rt*(p - t_).vector()); const Point3 q(Rt*(p - t_).vector());
if (Dpose) { if (Dpose) {
const double wx = q.x(), wy = q.y(), wz = q.z(); const double wx = q.x(), wy = q.y(), wz = q.z();
@ -280,7 +280,7 @@ Point3 Pose3::transform_to(const Point3& p, boost::optional<Matrix36&> Dpose,
/* ************************************************************************* */ /* ************************************************************************* */
Point3 Pose3::transform_to(const Point3& p, boost::optional<Matrix&> Dpose, Point3 Pose3::transform_to(const Point3& p, boost::optional<Matrix&> Dpose,
boost::optional<Matrix&> Dpoint) const { boost::optional<Matrix&> Dpoint) const {
const Matrix3& Rt = R_.transpose(); const Matrix3 Rt = R_.transpose();
const Point3 q(Rt*(p - t_).vector()); const Point3 q(Rt*(p - t_).vector());
if (Dpose) { if (Dpose) {
const double wx = q.x(), wy = q.y(), wz = q.z(); const double wx = q.x(), wy = q.y(), wz = q.z();

27
gtsam/linear/JacobianFactor.h
View File

@ -82,20 +82,22 @@ namespace gtsam {
class GTSAM_EXPORT JacobianFactor : public GaussianFactor class GTSAM_EXPORT JacobianFactor : public GaussianFactor
{ {
public: public:
typedef JacobianFactor This; ///< Typedef to this class typedef JacobianFactor This; ///< Typedef to this class
typedef GaussianFactor Base; ///< Typedef to base class typedef GaussianFactor Base; ///< Typedef to base class
typedef boost::shared_ptr<This> shared_ptr; ///< shared_ptr to this 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::Block ABlock;
typedef VerticalBlockMatrix::constBlock constABlock; typedef VerticalBlockMatrix::constBlock constABlock;
typedef ABlock::ColXpr BVector; typedef ABlock::ColXpr BVector;
typedef constABlock::ConstColXpr constBVector; 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 */ /** Convert from other GaussianFactor */
explicit JacobianFactor(const GaussianFactor& gf); explicit JacobianFactor(const GaussianFactor& gf);
@ -328,6 +330,21 @@ namespace gtsam {
private: 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 */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
template<class ARCHIVE> template<class ARCHIVE>

26
gtsam/linear/NoiseModel.cpp
View File

@ -339,12 +339,12 @@ Vector Constrained::whiten(const Vector& v) const {
// a hard constraint, we don't do anything. // a hard constraint, we don't do anything.
const Vector& a = v; const Vector& a = v;
const Vector& b = sigmas_; const Vector& b = sigmas_;
// Now allows for whiten augmented vector with a new additional part coming size_t n = a.size();
// from the Lagrange multiplier. So a.size() >= b.size() assert (b.size()==a.size());
Vector c = a; Vector c(n);
for( DenseIndex i = 0; i < b.size(); i++ ) { for( size_t i = 0; i < n; i++ ) {
const double& ai = a(i), &bi = b(i); const double& ai = a(i), &bi = b(i);
if (bi!=0) c(i) = ai/bi; c(i) = (bi==0.0) ? ai : ai/bi; // NOTE: not ediv_()
} }
return c; return c;
} }
@ -362,11 +362,7 @@ double Constrained::distance(const Vector& v) const {
/* ************************************************************************* */ /* ************************************************************************* */
Matrix Constrained::Whiten(const Matrix& H) const { Matrix Constrained::Whiten(const Matrix& H) const {
// selective scaling // selective scaling
// Now allow augmented Matrix with a new additional part coming return vector_scale(invsigmas(), H, true);
// from the Lagrange multiplier.
Matrix M(H.block(0, 0, dim(), H.cols()));
Constrained::WhitenInPlace(M);
return M;
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -375,8 +371,6 @@ void Constrained::WhitenInPlace(Matrix& H) const {
// Scale row i of H by sigmas[i], basically multiplying H with diag(sigmas) // Scale row i of H by sigmas[i], basically multiplying H with diag(sigmas)
// Set inf_mask flag is true so that if invsigmas[i] is inf, i.e. sigmas[i] = 0, // Set inf_mask flag is true so that if invsigmas[i] is inf, i.e. sigmas[i] = 0,
// indicating a hard constraint, we leave H's row i in place. // indicating a hard constraint, we leave H's row i in place.
// Now allow augmented Matrix with a new additional part coming
// from the Lagrange multiplier.
// Inlined: vector_scale_inplace(invsigmas(), H, true); // Inlined: vector_scale_inplace(invsigmas(), H, true);
// vector_scale_inplace(v, A, true); // vector_scale_inplace(v, A, true);
for (DenseIndex i=0; i<(DenseIndex)dim_; ++i) { for (DenseIndex i=0; i<(DenseIndex)dim_; ++i) {
@ -399,14 +393,12 @@ void Constrained::WhitenInPlace(Eigen::Block<Matrix> H) const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Constrained::shared_ptr Constrained::unit(size_t augmentedDim) const { Constrained::shared_ptr Constrained::unit() const {
Vector sigmas = ones(dim()+augmentedDim); Vector sigmas = ones(dim());
for (size_t i=0; i<dim(); ++i) for (size_t i=0; i<dim(); ++i)
if (this->sigmas_(i) == 0.0) if (this->sigmas_(i) == 0.0)
sigmas(i) = 0.0; sigmas(i) = 0.0;
Vector augmentedMu = zero(dim()+augmentedDim); return MixedSigmas(mu_, sigmas);
subInsert(augmentedMu, mu_, 0);
return MixedSigmas(augmentedMu, sigmas);
} }
/* ************************************************************************* */ /* ************************************************************************* */

15
gtsam/linear/NoiseModel.h
View File

@ -61,6 +61,11 @@ namespace gtsam {
Base(size_t dim = 1):dim_(dim) {} Base(size_t dim = 1):dim_(dim) {}
virtual ~Base() {} virtual ~Base() {}
/** true if a constrained noise mode, saves slow/clumsy dynamic casting */
virtual bool is_constrained() const {
return false;
}
/// Dimensionality /// Dimensionality
inline size_t dim() const { return dim_;} inline size_t dim() const { return dim_;}
@ -385,6 +390,11 @@ namespace gtsam {
virtual ~Constrained() {} virtual ~Constrained() {}
/** true if a constrained noise mode, saves slow/clumsy dynamic casting */
virtual bool is_constrained() const {
return true;
}
/// Access mu as a vector /// Access mu as a vector
const Vector& mu() const { return mu_; } const Vector& mu() const { return mu_; }
@ -481,9 +491,8 @@ namespace gtsam {
/** /**
* Returns a Unit version of a constrained noisemodel in which * Returns a Unit version of a constrained noisemodel in which
* constrained sigmas remain constrained and the rest are unit scaled * constrained sigmas remain constrained and the rest are unit scaled
* Now support augmented part from the Lagrange multiplier.
*/ */
shared_ptr unit(size_t augmentedDim = 0) const; shared_ptr unit() const;
private: private:
/** Serialization function */ /** Serialization function */
@ -732,7 +741,7 @@ namespace gtsam {
}; };
/// Cauchy implements the "Cauchy" robust error model (Lee2013IROS). Contributed by: /// Cauchy implements the "Cauchy" robust error model (Lee2013IROS). Contributed by:
/// Dipl.-Inform. Jan Oberl<EFBFBD>nder (M.Sc.), FZI Research Center for /// Dipl.-Inform. Jan Oberlaender (M.Sc.), FZI Research Center for
/// Information Technology, Karlsruhe, Germany. /// Information Technology, Karlsruhe, Germany.
/// oberlaender@fzi.de /// oberlaender@fzi.de
/// Thanks Jan! /// Thanks Jan!

48
gtsam/nonlinear/NonlinearFactor.cpp
View File

@ -22,20 +22,21 @@
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
void NonlinearFactor::print(const std::string& s, void NonlinearFactor::print(const std::string& s,
const KeyFormatter& keyFormatter) const { const KeyFormatter& keyFormatter) const {
std::cout << s << " keys = { "; std::cout << s << " keys = { ";
BOOST_FOREACH(Key key, this->keys()) { BOOST_FOREACH(Key key, keys()) {
std::cout << keyFormatter(key) << " "; std::cout << keyFormatter(key) << " ";
} }
std::cout << "}" << std::endl; std::cout << "}" << std::endl;
} }
/* ************************************************************************* */
bool NonlinearFactor::equals(const NonlinearFactor& f, double tol) const { bool NonlinearFactor::equals(const NonlinearFactor& f, double tol) const {
return Base::equals(f); return Base::equals(f);
} }
/* ************************************************************************* */
NonlinearFactor::shared_ptr NonlinearFactor::rekey( NonlinearFactor::shared_ptr NonlinearFactor::rekey(
const std::map<Key, Key>& rekey_mapping) const { const std::map<Key, Key>& rekey_mapping) const {
shared_ptr new_factor = clone(); shared_ptr new_factor = clone();
@ -48,22 +49,23 @@ NonlinearFactor::shared_ptr NonlinearFactor::rekey(
return new_factor; return new_factor;
} }
/* ************************************************************************* */
NonlinearFactor::shared_ptr NonlinearFactor::rekey( NonlinearFactor::shared_ptr NonlinearFactor::rekey(
const std::vector<Key>& new_keys) const { const std::vector<Key>& new_keys) const {
assert(new_keys.size() == this->keys().size()); assert(new_keys.size() == keys().size());
shared_ptr new_factor = clone(); shared_ptr new_factor = clone();
new_factor->keys() = new_keys; new_factor->keys() = new_keys;
return new_factor; return new_factor;
} }
/* ************************************************************************* */ /* ************************************************************************* */
void NoiseModelFactor::print(const std::string& s, void NoiseModelFactor::print(const std::string& s,
const KeyFormatter& keyFormatter) const { const KeyFormatter& keyFormatter) const {
Base::print(s, keyFormatter); Base::print(s, keyFormatter);
this->noiseModel_->print(" noise model: "); noiseModel_->print(" noise model: ");
} }
/* ************************************************************************* */
bool NoiseModelFactor::equals(const NonlinearFactor& f, double tol) const { bool NoiseModelFactor::equals(const NonlinearFactor& f, double tol) const {
const NoiseModelFactor* e = dynamic_cast<const NoiseModelFactor*>(&f); const NoiseModelFactor* e = dynamic_cast<const NoiseModelFactor*>(&f);
return e && Base::equals(f, tol) return e && Base::equals(f, tol)
@ -72,6 +74,7 @@ bool NoiseModelFactor::equals(const NonlinearFactor& f, double tol) const {
&& noiseModel_->equals(*e->noiseModel_, tol))); && noiseModel_->equals(*e->noiseModel_, tol)));
} }
/* ************************************************************************* */
static void check(const SharedNoiseModel& noiseModel, size_t m) { static void check(const SharedNoiseModel& noiseModel, size_t m) {
if (!noiseModel) if (!noiseModel)
throw std::invalid_argument("NoiseModelFactor: no NoiseModel."); throw std::invalid_argument("NoiseModelFactor: no NoiseModel.");
@ -80,14 +83,16 @@ static void check(const SharedNoiseModel& noiseModel, size_t m) {
"NoiseModelFactor was created with a NoiseModel of incorrect dimension."); "NoiseModelFactor was created with a NoiseModel of incorrect dimension.");
} }
/* ************************************************************************* */
Vector NoiseModelFactor::whitenedError(const Values& c) const { Vector NoiseModelFactor::whitenedError(const Values& c) const {
const Vector b = unwhitenedError(c); const Vector b = unwhitenedError(c);
check(noiseModel_, b.size()); check(noiseModel_, b.size());
return noiseModel_->whiten(b); return noiseModel_->whiten(b);
} }
/* ************************************************************************* */
double NoiseModelFactor::error(const Values& c) const { double NoiseModelFactor::error(const Values& c) const {
if (this->active(c)) { if (active(c)) {
const Vector b = unwhitenedError(c); const Vector b = unwhitenedError(c);
check(noiseModel_, b.size()); check(noiseModel_, b.size());
return 0.5 * noiseModel_->distance(b); return 0.5 * noiseModel_->distance(b);
@ -96,41 +101,36 @@ double NoiseModelFactor::error(const Values& c) const {
} }
} }
/* ************************************************************************* */
boost::shared_ptr<GaussianFactor> NoiseModelFactor::linearize( boost::shared_ptr<GaussianFactor> NoiseModelFactor::linearize(
const Values& x) const { const Values& x) const {
// Only linearize if the factor is active // Only linearize if the factor is active
if (!this->active(x)) if (!active(x))
return boost::shared_ptr<JacobianFactor>(); return boost::shared_ptr<JacobianFactor>();
// Call evaluate error to get Jacobians and RHS vector b // Call evaluate error to get Jacobians and RHS vector b
std::vector<Matrix> A(this->size()); std::vector<Matrix> A(size());
Vector b = -unwhitenedError(x, A); Vector b = -unwhitenedError(x, A);
check(noiseModel_, b.size()); check(noiseModel_, b.size());
// Whiten the corresponding system now // Whiten the corresponding system now
this->noiseModel_->WhitenSystem(A, b); noiseModel_->WhitenSystem(A, b);
// Fill in terms, needed to create JacobianFactor below // Fill in terms, needed to create JacobianFactor below
std::vector<std::pair<Key, Matrix> > terms(this->size()); std::vector<std::pair<Key, Matrix> > terms(size());
for (size_t j = 0; j < this->size(); ++j) { for (size_t j = 0; j < size(); ++j) {
terms[j].first = this->keys()[j]; terms[j].first = keys()[j];
terms[j].second.swap(A[j]); terms[j].second.swap(A[j]);
} }
// TODO pass unwhitened + noise model to Gaussian factor // TODO pass unwhitened + noise model to Gaussian factor
// For now, only linearized constrained factors have noise model at linear level!!! if (noiseModel_->is_constrained())
noiseModel::Constrained::shared_ptr constrained = // return GaussianFactor::shared_ptr(
boost::dynamic_pointer_cast<noiseModel::Constrained>(this->noiseModel_); new JacobianFactor(terms, b,
if (constrained) { boost::static_pointer_cast<noiseModel::Constrained>(noiseModel_)->unit()));
// Create a factor of reduced row dimension d_ else
size_t d_ = b.size() - constrained->dim(); return GaussianFactor::shared_ptr(new JacobianFactor(terms, b));
Vector zero_ = zero(d_);
Vector b_ = concatVectors(2, &b, &zero_);
noiseModel::Constrained::shared_ptr model = constrained->unit(d_);
return boost::make_shared<JacobianFactor>(terms, b_, model);
} else
return boost::make_shared<JacobianFactor>(terms, b);
} }
/* ************************************************************************* */ /* ************************************************************************* */

28
gtsam_unstable/nonlinear/Expression-inl.h
View File

@ -23,9 +23,11 @@
#include <gtsam/base/Matrix.h> #include <gtsam/base/Matrix.h>
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <gtsam/base/Manifold.h> #include <gtsam/base/Manifold.h>
#include <gtsam/base/VerticalBlockMatrix.h>
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp> #include <boost/tuple/tuple.hpp>
#include <boost/bind.hpp>
// template meta-programming headers // template meta-programming headers
#include <boost/mpl/vector.hpp> #include <boost/mpl/vector.hpp>
@ -48,7 +50,25 @@ namespace gtsam {
template<typename T> template<typename T>
class Expression; class Expression;
typedef std::map<Key, Eigen::Block<Matrix> > JacobianMap; /**
* Expressions are designed to write their derivatives into an already allocated
* Jacobian of the correct size, of type VerticalBlockMatrix.
* The JacobianMap provides a mapping from keys to the underlying blocks.
*/
class JacobianMap {
const FastVector<Key>& keys_;
VerticalBlockMatrix& Ab_;
public:
JacobianMap(const FastVector<Key>& keys, VerticalBlockMatrix& Ab) :
keys_(keys), Ab_(Ab) {
}
/// Access via key
VerticalBlockMatrix::Block operator()(Key key) {
FastVector<Key>::const_iterator it = std::find(keys_.begin(),keys_.end(),key);
DenseIndex block = it - keys_.begin();
return Ab_(block);
}
};
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
/** /**
@ -78,16 +98,14 @@ struct CallRecord {
template<int ROWS, int COLS> template<int ROWS, int COLS>
void handleLeafCase(const Eigen::Matrix<double, ROWS, COLS>& dTdA, void handleLeafCase(const Eigen::Matrix<double, ROWS, COLS>& dTdA,
JacobianMap& jacobians, Key key) { JacobianMap& jacobians, Key key) {
JacobianMap::iterator it = jacobians.find(key); jacobians(key).block < ROWS, COLS > (0, 0) += dTdA; // block makes HUGE difference
it->second.block<ROWS, COLS>(0, 0) += dTdA; // block makes HUGE difference
} }
/// Handle Leaf Case for Dynamic Matrix type (slower) /// Handle Leaf Case for Dynamic Matrix type (slower)
template<> template<>
void handleLeafCase( void handleLeafCase(
const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>& dTdA, const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>& dTdA,
JacobianMap& jacobians, Key key) { JacobianMap& jacobians, Key key) {
JacobianMap::iterator it = jacobians.find(key); jacobians(key) += dTdA;
it->second += dTdA;
} }
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------

7
gtsam_unstable/nonlinear/Expression.h
View File

@ -123,7 +123,7 @@ public:
} }
/// Return value and derivatives, reverse AD version /// Return value and derivatives, reverse AD version
T reverse(const Values& values, JacobianMap& jacobians) const { T value(const Values& values, JacobianMap& jacobians) const {
// The following piece of code is absolutely crucial for performance. // The following piece of code is absolutely crucial for performance.
// We allocate a block of memory on the stack, which can be done at runtime // We allocate a block of memory on the stack, which can be done at runtime
// with modern C++ compilers. The traceExecution then fills this memory // with modern C++ compilers. The traceExecution then fills this memory
@ -142,11 +142,6 @@ public:
return root_->value(values); return root_->value(values);
} }
/// Return value and derivatives
T value(const Values& values, JacobianMap& jacobians) const {
return reverse(values, jacobians);
}
const boost::shared_ptr<ExpressionNode<T> >& root() const { const boost::shared_ptr<ExpressionNode<T> >& root() const {
return root_; return root_;
} }

71
gtsam_unstable/nonlinear/ExpressionFactor.h
View File

@ -75,7 +75,7 @@ public:
} }
/** /**
* Error function *without* the NoiseModel, \f$ z-h(x) \f$. * Error function *without* the NoiseModel, \f$ h(x)-z \f$.
* We override this method to provide * We override this method to provide
* both the function evaluation and its derivative(s) in H. * both the function evaluation and its derivative(s) in H.
*/ */
@ -85,17 +85,19 @@ public:
// H should be pre-allocated // H should be pre-allocated
assert(H->size()==size()); assert(H->size()==size());
// Create and zero out blocks to be passed to expression_ VerticalBlockMatrix Ab(dimensions_, Dim);
JacobianMap blocks;
for (DenseIndex i = 0; i < size(); i++) { // Wrap keys and VerticalBlockMatrix into structure passed to expression_
Matrix& Hi = H->at(i); JacobianMap map(keys_, Ab);
Hi.resize(Dim, dimensions_[i]); Ab.matrix().setZero();
Hi.setZero(); // zero out
Eigen::Block<Matrix> block = Hi.block(0, 0, Dim, dimensions_[i]); // Evaluate error to get Jacobians and RHS vector b
blocks.insert(std::make_pair(keys_[i], block)); T value = expression_.value(x, map); // <<< Reverse AD happens here !
}
// Copy blocks into the vector of jacobians passed in
for (DenseIndex i = 0; i < size(); i++)
H->at(i) = Ab(i);
T value = expression_.value(x, blocks);
return measurement_.localCoordinates(value); return measurement_.localCoordinates(value);
} else { } else {
const T& value = expression_.value(x); const T& value = expression_.value(x);
@ -105,41 +107,34 @@ public:
virtual boost::shared_ptr<GaussianFactor> linearize(const Values& x) const { virtual boost::shared_ptr<GaussianFactor> linearize(const Values& x) const {
// This method has been heavily optimized for maximum performance. // Only linearize if the factor is active
// We allocate a VerticalBlockMatrix on the stack first, and then create if (!active(x))
// Eigen::Block<Matrix> views on this piece of memory which is then passed return boost::shared_ptr<JacobianFactor>();
// to [expression_.value] below, which writes directly into Ab_.
// Another malloc saved by creating a Matrix on the stack // Create a writeable JacobianFactor in advance
double memory[Dim * augmentedCols_]; // In case noise model is constrained, we need to provide a noise model
Eigen::Map<Eigen::Matrix<double, Dim, Eigen::Dynamic> > // bool constrained = noiseModel_->is_constrained();
matrix(memory, Dim, augmentedCols_); boost::shared_ptr<JacobianFactor> factor(
matrix.setZero(); // zero out constrained ? new JacobianFactor(keys_, dimensions_, Dim,
boost::static_pointer_cast<noiseModel::Constrained>(noiseModel_)->unit()) :
new JacobianFactor(keys_, dimensions_, Dim));
// Construct block matrix, is of right size but un-initialized // Wrap keys and VerticalBlockMatrix into structure passed to expression_
VerticalBlockMatrix Ab(dimensions_, matrix, true); VerticalBlockMatrix& Ab = factor->matrixObject();
JacobianMap map(keys_, Ab);
// Create blocks into Ab_ to be passed to expression_ // Zero out Jacobian so we can simply add to it
JacobianMap blocks; Ab.matrix().setZero();
for (DenseIndex i = 0; i < size(); i++)
blocks.insert(std::make_pair(keys_[i], Ab(i)));
// Evaluate error to get Jacobians and RHS vector b // Evaluate error to get Jacobians and RHS vector b
T value = expression_.value(x, blocks); // <<< Reverse AD happens here ! T value = expression_.value(x, map); // <<< Reverse AD happens here !
Ab(size()).col(0) = -measurement_.localCoordinates(value); Ab(size()).col(0) = -measurement_.localCoordinates(value);
// Whiten the corresponding system now // Whiten the corresponding system, Ab already contains RHS
// TODO ! this->noiseModel_->WhitenSystem(Ab); Vector dummy(Dim);
noiseModel_->WhitenSystem(Ab.matrix(),dummy);
// TODO pass unwhitened + noise model to Gaussian factor return 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);
} }
}; };
// ExpressionFactor // ExpressionFactor

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

@ -46,11 +46,8 @@ static const Rot3 someR = Rot3::RzRyRx(1, 2, 3);
TEST(Expression, constant) { TEST(Expression, constant) {
Expression<Rot3> R(someR); Expression<Rot3> R(someR);
Values values; Values values;
JacobianMap actualMap; Rot3 actual = R.value(values);
Rot3 actual = R.value(values, actualMap);
EXPECT(assert_equal(someR, actual)); EXPECT(assert_equal(someR, actual));
JacobianMap expected;
EXPECT(actualMap == expected);
EXPECT_LONGS_EQUAL(0, R.traceSize()) EXPECT_LONGS_EQUAL(0, R.traceSize())
} }
@ -61,15 +58,8 @@ TEST(Expression, Leaf) {
Values values; Values values;
values.insert(100, someR); values.insert(100, someR);
JacobianMap expected; Rot3 actual2 = R.value(values);
Matrix H = eye(3);
expected.insert(make_pair(100, H.block(0, 0, 3, 3)));
JacobianMap actualMap2;
actualMap2.insert(make_pair(100, H.block(0, 0, 3, 3)));
Rot3 actual2 = R.reverse(values, actualMap2);
EXPECT(assert_equal(someR, actual2)); EXPECT(assert_equal(someR, actual2));
EXPECT(actualMap2 == expected);
} }
/* ************************************************************************* */ /* ************************************************************************* */

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

@ -28,6 +28,9 @@
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <boost/assign/list_of.hpp>
using boost::assign::list_of;
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
@ -62,41 +65,41 @@ TEST(ExpressionFactor, Leaf) {
EXPECT( assert_equal(*old.linearize(values), *gf2, 1e-9)); EXPECT( assert_equal(*old.linearize(values), *gf2, 1e-9));
} }
///* ************************************************************************* */ /* ************************************************************************* */
//// non-zero noise model // non-zero noise model
//TEST(ExpressionFactor, Model) { TEST(ExpressionFactor, Model) {
// using namespace leaf; using namespace leaf;
//
// SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.01)); SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.01));
//
// // Create old-style factor to create expected value and derivatives // Create old-style factor to create expected value and derivatives
// PriorFactor<Point2> old(2, Point2(0, 0), model); PriorFactor<Point2> old(2, Point2(0, 0), model);
//
// // Concise version // Concise version
// ExpressionFactor<Point2> f(model, Point2(0, 0), p); ExpressionFactor<Point2> f(model, Point2(0, 0), p);
// EXPECT_DOUBLES_EQUAL(old.error(values), f.error(values), 1e-9); EXPECT_DOUBLES_EQUAL(old.error(values), f.error(values), 1e-9);
// EXPECT_LONGS_EQUAL(2, f.dim()); EXPECT_LONGS_EQUAL(2, f.dim());
// boost::shared_ptr<GaussianFactor> gf2 = f.linearize(values); boost::shared_ptr<GaussianFactor> gf2 = f.linearize(values);
// EXPECT( assert_equal(*old.linearize(values), *gf2, 1e-9)); EXPECT( assert_equal(*old.linearize(values), *gf2, 1e-9));
//} }
//
///* ************************************************************************* */ /* ************************************************************************* */
//// Constrained noise model // Constrained noise model
//TEST(ExpressionFactor, Constrained) { TEST(ExpressionFactor, Constrained) {
// using namespace leaf; using namespace leaf;
//
// SharedDiagonal model = noiseModel::Constrained::MixedSigmas(Vector2(0.2, 0)); SharedDiagonal model = noiseModel::Constrained::MixedSigmas(Vector2(0.2, 0));
//
// // Create old-style factor to create expected value and derivatives // Create old-style factor to create expected value and derivatives
// PriorFactor<Point2> old(2, Point2(0, 0), model); PriorFactor<Point2> old(2, Point2(0, 0), model);
//
// // Concise version // Concise version
// ExpressionFactor<Point2> f(model, Point2(0, 0), p); ExpressionFactor<Point2> f(model, Point2(0, 0), p);
// EXPECT_DOUBLES_EQUAL(old.error(values), f.error(values), 1e-9); EXPECT_DOUBLES_EQUAL(old.error(values), f.error(values), 1e-9);
// EXPECT_LONGS_EQUAL(2, f.dim()); EXPECT_LONGS_EQUAL(2, f.dim());
// boost::shared_ptr<GaussianFactor> gf2 = f.linearize(values); boost::shared_ptr<GaussianFactor> gf2 = f.linearize(values);
// EXPECT( assert_equal(*old.linearize(values), *gf2, 1e-9)); EXPECT( assert_equal(*old.linearize(values), *gf2, 1e-9));
//} }
/* ************************************************************************* */ /* ************************************************************************* */
// Unary(Leaf)) // Unary(Leaf))

2
gtsam_unstable/timing/timeCameraExpression.cpp
View File

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

2
gtsam_unstable/timing/timeOneCameraExpression.cpp
View File

@ -23,7 +23,7 @@
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
#define time timeMultiThreaded #define time timeSingleThreaded
int main() { int main() {