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

Merge branch 'develop'

release/4.3a0
Luca 2015-01-26 10:20:50 -05:00
parent 8de7e2e0d2 b1cea2bee7
commit 3608429ae7
66 changed files with 1701 additions and 312 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
.cproject
View File

@ -1551,6 +1551,14 @@
<useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testLabeledSymbol.run" path="build/gtsam/inference/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testLabeledSymbol.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -2521,6 +2529,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testOptionalJacobian.run" path="build/gtsam/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testOptionalJacobian.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check.tests" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>

2
cmake/GtsamBuildTypes.cmake
View File

@ -54,7 +54,7 @@ if(NOT FIRST_PASS_DONE)
endif()
# Clang on Mac uses a template depth that is less than standard and is too small
if("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang")
if(${CMAKE_CXX_COMPILER_ID} STREQUAL "Clang")
if(NOT "${CMAKE_CXX_COMPILER_VERSION}" VERSION_LESS "5.0")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -ftemplate-depth=1024")
endif()

3
examples/RangeISAMExample_plaza2.cpp
View File

@ -82,7 +82,8 @@ vector<RangeTriple> readTriples() {
ifstream is(data_file.c_str());
while (is) {
double t, sender, receiver, range;
double t, sender, range;
size_t receiver;
is >> t >> sender >> receiver >> range;
triples.push_back(RangeTriple(t, receiver, range));
}

8
examples/TimeTBB.cpp
View File

@ -134,7 +134,7 @@ map<int, double> testWithMemoryAllocation()
tbb::parallel_for(tbb::blocked_range<size_t>(0, numberOfProblems), WorkerWithAllocation(results));
tbb::tick_count t1 = tbb::tick_count::now();
cout << "With memory allocation, grain size = " << grainSize << ", time = " << (t1 - t0).seconds() << endl;
timingResults[grainSize] = (t1 - t0).seconds();
timingResults[(int)grainSize] = (t1 - t0).seconds();
}
return timingResults;
@ -152,9 +152,9 @@ int main(int argc, char* argv[])
BOOST_FOREACH(size_t n, numThreads)
{
cout << "With " << n << " threads:" << endl;
tbb::task_scheduler_init init(n);
results[n].grainSizesWithoutAllocation = testWithoutMemoryAllocation();
results[n].grainSizesWithAllocation = testWithMemoryAllocation();
tbb::task_scheduler_init init((int)n);
results[(int)n].grainSizesWithoutAllocation = testWithoutMemoryAllocation();
results[(int)n].grainSizesWithAllocation = testWithMemoryAllocation();
cout << endl;
}

79
gtsam.h
View File

@ -629,28 +629,13 @@ class Cal3_S2 {
void serialize() const;
};
#include <gtsam/geometry/Cal3DS2.h>
class Cal3DS2 {
#include <gtsam/geometry/Cal3DS2_Base.h>
virtual class Cal3DS2_Base {
// Standard Constructors
Cal3DS2();
Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2, double k3, double k4);
Cal3DS2(Vector v);
Cal3DS2_Base();
// Testable
void print(string s) const;
bool equals(const gtsam::Cal3DS2& rhs, double tol) const;
// Manifold
static size_t Dim();
size_t dim() const;
gtsam::Cal3DS2 retract(Vector v) const;
Vector localCoordinates(const gtsam::Cal3DS2& c) const;
// Action on Point2
gtsam::Point2 calibrate(const gtsam::Point2& p, double tol) const;
gtsam::Point2 calibrate(const gtsam::Point2& p) const;
gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
// TODO: D2d functions that start with an uppercase letter
// Standard Interface
double fx() const;
@ -658,14 +643,66 @@ class Cal3DS2 {
double skew() const;
double px() const;
double py() const;
Vector vector() const;
Vector k() const;
//Matrix K() const; //FIXME: Uppercase
double k1() const;
double k2() const;
// Action on Point2
gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
gtsam::Point2 calibrate(const gtsam::Point2& p, double tol) const;
gtsam::Point2 calibrate(const gtsam::Point2& p) const;
// enabling serialization functionality
void serialize() const;
};
#include <gtsam/geometry/Cal3DS2.h>
virtual class Cal3DS2 : gtsam::Cal3DS2_Base {
// Standard Constructors
Cal3DS2();
Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2);
Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2, double p1, double p2);
Cal3DS2(Vector v);
// Testable
bool equals(const gtsam::Cal3DS2& rhs, double tol) const;
// Manifold
size_t dim() const;
static size_t Dim();
gtsam::Cal3DS2 retract(Vector v) const;
Vector localCoordinates(const gtsam::Cal3DS2& c) const;
// enabling serialization functionality
void serialize() const;
};
#include <gtsam/geometry/Cal3Unified.h>
virtual class Cal3Unified : gtsam::Cal3DS2_Base {
// Standard Constructors
Cal3Unified();
Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1, double k2);
Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1, double k2, double p1, double p2, double xi);
Cal3Unified(Vector v);
// Testable
bool equals(const gtsam::Cal3Unified& rhs, double tol) const;
// Standard Interface
double xi() const;
gtsam::Point2 spaceToNPlane(const gtsam::Point2& p) const;
gtsam::Point2 nPlaneToSpace(const gtsam::Point2& p) const;
// Manifold
size_t dim() const;
static size_t Dim();
gtsam::Cal3Unified retract(Vector v) const;
Vector localCoordinates(const gtsam::Cal3Unified& c) const;
// enabling serialization functionality
void serialize() const;
};
#include <gtsam/geometry/Cal3_S2Stereo.h>
class Cal3_S2Stereo {
// Standard Constructors
Cal3_S2Stereo();

4
gtsam/3rdparty/metis/CMakeLists.txt vendored
View File

@ -2,14 +2,14 @@ cmake_minimum_required(VERSION 2.8)
project(METIS)
# Add flags for currect directory and below
if ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang")
if (${CMAKE_CXX_COMPILER_ID} STREQUAL "Clang")
add_definitions(-Wno-unused-variable)
if (CMAKE_CXX_COMPILER_VERSION VERSION_GREATER 5.0 OR CMAKE_CXX_COMPILER_VERSION VERSION_EQUAL 5.0)
add_definitions(-Wno-sometimes-uninitialized)
endif()
endif()
if(NOT ("${CMAKE_C_COMPILER_ID}" MATCHES "MSVC" OR "${CMAKE_CXX_COMPILER_ID}" MATCHES "MSVC"))
if(NOT (${CMAKE_C_COMPILER_ID} MATCHES "MSVC" OR ${CMAKE_CXX_COMPILER_ID} MATCHES "MSVC"))
#add_definitions(-Wno-unknown-pragmas)
endif()

2
gtsam/base/Manifold.h
View File

@ -71,7 +71,7 @@ struct HasManifoldPrereqs {
};
/// Extra manifold traits for fixed-dimension types
template<class Class, size_t N>
template<class Class, int N>
struct ManifoldImpl {
// Compile-time dimensionality
static int GetDimension(const Class&) {

2
gtsam/base/Matrix.h
View File

@ -238,7 +238,7 @@ Eigen::Block<const MATRIX> sub(const MATRIX& A, size_t i1, size_t i2, size_t j1,
* @param j is the column of the upper left corner insert location
*/
template <typename Derived1, typename Derived2>
GTSAM_EXPORT void insertSub(Eigen::MatrixBase<Derived1>& fullMatrix, const Eigen::MatrixBase<Derived2>& subMatrix, size_t i, size_t j) {
void insertSub(Eigen::MatrixBase<Derived1>& fullMatrix, const Eigen::MatrixBase<Derived2>& subMatrix, size_t i, size_t j) {
fullMatrix.block(i, j, subMatrix.rows(), subMatrix.cols()) = subMatrix;
}

4
gtsam/base/OptionalJacobian.h
View File

@ -100,7 +100,7 @@ public:
/// Return true is allocated, false if default constructor was used
operator bool() const {
return map_.data();
return map_.data() != NULL;
}
/// De-reference, like boost optional
@ -156,7 +156,7 @@ public:
/// Return true is allocated, false if default constructor was used
operator bool() const {
return pointer_;
return pointer_!=NULL;
}
/// De-reference, like boost optional

3
gtsam/geometry/Cal3Bundler.h
View File

@ -174,4 +174,7 @@ private:
template<>
struct traits<Cal3Bundler> : public internal::Manifold<Cal3Bundler> {};
template<>
struct traits<const Cal3Bundler> : public internal::Manifold<Cal3Bundler> {};
} // namespace gtsam

17
gtsam/geometry/Cal3DS2.h
View File

@ -68,7 +68,7 @@ public:
/// @{
/// print with optional string
void print(const std::string& s = "") const ;
virtual void print(const std::string& s = "") const ;
/// assert equality up to a tolerance
bool equals(const Cal3DS2& K, double tol = 10e-9) const;
@ -89,10 +89,20 @@ public:
/// Return dimensions of calibration manifold object
static size_t Dim() { return 9; } //TODO: make a final dimension variable
/// @}
/// @name Clone
/// @{
/// @return a deep copy of this object
virtual boost::shared_ptr<Base> clone() const {
return boost::shared_ptr<Base>(new Cal3DS2(*this));
}
/// @}
private:
/// @}
/// @name Advanced Interface
/// @{
@ -112,5 +122,8 @@ private:
template<>
struct traits<Cal3DS2> : public internal::Manifold<Cal3DS2> {};
template<>
struct traits<const Cal3DS2> : public internal::Manifold<Cal3DS2> {};
}

28
gtsam/geometry/Cal3DS2_Base.h
View File

@ -45,9 +45,6 @@ protected:
double p1_, p2_ ; // tangential distortion
public:
Matrix3 K() const ;
Vector4 k() const { return Vector4(k1_, k2_, p1_, p2_); }
Vector9 vector() const ;
/// @name Standard Constructors
/// @{
@ -59,6 +56,8 @@ public:
double k1, double k2, double p1 = 0.0, double p2 = 0.0) :
fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0), k1_(k1), k2_(k2), p1_(p1), p2_(p2) {}
virtual ~Cal3DS2_Base() {}
/// @}
/// @name Advanced Constructors
/// @{
@ -70,7 +69,7 @@ public:
/// @{
/// print with optional string
void print(const std::string& s = "") const ;
virtual void print(const std::string& s = "") const ;
/// assert equality up to a tolerance
bool equals(const Cal3DS2_Base& K, double tol = 10e-9) const;
@ -106,6 +105,15 @@ public:
/// Second tangential distortion coefficient
inline double p2() const { return p2_;}
/// return calibration matrix -- not really applicable
Matrix3 K() const;
/// return distortion parameter vector
Vector4 k() const { return Vector4(k1_, k2_, p1_, p2_); }
/// Return all parameters as a vector
Vector9 vector() const;
/**
* convert intrinsic coordinates xy to (distorted) image coordinates uv
* @param p point in intrinsic coordinates
@ -126,9 +134,19 @@ public:
/// Derivative of uncalibrate wrpt the calibration parameters
Matrix29 D2d_calibration(const Point2& p) const ;
private:
/// @}
/// @name Clone
/// @{
/// @return a deep copy of this object
virtual boost::shared_ptr<Cal3DS2_Base> clone() const {
return boost::shared_ptr<Cal3DS2_Base>(new Cal3DS2_Base(*this));
}
/// @}
private:
/// @name Advanced Interface
/// @{

13
gtsam/geometry/Cal3Unified.h
View File

@ -50,9 +50,8 @@ private:
double xi_; // mirror parameter
public:
enum { dimension = 10 };
Vector10 vector() const ;
enum { dimension = 10 };
/// @name Standard Constructors
/// @{
@ -77,7 +76,7 @@ public:
/// @{
/// print with optional string
void print(const std::string& s = "") const ;
virtual void print(const std::string& s = "") const ;
/// assert equality up to a tolerance
bool equals(const Cal3Unified& K, double tol = 10e-9) const;
@ -125,6 +124,11 @@ public:
/// Return dimensions of calibration manifold object
static size_t Dim() { return 10; } //TODO: make a final dimension variable
/// Return all parameters as a vector
Vector10 vector() const ;
/// @}
private:
/** Serialization function */
@ -142,5 +146,8 @@ private:
template<>
struct traits<Cal3Unified> : public internal::Manifold<Cal3Unified> {};
template<>
struct traits<const Cal3Unified> : public internal::Manifold<Cal3Unified> {};
}

3
gtsam/geometry/Cal3_S2.h
View File

@ -226,4 +226,7 @@ private:
template<>
struct traits<Cal3_S2> : public internal::Manifold<Cal3_S2> {};
template<>
struct traits<const Cal3_S2> : public internal::Manifold<Cal3_S2> {};
} // \ namespace gtsam

4
gtsam/geometry/Cal3_S2Stereo.h
View File

@ -158,4 +158,8 @@ namespace gtsam {
struct traits<Cal3_S2Stereo> : public internal::Manifold<Cal3_S2Stereo> {
};
template<>
struct traits<const Cal3_S2Stereo> : public internal::Manifold<Cal3_S2Stereo> {
};
} // \ namespace gtsam

3
gtsam/geometry/CalibratedCamera.h
View File

@ -204,5 +204,8 @@ private:
template<>
struct traits<CalibratedCamera> : public internal::Manifold<CalibratedCamera> {};
template<>
struct traits<const CalibratedCamera> : public internal::Manifold<CalibratedCamera> {};
}

3
gtsam/geometry/EssentialMatrix.h
View File

@ -201,5 +201,8 @@ private:
template<>
struct traits<EssentialMatrix> : public internal::Manifold<EssentialMatrix> {};
template<>
struct traits<const EssentialMatrix> : public internal::Manifold<EssentialMatrix> {};
} // gtsam

3
gtsam/geometry/PinholeCamera.h
View File

@ -499,4 +499,7 @@ private:
template<typename Calibration>
struct traits< PinholeCamera<Calibration> > : public internal::Manifold<PinholeCamera<Calibration> > {};
template<typename Calibration>
struct traits< const PinholeCamera<Calibration> > : public internal::Manifold<PinholeCamera<Calibration> > {};
} // \ gtsam

3
gtsam/geometry/Point3.h
View File

@ -197,4 +197,7 @@ namespace gtsam {
template<>
struct traits<Point3> : public internal::VectorSpace<Point3> {};
template<>
struct traits<const Point3> : public internal::VectorSpace<Point3> {};
}

3
gtsam/geometry/Pose2.h
View File

@ -293,5 +293,8 @@ GTSAM_EXPORT boost::optional<Pose2> align(const std::vector<Point2Pair>& pairs);
template<>
struct traits<Pose2> : public internal::LieGroupTraits<Pose2> {};
template<>
struct traits<const Pose2> : public internal::LieGroupTraits<Pose2> {};
} // namespace gtsam

7
gtsam/geometry/Pose3.h
View File

@ -323,7 +323,10 @@ typedef std::pair<Point3, Point3> Point3Pair;
GTSAM_EXPORT boost::optional<Pose3> align(const std::vector<Point3Pair>& pairs);
template<>
struct traits<Pose3> : public internal::LieGroupTraits<Pose3> {
};
struct traits<Pose3> : public internal::LieGroupTraits<Pose3> {};
template<>
struct traits<const Pose3> : public internal::LieGroupTraits<Pose3> {};
} // namespace gtsam

3
gtsam/geometry/Rot2.h
View File

@ -210,4 +210,7 @@ namespace gtsam {
template<>
struct traits<Rot2> : public internal::LieGroupTraits<Rot2> {};
template<>
struct traits<const Rot2> : public internal::LieGroupTraits<Rot2> {};
} // gtsam

3
gtsam/geometry/Rot3.h
View File

@ -463,5 +463,8 @@ namespace gtsam {
template<>
struct traits<Rot3> : public internal::LieGroupTraits<Rot3> {};
template<>
struct traits<const Rot3> : public internal::LieGroupTraits<Rot3> {};
}

3
gtsam/geometry/SO3.h
View File

@ -85,6 +85,7 @@ public:
template<>
struct traits<SO3> : public internal::LieGroupTraits<SO3> {};
template<>
struct traits<const SO3> : public internal::LieGroupTraits<SO3> {};
} // end namespace gtsam

2
gtsam/geometry/StereoCamera.h
View File

@ -149,4 +149,6 @@ private:
template<>
struct traits<StereoCamera> : public internal::Manifold<StereoCamera> {};
template<>
struct traits<const StereoCamera> : public internal::Manifold<StereoCamera> {};
}

3
gtsam/geometry/StereoPoint2.h
View File

@ -176,4 +176,7 @@ namespace gtsam {
template<>
struct traits<StereoPoint2> : public internal::Manifold<StereoPoint2> {};
template<>
struct traits<const StereoPoint2> : public internal::Manifold<StereoPoint2> {};
}

10
gtsam/geometry/Unit3.h
View File

@ -155,7 +155,13 @@ private:
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(p_);
ar & BOOST_SERIALIZATION_NVP(B_);
// homebrew serialize Eigen Matrix
ar & boost::serialization::make_nvp("B11", (*B_)(0,0));
ar & boost::serialization::make_nvp("B12", (*B_)(0,1));
ar & boost::serialization::make_nvp("B21", (*B_)(1,0));
ar & boost::serialization::make_nvp("B22", (*B_)(1,1));
ar & boost::serialization::make_nvp("B31", (*B_)(2,0));
ar & boost::serialization::make_nvp("B32", (*B_)(2,1));
}
/// @}
@ -165,5 +171,7 @@ private:
// Define GTSAM traits
template <> struct traits<Unit3> : public internal::Manifold<Unit3> {};
template <> struct traits<const Unit3> : public internal::Manifold<Unit3> {};
} // namespace gtsam

14
gtsam/geometry/tests/testSerializationGeometry.cpp
View File

@ -19,6 +19,8 @@
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Unit3.h>
#include <gtsam/geometry/EssentialMatrix.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/Cal3_S2Stereo.h>
#include <gtsam/geometry/CalibratedCamera.h>
@ -41,6 +43,9 @@ static Point3 pt3(1.0, 2.0, 3.0);
static Rot3 rt3 = Rot3::RzRyRx(1.0, 3.0, 2.0);
static Pose3 pose3(rt3, pt3);
static Unit3 unit3(1.0, 2.1, 3.4);
static EssentialMatrix ematrix(rt3, unit3);
static Cal3_S2 cal1(1.0, 2.0, 0.3, 0.1, 0.5);
static Cal3DS2 cal2(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0);
static Cal3Bundler cal3(1.0, 2.0, 3.0);
@ -59,6 +64,9 @@ TEST (Serialization, text_geometry) {
EXPECT(equalsObj<gtsam::Pose2>(Pose2(1.0, 2.0, 0.3)));
EXPECT(equalsObj<gtsam::Rot2>(Rot2::fromDegrees(30.0)));
EXPECT(equalsObj<gtsam::Unit3>(Unit3(1.0, 2.1, 3.4)));
EXPECT(equalsObj<gtsam::EssentialMatrix>(EssentialMatrix(rt3, unit3)));
EXPECT(equalsObj(pt3));
EXPECT(equalsObj<gtsam::Rot3>(rt3));
EXPECT(equalsObj<gtsam::Pose3>(Pose3(rt3, pt3)));
@ -81,6 +89,9 @@ TEST (Serialization, xml_geometry) {
EXPECT(equalsXML<gtsam::Pose2>(Pose2(1.0, 2.0, 0.3)));
EXPECT(equalsXML<gtsam::Rot2>(Rot2::fromDegrees(30.0)));
EXPECT(equalsXML<gtsam::Unit3>(Unit3(1.0, 2.1, 3.4)));
EXPECT(equalsXML<gtsam::EssentialMatrix>(EssentialMatrix(rt3, unit3)));
EXPECT(equalsXML<gtsam::Point3>(pt3));
EXPECT(equalsXML<gtsam::Rot3>(rt3));
EXPECT(equalsXML<gtsam::Pose3>(Pose3(rt3, pt3)));
@ -102,6 +113,9 @@ TEST (Serialization, binary_geometry) {
EXPECT(equalsBinary<gtsam::Pose2>(Pose2(1.0, 2.0, 0.3)));
EXPECT(equalsBinary<gtsam::Rot2>(Rot2::fromDegrees(30.0)));
EXPECT(equalsBinary<gtsam::Unit3>(Unit3(1.0, 2.1, 3.4)));
EXPECT(equalsBinary<gtsam::EssentialMatrix>(EssentialMatrix(rt3, unit3)));
EXPECT(equalsBinary<gtsam::Point3>(pt3));
EXPECT(equalsBinary<gtsam::Rot3>(rt3));
EXPECT(equalsBinary<gtsam::Pose3>(Pose3(rt3, pt3)));

22
gtsam/inference/LabeledSymbol.cpp
View File

@ -17,12 +17,8 @@
#include <iostream>
#include <boost/mpl/char.hpp>
#include <boost/format.hpp>
#include <boost/function.hpp>
#include <boost/lambda/bind.hpp>
#include <boost/lambda/construct.hpp>
#include <boost/lambda/lambda.hpp>
#include <boost/bind.hpp>
#include <boost/lexical_cast.hpp>
@ -111,23 +107,21 @@ bool LabeledSymbol::operator!=(gtsam::Key comp) const {
}
/* ************************************************************************* */
static LabeledSymbol make(gtsam::Key key) { return LabeledSymbol(key);}
boost::function<bool(gtsam::Key)> LabeledSymbol::TypeTest(unsigned char c) {
namespace bl = boost::lambda;
return bl::bind(&LabeledSymbol::chr, bl::bind(bl::constructor<LabeledSymbol>(), bl::_1)) == c;
return boost::bind(&LabeledSymbol::chr, boost::bind(make, _1)) == c;
}
/* ************************************************************************* */
boost::function<bool(gtsam::Key)> LabeledSymbol::LabelTest(unsigned char label) {
namespace bl = boost::lambda;
return bl::bind(&LabeledSymbol::label, bl::bind(bl::constructor<LabeledSymbol>(), bl::_1)) == label;
return boost::bind(&LabeledSymbol::label, boost::bind(make, _1)) == label;
}
/* ************************************************************************* */
boost::function<bool(gtsam::Key)> LabeledSymbol::TypeLabelTest(unsigned char c, unsigned char label) {
namespace bl = boost::lambda;
return bl::bind(&LabeledSymbol::chr, bl::bind(bl::constructor<LabeledSymbol>(), bl::_1)) == c &&
bl::bind(&LabeledSymbol::label, bl::bind(bl::constructor<LabeledSymbol>(), bl::_1)) == label;
return boost::bind(&LabeledSymbol::chr, boost::bind(make, _1)) == c &&
boost::bind(&LabeledSymbol::label, boost::bind(make, _1)) == label;
}
/* ************************************************************************* */
} // \namespace gtsam

19
gtsam/inference/Symbol.cpp
View File

@ -18,19 +18,8 @@
#include <gtsam/inference/Symbol.h>
#include <boost/mpl/char.hpp>
#include <boost/format.hpp>
#include <boost/function.hpp>
#include <boost/lambda/construct.hpp>
#ifdef __GNUC__
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-variable"
#endif
#include <boost/lambda/bind.hpp>
#include <boost/lambda/lambda.hpp>
#ifdef __GNUC__
#pragma GCC diagnostic pop
#endif
#include <boost/bind.hpp>
#include <limits.h>
#include <list>
@ -71,10 +60,10 @@ Symbol::operator std::string() const {
return str(boost::format("%c%d") % c_ % j_);
}
static Symbol make(gtsam::Key key) { return Symbol(key);}
boost::function<bool(Key)> Symbol::ChrTest(unsigned char c) {
namespace bl = boost::lambda;
return bl::bind(&Symbol::chr, bl::bind(bl::constructor<Symbol>(), bl::_1))
== c;
return bind(&Symbol::chr, bind(make, _1)) == c;
}
} // namespace gtsam

17
gtsam/inference/tests/testKey.cpp
View File

@ -14,14 +14,14 @@
* @author Alex Cunningham
*/
#include <boost/assign/std/list.hpp> // for operator +=
using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/inference/Symbol.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/assign/std/list.hpp> // for operator +=
using namespace boost::assign;
using namespace std;
using namespace gtsam;
@ -65,6 +65,13 @@ TEST(Key, KeySymbolEncoding) {
EXPECT(assert_equal(symbol, Symbol(key)));
}
/* ************************************************************************* */
TEST(Key, ChrTest) {
Key key = Symbol('c',3);
EXPECT(Symbol::ChrTest('c')(key));
EXPECT(!Symbol::ChrTest('d')(key));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

26
gtsam/inference/tests/testLabeledSymbol.cpp
View File

@ -14,20 +14,19 @@
* @author Alex Cunningham
*/
#include <boost/assign/std/list.hpp> // for operator +=
using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
#include <gtsam/inference/LabeledSymbol.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/inference/LabeledSymbol.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/assign/std/list.hpp> // for operator +=
using namespace boost::assign;
using namespace std;
using namespace gtsam;
/* ************************************************************************* */
TEST( testLabeledSymbol, KeyLabeledSymbolConversion ) {
TEST(LabeledSymbol, KeyLabeledSymbolConversion ) {
LabeledSymbol expected('x', 'A', 4);
Key key(expected);
LabeledSymbol actual(key);
@ -36,7 +35,7 @@ TEST( testLabeledSymbol, KeyLabeledSymbolConversion ) {
}
/* ************************************************************************* */
TEST( testLabeledSymbol, KeyLabeledSymbolEncoding ) {
TEST(LabeledSymbol, KeyLabeledSymbolEncoding ) {
// Test encoding of LabeledSymbol <-> size_t <-> string
// Encoding scheme:
@ -69,6 +68,17 @@ TEST( testLabeledSymbol, KeyLabeledSymbolEncoding ) {
}
}
/* ************************************************************************* */
TEST(LabeledSymbol, ChrTest) {
Key key = LabeledSymbol('c','A',3);
EXPECT(LabeledSymbol::TypeTest('c')(key));
EXPECT(!LabeledSymbol::TypeTest('d')(key));
EXPECT(LabeledSymbol::LabelTest('A')(key));
EXPECT(!LabeledSymbol::LabelTest('D')(key));
EXPECT(LabeledSymbol::TypeLabelTest('c','A')(key));
EXPECT(!LabeledSymbol::TypeLabelTest('c','D')(key));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

10
gtsam/linear/GaussianFactor.h
View File

@ -100,7 +100,7 @@ namespace gtsam {
/// Return the diagonal of the Hessian for this factor
virtual VectorValues hessianDiagonal() const = 0;
/// Return the diagonal of the Hessian for this factor (raw memory version)
/// Raw memory access version of hessianDiagonal
virtual void hessianDiagonal(double* d) const = 0;
/// Return the block diagonal of the Hessian for this factor
@ -122,16 +122,10 @@ namespace gtsam {
/// y += alpha * A'*A*x
virtual void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const = 0;
/// y += alpha * A'*A*x
virtual void multiplyHessianAdd(double alpha, const double* x, double* y, std::vector<size_t> keys) const = 0;
/// y += alpha * A'*A*x
virtual void multiplyHessianAdd(double alpha, const double* x, double* y) const = 0;
/// A'*b for Jacobian, eta for Hessian
virtual VectorValues gradientAtZero() const = 0;
/// A'*b for Jacobian, eta for Hessian (raw memory version)
/// Raw memory access version of gradientAtZero
virtual void gradientAtZero(double* d) const = 0;
/// Gradient wrt a key at any values

9
gtsam/linear/GaussianFactorGraph.cpp
View File

@ -357,15 +357,6 @@ namespace gtsam {
f->multiplyHessianAdd(alpha, x, y);
}
/* ************************************************************************* */
void GaussianFactorGraph::multiplyHessianAdd(double alpha,
const double* x, double* y) const {
vector<size_t> FactorKeys = getkeydim();
BOOST_FOREACH(const GaussianFactor::shared_ptr& f, *this)
f->multiplyHessianAdd(alpha, x, y, FactorKeys);
}
/* ************************************************************************* */
void GaussianFactorGraph::multiplyInPlace(const VectorValues& x, Errors& e) const {
multiplyInPlace(x, e.begin());

4
gtsam/linear/GaussianFactorGraph.h
View File

@ -310,10 +310,6 @@ namespace gtsam {
void multiplyHessianAdd(double alpha, const VectorValues& x,
VectorValues& y) const;
///** y += alpha*A'A*x */
void multiplyHessianAdd(double alpha, const double* x,
double* y) const;
///** In-place version e <- A*x that overwrites e. */
void multiplyInPlace(const VectorValues& x, Errors& e) const;

74
gtsam/linear/HessianFactor.cpp
View File

@ -359,20 +359,10 @@ VectorValues HessianFactor::hessianDiagonal() const {
}
/* ************************************************************************* */
// TODO: currently assumes all variables of the same size 9 and keys arranged from 0 to n
// Raw memory access version should be called in Regular Factors only currently
void HessianFactor::hessianDiagonal(double* d) const {
// Use eigen magic to access raw memory
typedef Eigen::Matrix<double, 9, 1> DVector;
typedef Eigen::Map<DVector> DMap;
// Loop over all variables in the factor
for (DenseIndex pos = 0; pos < (DenseIndex)size(); ++pos) {
Key j = keys_[pos];
// Get the diagonal block, and insert its diagonal
const Matrix& B = info_(pos, pos).selfadjointView();
DMap(d + 9 * j) += B.diagonal();
}
throw std::runtime_error(
"HessianFactor::hessianDiagonal raw memory access is allowed for Regular Factors only");
}
/* ************************************************************************* */
@ -548,48 +538,6 @@ void HessianFactor::multiplyHessianAdd(double alpha, const VectorValues& x,
}
}
/* ************************************************************************* */
void HessianFactor::multiplyHessianAdd(double alpha, const double* x,
double* yvalues, vector<size_t> offsets) const {
// Use eigen magic to access raw memory
typedef Eigen::Matrix<double, Eigen::Dynamic, 1> DVector;
typedef Eigen::Map<DVector> DMap;
typedef Eigen::Map<const DVector> ConstDMap;
// Create a vector of temporary y values, corresponding to rows i
vector<Vector> y;
y.reserve(size());
for (const_iterator it = begin(); it != end(); it++)
y.push_back(zero(getDim(it)));
// Accessing the VectorValues one by one is expensive
// So we will loop over columns to access x only once per column
// And fill the above temporary y values, to be added into yvalues after
for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) {
DenseIndex i = 0;
for (; i < j; ++i)
y[i] += info_(i, j).knownOffDiagonal()
* ConstDMap(x + offsets[keys_[j]],
offsets[keys_[j] + 1] - offsets[keys_[j]]);
// blocks on the diagonal are only half
y[i] += info_(j, j).selfadjointView()
* ConstDMap(x + offsets[keys_[j]],
offsets[keys_[j] + 1] - offsets[keys_[j]]);
// for below diagonal, we take transpose block from upper triangular part
for (i = j + 1; i < (DenseIndex) size(); ++i)
y[i] += info_(i, j).knownOffDiagonal()
* ConstDMap(x + offsets[keys_[j]],
offsets[keys_[j] + 1] - offsets[keys_[j]]);
}
// copy to yvalues
for (DenseIndex i = 0; i < (DenseIndex) size(); ++i)
DMap(yvalues + offsets[keys_[i]], offsets[keys_[i] + 1] - offsets[keys_[i]]) +=
alpha * y[i];
}
/* ************************************************************************* */
VectorValues HessianFactor::gradientAtZero() const {
VectorValues g;
@ -600,20 +548,10 @@ VectorValues HessianFactor::gradientAtZero() const {
}
/* ************************************************************************* */
// TODO: currently assumes all variables of the same size 9 and keys arranged from 0 to n
// Raw memory access version should be called in Regular Factors only currently
void HessianFactor::gradientAtZero(double* d) const {
// Use eigen magic to access raw memory
typedef Eigen::Matrix<double, 9, 1> DVector;
typedef Eigen::Map<DVector> DMap;
// Loop over all variables in the factor
for (DenseIndex pos = 0; pos < (DenseIndex)size(); ++pos) {
Key j = keys_[pos];
// Get the diagonal block, and insert its diagonal
DVector dj = -info_(pos,size()).knownOffDiagonal();
DMap(d + 9 * j) += dj;
}
throw std::runtime_error(
"HessianFactor::gradientAtZero raw memory access is allowed for Regular Factors only");
}
/* ************************************************************************* */

7
gtsam/linear/HessianFactor.h
View File

@ -340,7 +340,7 @@ namespace gtsam {
/// Return the diagonal of the Hessian for this factor
virtual VectorValues hessianDiagonal() const;
/* ************************************************************************* */
/// Raw memory access version of hessianDiagonal
virtual void hessianDiagonal(double* d) const;
/// Return the block diagonal of the Hessian for this factor
@ -380,13 +380,10 @@ namespace gtsam {
/** y += alpha * A'*A*x */
void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const;
void multiplyHessianAdd(double alpha, const double* x, double* y, std::vector<size_t> keys) const;
void multiplyHessianAdd(double alpha, const double* x, double* y) const {};
/// eta for Hessian
VectorValues gradientAtZero() const;
/// Raw memory access version of gradientAtZero
virtual void gradientAtZero(double* d) const;
/**

54
gtsam/linear/JacobianFactor.cpp
View File

@ -461,22 +461,9 @@ VectorValues JacobianFactor::hessianDiagonal() const {
}
/* ************************************************************************* */
// TODO: currently assumes all variables of the same size 9 and keys arranged from 0 to n
// Raw memory access version should be called in Regular Factors only currently
void JacobianFactor::hessianDiagonal(double* d) const {
// Use eigen magic to access raw memory
typedef Eigen::Matrix<double, 9, 1> DVector;
typedef Eigen::Map<DVector> DMap;
// Loop over all variables in the factor
for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) {
// Get the diagonal block, and insert its diagonal
DVector dj;
for (size_t k = 0; k < 9; ++k)
dj(k) = Ab_(j).col(k).squaredNorm();
DMap(d + 9 * j) += dj;
}
throw std::runtime_error("JacobianFactor::hessianDiagonal raw memory access is allowed for Regular Factors only");
}
/* ************************************************************************* */
@ -528,40 +515,6 @@ void JacobianFactor::multiplyHessianAdd(double alpha, const VectorValues& x,
transposeMultiplyAdd(alpha, Ax, y);
}
void JacobianFactor::multiplyHessianAdd(double alpha, const double* x,
double* y, std::vector<size_t> keys) const {
// Use eigen magic to access raw memory
typedef Eigen::Matrix<double, Eigen::Dynamic, 1> DVector;
typedef Eigen::Map<DVector> DMap;
typedef Eigen::Map<const DVector> ConstDMap;
if (empty())
return;
Vector Ax = zero(Ab_.rows());
// Just iterate over all A matrices and multiply in correct config part
for (size_t pos = 0; pos < size(); ++pos)
Ax += Ab_(pos)
* ConstDMap(x + keys[keys_[pos]],
keys[keys_[pos] + 1] - keys[keys_[pos]]);
// Deal with noise properly, need to Double* whiten as we are dividing by variance
if (model_) {
model_->whitenInPlace(Ax);
model_->whitenInPlace(Ax);
}
// multiply with alpha
Ax *= alpha;
// Again iterate over all A matrices and insert Ai^e into y
for (size_t pos = 0; pos < size(); ++pos)
DMap(y + keys[keys_[pos]], keys[keys_[pos] + 1] - keys[keys_[pos]]) += Ab_(
pos).transpose() * Ax;
}
/* ************************************************************************* */
VectorValues JacobianFactor::gradientAtZero() const {
VectorValues g;
@ -574,8 +527,9 @@ VectorValues JacobianFactor::gradientAtZero() const {
}
/* ************************************************************************* */
// Raw memory access version should be called in Regular Factors only currently
void JacobianFactor::gradientAtZero(double* d) const {
throw std::runtime_error("Raw memory version of gradientAtZero not implemented for Jacobian factor");
throw std::runtime_error("JacobianFactor::gradientAtZero raw memory access is allowed for Regular Factors only");
}
/* ************************************************************************* */

4
gtsam/linear/JacobianFactor.h
View File

@ -283,10 +283,6 @@ namespace gtsam {
/** y += alpha * A'*A*x */
void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const;
void multiplyHessianAdd(double alpha, const double* x, double* y, std::vector<size_t> keys) const;
void multiplyHessianAdd(double alpha, const double* x, double* y) const {};
/// A'*b for Jacobian
VectorValues gradientAtZero() const;

2
gtsam/linear/Preconditioner.cpp
View File

@ -126,7 +126,9 @@ void BlockJacobiPreconditioner::transposeSolve(const Vector& y, Vector& x) const
void BlockJacobiPreconditioner::build(
const GaussianFactorGraph &gfg, const KeyInfo &keyInfo, const std::map<Key,Vector> &lambda)
{
// n is the number of keys
const size_t n = keyInfo.size();
// dims_ is a vector that contains the dimension of keys
dims_ = keyInfo.colSpec();
/* prepare the buffer of block diagonals */

25
gtsam/linear/tests/testGaussianFactorGraph.cpp
View File

@ -315,30 +315,6 @@ TEST( GaussianFactorGraph, multiplyHessianAdd2 )
EXPECT(assert_equal(2*expected, actual));
}
/* ************************************************************************* */
TEST( GaussianFactorGraph, multiplyHessianAdd3 )
{
GaussianFactorGraph gfg = createGaussianFactorGraphWithHessianFactor();
// brute force
Matrix AtA; Vector eta; boost::tie(AtA,eta) = gfg.hessian();
Vector X(6); X<<1,2,3,4,5,6;
Vector Y(6); Y<<-450, -450, 300, 400, 2950, 3450;
EXPECT(assert_equal(Y,AtA*X));
double* x = &X[0];
Vector fast_y = gtsam::zero(6);
gfg.multiplyHessianAdd(1.0, x, fast_y.data());
EXPECT(assert_equal(Y, fast_y));
// now, do it with non-zero y
gfg.multiplyHessianAdd(1.0, x, fast_y.data());
EXPECT(assert_equal(2*Y, fast_y));
}
/* ************************************************************************* */
TEST( GaussianFactorGraph, matricesMixed )
{
@ -351,7 +327,6 @@ TEST( GaussianFactorGraph, matricesMixed )
EXPECT(assert_equal(A.transpose()*b, eta));
}
/* ************************************************************************* */
TEST( GaussianFactorGraph, gradientAtZero )
{

4
gtsam/nonlinear/Values-inl.h
View File

@ -289,13 +289,13 @@ namespace gtsam {
}
/* ************************************************************************* */
// insert a plain value using the default chart
// insert a templated value
template<typename ValueType>
void Values::insert(Key j, const ValueType& val) {
insert(j, static_cast<const Value&>(GenericValue<ValueType>(val)));
}
// update with default chart
// update with templated value
template <typename ValueType>
void Values::update(Key j, const ValueType& val) {
update(j, static_cast<const Value&>(GenericValue<ValueType >(val)));

18
gtsam/nonlinear/Values.h
View File

@ -260,7 +260,6 @@ namespace gtsam {
/** Templated version to add a variable with the given j,
* throws KeyAlreadyExists<J> if j is already present
* if no chart is specified, the DefaultChart<ValueType> is used
*/
template <typename ValueType>
void insert(Key j, const ValueType& val);
@ -272,15 +271,6 @@ namespace gtsam {
/// version for double
void insertDouble(Key j, double c) { insert<double>(j,c); }
/// overloaded insert version that also specifies a chart
template <typename ValueType, typename Chart>
void insert(Key j, const ValueType& val);
/// overloaded insert version that also specifies a chart initializer
template <typename ValueType, typename Chart>
void insert(Key j, const ValueType& val, Chart chart);
/** insert that mimics the STL map insert - if the value already exists, the map is not modified
* and an iterator to the existing value is returned, along with 'false'. If the value did not
* exist, it is inserted and an iterator pointing to the new element, along with 'true', is
@ -297,14 +287,6 @@ namespace gtsam {
template <typename T>
void update(Key j, const T& val);
/// overloaded insert version that also specifies a chart
template <typename T, typename Chart>
void update(Key j, const T& val);
/// overloaded insert version that also specifies a chart initializer
template <typename T, typename Chart>
void update(Key j, const T& val, Chart chart);
/** update the current available values without adding new ones */
void update(const Values& values);

87
gtsam/slam/RegularHessianFactor.h
View File

@ -51,18 +51,12 @@ public:
HessianFactor(keys, augmentedInformation) {
}
/** y += alpha * A'*A*x */
void multiplyHessianAdd(double alpha, const VectorValues& x,
VectorValues& y) const {
HessianFactor::multiplyHessianAdd(alpha, x, y);
}
// Scratch space for multiplyHessianAdd
typedef Eigen::Matrix<double, D, 1> DVector;
mutable std::vector<DVector> y;
void multiplyHessianAdd(double alpha, const double* x,
double* yvalues) const {
/** y += alpha * A'*A*x */
void multiplyHessianAdd(double alpha, const double* x, double* yvalues) const {
// Create a vector of temporary y values, corresponding to rows i
y.resize(size());
BOOST_FOREACH(DVector & yi, y)
@ -95,6 +89,83 @@ public:
}
}
void multiplyHessianAdd(double alpha, const double* x, double* yvalues,
std::vector<size_t> offsets) const {
// Use eigen magic to access raw memory
typedef Eigen::Matrix<double, Eigen::Dynamic, 1> DVector;
typedef Eigen::Map<DVector> DMap;
typedef Eigen::Map<const DVector> ConstDMap;
// Create a vector of temporary y values, corresponding to rows i
std::vector<Vector> y;
y.reserve(size());
for (const_iterator it = begin(); it != end(); it++)
y.push_back(zero(getDim(it)));
// Accessing the VectorValues one by one is expensive
// So we will loop over columns to access x only once per column
// And fill the above temporary y values, to be added into yvalues after
for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) {
DenseIndex i = 0;
for (; i < j; ++i)
y[i] += info_(i, j).knownOffDiagonal()
* ConstDMap(x + offsets[keys_[j]],
offsets[keys_[j] + 1] - offsets[keys_[j]]);
// blocks on the diagonal are only half
y[i] += info_(j, j).selfadjointView()
* ConstDMap(x + offsets[keys_[j]],
offsets[keys_[j] + 1] - offsets[keys_[j]]);
// for below diagonal, we take transpose block from upper triangular part
for (i = j + 1; i < (DenseIndex) size(); ++i)
y[i] += info_(i, j).knownOffDiagonal()
* ConstDMap(x + offsets[keys_[j]],
offsets[keys_[j] + 1] - offsets[keys_[j]]);
}
// copy to yvalues
for (DenseIndex i = 0; i < (DenseIndex) size(); ++i)
DMap(yvalues + offsets[keys_[i]], offsets[keys_[i] + 1] - offsets[keys_[i]]) +=
alpha * y[i];
}
/** Return the diagonal of the Hessian for this factor (raw memory version) */
virtual void hessianDiagonal(double* d) const {
// Use eigen magic to access raw memory
//typedef Eigen::Matrix<double, 9, 1> DVector;
typedef Eigen::Matrix<double, D, 1> DVector;
typedef Eigen::Map<DVector> DMap;
// Loop over all variables in the factor
for (DenseIndex pos = 0; pos < (DenseIndex)size(); ++pos) {
Key j = keys_[pos];
// Get the diagonal block, and insert its diagonal
const Matrix& B = info_(pos, pos).selfadjointView();
//DMap(d + 9 * j) += B.diagonal();
DMap(d + D * j) += B.diagonal();
}
}
/* ************************************************************************* */
// TODO: currently assumes all variables of the same size 9 and keys arranged from 0 to n
virtual void gradientAtZero(double* d) const {
// Use eigen magic to access raw memory
//typedef Eigen::Matrix<double, 9, 1> DVector;
typedef Eigen::Matrix<double, D, 1> DVector;
typedef Eigen::Map<DVector> DMap;
// Loop over all variables in the factor
for (DenseIndex pos = 0; pos < (DenseIndex)size(); ++pos) {
Key j = keys_[pos];
// Get the diagonal block, and insert its diagonal
VectorD dj = -info_(pos,size()).knownOffDiagonal();
//DMap(d + 9 * j) += dj;
DMap(d + D * j) += dj;
}
}
};
}

4
gtsam/slam/RegularImplicitSchurFactor.h
View File

@ -159,7 +159,7 @@ public:
* @brief add the contribution of this factor to the diagonal of the hessian
* d(output) = d(input) + deltaHessianFactor
*/
void hessianDiagonal(double* d) const {
virtual void hessianDiagonal(double* d) const {
// diag(Hessian) = diag(F' * (I - E * PointCov * E') * F);
// Use eigen magic to access raw memory
typedef Eigen::Matrix<double, D, 1> DVector;
@ -434,7 +434,7 @@ public:
/**
* Calculate gradient, which is -F'Q*b, see paper - RAW MEMORY ACCESS
*/
void gradientAtZero(double* d) const {
virtual void gradientAtZero(double* d) const {
// Use eigen magic to access raw memory
typedef Eigen::Matrix<double, D, 1> DVector;

174
gtsam/slam/RegularJacobianFactor.h Normal file
View File

@ -0,0 +1,174 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file RegularJacobianFactor.h
* @brief JacobianFactor class with fixed sized blcoks
* @author Sungtae An
* @date Nov 11, 2014
*/
#pragma once
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/VectorValues.h>
#include <boost/foreach.hpp>
#include <vector>
namespace gtsam {
template<size_t D>
class RegularJacobianFactor: public JacobianFactor {
private:
/** Use eigen magic to access raw memory */
typedef Eigen::Matrix<double, D, 1> DVector;
typedef Eigen::Map<DVector> DMap;
typedef Eigen::Map<const DVector> ConstDMap;
public:
/** Construct an n-ary factor
* @tparam TERMS A container whose value type is std::pair<Key, Matrix>, specifying the
* collection of keys and matrices making up the factor. */
template<typename TERMS>
RegularJacobianFactor(const TERMS& terms, const Vector& b,
const SharedDiagonal& model = SharedDiagonal()) :
JacobianFactor(terms, b, model) {
}
/** Constructor with arbitrary number keys, and where the augmented matrix is given all together
* instead of in block terms. Note that only the active view of the provided augmented matrix
* is used, and that the matrix data is copied into a newly-allocated matrix in the constructed
* factor. */
template<typename KEYS>
RegularJacobianFactor(const KEYS& keys,
const VerticalBlockMatrix& augmentedMatrix,
const SharedDiagonal& sigmas = SharedDiagonal()) :
JacobianFactor(keys, augmentedMatrix, sigmas) {
}
/** Raw memory access version of multiplyHessianAdd y += alpha * A'*A*x
* Note: this is not assuming a fixed dimension for the variables,
* but requires the vector accumulatedDims to tell the dimension of
* each variable: e.g.: x0 has dim 3, x2 has dim 6, x3 has dim 2,
* then accumulatedDims is [0 3 9 11 13]
* NOTE: size of accumulatedDims is size of keys + 1!! */
void multiplyHessianAdd(double alpha, const double* x, double* y,
const std::vector<size_t>& accumulatedDims) const {
/// Use eigen magic to access raw memory
typedef Eigen::Matrix<double, Eigen::Dynamic, 1> VectorD;
typedef Eigen::Map<VectorD> MapD;
typedef Eigen::Map<const VectorD> ConstMapD;
if (empty())
return;
Vector Ax = zero(Ab_.rows());
/// Just iterate over all A matrices and multiply in correct config part (looping over keys)
/// E.g.: Jacobian A = [A0 A1 A2] multiplies x = [x0 x1 x2]'
/// Hence: Ax = A0 x0 + A1 x1 + A2 x2 (hence we loop over the keys and accumulate)
for (size_t pos = 0; pos < size(); ++pos)
{
Ax += Ab_(pos)
* ConstMapD(x + accumulatedDims[keys_[pos]], accumulatedDims[keys_[pos] + 1] - accumulatedDims[keys_[pos]]);
}
/// Deal with noise properly, need to Double* whiten as we are dividing by variance
if (model_) {
model_->whitenInPlace(Ax);
model_->whitenInPlace(Ax);
}
/// multiply with alpha
Ax *= alpha;
/// Again iterate over all A matrices and insert Ai^T into y
for (size_t pos = 0; pos < size(); ++pos){
MapD(y + accumulatedDims[keys_[pos]], accumulatedDims[keys_[pos] + 1] - accumulatedDims[keys_[pos]]) += Ab_(
pos).transpose() * Ax;
}
}
/** Raw memory access version of multiplyHessianAdd */
void multiplyHessianAdd(double alpha, const double* x, double* y) const {
if (empty()) return;
Vector Ax = zero(Ab_.rows());
// Just iterate over all A matrices and multiply in correct config part
for(size_t pos=0; pos<size(); ++pos)
Ax += Ab_(pos) * ConstDMap(x + D * keys_[pos]);
// Deal with noise properly, need to Double* whiten as we are dividing by variance
if (model_) { model_->whitenInPlace(Ax); model_->whitenInPlace(Ax); }
// multiply with alpha
Ax *= alpha;
// Again iterate over all A matrices and insert Ai^e into y
for(size_t pos=0; pos<size(); ++pos)
DMap(y + D * keys_[pos]) += Ab_(pos).transpose() * Ax;
}
/** Raw memory access version of hessianDiagonal
* TODO: currently assumes all variables of the same size D (templated) and keys arranged from 0 to n
*/
virtual void hessianDiagonal(double* d) const {
// Loop over all variables in the factor
for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) {
// Get the diagonal block, and insert its diagonal
DVector dj;
for (size_t k = 0; k < D; ++k){
if (model_){
Vector column_k = Ab_(j).col(k);
column_k = model_->whiten(column_k);
dj(k) = dot(column_k, column_k);
}else{
dj(k) = Ab_(j).col(k).squaredNorm();
}
}
DMap(d + D * j) += dj;
}
}
/** Raw memory access version of gradientAtZero
* TODO: currently assumes all variables of the same size D (templated) and keys arranged from 0 to n
*/
virtual void gradientAtZero(double* d) const {
// Get vector b not weighted
Vector b = getb();
// Whitening b
if (model_) {
b = model_->whiten(b);
b = model_->whiten(b);
}
// Just iterate over all A matrices
for (DenseIndex j = 0; j < (DenseIndex)size(); ++j) {
DVector dj;
// gradient -= A'*b/sigma^2
// Computing with each column
for (size_t k = 0; k < D; ++k){
Vector column_k = Ab_(j).col(k);
dj(k) = -1.0*dot(b, column_k);
}
DMap(d + D * j) += dj;
}
}
};
}

1
gtsam/slam/dataset.cpp
View File

@ -19,6 +19,7 @@
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/BearingRangeFactor.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/linear/Sampler.h>
#include <gtsam/inference/Symbol.h>

100
gtsam/slam/tests/testRegularHessianFactor.cpp Normal file
View File

@ -0,0 +1,100 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file testRegularHessianFactor.cpp
* @author Frank Dellaert
* @date March 4, 2014
*/
#include <gtsam/linear/VectorValues.h>
#include <CppUnitLite/TestHarness.h>
//#include <gtsam_unstable/slam/RegularHessianFactor.h>
#include <gtsam/slam/RegularHessianFactor.h>
#include <boost/assign/std/vector.hpp>
#include <boost/assign/std/map.hpp>
#include <boost/assign/list_of.hpp>
using namespace std;
using namespace gtsam;
using namespace boost::assign;
const double tol = 1e-5;
/* ************************************************************************* */
TEST(RegularHessianFactor, ConstructorNWay)
{
Matrix G11 = (Matrix(2,2) << 111, 112, 113, 114).finished();
Matrix G12 = (Matrix(2,2) << 121, 122, 123, 124).finished();
Matrix G13 = (Matrix(2,2) << 131, 132, 133, 134).finished();
Matrix G22 = (Matrix(2,2) << 221, 222, 222, 224).finished();
Matrix G23 = (Matrix(2,2) << 231, 232, 233, 234).finished();
Matrix G33 = (Matrix(2,2) << 331, 332, 332, 334).finished();
Vector g1 = (Vector(2) << -7, -9).finished();
Vector g2 = (Vector(2) << -9, 1).finished();
Vector g3 = (Vector(2) << 2, 3).finished();
double f = 10;
std::vector<Key> js;
js.push_back(0); js.push_back(1); js.push_back(3);
std::vector<Matrix> Gs;
Gs.push_back(G11); Gs.push_back(G12); Gs.push_back(G13); Gs.push_back(G22); Gs.push_back(G23); Gs.push_back(G33);
std::vector<Vector> gs;
gs.push_back(g1); gs.push_back(g2); gs.push_back(g3);
RegularHessianFactor<2> factor(js, Gs, gs, f);
// multiplyHessianAdd:
{
// brute force
Matrix AtA = factor.information();
HessianFactor::const_iterator i1 = factor.begin();
HessianFactor::const_iterator i2 = i1 + 1;
Vector X(6); X << 1,2,3,4,5,6;
Vector Y(6); Y << 2633, 2674, 4465, 4501, 5669, 5696;
EXPECT(assert_equal(Y,AtA*X));
VectorValues x = map_list_of<Key, Vector>
(0, (Vector(2) << 1,2).finished())
(1, (Vector(2) << 3,4).finished())
(3, (Vector(2) << 5,6).finished());
VectorValues expected;
expected.insert(0, Y.segment<2>(0));
expected.insert(1, Y.segment<2>(2));
expected.insert(3, Y.segment<2>(4));
// RAW ACCESS
Vector expected_y(8); expected_y << 2633, 2674, 4465, 4501, 0, 0, 5669, 5696;
Vector fast_y = gtsam::zero(8);
double xvalues[8] = {1,2,3,4,0,0,5,6};
factor.multiplyHessianAdd(1, xvalues, fast_y.data());
EXPECT(assert_equal(expected_y, fast_y));
// now, do it with non-zero y
factor.multiplyHessianAdd(1, xvalues, fast_y.data());
EXPECT(assert_equal(2*expected_y, fast_y));
// check some expressions
EXPECT(assert_equal(G12,factor.info(i1,i2).knownOffDiagonal()));
EXPECT(assert_equal(G22,factor.info(i2,i2).selfadjointView()));
EXPECT(assert_equal((Matrix)G12.transpose(),factor.info(i2,i1).knownOffDiagonal()));
}
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */

224
gtsam/slam/tests/testRegularJacobianFactor.cpp Normal file
View File

@ -0,0 +1,224 @@
/**
* @file testRegularJacobianFactor.cpp
* @brief unit test regular jacobian factors
* @author Sungtae An
* @date Nov 12, 2014
*/
#include <gtsam/base/TestableAssertions.h>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/slam/RegularJacobianFactor.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/GaussianConditional.h>
#include <gtsam/linear/VectorValues.h>
#include <boost/assign/std/vector.hpp>
#include <boost/assign/list_of.hpp>
#include <boost/range/iterator_range.hpp>
#include <boost/range/adaptor/map.hpp>
using namespace std;
using namespace gtsam;
using namespace boost::assign;
static const size_t fixedDim = 3;
static const size_t nrKeys = 3;
// Keys are assumed to be from 0 to n
namespace {
namespace simple {
// Terms we'll use
const vector<pair<Key, Matrix> > terms = list_of<pair<Key,Matrix> >
(make_pair(0, Matrix3::Identity()))
(make_pair(1, 2*Matrix3::Identity()))
(make_pair(2, 3*Matrix3::Identity()));
// RHS and sigmas
const Vector b = (Vector(3) << 1., 2., 3.).finished();
const SharedDiagonal noise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.5,0.5,0.5).finished());
}
namespace simple2 {
// Terms
const vector<pair<Key, Matrix> > terms2 = list_of<pair<Key,Matrix> >
(make_pair(0, 2*Matrix3::Identity()))
(make_pair(1, 4*Matrix3::Identity()))
(make_pair(2, 6*Matrix3::Identity()));
// RHS
const Vector b2 = (Vector(3) << 2., 4., 6.).finished();
}
}
/* ************************************************************************* */
// Convert from double* to VectorValues
VectorValues double2vv(const double* x,
const size_t nrKeys, const size_t dim) {
// create map with dimensions
std::map<gtsam::Key, size_t> dims;
for (size_t i = 0; i < nrKeys; i++)
dims.insert(make_pair(i, dim));
size_t n = nrKeys*dim;
Vector xVec(n);
for (size_t i = 0; i < n; i++){
xVec(i) = x[i];
}
return VectorValues(xVec, dims);
}
/* ************************************************************************* */
void vv2double(const VectorValues& vv, double* y,
const size_t nrKeys, const size_t dim) {
// create map with dimensions
std::map<gtsam::Key, size_t> dims;
for (size_t i = 0; i < nrKeys; i++)
dims.insert(make_pair(i, dim));
Vector yvector = vv.vector(dims);
size_t n = nrKeys*dim;
for (size_t j = 0; j < n; j++)
y[j] = yvector[j];
}
/* ************************************************************************* */
TEST(RegularJacobianFactor, constructorNway)
{
using namespace simple;
JacobianFactor factor(terms[0].first, terms[0].second,
terms[1].first, terms[1].second, terms[2].first, terms[2].second, b, noise);
RegularJacobianFactor<fixedDim> regularFactor(terms, b, noise);
LONGS_EQUAL((long)terms[2].first, (long)regularFactor.keys().back());
EXPECT(assert_equal(terms[2].second, regularFactor.getA(regularFactor.end() - 1)));
EXPECT(assert_equal(b, factor.getb()));
EXPECT(assert_equal(b, regularFactor.getb()));
EXPECT(noise == factor.get_model());
EXPECT(noise == regularFactor.get_model());
}
/* ************************************************************************* */
TEST(RegularJacobianFactor, hessianDiagonal)
{
using namespace simple;
JacobianFactor factor(terms[0].first, terms[0].second,
terms[1].first, terms[1].second, terms[2].first, terms[2].second, b, noise);
RegularJacobianFactor<fixedDim> regularFactor(terms, b, noise);
// we compute hessian diagonal from the standard Jacobian
VectorValues expectedHessianDiagonal = factor.hessianDiagonal();
// we compare against the Raw memory access implementation of hessianDiagonal
double actualValue[9]={0};
regularFactor.hessianDiagonal(actualValue);
VectorValues actualHessianDiagonalRaw = double2vv(actualValue,nrKeys,fixedDim);
EXPECT(assert_equal(expectedHessianDiagonal, actualHessianDiagonalRaw));
}
/* ************************************************************************* */
TEST(RegularJacobian, gradientAtZero)
{
using namespace simple;
JacobianFactor factor(terms[0].first, terms[0].second,
terms[1].first, terms[1].second, terms[2].first, terms[2].second, b, noise);
RegularJacobianFactor<fixedDim> regularFactor(terms, b, noise);
// we compute gradient at zero from the standard Jacobian
VectorValues expectedGradientAtZero = factor.gradientAtZero();
//EXPECT(assert_equal(expectedGradientAtZero, regularFactor.gradientAtZero()));
// we compare against the Raw memory access implementation of gradientAtZero
double actualValue[9]={0};
regularFactor.gradientAtZero(actualValue);
VectorValues actualGradientAtZeroRaw = double2vv(actualValue,nrKeys,fixedDim);
EXPECT(assert_equal(expectedGradientAtZero, actualGradientAtZeroRaw));
}
/* ************************************************************************* */
TEST(RegularJacobian, gradientAtZero_multiFactors)
{
using namespace simple;
JacobianFactor factor(terms[0].first, terms[0].second,
terms[1].first, terms[1].second, terms[2].first, terms[2].second, b, noise);
RegularJacobianFactor<fixedDim> regularFactor(terms, b, noise);
// we compute gradient at zero from the standard Jacobian
VectorValues expectedGradientAtZero = factor.gradientAtZero();
// we compare against the Raw memory access implementation of gradientAtZero
double actualValue[9]={0};
regularFactor.gradientAtZero(actualValue);
VectorValues actualGradientAtZeroRaw = double2vv(actualValue,nrKeys,fixedDim);
EXPECT(assert_equal(expectedGradientAtZero, actualGradientAtZeroRaw));
// One more factor
using namespace simple2;
JacobianFactor factor2(terms2[0].first, terms2[0].second,
terms2[1].first, terms2[1].second, terms2[2].first, terms2[2].second, b2, noise);
RegularJacobianFactor<fixedDim> regularFactor2(terms2, b2, noise);
// we accumulate computed gradient at zero from the standard Jacobian
VectorValues expectedGradientAtZero2 = expectedGradientAtZero.add(factor2.gradientAtZero());
// we compare against the Raw memory access implementation of gradientAtZero
regularFactor2.gradientAtZero(actualValue);
VectorValues actualGradientAtZeroRaw2 = double2vv(actualValue,nrKeys,fixedDim);
EXPECT(assert_equal(expectedGradientAtZero2, actualGradientAtZeroRaw2));
}
/* ************************************************************************* */
TEST(RegularJacobian, multiplyHessianAdd)
{
using namespace simple;
JacobianFactor factor(terms[0].first, terms[0].second,
terms[1].first, terms[1].second, terms[2].first, terms[2].second, b, noise);
RegularJacobianFactor<fixedDim> regularFactor(terms, b, noise);
// arbitrary vector X
VectorValues X;
X.insert(0, (Vector(3) << 10.,20.,30.).finished());
X.insert(1, (Vector(3) << 10.,20.,30.).finished());
X.insert(2, (Vector(3) << 10.,20.,30.).finished());
// arbitrary vector Y
VectorValues Y;
Y.insert(0, (Vector(3) << 10.,10.,10.).finished());
Y.insert(1, (Vector(3) << 20.,20.,20.).finished());
Y.insert(2, (Vector(3) << 30.,30.,30.).finished());
// multiplyHessianAdd Y += alpha*A'A*X
double alpha = 2.0;
VectorValues expectedMHA = Y;
factor.multiplyHessianAdd(alpha, X, expectedMHA);
// create data for raw memory access
double XRaw[9];
vv2double(X, XRaw, nrKeys, fixedDim);
// test 1st version: multiplyHessianAdd(double alpha, const double* x, double* y)
double actualMHARaw[9];
vv2double(Y, actualMHARaw, nrKeys, fixedDim);
regularFactor.multiplyHessianAdd(alpha, XRaw, actualMHARaw);
VectorValues actualMHARawVV = double2vv(actualMHARaw,nrKeys,fixedDim);
EXPECT(assert_equal(expectedMHA,actualMHARawVV));
// test 2nd version: multiplyHessianAdd(double alpha, const double* x, double* y, std::vector<size_t> keys)
double actualMHARaw2[9];
vv2double(Y, actualMHARaw2, nrKeys, fixedDim);
vector<size_t> dims;
size_t accumulatedDim = 0;
for (size_t i = 0; i < nrKeys+1; i++){
dims.push_back(accumulatedDim);
accumulatedDim += fixedDim;
}
regularFactor.multiplyHessianAdd(alpha, XRaw, actualMHARaw2, dims);
VectorValues actualMHARawVV2 = double2vv(actualMHARaw2,nrKeys,fixedDim);
EXPECT(assert_equal(expectedMHA,actualMHARawVV2));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */

2
gtsam_unstable/slam/PartialPriorFactor.h
View File

@ -78,7 +78,7 @@ namespace gtsam {
/** Indices Constructor: specify the mask with a set of indices */
PartialPriorFactor(Key key, const std::vector<size_t>& mask, const Vector& prior,
const SharedNoiseModel& model) :
Base(model, key), prior_(prior), mask_(mask), H_(zeros(mask.size(), T::Dim())) {
Base(model, key), prior_(prior), mask_(mask), H_(zeros(mask.size(), T::dimension)) {
assert((size_t)prior_.size() == mask.size());
assert(model->dim() == (size_t) prior.size());
this->fillH();

4
matlab/+gtsam/covarianceEllipse.m
View File

@ -8,6 +8,8 @@ function h = covarianceEllipse(x,P,color, k)
% it is assumed x and y are the first two components of state x
% k is scaling for std deviations, defaults to 1 std
hold on
[e,s] = eig(P(1:2,1:2));
s1 = s(1,1);
s2 = s(2,2);
@ -32,4 +34,4 @@ h = line(ex,ey,'color',color);
y = c(2) + points(2,:);
end
end
end

8
matlab/+gtsam/covarianceEllipse3D.m
View File

@ -1,4 +1,4 @@
function covarianceEllipse3D(c,P)
function sc = covarianceEllipse3D(c,P,scale)
% covarianceEllipse3D plots a Gaussian as an uncertainty ellipse
% Based on Maybeck Vol 1, page 366
% k=2.296 corresponds to 1 std, 68.26% of all probability
@ -6,10 +6,16 @@ function covarianceEllipse3D(c,P)
%
% Modified from http://www.mathworks.com/matlabcentral/newsreader/view_thread/42966
hold on
[e,s] = svd(P);
k = 11.82;
radii = k*sqrt(diag(s));
if exist('scale', 'var')
radii = radii * scale;
end
% generate data for "unrotated" ellipsoid
[xc,yc,zc] = ellipsoid(0,0,0,radii(1),radii(2),radii(3),8);

89
matlab/+gtsam/cylinderSampleProjection.m Normal file
View File

@ -0,0 +1,89 @@
function [visiblePoints] = cylinderSampleProjection(K, pose, imageSize, cylinders)
% Input:
% Output:
% visiblePoints: data{k} 3D Point in overal point clouds with index k
% Z{k} 2D measurements in overal point clouds with index k
% index {i}{j}
% i: the cylinder index;
% j: the point index on the cylinder;
%
% @Description: Project sampled points on cylinder to camera frame
% @Authors: Zhaoyang Lv
import gtsam.*
camera = SimpleCamera(pose, K);
%% memory allocation
cylinderNum = length(cylinders);
%% check visiblity of points on each cylinder
pointCloudIndex = 0;
visiblePointIdx = 1;
for i = 1:cylinderNum
pointNum = length(cylinders{i}.Points);
% to check point visibility
for j = 1:pointNum
pointCloudIndex = pointCloudIndex + 1;
% Cheirality Exception
sampledPoint3 = cylinders{i}.Points{j};
sampledPoint3local = pose.transform_to(sampledPoint3);
if sampledPoint3local.z <= 0
continue;
end
Z2d = camera.project(sampledPoint3);
% ignore points not visible in the scene
if Z2d.x < 0 || Z2d.x >= imageSize.x || Z2d.y < 0 || Z2d.y >= imageSize.y
continue;
end
% ignore points occluded
% use a simple math hack to check occlusion:
% 1. All points in front of cylinders' surfaces are visible
% 2. For points behind the cylinders' surfaces, the cylinder
visible = true;
for k = 1:cylinderNum
rayCameraToPoint = pose.translation().between(sampledPoint3).vector();
rayCameraToCylinder = pose.translation().between(cylinders{k}.centroid).vector();
rayCylinderToPoint = cylinders{k}.centroid.between(sampledPoint3).vector();
% Condition 1: all points in front of the cylinders'
% surfaces are visible
if dot(rayCylinderToPoint, rayCameraToCylinder) < 0
continue;
else
projectedRay = dot(rayCameraToCylinder, rayCameraToPoint) / norm(rayCameraToCylinder);
if projectedRay > 0
%rayCylinderToProjected = rayCameraToCylinder - norm(projectedRay) / norm(rayCameraToPoint) * rayCameraToPoint;
if rayCylinderToPoint(1) > cylinders{k}.radius && ...
rayCylinderToPoint(2) > cylinders{k}.radius
continue;
else
visible = false;
break;
end
end
end
end
if visible
visiblePoints.data{visiblePointIdx} = sampledPoint3;
visiblePoints.Z{visiblePointIdx} = Z2d;
visiblePoints.cylinderIdx{visiblePointIdx} = i;
visiblePoints.overallIdx{visiblePointIdx} = pointCloudIndex;
visiblePointIdx = visiblePointIdx + 1;
end
end
end
end

93
matlab/+gtsam/cylinderSampleProjectionStereo.m Normal file
View File

@ -0,0 +1,93 @@
function [visiblePoints] = cylinderSampleProjectionStereo(K, pose, imageSize, cylinders)
import gtsam.*
%% memory allocation
cylinderNum = length(cylinders);
visiblePoints.data = cell(1);
visiblePoints.Z = cell(1);
visiblePoints.cylinderIdx = cell(1);
visiblePoints.overallIdx = cell(1);
%% check visiblity of points on each cylinder
pointCloudIndex = 0;
visiblePointIdx = 1;
for i = 1:cylinderNum
pointNum = length(cylinders{i}.Points);
% to check point visibility
for j = 1:pointNum
pointCloudIndex = pointCloudIndex + 1;
% For Cheirality Exception
sampledPoint3 = cylinders{i}.Points{j};
sampledPoint3local = pose.transform_to(sampledPoint3);
if sampledPoint3local.z < 0
continue;
end
% measurements
Z.du = K.fx() * K.baseline() / sampledPoint3local.z;
Z.uL = K.fx() * sampledPoint3local.x / sampledPoint3local.z + K.px();
Z.uR = Z.uL + Z.du;
Z.v = K.fy() / sampledPoint3local.z + K.py();
% ignore points not visible in the scene
if Z.uL < 0 || Z.uL >= imageSize.x || ...
Z.uR < 0 || Z.uR >= imageSize.x || ...
Z.v < 0 || Z.v >= imageSize.y
continue;
end
% too small disparity may call indeterminant system exception
if Z.du < 0.6
continue;
end
% ignore points occluded
% use a simple math hack to check occlusion:
% 1. All points in front of cylinders' surfaces are visible
% 2. For points behind the cylinders' surfaces, the cylinder
visible = true;
for k = 1:cylinderNum
rayCameraToPoint = pose.translation().between(sampledPoint3).vector();
rayCameraToCylinder = pose.translation().between(cylinders{k}.centroid).vector();
rayCylinderToPoint = cylinders{k}.centroid.between(sampledPoint3).vector();
% Condition 1: all points in front of the cylinders'
% surfaces are visible
if dot(rayCylinderToPoint, rayCameraToCylinder) < 0
continue;
else
projectedRay = dot(rayCameraToCylinder, rayCameraToPoint) / norm(rayCameraToCylinder);
if projectedRay > 0
%rayCylinderToProjected = rayCameraToCylinder - norm(projectedRay) / norm(rayCameraToPoint) * rayCameraToPoint;
if rayCylinderToPoint(1) > cylinders{k}.radius && ...
rayCylinderToPoint(2) > cylinders{k}.radius
continue;
else
visible = false;
break;
end
end
end
end
if visible
visiblePoints.data{visiblePointIdx} = sampledPoint3;
visiblePoints.Z{visiblePointIdx} = Z;
visiblePoints.cylinderIdx{visiblePointIdx} = i;
visiblePoints.overallIdx{visiblePointIdx} = pointCloudIndex;
visiblePointIdx = visiblePointIdx + 1;
end
end
end
end

26
matlab/+gtsam/cylinderSampling.m Normal file
View File

@ -0,0 +1,26 @@
function [cylinder] = cylinderSampling(baseCentroid, radius, height, density)
%
% @author: Zhaoyang Lv
import gtsam.*
% calculate the cylinder area
area = 2 * pi * radius * height;
pointsNum = round(area * density);
points3 = cell(pointsNum, 1);
% sample the points
for i = 1:pointsNum
theta = 2 * pi * rand;
x = radius * cos(theta) + baseCentroid.x;
y = radius * sin(theta) + baseCentroid.y;
z = height * rand;
points3{i,1} = Point3([x,y,z]');
end
cylinder.area = area;
cylinder.radius = radius;
cylinder.height = height;
cylinder.Points = points3;
cylinder.centroid = Point3(baseCentroid.x, baseCentroid.y, height/2);
end

10
matlab/+gtsam/plotCamera.m
View File

@ -1,18 +1,20 @@
function plotCamera(pose, axisLength)
hold on
C = pose.translation().vector();
R = pose.rotation().matrix();
xAxis = C+R(:,1)*axisLength;
L = [C xAxis]';
line(L(:,1),L(:,2),L(:,3),'Color','r');
h_x = line(L(:,1),L(:,2),L(:,3),'Color','r');
yAxis = C+R(:,2)*axisLength;
L = [C yAxis]';
line(L(:,1),L(:,2),L(:,3),'Color','g');
h_y = line(L(:,1),L(:,2),L(:,3),'Color','g');
zAxis = C+R(:,3)*axisLength;
L = [C zAxis]';
line(L(:,1),L(:,2),L(:,3),'Color','b');
h_z = line(L(:,1),L(:,2),L(:,3),'Color','b');
axis equal
end
end

35
matlab/+gtsam/plotCylinderSamples.m Normal file
View File

@ -0,0 +1,35 @@
function plotCylinderSamples(cylinders, options, figID)
% plot the cylinders on the given field
% @author: Zhaoyang Lv
figure(figID);
holdstate = ishold;
hold on
num = size(cylinders, 1);
sampleDensity = 120;
for i = 1:num
[X,Y,Z] = cylinder(cylinders{i}.radius, sampleDensity * cylinders{i}.radius * cylinders{i}.height);
X = X + cylinders{i}.centroid.x;
Y = Y + cylinders{i}.centroid.y;
Z = Z * cylinders{i}.height;
cylinderHandle = surf(X,Y,Z);
set(cylinderHandle, 'FaceAlpha', 0.5);
hold on
end
axis equal
axis([0, options.fieldSize.x, 0, options.fieldSize.y, 0, 20]);
grid on
if ~holdstate
hold off
end
end

177
matlab/+gtsam/plotFlyingResults.m Normal file
View File

@ -0,0 +1,177 @@
function plotFlyingResults(pts3d, poses, posesCov, cylinders, options)
% plot the visible points on the cylinders and trajectories
%
% author: Zhaoyang Lv
import gtsam.*
figID = 1;
figure(figID);
set(gcf, 'Position', [80,1,1800,1000]);
%% plot all the cylinders and sampled points
axis equal
axis([0, options.fieldSize.x, 0, options.fieldSize.y, 0, options.height + 30]);
xlabel('X (m)');
ylabel('Y (m)');
zlabel('Height (m)');
h = cameratoolbar('Show');
if options.camera.IS_MONO
h_title = title('Quadrotor Flight Simulation with Monocular Camera');
else
h_title = title('Quadrotor Flight Simulation with Stereo Camera');
end
text(100,1750,0, sprintf('Flying Speed: %0.1f\n', options.speed))
view([30, 30]);
hlight = camlight('headlight');
lighting gouraud
if(options.writeVideo)
videoObj = VideoWriter('Camera_Flying_Example.avi');
videoObj.Quality = 100;
videoObj.FrameRate = options.camera.fps;
open(videoObj);
end
sampleDensity = 120;
cylinderNum = length(cylinders);
h_cylinder = cell(cylinderNum);
for i = 1:cylinderNum
hold on
[X,Y,Z] = cylinder(cylinders{i}.radius, sampleDensity * cylinders{i}.radius * cylinders{i}.height);
X = X + cylinders{i}.centroid.x;
Y = Y + cylinders{i}.centroid.y;
Z = Z * cylinders{i}.height;
h_cylinder{i} = surf(X,Y,Z);
set(h_cylinder{i}, 'FaceColor', [0 0 1], 'FaceAlpha', 0.2);
h_cylinder{i}.AmbientStrength = 0.8;
end
%% plot trajectories and points
posesSize = length(poses);
pointSize = length(pts3d);
for i = 1:posesSize
if i > 1
hold on
plot3([poses{i}.x; poses{i-1}.x], [poses{i}.y; poses{i-1}.y], [poses{i}.z; poses{i-1}.z], '-b');
end
if exist('h_pose_cov', 'var')
delete(h_pose_cov);
end
%plotCamera(poses{i}, 3);
gRp = poses{i}.rotation().matrix(); % rotation from pose to global
C = poses{i}.translation().vector();
axisLength = 3;
xAxis = C+gRp(:,1)*axisLength;
L = [C xAxis]';
line(L(:,1),L(:,2),L(:,3),'Color','r');
yAxis = C+gRp(:,2)*axisLength;
L = [C yAxis]';
line(L(:,1),L(:,2),L(:,3),'Color','g');
zAxis = C+gRp(:,3)*axisLength;
L = [C zAxis]';
line(L(:,1),L(:,2),L(:,3),'Color','b');
pPp = posesCov{i}(4:6,4:6); % covariance matrix in pose coordinate frame
gPp = gRp*pPp*gRp'; % convert the covariance matrix to global coordinate frame
h_pose_cov = gtsam.covarianceEllipse3D(C, gPp, options.plot.covarianceScale);
if exist('h_point', 'var')
for j = 1:pointSize
delete(h_point{j});
end
end
if exist('h_point_cov', 'var')
for j = 1:pointSize
delete(h_point_cov{j});
end
end
h_point = cell(pointSize, 1);
h_point_cov = cell(pointSize, 1);
for j = 1:pointSize
if ~isempty(pts3d{j}.cov{i})
hold on
h_point{j} = plot3(pts3d{j}.data.x, pts3d{j}.data.y, pts3d{j}.data.z);
h_point_cov{j} = gtsam.covarianceEllipse3D([pts3d{j}.data.x; pts3d{j}.data.y; pts3d{j}.data.z], ...
pts3d{j}.cov{i}, options.plot.covarianceScale);
end
end
axis equal
axis([0, options.fieldSize.x, 0, options.fieldSize.y, 0, options.height + 30]);
drawnow
if options.writeVideo
currFrame = getframe(gcf);
writeVideo(videoObj, currFrame);
end
end
if exist('h_pose_cov', 'var')
delete(h_pose_cov);
end
% wait for two seconds
pause(2);
%% change views angle
for i = 30 : i : 90
view([30, i]);
if options.writeVideo
currFrame = getframe(gcf);
writeVideo(videoObj, currFrame);
end
drawnow
end
% changing perspective
%% camera flying through video
camzoom(0.8);
for i = 1 : posesSize
hold on
campos([poses{i}.x, poses{i}.y, poses{i}.z]);
camtarget([options.fieldSize.x/2, options.fieldSize.y/2, 0]);
camlight(hlight, 'headlight');
if options.writeVideo
currFrame = getframe(gcf);
writeVideo(videoObj, currFrame);
end
drawnow
end
%%close video
if(options.writeVideo)
close(videoObj);
end
end

113
matlab/+gtsam/points2DTrackMonocular.m Normal file
View File

@ -0,0 +1,113 @@
function pts2dTracksMono = points2DTrackMonocular(K, cameraPoses, imageSize, cylinders)
% Assess how accurately we can reconstruct points from a particular monocular camera setup.
% After creation of the factor graph for each track, linearize it around ground truth.
% There is no optimization
% @author: Zhaoyang Lv
import gtsam.*
%% create graph
graph = NonlinearFactorGraph;
%% create the noise factors
poseNoiseSigmas = [0.001 0.001 0.001 0.1 0.1 0.1]';
posePriorNoise = noiseModel.Diagonal.Sigmas(poseNoiseSigmas);
measurementNoiseSigma = 1.0;
measurementNoise = noiseModel.Isotropic.Sigma(2, measurementNoiseSigma);
cameraPosesNum = length(cameraPoses);
%% add measurements and initial camera & points values
pointsNum = 0;
cylinderNum = length(cylinders);
points3d = cell(0);
for i = 1:cylinderNum
cylinderPointsNum = length(cylinders{i}.Points);
pointsNum = pointsNum + cylinderPointsNum;
for j = 1:cylinderPointsNum
points3d{end+1}.data = cylinders{i}.Points{j};
points3d{end}.Z = cell(0);
points3d{end}.camConstraintIdx = cell(0);
points3d{end}.added = cell(0);
points3d{end}.visiblity = false;
points3d{end}.cov = cell(cameraPosesNum);
end
end
graph.add(PriorFactorPose3(symbol('x', 1), cameraPoses{1}, posePriorNoise));
%% initialize graph and values
initialEstimate = Values;
for i = 1:pointsNum
point_j = points3d{i}.data.retract(0.1*randn(3,1));
initialEstimate.insert(symbol('p', i), point_j);
end
pts3d = cell(cameraPosesNum, 1);
cameraPosesCov = cell(cameraPosesNum, 1);
marginals = Values;
for i = 1:cameraPosesNum
cameraPose = cameraPoses{i};
pts3d{i} = cylinderSampleProjection(K, cameraPose, imageSize, cylinders);
measurementNum = length(pts3d{i}.Z);
for j = 1:measurementNum
index = pts3d{i}.overallIdx{j};
points3d{index}.Z{end+1} = pts3d{i}.Z{j};
points3d{index}.camConstraintIdx{end+1} = i;
points3d{index}.added{end+1} = false;
if length(points3d{index}.Z) < 2
continue;
else
for k = 1:length(points3d{index}.Z)
if ~points3d{index}.added{k}
graph.add(GenericProjectionFactorCal3_S2(points3d{index}.Z{k}, ...
measurementNoise, symbol('x', points3d{index}.camConstraintIdx{k}), ...
symbol('p', index), K) );
points3d{index}.added{k} = true;
end
end
end
points3d{index}.visiblity = true;
end
pose_i = cameraPoses{i}.retract(0.1*randn(6,1));
initialEstimate.insert(symbol('x', i), pose_i);
marginals = Marginals(graph, initialEstimate);
for j = 1:pointsNum
if points3d{j}.visiblity
points3d{j}.cov{i} = marginals.marginalCovariance(symbol('p',j));
end
end
cameraPosesCov{i} = marginals.marginalCovariance(symbol('x',i));
end
%% Print the graph
graph.print(sprintf('\nFactor graph:\n'));
%% Plot the result
plotFlyingResults(points3d, cameraPoses, cameraPosesCov, cylinders, options);
%% get all the points track information
for i = 1:pointsNum
if ~points3d{i}.visiblity
continue;
end
pts2dTracksMono.pt3d{end+1} = points3d{i}.data;
pts2dTracksMono.Z{end+1} = points3d{i}.Z;
if length(points3d{i}.Z) == 1
%pts2dTracksMono.cov{i} singular matrix
else
pts2dTracksMono.cov{end+1} = marginals.marginalCovariance(symbol('p', i));
end
end
end

101
matlab/+gtsam/points2DTrackStereo.m Normal file
View File

@ -0,0 +1,101 @@
function [pts2dTracksStereo, initialEstimate] = points2DTrackStereo(K, cameraPoses, options, cylinders)
% Assess how accurately we can reconstruct points from a particular monocular camera setup.
% After creation of the factor graph for each track, linearize it around ground truth.
% There is no optimization
%
% @author: Zhaoyang Lv
import gtsam.*
%% create graph
graph = NonlinearFactorGraph;
%% create the noise factors
poseNoiseSigmas = [0.001 0.001 0.001 0.1 0.1 0.1]';
posePriorNoise = noiseModel.Diagonal.Sigmas(poseNoiseSigmas);
stereoNoise = noiseModel.Isotropic.Sigma(3, 0.05);
cameraPosesNum = length(cameraPoses);
%% add measurements and initial camera & points values
pointsNum = 0;
cylinderNum = length(cylinders);
points3d = cell(0);
for i = 1:cylinderNum
cylinderPointsNum = length(cylinders{i}.Points);
pointsNum = pointsNum + length(cylinders{i}.Points);
for j = 1:cylinderPointsNum
points3d{end+1}.data = cylinders{i}.Points{j};
points3d{end}.Z = cell(0);
points3d{end}.cameraConstraint = cell(0);
points3d{end}.visiblity = false;
points3d{end}.cov = cell(cameraPosesNum);
end
end
graph.add(PriorFactorPose3(symbol('x', 1), cameraPoses{1}, posePriorNoise));
%% initialize graph and values
initialEstimate = Values;
for i = 1:pointsNum
point_j = points3d{i}.data.retract(0.05*randn(3,1));
initialEstimate.insert(symbol('p', i), point_j);
end
pts3d = cell(cameraPosesNum, 1);
cameraPosesCov = cell(cameraPosesNum, 1);
for i = 1:cameraPosesNum
pts3d{i} = cylinderSampleProjectionStereo(K, cameraPoses{i}, options.camera.resolution, cylinders);
if isempty(pts3d{i}.Z)
continue;
end
measurementNum = length(pts3d{i}.Z);
for j = 1:measurementNum
index = pts3d{i}.overallIdx{j};
points3d{index}.Z{end+1} = pts3d{i}.Z{j};
points3d{index}.cameraConstraint{end+1} = i;
points3d{index}.visiblity = true;
graph.add(GenericStereoFactor3D(StereoPoint2(pts3d{i}.Z{j}.uL, pts3d{i}.Z{j}.uR, pts3d{i}.Z{j}.v), ...
stereoNoise, symbol('x', i), symbol('p', index), K));
end
pose_i = cameraPoses{i}.retract(poseNoiseSigmas);
initialEstimate.insert(symbol('x', i), pose_i);
%% linearize the graph
marginals = Marginals(graph, initialEstimate);
for j = 1:pointsNum
if points3d{j}.visiblity
points3d{j}.cov{i} = marginals.marginalCovariance(symbol('p', j));
end
end
cameraPosesCov{i} = marginals.marginalCovariance(symbol('x', i));
end
%% Plot the result
plotFlyingResults(points3d, cameraPoses, cameraPosesCov, cylinders, options);
%% get all the 2d points track information
pts2dTracksStereo.pt3d = cell(0);
pts2dTracksStereo.Z = cell(0);
pts2dTracksStereo.cov = cell(0);
for i = 1:pointsNum
if ~points3d{i}.visiblity
continue;
end
pts2dTracksStereo.pt3d{end+1} = points3d{i}.data;
pts2dTracksStereo.Z{end+1} = points3d{i}.Z;
pts2dTracksStereo.cov{end+1} = marginals.marginalCovariance(symbol('p', i));
end
%
% %% plot the result with covariance ellipses
% plotFlyingResults(pts2dTracksStereo.pt3d, pts2dTracksStereo.cov, cameraPoses, cameraPosesCov, cylinders, options);
end

179
matlab/gtsam_examples/CameraFlyingExample.m Normal file
View File

@ -0,0 +1,179 @@
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% GTSAM Copyright 2010, Georgia Tech Research Corporation,
% Atlanta, Georgia 30332-0415
% All Rights Reserved
% Authors: Frank Dellaert, et al. (see THANKS for the full author list)
%
% See LICENSE for the license information
%
% @brief A camera flying example through a field of cylinder landmarks
% @author Zhaoyang Lv
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear all;
clc;
clf;
import gtsam.*
% test or run
options.enableTests = false;
%% cylinder options
% the number of cylinders in the field
options.cylinder.cylinderNum = 15; % pls be smaller than 20
% cylinder size
options.cylinder.radius = 3; % pls be smaller than 5
options.cylinder.height = 10;
% point density on cylinder
options.cylinder.pointDensity = 0.1;
%% camera options
% parameters set according to the stereo camera:
% http://www.matrix-vision.com/USB2.0-single-board-camera-mvbluefox-mlc.html
% set up monocular camera or stereo camera
options.camera.IS_MONO = false;
% the field of view of camera
options.camera.fov = 120;
% fps for image
options.camera.fps = 25;
% camera pixel resolution
options.camera.resolution = Point2(752, 480);
% camera horizon
options.camera.horizon = 60;
% camera baseline
options.camera.baseline = 0.05;
% camera focal length
options.camera.f = round(options.camera.resolution.x * options.camera.horizon / ...
options.camera.fov);
% camera focal baseline
options.camera.fB = options.camera.f * options.camera.baseline;
% camera disparity
options.camera.disparity = options.camera.fB / options.camera.horizon;
% Monocular Camera Calibration
options.camera.monoK = Cal3_S2(options.camera.f, options.camera.f, 0, ...
options.camera.resolution.x/2, options.camera.resolution.y/2);
% Stereo Camera Calibration
options.camera.stereoK = Cal3_S2Stereo(options.camera.f, options.camera.f, 0, ...
options.camera.resolution.x/2, options.camera.resolution.y/2, options.camera.disparity);
% write video output
options.writeVideo = true;
% the testing field size (unit: meter)
options.fieldSize = Point2([100, 100]');
% camera flying speed (unit: meter / second)
options.speed = 20;
% camera flying height
options.height = 30;
%% ploting options
% display covariance scaling factor, default to be 1.
% The covariance visualization default models 99% of all probablity
options.plot.covarianceScale = 1;
% plot the trajectory covariance
options.plot.DISP_TRAJ_COV = true;
% plot points covariance
options.plot.POINTS_COV = true;
%% This is for tests
if options.enableTests
% test1: visibility test in monocular camera
cylinders{1}.centroid = Point3(30, 50, 5);
cylinders{2}.centroid = Point3(50, 50, 5);
cylinders{3}.centroid = Point3(70, 50, 5);
for i = 1:3
cylinders{i}.radius = 5;
cylinders{i}.height = 10;
cylinders{i}.Points{1} = cylinders{i}.centroid.compose(Point3(-cylinders{i}.radius, 0, 0));
cylinders{i}.Points{2} = cylinders{i}.centroid.compose(Point3(cylinders{i}.radius, 0, 0));
end
camera = SimpleCamera.Lookat(Point3(10, 50, 10), ...
Point3(options.fieldSize.x/2, options.fieldSize.y/2, 0), ...
Point3([0,0,1]'), options.monoK);
pose = camera.pose;
prjMonoResult = cylinderSampleProjection(options.camera.monoK, pose, ...
options.camera.resolution, cylinders);
% test2: visibility test in stereo camera
prjStereoResult = cylinderSampleProjectionStereo(options.camera.stereoK, ...
pose, options.camera.resolution, cylinders);
end
%% generate a set of cylinders and point samples on cylinders
cylinderNum = options.cylinder.cylinderNum;
cylinders = cell(cylinderNum, 1);
baseCentroid = cell(cylinderNum, 1);
theta = 0;
i = 1;
while i <= cylinderNum
theta = theta + 2*pi/10;
x = 40 * rand * cos(theta) + options.fieldSize.x/2;
y = 20 * rand * sin(theta) + options.fieldSize.y/2;
baseCentroid{i} = Point2([x, y]');
% prevent two cylinders interact with each other
regenerate = false;
for j = 1:i-1
if i > 1 && baseCentroid{i}.dist(baseCentroid{j}) < options.cylinder.radius * 2
regenerate = true;
break;
end
end
if regenerate
continue;
end
cylinders{i,1} = cylinderSampling(baseCentroid{i}, options.cylinder.radius, ...
options.cylinder.height, options.cylinder.pointDensity);
i = i+1;
end
%% generate ground truth camera trajectories: a line
KMono = Cal3_S2(525,525,0,320,240);
cameraPoses = cell(0);
theta = 0;
t = Point3(5, 5, options.height);
i = 0;
while 1
i = i+1;
distance = options.speed / options.camera.fps;
angle = 0.1*pi*(rand-0.5);
inc_t = Point3(distance * cos(angle), ...
distance * sin(angle), 0);
t = t.compose(inc_t);
if t.x > options.fieldSize.x - 10 || t.y > options.fieldSize.y - 10;
break;
end
%t = Point3([(i-1)*(options.fieldSize.x - 10)/options.poseNum + 10, ...
% 15, 10]');
camera = SimpleCamera.Lookat(t, ...
Point3(options.fieldSize.x/2, options.fieldSize.y/2, 0), ...
Point3([0,0,1]'), options.camera.monoK);
cameraPoses{end+1} = camera.pose;
end
%% set up camera and get measurements
if options.camera.IS_MONO
% use Monocular Camera
pts2dTracksMono = points2DTrackMonocular(options.camera.monoK, cameraPoses, ...
options.camera.resolution, cylinders);
else
% use Stereo Camera
pts2dTracksStereo = points2DTrackStereo(options.camera.stereoK, ...
cameraPoses, options, cylinders);
end

7
matlab/gtsam_tests/testCal3Unified.m Normal file
View File

@ -0,0 +1,7 @@
% test Cal3Unified
import gtsam.*;
K = Cal3Unified;
EXPECT('fx',K.fx()==1);
EXPECT('fy',K.fy()==1);

21
matlab/gtsam_tests/test_gtsam.m
View File

@ -1,17 +1,25 @@
% Test runner script - runs each test
% display 'Starting: testPriorFactor'
% testPriorFactor
%% geometry
display 'Starting: testCal3Unified'
testCal3Unified
display 'Starting: testValues'
testValues
%% linear
display 'Starting: testKalmanFilter'
testKalmanFilter
display 'Starting: testJacobianFactor'
testJacobianFactor
display 'Starting: testKalmanFilter'
testKalmanFilter
%% nonlinear
display 'Starting: testValues'
testValues
%% SLAM
display 'Starting: testPriorFactor'
testPriorFactor
%% examples
display 'Starting: testLocalizationExample'
testLocalizationExample
@ -36,6 +44,7 @@ testStereoVOExample
display 'Starting: testVisualISAMExample'
testVisualISAMExample
%% MATLAB specific
display 'Starting: testUtilities'
testUtilities

1
matlab/unstable_examples/.gitignore vendored
View File

@ -1 +1,2 @@
*.m~
*.avi

2
tests/CMakeLists.txt
View File

@ -2,7 +2,7 @@
# note the source dir on each
set (tests_exclude "")
if ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang") # might not be best test - Richard & Jason & Frank
if (${CMAKE_CXX_COMPILER_ID} STREQUAL "Clang") # might not be best test - Richard & Jason & Frank
# clang linker segfaults on large testSerializationSLAM
list (APPEND tests_exclude "testSerializationSLAM.cpp")
endif()

12
tests/testPreconditioner.cpp
View File

@ -87,19 +87,19 @@ TEST(PCGSolver, simpleLinearSystem) {
simpleGFG += JacobianFactor(0, (Matrix(2,2)<< 1, 0, 0, 1).finished(), (Vector(2) << 0, 0).finished(), unit2);
simpleGFG += JacobianFactor(1, (Matrix(2,2)<< 1, 0, 0, 1).finished(), (Vector(2) << 0, 0).finished(), unit2);
simpleGFG += JacobianFactor(2, (Matrix(2,2)<< 1, 0, 0, 1).finished(), (Vector(2) << 0, 0).finished(), unit2);
simpleGFG.print("system");
//simpleGFG.print("system");
// Expected solution
VectorValues expectedSolution;
expectedSolution.insert(0, (Vector(2) << 0.100498, -0.196756).finished());
expectedSolution.insert(2, (Vector(2) << -0.0990413, -0.0980577).finished());
expectedSolution.insert(1, (Vector(2) << -0.0973252, 0.100582).finished());
expectedSolution.print("Expected");
//expectedSolution.print("Expected");
// Solve the system using direct method
VectorValues deltaDirect = simpleGFG.optimize();
EXPECT(assert_equal(expectedSolution, deltaDirect, 1e-5));
deltaDirect.print("Direct");
//deltaDirect.print("Direct");
// Solve the system using Preconditioned Conjugate Gradient solver
// Common PCG parameters
@ -107,19 +107,19 @@ TEST(PCGSolver, simpleLinearSystem) {
pcg->setMaxIterations(500);
pcg->setEpsilon_abs(0.0);
pcg->setEpsilon_rel(0.0);
pcg->setVerbosity("ERROR");
//pcg->setVerbosity("ERROR");
// With Dummy preconditioner
pcg->preconditioner_ = boost::make_shared<gtsam::DummyPreconditionerParameters>();
VectorValues deltaPCGDummy = PCGSolver(*pcg).optimize(simpleGFG);
EXPECT(assert_equal(expectedSolution, deltaPCGDummy, 1e-5));
deltaPCGDummy.print("PCG Dummy");
//deltaPCGDummy.print("PCG Dummy");
// With Block-Jacobi preconditioner
pcg->preconditioner_ = boost::make_shared<gtsam::BlockJacobiPreconditionerParameters>();
VectorValues deltaPCGJacobi = PCGSolver(*pcg).optimize(simpleGFG);
EXPECT(assert_equal(expectedSolution, deltaPCGJacobi, 1e-5));
deltaPCGJacobi.print("PCG Jacobi");
//deltaPCGJacobi.print("PCG Jacobi");
}