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

Merge branch 'develop' into feature/adding-scripts-simplify

release/4.3a0
Richard Roberts 2014-02-13 15:25:27 -05:00
parent f283150eb4 e1b4e3c67a
commit 72ec292022
12 changed files with 44 additions and 113 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
CMakeLists.txt
View File

@ -118,6 +118,7 @@ else()
if(Boost_TIMER_LIBRARY)
list(APPEND GTSAM_BOOST_LIBRARIES ${Boost_TIMER_LIBRARY} ${Boost_CHRONO_LIBRARY})
else()
list(APPEND GTSAM_BOOST_LIBRARIES rt) # When using the header-only boost timer library, need -lrt
message("WARNING: GTSAM timing instrumentation will use the older, less accurate, Boost timer library because boost older than 1.48 was found.")
endif()
endif()

90
gtsam/base/SymmetricBlockMatrix.h
View File

@ -61,32 +61,32 @@ namespace gtsam {
/// Construct from a container of the sizes of each block.
template<typename CONTAINER>
SymmetricBlockMatrix(const CONTAINER& dimensions) :
SymmetricBlockMatrix(const CONTAINER& dimensions, bool appendOneDimension = false) :
blockStart_(0)
{
fillOffsets(dimensions.begin(), dimensions.end());
fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension);
matrix_.resize(variableColOffsets_.back(), variableColOffsets_.back());
assertInvariants();
}
/// Construct from iterator over the sizes of each vertical block.
template<typename ITERATOR>
SymmetricBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim) :
SymmetricBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim, bool appendOneDimension = false) :
blockStart_(0)
{
fillOffsets(firstBlockDim, lastBlockDim);
fillOffsets(firstBlockDim, lastBlockDim, appendOneDimension);
matrix_.resize(variableColOffsets_.back(), variableColOffsets_.back());
assertInvariants();
}
/// Construct from a container of the sizes of each vertical block and a pre-prepared matrix.
template<typename CONTAINER>
SymmetricBlockMatrix(const CONTAINER& dimensions, const Matrix& matrix) :
SymmetricBlockMatrix(const CONTAINER& dimensions, const Matrix& matrix, bool appendOneDimension = false) :
blockStart_(0)
{
matrix_.resize(matrix.rows(), matrix.cols());
matrix_.triangularView<Eigen::Upper>() = matrix.triangularView<Eigen::Upper>();
fillOffsets(dimensions.begin(), dimensions.end());
fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension);
if(matrix_.rows() != matrix_.cols())
throw std::invalid_argument("Requested to create a SymmetricBlockMatrix from a non-square matrix.");
if(variableColOffsets_.back() != matrix_.cols())
@ -211,85 +211,21 @@ namespace gtsam {
return variableColOffsets_[block + blockStart_];
}
//void checkRange(DenseIndex i_startBlock, DenseIndex i_endBlock, DenseIndex j_startBlock, DenseIndex j_endBlock) const
//{
// const DenseIndex i_actualStartBlock = i_startBlock + blockStart_;
// const DenseIndex i_actualEndBlock = i_endBlock + blockStart_;
// const DenseIndex j_actualStartBlock = j_startBlock + blockStart_;
// const DenseIndex j_actualEndBlock = j_endBlock + blockStart_;
// checkBlock(i_actualStartBlock);
// checkBlock(j_actualStartBlock);
// if(i_startBlock != 0 || i_endBlock != 0) {
// checkBlock(i_actualStartBlock);
// assert(i_actualEndBlock < (DenseIndex)variableColOffsets_.size());
// }
// if(j_startBlock != 0 || j_endBlock != 0) {
// checkBlock(j_actualStartBlock);
// assert(j_actualEndBlock < (DenseIndex)variableColOffsets_.size());
// }
//}
//void checkRange(DenseIndex startBlock, DenseIndex endBlock) const
//{
// const DenseIndex actualStartBlock = startBlock + blockStart_;
// const DenseIndex actualEndBlock = endBlock + blockStart_;
// checkBlock(actualStartBlock);
// if(startBlock != 0 || endBlock != 0) {
// checkBlock(actualStartBlock);
// assert(actualEndBlock < (DenseIndex)variableColOffsets_.size());
// }
//}
//Block rangeUnchecked(DenseIndex i_startBlock, DenseIndex i_endBlock, DenseIndex j_startBlock, DenseIndex j_endBlock)
//{
// const DenseIndex i_actualStartBlock = i_startBlock + blockStart_;
// const DenseIndex i_actualEndBlock = i_endBlock + blockStart_;
// const DenseIndex j_actualStartBlock = j_startBlock + blockStart_;
// const DenseIndex j_actualEndBlock = j_endBlock + blockStart_;
// return Block(matrix(),
// variableColOffsets_[i_actualStartBlock],
// variableColOffsets_[j_actualStartBlock],
// variableColOffsets_[i_actualEndBlock] - variableColOffsets_[i_actualStartBlock],
// variableColOffsets_[j_actualEndBlock] - variableColOffsets_[j_actualStartBlock]);
//}
//constBlock rangeUnchecked(DenseIndex i_startBlock, DenseIndex i_endBlock, DenseIndex j_startBlock, DenseIndex j_endBlock) const
//{
// // Convert Block to constBlock
// const Block block = const_cast<This*>(this)->rangeUnchecked(i_startBlock, i_endBlock, j_startBlock, j_endBlock);
// return constBlock(matrix(), block.Base::Base::, block.startCol(), block.rows(), block.cols());
//}
//Block rangeUnchecked(DenseIndex startBlock, DenseIndex endBlock)
//{
// const DenseIndex actualStartBlock = startBlock + blockStart_;
// const DenseIndex actualEndBlock = endBlock + blockStart_;
// return Block(matrix(),
// variableColOffsets_[actualStartBlock],
// variableColOffsets_[actualStartBlock],
// variableColOffsets_[actualEndBlock] - variableColOffsets_[actualStartBlock],
// variableColOffsets_[actualEndBlock] - variableColOffsets_[actualStartBlock]);
//}
//constBlock rangeUnchecked(DenseIndex startBlock, DenseIndex endBlock) const
//{
// // Convert Block to constBlock
// const Block block = const_cast<This*>(this)->rangeUnchecked(startBlock, endBlock);
// return constBlock(matrix(), block.startRow(), block.startCol(), block.rows(), block.cols());
//}
template<typename ITERATOR>
void fillOffsets(ITERATOR firstBlockDim, ITERATOR lastBlockDim)
void fillOffsets(ITERATOR firstBlockDim, ITERATOR lastBlockDim, bool appendOneDimension)
{
variableColOffsets_.resize((lastBlockDim-firstBlockDim)+1);
variableColOffsets_.resize((lastBlockDim-firstBlockDim) + 1 + (appendOneDimension ? 1 : 0));
variableColOffsets_[0] = 0;
DenseIndex j=0;
for(ITERATOR dim=firstBlockDim; dim!=lastBlockDim; ++dim) {
variableColOffsets_[j+1] = variableColOffsets_[j] + *dim;
++ j;
}
if(appendOneDimension)
{
variableColOffsets_[j+1] = variableColOffsets_[j] + 1;
++ j;
}
}
friend class VerticalBlockMatrix;

21
gtsam/base/VerticalBlockMatrix.h
View File

@ -65,20 +65,20 @@ namespace gtsam {
/** Construct from a container of the sizes of each vertical block. */
template<typename CONTAINER>
VerticalBlockMatrix(const CONTAINER& dimensions, DenseIndex height) :
VerticalBlockMatrix(const CONTAINER& dimensions, DenseIndex height, bool appendOneDimension = false) :
rowStart_(0), rowEnd_(height), blockStart_(0)
{
fillOffsets(dimensions.begin(), dimensions.end());
fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension);
matrix_.resize(height, variableColOffsets_.back());
assertInvariants();
}
/** Construct from a container of the sizes of each vertical block and a pre-prepared matrix. */
template<typename CONTAINER>
VerticalBlockMatrix(const CONTAINER& dimensions, const Matrix& matrix) :
VerticalBlockMatrix(const CONTAINER& dimensions, const Matrix& matrix, bool appendOneDimension = false) :
matrix_(matrix), rowStart_(0), rowEnd_(matrix.rows()), blockStart_(0)
{
fillOffsets(dimensions.begin(), dimensions.end());
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.");
assertInvariants();
@ -87,10 +87,10 @@ namespace gtsam {
/**
* Construct from iterator over the sizes of each vertical block. */
template<typename ITERATOR>
VerticalBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim, DenseIndex height) :
VerticalBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim, DenseIndex height, bool appendOneDimension = false) :
rowStart_(0), rowEnd_(height), blockStart_(0)
{
fillOffsets(firstBlockDim, lastBlockDim);
fillOffsets(firstBlockDim, lastBlockDim, appendOneDimension);
matrix_.resize(height, variableColOffsets_.back());
assertInvariants();
}
@ -202,14 +202,19 @@ namespace gtsam {
}
template<typename ITERATOR>
void fillOffsets(ITERATOR firstBlockDim, ITERATOR lastBlockDim) {
variableColOffsets_.resize((lastBlockDim-firstBlockDim)+1);
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) {
variableColOffsets_[j+1] = variableColOffsets_[j] + *dim;
++ j;
}
if(appendOneDimension)
{
variableColOffsets_[j+1] = variableColOffsets_[j] + 1;
++ j;
}
}
friend class SymmetricBlockMatrix;

4
gtsam/geometry/Rot3.cpp
View File

@ -45,10 +45,10 @@ Rot3 Rot3::rodriguez(const Sphere2& w, double theta) {
}
/* ************************************************************************* */
Rot3 Rot3::Random(boost::random::mt19937 & rng) {
Rot3 Rot3::Random(boost::mt19937 & rng) {
// TODO allow any engine without including all of boost :-(
Sphere2 w = Sphere2::Random(rng);
boost::random::uniform_real_distribution<double> randomAngle(-M_PI,M_PI);
boost::uniform_real<double> randomAngle(-M_PI,M_PI);
double angle = randomAngle(rng);
return rodriguez(w,angle);
}

2
gtsam/geometry/Rot3.h
View File

@ -104,7 +104,7 @@ namespace gtsam {
Rot3(const Quaternion& q);
/// Random, generates a random axis, then random angle \in [-p,pi]
static Rot3 Random(boost::random::mt19937 & rng);
static Rot3 Random(boost::mt19937 & rng);
/** Virtual destructor */
virtual ~Rot3() {}

11
gtsam/geometry/Sphere2.cpp
View File

@ -22,6 +22,7 @@
#include <gtsam/geometry/Point2.h>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_on_sphere.hpp>
#include <boost/random/variate_generator.hpp>
#include <iostream>
using namespace std;
@ -44,10 +45,14 @@ Sphere2 Sphere2::FromPoint3(const Point3& point,
}
/* ************************************************************************* */
Sphere2 Sphere2::Random(boost::random::mt19937 & rng) {
Sphere2 Sphere2::Random(boost::mt19937 & rng) {
// TODO allow any engine without including all of boost :-(
boost::random::uniform_on_sphere<double> randomDirection(3);
vector<double> d = randomDirection(rng);
boost::uniform_on_sphere<double> randomDirection(3);
// This variate_generator object is required for versions of boost somewhere
// around 1.46, instead of drawing directly using boost::uniform_on_sphere(rng).
boost::variate_generator<boost::mt19937&, boost::uniform_on_sphere<double> >
generator(rng, randomDirection);
vector<double> d = generator();
Sphere2 result;
result.p_ = Point3(d[0], d[1], d[2]);
return result;

15
gtsam/geometry/Sphere2.h
View File

@ -22,23 +22,12 @@
#include <gtsam/geometry/Point3.h>
#include <gtsam/base/DerivedValue.h>
#include <boost/random/mersenne_twister.hpp>
#ifndef SPHERE2_DEFAULT_COORDINATES_MODE
#define SPHERE2_DEFAULT_COORDINATES_MODE Sphere2::RENORM
#endif
// (Cumbersome) forward declaration for random generator
namespace boost {
namespace random {
template<class UIntType, std::size_t w, std::size_t n, std::size_t m,
std::size_t r, UIntType a, std::size_t u, UIntType d, std::size_t s,
UIntType b, std::size_t t, UIntType c, std::size_t l, UIntType f>
class mersenne_twister_engine;
typedef mersenne_twister_engine<uint32_t, 32, 624, 397, 31, 0x9908b0df, 11,
0xffffffff, 7, 0x9d2c5680, 15, 0xefc60000, 18, 1812433253> mt19937;
}
}
namespace gtsam {
/// Represents a 3D point on a unit sphere.
@ -75,7 +64,7 @@ public:
boost::optional<Matrix&> H = boost::none);
/// Random direction, using boost::uniform_on_sphere
static Sphere2 Random(boost::random::mt19937 & rng);
static Sphere2 Random(boost::mt19937 & rng);
/// @}

2
gtsam/geometry/tests/testSphere2.cpp
View File

@ -312,7 +312,7 @@ TEST(Sphere2, localCoordinates_retract_expmap) {
//*******************************************************************************
TEST(Sphere2, Random) {
boost::random::mt19937 rng(42);
boost::mt19937 rng(42);
// Check that is deterministic given same random seed
Point3 expected(-0.667578, 0.671447, 0.321713);
Point3 actual = Sphere2::Random(rng).point3();

3
gtsam/linear/GaussianConditional-inl.h
View File

@ -19,9 +19,6 @@
#pragma once
#include <boost/range/join.hpp>
#include <boost/assign/list_of.hpp>
namespace gtsam {
/* ************************************************************************* */

3
gtsam/linear/HessianFactor.cpp
View File

@ -42,7 +42,6 @@
#include <boost/assign/list_of.hpp>
#include <boost/range/adaptor/transformed.hpp>
#include <boost/range/adaptor/map.hpp>
#include <boost/range/join.hpp>
#include <boost/range/algorithm/copy.hpp>
#include <sstream>
@ -179,7 +178,7 @@ DenseIndex _getSizeHF(const Vector& m) { return m.size(); }
/* ************************************************************************* */
HessianFactor::HessianFactor(const std::vector<Key>& js, const std::vector<Matrix>& Gs,
const std::vector<Vector>& gs, double f) :
GaussianFactor(js), info_(br::join(gs | br::transformed(&_getSizeHF), ListOfOne((DenseIndex)1)))
GaussianFactor(js), info_(gs | br::transformed(&_getSizeHF), true)
{
// Get the number of variables
size_t variable_count = js.size();

3
gtsam/linear/JacobianFactor-inl.h
View File

@ -21,7 +21,6 @@
#include <gtsam/linear/linearExceptions.h>
#include <boost/assign/list_of.hpp>
#include <boost/range/adaptor/transformed.hpp>
#include <boost/range/join.hpp>
#include <boost/range/algorithm/for_each.hpp>
#include <boost/foreach.hpp>
@ -123,7 +122,7 @@ namespace gtsam {
// matrices, then extract the number of columns e.g. dimensions in each matrix. Then joins with
// a single '1' to add a dimension for the b vector.
{
Ab_ = VerticalBlockMatrix(br::join(terms | transformed(&internal::getColsJF), ListOfOne((DenseIndex)1)), b.size());
Ab_ = VerticalBlockMatrix(terms | transformed(&internal::getColsJF), b.size(), true);
}
// Check and add terms

2
gtsam/linear/JacobianFactor.cpp
View File

@ -283,7 +283,7 @@ namespace gtsam {
// Allocate matrix and copy keys in order
gttic(allocate);
Ab_ = VerticalBlockMatrix(boost::join(varDims, ListOfOne((DenseIndex)1)), m); // Allocate augmented matrix
Ab_ = VerticalBlockMatrix(varDims, m, true); // Allocate augmented matrix
Base::keys_.resize(orderedSlots.size());
boost::range::copy( // Get variable keys
orderedSlots | boost::adaptors::indirected | boost::adaptors::map_keys, Base::keys_.begin());