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

Merged from branch 'trunk' into 'isam2-chain-optimization'

release/4.3a0
Richard Roberts 2013-01-08 16:32:00 +00:00
parent 3cf45fccba 4f4898beb1
commit 494b022382
16 changed files with 365 additions and 157 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

168
.cproject
View File

@ -1,5 +1,7 @@
<?xml version="1.0" encoding="UTF-8" standalone="no"?>
<?fileVersion 4.0.0?><cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage">
<?fileVersion 4.0.0?>
<cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage">
<storageModule moduleId="org.eclipse.cdt.core.settings">
<cconfiguration id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544">
<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544" moduleId="org.eclipse.cdt.core.settings" name="MacOSX GCC">
@ -1505,90 +1507,82 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/testStereoCamera.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>tests/testStereoCamera.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/testRot3M.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>tests/testRot3M.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/testPoint3.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>tests/testPoint3.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/testCalibratedCamera.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>tests/testCalibratedCamera.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/timeRot3.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>tests/timeRot3.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/timePose3.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>tests/timePose3.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/timeStereoCamera.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>tests/timeStereoCamera.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="geometry.testHomography2.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j1 VERBOSE=1</buildArguments>
<buildTarget>geometry.testHomography2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="geometry.testPoint2.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<target name="testStereoCamera.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>geometry.testPoint2.run</buildTarget>
<buildTarget>testStereoCamera.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="geometry.testPose2.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<target name="testRot3M.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>geometry.testPose2.run</buildTarget>
<buildTarget>testRot3M.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="geometry.testPose3.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<target name="testPoint3.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>geometry.testPose3.run</buildTarget>
<buildTarget>testPoint3.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testCalibratedCamera.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testCalibratedCamera.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="timeRot3.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>timeRot3.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="timePose3.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>timePose3.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="timeStereoCamera.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>timeStereoCamera.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testPoint2.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testPoint2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testPose2.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testPose2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testPose3.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testPose3.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
@ -1753,6 +1747,38 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="timeMatrixOps" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>timeMatrixOps</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="timeTest" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>timeTest</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="timeVirtual" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>timeVirtual</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="timeVirtual2" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>timeVirtual2</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testVectorValues.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>

47
gtsam.h
View File

@ -84,38 +84,44 @@
* - TODO: Handle gtsam::Rot3M conversions to quaternions
*/
/*namespace std {
namespace std {
#include <vector>
template<T>
//Module.cpp needs to be changed to allow lowercase classnames
class vector
{
//Do we need these?
//Capacity
size_t size() const;
/*size_t size() const;
size_t max_size() const;
void resize(size_t sz, T c = T());
//void resize(size_t sz);
size_t capacity() const;
bool empty() const;
void reserve(size_t n);
//Element acces
T& at(size_t n);
const T& at(size_t n) const;
T& front();
const T& front() const;
T& back();
const T& back() const;
T* at(size_t n);
T* front();
T* back();
//Modifiers
void assign(size_t n, const T& u);
void push_back(const T& x);
void pop_back();
void pop_back();*/
};
}*/
//typedef std::vector
#include<list>
template<T>
class list
{
};
}
namespace gtsam {
//typedef std::vector<Index> IndexVector;
//*************************************************************************
// base
//*************************************************************************
@ -255,6 +261,7 @@ virtual class Point2 : gtsam::Value {
double y() const;
Vector vector() const;
double dist(const gtsam::Point2& p2) const;
double norm() const;
};
virtual class StereoPoint2 : gtsam::Value {
@ -383,7 +390,7 @@ virtual class Rot3 : gtsam::Value {
// Group
static gtsam::Rot3 identity();
gtsam::Rot3 inverse() const;
gtsam::Rot3 inverse() const;
gtsam::Rot3 compose(const gtsam::Rot3& p2) const;
gtsam::Rot3 between(const gtsam::Rot3& p2) const;
@ -777,7 +784,7 @@ virtual class BayesNet {
// Standard interface
size_t size() const;
void printStats(string s) const;
void saveGraph(string s) const;
void saveGraph(string s) const;
CONDITIONAL* front() const;
CONDITIONAL* back() const;
void push_back(CONDITIONAL* conditional);
@ -840,6 +847,7 @@ virtual class BayesTreeClique {
#include <gtsam/inference/SymbolicFactorGraph.h>
typedef gtsam::BayesNet<gtsam::IndexConditional> SymbolicBayesNetBase;
virtual class SymbolicBayesNet : gtsam::SymbolicBayesNetBase {
// Standard Constructors and Named Constructors
SymbolicBayesNet();
@ -1350,13 +1358,13 @@ virtual class ConjugateGradientParameters : gtsam::IterativeOptimizationParamete
void setReset(size_t value);
void setEpsilon_rel(double value);
void setEpsilon_abs(double value);
void print(string s);
void print();
};
#include <gtsam/linear/SubgraphSolver.h>
virtual class SubgraphSolverParameters : gtsam::ConjugateGradientParameters {
SubgraphSolverParameters();
void print(string s) const;
void print() const;
};
class SubgraphSolver {
@ -1936,6 +1944,8 @@ template<POSE, LANDMARK, CALIBRATION>
virtual class GenericProjectionFactor : gtsam::NonlinearFactor {
GenericProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel,
size_t poseKey, size_t pointKey, const CALIBRATION* k);
GenericProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel,
size_t poseKey, size_t pointKey, const CALIBRATION* k, const POSE& body_P_sensor);
gtsam::Point2 measured() const;
CALIBRATION* calibration() const;
};
@ -1999,6 +2009,7 @@ namespace utilities {
void perturbPoint3(gtsam::Values& values, double sigma, int seed);
void insertBackprojections(gtsam::Values& values, const gtsam::SimpleCamera& c, Vector J, Matrix Z, double depth);
void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K);
void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K, const gtsam::Pose3& body_P_sensor);
Matrix reprojectionErrors(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values);
} //\namespace utilities

7
gtsam/base/CMakeLists.txt
View File

@ -9,8 +9,11 @@ set(base_local_libs
# Files to exclude from compilation of tests and timing scripts
set(base_excluded_files
# "${CMAKE_CURRENT_SOURCE_DIR}/tests/testTypedDiscreteFactor.cpp" # Example of excluding a test
"" # Add to this list, with full path, to exclude
# "${CMAKE_CURRENT_SOURCE_DIR}/tests/testTypedDiscreteFactor.cpp" # Example of excluding a test
"${CMAKE_CURRENT_SOURCE_DIR}/tests/timeTest.cpp"
"${CMAKE_CURRENT_SOURCE_DIR}/tests/timeVirtual.cpp"
"${CMAKE_CURRENT_SOURCE_DIR}/tests/timeVirtual2.cpp"
# "" # Add to this list, with full path, to exclude
)
# Build tests

45
gtsam/base/LieMatrix.h
View File

@ -31,6 +31,9 @@ namespace gtsam {
*/
struct LieMatrix : public Matrix, public DerivedValue<LieMatrix> {
/// @name Constructors
/// @{
/** default constructor - should be unnecessary */
LieMatrix() {}
@ -48,10 +51,9 @@ struct LieMatrix : public Matrix, public DerivedValue<LieMatrix> {
/** Specify arguments directly, as in Matrix_() - always force these to be doubles */
LieMatrix(size_t m, size_t n, ...);
/** get the underlying vector */
inline Matrix matrix() const {
return static_cast<Matrix>(*this);
}
/// @}
/// @name Testable interface
/// @{
/** print @param s optional string naming the object */
inline void print(const std::string& name="") const {
@ -62,8 +64,19 @@ struct LieMatrix : public Matrix, public DerivedValue<LieMatrix> {
inline bool equals(const LieMatrix& expected, double tol=1e-5) const {
return gtsam::equal_with_abs_tol(matrix(), expected.matrix(), tol);
}
/// @}
/// @name Standard Interface
/// @{
// Manifold requirements
/** get the underlying vector */
inline Matrix matrix() const {
return static_cast<Matrix>(*this);
}
/// @}
/// @name Manifold interface
/// @{
/** Returns dimensionality of the tangent space */
inline size_t dim() const { return this->size(); }
@ -72,22 +85,26 @@ struct LieMatrix : public Matrix, public DerivedValue<LieMatrix> {
* tangent space vector correspond to the matrix entries arranged in
* *row-major* order. */
inline LieMatrix retract(const Vector& v) const {
if(v.size() != this->rows() * this->cols())
if(v.size() != this->size())
throw std::invalid_argument("LieMatrix::retract called with Vector of incorrect size");
return LieMatrix(*this + Eigen::Map<const Matrix>(&v(0), this->cols(), this->rows()).transpose());
return LieMatrix(*this +
Eigen::Map<const Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor> >(
&v(0), this->rows(), this->cols()));
}
/** @return the local coordinates of another object. The elements of the
* tangent space vector correspond to the matrix entries arranged in
* *row-major* order. */
inline Vector localCoordinates(const LieMatrix& t2) const {
Vector result(this->rows() * this->cols());
Eigen::Map<Matrix>(&result(0), this->cols(), this->rows()).transpose() =
(t2 - *this);
Vector result(this->size());
Eigen::Map<Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor> >(
&result(0), this->rows(), this->cols()) = t2 - *this;
return result;
}
// Group requirements
/// @}
/// @name Group interface
/// @{
/** identity - NOTE: no known size at compile time - so zero length */
inline static LieMatrix identity() {
@ -125,7 +142,9 @@ struct LieMatrix : public Matrix, public DerivedValue<LieMatrix> {
return LieMatrix(-(*this));
}
// Lie functions
/// @}
/// @name Lie group interface
/// @{
/** Expmap around identity */
static inline LieMatrix Expmap(const Vector& v) {
@ -135,6 +154,8 @@ struct LieMatrix : public Matrix, public DerivedValue<LieMatrix> {
/** Logmap around identity - just returns with default cast back */
static inline Vector Logmap(const LieMatrix& p) {
return Eigen::Map<const Vector>(&p(0,0), p.dim()); }
/// @}
private:

12
gtsam/base/tests/timeMatrixOps.cpp
View File

@ -38,8 +38,8 @@ using namespace boost::lambda;
static boost::variate_generator<boost::mt19937, boost::uniform_real<> > rng(boost::mt19937(), boost::uniform_real<>(-1.0, 0.0));
//typedef ublas::matrix<double> matrix;
//typedef ublas::matrix_range<matrix> matrix_range;
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> matrix;
typedef Eigen::Block<matrix> matrix_block;
//typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> matrix;
//typedef Eigen::Block<matrix> matrix_block;
//using ublas::range;
//using ublas::triangular_matrix;
@ -57,10 +57,10 @@ int main(int argc, char* argv[]) {
assert(m > n);
boost::variate_generator<boost::mt19937, boost::uniform_int<size_t> > rni(boost::mt19937(), boost::uniform_int<size_t>(0,m-1));
boost::variate_generator<boost::mt19937, boost::uniform_int<size_t> > rnj(boost::mt19937(), boost::uniform_int<size_t>(0,n-1));
matrix mat(m,n);
matrix_block full = mat.block(0, 0, m, n);
matrix_block top = mat.block(0, 0, n, n);
matrix_block block = mat.block(m/4, n/4, m-m/2, n-n/2);
gtsam::Matrix mat((int)m,(int)n);
gtsam::SubMatrix full = mat.block(0, 0, m, n);
gtsam::SubMatrix top = mat.block(0, 0, n, n);
gtsam::SubMatrix block = mat.block(m/4, n/4, m-m/2, n-n/2);
cout << format(" Basic: %1%x%2%\n") % m % n;
cout << format(" Full: mat(%1%:%2%, %3%:%4%)\n") % 0 % m % 0 % n;

1
gtsam/base/tests/timeTest.cpp
View File

@ -19,6 +19,7 @@
int main(int argc, char *argv[]) {
// FIXME: ticPush_ does not exist
ticPush_("1", "top 1");
ticPush_("1", "sub 1");
gttic_("sub sub a");

11
gtsam/base/types.h
View File

@ -85,13 +85,16 @@ namespace gtsam {
#ifdef _MSC_VER
// Define some common g++ functions and macros that MSVC does not have
// Define some common g++ functions and macros we use that MSVC does not have
#include <boost/math/special_functions/fpclassify.hpp>
namespace std {
using boost::math::isfinite;
using boost::math::isnan;
using boost::math::isinf;
template<typename T> inline int isfinite(T a) {
return (int)boost::math::isfinite(a); }
template<typename T> inline int isnan(T a) {
return (int)boost::math::isnan(a); }
template<typename T> inline int isinf(T a) {
return (int)boost::math::isinf(a); }
}
#include <boost/math/constants/constants.hpp>

35
gtsam/geometry/tests/testPose2.cpp
View File

@ -109,30 +109,33 @@ TEST(Pose2, expmap3) {
}
/* ************************************************************************* */
TEST(Pose2, expmap0) {
Pose2 pose(M_PI/2.0, Point2(1, 2));
//#ifdef SLOW_BUT_CORRECT_EXPMAP
Pose2 expected(1.01491, 2.01013, 1.5888);
//#else
// Pose2 expected(M_PI/2.0+0.018, Point2(1.015, 2.01));
//#endif
Pose2 actual = pose * (Pose2::Expmap(Vector_(3, 0.01, -0.015, 0.018)));
TEST(Pose2, expmap0a) {
Pose2 expected(0.0101345, -0.0149092, 0.018);
Pose2 actual = Pose2::Expmap(Vector_(3, 0.01, -0.015, 0.018));
EXPECT(assert_equal(expected, actual, 1e-5));
}
/* ************************************************************************* */
TEST(Pose2, expmap0_full) {
Pose2 pose(M_PI/2.0, Point2(1, 2));
Pose2 expected(1.01491, 2.01013, 1.5888);
Pose2 actual = pose * Pose2::Expmap(Vector_(3, 0.01, -0.015, 0.018));
TEST(Pose2, expmap0b) {
// a quarter turn
Pose2 expected(1.0, 1.0, M_PI/2);
Pose2 actual = Pose2::Expmap(Vector_(3, M_PI/2, 0.0, M_PI/2));
EXPECT(assert_equal(expected, actual, 1e-5));
}
/* ************************************************************************* */
TEST(Pose2, expmap0_full2) {
Pose2 pose(M_PI/2.0, Point2(1, 2));
Pose2 expected(1.01491, 2.01013, 1.5888);
Pose2 actual = pose * Pose2::Expmap(Vector_(3, 0.01, -0.015, 0.018));
TEST(Pose2, expmap0c) {
// a half turn
Pose2 expected(0.0, 2.0, M_PI);
Pose2 actual = Pose2::Expmap(Vector_(3, M_PI, 0.0, M_PI));
EXPECT(assert_equal(expected, actual, 1e-5));
}
/* ************************************************************************* */
TEST(Pose2, expmap0d) {
// a full turn
Pose2 expected(0, 0, 0);
Pose2 actual = Pose2::Expmap(Vector_(3, 2*M_PI, 0.0, 2*M_PI));
EXPECT(assert_equal(expected, actual, 1e-5));
}

6
gtsam/linear/CMakeLists.txt
View File

@ -13,8 +13,10 @@ set(linear_local_libs
# Files to exclude from compilation of tests and timing scripts
set(linear_excluded_files
# "${CMAKE_CURRENT_SOURCE_DIR}/tests/testTypedDiscreteFactor.cpp" # Example of excluding a test
"" # Add to this list, with full path, to exclude
# "${CMAKE_CURRENT_SOURCE_DIR}/tests/testTypedDiscreteFactor.cpp" # Example of excluding a test
"${CMAKE_CURRENT_SOURCE_DIR}/tests/timeSLAMlike.cpp"
"${CMAKE_CURRENT_SOURCE_DIR}/tests/timeFactorOverhead.cpp"
# "" # Add to this list, with full path, to exclude
)
# Build tests

12
gtsam/linear/ConjugateGradientSolver.h
View File

@ -61,14 +61,14 @@ public:
inline void setEpsilon_rel(double value) { epsilon_rel_ = value; }
inline void setEpsilon_abs(double value) { epsilon_abs_ = value; }
virtual void print(const std::string &s="") const {
virtual void print() const {
Base::print();
std::cout << "ConjugateGradientParameters" << std::endl
<< "minIter: " << minIterations_ << std::endl
<< "maxIter: " << maxIterations_ << std::endl
<< "resetIter: " << reset_ << std::endl
<< "eps_rel: " << epsilon_rel_ << std::endl
<< "eps_abs: " << epsilon_abs_ << std::endl;
<< "minIter: " << minIterations_ << std::endl
<< "maxIter: " << maxIterations_ << std::endl
<< "resetIter: " << reset_ << std::endl
<< "eps_rel: " << epsilon_rel_ << std::endl
<< "eps_abs: " << epsilon_abs_ << std::endl;
}
};

7
gtsam/linear/IterativeSolver.h
View File

@ -48,11 +48,10 @@ namespace gtsam {
void setKernel(const std::string &s) ;
void setVerbosity(const std::string &s) ;
void print() const {
virtual void print() const {
std::cout << "IterativeOptimizationParameters" << std::endl
<< "kernel: " << kernelTranslator(kernel_) << std::endl
<< "verbosity: " << verbosityTranslator(verbosity_) << std::endl
<< std::endl;
<< "kernel: " << kernelTranslator(kernel_) << std::endl
<< "verbosity: " << verbosityTranslator(verbosity_) << std::endl;
}
static Kernel kernelTranslator(const std::string &s);

2
gtsam/linear/SubgraphSolver.h
View File

@ -23,7 +23,7 @@ class SubgraphSolverParameters : public ConjugateGradientParameters {
public:
typedef ConjugateGradientParameters Base;
SubgraphSolverParameters() : Base() {}
virtual void print(const std::string &s="") const { Base::print(s); }
virtual void print() const { Base::print(); }
};
/**

2
gtsam/linear/tests/timeSLAMlike.cpp
View File

@ -17,7 +17,7 @@
*/
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/SharedDiagonal.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/linear/GaussianSequentialSolver.h>
#include <gtsam/linear/GaussianMultifrontalSolver.h>

136
gtsam/nonlinear/NonlinearFactorGraph.cpp
View File

@ -18,9 +18,12 @@
*/
#include <cmath>
#include <limits>
#include <boost/foreach.hpp>
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/inference/inference.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
using namespace std;
@ -43,24 +46,135 @@ void NonlinearFactorGraph::print(const std::string& str, const KeyFormatter& key
}
/* ************************************************************************* */
void NonlinearFactorGraph::saveGraph(std::ostream &stm, const KeyFormatter& keyFormatter) const {
void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values,
const GraphvizFormatting& graphvizFormatting,
const KeyFormatter& keyFormatter) const
{
stm << "graph {\n";
stm << " size=\"" << graphvizFormatting.figureWidthInches << "," <<
graphvizFormatting.figureHeightInches << "\";\n\n";
FastSet<Key> keys = this->keys();
// Local utility function to extract x and y coordinates
struct { boost::optional<Point2> operator()(
const Value& value, const GraphvizFormatting& graphvizFormatting)
{
if(const Pose2* p = dynamic_cast<const Pose2*>(&value)) {
double x, y;
switch (graphvizFormatting.paperHorizontalAxis) {
case GraphvizFormatting::X: x = p->x(); break;
case GraphvizFormatting::Y: x = p->y(); break;
case GraphvizFormatting::Z: x = 0.0; break;
case GraphvizFormatting::NEGX: x = -p->x(); break;
case GraphvizFormatting::NEGY: x = -p->y(); break;
case GraphvizFormatting::NEGZ: x = 0.0; break;
}
switch (graphvizFormatting.paperVerticalAxis) {
case GraphvizFormatting::X: y = p->x(); break;
case GraphvizFormatting::Y: y = p->y(); break;
case GraphvizFormatting::Z: y = 0.0; break;
case GraphvizFormatting::NEGX: y = -p->x(); break;
case GraphvizFormatting::NEGY: y = -p->y(); break;
case GraphvizFormatting::NEGZ: y = 0.0; break;
}
return Point2(x,y);
} else if(const Pose3* p = dynamic_cast<const Pose3*>(&value)) {
double x, y;
switch (graphvizFormatting.paperHorizontalAxis) {
case GraphvizFormatting::X: x = p->x(); break;
case GraphvizFormatting::Y: x = p->y(); break;
case GraphvizFormatting::Z: x = p->z(); break;
case GraphvizFormatting::NEGX: x = -p->x(); break;
case GraphvizFormatting::NEGY: x = -p->y(); break;
case GraphvizFormatting::NEGZ: x = -p->z(); break;
}
switch (graphvizFormatting.paperVerticalAxis) {
case GraphvizFormatting::X: y = p->x(); break;
case GraphvizFormatting::Y: y = p->y(); break;
case GraphvizFormatting::Z: y = p->z(); break;
case GraphvizFormatting::NEGX: y = -p->x(); break;
case GraphvizFormatting::NEGY: y = -p->y(); break;
case GraphvizFormatting::NEGZ: y = -p->z(); break;
}
return Point2(x,y);
} else {
return boost::none;
}
}} getXY;
// Find bounds
double minX = numeric_limits<double>::infinity(), maxX = -numeric_limits<double>::infinity();
double minY = numeric_limits<double>::infinity(), maxY = -numeric_limits<double>::infinity();
BOOST_FOREACH(Key key, keys) {
if(values.exists(key)) {
boost::optional<Point2> xy = getXY(values.at(key), graphvizFormatting);
if(xy) {
if(xy->x() < minX)
minX = xy->x();
if(xy->x() > maxX)
maxX = xy->x();
if(xy->y() < minY)
minY = xy->y();
if(xy->y() > maxY)
maxY = xy->y();
}
}
}
// Create nodes for each variable in the graph
BOOST_FOREACH(Key key, this->keys()) {
BOOST_FOREACH(Key key, keys) {
// Label the node with the label from the KeyFormatter
stm << " var" << key << "[label=\"" << keyFormatter(key) << "\"];\n"; }
stm << " var" << key << "[label=\"" << keyFormatter(key) << "\"";
if(values.exists(key)) {
boost::optional<Point2> xy = getXY(values.at(key), graphvizFormatting);
if(xy)
stm << ", pos=\"" << graphvizFormatting.scale*(xy->x() - minX) << "," << graphvizFormatting.scale*(xy->y() - minY) << "!\"";
}
stm << "];\n";
}
stm << "\n";
// Create factors and variable connections
for(size_t i = 0; i < this->size(); ++i) {
// Make each factor a dot
stm << " factor" << i << "[label=\"\", shape=point];\n";
if(graphvizFormatting.mergeSimilarFactors) {
// Remove duplicate factors
FastSet<vector<Key> > structure;
BOOST_FOREACH(const sharedFactor& factor, *this) {
if(factor) {
vector<Key> factorKeys = factor->keys();
std::sort(factorKeys.begin(), factorKeys.end());
structure.insert(factorKeys);
}
}
// Make factor-variable connections
if(this->at(i)) {
BOOST_FOREACH(Key key, *this->at(i)) {
stm << " var" << key << "--" << "factor" << i << ";\n"; } }
// Create factors and variable connections
size_t i = 0;
BOOST_FOREACH(const vector<Key>& factorKeys, structure) {
// Make each factor a dot
stm << " factor" << i << "[label=\"\", shape=point";
{
map<size_t, Point2>::const_iterator pos = graphvizFormatting.factorPositions.find(i);
if(pos != graphvizFormatting.factorPositions.end())
stm << ", pos=\"" << graphvizFormatting.scale*(pos->second.x() - minX) << "," << graphvizFormatting.scale*(pos->second.y() - minY) << "!\"";
}
stm << "];\n";
// Make factor-variable connections
BOOST_FOREACH(Key key, factorKeys) {
stm << " var" << key << "--" << "factor" << i << ";\n"; }
++ i;
}
} else {
// Create factors and variable connections
for(size_t i = 0; i < this->size(); ++i) {
// Make each factor a dot
stm << " factor" << i << "[label=\"\", shape=point];\n";
// Make factor-variable connections
if(this->at(i)) {
BOOST_FOREACH(Key key, *this->at(i)) {
stm << " var" << key << "--" << "factor" << i << ";\n"; } }
}
}
stm << "}\n";

27
gtsam/nonlinear/NonlinearFactorGraph.h
View File

@ -21,12 +21,35 @@
#pragma once
#include <gtsam/geometry/Point2.h>
#include <gtsam/inference/SymbolicFactorGraph.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
namespace gtsam {
/**
* Formatting options when saving in GraphViz format using
* NonlinearFactorGraph::saveGraph.
*/
struct GraphvizFormatting {
enum Axis { X, Y, Z, NEGX, NEGY, NEGZ }; ///< World axes to be assigned to paper axes
Axis paperHorizontalAxis; ///< The world axis assigned to the horizontal paper axis
Axis paperVerticalAxis; ///< The world axis assigned to the vertical paper axis
double figureWidthInches; ///< The figure width on paper in inches
double figureHeightInches; ///< The figure height on paper in inches
double scale; ///< Scale all positions to reduce / increase density
bool mergeSimilarFactors; ///< Merge multiple factors that have the same connectivity
std::map<size_t, Point2> factorPositions; ///< (optional for each factor) Manually specify factor "dot" positions.
/// Default constructor sets up robot coordinates. Paper horizontal is robot Y,
/// paper vertical is robot X. Default figure size of 5x5 in.
GraphvizFormatting() :
paperHorizontalAxis(Y), paperVerticalAxis(X),
figureWidthInches(5), figureHeightInches(5), scale(1),
mergeSimilarFactors(false) {}
};
/**
* A non-linear factor graph is a graph of non-Gaussian, i.e. non-linear factors,
* which derive from NonlinearFactor. The values structures are typically (in SAM) more general
@ -47,7 +70,9 @@ namespace gtsam {
void print(const std::string& str = "NonlinearFactorGraph: ", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
/** Write the graph in GraphViz format for visualization */
void saveGraph(std::ostream& stm, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
void saveGraph(std::ostream& stm, const Values& values = Values(),
const GraphvizFormatting& graphvizFormatting = GraphvizFormatting(),
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
/** return keys as an ordered set - ordering is by key value */
FastSet<Key> keys() const;

4
matlab.h
View File

@ -114,14 +114,14 @@ namespace gtsam {
/// insert multiple projection factors for a single pose key
void insertProjectionFactors(NonlinearFactorGraph& graph, Key i, const Vector& J, const Matrix& Z,
const SharedNoiseModel& model, const shared_ptrK K) {
const SharedNoiseModel& model, const shared_ptrK K, const Pose3& body_P_sensor = Pose3()) {
if (Z.rows() != 2) throw std::invalid_argument("addMeasurements: Z must be 2*K");
if (Z.cols() != J.size()) throw std::invalid_argument(
"addMeasurements: J and Z must have same number of entries");
for (int k = 0; k < Z.cols(); k++) {
graph.push_back(
boost::make_shared<GenericProjectionFactor<Pose3, Point3> >
(Point2(Z(0, k), Z(1, k)), model, i, Key(J(k)), K));
(Point2(Z(0, k), Z(1, k)), model, i, Key(J(k)), K, body_P_sensor));
}
}