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

Merge branch 'develop' into refactor/ExpressionTests

release/4.3a0
Jose Luis Blanco Claraco 2021-07-19 11:56:58 +02:00
parent 0fe12ec984 fda659674e
commit de68189559
No known key found for this signature in database
GPG Key ID: D443304FBD70A641
279 changed files with 12343 additions and 6673 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
.github/scripts/boost.sh vendored
View File

@ -2,7 +2,7 @@
BOOST_FOLDER=boost_${BOOST_VERSION//./_}
# Download Boost
wget https://dl.bintray.com/boostorg/release/${BOOST_VERSION}/source/${BOOST_FOLDER}.tar.gz
wget https://boostorg.jfrog.io/artifactory/main/release/${BOOST_VERSION}/source/${BOOST_FOLDER}.tar.gz
# Unzip
tar -zxf ${BOOST_FOLDER}.tar.gz

2
.github/workflows/build-linux.yml vendored
View File

@ -57,7 +57,7 @@ jobs:
# 15CF4D18AF4F7421 is the GPG key for the LLVM apt repository
# This key is not in the keystore by default for Ubuntu so we need to add it.
LLVM_KEY=15CF4D18AF4F7421
gpg --keyserver ipv4.pool.sks-keyservers.net --recv-key $LLVM_KEY || gpg --keyserver ha.pool.sks-keyservers.net --recv-key $LLVM_KEY
gpg --keyserver keyserver.ubuntu.com --recv-key $LLVM_KEY || gpg --keyserver hkp://keyserver.ubuntu.com:80 --recv-key $LLVM_KEY
gpg -a --export $LLVM_KEY | sudo apt-key add -
sudo add-apt-repository "deb http://apt.llvm.org/bionic/ llvm-toolchain-bionic-9 main"
fi

30
.github/workflows/build-python.yml vendored
View File

@ -19,22 +19,20 @@ jobs:
# Github Actions requires a single row to be added to the build matrix.
# See https://help.github.com/en/articles/workflow-syntax-for-github-actions.
name: [
# ubuntu-18.04-gcc-5, #TODO Enable once the Python wrapper is optimized for memory
ubuntu-18.04-gcc-5,
ubuntu-18.04-gcc-9,
ubuntu-18.04-clang-9,
macOS-10.15-xcode-11.3.1,
# ubuntu-18.04-gcc-5-tbb,
ubuntu-18.04-gcc-5-tbb,
]
#TODO update wrapper to prevent OOM
# build_type: [Debug, Release]
build_type: [Release]
build_type: [Debug, Release]
python_version: [3]
include:
# - name: ubuntu-18.04-gcc-5
# os: ubuntu-18.04
# compiler: gcc
# version: "5"
- name: ubuntu-18.04-gcc-5
os: ubuntu-18.04
compiler: gcc
version: "5"
- name: ubuntu-18.04-gcc-9
os: ubuntu-18.04
@ -46,7 +44,7 @@ jobs:
compiler: clang
version: "9"
#NOTE temporarily added this as it is a required check.
# NOTE temporarily added this as it is a required check.
- name: ubuntu-18.04-clang-9
os: ubuntu-18.04
compiler: clang
@ -59,11 +57,11 @@ jobs:
compiler: xcode
version: "11.3.1"
# - name: ubuntu-18.04-gcc-5-tbb
# os: ubuntu-18.04
# compiler: gcc
# version: "5"
# flag: tbb
- name: ubuntu-18.04-gcc-5-tbb
os: ubuntu-18.04
compiler: gcc
version: "5"
flag: tbb
steps:
- name: Checkout
@ -77,7 +75,7 @@ jobs:
# 15CF4D18AF4F7421 is the GPG key for the LLVM apt repository
# This key is not in the keystore by default for Ubuntu so we need to add it.
LLVM_KEY=15CF4D18AF4F7421
gpg --keyserver ipv4.pool.sks-keyservers.net --recv-key $LLVM_KEY || gpg --keyserver ha.pool.sks-keyservers.net --recv-key $LLVM_KEY
gpg --keyserver keyserver.ubuntu.com --recv-key $LLVM_KEY || gpg --keyserver hkp://keyserver.ubuntu.com:80 --recv-key $LLVM_KEY
gpg -a --export $LLVM_KEY | sudo apt-key add -
sudo add-apt-repository "deb http://apt.llvm.org/bionic/ llvm-toolchain-bionic-9 main"
fi

2
.github/workflows/build-special.yml vendored
View File

@ -64,7 +64,7 @@ jobs:
run: |
# LLVM 9 is not in Bionic's repositories so we add the official LLVM repository.
if [ "${{ matrix.compiler }}" = "clang" ] && [ "${{ matrix.version }}" = "9" ]; then
gpg --keyserver pool.sks-keyservers.net --recv-key 15CF4D18AF4F7421
gpg --keyserver keyserver.ubuntu.com --recv-key $LLVM_KEY || gpg --keyserver hkp://keyserver.ubuntu.com:80 --recv-key $LLVM_KEY
gpg -a --export 15CF4D18AF4F7421 | sudo apt-key add -
sudo add-apt-repository "deb http://apt.llvm.org/bionic/ llvm-toolchain-bionic-9 main"
fi

7
INSTALL.md
View File

@ -20,9 +20,10 @@ $ make install
Optional dependent libraries:
- If TBB is installed and detectable by CMake GTSAM will use it automatically.
Ensure that CMake prints "Use Intel TBB : Yes". To disable the use of TBB,
disable the CMake flag GTSAM_WITH_TBB (enabled by default). On Ubuntu, TBB
may be installed from the Ubuntu repositories, and for other platforms it
may be downloaded from https://www.threadingbuildingblocks.org/
disable the CMake flag `GTSAM_WITH_TBB` (enabled by default) by providing
the argument `-DGTSAM_WITH_TBB=OFF` to `cmake`. On Ubuntu, TBB may be
installed from the Ubuntu repositories, and for other platforms it may be
downloaded from https://www.threadingbuildingblocks.org/
- GTSAM may be configured to use MKL by toggling `GTSAM_WITH_EIGEN_MKL` and
`GTSAM_WITH_EIGEN_MKL_OPENMP` to `ON`; however, best performance is usually
achieved with MKL disabled. We therefore advise you to benchmark your problem

3
examples/CombinedImuFactorsExample.cpp
View File

@ -23,7 +23,8 @@
* A row starting with "i" is the first initial position formatted with
* N, E, D, qx, qY, qZ, qW, velN, velE, velD
* A row starting with "0" is an imu measurement
* linAccN, linAccE, linAccD, angVelN, angVelE, angVelD
* (body frame - Forward, Right, Down)
* linAccX, linAccY, linAccZ, angVelX, angVelY, angVelX
* A row starting with "1" is a gps correction formatted with
* N, E, D, qX, qY, qZ, qW
* Note that for GPS correction, we're only using the position not the

63
examples/CreateSFMExampleData.cpp
View File

@ -15,8 +15,8 @@
* @author Frank Dellaert
*/
#include <gtsam/slam/dataset.h>
#include <gtsam/geometry/CalibratedCamera.h>
#include <gtsam/slam/dataset.h>
#include <boost/assign/std/vector.hpp>
@ -26,22 +26,16 @@ using namespace gtsam;
/* ************************************************************************* */
void createExampleBALFile(const string& filename, const vector<Point3>& P,
const Pose3& pose1, const Pose3& pose2, const Cal3Bundler& K =
Cal3Bundler()) {
void createExampleBALFile(const string& filename, const vector<Point3>& points,
const Pose3& pose1, const Pose3& pose2,
const Cal3Bundler& K = Cal3Bundler()) {
// Class that will gather all data
SfmData data;
// Create two cameras
Rot3 aRb = Rot3::Yaw(M_PI_2);
Point3 aTb(0.1, 0, 0);
Pose3 identity, aPb(aRb, aTb);
// Create two cameras and add them to data
data.cameras.push_back(SfmCamera(pose1, K));
data.cameras.push_back(SfmCamera(pose2, K));
for(const Point3& p: P) {
for (const Point3& p : points) {
// Create the track
SfmTrack track;
track.p = p;
@ -51,7 +45,7 @@ void createExampleBALFile(const string& filename, const vector<Point3>& P,
// Project points in both cameras
for (size_t i = 0; i < 2; i++)
track.measurements.push_back(make_pair(i, data.cameras[i].project(p)));
track.measurements.push_back(make_pair(i, data.cameras[i].project(p)));
// Add track to data
data.tracks.push_back(track);
@ -63,49 +57,66 @@ void createExampleBALFile(const string& filename, const vector<Point3>& P,
/* ************************************************************************* */
void create5PointExample1() {
// Create two cameras poses
Rot3 aRb = Rot3::Yaw(M_PI_2);
Point3 aTb(0.1, 0, 0);
Pose3 pose1, pose2(aRb, aTb);
// Create test data, we need at least 5 points
vector<Point3> P;
P += Point3(0, 0, 1), Point3(-0.1, 0, 1), Point3(0.1, 0, 1), //
Point3(0, 0.5, 0.5), Point3(0, -0.5, 0.5);
vector<Point3> points = {
{0, 0, 1}, {-0.1, 0, 1}, {0.1, 0, 1}, {0, 0.5, 0.5}, {0, -0.5, 0.5}};
// Assumes example is run in ${GTSAM_TOP}/build/examples
const string filename = "../../examples/data/5pointExample1.txt";
createExampleBALFile(filename, P, pose1, pose2);
const string filename = "../../examples/Data/5pointExample1.txt";
createExampleBALFile(filename, points, pose1, pose2);
}
/* ************************************************************************* */
void create5PointExample2() {
// Create two cameras poses
Rot3 aRb = Rot3::Yaw(M_PI_2);
Point3 aTb(10, 0, 0);
Pose3 pose1, pose2(aRb, aTb);
// Create test data, we need at least 5 points
vector<Point3> P;
P += Point3(0, 0, 100), Point3(-10, 0, 100), Point3(10, 0, 100), //
Point3(0, 50, 50), Point3(0, -50, 50), Point3(-20, 0, 80), Point3(20, -50, 80);
vector<Point3> points = {{0, 0, 100}, {-10, 0, 100}, {10, 0, 100}, //
{0, 50, 50}, {0, -50, 50}, {-20, 0, 80}, //
{20, -50, 80}};
// Assumes example is run in ${GTSAM_TOP}/build/examples
const string filename = "../../examples/data/5pointExample2.txt";
const string filename = "../../examples/Data/5pointExample2.txt";
Cal3Bundler K(500, 0, 0);
createExampleBALFile(filename, P, pose1, pose2,K);
createExampleBALFile(filename, points, pose1, pose2, K);
}
/* ************************************************************************* */
void create18PointExample1() {
// Create two cameras poses
Rot3 aRb = Rot3::Yaw(M_PI_2);
Point3 aTb(0.1, 0, 0);
Pose3 pose1, pose2(aRb, aTb);
// Create test data, we need 15 points
vector<Point3> points = {
{0, 0, 1}, {-0.1, 0, 1}, {0.1, 0, 1}, //
{0, 0.5, 0.5}, {0, -0.5, 0.5}, {-1, -0.5, 2}, //
{-1, 0.5, 2}, {0.25, -0.5, 1.5}, {0.25, 0.5, 1.5}, //
{-0.1, -0.5, 0.5}, {0.1, -0.5, 1}, {0.1, 0.5, 1}, //
{-0.1, 0, 0.5}, {-0.1, 0.5, 0.5}, {0, 0, 0.5}, //
{0.1, -0.5, 0.5}, {0.1, 0, 0.5}, {0.1, 0.5, 0.5}};
// Assumes example is run in ${GTSAM_TOP}/build/examples
const string filename = "../../examples/Data/18pointExample1.txt";
createExampleBALFile(filename, points, pose1, pose2);
}
int main(int argc, char* argv[]) {
create5PointExample1();
create5PointExample2();
create18PointExample1();
return 0;
}
/* ************************************************************************* */

131
examples/Data/18pointExample1.txt Normal file
View File

@ -0,0 +1,131 @@
2 18 36
0 0 0 -0
1 0 -6.123233995736766344e-18 -0.10000000000000000555
0 1 -0.10000000000000000555 -0
1 1 -1.2246467991473532688e-17 -0.2000000000000000111
0 2 0.10000000000000000555 -0
1 2 0 -0
0 3 0 -1
1 3 1 -0.20000000000000006661
0 4 0 1
1 4 -1 -0.19999999999999995559
0 5 -0.5 0.25
1 5 -0.25000000000000005551 -0.55000000000000004441
0 6 -0.5 -0.25
1 6 0.24999999999999997224 -0.55000000000000004441
0 7 0.16666666666666665741 0.33333333333333331483
1 7 -0.33333333333333331483 0.10000000000000000555
0 8 0.16666666666666665741 -0.33333333333333331483
1 8 0.33333333333333331483 0.099999999999999977796
0 9 -0.2000000000000000111 1
1 9 -1 -0.39999999999999996669
0 10 0.10000000000000000555 0.5
1 10 -0.5 3.0616169978683830179e-17
0 11 0.10000000000000000555 -0.5
1 11 0.5 -3.0616169978683830179e-17
0 12 -0.2000000000000000111 -0
1 12 -2.4492935982947065376e-17 -0.4000000000000000222
0 13 -0.2000000000000000111 -1
1 13 1 -0.40000000000000007772
0 14 0 -0
1 14 -1.2246467991473532688e-17 -0.2000000000000000111
0 15 0.2000000000000000111 1
1 15 -1 6.1232339957367660359e-17
0 16 0.2000000000000000111 -0
1 16 0 -0
0 17 0.2000000000000000111 -1
1 17 1 -6.1232339957367660359e-17
3.141592653589793116
0
0
-0
0
0
1
0
0
2.2214414690791830509
2.2214414690791826068
0
-6.123233995736766344e-18
-0.10000000000000000555
0
1
0
0
0
0
1
-0.10000000000000000555
0
1
0.10000000000000000555
0
1
0
0.5
0.5
0
-0.5
0.5
-1
-0.5
2
-1
0.5
2
0.25
-0.5
1.5
0.25
0.5
1.5
-0.10000000000000000555
-0.5
0.5
0.10000000000000000555
-0.5
1
0.10000000000000000555
0.5
1
-0.10000000000000000555
0
0.5
-0.10000000000000000555
0.5
0.5
0
0
0.5
0.10000000000000000555
-0.5
0.5
0.10000000000000000555
0
0.5
0.10000000000000000555
0.5
0.5

3
examples/ImuFactorsExample.cpp
View File

@ -25,7 +25,8 @@
* A row starting with "i" is the first initial position formatted with
* N, E, D, qx, qY, qZ, qW, velN, velE, velD
* A row starting with "0" is an imu measurement
* linAccN, linAccE, linAccD, angVelN, angVelE, angVelD
* (body frame - Forward, Right, Down)
* linAccX, linAccY, linAccZ, angVelX, angVelY, angVelX
* A row starting with "1" is a gps correction formatted with
* N, E, D, qX, qY, qZ, qW
* Note that for GPS correction, we're only using the position not the

4
gtsam/base/Matrix.h
View File

@ -29,7 +29,7 @@
#include <gtsam/config.h>
#include <boost/format.hpp>
#include <boost/function.hpp>
#include <functional>
#include <boost/tuple/tuple.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
@ -489,7 +489,7 @@ struct MultiplyWithInverseFunction {
// The function phi should calculate f(a)*b, with derivatives in a and b.
// Naturally, the derivative in b is f(a).
typedef boost::function<VectorN(
typedef std::function<VectorN(
const T&, const VectorN&, OptionalJacobian<N, M>, OptionalJacobian<N, N>)>
Operator;

7
gtsam/base/Testable.h
View File

@ -34,8 +34,9 @@
#pragma once
#include <boost/concept_check.hpp>
#include <boost/shared_ptr.hpp>
#include <functional>
#include <iostream>
#include <memory>
#include <string>
#define GTSAM_PRINT(x)((x).print(#x))
@ -119,10 +120,10 @@ namespace gtsam {
* Binary predicate on shared pointers
*/
template<class V>
struct equals_star : public std::function<bool(const boost::shared_ptr<V>&, const boost::shared_ptr<V>&)> {
struct equals_star : public std::function<bool(const std::shared_ptr<V>&, const std::shared_ptr<V>&)> {
double tol_;
equals_star(double tol = 1e-9) : tol_(tol) {}
bool operator()(const boost::shared_ptr<V>& expected, const boost::shared_ptr<V>& actual) {
bool operator()(const std::shared_ptr<V>& expected, const std::shared_ptr<V>& actual) {
if (!actual && !expected) return true;
return actual && expected && traits<V>::Equals(*actual,*expected, tol_);
}

115
gtsam/base/base.i Normal file
View File

@ -0,0 +1,115 @@
//*************************************************************************
// base
//*************************************************************************
namespace gtsam {
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/geometry/Cal3DS2.h>
#include <gtsam/geometry/Cal3Fisheye.h>
#include <gtsam/geometry/Cal3Unified.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/CalibratedCamera.h>
#include <gtsam/geometry/EssentialMatrix.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Rot2.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/StereoPoint2.h>
#include <gtsam/navigation/ImuBias.h>
// #####
#include <gtsam/base/debug.h>
bool isDebugVersion();
#include <gtsam/base/DSFMap.h>
class IndexPair {
IndexPair();
IndexPair(size_t i, size_t j);
size_t i() const;
size_t j() const;
};
// template<KEY = {gtsam::IndexPair}>
// class DSFMap {
// DSFMap();
// KEY find(const KEY& key) const;
// void merge(const KEY& x, const KEY& y);
// std::map<KEY, Set> sets();
// };
class IndexPairSet {
IndexPairSet();
// common STL methods
size_t size() const;
bool empty() const;
void clear();
// structure specific methods
void insert(gtsam::IndexPair key);
bool erase(gtsam::IndexPair key); // returns true if value was removed
bool count(gtsam::IndexPair key) const; // returns true if value exists
};
class IndexPairVector {
IndexPairVector();
IndexPairVector(const gtsam::IndexPairVector& other);
// common STL methods
size_t size() const;
bool empty() const;
void clear();
// structure specific methods
gtsam::IndexPair at(size_t i) const;
void push_back(gtsam::IndexPair key) const;
};
gtsam::IndexPairVector IndexPairSetAsArray(gtsam::IndexPairSet& set);
class IndexPairSetMap {
IndexPairSetMap();
// common STL methods
size_t size() const;
bool empty() const;
void clear();
// structure specific methods
gtsam::IndexPairSet at(gtsam::IndexPair& key);
};
class DSFMapIndexPair {
DSFMapIndexPair();
gtsam::IndexPair find(const gtsam::IndexPair& key) const;
void merge(const gtsam::IndexPair& x, const gtsam::IndexPair& y);
gtsam::IndexPairSetMap sets();
};
#include <gtsam/base/Matrix.h>
bool linear_independent(Matrix A, Matrix B, double tol);
#include <gtsam/base/Value.h>
virtual class Value {
// No constructors because this is an abstract class
// Testable
void print(string s = "") const;
// Manifold
size_t dim() const;
};
#include <gtsam/base/GenericValue.h>
template <T = {Vector, Matrix, gtsam::Point2, gtsam::Point3, gtsam::Rot2,
gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::StereoPoint2,
gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler,
gtsam::Cal3Fisheye, gtsam::Cal3Unified, gtsam::EssentialMatrix,
gtsam::CalibratedCamera, gtsam::imuBias::ConstantBias}>
virtual class GenericValue : gtsam::Value {
void serializable() const;
};
} // namespace gtsam

2
gtsam/base/lieProxies.h
View File

@ -24,7 +24,7 @@
*
* These should not be used outside of tests, as they are just remappings
* of the original functions. We use these to avoid needing to do
* too much boost::bind magic or writing a bunch of separate proxy functions.
* too much std::bind magic or writing a bunch of separate proxy functions.
*
* Don't expect all classes to work for all of these functions.
*/

378
gtsam/base/numericalDerivative.h
View File

@ -18,15 +18,7 @@
// \callgraph
#pragma once
#include <boost/function.hpp>
#ifdef __GNUC__
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-variable"
#endif
#include <boost/bind.hpp>
#ifdef __GNUC__
#pragma GCC diagnostic pop
#endif
#include <functional>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/linear/JacobianFactor.h>
@ -45,13 +37,13 @@ namespace gtsam {
* for a function with one relevant param and an optional derivative:
* Foo bar(const Obj& a, boost::optional<Matrix&> H1)
* Use boost.bind to restructure:
* boost::bind(bar, _1, boost::none)
* std::bind(bar, std::placeholders::_1, boost::none)
* This syntax will fix the optional argument to boost::none, while using the first argument provided
*
* For member functions, such as below, with an instantiated copy instanceOfSomeClass
* Foo SomeClass::bar(const Obj& a)
* Use boost bind as follows to create a function pointer that uses the member function:
* boost::bind(&SomeClass::bar, ref(instanceOfSomeClass), _1)
* std::bind(&SomeClass::bar, ref(instanceOfSomeClass), std::placeholders::_1)
*
* For additional details, see the documentation:
* http://www.boost.org/doc/libs/release/libs/bind/bind.html
@ -76,7 +68,7 @@ struct FixedSizeMatrix {
template <class X, int N = traits<X>::dimension>
typename Eigen::Matrix<double, N, 1> numericalGradient(
boost::function<double(const X&)> h, const X& x, double delta = 1e-5) {
std::function<double(const X&)> h, const X& x, double delta = 1e-5) {
double factor = 1.0 / (2.0 * delta);
BOOST_STATIC_ASSERT_MSG(
@ -116,7 +108,7 @@ typename Eigen::Matrix<double, N, 1> numericalGradient(
template <class Y, class X, int N = traits<X>::dimension>
// TODO Should compute fixed-size matrix
typename internal::FixedSizeMatrix<Y, X>::type numericalDerivative11(
boost::function<Y(const X&)> h, const X& x, double delta = 1e-5) {
std::function<Y(const X&)> h, const X& x, double delta = 1e-5) {
typedef typename internal::FixedSizeMatrix<Y,X>::type Matrix;
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
@ -157,7 +149,8 @@ typename internal::FixedSizeMatrix<Y, X>::type numericalDerivative11(
template<class Y, class X>
typename internal::FixedSizeMatrix<Y,X>::type numericalDerivative11(Y (*h)(const X&), const X& x,
double delta = 1e-5) {
return numericalDerivative11<Y, X>(boost::bind(h, _1), x, delta);
return numericalDerivative11<Y, X>(std::bind(h, std::placeholders::_1), x,
delta);
}
/**
@ -170,20 +163,23 @@ typename internal::FixedSizeMatrix<Y,X>::type numericalDerivative11(Y (*h)(const
* @tparam int N is the dimension of the X1 input value if variable dimension type but known at test time
*/
template<class Y, class X1, class X2, int N = traits<X1>::dimension>
typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative21(const boost::function<Y(const X1&, const X2&)>& h,
typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative21(const std::function<Y(const X1&, const X2&)>& h,
const X1& x1, const X2& x2, double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X1, N>(boost::bind(h, _1, boost::cref(x2)), x1, delta);
return numericalDerivative11<Y, X1, N>(
std::bind(h, std::placeholders::_1, std::cref(x2)), x1, delta);
}
/** use a raw C++ function pointer */
template<class Y, class X1, class X2>
typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative21(Y (*h)(const X1&, const X2&), const X1& x1,
const X2& x2, double delta = 1e-5) {
return numericalDerivative21<Y, X1, X2>(boost::bind(h, _1, _2), x1, x2, delta);
return numericalDerivative21<Y, X1, X2>(
std::bind(h, std::placeholders::_1, std::placeholders::_2), x1, x2,
delta);
}
/**
@ -196,20 +192,23 @@ typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative21(Y (*h)(cons
* @tparam int N is the dimension of the X2 input value if variable dimension type but known at test time
*/
template<class Y, class X1, class X2, int N = traits<X2>::dimension>
typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative22(boost::function<Y(const X1&, const X2&)> h,
typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative22(std::function<Y(const X1&, const X2&)> h,
const X1& x1, const X2& x2, double delta = 1e-5) {
// BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
// "Template argument X1 must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X2>::structure_category>::value),
"Template argument X2 must be a manifold type.");
return numericalDerivative11<Y, X2, N>(boost::bind(h, boost::cref(x1), _1), x2, delta);
return numericalDerivative11<Y, X2, N>(
std::bind(h, std::cref(x1), std::placeholders::_1), x2, delta);
}
/** use a raw C++ function pointer */
template<class Y, class X1, class X2>
typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative22(Y (*h)(const X1&, const X2&), const X1& x1,
const X2& x2, double delta = 1e-5) {
return numericalDerivative22<Y, X1, X2>(boost::bind(h, _1, _2), x1, x2, delta);
return numericalDerivative22<Y, X1, X2>(
std::bind(h, std::placeholders::_1, std::placeholders::_2), x1, x2,
delta);
}
/**
@ -225,20 +224,24 @@ typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative22(Y (*h)(cons
*/
template<class Y, class X1, class X2, class X3, int N = traits<X1>::dimension>
typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative31(
boost::function<Y(const X1&, const X2&, const X3&)> h, const X1& x1,
std::function<Y(const X1&, const X2&, const X3&)> h, const X1& x1,
const X2& x2, const X3& x3, double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X1, N>(boost::bind(h, _1, boost::cref(x2), boost::cref(x3)), x1, delta);
return numericalDerivative11<Y, X1, N>(
std::bind(h, std::placeholders::_1, std::cref(x2), std::cref(x3)),
x1, delta);
}
template<class Y, class X1, class X2, class X3>
typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative31(Y (*h)(const X1&, const X2&, const X3&),
const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
return numericalDerivative31<Y, X1, X2, X3>(boost::bind(h, _1, _2, _3), x1,
x2, x3, delta);
return numericalDerivative31<Y, X1, X2, X3>(
std::bind(h, std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3),
x1, x2, x3, delta);
}
/**
@ -254,20 +257,24 @@ typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative31(Y (*h)(cons
*/
template<class Y, class X1, class X2, class X3, int N = traits<X2>::dimension>
typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative32(
boost::function<Y(const X1&, const X2&, const X3&)> h, const X1& x1,
std::function<Y(const X1&, const X2&, const X3&)> h, const X1& x1,
const X2& x2, const X3& x3, double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X2>::structure_category>::value),
"Template argument X2 must be a manifold type.");
return numericalDerivative11<Y, X2, N>(boost::bind(h, boost::cref(x1), _1, boost::cref(x3)), x2, delta);
return numericalDerivative11<Y, X2, N>(
std::bind(h, std::cref(x1), std::placeholders::_1, std::cref(x3)),
x2, delta);
}
template<class Y, class X1, class X2, class X3>
inline typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative32(Y (*h)(const X1&, const X2&, const X3&),
const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
return numericalDerivative32<Y, X1, X2, X3>(boost::bind(h, _1, _2, _3), x1,
x2, x3, delta);
return numericalDerivative32<Y, X1, X2, X3>(
std::bind(h, std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3),
x1, x2, x3, delta);
}
/**
@ -283,20 +290,24 @@ inline typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative32(Y (*
*/
template<class Y, class X1, class X2, class X3, int N = traits<X3>::dimension>
typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative33(
boost::function<Y(const X1&, const X2&, const X3&)> h, const X1& x1,
std::function<Y(const X1&, const X2&, const X3&)> h, const X1& x1,
const X2& x2, const X3& x3, double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X3>::structure_category>::value),
"Template argument X3 must be a manifold type.");
return numericalDerivative11<Y, X3, N>(boost::bind(h, boost::cref(x1), boost::cref(x2), _1), x3, delta);
return numericalDerivative11<Y, X3, N>(
std::bind(h, std::cref(x1), std::cref(x2), std::placeholders::_1),
x3, delta);
}
template<class Y, class X1, class X2, class X3>
inline typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative33(Y (*h)(const X1&, const X2&, const X3&),
const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
return numericalDerivative33<Y, X1, X2, X3>(boost::bind(h, _1, _2, _3), x1,
x2, x3, delta);
return numericalDerivative33<Y, X1, X2, X3>(
std::bind(h, std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3),
x1, x2, x3, delta);
}
/**
@ -312,19 +323,25 @@ inline typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative33(Y (*
*/
template<class Y, class X1, class X2, class X3, class X4, int N = traits<X1>::dimension>
typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative41(
boost::function<Y(const X1&, const X2&, const X3&, const X4&)> h, const X1& x1,
std::function<Y(const X1&, const X2&, const X3&, const X4&)> h, const X1& x1,
const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X1, N>(boost::bind(h, _1, boost::cref(x2), boost::cref(x3), boost::cref(x4)), x1, delta);
return numericalDerivative11<Y, X1, N>(
std::bind(h, std::placeholders::_1, std::cref(x2), std::cref(x3),
std::cref(x4)),
x1, delta);
}
template<class Y, class X1, class X2, class X3, class X4>
inline typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative41(Y (*h)(const X1&, const X2&, const X3&, const X4&),
const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
return numericalDerivative41<Y, X1, X2, X3, X4>(boost::bind(h, _1, _2, _3, _4), x1, x2, x3, x4);
return numericalDerivative41<Y, X1, X2, X3, X4>(
std::bind(h, std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, std::placeholders::_4),
x1, x2, x3, x4);
}
/**
@ -340,19 +357,25 @@ inline typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative41(Y (*
*/
template<class Y, class X1, class X2, class X3, class X4, int N = traits<X2>::dimension>
typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative42(
boost::function<Y(const X1&, const X2&, const X3&, const X4&)> h, const X1& x1,
std::function<Y(const X1&, const X2&, const X3&, const X4&)> h, const X1& x1,
const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X2>::structure_category>::value),
"Template argument X2 must be a manifold type.");
return numericalDerivative11<Y, X2, N>(boost::bind(h, boost::cref(x1), _1, boost::cref(x3), boost::cref(x4)), x2, delta);
return numericalDerivative11<Y, X2, N>(
std::bind(h, std::cref(x1), std::placeholders::_1, std::cref(x3),
std::cref(x4)),
x2, delta);
}
template<class Y, class X1, class X2, class X3, class X4>
inline typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative42(Y (*h)(const X1&, const X2&, const X3&, const X4&),
const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
return numericalDerivative42<Y, X1, X2, X3, X4>(boost::bind(h, _1, _2, _3, _4), x1, x2, x3, x4);
return numericalDerivative42<Y, X1, X2, X3, X4>(
std::bind(h, std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, std::placeholders::_4),
x1, x2, x3, x4);
}
/**
@ -368,19 +391,25 @@ inline typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative42(Y (*
*/
template<class Y, class X1, class X2, class X3, class X4, int N = traits<X3>::dimension>
typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative43(
boost::function<Y(const X1&, const X2&, const X3&, const X4&)> h, const X1& x1,
std::function<Y(const X1&, const X2&, const X3&, const X4&)> h, const X1& x1,
const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X3>::structure_category>::value),
"Template argument X3 must be a manifold type.");
return numericalDerivative11<Y, X3, N>(boost::bind(h, boost::cref(x1), boost::cref(x2), _1, boost::cref(x4)), x3, delta);
return numericalDerivative11<Y, X3, N>(
std::bind(h, std::cref(x1), std::cref(x2), std::placeholders::_1,
std::cref(x4)),
x3, delta);
}
template<class Y, class X1, class X2, class X3, class X4>
inline typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative43(Y (*h)(const X1&, const X2&, const X3&, const X4&),
const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
return numericalDerivative43<Y, X1, X2, X3, X4>(boost::bind(h, _1, _2, _3, _4), x1, x2, x3, x4);
return numericalDerivative43<Y, X1, X2, X3, X4>(
std::bind(h, std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, std::placeholders::_4),
x1, x2, x3, x4);
}
/**
@ -396,19 +425,25 @@ inline typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative43(Y (*
*/
template<class Y, class X1, class X2, class X3, class X4, int N = traits<X4>::dimension>
typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative44(
boost::function<Y(const X1&, const X2&, const X3&, const X4&)> h, const X1& x1,
std::function<Y(const X1&, const X2&, const X3&, const X4&)> h, const X1& x1,
const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X4>::structure_category>::value),
"Template argument X4 must be a manifold type.");
return numericalDerivative11<Y, X4, N>(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), _1), x4, delta);
return numericalDerivative11<Y, X4, N>(
std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3),
std::placeholders::_1),
x4, delta);
}
template<class Y, class X1, class X2, class X3, class X4>
inline typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative44(Y (*h)(const X1&, const X2&, const X3&, const X4&),
const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
return numericalDerivative44<Y, X1, X2, X3, X4>(boost::bind(h, _1, _2, _3, _4), x1, x2, x3, x4);
return numericalDerivative44<Y, X1, X2, X3, X4>(
std::bind(h, std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, std::placeholders::_4),
x1, x2, x3, x4);
}
/**
@ -425,19 +460,26 @@ inline typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative44(Y (*
*/
template<class Y, class X1, class X2, class X3, class X4, class X5, int N = traits<X1>::dimension>
typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative51(
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
std::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X1, N>(boost::bind(h, _1, boost::cref(x2), boost::cref(x3), boost::cref(x4), boost::cref(x5)), x1, delta);
return numericalDerivative11<Y, X1, N>(
std::bind(h, std::placeholders::_1, std::cref(x2), std::cref(x3),
std::cref(x4), std::cref(x5)),
x1, delta);
}
template<class Y, class X1, class X2, class X3, class X4, class X5>
inline typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative51(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&),
const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
return numericalDerivative51<Y, X1, X2, X3, X4, X5>(boost::bind(h, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5);
return numericalDerivative51<Y, X1, X2, X3, X4, X5>(
std::bind(h, std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, std::placeholders::_4,
std::placeholders::_5),
x1, x2, x3, x4, x5);
}
/**
@ -454,19 +496,26 @@ inline typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative51(Y (*
*/
template<class Y, class X1, class X2, class X3, class X4, class X5, int N = traits<X2>::dimension>
typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative52(
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
std::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X2, N>(boost::bind(h, boost::cref(x1), _1, boost::cref(x3), boost::cref(x4), boost::cref(x5)), x2, delta);
return numericalDerivative11<Y, X2, N>(
std::bind(h, std::cref(x1), std::placeholders::_1, std::cref(x3),
std::cref(x4), std::cref(x5)),
x2, delta);
}
template<class Y, class X1, class X2, class X3, class X4, class X5>
inline typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative52(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&),
const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
return numericalDerivative52<Y, X1, X2, X3, X4, X5>(boost::bind(h, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5);
return numericalDerivative52<Y, X1, X2, X3, X4, X5>(
std::bind(h, std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, std::placeholders::_4,
std::placeholders::_5),
x1, x2, x3, x4, x5);
}
/**
@ -483,19 +532,26 @@ inline typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative52(Y (*
*/
template<class Y, class X1, class X2, class X3, class X4, class X5, int N = traits<X3>::dimension>
typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative53(
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
std::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X3, N>(boost::bind(h, boost::cref(x1), boost::cref(x2), _1, boost::cref(x4), boost::cref(x5)), x3, delta);
return numericalDerivative11<Y, X3, N>(
std::bind(h, std::cref(x1), std::cref(x2), std::placeholders::_1,
std::cref(x4), std::cref(x5)),
x3, delta);
}
template<class Y, class X1, class X2, class X3, class X4, class X5>
inline typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative53(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&),
const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
return numericalDerivative53<Y, X1, X2, X3, X4, X5>(boost::bind(h, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5);
return numericalDerivative53<Y, X1, X2, X3, X4, X5>(
std::bind(h, std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, std::placeholders::_4,
std::placeholders::_5),
x1, x2, x3, x4, x5);
}
/**
@ -512,19 +568,26 @@ inline typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative53(Y (*
*/
template<class Y, class X1, class X2, class X3, class X4, class X5, int N = traits<X4>::dimension>
typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative54(
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
std::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X4, N>(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), _1, boost::cref(x5)), x4, delta);
return numericalDerivative11<Y, X4, N>(
std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3),
std::placeholders::_1, std::cref(x5)),
x4, delta);
}
template<class Y, class X1, class X2, class X3, class X4, class X5>
inline typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative54(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&),
const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
return numericalDerivative54<Y, X1, X2, X3, X4, X5>(boost::bind(h, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5);
return numericalDerivative54<Y, X1, X2, X3, X4, X5>(
std::bind(h, std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, std::placeholders::_4,
std::placeholders::_5),
x1, x2, x3, x4, x5);
}
/**
@ -541,19 +604,26 @@ inline typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative54(Y (*
*/
template<class Y, class X1, class X2, class X3, class X4, class X5, int N = traits<X5>::dimension>
typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative55(
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
std::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X5, N>(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), boost::cref(x4), _1), x5, delta);
return numericalDerivative11<Y, X5, N>(
std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3),
std::cref(x4), std::placeholders::_1),
x5, delta);
}
template<class Y, class X1, class X2, class X3, class X4, class X5>
inline typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative55(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&),
const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
return numericalDerivative55<Y, X1, X2, X3, X4, X5>(boost::bind(h, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5);
return numericalDerivative55<Y, X1, X2, X3, X4, X5>(
std::bind(h, std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, std::placeholders::_4,
std::placeholders::_5),
x1, x2, x3, x4, x5);
}
/**
@ -571,19 +641,26 @@ inline typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative55(Y (*
*/
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6, int N = traits<X1>::dimension>
typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative61(
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
std::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X1, N>(boost::bind(h, _1, boost::cref(x2), boost::cref(x3), boost::cref(x4), boost::cref(x5), boost::cref(x6)), x1, delta);
return numericalDerivative11<Y, X1, N>(
std::bind(h, std::placeholders::_1, std::cref(x2), std::cref(x3),
std::cref(x4), std::cref(x5), std::cref(x6)),
x1, delta);
}
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
inline typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative61(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&),
const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
return numericalDerivative61<Y, X1, X2, X3, X4, X5, X6>(boost::bind(h, _1, _2, _3, _4, _5, _6), x1, x2, x3, x4, x5, x6);
return numericalDerivative61<Y, X1, X2, X3, X4, X5, X6>(
std::bind(h, std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, std::placeholders::_4,
std::placeholders::_5, std::placeholders::_6),
x1, x2, x3, x4, x5, x6);
}
/**
@ -601,19 +678,26 @@ inline typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative61(Y (*
*/
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6, int N = traits<X2>::dimension>
typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative62(
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
std::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X2, N>(boost::bind(h, boost::cref(x1), _1, boost::cref(x3), boost::cref(x4), boost::cref(x5), boost::cref(x6)), x2, delta);
return numericalDerivative11<Y, X2, N>(
std::bind(h, std::cref(x1), std::placeholders::_1, std::cref(x3),
std::cref(x4), std::cref(x5), std::cref(x6)),
x2, delta);
}
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
inline typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative62(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&),
const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
return numericalDerivative62<Y, X1, X2, X3, X4, X5, X6>(boost::bind(h, _1, _2, _3, _4, _5, _6), x1, x2, x3, x4, x5, x6);
return numericalDerivative62<Y, X1, X2, X3, X4, X5, X6>(
std::bind(h, std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, std::placeholders::_4,
std::placeholders::_5, std::placeholders::_6),
x1, x2, x3, x4, x5, x6);
}
/**
@ -631,19 +715,26 @@ inline typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative62(Y (*
*/
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6, int N = traits<X3>::dimension>
typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative63(
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
std::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X3, N>(boost::bind(h, boost::cref(x1), boost::cref(x2), _1, boost::cref(x4), boost::cref(x5), boost::cref(x6)), x3, delta);
return numericalDerivative11<Y, X3, N>(
std::bind(h, std::cref(x1), std::cref(x2), std::placeholders::_1,
std::cref(x4), std::cref(x5), std::cref(x6)),
x3, delta);
}
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
inline typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative63(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&),
const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
return numericalDerivative63<Y, X1, X2, X3, X4, X5, X6>(boost::bind(h, _1, _2, _3, _4, _5, _6), x1, x2, x3, x4, x5, x6);
return numericalDerivative63<Y, X1, X2, X3, X4, X5, X6>(
std::bind(h, std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, std::placeholders::_4,
std::placeholders::_5, std::placeholders::_6),
x1, x2, x3, x4, x5, x6);
}
/**
@ -661,19 +752,26 @@ inline typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative63(Y (*
*/
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6, int N = traits<X4>::dimension>
typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative64(
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
std::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X4, N>(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), _1, boost::cref(x5), boost::cref(x6)), x4, delta);
return numericalDerivative11<Y, X4, N>(
std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3),
std::placeholders::_1, std::cref(x5), std::cref(x6)),
x4, delta);
}
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
inline typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative64(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&),
const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
return numericalDerivative64<Y, X1, X2, X3, X4, X5>(boost::bind(h, _1, _2, _3, _4, _5, _6), x1, x2, x3, x4, x5, x6);
return numericalDerivative64<Y, X1, X2, X3, X4, X5>(
std::bind(h, std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, std::placeholders::_4,
std::placeholders::_5, std::placeholders::_6),
x1, x2, x3, x4, x5, x6);
}
/**
@ -691,19 +789,26 @@ inline typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative64(Y (*
*/
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6, int N = traits<X5>::dimension>
typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative65(
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
std::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X5, N>(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), boost::cref(x4), _1, boost::cref(x6)), x5, delta);
return numericalDerivative11<Y, X5, N>(
std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3),
std::cref(x4), std::placeholders::_1, std::cref(x6)),
x5, delta);
}
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
inline typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative65(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&),
const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
return numericalDerivative65<Y, X1, X2, X3, X4, X5, X6>(boost::bind(h, _1, _2, _3, _4, _5, _6), x1, x2, x3, x4, x5, x6);
return numericalDerivative65<Y, X1, X2, X3, X4, X5, X6>(
std::bind(h, std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, std::placeholders::_4,
std::placeholders::_5, std::placeholders::_6),
x1, x2, x3, x4, x5, x6);
}
/**
@ -721,20 +826,27 @@ inline typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative65(Y (*
*/
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6, int N = traits<X6>::dimension>
typename internal::FixedSizeMatrix<Y, X6>::type numericalDerivative66(
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h,
std::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h,
const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6,
double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X6, N>(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), boost::cref(x4), boost::cref(x5), _1), x6, delta);
return numericalDerivative11<Y, X6, N>(
std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3),
std::cref(x4), std::cref(x5), std::placeholders::_1),
x6, delta);
}
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
inline typename internal::FixedSizeMatrix<Y,X6>::type numericalDerivative66(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&),
const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
return numericalDerivative66<Y, X1, X2, X3, X4, X5, X6>(boost::bind(h, _1, _2, _3, _4, _5, _6), x1, x2, x3, x4, x5, x6);
return numericalDerivative66<Y, X1, X2, X3, X4, X5, X6>(
std::bind(h, std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, std::placeholders::_4,
std::placeholders::_5, std::placeholders::_6),
x1, x2, x3, x4, x5, x6);
}
/**
@ -746,22 +858,22 @@ inline typename internal::FixedSizeMatrix<Y,X6>::type numericalDerivative66(Y (*
* @return n*n Hessian matrix computed via central differencing
*/
template<class X>
inline typename internal::FixedSizeMatrix<X,X>::type numericalHessian(boost::function<double(const X&)> f, const X& x,
inline typename internal::FixedSizeMatrix<X,X>::type numericalHessian(std::function<double(const X&)> f, const X& x,
double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X>::structure_category>::value),
"Template argument X must be a manifold type.");
typedef Eigen::Matrix<double, traits<X>::dimension, 1> VectorD;
typedef boost::function<double(const X&)> F;
typedef boost::function<VectorD(F, const X&, double)> G;
typedef std::function<double(const X&)> F;
typedef std::function<VectorD(F, const X&, double)> G;
G ng = static_cast<G>(numericalGradient<X> );
return numericalDerivative11<VectorD, X>(boost::bind(ng, f, _1, delta), x,
delta);
return numericalDerivative11<VectorD, X>(
std::bind(ng, f, std::placeholders::_1, delta), x, delta);
}
template<class X>
inline typename internal::FixedSizeMatrix<X,X>::type numericalHessian(double (*f)(const X&), const X& x, double delta =
1e-5) {
return numericalHessian(boost::function<double(const X&)>(f), x, delta);
return numericalHessian(std::function<double(const X&)>(f), x, delta);
}
/** Helper class that computes the derivative of f w.r.t. x1, centered about
@ -769,80 +881,86 @@ inline typename internal::FixedSizeMatrix<X,X>::type numericalHessian(double (*f
*/
template<class X1, class X2>
class G_x1 {
const boost::function<double(const X1&, const X2&)>& f_;
const std::function<double(const X1&, const X2&)>& f_;
X1 x1_;
double delta_;
public:
typedef typename internal::FixedSizeMatrix<X1>::type Vector;
G_x1(const boost::function<double(const X1&, const X2&)>& f, const X1& x1,
G_x1(const std::function<double(const X1&, const X2&)>& f, const X1& x1,
double delta) :
f_(f), x1_(x1), delta_(delta) {
}
Vector operator()(const X2& x2) {
return numericalGradient<X1>(boost::bind(f_, _1, boost::cref(x2)), x1_, delta_);
return numericalGradient<X1>(
std::bind(f_, std::placeholders::_1, std::cref(x2)), x1_, delta_);
}
};
template<class X1, class X2>
inline typename internal::FixedSizeMatrix<X1,X2>::type numericalHessian212(
boost::function<double(const X1&, const X2&)> f, const X1& x1, const X2& x2,
std::function<double(const X1&, const X2&)> f, const X1& x1, const X2& x2,
double delta = 1e-5) {
typedef typename internal::FixedSizeMatrix<X1>::type Vector;
G_x1<X1, X2> g_x1(f, x1, delta);
return numericalDerivative11<Vector, X2>(
boost::function<Vector(const X2&)>(
boost::bind<Vector>(boost::ref(g_x1), _1)), x2, delta);
std::function<Vector(const X2&)>(
std::bind<Vector>(std::ref(g_x1), std::placeholders::_1)),
x2, delta);
}
template<class X1, class X2>
inline typename internal::FixedSizeMatrix<X1,X2>::type numericalHessian212(double (*f)(const X1&, const X2&),
const X1& x1, const X2& x2, double delta = 1e-5) {
return numericalHessian212(boost::function<double(const X1&, const X2&)>(f),
return numericalHessian212(std::function<double(const X1&, const X2&)>(f),
x1, x2, delta);
}
template<class X1, class X2>
inline typename internal::FixedSizeMatrix<X1,X1>::type numericalHessian211(
boost::function<double(const X1&, const X2&)> f, const X1& x1, const X2& x2,
std::function<double(const X1&, const X2&)> f, const X1& x1, const X2& x2,
double delta = 1e-5) {
typedef typename internal::FixedSizeMatrix<X1>::type Vector;
Vector (*numGrad)(boost::function<double(const X1&)>, const X1&,
Vector (*numGrad)(std::function<double(const X1&)>, const X1&,
double) = &numericalGradient<X1>;
boost::function<double(const X1&)> f2(boost::bind(f, _1, boost::cref(x2)));
std::function<double(const X1&)> f2(
std::bind(f, std::placeholders::_1, std::cref(x2)));
return numericalDerivative11<Vector, X1>(
boost::function<Vector(const X1&)>(boost::bind(numGrad, f2, _1, delta)),
std::function<Vector(const X1&)>(
std::bind(numGrad, f2, std::placeholders::_1, delta)),
x1, delta);
}
template<class X1, class X2>
inline typename internal::FixedSizeMatrix<X1,X1>::type numericalHessian211(double (*f)(const X1&, const X2&),
const X1& x1, const X2& x2, double delta = 1e-5) {
return numericalHessian211(boost::function<double(const X1&, const X2&)>(f),
return numericalHessian211(std::function<double(const X1&, const X2&)>(f),
x1, x2, delta);
}
template<class X1, class X2>
inline typename internal::FixedSizeMatrix<X2,X2>::type numericalHessian222(
boost::function<double(const X1&, const X2&)> f, const X1& x1, const X2& x2,
std::function<double(const X1&, const X2&)> f, const X1& x1, const X2& x2,
double delta = 1e-5) {
typedef typename internal::FixedSizeMatrix<X2>::type Vector;
Vector (*numGrad)(boost::function<double(const X2&)>, const X2&,
Vector (*numGrad)(std::function<double(const X2&)>, const X2&,
double) = &numericalGradient<X2>;
boost::function<double(const X2&)> f2(boost::bind(f, boost::cref(x1), _1));
std::function<double(const X2&)> f2(
std::bind(f, std::cref(x1), std::placeholders::_1));
return numericalDerivative11<Vector, X2>(
boost::function<Vector(const X2&)>(boost::bind(numGrad, f2, _1, delta)),
std::function<Vector(const X2&)>(
std::bind(numGrad, f2, std::placeholders::_1, delta)),
x2, delta);
}
template<class X1, class X2>
inline typename internal::FixedSizeMatrix<X2,X2>::type numericalHessian222(double (*f)(const X1&, const X2&),
const X1& x1, const X2& x2, double delta = 1e-5) {
return numericalHessian222(boost::function<double(const X1&, const X2&)>(f),
return numericalHessian222(std::function<double(const X1&, const X2&)>(f),
x1, x2, delta);
}
@ -852,15 +970,17 @@ inline typename internal::FixedSizeMatrix<X2,X2>::type numericalHessian222(doubl
/* **************************************************************** */
template<class X1, class X2, class X3>
inline typename internal::FixedSizeMatrix<X1,X1>::type numericalHessian311(
boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
std::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
const X2& x2, const X3& x3, double delta = 1e-5) {
typedef typename internal::FixedSizeMatrix<X1>::type Vector;
Vector (*numGrad)(boost::function<double(const X1&)>, const X1&,
Vector (*numGrad)(std::function<double(const X1&)>, const X1&,
double) = &numericalGradient<X1>;
boost::function<double(const X1&)> f2(boost::bind(f, _1, boost::cref(x2), boost::cref(x3)));
std::function<double(const X1&)> f2(std::bind(
f, std::placeholders::_1, std::cref(x2), std::cref(x3)));
return numericalDerivative11<Vector, X1>(
boost::function<Vector(const X1&)>(boost::bind(numGrad, f2, _1, delta)),
std::function<Vector(const X1&)>(
std::bind(numGrad, f2, std::placeholders::_1, delta)),
x1, delta);
}
@ -868,22 +988,24 @@ template<class X1, class X2, class X3>
inline typename internal::FixedSizeMatrix<X1,X1>::type numericalHessian311(double (*f)(const X1&, const X2&, const X3&),
const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
return numericalHessian311(
boost::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
std::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
delta);
}
/* **************************************************************** */
template<class X1, class X2, class X3>
inline typename internal::FixedSizeMatrix<X2,X2>::type numericalHessian322(
boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
std::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
const X2& x2, const X3& x3, double delta = 1e-5) {
typedef typename internal::FixedSizeMatrix<X2>::type Vector;
Vector (*numGrad)(boost::function<double(const X2&)>, const X2&,
Vector (*numGrad)(std::function<double(const X2&)>, const X2&,
double) = &numericalGradient<X2>;
boost::function<double(const X2&)> f2(boost::bind(f, boost::cref(x1), _1, boost::cref(x3)));
std::function<double(const X2&)> f2(std::bind(
f, std::cref(x1), std::placeholders::_1, std::cref(x3)));
return numericalDerivative11<Vector, X2>(
boost::function<Vector(const X2&)>(boost::bind(numGrad, f2, _1, delta)),
std::function<Vector(const X2&)>(
std::bind(numGrad, f2, std::placeholders::_1, delta)),
x2, delta);
}
@ -891,22 +1013,24 @@ template<class X1, class X2, class X3>
inline typename internal::FixedSizeMatrix<X2,X2>::type numericalHessian322(double (*f)(const X1&, const X2&, const X3&),
const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
return numericalHessian322(
boost::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
std::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
delta);
}
/* **************************************************************** */
template<class X1, class X2, class X3>
inline typename internal::FixedSizeMatrix<X3,X3>::type numericalHessian333(
boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
std::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
const X2& x2, const X3& x3, double delta = 1e-5) {
typedef typename internal::FixedSizeMatrix<X3>::type Vector;
Vector (*numGrad)(boost::function<double(const X3&)>, const X3&,
Vector (*numGrad)(std::function<double(const X3&)>, const X3&,
double) = &numericalGradient<X3>;
boost::function<double(const X3&)> f2(boost::bind(f, boost::cref(x1), boost::cref(x2), _1));
std::function<double(const X3&)> f2(std::bind(
f, std::cref(x1), std::cref(x2), std::placeholders::_1));
return numericalDerivative11<Vector, X3>(
boost::function<Vector(const X3&)>(boost::bind(numGrad, f2, _1, delta)),
std::function<Vector(const X3&)>(
std::bind(numGrad, f2, std::placeholders::_1, delta)),
x3, delta);
}
@ -914,35 +1038,41 @@ template<class X1, class X2, class X3>
inline typename internal::FixedSizeMatrix<X3,X3>::type numericalHessian333(double (*f)(const X1&, const X2&, const X3&),
const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
return numericalHessian333(
boost::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
std::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
delta);
}
/* **************************************************************** */
template<class X1, class X2, class X3>
inline typename internal::FixedSizeMatrix<X1,X2>::type numericalHessian312(
boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
std::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
const X2& x2, const X3& x3, double delta = 1e-5) {
return numericalHessian212<X1, X2>(
boost::function<double(const X1&, const X2&)>(boost::bind(f, _1, _2, boost::cref(x3))),
std::function<double(const X1&, const X2&)>(
std::bind(f, std::placeholders::_1, std::placeholders::_2,
std::cref(x3))),
x1, x2, delta);
}
template<class X1, class X2, class X3>
inline typename internal::FixedSizeMatrix<X1,X3>::type numericalHessian313(
boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
std::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
const X2& x2, const X3& x3, double delta = 1e-5) {
return numericalHessian212<X1, X3>(
boost::function<double(const X1&, const X3&)>(boost::bind(f, _1, boost::cref(x2), _2)),
std::function<double(const X1&, const X3&)>(
std::bind(f, std::placeholders::_1, std::cref(x2),
std::placeholders::_2)),
x1, x3, delta);
}
template<class X1, class X2, class X3>
inline typename internal::FixedSizeMatrix<X2,X3>::type numericalHessian323(
boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
std::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
const X2& x2, const X3& x3, double delta = 1e-5) {
return numericalHessian212<X2, X3>(
boost::function<double(const X2&, const X3&)>(boost::bind(f, boost::cref(x1), _1, _2)),
std::function<double(const X2&, const X3&)>(
std::bind(f, std::cref(x1), std::placeholders::_1,
std::placeholders::_2)),
x2, x3, delta);
}
@ -951,7 +1081,7 @@ template<class X1, class X2, class X3>
inline typename internal::FixedSizeMatrix<X1,X2>::type numericalHessian312(double (*f)(const X1&, const X2&, const X3&),
const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
return numericalHessian312(
boost::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
std::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
delta);
}
@ -959,7 +1089,7 @@ template<class X1, class X2, class X3>
inline typename internal::FixedSizeMatrix<X1,X3>::type numericalHessian313(double (*f)(const X1&, const X2&, const X3&),
const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
return numericalHessian313(
boost::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
std::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
delta);
}
@ -967,7 +1097,7 @@ template<class X1, class X2, class X3>
inline typename internal::FixedSizeMatrix<X2,X3>::type numericalHessian323(double (*f)(const X1&, const X2&, const X3&),
const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
return numericalHessian323(
boost::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
std::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
delta);
}

29
gtsam/base/utilities.h Normal file
View File

@ -0,0 +1,29 @@
#pragma once
namespace gtsam {
/**
* For Python __str__().
* Redirect std cout to a string stream so we can return a string representation
* of an object when it prints to cout.
* https://stackoverflow.com/questions/5419356/redirect-stdout-stderr-to-a-string
*/
struct RedirectCout {
/// constructor -- redirect stdout buffer to a stringstream buffer
RedirectCout() : ssBuffer_(), coutBuffer_(std::cout.rdbuf(ssBuffer_.rdbuf())) {}
/// return the string
std::string str() const {
return ssBuffer_.str();
}
/// destructor -- redirect stdout buffer to its original buffer
~RedirectCout() {
std::cout.rdbuf(coutBuffer_);
}
private:
std::stringstream ssBuffer_;
std::streambuf* coutBuffer_;
};
}

4
gtsam/discrete/DecisionTree-inl.h
View File

@ -450,7 +450,7 @@ namespace gtsam {
template<typename L, typename Y>
template<typename M, typename X>
DecisionTree<L, Y>::DecisionTree(const DecisionTree<M, X>& other,
const std::map<M, L>& map, boost::function<Y(const X&)> op) {
const std::map<M, L>& map, std::function<Y(const X&)> op) {
root_ = convert(other.root_, map, op);
}
@ -568,7 +568,7 @@ namespace gtsam {
template<typename M, typename X>
typename DecisionTree<L, Y>::NodePtr DecisionTree<L, Y>::convert(
const typename DecisionTree<M, X>::NodePtr& f, const std::map<M, L>& map,
boost::function<Y(const X&)> op) {
std::function<Y(const X&)> op) {
typedef DecisionTree<M, X> MX;
typedef typename MX::Leaf MXLeaf;

12
gtsam/discrete/DecisionTree.h
View File

@ -20,10 +20,12 @@
#pragma once
#include <gtsam/discrete/Assignment.h>
#include <boost/function.hpp>
#include <functional>
#include <iostream>
#include <vector>
#include <map>
#include <vector>
namespace gtsam {
@ -38,8 +40,8 @@ namespace gtsam {
public:
/** Handy typedefs for unary and binary function types */
typedef boost::function<Y(const Y&)> Unary;
typedef boost::function<Y(const Y&, const Y&)> Binary;
typedef std::function<Y(const Y&)> Unary;
typedef std::function<Y(const Y&, const Y&)> Binary;
/** A label annotated with cardinality */
typedef std::pair<L,size_t> LabelC;
@ -107,7 +109,7 @@ namespace gtsam {
/** Convert to a different type */
template<typename M, typename X> NodePtr
convert(const typename DecisionTree<M, X>::NodePtr& f, const std::map<M,
L>& map, boost::function<Y(const X&)> op);
L>& map, std::function<Y(const X&)> op);
/** Default constructor */
DecisionTree();
@ -143,7 +145,7 @@ namespace gtsam {
/** Convert from a different type */
template<typename M, typename X>
DecisionTree(const DecisionTree<M, X>& other,
const std::map<M, L>& map, boost::function<Y(const X&)> op);
const std::map<M, L>& map, std::function<Y(const X&)> op);
/// @}
/// @name Testable

3
gtsam/discrete/DiscreteBayesNet.h
View File

@ -55,6 +55,9 @@ namespace gtsam {
template<class DERIVEDCONDITIONAL>
DiscreteBayesNet(const FactorGraph<DERIVEDCONDITIONAL>& graph) : Base(graph) {}
/// Destructor
virtual ~DiscreteBayesNet() {}
/// @}
/// @name Testable

11
gtsam/discrete/DiscreteFactor.h
View File

@ -74,13 +74,14 @@ public:
/// @name Testable
/// @{
// equals
/// equals
virtual bool equals(const DiscreteFactor& lf, double tol = 1e-9) const = 0;
// print
virtual void print(const std::string& s = "DiscreteFactor\n",
const KeyFormatter& formatter = DefaultKeyFormatter) const {
Factor::print(s, formatter);
/// print
void print(
const std::string& s = "DiscreteFactor\n",
const KeyFormatter& formatter = DefaultKeyFormatter) const override {
Base::print(s, formatter);
}
/** Test whether the factor is empty */

8
gtsam/discrete/DiscreteFactorGraph.h
View File

@ -91,6 +91,9 @@ public:
template<class DERIVEDFACTOR>
DiscreteFactorGraph(const FactorGraph<DERIVEDFACTOR>& graph) : Base(graph) {}
/// Destructor
virtual ~DiscreteFactorGraph() {}
/// @name Testable
/// @{
@ -129,8 +132,9 @@ public:
double operator()(const DiscreteFactor::Values & values) const;
/// print
void print(const std::string& s = "DiscreteFactorGraph",
const KeyFormatter& formatter =DefaultKeyFormatter) const;
void print(
const std::string& s = "DiscreteFactorGraph",
const KeyFormatter& formatter = DefaultKeyFormatter) const override;
/** Solve the factor graph by performing variable elimination in COLAMD order using
* the dense elimination function specified in \c function,

2
gtsam/discrete/tests/testAlgebraicDecisionTree.cpp
View File

@ -54,7 +54,7 @@ void dot(const T&f, const string& filename) {
}
/** I can't get this to work !
class Mul: boost::function<double(const double&, const double&)> {
class Mul: std::function<double(const double&, const double&)> {
inline double operator()(const double& a, const double& b) {
return a * b;
}

2
gtsam/discrete/tests/testDecisionTree.cpp
View File

@ -196,7 +196,7 @@ TEST(DT, conversion)
map<string, Label> ordering;
ordering[A] = X;
ordering[B] = Y;
boost::function<bool(const int&)> op = convert;
std::function<bool(const int&)> op = convert;
BDT f2(f1, ordering, op);
// f1.print("f1");
// f2.print("f2");

30
gtsam/geometry/Cal3Bundler.cpp
View File

@ -96,23 +96,27 @@ Point2 Cal3Bundler::calibrate(const Point2& pi, OptionalJacobian<2, 3> Dcal,
OptionalJacobian<2, 2> Dp) const {
// Copied from Cal3DS2
// but specialized with k1, k2 non-zero only and fx=fy and s=0
double x = (pi.x() - u0_) / fx_, y = (pi.y() - v0_) / fx_;
const Point2 invKPi(x, y);
// initialize by ignoring the distortion at all, might be problematic for
// pixels around boundary
Point2 pn(x, y);
double px = (pi.x() - u0_) / fx_, py = (pi.y() - v0_) / fx_;
const Point2 invKPi(px, py);
Point2 pn;
// iterate until the uncalibrate is close to the actual pixel coordinate
const int maxIterations = 10;
int iteration;
for (iteration = 0; iteration < maxIterations; ++iteration) {
if (distance2(uncalibrate(pn), pi) <= tol_) break;
const double px = pn.x(), py = pn.y(), xx = px * px, yy = py * py;
const double rr = xx + yy;
int iteration = 0;
do {
// initialize pn with distortion included
const double rr = (px * px) + (py * py);
const double g = (1 + k1_ * rr + k2_ * rr * rr);
pn = invKPi / g;
}
if (distance2(uncalibrate(pn), pi) <= tol_) break;
// Set px and py using intrinsic coordinates since that is where radial
// distortion correction is done.
px = pn.x(), py = pn.y();
iteration++;
} while (iteration < maxIterations);
if (iteration >= maxIterations)
throw std::runtime_error(
@ -148,4 +152,4 @@ Matrix25 Cal3Bundler::D2d_intrinsic_calibration(const Point2& p) const {
return H;
}
} // \ namespace gtsam
} // namespace gtsam

14
gtsam/geometry/Cal3Fisheye.cpp
View File

@ -106,11 +106,21 @@ Point2 Cal3Fisheye::uncalibrate(const Point2& p, OptionalJacobian<2, 9> H1,
/* ************************************************************************* */
Point2 Cal3Fisheye::calibrate(const Point2& uv, OptionalJacobian<2, 9> Dcal,
OptionalJacobian<2, 2> Dp) const {
// initial gues just inverts the pinhole model
// Apply inverse camera matrix to map the pixel coordinate (u, v)
// of the equidistant fisheye image to angular coordinate space (xd, yd)
// with radius theta given in radians.
const double u = uv.x(), v = uv.y();
const double yd = (v - v0_) / fy_;
const double xd = (u - s_ * yd - u0_) / fx_;
Point2 pi(xd, yd);
const double theta = sqrt(xd * xd + yd * yd);
// Provide initial guess for the Gauss-Newton search.
// The angular coordinates given by (xd, yd) are mapped back to
// the focal plane of the perspective undistorted projection pi.
// See Cal3Unified.calibrate() using the same pattern for the
// undistortion of omnidirectional fisheye projection.
const double scale = (theta > 0) ? tan(theta) / theta : 1.0;
Point2 pi(scale * xd, scale * yd);
// Perform newtons method, break when solution converges past tol_,
// throw exception if max iterations are reached

25
gtsam/geometry/CalibratedCamera.h
View File

@ -119,22 +119,20 @@ public:
/// @name Standard Constructors
/// @{
/** default constructor */
PinholeBase() {
}
/// Default constructor
PinholeBase() {}
/** constructor with pose */
explicit PinholeBase(const Pose3& pose) :
pose_(pose) {
}
/// Constructor with pose
explicit PinholeBase(const Pose3& pose) : pose_(pose) {}
/// @}
/// @name Advanced Constructors
/// @{
explicit PinholeBase(const Vector &v) :
pose_(Pose3::Expmap(v)) {
}
explicit PinholeBase(const Vector& v) : pose_(Pose3::Expmap(v)) {}
/// Default destructor
virtual ~PinholeBase() = default;
/// @}
/// @name Testable
@ -144,7 +142,7 @@ public:
bool equals(const PinholeBase &camera, double tol = 1e-9) const;
/// print
void print(const std::string& s = "PinholeBase") const;
virtual void print(const std::string& s = "PinholeBase") const;
/// @}
/// @name Standard Interface
@ -324,6 +322,11 @@ public:
/// Return canonical coordinate
Vector localCoordinates(const CalibratedCamera& T2) const;
/// print
void print(const std::string& s = "CalibratedCamera") const override {
PinholeBase::print(s);
}
/// @deprecated
inline size_t dim() const {
return dimension;

98
gtsam/geometry/CameraSet.h
View File

@ -69,9 +69,12 @@ protected:
public:
/// Destructor
virtual ~CameraSet() = default;
/// Definitions for blocks of F
typedef Eigen::Matrix<double, ZDim, D> MatrixZD;
typedef std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> > FBlocks;
using MatrixZD = Eigen::Matrix<double, ZDim, D>;
using FBlocks = std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD>>;
/**
* print
@ -139,6 +142,57 @@ public:
return ErrorVector(project2(point, Fs, E), measured);
}
/**
* Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix
* G = F' * F - F' * E * P * E' * F
* g = F' * (b - E * P * E' * b)
* Fixed size version
*/
template<int N, int ND> // N = 2 or 3, ND is the camera dimension
static SymmetricBlockMatrix SchurComplement(
const std::vector< Eigen::Matrix<double, ZDim, ND>, Eigen::aligned_allocator< Eigen::Matrix<double, ZDim, ND> > >& Fs,
const Matrix& E, const Eigen::Matrix<double, N, N>& P, const Vector& b) {
// a single point is observed in m cameras
size_t m = Fs.size();
// Create a SymmetricBlockMatrix (augmented hessian, with extra row/column with info vector)
size_t M1 = ND * m + 1;
std::vector<DenseIndex> dims(m + 1); // this also includes the b term
std::fill(dims.begin(), dims.end() - 1, ND);
dims.back() = 1;
SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(M1, M1));
// Blockwise Schur complement
for (size_t i = 0; i < m; i++) { // for each camera
const Eigen::Matrix<double, ZDim, ND>& Fi = Fs[i];
const auto FiT = Fi.transpose();
const Eigen::Matrix<double, ZDim, N> Ei_P = //
E.block(ZDim * i, 0, ZDim, N) * P;
// D = (Dx2) * ZDim
augmentedHessian.setOffDiagonalBlock(i, m, FiT * b.segment<ZDim>(ZDim * i) // F' * b
- FiT * (Ei_P * (E.transpose() * b))); // D = (DxZDim) * (ZDimx3) * (N*ZDimm) * (ZDimm x 1)
// (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) )
augmentedHessian.setDiagonalBlock(i, FiT
* (Fi - Ei_P * E.block(ZDim * i, 0, ZDim, N).transpose() * Fi));
// upper triangular part of the hessian
for (size_t j = i + 1; j < m; j++) { // for each camera
const Eigen::Matrix<double, ZDim, ND>& Fj = Fs[j];
// (DxD) = (Dx2) * ( (2x2) * (2xD) )
augmentedHessian.setOffDiagonalBlock(i, j, -FiT
* (Ei_P * E.block(ZDim * j, 0, ZDim, N).transpose() * Fj));
}
} // end of for over cameras
augmentedHessian.diagonalBlock(m)(0, 0) += b.squaredNorm();
return augmentedHessian;
}
/**
* Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix
* G = F' * F - F' * E * P * E' * F
@ -148,45 +202,7 @@ public:
template<int N> // N = 2 or 3
static SymmetricBlockMatrix SchurComplement(const FBlocks& Fs,
const Matrix& E, const Eigen::Matrix<double, N, N>& P, const Vector& b) {
// a single point is observed in m cameras
size_t m = Fs.size();
// Create a SymmetricBlockMatrix
size_t M1 = D * m + 1;
std::vector<DenseIndex> dims(m + 1); // this also includes the b term
std::fill(dims.begin(), dims.end() - 1, D);
dims.back() = 1;
SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(M1, M1));
// Blockwise Schur complement
for (size_t i = 0; i < m; i++) { // for each camera
const MatrixZD& Fi = Fs[i];
const auto FiT = Fi.transpose();
const Eigen::Matrix<double, ZDim, N> Ei_P = //
E.block(ZDim * i, 0, ZDim, N) * P;
// D = (Dx2) * ZDim
augmentedHessian.setOffDiagonalBlock(i, m, FiT * b.segment<ZDim>(ZDim * i) // F' * b
- FiT * (Ei_P * (E.transpose() * b))); // D = (DxZDim) * (ZDimx3) * (N*ZDimm) * (ZDimm x 1)
// (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) )
augmentedHessian.setDiagonalBlock(i, FiT
* (Fi - Ei_P * E.block(ZDim * i, 0, ZDim, N).transpose() * Fi));
// upper triangular part of the hessian
for (size_t j = i + 1; j < m; j++) { // for each camera
const MatrixZD& Fj = Fs[j];
// (DxD) = (Dx2) * ( (2x2) * (2xD) )
augmentedHessian.setOffDiagonalBlock(i, j, -FiT
* (Ei_P * E.block(ZDim * j, 0, ZDim, N).transpose() * Fj));
}
} // end of for over cameras
augmentedHessian.diagonalBlock(m)(0, 0) += b.squaredNorm();
return augmentedHessian;
return SchurComplement<N,D>(Fs, E, P, b);
}
/// Computes Point Covariance P, with lambda parameter

4
gtsam/geometry/Cyclic.h
View File

@ -17,7 +17,9 @@
#include <gtsam/base/Group.h>
#include <gtsam/base/Testable.h>
#include <iostream> // for cout :-(
#include <cassert>
#include <iostream> // for cout :-(
namespace gtsam {

2
gtsam/geometry/PinholeCamera.h
View File

@ -148,7 +148,7 @@ public:
}
/// print
void print(const std::string& s = "PinholeCamera") const {
void print(const std::string& s = "PinholeCamera") const override {
Base::print(s);
K_.print(s + ".calibration");
}

2
gtsam/geometry/PinholePose.h
View File

@ -340,7 +340,7 @@ public:
}
/// print
void print(const std::string& s = "PinholePose") const {
void print(const std::string& s = "PinholePose") const override {
Base::print(s);
if (!K_)
std::cout << "s No calibration given" << std::endl;

13
gtsam/geometry/Point2.h
View File

@ -82,5 +82,18 @@ GTSAM_EXPORT std::list<Point2> circleCircleIntersection(Point2 c1, Point2 c2, bo
GTSAM_EXPORT std::list<Point2> circleCircleIntersection(Point2 c1, double r1,
Point2 c2, double r2, double tol = 1e-9);
template <typename A1, typename A2>
struct Range;
template <>
struct Range<Point2, Point2> {
typedef double result_type;
double operator()(const Point2& p, const Point2& q,
OptionalJacobian<1, 2> H1 = boost::none,
OptionalJacobian<1, 2> H2 = boost::none) {
return distance2(p, q, H1, H2);
}
};
} // \ namespace gtsam

2
gtsam/geometry/Pose3.h
View File

@ -142,7 +142,7 @@ public:
static Vector6 Logmap(const Pose3& pose, OptionalJacobian<6, 6> Hpose = boost::none);
/**
* Calculate Adjoint map, transforming a twist in the this pose's (i.e, body) frame to the world spatial frame
* Calculate Adjoint map, transforming a twist in this pose's (i.e, body) frame to the world spatial frame
* Ad_pose is 6*6 matrix that when applied to twist xi \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$, returns Ad_pose(xi)
*/
Matrix6 AdjointMap() const; /// FIXME Not tested - marked as incorrect

12
gtsam/geometry/Rot2.cpp
View File

@ -25,12 +25,8 @@ namespace gtsam {
/* ************************************************************************* */
Rot2 Rot2::fromCosSin(double c, double s) {
if (std::abs(c * c + s * s - 1.0) > 1e-9) {
double norm_cs = sqrt(c*c + s*s);
c = c/norm_cs;
s = s/norm_cs;
}
return Rot2(c, s);
Rot2 R(c, s);
return R.normalize();
}
/* ************************************************************************* */
@ -59,8 +55,8 @@ bool Rot2::equals(const Rot2& R, double tol) const {
/* ************************************************************************* */
Rot2& Rot2::normalize() {
double scale = c_*c_ + s_*s_;
if(std::abs(scale-1.0)>1e-10) {
scale = pow(scale, -0.5);
if(std::abs(scale-1.0) > 1e-10) {
scale = 1 / sqrt(scale);
c_ *= scale;
s_ *= scale;
}

3
gtsam/geometry/Rot2.h
View File

@ -50,6 +50,9 @@ namespace gtsam {
/** default constructor, zero rotation */
Rot2() : c_(1.0), s_(0.0) {}
/** copy constructor */
Rot2(const Rot2& r) : Rot2(r.c_, r.s_) {}
/// Constructor from angle in radians == exponential map at identity
Rot2(double theta) : c_(cos(theta)), s_(sin(theta)) {}

3
gtsam/geometry/SO3.cpp
View File

@ -278,7 +278,8 @@ Vector3 SO3::Logmap(const SO3& Q, ChartJacobian H) {
} else {
// when theta near 0, +-2pi, +-4pi, etc. (trace near 3.0)
// use Taylor expansion: theta \approx 1/2-(t-3)/12 + O((t-3)^2)
magnitude = 0.5 - tr_3 * tr_3 / 12.0;
// see https://github.com/borglab/gtsam/issues/746 for details
magnitude = 0.5 - tr_3 / 12.0;
}
omega = magnitude * Vector3(R32 - R23, R13 - R31, R21 - R12);
}

3
gtsam/geometry/SimpleCamera.h
View File

@ -21,7 +21,9 @@
#include <gtsam/geometry/BearingRange.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/geometry/Cal3DS2.h>
#include <gtsam/geometry/Cal3Fisheye.h>
#include <gtsam/geometry/Cal3Unified.h>
#include <gtsam/geometry/Cal3Fisheye.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/PinholeCamera.h>
@ -33,6 +35,7 @@ namespace gtsam {
using PinholeCameraCal3Bundler = gtsam::PinholeCamera<gtsam::Cal3Bundler>;
using PinholeCameraCal3DS2 = gtsam::PinholeCamera<gtsam::Cal3DS2>;
using PinholeCameraCal3Unified = gtsam::PinholeCamera<gtsam::Cal3Unified>;
using PinholeCameraCal3Fisheye = gtsam::PinholeCamera<gtsam::Cal3Fisheye>;
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
/**

1001
gtsam/geometry/geometry.i Normal file
View File

File diff suppressed because it is too large Load Diff

54
gtsam/geometry/tests/testCal3Bundler.cpp
View File

@ -62,6 +62,60 @@ Point2 calibrate_(const Cal3Bundler& k, const Point2& pt) {
return k.calibrate(pt);
}
/* ************************************************************************* */
TEST(Cal3Bundler, DuncalibrateDefault) {
Cal3Bundler trueK(1, 0, 0);
Matrix Dcal, Dp;
Point2 actual = trueK.uncalibrate(p, Dcal, Dp);
Point2 expected = p;
CHECK(assert_equal(expected, actual, 1e-7));
Matrix numerical1 = numericalDerivative21(uncalibrate_, trueK, p);
Matrix numerical2 = numericalDerivative22(uncalibrate_, trueK, p);
CHECK(assert_equal(numerical1, Dcal, 1e-7));
CHECK(assert_equal(numerical2, Dp, 1e-7));
}
/* ************************************************************************* */
TEST(Cal3Bundler, DcalibrateDefault) {
Cal3Bundler trueK(1, 0, 0);
Matrix Dcal, Dp;
Point2 pn(0.5, 0.5);
Point2 pi = trueK.uncalibrate(pn);
Point2 actual = trueK.calibrate(pi, Dcal, Dp);
CHECK(assert_equal(pn, actual, 1e-7));
Matrix numerical1 = numericalDerivative21(calibrate_, trueK, pi);
Matrix numerical2 = numericalDerivative22(calibrate_, trueK, pi);
CHECK(assert_equal(numerical1, Dcal, 1e-5));
CHECK(assert_equal(numerical2, Dp, 1e-5));
}
/* ************************************************************************* */
TEST(Cal3Bundler, DuncalibratePrincipalPoint) {
Cal3Bundler K(5, 0, 0, 2, 2);
Matrix Dcal, Dp;
Point2 actual = K.uncalibrate(p, Dcal, Dp);
Point2 expected(12, 17);
CHECK(assert_equal(expected, actual, 1e-7));
Matrix numerical1 = numericalDerivative21(uncalibrate_, K, p);
Matrix numerical2 = numericalDerivative22(uncalibrate_, K, p);
CHECK(assert_equal(numerical1, Dcal, 1e-7));
CHECK(assert_equal(numerical2, Dp, 1e-7));
}
/* ************************************************************************* */
TEST(Cal3Bundler, DcalibratePrincipalPoint) {
Cal3Bundler K(2, 0, 0, 2, 2);
Matrix Dcal, Dp;
Point2 pn(0.5, 0.5);
Point2 pi = K.uncalibrate(pn);
Point2 actual = K.calibrate(pi, Dcal, Dp);
CHECK(assert_equal(pn, actual, 1e-7));
Matrix numerical1 = numericalDerivative21(calibrate_, K, pi);
Matrix numerical2 = numericalDerivative22(calibrate_, K, pi);
CHECK(assert_equal(numerical1, Dcal, 1e-5));
CHECK(assert_equal(numerical2, Dp, 1e-5));
}
/* ************************************************************************* */
TEST(Cal3Bundler, Duncalibrate) {
Matrix Dcal, Dp;

5
gtsam/geometry/tests/testCalibratedCamera.cpp
View File

@ -24,6 +24,7 @@
#include <iostream>
using namespace std::placeholders;
using namespace std;
using namespace gtsam;
@ -52,8 +53,8 @@ TEST(CalibratedCamera, Create) {
EXPECT(assert_equal(camera, CalibratedCamera::Create(kDefaultPose, actualH)));
// Check derivative
boost::function<CalibratedCamera(Pose3)> f = //
boost::bind(CalibratedCamera::Create, _1, boost::none);
std::function<CalibratedCamera(Pose3)> f = //
std::bind(CalibratedCamera::Create, std::placeholders::_1, boost::none);
Matrix numericalH = numericalDerivative11<CalibratedCamera, Pose3>(f, kDefaultPose);
EXPECT(assert_equal(numericalH, actualH, 1e-9));
}

1
gtsam/geometry/tests/testCameraSet.cpp
View File

@ -20,7 +20,6 @@
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/bind.hpp>
using namespace std;
using namespace gtsam;

24
gtsam/geometry/tests/testEssentialMatrix.cpp
View File

@ -5,13 +5,15 @@
* @date December 17, 2013
*/
#include <gtsam/geometry/EssentialMatrix.h>
#include <gtsam/geometry/CalibratedCamera.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/Testable.h>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/CalibratedCamera.h>
#include <gtsam/geometry/EssentialMatrix.h>
#include <sstream>
using namespace std::placeholders;
using namespace std;
using namespace gtsam;
@ -39,15 +41,15 @@ TEST(EssentialMatrix, FromRotationAndDirection) {
1e-8));
Matrix expectedH1 = numericalDerivative11<EssentialMatrix, Rot3>(
boost::bind(EssentialMatrix::FromRotationAndDirection, _1, trueDirection, boost::none,
boost::none),
std::bind(EssentialMatrix::FromRotationAndDirection,
std::placeholders::_1, trueDirection, boost::none, boost::none),
trueRotation);
EXPECT(assert_equal(expectedH1, actualH1, 1e-7));
Matrix expectedH2 = numericalDerivative11<EssentialMatrix, Unit3>(
boost::bind(EssentialMatrix::FromRotationAndDirection, trueRotation, _1, boost::none,
boost::none),
trueDirection);
std::bind(EssentialMatrix::FromRotationAndDirection, trueRotation,
std::placeholders::_1, boost::none, boost::none),
trueDirection);
EXPECT(assert_equal(expectedH2, actualH2, 1e-7));
}
@ -173,7 +175,7 @@ TEST (EssentialMatrix, FromPose3_a) {
Pose3 pose(trueRotation, trueTranslation); // Pose between two cameras
EXPECT(assert_equal(trueE, EssentialMatrix::FromPose3(pose, actualH), 1e-8));
Matrix expectedH = numericalDerivative11<EssentialMatrix, Pose3>(
boost::bind(EssentialMatrix::FromPose3, _1, boost::none), pose);
std::bind(EssentialMatrix::FromPose3, std::placeholders::_1, boost::none), pose);
EXPECT(assert_equal(expectedH, actualH, 1e-7));
}
@ -186,7 +188,7 @@ TEST (EssentialMatrix, FromPose3_b) {
Pose3 pose(c1Rc2, c1Tc2); // Pose between two cameras
EXPECT(assert_equal(E, EssentialMatrix::FromPose3(pose, actualH), 1e-8));
Matrix expectedH = numericalDerivative11<EssentialMatrix, Pose3>(
boost::bind(EssentialMatrix::FromPose3, _1, boost::none), pose);
std::bind(EssentialMatrix::FromPose3, std::placeholders::_1, boost::none), pose);
EXPECT(assert_equal(expectedH, actualH, 1e-5));
}

10
gtsam/geometry/tests/testOrientedPlane3.cpp
View File

@ -23,6 +23,7 @@
#include <boost/assign/std/vector.hpp>
using namespace boost::assign;
using namespace std::placeholders;
using namespace gtsam;
using namespace std;
using boost::none;
@ -136,8 +137,9 @@ TEST(OrientedPlane3, errorVector) {
Vector2(actual[0], actual[1])));
EXPECT(assert_equal(plane1.distance() - plane2.distance(), actual[2]));
boost::function<Vector3(const OrientedPlane3&, const OrientedPlane3&)> f =
boost::bind(&OrientedPlane3::errorVector, _1, _2, boost::none, boost::none);
std::function<Vector3(const OrientedPlane3&, const OrientedPlane3&)> f =
std::bind(&OrientedPlane3::errorVector, std::placeholders::_1,
std::placeholders::_2, boost::none, boost::none);
expectedH1 = numericalDerivative21(f, plane1, plane2);
expectedH2 = numericalDerivative22(f, plane1, plane2);
EXPECT(assert_equal(expectedH1, actualH1, 1e-5));
@ -148,8 +150,8 @@ TEST(OrientedPlane3, errorVector) {
TEST(OrientedPlane3, jacobian_retract) {
OrientedPlane3 plane(-1, 0.1, 0.2, 5);
Matrix33 H_actual;
boost::function<OrientedPlane3(const Vector3&)> f =
boost::bind(&OrientedPlane3::retract, plane, _1, boost::none);
std::function<OrientedPlane3(const Vector3&)> f = std::bind(
&OrientedPlane3::retract, plane, std::placeholders::_1, boost::none);
{
Vector3 v(-0.1, 0.2, 0.3);
plane.retract(v, H_actual);

10
gtsam/geometry/tests/testPinholeCamera.cpp
View File

@ -27,6 +27,7 @@
#include <cmath>
#include <iostream>
using namespace std::placeholders;
using namespace std;
using namespace gtsam;
@ -64,8 +65,9 @@ TEST(PinholeCamera, Create) {
EXPECT(assert_equal(camera, Camera::Create(pose,K, actualH1, actualH2)));
// Check derivative
boost::function<Camera(Pose3,Cal3_S2)> f = //
boost::bind(Camera::Create,_1,_2,boost::none,boost::none);
std::function<Camera(Pose3, Cal3_S2)> f = //
std::bind(Camera::Create, std::placeholders::_1, std::placeholders::_2,
boost::none, boost::none);
Matrix numericalH1 = numericalDerivative21<Camera,Pose3,Cal3_S2>(f,pose,K);
EXPECT(assert_equal(numericalH1, actualH1, 1e-9));
Matrix numericalH2 = numericalDerivative22<Camera,Pose3,Cal3_S2>(f,pose,K);
@ -79,8 +81,8 @@ TEST(PinholeCamera, Pose) {
EXPECT(assert_equal(pose, camera.getPose(actualH)));
// Check derivative
boost::function<Pose3(Camera)> f = //
boost::bind(&Camera::getPose,_1,boost::none);
std::function<Pose3(Camera)> f = //
std::bind(&Camera::getPose, std::placeholders::_1, boost::none);
Matrix numericalH = numericalDerivative11<Pose3,Camera>(f,camera);
EXPECT(assert_equal(numericalH, actualH, 1e-9));
}

4
gtsam/geometry/tests/testPinholePose.cpp
View File

@ -65,8 +65,8 @@ TEST(PinholeCamera, Pose) {
EXPECT(assert_equal(pose, camera.getPose(actualH)));
// Check derivative
boost::function<Pose3(Camera)> f = //
boost::bind(&Camera::getPose,_1,boost::none);
std::function<Pose3(Camera)> f = //
std::bind(&Camera::getPose,_1,boost::none);
Matrix numericalH = numericalDerivative11<Pose3,Camera>(f,camera);
EXPECT(assert_equal(numericalH, actualH, 1e-9));
}

1
gtsam/geometry/tests/testPinholeSet.cpp
View File

@ -20,7 +20,6 @@
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/bind.hpp>
using namespace std;
using namespace gtsam;

21
gtsam/geometry/tests/testPoint3.cpp
View File

@ -14,11 +14,14 @@
* @brief Unit tests for Point3 class
*/
#include <gtsam/geometry/Point3.h>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/geometry/Point3.h>
#include <boost/function.hpp>
using namespace std::placeholders;
using namespace gtsam;
GTSAM_CONCEPT_TESTABLE_INST(Point3)
@ -98,7 +101,7 @@ TEST( Point3, dot) {
// Use numerical derivatives to calculate the expected Jacobians
Matrix H1, H2;
boost::function<double(const Point3&, const Point3&)> f =
std::function<double(const Point3&, const Point3&)> f =
[](const Point3& p, const Point3& q) { return gtsam::dot(p, q); };
{
gtsam::dot(p, q, H1, H2);
@ -120,8 +123,9 @@ TEST( Point3, dot) {
/* ************************************************************************* */
TEST(Point3, cross) {
Matrix aH1, aH2;
boost::function<Point3(const Point3&, const Point3&)> f =
boost::bind(&gtsam::cross, _1, _2, boost::none, boost::none);
std::function<Point3(const Point3&, const Point3&)> f =
std::bind(&gtsam::cross, std::placeholders::_1, std::placeholders::_2,
boost::none, boost::none);
const Point3 omega(0, 1, 0), theta(4, 6, 8);
cross(omega, theta, aH1, aH2);
EXPECT(assert_equal(numericalDerivative21(f, omega, theta), aH1));
@ -139,8 +143,9 @@ TEST( Point3, cross2) {
// Use numerical derivatives to calculate the expected Jacobians
Matrix H1, H2;
boost::function<Point3(const Point3&, const Point3&)> f = boost::bind(&gtsam::cross, _1, _2, //
boost::none, boost::none);
std::function<Point3(const Point3&, const Point3&)> f =
std::bind(&gtsam::cross, std::placeholders::_1, std::placeholders::_2, //
boost::none, boost::none);
{
gtsam::cross(p, q, H1, H2);
EXPECT(assert_equal(numericalDerivative21<Point3,Point3>(f, p, q), H1, 1e-9));
@ -160,7 +165,7 @@ TEST (Point3, normalize) {
Point3 expected(point / sqrt(14.0));
EXPECT(assert_equal(expected, normalize(point, actualH), 1e-8));
Matrix expectedH = numericalDerivative11<Point3, Point3>(
boost::bind(gtsam::normalize, _1, boost::none), point);
std::bind(gtsam::normalize, std::placeholders::_1, boost::none), point);
EXPECT(assert_equal(expectedH, actualH, 1e-8));
}

9
gtsam/geometry/tests/testPose3.cpp
View File

@ -22,6 +22,7 @@
#include <boost/assign/std/vector.hpp> // for operator +=
using namespace boost::assign;
using namespace std::placeholders;
#include <CppUnitLite/TestHarness.h>
#include <cmath>
@ -213,7 +214,7 @@ TEST(Pose3, translation) {
EXPECT(assert_equal(Point3(3.5, -8.2, 4.2), T.translation(actualH), 1e-8));
Matrix numericalH = numericalDerivative11<Point3, Pose3>(
boost::bind(&Pose3::translation, _1, boost::none), T);
std::bind(&Pose3::translation, std::placeholders::_1, boost::none), T);
EXPECT(assert_equal(numericalH, actualH, 1e-6));
}
@ -224,7 +225,7 @@ TEST(Pose3, rotation) {
EXPECT(assert_equal(R, T.rotation(actualH), 1e-8));
Matrix numericalH = numericalDerivative11<Rot3, Pose3>(
boost::bind(&Pose3::rotation, _1, boost::none), T);
std::bind(&Pose3::rotation, std::placeholders::_1, boost::none), T);
EXPECT(assert_equal(numericalH, actualH, 1e-6));
}
@ -1050,7 +1051,9 @@ TEST(Pose3, Create) {
Matrix63 actualH1, actualH2;
Pose3 actual = Pose3::Create(R, P2, actualH1, actualH2);
EXPECT(assert_equal(T, actual));
boost::function<Pose3(Rot3,Point3)> create = boost::bind(Pose3::Create,_1,_2,boost::none,boost::none);
std::function<Pose3(Rot3, Point3)> create =
std::bind(Pose3::Create, std::placeholders::_1, std::placeholders::_2,
boost::none, boost::none);
EXPECT(assert_equal(numericalDerivative21<Pose3,Rot3,Point3>(create, R, P2), actualH1, 1e-9));
EXPECT(assert_equal(numericalDerivative22<Pose3,Rot3,Point3>(create, R, P2), actualH2, 1e-9));
}

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

@ -825,7 +825,7 @@ TEST(Rot3, RQ_derivative) {
const auto R = Rot3::RzRyRx(xyz).matrix();
const auto num = numericalDerivative11(RQ_proxy, R);
Matrix39 calc;
RQ(R, calc).second;
RQ(R, calc);
const auto err = vec_err.second;
CHECK(assert_equal(num, calc, err));

25
gtsam/geometry/tests/testSO3.cpp
View File

@ -15,13 +15,12 @@
* @author Frank Dellaert
**/
#include <gtsam/geometry/SO3.h>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/testLie.h>
#include <gtsam/geometry/SO3.h>
#include <CppUnitLite/TestHarness.h>
using namespace std::placeholders;
using namespace std;
using namespace gtsam;
@ -209,7 +208,7 @@ TEST(SO3, ExpmapDerivative) {
TEST(SO3, ExpmapDerivative2) {
const Vector3 theta(0.1, 0, 0.1);
const Matrix Jexpected = numericalDerivative11<SO3, Vector3>(
boost::bind(&SO3::Expmap, _1, boost::none), theta);
std::bind(&SO3::Expmap, std::placeholders::_1, boost::none), theta);
CHECK(assert_equal(Jexpected, SO3::ExpmapDerivative(theta)));
CHECK(assert_equal(Matrix3(Jexpected.transpose()),
@ -220,7 +219,7 @@ TEST(SO3, ExpmapDerivative2) {
TEST(SO3, ExpmapDerivative3) {
const Vector3 theta(10, 20, 30);
const Matrix Jexpected = numericalDerivative11<SO3, Vector3>(
boost::bind(&SO3::Expmap, _1, boost::none), theta);
std::bind(&SO3::Expmap, std::placeholders::_1, boost::none), theta);
CHECK(assert_equal(Jexpected, SO3::ExpmapDerivative(theta)));
CHECK(assert_equal(Matrix3(Jexpected.transpose()),
@ -275,7 +274,7 @@ TEST(SO3, ExpmapDerivative5) {
TEST(SO3, ExpmapDerivative6) {
const Vector3 thetahat(0.1, 0, 0.1);
const Matrix Jexpected = numericalDerivative11<SO3, Vector3>(
boost::bind(&SO3::Expmap, _1, boost::none), thetahat);
std::bind(&SO3::Expmap, std::placeholders::_1, boost::none), thetahat);
Matrix3 Jactual;
SO3::Expmap(thetahat, Jactual);
EXPECT(assert_equal(Jexpected, Jactual));
@ -286,7 +285,7 @@ TEST(SO3, LogmapDerivative) {
const Vector3 thetahat(0.1, 0, 0.1);
const SO3 R = SO3::Expmap(thetahat); // some rotation
const Matrix Jexpected = numericalDerivative11<Vector, SO3>(
boost::bind(&SO3::Logmap, _1, boost::none), R);
std::bind(&SO3::Logmap, std::placeholders::_1, boost::none), R);
const Matrix3 Jactual = SO3::LogmapDerivative(thetahat);
EXPECT(assert_equal(Jexpected, Jactual));
}
@ -296,7 +295,7 @@ TEST(SO3, JacobianLogmap) {
const Vector3 thetahat(0.1, 0, 0.1);
const SO3 R = SO3::Expmap(thetahat); // some rotation
const Matrix Jexpected = numericalDerivative11<Vector, SO3>(
boost::bind(&SO3::Logmap, _1, boost::none), R);
std::bind(&SO3::Logmap, std::placeholders::_1, boost::none), R);
Matrix3 Jactual;
SO3::Logmap(R, Jactual);
EXPECT(assert_equal(Jexpected, Jactual));
@ -306,7 +305,7 @@ TEST(SO3, JacobianLogmap) {
TEST(SO3, ApplyDexp) {
Matrix aH1, aH2;
for (bool nearZeroApprox : {true, false}) {
boost::function<Vector3(const Vector3&, const Vector3&)> f =
std::function<Vector3(const Vector3&, const Vector3&)> f =
[=](const Vector3& omega, const Vector3& v) {
return so3::DexpFunctor(omega, nearZeroApprox).applyDexp(v);
};
@ -329,7 +328,7 @@ TEST(SO3, ApplyDexp) {
TEST(SO3, ApplyInvDexp) {
Matrix aH1, aH2;
for (bool nearZeroApprox : {true, false}) {
boost::function<Vector3(const Vector3&, const Vector3&)> f =
std::function<Vector3(const Vector3&, const Vector3&)> f =
[=](const Vector3& omega, const Vector3& v) {
return so3::DexpFunctor(omega, nearZeroApprox).applyInvDexp(v);
};
@ -355,7 +354,7 @@ TEST(SO3, vec) {
Matrix actualH;
const Vector9 actual = R2.vec(actualH);
CHECK(assert_equal(expected, actual));
boost::function<Vector9(const SO3&)> f = [](const SO3& Q) { return Q.vec(); };
std::function<Vector9(const SO3&)> f = [](const SO3& Q) { return Q.vec(); };
const Matrix numericalH = numericalDerivative11(f, R2, 1e-5);
CHECK(assert_equal(numericalH, actualH));
}
@ -369,7 +368,7 @@ TEST(Matrix, compose) {
Matrix actualH;
const Matrix3 actual = so3::compose(M, R, actualH);
CHECK(assert_equal(expected, actual));
boost::function<Matrix3(const Matrix3&)> f = [R](const Matrix3& M) {
std::function<Matrix3(const Matrix3&)> f = [R](const Matrix3& M) {
return so3::compose(M, R);
};
Matrix numericalH = numericalDerivative11(f, M, 1e-2);

6
gtsam/geometry/tests/testSO4.cpp
View File

@ -166,7 +166,7 @@ TEST(SO4, vec) {
Matrix actualH;
const Vector16 actual = Q2.vec(actualH);
EXPECT(assert_equal(expected, actual));
boost::function<Vector16(const SO4&)> f = [](const SO4& Q) {
std::function<Vector16(const SO4&)> f = [](const SO4& Q) {
return Q.vec();
};
const Matrix numericalH = numericalDerivative11(f, Q2, 1e-5);
@ -179,7 +179,7 @@ TEST(SO4, topLeft) {
Matrix actualH;
const Matrix3 actual = topLeft(Q3, actualH);
EXPECT(assert_equal(expected, actual));
boost::function<Matrix3(const SO4&)> f = [](const SO4& Q3) {
std::function<Matrix3(const SO4&)> f = [](const SO4& Q3) {
return topLeft(Q3);
};
const Matrix numericalH = numericalDerivative11(f, Q3, 1e-5);
@ -192,7 +192,7 @@ TEST(SO4, stiefel) {
Matrix actualH;
const Matrix43 actual = stiefel(Q3, actualH);
EXPECT(assert_equal(expected, actual));
boost::function<Matrix43(const SO4&)> f = [](const SO4& Q3) {
std::function<Matrix43(const SO4&)> f = [](const SO4& Q3) {
return stiefel(Q3);
};
const Matrix numericalH = numericalDerivative11(f, Q3, 1e-5);

4
gtsam/geometry/tests/testSOn.cpp
View File

@ -189,7 +189,7 @@ Matrix RetractJacobian(size_t n) { return SOn::VectorizedGenerators(n); }
/// Test Jacobian of Retract at origin
TEST(SOn, RetractJacobian) {
Matrix actualH = RetractJacobian(3);
boost::function<Matrix(const Vector &)> h = [](const Vector &v) {
std::function<Matrix(const Vector &)> h = [](const Vector &v) {
return SOn::ChartAtOrigin::Retract(v).matrix();
};
Vector3 v;
@ -205,7 +205,7 @@ TEST(SOn, vec) {
SOn Q = SOn::ChartAtOrigin::Retract(v);
Matrix actualH;
const Vector actual = Q.vec(actualH);
boost::function<Vector(const SOn &)> h = [](const SOn &Q) { return Q.vec(); };
std::function<Vector(const SOn &)> h = [](const SOn &Q) { return Q.vec(); };
const Matrix H = numericalDerivative11<Vector, SOn, 10>(h, Q, 1e-5);
CHECK(assert_equal(H, actualH));
}

30
gtsam/geometry/tests/testSimilarity3.cpp
View File

@ -16,23 +16,22 @@
* @author Zhaoyang Lv
*/
#include <gtsam/geometry/Similarity3.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/ExpressionFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/inference/Symbol.h>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/testLie.h>
#include <gtsam/base/Testable.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Similarity3.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/ExpressionFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/slam/BetweenFactor.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/function.hpp>
#include <boost/bind.hpp>
#include <functional>
using namespace std::placeholders;
using namespace gtsam;
using namespace std;
using symbol_shorthand::X;
@ -242,8 +241,9 @@ TEST(Similarity3, GroupAction) {
EXPECT(assert_equal(Point3(2, 6, 6), Td.transformFrom(pa)));
// Test derivative
boost::function<Point3(Similarity3, Point3)> f = boost::bind(
&Similarity3::transformFrom, _1, _2, boost::none, boost::none);
// Use lambda to resolve overloaded method
std::function<Point3(const Similarity3&, const Point3&)>
f = [](const Similarity3& S, const Point3& p){ return S.transformFrom(p); };
Point3 q(1, 2, 3);
for (const auto& T : { T1, T2, T3, T4, T5, T6 }) {

41
gtsam/geometry/tests/testUnit3.cpp
View File

@ -31,13 +31,13 @@
#include <CppUnitLite/TestHarness.h>
#include <boost/bind.hpp>
#include <boost/assign/std/vector.hpp>
#include <cmath>
#include <random>
using namespace boost::assign;
using namespace std::placeholders;
using namespace gtsam;
using namespace std;
using gtsam::symbol_shorthand::U;
@ -126,8 +126,9 @@ TEST(Unit3, dot) {
// Use numerical derivatives to calculate the expected Jacobians
Matrix H1, H2;
boost::function<double(const Unit3&, const Unit3&)> f = boost::bind(&Unit3::dot, _1, _2, //
boost::none, boost::none);
std::function<double(const Unit3&, const Unit3&)> f =
std::bind(&Unit3::dot, std::placeholders::_1, std::placeholders::_2, //
boost::none, boost::none);
{
p.dot(q, H1, H2);
EXPECT(assert_equal(numericalDerivative21<double,Unit3>(f, p, q), H1, 1e-5));
@ -157,13 +158,13 @@ TEST(Unit3, error) {
// Use numerical derivatives to calculate the expected Jacobian
{
expected = numericalDerivative11<Vector2,Unit3>(
boost::bind(&Unit3::error, &p, _1, boost::none), q);
std::bind(&Unit3::error, &p, std::placeholders::_1, boost::none), q);
p.error(q, actual);
EXPECT(assert_equal(expected.transpose(), actual, 1e-5));
}
{
expected = numericalDerivative11<Vector2,Unit3>(
boost::bind(&Unit3::error, &p, _1, boost::none), r);
std::bind(&Unit3::error, &p, std::placeholders::_1, boost::none), r);
p.error(r, actual);
EXPECT(assert_equal(expected.transpose(), actual, 1e-5));
}
@ -184,25 +185,33 @@ TEST(Unit3, error2) {
// Use numerical derivatives to calculate the expected Jacobian
{
expected = numericalDerivative21<Vector2, Unit3, Unit3>(
boost::bind(&Unit3::errorVector, _1, _2, boost::none, boost::none), p, q);
std::bind(&Unit3::errorVector, std::placeholders::_1,
std::placeholders::_2, boost::none, boost::none),
p, q);
p.errorVector(q, actual, boost::none);
EXPECT(assert_equal(expected, actual, 1e-5));
}
{
expected = numericalDerivative21<Vector2, Unit3, Unit3>(
boost::bind(&Unit3::errorVector, _1, _2, boost::none, boost::none), p, r);
std::bind(&Unit3::errorVector, std::placeholders::_1,
std::placeholders::_2, boost::none, boost::none),
p, r);
p.errorVector(r, actual, boost::none);
EXPECT(assert_equal(expected, actual, 1e-5));
}
{
expected = numericalDerivative22<Vector2, Unit3, Unit3>(
boost::bind(&Unit3::errorVector, _1, _2, boost::none, boost::none), p, q);
std::bind(&Unit3::errorVector, std::placeholders::_1,
std::placeholders::_2, boost::none, boost::none),
p, q);
p.errorVector(q, boost::none, actual);
EXPECT(assert_equal(expected, actual, 1e-5));
}
{
expected = numericalDerivative22<Vector2, Unit3, Unit3>(
boost::bind(&Unit3::errorVector, _1, _2, boost::none, boost::none), p, r);
std::bind(&Unit3::errorVector, std::placeholders::_1,
std::placeholders::_2, boost::none, boost::none),
p, r);
p.errorVector(r, boost::none, actual);
EXPECT(assert_equal(expected, actual, 1e-5));
}
@ -220,13 +229,13 @@ TEST(Unit3, distance) {
// Use numerical derivatives to calculate the expected Jacobian
{
expected = numericalGradient<Unit3>(
boost::bind(&Unit3::distance, &p, _1, boost::none), q);
std::bind(&Unit3::distance, &p, std::placeholders::_1, boost::none), q);
p.distance(q, actual);
EXPECT(assert_equal(expected.transpose(), actual, 1e-5));
}
{
expected = numericalGradient<Unit3>(
boost::bind(&Unit3::distance, &p, _1, boost::none), r);
std::bind(&Unit3::distance, &p, std::placeholders::_1, boost::none), r);
p.distance(r, actual);
EXPECT(assert_equal(expected.transpose(), actual, 1e-5));
}
@ -318,7 +327,7 @@ TEST(Unit3, basis) {
Matrix62 actualH;
Matrix62 expectedH = numericalDerivative11<Vector6, Unit3>(
boost::bind(BasisTest, _1, boost::none), p);
std::bind(BasisTest, std::placeholders::_1, boost::none), p);
// without H, first time
EXPECT(assert_equal(expected, p.basis(), 1e-6));
@ -347,7 +356,7 @@ TEST(Unit3, basis_derivatives) {
p.basis(actualH);
Matrix62 expectedH = numericalDerivative11<Vector6, Unit3>(
boost::bind(BasisTest, _1, boost::none), p);
std::bind(BasisTest, std::placeholders::_1, boost::none), p);
EXPECT(assert_equal(expectedH, actualH, 1e-5));
}
}
@ -375,8 +384,8 @@ TEST(Unit3, retract) {
TEST (Unit3, jacobian_retract) {
Matrix22 H;
Unit3 p;
boost::function<Unit3(const Vector2&)> f =
boost::bind(&Unit3::retract, p, _1, boost::none);
std::function<Unit3(const Vector2&)> f =
std::bind(&Unit3::retract, p, std::placeholders::_1, boost::none);
{
Vector2 v (-0.2, 0.1);
p.retract(v, H);
@ -439,7 +448,7 @@ TEST (Unit3, FromPoint3) {
Unit3 expected(point);
EXPECT(assert_equal(expected, Unit3::FromPoint3(point, actualH), 1e-5));
Matrix expectedH = numericalDerivative11<Unit3, Point3>(
boost::bind(Unit3::FromPoint3, _1, boost::none), point);
std::bind(Unit3::FromPoint3, std::placeholders::_1, boost::none), point);
EXPECT(assert_equal(expectedH, actualH, 1e-5));
}

10
gtsam/geometry/triangulation.h
View File

@ -18,14 +18,16 @@
#pragma once
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/geometry/Cal3Fisheye.h>
#include <gtsam/geometry/Cal3Unified.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/CameraSet.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/slam/TriangulationFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/slam/TriangulationFactor.h>
namespace gtsam {
@ -499,6 +501,8 @@ TriangulationResult triangulateSafe(const CameraSet<CAMERA>& cameras,
// Vector of Cameras - used by the Python/MATLAB wrapper
using CameraSetCal3Bundler = CameraSet<PinholeCamera<Cal3Bundler>>;
using CameraSetCal3_S2 = CameraSet<PinholeCamera<Cal3_S2>>;
using CameraSetCal3Fisheye = CameraSet<PinholeCamera<Cal3Fisheye>>;
using CameraSetCal3Unified = CameraSet<PinholeCamera<Cal3Unified>>;
} // \namespace gtsam

3446
gtsam/gtsam.i
View File

File diff suppressed because it is too large Load Diff

52
gtsam/inference/BayesNet-inst.h
View File

@ -26,30 +26,30 @@
namespace gtsam {
/* ************************************************************************* */
template<class CONDITIONAL>
void BayesNet<CONDITIONAL>::print(const std::string& s, const KeyFormatter& formatter) const
{
Base::print(s, formatter);
}
/* ************************************************************************* */
template<class CONDITIONAL>
void BayesNet<CONDITIONAL>::saveGraph(const std::string &s, const KeyFormatter& keyFormatter) const
{
std::ofstream of(s.c_str());
of << "digraph G{\n";
for (auto conditional: boost::adaptors::reverse(*this)) {
typename CONDITIONAL::Frontals frontals = conditional->frontals();
Key me = frontals.front();
typename CONDITIONAL::Parents parents = conditional->parents();
for(Key p: parents)
of << keyFormatter(p) << "->" << keyFormatter(me) << std::endl;
}
of << "}";
of.close();
}
/* ************************************************************************* */
template <class CONDITIONAL>
void BayesNet<CONDITIONAL>::print(
const std::string& s, const KeyFormatter& formatter) const {
Base::print(s, formatter);
}
/* ************************************************************************* */
template <class CONDITIONAL>
void BayesNet<CONDITIONAL>::saveGraph(const std::string& s,
const KeyFormatter& keyFormatter) const {
std::ofstream of(s.c_str());
of << "digraph G{\n";
for (auto conditional : boost::adaptors::reverse(*this)) {
typename CONDITIONAL::Frontals frontals = conditional->frontals();
Key me = frontals.front();
typename CONDITIONAL::Parents parents = conditional->parents();
for (Key p : parents)
of << keyFormatter(p) << "->" << keyFormatter(me) << std::endl;
}
of << "}";
of.close();
}
} // namespace gtsam

16
gtsam/inference/BayesNet.h
View File

@ -57,16 +57,18 @@ namespace gtsam {
/// @name Testable
/// @{
/** print out graph */
void print(const std::string& s = "BayesNet",
const KeyFormatter& formatter = DefaultKeyFormatter) const;
/** print out graph */
void print(
const std::string& s = "BayesNet",
const KeyFormatter& formatter = DefaultKeyFormatter) const override;
/// @}
/// @}
/// @name Standard Interface
/// @{
/// @name Standard Interface
/// @{
void saveGraph(const std::string &s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
void saveGraph(const std::string& s,
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
};
}

4
gtsam/inference/EliminateableFactorGraph.h
View File

@ -19,7 +19,7 @@
#pragma once
#include <boost/shared_ptr.hpp>
#include <boost/function.hpp>
#include <functional>
#include <boost/variant.hpp>
#include <boost/optional.hpp>
@ -86,7 +86,7 @@ namespace gtsam {
typedef std::pair<boost::shared_ptr<ConditionalType>, boost::shared_ptr<_FactorType> > EliminationResult;
/// The function type that does a single dense elimination step on a subgraph.
typedef boost::function<EliminationResult(const FactorGraphType&, const Ordering&)> Eliminate;
typedef std::function<EliminationResult(const FactorGraphType&, const Ordering&)> Eliminate;
/// Typedef for an optional variable index as an argument to elimination functions
typedef boost::optional<const VariableIndex&> OptionalVariableIndex;

1
gtsam/inference/EliminationTree-inst.h
View File

@ -18,7 +18,6 @@
#pragma once
#include <boost/make_shared.hpp>
#include <boost/bind.hpp>
#include <stack>
#include <gtsam/base/timing.h>

4
gtsam/inference/Factor.cpp
View File

@ -33,8 +33,8 @@ namespace gtsam {
/* ************************************************************************* */
void Factor::printKeys(const std::string& s, const KeyFormatter& formatter) const {
std::cout << s << " ";
for(Key key: keys_) std::cout << " " << formatter(key);
std::cout << (s.empty() ? "" : s + " ");
for (Key key : keys_) std::cout << " " << formatter(key);
std::cout << std::endl;
}

57
gtsam/inference/Factor.h
View File

@ -102,43 +102,52 @@ typedef FastSet<FactorIndex> FactorIndexSet;
/// @}
public:
/// @name Standard Interface
/// @{
/// Default destructor
// public since it is required for boost serialization and static methods.
// virtual since it is public.
// http://isocpp.github.io/CppCoreGuidelines/CppCoreGuidelines#Rc-dtor-virtual
virtual ~Factor() = default;
/// First key
Key front() const { return keys_.front(); }
/// @name Standard Interface
/// @{
/// Last key
Key back() const { return keys_.back(); }
/// First key
Key front() const { return keys_.front(); }
/// find
const_iterator find(Key key) const { return std::find(begin(), end(), key); }
/// Last key
Key back() const { return keys_.back(); }
/// Access the factor's involved variable keys
const KeyVector& keys() const { return keys_; }
/// find
const_iterator find(Key key) const { return std::find(begin(), end(), key); }
/** Iterator at beginning of involved variable keys */
const_iterator begin() const { return keys_.begin(); }
/// Access the factor's involved variable keys
const KeyVector& keys() const { return keys_; }
/** Iterator at end of involved variable keys */
const_iterator end() const { return keys_.end(); }
/** Iterator at beginning of involved variable keys */
const_iterator begin() const { return keys_.begin(); }
/**
/** Iterator at end of involved variable keys */
const_iterator end() const { return keys_.end(); }
/**
* @return the number of variables involved in this factor
*/
size_t size() const { return keys_.size(); }
size_t size() const { return keys_.size(); }
/// @}
/// @}
/// @name Testable
/// @{
/// @name Testable
/// @{
/// print
virtual void print(
const std::string& s = "Factor",
const KeyFormatter& formatter = DefaultKeyFormatter) const;
/// print
void print(const std::string& s = "Factor", const KeyFormatter& formatter = DefaultKeyFormatter) const;
/// print only keys
void printKeys(const std::string& s = "Factor", const KeyFormatter& formatter = DefaultKeyFormatter) const;
/// print only keys
virtual void printKeys(
const std::string& s = "Factor",
const KeyFormatter& formatter = DefaultKeyFormatter) const;
protected:
/// check equality

4
gtsam/inference/FactorGraph-inst.h
View File

@ -23,8 +23,6 @@
#include <gtsam/inference/FactorGraph.h>
#include <boost/bind.hpp>
#include <stdio.h>
#include <algorithm>
#include <iostream> // for cout :-(
@ -37,7 +35,7 @@ namespace gtsam {
template <class FACTOR>
void FactorGraph<FACTOR>::print(const std::string& s,
const KeyFormatter& formatter) const {
std::cout << s << std::endl;
std::cout << (s.empty() ? "" : s + " ") << std::endl;
std::cout << "size: " << size() << std::endl;
for (size_t i = 0; i < factors_.size(); i++) {
std::stringstream ss;

13
gtsam/inference/FactorGraph.h
View File

@ -29,7 +29,6 @@
#include <Eigen/Core> // for Eigen::aligned_allocator
#include <boost/assign/list_inserter.hpp>
#include <boost/bind.hpp>
#include <boost/make_shared.hpp>
#include <boost/serialization/nvp.hpp>
#include <boost/serialization/vector.hpp>
@ -148,6 +147,10 @@ class FactorGraph {
/// @}
public:
/// Default destructor
// Public and virtual so boost serialization can call it.
virtual ~FactorGraph() = default;
/// @name Adding Single Factors
/// @{
@ -285,9 +288,9 @@ class FactorGraph {
/// @name Testable
/// @{
/** print out graph */
void print(const std::string& s = "FactorGraph",
const KeyFormatter& formatter = DefaultKeyFormatter) const;
/// print out graph
virtual void print(const std::string& s = "FactorGraph",
const KeyFormatter& formatter = DefaultKeyFormatter) const;
/** Check equality */
bool equals(const This& fg, double tol = 1e-9) const;
@ -355,7 +358,7 @@ class FactorGraph {
/** delete factor without re-arranging indexes by inserting a nullptr pointer
*/
void remove(size_t i) { factors_[i].reset(); }
void remove(size_t i) { factors_.at(i).reset(); }
/** replace a factor by index */
void replace(size_t index, sharedFactor factor) { at(index) = factor; }

41
gtsam/inference/LabeledSymbol.cpp
View File

@ -15,14 +15,11 @@
* @author: Alex Cunningham
*/
#include <iostream>
#include <gtsam/inference/LabeledSymbol.h>
#include <boost/format.hpp>
#include <boost/bind.hpp>
#include <boost/lexical_cast.hpp>
#include <gtsam/inference/LabeledSymbol.h>
#include <iostream>
namespace gtsam {
@ -109,17 +106,37 @@ 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) {
return boost::bind(&LabeledSymbol::chr, boost::bind(make, _1)) == c;
std::function<bool(gtsam::Key)> LabeledSymbol::TypeTest(unsigned char c) {
// Use lambda function to check equality
auto equals = [](unsigned char s, unsigned char c) { return s == c; };
return std::bind(
equals,
std::bind(&LabeledSymbol::chr, std::bind(make, std::placeholders::_1)),
c);
}
boost::function<bool(gtsam::Key)> LabeledSymbol::LabelTest(unsigned char label) {
return boost::bind(&LabeledSymbol::label, boost::bind(make, _1)) == label;
std::function<bool(gtsam::Key)> LabeledSymbol::LabelTest(unsigned char label) {
// Use lambda function to check equality
auto equals = [](unsigned char s, unsigned char c) { return s == c; };
return std::bind(
equals,
std::bind(&LabeledSymbol::label, std::bind(make, std::placeholders::_1)),
label);
}
boost::function<bool(gtsam::Key)> LabeledSymbol::TypeLabelTest(unsigned char c, unsigned char label) {
return boost::bind(&LabeledSymbol::chr, boost::bind(make, _1)) == c &&
boost::bind(&LabeledSymbol::label, boost::bind(make, _1)) == label;
std::function<bool(gtsam::Key)> LabeledSymbol::TypeLabelTest(unsigned char c, unsigned char label) {
// Use lambda functions for && and ==
auto logical_and = [](bool is_type, bool is_label) { return is_type == is_label; };
auto equals = [](unsigned char s, unsigned char c) { return s == c; };
return std::bind(logical_and,
std::bind(equals,
std::bind(&LabeledSymbol::chr,
std::bind(make, std::placeholders::_1)),
c),
std::bind(equals,
std::bind(&LabeledSymbol::label,
std::bind(make, std::placeholders::_1)),
label));
}
/* ************************************************************************* */

8
gtsam/inference/LabeledSymbol.h
View File

@ -19,8 +19,8 @@
#pragma once
#include <functional>
#include <gtsam/inference/Symbol.h>
#include <boost/function.hpp>
namespace gtsam {
@ -89,13 +89,13 @@ public:
*/
// Checks only the type
static boost::function<bool(gtsam::Key)> TypeTest(unsigned char c);
static std::function<bool(gtsam::Key)> TypeTest(unsigned char c);
// Checks only the robot ID (label_)
static boost::function<bool(gtsam::Key)> LabelTest(unsigned char label);
static std::function<bool(gtsam::Key)> LabelTest(unsigned char label);
// Checks both type and the robot ID
static boost::function<bool(gtsam::Key)> TypeLabelTest(unsigned char c, unsigned char label);
static std::function<bool(gtsam::Key)> TypeLabelTest(unsigned char c, unsigned char label);
// Converts to upper/lower versions of labels
LabeledSymbol upper() const { return LabeledSymbol(c_, toupper(label_), j_); }

9
gtsam/inference/Symbol.cpp
View File

@ -18,8 +18,8 @@
#include <gtsam/inference/Symbol.h>
#include <boost/bind/bind.hpp>
#include <boost/format.hpp>
#include <boost/bind.hpp>
#include <limits.h>
#include <list>
@ -62,8 +62,11 @@ Symbol::operator std::string() const {
static Symbol make(gtsam::Key key) { return Symbol(key);}
boost::function<bool(Key)> Symbol::ChrTest(unsigned char c) {
return bind(&Symbol::chr, bind(make, _1)) == c;
std::function<bool(Key)> Symbol::ChrTest(unsigned char c) {
auto equals = [](unsigned char s, unsigned char c) { return s == c; };
return std::bind(
equals, std::bind(&Symbol::chr, std::bind(make, std::placeholders::_1)),
c);
}
GTSAM_EXPORT std::ostream &operator<<(std::ostream &os, const Symbol &symbol) {

7
gtsam/inference/Symbol.h
View File

@ -18,11 +18,12 @@
#pragma once
#include <gtsam/inference/Key.h>
#include <gtsam/base/Testable.h>
#include <gtsam/inference/Key.h>
#include <boost/serialization/nvp.hpp>
#include <boost/function.hpp>
#include <cstdint>
#include <functional>
namespace gtsam {
@ -114,7 +115,7 @@ public:
* Values::filter() function to retrieve all key-value pairs with the
* requested character.
*/
static boost::function<bool(Key)> ChrTest(unsigned char c);
static std::function<bool(Key)> ChrTest(unsigned char c);
/// Output stream operator that can be used with key_formatter (see Key.h).
GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &, const Symbol &);

1
gtsam/inference/inference-inst.h
View File

@ -20,7 +20,6 @@
#pragma once
#include <boost/shared_ptr.hpp>
#include <boost/bind.hpp>
#include <utility>
#include <gtsam/base/treeTraversal-inst.h>

10
gtsam/linear/GaussianBayesNet.h
View File

@ -55,6 +55,9 @@ namespace gtsam {
template<class DERIVEDCONDITIONAL>
GaussianBayesNet(const FactorGraph<DERIVEDCONDITIONAL>& graph) : Base(graph) {}
/// Destructor
virtual ~GaussianBayesNet() {}
/// @}
/// @name Testable
@ -177,6 +180,13 @@ namespace gtsam {
*/
VectorValues backSubstituteTranspose(const VectorValues& gx) const;
/// print graph
void print(
const std::string& s = "",
const KeyFormatter& formatter = DefaultKeyFormatter) const override {
Base::print(s, formatter);
}
/**
* @brief Save the GaussianBayesNet as an image. Requires `dot` to be
* installed.

7
gtsam/linear/GaussianFactor.h
View File

@ -54,8 +54,11 @@ namespace gtsam {
virtual ~GaussianFactor() {}
// Implementing Testable interface
virtual void print(const std::string& s = "",
const KeyFormatter& formatter = DefaultKeyFormatter) const = 0;
/// print
void print(
const std::string& s = "",
const KeyFormatter& formatter = DefaultKeyFormatter) const override = 0;
/** Equals for testable */
virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const = 0;

8
gtsam/linear/HessianFactor.cpp
View File

@ -32,14 +32,6 @@
#include <boost/format.hpp>
#include <boost/make_shared.hpp>
#include <boost/tuple/tuple.hpp>
#ifdef __GNUC__
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-variable"
#endif
#include <boost/bind.hpp>
#ifdef __GNUC__
#pragma GCC diagnostic pop
#endif
#include <boost/assign/list_of.hpp>
#include <boost/range/adaptor/transformed.hpp>
#include <boost/range/adaptor/map.hpp>

8
gtsam/linear/JacobianFactor.cpp
View File

@ -35,14 +35,6 @@
#include <boost/format.hpp>
#include <boost/make_shared.hpp>
#include <boost/array.hpp>
#ifdef __GNUC__
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-variable"
#endif
#include <boost/bind.hpp>
#ifdef __GNUC__
#pragma GCC diagnostic pop
#endif
#include <boost/range/algorithm/copy.hpp>
#include <boost/range/adaptor/indirected.hpp>
#include <boost/range/adaptor/map.hpp>

4
gtsam/linear/Preconditioner.h
View File

@ -44,9 +44,9 @@ struct GTSAM_EXPORT PreconditionerParameters {
inline Kernel kernel() const { return kernel_; }
inline Verbosity verbosity() const { return verbosity_; }
void print() const ;
void print() const;
virtual void print(std::ostream &os) const ;
virtual void print(std::ostream &os) const;
static Kernel kernelTranslator(const std::string &s);
static Verbosity verbosityTranslator(const std::string &s);

5
gtsam/linear/VectorValues.cpp
View File

@ -18,7 +18,7 @@
#include <gtsam/linear/VectorValues.h>
#include <boost/bind.hpp>
#include <boost/bind/bind.hpp>
#include <boost/range/combine.hpp>
#include <boost/range/numeric.hpp>
#include <boost/range/adaptor/transformed.hpp>
@ -38,7 +38,8 @@ namespace gtsam {
{
// Merge using predicate for comparing first of pair
merge(first.begin(), first.end(), second.begin(), second.end(), inserter(values_, values_.end()),
boost::bind(&less<Key>::operator(), less<Key>(), boost::bind(&KeyValuePair::first, _1), boost::bind(&KeyValuePair::first, _2)));
std::bind(&less<Key>::operator(), less<Key>(), std::bind(&KeyValuePair::first, std::placeholders::_1),
std::bind(&KeyValuePair::first, std::placeholders::_2)));
if(size() != first.size() + second.size())
throw invalid_argument("Requested to merge two VectorValues that have one or more variables in common.");
}

653
gtsam/linear/linear.i Normal file
View File

@ -0,0 +1,653 @@
//*************************************************************************
// linear
//*************************************************************************
namespace gtsam {
namespace noiseModel {
#include <gtsam/linear/NoiseModel.h>
virtual class Base {
void print(string s = "") const;
// Methods below are available for all noise models. However, can't add them
// because wrap (incorrectly) thinks robust classes derive from this Base as well.
// bool isConstrained() const;
// bool isUnit() const;
// size_t dim() const;
// Vector sigmas() const;
};
virtual class Gaussian : gtsam::noiseModel::Base {
static gtsam::noiseModel::Gaussian* Information(Matrix R);
static gtsam::noiseModel::Gaussian* SqrtInformation(Matrix R);
static gtsam::noiseModel::Gaussian* Covariance(Matrix R);
bool equals(gtsam::noiseModel::Base& expected, double tol);
// access to noise model
Matrix R() const;
Matrix information() const;
Matrix covariance() const;
// Whitening operations
Vector whiten(Vector v) const;
Vector unwhiten(Vector v) const;
Matrix Whiten(Matrix H) const;
// enabling serialization functionality
void serializable() const;
};
virtual class Diagonal : gtsam::noiseModel::Gaussian {
static gtsam::noiseModel::Diagonal* Sigmas(Vector sigmas);
static gtsam::noiseModel::Diagonal* Variances(Vector variances);
static gtsam::noiseModel::Diagonal* Precisions(Vector precisions);
Matrix R() const;
// access to noise model
Vector sigmas() const;
Vector invsigmas() const;
Vector precisions() const;
// enabling serialization functionality
void serializable() const;
};
virtual class Constrained : gtsam::noiseModel::Diagonal {
static gtsam::noiseModel::Constrained* MixedSigmas(Vector mu, Vector sigmas);
static gtsam::noiseModel::Constrained* MixedSigmas(double m, Vector sigmas);
static gtsam::noiseModel::Constrained* MixedVariances(Vector mu, Vector variances);
static gtsam::noiseModel::Constrained* MixedVariances(Vector variances);
static gtsam::noiseModel::Constrained* MixedPrecisions(Vector mu, Vector precisions);
static gtsam::noiseModel::Constrained* MixedPrecisions(Vector precisions);
static gtsam::noiseModel::Constrained* All(size_t dim);
static gtsam::noiseModel::Constrained* All(size_t dim, double mu);
gtsam::noiseModel::Constrained* unit() const;
// enabling serialization functionality
void serializable() const;
};
virtual class Isotropic : gtsam::noiseModel::Diagonal {
static gtsam::noiseModel::Isotropic* Sigma(size_t dim, double sigma);
static gtsam::noiseModel::Isotropic* Variance(size_t dim, double varianace);
static gtsam::noiseModel::Isotropic* Precision(size_t dim, double precision);
// access to noise model
double sigma() const;
// enabling serialization functionality
void serializable() const;
};
virtual class Unit : gtsam::noiseModel::Isotropic {
static gtsam::noiseModel::Unit* Create(size_t dim);
// enabling serialization functionality
void serializable() const;
};
namespace mEstimator {
virtual class Base {
void print(string s = "") const;
};
virtual class Null: gtsam::noiseModel::mEstimator::Base {
Null();
static gtsam::noiseModel::mEstimator::Null* Create();
// enabling serialization functionality
void serializable() const;
double weight(double error) const;
double loss(double error) const;
};
virtual class Fair: gtsam::noiseModel::mEstimator::Base {
Fair(double c);
static gtsam::noiseModel::mEstimator::Fair* Create(double c);
// enabling serialization functionality
void serializable() const;
double weight(double error) const;
double loss(double error) const;
};
virtual class Huber: gtsam::noiseModel::mEstimator::Base {
Huber(double k);
static gtsam::noiseModel::mEstimator::Huber* Create(double k);
// enabling serialization functionality
void serializable() const;
double weight(double error) const;
double loss(double error) const;
};
virtual class Cauchy: gtsam::noiseModel::mEstimator::Base {
Cauchy(double k);
static gtsam::noiseModel::mEstimator::Cauchy* Create(double k);
// enabling serialization functionality
void serializable() const;
double weight(double error) const;
double loss(double error) const;
};
virtual class Tukey: gtsam::noiseModel::mEstimator::Base {
Tukey(double k);
static gtsam::noiseModel::mEstimator::Tukey* Create(double k);
// enabling serialization functionality
void serializable() const;
double weight(double error) const;
double loss(double error) const;
};
virtual class Welsch: gtsam::noiseModel::mEstimator::Base {
Welsch(double k);
static gtsam::noiseModel::mEstimator::Welsch* Create(double k);
// enabling serialization functionality
void serializable() const;
double weight(double error) const;
double loss(double error) const;
};
virtual class GemanMcClure: gtsam::noiseModel::mEstimator::Base {
GemanMcClure(double c);
static gtsam::noiseModel::mEstimator::GemanMcClure* Create(double c);
// enabling serialization functionality
void serializable() const;
double weight(double error) const;
double loss(double error) const;
};
virtual class DCS: gtsam::noiseModel::mEstimator::Base {
DCS(double c);
static gtsam::noiseModel::mEstimator::DCS* Create(double c);
// enabling serialization functionality
void serializable() const;
double weight(double error) const;
double loss(double error) const;
};
virtual class L2WithDeadZone: gtsam::noiseModel::mEstimator::Base {
L2WithDeadZone(double k);
static gtsam::noiseModel::mEstimator::L2WithDeadZone* Create(double k);
// enabling serialization functionality
void serializable() const;
double weight(double error) const;
double loss(double error) const;
};
}///\namespace mEstimator
virtual class Robust : gtsam::noiseModel::Base {
Robust(const gtsam::noiseModel::mEstimator::Base* robust, const gtsam::noiseModel::Base* noise);
static gtsam::noiseModel::Robust* Create(const gtsam::noiseModel::mEstimator::Base* robust, const gtsam::noiseModel::Base* noise);
// enabling serialization functionality
void serializable() const;
};
}///\namespace noiseModel
#include <gtsam/linear/Sampler.h>
class Sampler {
// Constructors
Sampler(gtsam::noiseModel::Diagonal* model, int seed);
Sampler(Vector sigmas, int seed);
// Standard Interface
size_t dim() const;
Vector sigmas() const;
gtsam::noiseModel::Diagonal* model() const;
Vector sample();
};
#include <gtsam/linear/VectorValues.h>
class VectorValues {
//Constructors
VectorValues();
VectorValues(const gtsam::VectorValues& other);
//Named Constructors
static gtsam::VectorValues Zero(const gtsam::VectorValues& model);
//Standard Interface
size_t size() const;
size_t dim(size_t j) const;
bool exists(size_t j) const;
void print(string s = "VectorValues",
const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
bool equals(const gtsam::VectorValues& expected, double tol) const;
void insert(size_t j, Vector value);
Vector vector() const;
Vector at(size_t j) const;
void update(const gtsam::VectorValues& values);
//Advanced Interface
void setZero();
gtsam::VectorValues add(const gtsam::VectorValues& c) const;
void addInPlace(const gtsam::VectorValues& c);
gtsam::VectorValues subtract(const gtsam::VectorValues& c) const;
gtsam::VectorValues scale(double a) const;
void scaleInPlace(double a);
bool hasSameStructure(const gtsam::VectorValues& other) const;
double dot(const gtsam::VectorValues& V) const;
double norm() const;
double squaredNorm() const;
// enabling serialization functionality
void serialize() const;
// enable pickling in python
void pickle() const;
};
#include <gtsam/linear/GaussianFactor.h>
virtual class GaussianFactor {
gtsam::KeyVector keys() const;
void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
bool equals(const gtsam::GaussianFactor& lf, double tol) const;
double error(const gtsam::VectorValues& c) const;
gtsam::GaussianFactor* clone() const;
gtsam::GaussianFactor* negate() const;
Matrix augmentedInformation() const;
Matrix information() const;
Matrix augmentedJacobian() const;
pair<Matrix, Vector> jacobian() const;
size_t size() const;
bool empty() const;
};
#include <gtsam/linear/JacobianFactor.h>
virtual class JacobianFactor : gtsam::GaussianFactor {
//Constructors
JacobianFactor();
JacobianFactor(const gtsam::GaussianFactor& factor);
JacobianFactor(Vector b_in);
JacobianFactor(size_t i1, Matrix A1, Vector b,
const gtsam::noiseModel::Diagonal* model);
JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, Vector b,
const gtsam::noiseModel::Diagonal* model);
JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, size_t i3, Matrix A3,
Vector b, const gtsam::noiseModel::Diagonal* model);
JacobianFactor(const gtsam::GaussianFactorGraph& graph);
//Testable
void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
void printKeys(string s) const;
bool equals(const gtsam::GaussianFactor& lf, double tol) const;
size_t size() const;
Vector unweighted_error(const gtsam::VectorValues& c) const;
Vector error_vector(const gtsam::VectorValues& c) const;
double error(const gtsam::VectorValues& c) const;
//Standard Interface
Matrix getA() const;
Vector getb() const;
size_t rows() const;
size_t cols() const;
bool isConstrained() const;
pair<Matrix, Vector> jacobianUnweighted() const;
Matrix augmentedJacobianUnweighted() const;
void transposeMultiplyAdd(double alpha, Vector e, gtsam::VectorValues& x) const;
gtsam::JacobianFactor whiten() const;
pair<gtsam::GaussianConditional*, gtsam::JacobianFactor*> eliminate(const gtsam::Ordering& keys) const;
void setModel(bool anyConstrained, Vector sigmas);
gtsam::noiseModel::Diagonal* get_model() const;
// enabling serialization functionality
void serialize() const;
// enable pickling in python
void pickle() const;
};
#include <gtsam/linear/HessianFactor.h>
virtual class HessianFactor : gtsam::GaussianFactor {
//Constructors
HessianFactor();
HessianFactor(const gtsam::GaussianFactor& factor);
HessianFactor(size_t j, Matrix G, Vector g, double f);
HessianFactor(size_t j, Vector mu, Matrix Sigma);
HessianFactor(size_t j1, size_t j2, Matrix G11, Matrix G12, Vector g1, Matrix G22,
Vector g2, double f);
HessianFactor(size_t j1, size_t j2, size_t j3, Matrix G11, Matrix G12, Matrix G13,
Vector g1, Matrix G22, Matrix G23, Vector g2, Matrix G33, Vector g3,
double f);
HessianFactor(const gtsam::GaussianFactorGraph& factors);
//Testable
size_t size() const;
void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
void printKeys(string s) const;
bool equals(const gtsam::GaussianFactor& lf, double tol) const;
double error(const gtsam::VectorValues& c) const;
//Standard Interface
size_t rows() const;
Matrix information() const;
double constantTerm() const;
Vector linearTerm() const;
// enabling serialization functionality
void serialize() const;
// enable pickling in python
void pickle() const;
};
#include <gtsam/linear/GaussianFactorGraph.h>
class GaussianFactorGraph {
GaussianFactorGraph();
GaussianFactorGraph(const gtsam::GaussianBayesNet& bayesNet);
GaussianFactorGraph(const gtsam::GaussianBayesTree& bayesTree);
// From FactorGraph
void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
bool equals(const gtsam::GaussianFactorGraph& lfgraph, double tol) const;
size_t size() const;
gtsam::GaussianFactor* at(size_t idx) const;
gtsam::KeySet keys() const;
gtsam::KeyVector keyVector() const;
bool exists(size_t idx) const;
// Building the graph
void push_back(const gtsam::GaussianFactor* factor);
void push_back(const gtsam::GaussianConditional* conditional);
void push_back(const gtsam::GaussianFactorGraph& graph);
void push_back(const gtsam::GaussianBayesNet& bayesNet);
void push_back(const gtsam::GaussianBayesTree& bayesTree);
void add(const gtsam::GaussianFactor& factor);
void add(Vector b);
void add(size_t key1, Matrix A1, Vector b, const gtsam::noiseModel::Diagonal* model);
void add(size_t key1, Matrix A1, size_t key2, Matrix A2, Vector b,
const gtsam::noiseModel::Diagonal* model);
void add(size_t key1, Matrix A1, size_t key2, Matrix A2, size_t key3, Matrix A3,
Vector b, const gtsam::noiseModel::Diagonal* model);
// error and probability
double error(const gtsam::VectorValues& c) const;
double probPrime(const gtsam::VectorValues& c) const;
gtsam::GaussianFactorGraph clone() const;
gtsam::GaussianFactorGraph negate() const;
// Optimizing and linear algebra
gtsam::VectorValues optimize() const;
gtsam::VectorValues optimize(const gtsam::Ordering& ordering) const;
gtsam::VectorValues optimizeGradientSearch() const;
gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const;
gtsam::VectorValues gradientAtZero() const;
// Elimination and marginals
gtsam::GaussianBayesNet* eliminateSequential();
gtsam::GaussianBayesNet* eliminateSequential(const gtsam::Ordering& ordering);
gtsam::GaussianBayesTree* eliminateMultifrontal();
gtsam::GaussianBayesTree* eliminateMultifrontal(const gtsam::Ordering& ordering);
pair<gtsam::GaussianBayesNet*, gtsam::GaussianFactorGraph*> eliminatePartialSequential(
const gtsam::Ordering& ordering);
pair<gtsam::GaussianBayesNet*, gtsam::GaussianFactorGraph*> eliminatePartialSequential(
const gtsam::KeyVector& keys);
pair<gtsam::GaussianBayesTree*, gtsam::GaussianFactorGraph*> eliminatePartialMultifrontal(
const gtsam::Ordering& ordering);
pair<gtsam::GaussianBayesTree*, gtsam::GaussianFactorGraph*> eliminatePartialMultifrontal(
const gtsam::KeyVector& keys);
gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& ordering);
gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& key_vector);
gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& ordering,
const gtsam::Ordering& marginalizedVariableOrdering);
gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& key_vector,
const gtsam::Ordering& marginalizedVariableOrdering);
gtsam::GaussianFactorGraph* marginal(const gtsam::KeyVector& key_vector);
// Conversion to matrices
Matrix sparseJacobian_() const;
Matrix augmentedJacobian() const;
Matrix augmentedJacobian(const gtsam::Ordering& ordering) const;
pair<Matrix,Vector> jacobian() const;
pair<Matrix,Vector> jacobian(const gtsam::Ordering& ordering) const;
Matrix augmentedHessian() const;
Matrix augmentedHessian(const gtsam::Ordering& ordering) const;
pair<Matrix,Vector> hessian() const;
pair<Matrix,Vector> hessian(const gtsam::Ordering& ordering) const;
// enabling serialization functionality
void serialize() const;
// enable pickling in python
void pickle() const;
};
#include <gtsam/linear/GaussianConditional.h>
virtual class GaussianConditional : gtsam::JacobianFactor {
//Constructors
GaussianConditional(size_t key, Vector d, Matrix R, const gtsam::noiseModel::Diagonal* sigmas);
GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S,
const gtsam::noiseModel::Diagonal* sigmas);
GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S,
size_t name2, Matrix T, const gtsam::noiseModel::Diagonal* sigmas);
//Constructors with no noise model
GaussianConditional(size_t key, Vector d, Matrix R);
GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S);
GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S,
size_t name2, Matrix T);
//Standard Interface
void print(string s = "GaussianConditional",
const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
bool equals(const gtsam::GaussianConditional& cg, double tol) const;
// Advanced Interface
gtsam::VectorValues solve(const gtsam::VectorValues& parents) const;
gtsam::VectorValues solveOtherRHS(const gtsam::VectorValues& parents,
const gtsam::VectorValues& rhs) const;
void solveTransposeInPlace(gtsam::VectorValues& gy) const;
void scaleFrontalsBySigma(gtsam::VectorValues& gy) const;
Matrix R() const;
Matrix S() const;
Vector d() const;
// enabling serialization functionality
void serialize() const;
};
#include <gtsam/linear/GaussianDensity.h>
virtual class GaussianDensity : gtsam::GaussianConditional {
//Constructors
GaussianDensity(size_t key, Vector d, Matrix R, const gtsam::noiseModel::Diagonal* sigmas);
//Standard Interface
void print(string s = "GaussianDensity",
const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
bool equals(const gtsam::GaussianDensity &cg, double tol) const;
Vector mean() const;
Matrix covariance() const;
};
#include <gtsam/linear/GaussianBayesNet.h>
virtual class GaussianBayesNet {
//Constructors
GaussianBayesNet();
GaussianBayesNet(const gtsam::GaussianConditional* conditional);
// Testable
void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
bool equals(const gtsam::GaussianBayesNet& other, double tol) const;
size_t size() const;
// FactorGraph derived interface
// size_t size() const;
gtsam::GaussianConditional* at(size_t idx) const;
gtsam::KeySet keys() const;
bool exists(size_t idx) const;
void saveGraph(const string& s) const;
gtsam::GaussianConditional* front() const;
gtsam::GaussianConditional* back() const;
void push_back(gtsam::GaussianConditional* conditional);
void push_back(const gtsam::GaussianBayesNet& bayesNet);
gtsam::VectorValues optimize() const;
gtsam::VectorValues optimize(gtsam::VectorValues& solutionForMissing) const;
gtsam::VectorValues optimizeGradientSearch() const;
gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const;
gtsam::VectorValues gradientAtZero() const;
double error(const gtsam::VectorValues& x) const;
double determinant() const;
double logDeterminant() const;
gtsam::VectorValues backSubstitute(const gtsam::VectorValues& gx) const;
gtsam::VectorValues backSubstituteTranspose(const gtsam::VectorValues& gx) const;
};
#include <gtsam/linear/GaussianBayesTree.h>
virtual class GaussianBayesTree {
// Standard Constructors and Named Constructors
GaussianBayesTree();
GaussianBayesTree(const gtsam::GaussianBayesTree& other);
bool equals(const gtsam::GaussianBayesTree& other, double tol) const;
void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter);
size_t size() const;
bool empty() const;
size_t numCachedSeparatorMarginals() const;
void saveGraph(string s) const;
gtsam::VectorValues optimize() const;
gtsam::VectorValues optimizeGradientSearch() const;
gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const;
gtsam::VectorValues gradientAtZero() const;
double error(const gtsam::VectorValues& x) const;
double determinant() const;
double logDeterminant() const;
Matrix marginalCovariance(size_t key) const;
gtsam::GaussianConditional* marginalFactor(size_t key) const;
gtsam::GaussianFactorGraph* joint(size_t key1, size_t key2) const;
gtsam::GaussianBayesNet* jointBayesNet(size_t key1, size_t key2) const;
};
#include <gtsam/linear/Errors.h>
class Errors {
//Constructors
Errors();
Errors(const gtsam::VectorValues& V);
//Testable
void print(string s = "Errors");
bool equals(const gtsam::Errors& expected, double tol) const;
};
#include <gtsam/linear/GaussianISAM.h>
class GaussianISAM {
//Constructor
GaussianISAM();
//Standard Interface
void update(const gtsam::GaussianFactorGraph& newFactors);
void saveGraph(string s) const;
void clear();
};
#include <gtsam/linear/IterativeSolver.h>
virtual class IterativeOptimizationParameters {
string getVerbosity() const;
void setVerbosity(string s) ;
};
//virtual class IterativeSolver {
// IterativeSolver();
// gtsam::VectorValues optimize ();
//};
#include <gtsam/linear/ConjugateGradientSolver.h>
virtual class ConjugateGradientParameters : gtsam::IterativeOptimizationParameters {
ConjugateGradientParameters();
int getMinIterations() const ;
int getMaxIterations() const ;
int getReset() const;
double getEpsilon_rel() const;
double getEpsilon_abs() const;
void setMinIterations(int value);
void setMaxIterations(int value);
void setReset(int value);
void setEpsilon_rel(double value);
void setEpsilon_abs(double value);
};
#include <gtsam/linear/Preconditioner.h>
virtual class PreconditionerParameters {
PreconditionerParameters();
};
virtual class DummyPreconditionerParameters : gtsam::PreconditionerParameters {
DummyPreconditionerParameters();
};
#include <gtsam/linear/PCGSolver.h>
virtual class PCGSolverParameters : gtsam::ConjugateGradientParameters {
PCGSolverParameters();
void print(string s = "");
void setPreconditionerParams(gtsam::PreconditionerParameters* preconditioner);
};
#include <gtsam/linear/SubgraphSolver.h>
virtual class SubgraphSolverParameters : gtsam::ConjugateGradientParameters {
SubgraphSolverParameters();
};
virtual class SubgraphSolver {
SubgraphSolver(const gtsam::GaussianFactorGraph &A, const gtsam::SubgraphSolverParameters &parameters, const gtsam::Ordering& ordering);
SubgraphSolver(const gtsam::GaussianFactorGraph &Ab1, const gtsam::GaussianFactorGraph* Ab2, const gtsam::SubgraphSolverParameters &parameters, const gtsam::Ordering& ordering);
gtsam::VectorValues optimize() const;
};
#include <gtsam/linear/KalmanFilter.h>
class KalmanFilter {
KalmanFilter(size_t n);
// gtsam::GaussianDensity* init(Vector x0, const gtsam::SharedDiagonal& P0);
gtsam::GaussianDensity* init(Vector x0, Matrix P0);
void print(string s = "") const;
static size_t step(gtsam::GaussianDensity* p);
gtsam::GaussianDensity* predict(gtsam::GaussianDensity* p, Matrix F,
Matrix B, Vector u, const gtsam::noiseModel::Diagonal* modelQ);
gtsam::GaussianDensity* predictQ(gtsam::GaussianDensity* p, Matrix F,
Matrix B, Vector u, Matrix Q);
gtsam::GaussianDensity* predict2(gtsam::GaussianDensity* p, Matrix A0,
Matrix A1, Vector b, const gtsam::noiseModel::Diagonal* model);
gtsam::GaussianDensity* update(gtsam::GaussianDensity* p, Matrix H,
Vector z, const gtsam::noiseModel::Diagonal* model);
gtsam::GaussianDensity* updateQ(gtsam::GaussianDensity* p, Matrix H,
Vector z, Matrix Q);
};
}

8
gtsam/linear/tests/testGaussianBayesNet.cpp
View File

@ -25,12 +25,14 @@
#include <boost/tuple/tuple.hpp>
#include <boost/assign/list_of.hpp>
#include <boost/assign/std/list.hpp> // for operator +=
using namespace boost::assign;
#include <boost/bind/bind.hpp>
// STL/C++
#include <iostream>
#include <sstream>
using namespace boost::assign;
using namespace std::placeholders;
using namespace std;
using namespace gtsam;
@ -268,11 +270,11 @@ TEST(GaussianBayesNet, ComputeSteepestDescentPoint) {
// Compute the Hessian numerically
Matrix hessian = numericalHessian<Vector10>(
boost::bind(&computeError, gbn, _1), Vector10::Zero());
std::bind(&computeError, gbn, std::placeholders::_1), Vector10::Zero());
// Compute the gradient numerically
Vector gradient = numericalGradient<Vector10>(
boost::bind(&computeError, gbn, _1), Vector10::Zero());
std::bind(&computeError, gbn, std::placeholders::_1), Vector10::Zero());
// Compute the gradient using dense matrices
Matrix augmentedHessian = GaussianFactorGraph(gbn).augmentedHessian();

7
gtsam/linear/tests/testGaussianBayesTree.cpp
View File

@ -21,7 +21,6 @@
#include <boost/assign/list_of.hpp>
#include <boost/assign/std/list.hpp> // for operator +=
#include <boost/assign/std/set.hpp> // for operator +=
using namespace boost::assign;
#include <gtsam/base/debug.h>
#include <gtsam/base/numericalDerivative.h>
@ -29,6 +28,8 @@ using namespace boost::assign;
#include <gtsam/linear/GaussianBayesTree.h>
#include <gtsam/linear/GaussianConditional.h>
using namespace boost::assign;
using namespace std::placeholders;
using namespace std;
using namespace gtsam;
@ -258,11 +259,11 @@ TEST(GaussianBayesTree, ComputeSteepestDescentPointBT) {
// Compute the Hessian numerically
Matrix hessian = numericalHessian<Vector10>(
boost::bind(&computeError, bt, _1), Vector10::Zero());
std::bind(&computeError, bt, std::placeholders::_1), Vector10::Zero());
// Compute the gradient numerically
Vector gradient = numericalGradient<Vector10>(
boost::bind(&computeError, bt, _1), Vector10::Zero());
std::bind(&computeError, bt, std::placeholders::_1), Vector10::Zero());
// Compute the gradient using dense matrices
Matrix augmentedHessian = GaussianFactorGraph(bt).augmentedHessian();

3
gtsam/navigation/AttitudeFactor.cpp
View File

@ -42,7 +42,8 @@ Vector AttitudeFactor::attitudeError(const Rot3& nRb,
//***************************************************************************
void Rot3AttitudeFactor::print(const string& s,
const KeyFormatter& keyFormatter) const {
cout << s << "Rot3AttitudeFactor on " << keyFormatter(this->key()) << "\n";
cout << (s.empty() ? "" : s + " ") << "Rot3AttitudeFactor on "
<< keyFormatter(this->key()) << "\n";
nZ_.print(" measured direction in nav frame: ");
bRef_.print(" reference direction in body frame: ");
this->noiseModel_->print(" noise model: ");

8
gtsam/navigation/AttitudeFactor.h
View File

@ -114,8 +114,8 @@ public:
}
/** print */
void print(const std::string& s, const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const override;
void print(const std::string& s = "", const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const override;
/** equals */
bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
@ -188,8 +188,8 @@ public:
}
/** print */
void print(const std::string& s, const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const override;
void print(const std::string& s = "", const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const override;
/** equals */
bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;

3
gtsam/navigation/GPSFactor.cpp
View File

@ -24,7 +24,8 @@ namespace gtsam {
//***************************************************************************
void GPSFactor::print(const string& s, const KeyFormatter& keyFormatter) const {
cout << s << "GPSFactor on " << keyFormatter(key()) << "\n";
cout << (s.empty() ? "" : s + " ") << "GPSFactor on " << keyFormatter(key())
<< "\n";
cout << " GPS measurement: " << nT_ << "\n";
noiseModel_->print(" noise model: ");
}

8
gtsam/navigation/GPSFactor.h
View File

@ -71,8 +71,8 @@ public:
}
/// print
void print(const std::string& s, const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const override;
void print(const std::string& s = "", const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const override;
/// equals
bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
@ -143,8 +143,8 @@ public:
}
/// print
void print(const std::string& s, const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const override;
void print(const std::string& s = "", const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const override;
/// equals
bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;

141
gtsam/navigation/MagPoseFactor.h Normal file
View File

@ -0,0 +1,141 @@
/* ----------------------------------------------------------------------------
* 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
* -------------------------------------------------------------------------- */
#pragma once
#include <gtsam/geometry/concepts.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
namespace gtsam {
/**
* Factor to estimate rotation of a Pose2 or Pose3 given a magnetometer reading.
* This version uses the measurement model bM = scale * bRn * direction + bias,
* where bRn is the rotation of the body in the nav frame, and scale, direction,
* and bias are assumed to be known. If the factor is constructed with a
* body_P_sensor, then the magnetometer measurements and bias should be
* expressed in the sensor frame.
*/
template <class POSE>
class MagPoseFactor: public NoiseModelFactor1<POSE> {
private:
using This = MagPoseFactor<POSE>;
using Base = NoiseModelFactor1<POSE>;
using Point = typename POSE::Translation; ///< Could be a Vector2 or Vector3 depending on POSE.
using Rot = typename POSE::Rotation;
const Point measured_; ///< The measured magnetometer data in the body frame.
const Point nM_; ///< Local magnetic field (mag output units) in the nav frame.
const Point bias_; ///< The bias vector (mag output units) in the body frame.
boost::optional<POSE> body_P_sensor_; ///< The pose of the sensor in the body frame.
static const int MeasDim = Point::RowsAtCompileTime;
static const int PoseDim = traits<POSE>::dimension;
static const int RotDim = traits<Rot>::dimension;
/// Shorthand for a smart pointer to a factor.
using shared_ptr = boost::shared_ptr<MagPoseFactor<POSE>>;
/// Concept check by type.
GTSAM_CONCEPT_TESTABLE_TYPE(POSE)
GTSAM_CONCEPT_POSE_TYPE(POSE)
public:
~MagPoseFactor() override {}
/// Default constructor - only use for serialization.
MagPoseFactor() {}
/**
* Construct the factor.
* @param pose_key of the unknown pose nPb in the factor graph
* @param measured magnetometer reading, a Point2 or Point3
* @param scale by which a unit vector is scaled to yield a magnetometer reading
* @param direction of the local magnetic field, see e.g. http://www.ngdc.noaa.gov/geomag-web/#igrfwmm
* @param bias of the magnetometer, modeled as purely additive (after scaling)
* @param model of the additive Gaussian noise that is assumed
* @param body_P_sensor an optional transform of the magnetometer in the body frame
*/
MagPoseFactor(Key pose_key,
const Point& measured,
double scale,
const Point& direction,
const Point& bias,
const SharedNoiseModel& model,
const boost::optional<POSE>& body_P_sensor)
: Base(model, pose_key),
measured_(body_P_sensor ? body_P_sensor->rotation() * measured : measured),
nM_(scale * direction.normalized()),
bias_(body_P_sensor ? body_P_sensor->rotation() * bias : bias),
body_P_sensor_(body_P_sensor) {}
/// @return a deep copy of this factor.
NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<NonlinearFactor>(
NonlinearFactor::shared_ptr(new This(*this)));
}
/// Implement functions needed for Testable.
// Print out the factor.
void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
Base::print(s, keyFormatter);
gtsam::print(Vector(nM_), "local field (nM): ");
gtsam::print(Vector(measured_), "measured field (bM): ");
gtsam::print(Vector(bias_), "magnetometer bias: ");
}
/// Equals function.
bool equals(const NonlinearFactor& expected, double tol=1e-9) const override {
const This *e = dynamic_cast<const This*> (&expected);
return e != nullptr && Base::equals(*e, tol) &&
gtsam::equal_with_abs_tol(this->measured_, e->measured_, tol) &&
gtsam::equal_with_abs_tol(this->nM_, e->nM_, tol) &&
gtsam::equal_with_abs_tol(this->bias_, e->bias_, tol);
}
/// Implement functions needed to derive from Factor.
/**
* Return the factor's error h(x) - z, and the optional Jacobian. Note that
* the measurement error is expressed in the body frame.
*/
Vector evaluateError(const POSE& nPb, boost::optional<Matrix&> H = boost::none) const override {
// Predict the measured magnetic field h(x) in the *body* frame.
// If body_P_sensor was given, bias_ will have been rotated into the body frame.
Matrix H_rot = Matrix::Zero(MeasDim, RotDim);
const Point hx = nPb.rotation().unrotate(nM_, H_rot, boost::none) + bias_;
if (H) {
// Fill in the relevant part of the Jacobian (just rotation columns).
*H = Matrix::Zero(MeasDim, PoseDim);
const size_t rot_col0 = nPb.rotationInterval().first;
(*H).block(0, rot_col0, MeasDim, RotDim) = H_rot;
}
return (hx - measured_);
}
private:
/// Serialization function.
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & boost::serialization::make_nvp("NoiseModelFactor1",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(measured_);
ar & BOOST_SERIALIZATION_NVP(nM_);
ar & BOOST_SERIALIZATION_NVP(bias_);
ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
}
}; // \class MagPoseFactor
} /// namespace gtsam

355
gtsam/navigation/navigation.i Normal file
View File

@ -0,0 +1,355 @@
//*************************************************************************
// Navigation
//*************************************************************************
namespace gtsam {
namespace imuBias {
#include <gtsam/navigation/ImuBias.h>
class ConstantBias {
// Constructors
ConstantBias();
ConstantBias(Vector biasAcc, Vector biasGyro);
// Testable
void print(string s = "") const;
bool equals(const gtsam::imuBias::ConstantBias& expected, double tol) const;
// Group
static gtsam::imuBias::ConstantBias identity();
gtsam::imuBias::ConstantBias inverse() const;
gtsam::imuBias::ConstantBias compose(const gtsam::imuBias::ConstantBias& b) const;
gtsam::imuBias::ConstantBias between(const gtsam::imuBias::ConstantBias& b) const;
// Operator Overloads
gtsam::imuBias::ConstantBias operator-() const;
gtsam::imuBias::ConstantBias operator+(const gtsam::imuBias::ConstantBias& b) const;
gtsam::imuBias::ConstantBias operator-(const gtsam::imuBias::ConstantBias& b) const;
// Manifold
gtsam::imuBias::ConstantBias retract(Vector v) const;
Vector localCoordinates(const gtsam::imuBias::ConstantBias& b) const;
// Lie Group
static gtsam::imuBias::ConstantBias Expmap(Vector v);
static Vector Logmap(const gtsam::imuBias::ConstantBias& b);
// Standard Interface
Vector vector() const;
Vector accelerometer() const;
Vector gyroscope() const;
Vector correctAccelerometer(Vector measurement) const;
Vector correctGyroscope(Vector measurement) const;
};
}///\namespace imuBias
#include <gtsam/navigation/NavState.h>
class NavState {
// Constructors
NavState();
NavState(const gtsam::Rot3& R, const gtsam::Point3& t, Vector v);
NavState(const gtsam::Pose3& pose, Vector v);
// Testable
void print(string s = "") const;
bool equals(const gtsam::NavState& expected, double tol) const;
// Access
gtsam::Rot3 attitude() const;
gtsam::Point3 position() const;
Vector velocity() const;
gtsam::Pose3 pose() const;
gtsam::NavState retract(const Vector& x) const;
Vector localCoordinates(const gtsam::NavState& g) const;
};
#include <gtsam/navigation/PreintegratedRotation.h>
virtual class PreintegratedRotationParams {
PreintegratedRotationParams();
// Testable
void print(string s = "") const;
bool equals(const gtsam::PreintegratedRotationParams& expected, double tol);
void setGyroscopeCovariance(Matrix cov);
void setOmegaCoriolis(Vector omega);
void setBodyPSensor(const gtsam::Pose3& pose);
Matrix getGyroscopeCovariance() const;
boost::optional<Vector> getOmegaCoriolis() const;
boost::optional<gtsam::Pose3> getBodyPSensor() const;
};
#include <gtsam/navigation/PreintegrationParams.h>
virtual class PreintegrationParams : gtsam::PreintegratedRotationParams {
PreintegrationParams(Vector n_gravity);
static gtsam::PreintegrationParams* MakeSharedD(double g);
static gtsam::PreintegrationParams* MakeSharedU(double g);
static gtsam::PreintegrationParams* MakeSharedD(); // default g = 9.81
static gtsam::PreintegrationParams* MakeSharedU(); // default g = 9.81
// Testable
void print(string s = "") const;
bool equals(const gtsam::PreintegrationParams& expected, double tol);
void setAccelerometerCovariance(Matrix cov);
void setIntegrationCovariance(Matrix cov);
void setUse2ndOrderCoriolis(bool flag);
Matrix getAccelerometerCovariance() const;
Matrix getIntegrationCovariance() const;
bool getUse2ndOrderCoriolis() const;
};
#include <gtsam/navigation/ImuFactor.h>
class PreintegratedImuMeasurements {
// Constructors
PreintegratedImuMeasurements(const gtsam::PreintegrationParams* params);
PreintegratedImuMeasurements(const gtsam::PreintegrationParams* params,
const gtsam::imuBias::ConstantBias& bias);
// Testable
void print(string s = "") const;
bool equals(const gtsam::PreintegratedImuMeasurements& expected, double tol);
// Standard Interface
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega,
double deltaT);
void resetIntegration();
void resetIntegrationAndSetBias(const gtsam::imuBias::ConstantBias& biasHat);
Matrix preintMeasCov() const;
Vector preintegrated() const;
double deltaTij() const;
gtsam::Rot3 deltaRij() const;
Vector deltaPij() const;
Vector deltaVij() const;
gtsam::imuBias::ConstantBias biasHat() const;
Vector biasHatVector() const;
gtsam::NavState predict(const gtsam::NavState& state_i,
const gtsam::imuBias::ConstantBias& bias) const;
};
virtual class ImuFactor: gtsam::NonlinearFactor {
ImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j,
size_t bias,
const gtsam::PreintegratedImuMeasurements& preintegratedMeasurements);
// Standard Interface
gtsam::PreintegratedImuMeasurements preintegratedMeasurements() const;
Vector evaluateError(const gtsam::Pose3& pose_i, Vector vel_i,
const gtsam::Pose3& pose_j, Vector vel_j,
const gtsam::imuBias::ConstantBias& bias);
};
#include <gtsam/navigation/CombinedImuFactor.h>
virtual class PreintegrationCombinedParams : gtsam::PreintegrationParams {
PreintegrationCombinedParams(Vector n_gravity);
static gtsam::PreintegrationCombinedParams* MakeSharedD(double g);
static gtsam::PreintegrationCombinedParams* MakeSharedU(double g);
static gtsam::PreintegrationCombinedParams* MakeSharedD(); // default g = 9.81
static gtsam::PreintegrationCombinedParams* MakeSharedU(); // default g = 9.81
// Testable
void print(string s = "") const;
bool equals(const gtsam::PreintegrationCombinedParams& expected, double tol);
void setBiasAccCovariance(Matrix cov);
void setBiasOmegaCovariance(Matrix cov);
void setBiasAccOmegaInt(Matrix cov);
Matrix getBiasAccCovariance() const ;
Matrix getBiasOmegaCovariance() const ;
Matrix getBiasAccOmegaInt() const;
};
class PreintegratedCombinedMeasurements {
// Constructors
PreintegratedCombinedMeasurements(const gtsam::PreintegrationCombinedParams* params);
PreintegratedCombinedMeasurements(const gtsam::PreintegrationCombinedParams* params,
const gtsam::imuBias::ConstantBias& bias);
// Testable
void print(string s = "Preintegrated Measurements:") const;
bool equals(const gtsam::PreintegratedCombinedMeasurements& expected,
double tol);
// Standard Interface
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega,
double deltaT);
void resetIntegration();
void resetIntegrationAndSetBias(const gtsam::imuBias::ConstantBias& biasHat);
Matrix preintMeasCov() const;
double deltaTij() const;
gtsam::Rot3 deltaRij() const;
Vector deltaPij() const;
Vector deltaVij() const;
gtsam::imuBias::ConstantBias biasHat() const;
Vector biasHatVector() const;
gtsam::NavState predict(const gtsam::NavState& state_i,
const gtsam::imuBias::ConstantBias& bias) const;
};
virtual class CombinedImuFactor: gtsam::NonlinearFactor {
CombinedImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j,
size_t bias_i, size_t bias_j,
const gtsam::PreintegratedCombinedMeasurements& CombinedPreintegratedMeasurements);
// Standard Interface
gtsam::PreintegratedCombinedMeasurements preintegratedMeasurements() const;
Vector evaluateError(const gtsam::Pose3& pose_i, Vector vel_i,
const gtsam::Pose3& pose_j, Vector vel_j,
const gtsam::imuBias::ConstantBias& bias_i,
const gtsam::imuBias::ConstantBias& bias_j);
};
#include <gtsam/navigation/AHRSFactor.h>
class PreintegratedAhrsMeasurements {
// Standard Constructor
PreintegratedAhrsMeasurements(Vector bias, Matrix measuredOmegaCovariance);
PreintegratedAhrsMeasurements(const gtsam::PreintegratedAhrsMeasurements& rhs);
// Testable
void print(string s = "Preintegrated Measurements: ") const;
bool equals(const gtsam::PreintegratedAhrsMeasurements& expected, double tol);
// get Data
gtsam::Rot3 deltaRij() const;
double deltaTij() const;
Vector biasHat() const;
// Standard Interface
void integrateMeasurement(Vector measuredOmega, double deltaT);
void resetIntegration() ;
};
virtual class AHRSFactor : gtsam::NonlinearFactor {
AHRSFactor(size_t rot_i, size_t rot_j,size_t bias,
const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, Vector omegaCoriolis);
AHRSFactor(size_t rot_i, size_t rot_j, size_t bias,
const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, Vector omegaCoriolis,
const gtsam::Pose3& body_P_sensor);
// Standard Interface
gtsam::PreintegratedAhrsMeasurements preintegratedMeasurements() const;
Vector evaluateError(const gtsam::Rot3& rot_i, const gtsam::Rot3& rot_j,
Vector bias) const;
gtsam::Rot3 predict(const gtsam::Rot3& rot_i, Vector bias,
const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements,
Vector omegaCoriolis) const;
};
#include <gtsam/navigation/AttitudeFactor.h>
//virtual class AttitudeFactor : gtsam::NonlinearFactor {
// AttitudeFactor(const Unit3& nZ, const Unit3& bRef);
// AttitudeFactor();
//};
virtual class Rot3AttitudeFactor : gtsam::NonlinearFactor{
Rot3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model,
const gtsam::Unit3& bRef);
Rot3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model);
Rot3AttitudeFactor();
void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
bool equals(const gtsam::NonlinearFactor& expected, double tol) const;
gtsam::Unit3 nZ() const;
gtsam::Unit3 bRef() const;
};
virtual class Pose3AttitudeFactor : gtsam::NonlinearFactor {
Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ,
const gtsam::noiseModel::Diagonal* model,
const gtsam::Unit3& bRef);
Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ,
const gtsam::noiseModel::Diagonal* model);
Pose3AttitudeFactor();
void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
bool equals(const gtsam::NonlinearFactor& expected, double tol) const;
gtsam::Unit3 nZ() const;
gtsam::Unit3 bRef() const;
};
#include <gtsam/navigation/GPSFactor.h>
virtual class GPSFactor : gtsam::NonlinearFactor{
GPSFactor(size_t key, const gtsam::Point3& gpsIn,
const gtsam::noiseModel::Base* model);
// Testable
void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
bool equals(const gtsam::GPSFactor& expected, double tol);
// Standard Interface
gtsam::Point3 measurementIn() const;
};
virtual class GPSFactor2 : gtsam::NonlinearFactor {
GPSFactor2(size_t key, const gtsam::Point3& gpsIn,
const gtsam::noiseModel::Base* model);
// Testable
void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
bool equals(const gtsam::GPSFactor2& expected, double tol);
// Standard Interface
gtsam::Point3 measurementIn() const;
};
#include <gtsam/navigation/Scenario.h>
virtual class Scenario {
gtsam::Pose3 pose(double t) const;
Vector omega_b(double t) const;
Vector velocity_n(double t) const;
Vector acceleration_n(double t) const;
gtsam::Rot3 rotation(double t) const;
gtsam::NavState navState(double t) const;
Vector velocity_b(double t) const;
Vector acceleration_b(double t) const;
};
virtual class ConstantTwistScenario : gtsam::Scenario {
ConstantTwistScenario(Vector w, Vector v);
ConstantTwistScenario(Vector w, Vector v,
const gtsam::Pose3& nTb0);
};
virtual class AcceleratingScenario : gtsam::Scenario {
AcceleratingScenario(const gtsam::Rot3& nRb, const gtsam::Point3& p0,
Vector v0, Vector a_n,
Vector omega_b);
};
#include <gtsam/navigation/ScenarioRunner.h>
class ScenarioRunner {
ScenarioRunner(const gtsam::Scenario& scenario,
const gtsam::PreintegrationParams* p,
double imuSampleTime,
const gtsam::imuBias::ConstantBias& bias);
Vector gravity_n() const;
Vector actualAngularVelocity(double t) const;
Vector actualSpecificForce(double t) const;
Vector measuredAngularVelocity(double t) const;
Vector measuredSpecificForce(double t) const;
double imuSampleTime() const;
gtsam::PreintegratedImuMeasurements integrate(
double T, const gtsam::imuBias::ConstantBias& estimatedBias,
bool corrupted) const;
gtsam::NavState predict(
const gtsam::PreintegratedImuMeasurements& pim,
const gtsam::imuBias::ConstantBias& estimatedBias) const;
Matrix estimateCovariance(
double T, size_t N,
const gtsam::imuBias::ConstantBias& estimatedBias) const;
Matrix estimateNoiseCovariance(size_t N) const;
};
}

46
gtsam/navigation/tests/testAHRSFactor.cpp
View File

@ -25,9 +25,9 @@
#include <gtsam/base/debug.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/bind.hpp>
#include <list>
using namespace std::placeholders;
using namespace std;
using namespace gtsam;
@ -174,17 +174,17 @@ TEST(AHRSFactor, Error) {
// Expected Jacobians
Matrix H1e = numericalDerivative11<Vector3, Rot3>(
boost::bind(&callEvaluateError, factor, _1, x2, bias), x1);
std::bind(&callEvaluateError, factor, std::placeholders::_1, x2, bias), x1);
Matrix H2e = numericalDerivative11<Vector3, Rot3>(
boost::bind(&callEvaluateError, factor, x1, _1, bias), x2);
std::bind(&callEvaluateError, factor, x1, std::placeholders::_1, bias), x2);
Matrix H3e = numericalDerivative11<Vector3, Vector3>(
boost::bind(&callEvaluateError, factor, x1, x2, _1), bias);
std::bind(&callEvaluateError, factor, x1, x2, std::placeholders::_1), bias);
// Check rotation Jacobians
Matrix RH1e = numericalDerivative11<Rot3, Rot3>(
boost::bind(&evaluateRotationError, factor, _1, x2, bias), x1);
std::bind(&evaluateRotationError, factor, std::placeholders::_1, x2, bias), x1);
Matrix RH2e = numericalDerivative11<Rot3, Rot3>(
boost::bind(&evaluateRotationError, factor, x1, _1, bias), x2);
std::bind(&evaluateRotationError, factor, x1, std::placeholders::_1, bias), x2);
// Actual Jacobians
Matrix H1a, H2a, H3a;
@ -233,19 +233,19 @@ TEST(AHRSFactor, ErrorWithBiases) {
// Expected Jacobians
Matrix H1e = numericalDerivative11<Vector, Rot3>(
boost::bind(&callEvaluateError, factor, _1, x2, bias), x1);
std::bind(&callEvaluateError, factor, std::placeholders::_1, x2, bias), x1);
Matrix H2e = numericalDerivative11<Vector, Rot3>(
boost::bind(&callEvaluateError, factor, x1, _1, bias), x2);
std::bind(&callEvaluateError, factor, x1, std::placeholders::_1, bias), x2);
Matrix H3e = numericalDerivative11<Vector, Vector3>(
boost::bind(&callEvaluateError, factor, x1, x2, _1), bias);
std::bind(&callEvaluateError, factor, x1, x2, std::placeholders::_1), bias);
// Check rotation Jacobians
Matrix RH1e = numericalDerivative11<Rot3, Rot3>(
boost::bind(&evaluateRotationError, factor, _1, x2, bias), x1);
std::bind(&evaluateRotationError, factor, std::placeholders::_1, x2, bias), x1);
Matrix RH2e = numericalDerivative11<Rot3, Rot3>(
boost::bind(&evaluateRotationError, factor, x1, _1, bias), x2);
std::bind(&evaluateRotationError, factor, x1, std::placeholders::_1, bias), x2);
Matrix RH3e = numericalDerivative11<Rot3, Vector3>(
boost::bind(&evaluateRotationError, factor, x1, x2, _1), bias);
std::bind(&evaluateRotationError, factor, x1, x2, std::placeholders::_1), bias);
// Actual Jacobians
Matrix H1a, H2a, H3a;
@ -268,7 +268,7 @@ TEST( AHRSFactor, PartialDerivativeExpmap ) {
// Compute numerical derivatives
Matrix expectedDelRdelBiasOmega = numericalDerivative11<Rot3, Vector3>(
boost::bind(&evaluateRotation, measuredOmega, _1, deltaT), biasOmega);
std::bind(&evaluateRotation, measuredOmega, std::placeholders::_1, deltaT), biasOmega);
const Matrix3 Jr = Rot3::ExpmapDerivative(
(measuredOmega - biasOmega) * deltaT);
@ -293,7 +293,7 @@ TEST( AHRSFactor, PartialDerivativeLogmap ) {
// Compute numerical derivatives
Matrix expectedDelFdeltheta = numericalDerivative11<Vector3, Vector3>(
boost::bind(&evaluateLogRotation, thetahat, _1), deltatheta);
std::bind(&evaluateLogRotation, thetahat, std::placeholders::_1), deltatheta);
const Vector3 x = thetahat; // parametrization of so(3)
const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^
@ -367,7 +367,7 @@ TEST( AHRSFactor, FirstOrderPreIntegratedMeasurements ) {
// Compute numerical derivatives
Matrix expectedDelRdelBias =
numericalDerivative11<Rot3, Vector3>(
boost::bind(&evaluatePreintegratedMeasurementsRotation, _1,
std::bind(&evaluatePreintegratedMeasurementsRotation, std::placeholders::_1,
measuredOmegas, deltaTs, Vector3(M_PI / 100.0, 0.0, 0.0)), bias);
Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3);
@ -409,19 +409,19 @@ TEST( AHRSFactor, ErrorWithBiasesAndSensorBodyDisplacement ) {
// Expected Jacobians
Matrix H1e = numericalDerivative11<Vector, Rot3>(
boost::bind(&callEvaluateError, factor, _1, x2, bias), x1);
std::bind(&callEvaluateError, factor, std::placeholders::_1, x2, bias), x1);
Matrix H2e = numericalDerivative11<Vector, Rot3>(
boost::bind(&callEvaluateError, factor, x1, _1, bias), x2);
std::bind(&callEvaluateError, factor, x1, std::placeholders::_1, bias), x2);
Matrix H3e = numericalDerivative11<Vector, Vector3>(
boost::bind(&callEvaluateError, factor, x1, x2, _1), bias);
std::bind(&callEvaluateError, factor, x1, x2, std::placeholders::_1), bias);
// Check rotation Jacobians
Matrix RH1e = numericalDerivative11<Rot3, Rot3>(
boost::bind(&evaluateRotationError, factor, _1, x2, bias), x1);
std::bind(&evaluateRotationError, factor, std::placeholders::_1, x2, bias), x1);
Matrix RH2e = numericalDerivative11<Rot3, Rot3>(
boost::bind(&evaluateRotationError, factor, x1, _1, bias), x2);
std::bind(&evaluateRotationError, factor, x1, std::placeholders::_1, bias), x2);
Matrix RH3e = numericalDerivative11<Rot3, Vector3>(
boost::bind(&evaluateRotationError, factor, x1, x2, _1), bias);
std::bind(&evaluateRotationError, factor, x1, x2, std::placeholders::_1), bias);
// Actual Jacobians
Matrix H1a, H2a, H3a;
@ -458,8 +458,8 @@ TEST (AHRSFactor, predictTest) {
// AHRSFactor::PreintegratedMeasurements::predict
Matrix expectedH = numericalDerivative11<Vector3, Vector3>(
boost::bind(&AHRSFactor::PreintegratedMeasurements::predict,
&pim, _1, boost::none), bias);
std::bind(&AHRSFactor::PreintegratedMeasurements::predict,
&pim, std::placeholders::_1, boost::none), bias);
// Actual Jacobians
Matrix H;

10
gtsam/navigation/tests/testAttitudeFactor.cpp
View File

@ -21,8 +21,11 @@
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/serialization.h>
#include <gtsam/base/serializationTestHelpers.h>
#include <boost/bind/bind.hpp>
#include <CppUnitLite/TestHarness.h>
using namespace std::placeholders;
using namespace std;
using namespace gtsam;
@ -47,8 +50,9 @@ TEST( Rot3AttitudeFactor, Constructor ) {
EXPECT(assert_equal((Vector) Z_2x1,factor.evaluateError(nRb),1e-5));
// Calculate numerical derivatives
Matrix expectedH = numericalDerivative11<Vector,Rot3>(
boost::bind(&Rot3AttitudeFactor::evaluateError, &factor, _1, boost::none),
Matrix expectedH = numericalDerivative11<Vector, Rot3>(
std::bind(&Rot3AttitudeFactor::evaluateError, &factor,
std::placeholders::_1, boost::none),
nRb);
// Use the factor to calculate the derivative
@ -114,7 +118,7 @@ TEST( Pose3AttitudeFactor, Constructor ) {
// Calculate numerical derivatives
Matrix expectedH = numericalDerivative11<Vector,Pose3>(
boost::bind(&Pose3AttitudeFactor::evaluateError, &factor, _1,
std::bind(&Pose3AttitudeFactor::evaluateError, &factor, std::placeholders::_1,
boost::none), T);
// Use the factor to calculate the derivative

1
gtsam/navigation/tests/testCombinedImuFactor.cpp
View File

@ -29,7 +29,6 @@
#include <CppUnitLite/TestHarness.h>
#include <boost/bind.hpp>
#include <list>
#include "imuFactorTesting.h"

7
gtsam/navigation/tests/testGPSFactor.cpp
View File

@ -20,11 +20,14 @@
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <boost/bind/bind.hpp>
#include <CppUnitLite/TestHarness.h>
#include <GeographicLib/Config.h>
#include <GeographicLib/LocalCartesian.hpp>
using namespace std::placeholders;
using namespace std;
using namespace gtsam;
using namespace GeographicLib;
@ -69,7 +72,7 @@ TEST( GPSFactor, Constructor ) {
// Calculate numerical derivatives
Matrix expectedH = numericalDerivative11<Vector,Pose3>(
boost::bind(&GPSFactor::evaluateError, &factor, _1, boost::none), T);
std::bind(&GPSFactor::evaluateError, &factor, _1, boost::none), T);
// Use the factor to calculate the derivative
Matrix actualH;
@ -98,7 +101,7 @@ TEST( GPSFactor2, Constructor ) {
// Calculate numerical derivatives
Matrix expectedH = numericalDerivative11<Vector,NavState>(
boost::bind(&GPSFactor2::evaluateError, &factor, _1, boost::none), T);
std::bind(&GPSFactor2::evaluateError, &factor, _1, boost::none), T);
// Use the factor to calculate the derivative
Matrix actualH;

12
gtsam/navigation/tests/testImuBias.cpp
View File

@ -19,8 +19,8 @@
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/bind.hpp>
using namespace std::placeholders;
using namespace std;
using namespace gtsam;
@ -127,8 +127,9 @@ TEST(ImuBias, operatorSubB) {
TEST(ImuBias, Correct1) {
Matrix aH1, aH2;
const Vector3 measurement(1, 2, 3);
boost::function<Vector3(const Bias&, const Vector3&)> f = boost::bind(
&Bias::correctAccelerometer, _1, _2, boost::none, boost::none);
std::function<Vector3(const Bias&, const Vector3&)> f =
std::bind(&Bias::correctAccelerometer, std::placeholders::_1,
std::placeholders::_2, boost::none, boost::none);
bias1.correctAccelerometer(measurement, aH1, aH2);
EXPECT(assert_equal(numericalDerivative21(f, bias1, measurement), aH1));
EXPECT(assert_equal(numericalDerivative22(f, bias1, measurement), aH2));
@ -138,8 +139,9 @@ TEST(ImuBias, Correct1) {
TEST(ImuBias, Correct2) {
Matrix aH1, aH2;
const Vector3 measurement(1, 2, 3);
boost::function<Vector3(const Bias&, const Vector3&)> f =
boost::bind(&Bias::correctGyroscope, _1, _2, boost::none, boost::none);
std::function<Vector3(const Bias&, const Vector3&)> f =
std::bind(&Bias::correctGyroscope, std::placeholders::_1,
std::placeholders::_2, boost::none, boost::none);
bias1.correctGyroscope(measurement, aH1, aH2);
EXPECT(assert_equal(numericalDerivative21(f, bias1, measurement), aH1));
EXPECT(assert_equal(numericalDerivative22(f, bias1, measurement), aH2));

36
gtsam/navigation/tests/testImuFactor.cpp
View File

@ -30,7 +30,7 @@
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/bind.hpp>
#include <boost/bind/bind.hpp>
#include <list>
#include "imuFactorTesting.h"
@ -146,8 +146,9 @@ TEST(ImuFactor, PreintegratedMeasurements) {
Matrix9 aH1, aH2;
Matrix96 aH3;
actual.computeError(x1, x2, bias, aH1, aH2, aH3);
boost::function<Vector9(const NavState&, const NavState&, const Bias&)> f =
boost::bind(&PreintegrationBase::computeError, actual, _1, _2, _3,
std::function<Vector9(const NavState&, const NavState&, const Bias&)> f =
std::bind(&PreintegrationBase::computeError, actual,
std::placeholders::_1, std::placeholders::_2, std::placeholders::_3,
boost::none, boost::none, boost::none);
EXPECT(assert_equal(numericalDerivative31(f, x1, x2, bias), aH1, 1e-9));
EXPECT(assert_equal(numericalDerivative32(f, x1, x2, bias), aH2, 1e-9));
@ -203,20 +204,20 @@ TEST(ImuFactor, PreintegrationBaseMethods) {
Matrix96 actualH;
pim.biasCorrectedDelta(kZeroBias, actualH);
Matrix expectedH = numericalDerivative11<Vector9, Bias>(
boost::bind(&PreintegrationBase::biasCorrectedDelta, pim, _1,
boost::none), kZeroBias);
std::bind(&PreintegrationBase::biasCorrectedDelta, pim,
std::placeholders::_1, boost::none), kZeroBias);
EXPECT(assert_equal(expectedH, actualH));
Matrix9 aH1;
Matrix96 aH2;
NavState predictedState = pim.predict(state1, kZeroBias, aH1, aH2);
Matrix eH1 = numericalDerivative11<NavState, NavState>(
boost::bind(&PreintegrationBase::predict, pim, _1, kZeroBias, boost::none,
boost::none), state1);
std::bind(&PreintegrationBase::predict, pim, std::placeholders::_1,
kZeroBias, boost::none, boost::none), state1);
EXPECT(assert_equal(eH1, aH1));
Matrix eH2 = numericalDerivative11<NavState, Bias>(
boost::bind(&PreintegrationBase::predict, pim, state1, _1, boost::none,
boost::none), kZeroBias);
std::bind(&PreintegrationBase::predict, pim, state1,
std::placeholders::_1, boost::none, boost::none), kZeroBias);
EXPECT(assert_equal(eH2, aH2));
}
@ -277,12 +278,12 @@ TEST(ImuFactor, ErrorAndJacobians) {
// Make sure rotation part is correct when error is interpreted as axis-angle
// Jacobians are around zero, so the rotation part is the same as:
Matrix H1Rot3 = numericalDerivative11<Rot3, Pose3>(
boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, kZeroBias),
std::bind(&evaluateRotationError, factor, std::placeholders::_1, v1, x2, v2, kZeroBias),
x1);
EXPECT(assert_equal(H1Rot3, H1a.topRows(3)));
Matrix H3Rot3 = numericalDerivative11<Rot3, Pose3>(
boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, kZeroBias),
std::bind(&evaluateRotationError, factor, x1, v1, std::placeholders::_1, v2, kZeroBias),
x2);
EXPECT(assert_equal(H3Rot3, H3a.topRows(3)));
@ -332,8 +333,8 @@ TEST(ImuFactor, ErrorAndJacobianWithBiases) {
Matrix96 actualH;
pim.biasCorrectedDelta(bias, actualH);
Matrix expectedH = numericalDerivative11<Vector9, Bias>(
boost::bind(&PreintegrationBase::biasCorrectedDelta, pim, _1,
boost::none), bias);
std::bind(&PreintegrationBase::biasCorrectedDelta, pim,
std::placeholders::_1, boost::none), bias);
EXPECT(assert_equal(expectedH, actualH));
// Create factor
@ -521,7 +522,8 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) {
pim.correctMeasurementsBySensorPose(measuredAcc, measuredOmega,
boost::none, D_correctedAcc_measuredOmega, boost::none);
Matrix3 expectedD = numericalDerivative11<Vector3, Vector3>(
boost::bind(correctedAcc, pim, measuredAcc, _1), measuredOmega, 1e-6);
std::bind(correctedAcc, pim, measuredAcc, std::placeholders::_1),
measuredOmega, 1e-6);
EXPECT(assert_equal(expectedD, D_correctedAcc_measuredOmega, 1e-5));
double dt = 0.1;
@ -532,13 +534,15 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) {
// pim.updatedDeltaXij(measuredAcc, measuredOmega, dt, boost::none, G1, G2);
//
// Matrix93 expectedG1 = numericalDerivative21<NavState, Vector3, Vector3>(
// boost::bind(&PreintegratedImuMeasurements::updatedDeltaXij, pim, _1, _2,
// std::bind(&PreintegratedImuMeasurements::updatedDeltaXij, pim,
// std::placeholders::_1, std::placeholders::_2,
// dt, boost::none, boost::none, boost::none), measuredAcc,
// measuredOmega, 1e-6);
// EXPECT(assert_equal(expectedG1, G1, 1e-5));
//
// Matrix93 expectedG2 = numericalDerivative22<NavState, Vector3, Vector3>(
// boost::bind(&PreintegratedImuMeasurements::updatedDeltaXij, pim, _1, _2,
// std::bind(&PreintegratedImuMeasurements::updatedDeltaXij, pim,
// std::placeholders::_1, std::placeholders::_2,
// dt, boost::none, boost::none, boost::none), measuredAcc,
// measuredOmega, 1e-6);
// EXPECT(assert_equal(expectedG2, G2, 1e-5));

19
gtsam/navigation/tests/testMagFactor.cpp
View File

@ -20,10 +20,13 @@
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <boost/bind/bind.hpp>
#include <CppUnitLite/TestHarness.h>
#include <GeographicLib/LocalCartesian.hpp>
using namespace std::placeholders;
using namespace std;
using namespace gtsam;
using namespace GeographicLib;
@ -61,7 +64,7 @@ TEST( MagFactor, unrotate ) {
Point3 expected(22735.5, 314.502, 44202.5);
EXPECT( assert_equal(expected, MagFactor::unrotate(theta,nM,H),1e-1));
EXPECT( assert_equal(numericalDerivative11<Point3,Rot2> //
(boost::bind(&MagFactor::unrotate, _1, nM, none), theta), H, 1e-6));
(std::bind(&MagFactor::unrotate, _1, nM, none), theta), H, 1e-6));
}
// *************************************************************************
@ -73,35 +76,35 @@ TEST( MagFactor, Factors ) {
MagFactor f(1, measured, s, dir, bias, model);
EXPECT( assert_equal(Z_3x1,f.evaluateError(theta,H1),1e-5));
EXPECT( assert_equal((Matrix)numericalDerivative11<Vector,Rot2> //
(boost::bind(&MagFactor::evaluateError, &f, _1, none), theta), H1, 1e-7));
(std::bind(&MagFactor::evaluateError, &f, _1, none), theta), H1, 1e-7));
// MagFactor1
MagFactor1 f1(1, measured, s, dir, bias, model);
EXPECT( assert_equal(Z_3x1,f1.evaluateError(nRb,H1),1e-5));
EXPECT( assert_equal(numericalDerivative11<Vector,Rot3> //
(boost::bind(&MagFactor1::evaluateError, &f1, _1, none), nRb), H1, 1e-7));
(std::bind(&MagFactor1::evaluateError, &f1, _1, none), nRb), H1, 1e-7));
// MagFactor2
MagFactor2 f2(1, 2, measured, nRb, model);
EXPECT( assert_equal(Z_3x1,f2.evaluateError(scaled,bias,H1,H2),1e-5));
EXPECT( assert_equal(numericalDerivative11<Vector,Point3> //
(boost::bind(&MagFactor2::evaluateError, &f2, _1, bias, none, none), scaled),//
(std::bind(&MagFactor2::evaluateError, &f2, _1, bias, none, none), scaled),//
H1, 1e-7));
EXPECT( assert_equal(numericalDerivative11<Vector,Point3> //
(boost::bind(&MagFactor2::evaluateError, &f2, scaled, _1, none, none), bias),//
(std::bind(&MagFactor2::evaluateError, &f2, scaled, _1, none, none), bias),//
H2, 1e-7));
// MagFactor2
MagFactor3 f3(1, 2, 3, measured, nRb, model);
EXPECT(assert_equal(Z_3x1,f3.evaluateError(s,dir,bias,H1,H2,H3),1e-5));
EXPECT(assert_equal((Matrix)numericalDerivative11<Vector,double> //
(boost::bind(&MagFactor3::evaluateError, &f3, _1, dir, bias, none, none, none), s),//
(std::bind(&MagFactor3::evaluateError, &f3, _1, dir, bias, none, none, none), s),//
H1, 1e-7));
EXPECT(assert_equal(numericalDerivative11<Vector,Unit3> //
(boost::bind(&MagFactor3::evaluateError, &f3, s, _1, bias, none, none, none), dir),//
(std::bind(&MagFactor3::evaluateError, &f3, s, _1, bias, none, none, none), dir),//
H2, 1e-7));
EXPECT(assert_equal(numericalDerivative11<Vector,Point3> //
(boost::bind(&MagFactor3::evaluateError, &f3, s, dir, _1, none, none, none), bias),//
(std::bind(&MagFactor3::evaluateError, &f3, s, dir, _1, none, none, none), bias),//
H3, 1e-7));
}

133
gtsam/navigation/tests/testMagPoseFactor.cpp Normal file
View File

@ -0,0 +1,133 @@
/* ----------------------------------------------------------------------------
* 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
* -------------------------------------------------------------------------- */
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/navigation/MagPoseFactor.h>
using namespace std::placeholders;
using namespace gtsam;
// *****************************************************************************
// Magnetic field in the nav frame (NED), with units of nT.
Point3 nM(22653.29982, -1956.83010, 44202.47862);
// Assumed scale factor (scales a unit vector to magnetometer units of nT).
double scale = 255.0 / 50000.0;
// Ground truth Pose2/Pose3 in the nav frame.
Pose3 n_P3_b = Pose3(Rot3::Yaw(-0.1), Point3(-3, 12, 5));
Pose2 n_P2_b = Pose2(Rot2(-0.1), Point2(-3, 12));
Rot3 n_R3_b = n_P3_b.rotation();
Rot2 n_R2_b = n_P2_b.rotation();
// Sensor bias (nT).
Point3 bias3(10, -10, 50);
Point2 bias2 = bias3.head(2);
Point3 dir3(nM.normalized());
Point2 dir2(nM.head(2).normalized());
// Compute the measured field in the sensor frame.
Point3 measured3 = n_R3_b.inverse() * (scale * dir3) + bias3;
// The 2D magnetometer will measure the "NE" field components.
Point2 measured2 = n_R2_b.inverse() * (scale * dir2) + bias2;
SharedNoiseModel model2 = noiseModel::Isotropic::Sigma(2, 0.25);
SharedNoiseModel model3 = noiseModel::Isotropic::Sigma(3, 0.25);
// Make up a rotation and offset of the sensor in the body frame.
Pose2 body_P2_sensor(Rot2(-0.30), Point2(1.0, -2.0));
Pose3 body_P3_sensor(Rot3::RzRyRx(Vector3(1.5, 0.9, -1.15)), Point3(-0.1, 0.2, 0.3));
// *****************************************************************************
// *****************************************************************************
TEST(MagPoseFactor, Constructors) {
MagPoseFactor<Pose2> f2a(Symbol('X', 0), measured2, scale, dir2, bias2, model2, boost::none);
MagPoseFactor<Pose3> f3a(Symbol('X', 0), measured3, scale, dir3, bias3, model3, boost::none);
// Try constructing with a body_P_sensor set.
MagPoseFactor<Pose2> f2b = MagPoseFactor<Pose2>(
Symbol('X', 0), measured2, scale, dir2, bias2, model2, body_P2_sensor);
MagPoseFactor<Pose3> f3b = MagPoseFactor<Pose3>(
Symbol('X', 0), measured3, scale, dir3, bias3, model3, body_P3_sensor);
f2b.print();
f3b.print();
}
// *****************************************************************************
TEST(MagPoseFactor, JacobianPose2) {
Matrix H2;
// Error should be zero at the groundtruth pose.
MagPoseFactor<Pose2> f(Symbol('X', 0), measured2, scale, dir2, bias2, model2, boost::none);
CHECK(gtsam::assert_equal(Z_2x1, f.evaluateError(n_P2_b, H2), 1e-5));
CHECK(gtsam::assert_equal(gtsam::numericalDerivative11<Vector, Pose2> //
(std::bind(&MagPoseFactor<Pose2>::evaluateError, &f,
std::placeholders::_1, boost::none),
n_P2_b),
H2, 1e-7));
}
// *****************************************************************************
TEST(MagPoseFactor, JacobianPose3) {
Matrix H3;
// Error should be zero at the groundtruth pose.
MagPoseFactor<Pose3> f(Symbol('X', 0), measured3, scale, dir3, bias3, model3, boost::none);
CHECK(gtsam::assert_equal(Z_3x1, f.evaluateError(n_P3_b, H3), 1e-5));
CHECK(gtsam::assert_equal(gtsam::numericalDerivative11<Vector, Pose3> //
(std::bind(&MagPoseFactor<Pose3>::evaluateError, &f,
std::placeholders::_1, boost::none),
n_P3_b),
H3, 1e-7));
}
// *****************************************************************************
TEST(MagPoseFactor, body_P_sensor2) {
Matrix H2;
// Because body_P_sensor is specified, we need to rotate the raw measurement
// from the body frame into the sensor frame "s".
const Rot2 nRs = n_R2_b * body_P2_sensor.rotation();
const Point2 sM = nRs.inverse() * (scale * dir2) + bias2;
MagPoseFactor<Pose2> f = MagPoseFactor<Pose2>(Symbol('X', 0), sM, scale, dir2, bias2, model2, body_P2_sensor);
CHECK(gtsam::assert_equal(Z_2x1, f.evaluateError(n_P2_b, H2), 1e-5));
CHECK(gtsam::assert_equal(gtsam::numericalDerivative11<Vector, Pose2> //
(std::bind(&MagPoseFactor<Pose2>::evaluateError, &f, std::placeholders::_1, boost::none), n_P2_b), H2, 1e-7));
}
// *****************************************************************************
TEST(MagPoseFactor, body_P_sensor3) {
Matrix H3;
// Because body_P_sensor is specified, we need to rotate the raw measurement
// from the body frame into the sensor frame "s".
const Rot3 nRs = n_R3_b * body_P3_sensor.rotation();
const Point3 sM = nRs.inverse() * (scale * dir3) + bias3;
MagPoseFactor<Pose3> f = MagPoseFactor<Pose3>(Symbol('X', 0), sM, scale, dir3, bias3, model3, body_P3_sensor);
CHECK(gtsam::assert_equal(Z_3x1, f.evaluateError(n_P3_b, H3), 1e-5));
CHECK(gtsam::assert_equal(gtsam::numericalDerivative11<Vector, Pose3> //
(std::bind(&MagPoseFactor<Pose3>::evaluateError, &f, std::placeholders::_1, boost::none), n_P3_b), H3, 1e-7));
}
// *****************************************************************************
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
// *****************************************************************************

19
gtsam/navigation/tests/testManifoldPreintegration.cpp
View File

@ -22,10 +22,11 @@
#include <gtsam/nonlinear/expressionTesting.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/bind.hpp>
#include "imuFactorTesting.h"
using namespace std::placeholders;
namespace testing {
// Create default parameters with Z-down and above noise parameters
static boost::shared_ptr<PreintegrationParams> Params() {
@ -41,21 +42,21 @@ static boost::shared_ptr<PreintegrationParams> Params() {
TEST(ManifoldPreintegration, BiasCorrectionJacobians) {
testing::SomeMeasurements measurements;
boost::function<Rot3(const Vector3&, const Vector3&)> deltaRij =
std::function<Rot3(const Vector3&, const Vector3&)> deltaRij =
[=](const Vector3& a, const Vector3& w) {
ManifoldPreintegration pim(testing::Params(), Bias(a, w));
testing::integrateMeasurements(measurements, &pim);
return pim.deltaRij();
};
boost::function<Point3(const Vector3&, const Vector3&)> deltaPij =
std::function<Point3(const Vector3&, const Vector3&)> deltaPij =
[=](const Vector3& a, const Vector3& w) {
ManifoldPreintegration pim(testing::Params(), Bias(a, w));
testing::integrateMeasurements(measurements, &pim);
return pim.deltaPij();
};
boost::function<Vector3(const Vector3&, const Vector3&)> deltaVij =
std::function<Vector3(const Vector3&, const Vector3&)> deltaVij =
[=](const Vector3& a, const Vector3& w) {
ManifoldPreintegration pim(testing::Params(), Bias(a, w));
testing::integrateMeasurements(measurements, &pim);
@ -96,10 +97,12 @@ TEST(ManifoldPreintegration, computeError) {
Matrix9 aH1, aH2;
Matrix96 aH3;
pim.computeError(x1, x2, bias, aH1, aH2, aH3);
boost::function<Vector9(const NavState&, const NavState&,
const imuBias::ConstantBias&)> f =
boost::bind(&ManifoldPreintegration::computeError, pim, _1, _2, _3,
boost::none, boost::none, boost::none);
std::function<Vector9(const NavState&, const NavState&,
const imuBias::ConstantBias&)>
f = std::bind(&ManifoldPreintegration::computeError, pim,
std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, boost::none, boost::none,
boost::none);
// NOTE(frank): tolerance of 1e-3 on H1 because approximate away from 0
EXPECT(assert_equal(numericalDerivative31(f, x1, x2, bias), aH1, 1e-9));
EXPECT(assert_equal(numericalDerivative32(f, x1, x2, bias), aH2, 1e-9));

49
gtsam/navigation/tests/testNavState.cpp
View File

@ -19,8 +19,11 @@
#include <gtsam/navigation/NavState.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/numericalDerivative.h>
#include <boost/bind/bind.hpp>
#include <CppUnitLite/TestHarness.h>
using namespace std::placeholders;
using namespace std;
using namespace gtsam;
@ -36,9 +39,9 @@ static const Vector9 kZeroXi = Vector9::Zero();
/* ************************************************************************* */
TEST(NavState, Constructor) {
boost::function<NavState(const Rot3&, const Point3&, const Vector3&)> create =
boost::bind(&NavState::Create, _1, _2, _3, boost::none, boost::none,
boost::none);
std::function<NavState(const Rot3&, const Point3&, const Vector3&)> create =
std::bind(&NavState::Create, std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, boost::none, boost::none, boost::none);
Matrix aH1, aH2, aH3;
EXPECT(
assert_equal(kState1,
@ -56,9 +59,9 @@ TEST(NavState, Constructor) {
/* ************************************************************************* */
TEST(NavState, Constructor2) {
boost::function<NavState(const Pose3&, const Vector3&)> construct =
boost::bind(&NavState::FromPoseVelocity, _1, _2, boost::none,
boost::none);
std::function<NavState(const Pose3&, const Vector3&)> construct =
std::bind(&NavState::FromPoseVelocity, std::placeholders::_1,
std::placeholders::_2, boost::none, boost::none);
Matrix aH1, aH2;
EXPECT(
assert_equal(kState1,
@ -73,7 +76,7 @@ TEST( NavState, Attitude) {
Rot3 actual = kState1.attitude(aH);
EXPECT(assert_equal(actual, kAttitude));
eH = numericalDerivative11<Rot3, NavState>(
boost::bind(&NavState::attitude, _1, boost::none), kState1);
std::bind(&NavState::attitude, std::placeholders::_1, boost::none), kState1);
EXPECT(assert_equal((Matrix )eH, aH));
}
@ -83,7 +86,8 @@ TEST( NavState, Position) {
Point3 actual = kState1.position(aH);
EXPECT(assert_equal(actual, kPosition));
eH = numericalDerivative11<Point3, NavState>(
boost::bind(&NavState::position, _1, boost::none), kState1);
std::bind(&NavState::position, std::placeholders::_1, boost::none),
kState1);
EXPECT(assert_equal((Matrix )eH, aH));
}
@ -93,7 +97,8 @@ TEST( NavState, Velocity) {
Velocity3 actual = kState1.velocity(aH);
EXPECT(assert_equal(actual, kVelocity));
eH = numericalDerivative11<Velocity3, NavState>(
boost::bind(&NavState::velocity, _1, boost::none), kState1);
std::bind(&NavState::velocity, std::placeholders::_1, boost::none),
kState1);
EXPECT(assert_equal((Matrix )eH, aH));
}
@ -103,7 +108,8 @@ TEST( NavState, BodyVelocity) {
Velocity3 actual = kState1.bodyVelocity(aH);
EXPECT(assert_equal<Velocity3>(actual, kAttitude.unrotate(kVelocity)));
eH = numericalDerivative11<Velocity3, NavState>(
boost::bind(&NavState::bodyVelocity, _1, boost::none), kState1);
std::bind(&NavState::bodyVelocity, std::placeholders::_1, boost::none),
kState1);
EXPECT(assert_equal((Matrix )eH, aH));
}
@ -134,8 +140,9 @@ TEST( NavState, Manifold ) {
// Check retract derivatives
Matrix9 aH1, aH2;
kState1.retract(xi, aH1, aH2);
boost::function<NavState(const NavState&, const Vector9&)> retract =
boost::bind(&NavState::retract, _1, _2, boost::none, boost::none);
std::function<NavState(const NavState&, const Vector9&)> retract =
std::bind(&NavState::retract, std::placeholders::_1,
std::placeholders::_2, boost::none, boost::none);
EXPECT(assert_equal(numericalDerivative21(retract, kState1, xi), aH1));
EXPECT(assert_equal(numericalDerivative22(retract, kState1, xi), aH2));
@ -146,9 +153,9 @@ TEST( NavState, Manifold ) {
EXPECT(assert_equal(numericalDerivative22(retract, state2, xi2), aH2));
// Check localCoordinates derivatives
boost::function<Vector9(const NavState&, const NavState&)> local =
boost::bind(&NavState::localCoordinates, _1, _2, boost::none,
boost::none);
std::function<Vector9(const NavState&, const NavState&)> local =
std::bind(&NavState::localCoordinates, std::placeholders::_1,
std::placeholders::_2, boost::none, boost::none);
// from state1 to state2
kState1.localCoordinates(state2, aH1, aH2);
EXPECT(assert_equal(numericalDerivative21(local, kState1, state2), aH1));
@ -165,8 +172,9 @@ TEST( NavState, Manifold ) {
/* ************************************************************************* */
static const double dt = 2.0;
boost::function<Vector9(const NavState&, const bool&)> coriolis = boost::bind(
&NavState::coriolis, _1, dt, kOmegaCoriolis, _2, boost::none);
std::function<Vector9(const NavState&, const bool&)> coriolis =
std::bind(&NavState::coriolis, std::placeholders::_1, dt, kOmegaCoriolis,
std::placeholders::_2, boost::none);
TEST(NavState, Coriolis) {
Matrix9 aH;
@ -241,9 +249,10 @@ TEST(NavState, CorrectPIM) {
xi << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4, -0.1, -0.2, -0.3;
double dt = 0.5;
Matrix9 aH1, aH2;
boost::function<Vector9(const NavState&, const Vector9&)> correctPIM =
boost::bind(&NavState::correctPIM, _1, _2, dt, kGravity, kOmegaCoriolis,
false, boost::none, boost::none);
std::function<Vector9(const NavState&, const Vector9&)> correctPIM =
std::bind(&NavState::correctPIM, std::placeholders::_1,
std::placeholders::_2, dt, kGravity, kOmegaCoriolis, false,
boost::none, boost::none);
kState1.correctPIM(xi, dt, kGravity, kOmegaCoriolis, false, aH1, aH2);
EXPECT(assert_equal(numericalDerivative21(correctPIM, kState1, xi), aH1));
EXPECT(assert_equal(numericalDerivative22(correctPIM, kState1, xi), aH2));

4
gtsam/navigation/tests/testScenario.cpp
View File

@ -19,9 +19,9 @@
#include <gtsam/navigation/Scenario.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/bind.hpp>
#include <cmath>
using namespace std::placeholders;
using namespace std;
using namespace gtsam;
@ -147,7 +147,7 @@ TEST(Scenario, Accelerating) {
{
// Check acceleration in nav
Matrix expected = numericalDerivative11<Vector3, double>(
boost::bind(&Scenario::velocity_n, scenario, _1), T);
std::bind(&Scenario::velocity_n, scenario, std::placeholders::_1), T);
EXPECT(assert_equal(Vector3(expected), scenario.acceleration_n(T), 1e-9));
}

17
gtsam/navigation/tests/testTangentPreintegration.cpp
View File

@ -22,10 +22,11 @@
#include <gtsam/nonlinear/expressionTesting.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/bind.hpp>
#include "imuFactorTesting.h"
using namespace std::placeholders;
static const double kDt = 0.1;
Vector9 f(const Vector9& zeta, const Vector3& a, const Vector3& w) {
@ -76,7 +77,7 @@ TEST(TangentPreintegration, UpdateEstimate2) {
TEST(ImuFactor, BiasCorrectionJacobians) {
testing::SomeMeasurements measurements;
boost::function<Vector9(const Vector3&, const Vector3&)> preintegrated =
std::function<Vector9(const Vector3&, const Vector3&)> preintegrated =
[=](const Vector3& a, const Vector3& w) {
TangentPreintegration pim(testing::Params(), Bias(a, w));
testing::integrateMeasurements(measurements, &pim);
@ -103,10 +104,12 @@ TEST(TangentPreintegration, computeError) {
Matrix9 aH1, aH2;
Matrix96 aH3;
pim.computeError(x1, x2, bias, aH1, aH2, aH3);
boost::function<Vector9(const NavState&, const NavState&,
const imuBias::ConstantBias&)> f =
boost::bind(&TangentPreintegration::computeError, pim, _1, _2, _3,
boost::none, boost::none, boost::none);
std::function<Vector9(const NavState&, const NavState&,
const imuBias::ConstantBias&)>
f = std::bind(&TangentPreintegration::computeError, pim,
std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3, boost::none, boost::none,
boost::none);
// NOTE(frank): tolerance of 1e-3 on H1 because approximate away from 0
EXPECT(assert_equal(numericalDerivative31(f, x1, x2, bias), aH1, 1e-9));
EXPECT(assert_equal(numericalDerivative32(f, x1, x2, bias), aH2, 1e-9));
@ -119,7 +122,7 @@ TEST(TangentPreintegration, Compose) {
TangentPreintegration pim(testing::Params());
testing::integrateMeasurements(measurements, &pim);
boost::function<Vector9(const Vector9&, const Vector9&)> f =
std::function<Vector9(const Vector9&, const Vector9&)> f =
[pim](const Vector9& zeta01, const Vector9& zeta12) {
return TangentPreintegration::Compose(zeta01, zeta12, pim.deltaTij());
};

76
gtsam/nonlinear/CustomFactor.cpp Normal file
View File

@ -0,0 +1,76 @@
/* ----------------------------------------------------------------------------
* 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 CustomFactor.cpp
* @brief Class to enable arbitrary factors with runtime swappable error function.
* @author Fan Jiang
*/
#include <gtsam/nonlinear/CustomFactor.h>
namespace gtsam {
/*
* Calculates the unwhitened error by invoking the callback functor (i.e. from Python).
*/
Vector CustomFactor::unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H) const {
if(this->active(x)) {
if(H) {
/*
* In this case, we pass the raw pointer to the `std::vector<Matrix>` object directly to pybind.
* As the type `std::vector<Matrix>` has been marked as opaque in `preamble.h`, any changes in
* Python will be immediately reflected on the C++ side.
*
* Example:
* ```
* def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]):
* <calculated error>
* if not H is None:
* <calculate the Jacobian>
* H[0] = J1
* H[1] = J2
* ...
* return error
* ```
*/
return this->error_function_(*this, x, H.get_ptr());
} else {
/*
* In this case, we pass the a `nullptr` to pybind, and it will translate to `None` in Python.
* Users can check for `None` in their callback to determine if the Jacobian is requested.
*/
return this->error_function_(*this, x, nullptr);
}
} else {
return Vector::Zero(this->dim());
}
}
void CustomFactor::print(const std::string &s, const KeyFormatter &keyFormatter) const {
std::cout << s << "CustomFactor on ";
auto keys_ = this->keys();
bool f = false;
for (const Key &k: keys_) {
if (f)
std::cout << ", ";
std::cout << keyFormatter(k);
f = true;
}
std::cout << "\n";
if (this->noiseModel_)
this->noiseModel_->print(" noise model: ");
else
std::cout << "no noise model" << std::endl;
}
}

104
gtsam/nonlinear/CustomFactor.h Normal file
View File

@ -0,0 +1,104 @@
/* ----------------------------------------------------------------------------
* 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 CustomFactor.h
* @brief Class to enable arbitrary factors with runtime swappable error function.
* @author Fan Jiang
*/
#pragma once
#include <gtsam/nonlinear/NonlinearFactor.h>
using namespace gtsam;
namespace gtsam {
using JacobianVector = std::vector<Matrix>;
class CustomFactor;
/*
* NOTE
* ==========
* pybind11 will invoke a copy if this is `JacobianVector &`, and modifications in Python will not be reflected.
*
* This is safe because this is passing a const pointer, and pybind11 will maintain the `std::vector` memory layout.
* Thus the pointer will never be invalidated.
*/
using CustomErrorFunction = std::function<Vector(const CustomFactor &, const Values &, const JacobianVector *)>;
/**
* @brief Custom factor that takes a std::function as the error
* @addtogroup nonlinear
* \nosubgrouping
*
* This factor is mainly for creating a custom factor in Python.
*/
class CustomFactor: public NoiseModelFactor {
protected:
CustomErrorFunction error_function_;
protected:
using Base = NoiseModelFactor;
using This = CustomFactor;
public:
/**
* Default Constructor for I/O
*/
CustomFactor() = default;
/**
* Constructor
* @param noiseModel shared pointer to noise model
* @param keys keys of the variables
* @param errorFunction the error functional
*/
CustomFactor(const SharedNoiseModel &noiseModel, const KeyVector &keys, const CustomErrorFunction &errorFunction) :
Base(noiseModel, keys) {
this->error_function_ = errorFunction;
}
~CustomFactor() override = default;
/**
* Calls the errorFunction closure, which is a std::function object
* One can check if a derivative is needed in the errorFunction by checking the length of Jacobian array
*/
Vector unwhitenedError(const Values &x, boost::optional<std::vector<Matrix> &> H = boost::none) const override;
/** print */
void print(const std::string &s,
const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override;
/**
* Mark not sendable
*/
bool sendable() const override {
return false;
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
ar & boost::serialization::make_nvp("CustomFactor",
boost::serialization::base_object<Base>(*this));
}
};
};

22
gtsam/nonlinear/Expression-inl.h
View File

@ -21,6 +21,7 @@
#include <gtsam/nonlinear/internal/ExpressionNode.h>
#include <boost/bind/bind.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/range/adaptor/map.hpp>
#include <boost/range/algorithm.hpp>
@ -82,7 +83,8 @@ template<typename A>
Expression<T>::Expression(const Expression<A>& expression,
T (A::*method)(typename MakeOptionalJacobian<T, A>::type) const) :
root_(
new internal::UnaryExpression<T, A>(boost::bind(method, _1, _2),
new internal::UnaryExpression<T, A>(std::bind(method,
std::placeholders::_1, std::placeholders::_2),
expression)) {
}
@ -95,7 +97,10 @@ Expression<T>::Expression(const Expression<A1>& expression1,
const Expression<A2>& expression2) :
root_(
new internal::BinaryExpression<T, A1, A2>(
boost::bind(method, _1, _2, _3, _4), expression1, expression2)) {
std::bind(method, std::placeholders::_1,
std::placeholders::_2, std::placeholders::_3,
std::placeholders::_4),
expression1, expression2)) {
}
/// Construct a binary method expression
@ -109,8 +114,11 @@ Expression<T>::Expression(const Expression<A1>& expression1,
const Expression<A2>& expression2, const Expression<A3>& expression3) :
root_(
new internal::TernaryExpression<T, A1, A2, A3>(
boost::bind(method, _1, _2, _3, _4, _5, _6), expression1,
expression2, expression3)) {
std::bind(method, std::placeholders::_1,
std::placeholders::_2, std::placeholders::_3,
std::placeholders::_4, std::placeholders::_5,
std::placeholders::_6),
expression1, expression2, expression3)) {
}
template<typename T>
@ -247,8 +255,10 @@ template<typename T>
Expression<T> operator*(const Expression<T>& expression1,
const Expression<T>& expression2) {
return Expression<T>(
boost::bind(internal::apply_compose<T>(), _1, _2, _3, _4), expression1,
expression2);
std::bind(internal::apply_compose<T>(), std::placeholders::_1,
std::placeholders::_2, std::placeholders::_3,
std::placeholders::_4),
expression1, expression2);
}
/// Construct an array of leaves

9
gtsam/nonlinear/Expression.h
View File

@ -24,7 +24,6 @@
#include <gtsam/base/OptionalJacobian.h>
#include <gtsam/base/VectorSpace.h>
#include <boost/bind.hpp>
#include <boost/make_shared.hpp>
#include <map>
@ -69,20 +68,20 @@ public:
// Expression<Point2>::BinaryFunction<PinholeCamera<Cal3_S2>,Point3>::type
template<class A1>
struct UnaryFunction {
typedef boost::function<
typedef std::function<
T(const A1&, typename MakeOptionalJacobian<T, A1>::type)> type;
};
template<class A1, class A2>
struct BinaryFunction {
typedef boost::function<
typedef std::function<
T(const A1&, const A2&, typename MakeOptionalJacobian<T, A1>::type,
typename MakeOptionalJacobian<T, A2>::type)> type;
};
template<class A1, class A2, class A3>
struct TernaryFunction {
typedef boost::function<
typedef std::function<
T(const A1&, const A2&, const A3&,
typename MakeOptionalJacobian<T, A1>::type,
typename MakeOptionalJacobian<T, A2>::type,
@ -240,7 +239,7 @@ class BinarySumExpression : public Expression<T> {
*/
template <typename T, typename A>
Expression<T> linearExpression(
const boost::function<T(A)>& f, const Expression<A>& expression,
const std::function<T(A)>& f, const Expression<A>& expression,
const Eigen::Matrix<double, traits<T>::dimension, traits<A>::dimension>& dTdA) {
// Use lambda to endow f with a linear Jacobian
typename Expression<T>::template UnaryFunction<A>::type g =

75
gtsam/nonlinear/GncOptimizer.h
View File

@ -74,6 +74,35 @@ class GncOptimizer {
}
}
// check that known inliers and outliers make sense:
std::vector<size_t> inconsistentlySpecifiedWeights; // measurements the user has incorrectly specified
// to be BOTH known inliers and known outliers
std::set_intersection(params.knownInliers.begin(),params.knownInliers.end(),
params.knownOutliers.begin(),params.knownOutliers.end(),
std::inserter(inconsistentlySpecifiedWeights, inconsistentlySpecifiedWeights.begin()));
if(inconsistentlySpecifiedWeights.size() > 0){ // if we have inconsistently specified weights, we throw an exception
params.print("params\n");
throw std::runtime_error("GncOptimizer::constructor: the user has selected one or more measurements"
" to be BOTH a known inlier and a known outlier.");
}
// check that known inliers are in the graph
for (size_t i = 0; i < params.knownInliers.size(); i++){
if( params.knownInliers[i] > nfg_.size()-1 ){ // outside graph
throw std::runtime_error("GncOptimizer::constructor: the user has selected one or more measurements"
"that are not in the factor graph to be known inliers.");
}
}
// check that known outliers are in the graph
for (size_t i = 0; i < params.knownOutliers.size(); i++){
if( params.knownOutliers[i] > nfg_.size()-1 ){ // outside graph
throw std::runtime_error("GncOptimizer::constructor: the user has selected one or more measurements"
"that are not in the factor graph to be known outliers.");
}
}
// initialize weights (if we don't have prior knowledge of inliers/outliers
// the weights are all initialized to 1.
weights_ = initializeWeightsFromKnownInliersAndOutliers();
// set default barcSq_ (inlier threshold)
double alpha = 0.99; // with this (default) probability, inlier residuals are smaller than barcSq_
setInlierCostThresholdsAtProbability(alpha);
@ -109,6 +138,17 @@ class GncOptimizer {
}
}
/** Set weights for each factor. This is typically not needed, but
* provides an extra interface for the user to initialize the weightst
* */
void setWeights(const Vector w) {
if(w.size() != nfg_.size()){
throw std::runtime_error("GncOptimizer::setWeights: the number of specified weights"
" does not match the size of the factor graph.");
}
weights_ = w;
}
/// Access a copy of the internal factor graph.
const NonlinearFactorGraph& getFactors() const { return nfg_; }
@ -132,26 +172,39 @@ class GncOptimizer {
&& equal(barcSq_, other.getInlierCostThresholds());
}
Vector initializeWeightsFromKnownInliersAndOutliers() const{
Vector weights = Vector::Ones(nfg_.size());
for (size_t i = 0; i < params_.knownOutliers.size(); i++){
weights[ params_.knownOutliers[i] ] = 0.0; // known to be outliers
}
return weights;
}
/// Compute optimal solution using graduated non-convexity.
Values optimize() {
// start by assuming all measurements are inliers
weights_ = Vector::Ones(nfg_.size());
BaseOptimizer baseOptimizer(nfg_, state_);
NonlinearFactorGraph graph_initial = this->makeWeightedGraph(weights_);
BaseOptimizer baseOptimizer(graph_initial, state_);
Values result = baseOptimizer.optimize();
double mu = initializeMu();
double prev_cost = nfg_.error(result);
double prev_cost = graph_initial.error(result);
double cost = 0.0; // this will be updated in the main loop
// handle the degenerate case that corresponds to small
// maximum residual errors at initialization
// For GM: if residual error is small, mu -> 0
// For TLS: if residual error is small, mu -> -1
if (mu <= 0) {
if (params_.verbosity >= GncParameters::Verbosity::SUMMARY) {
int nrUnknownInOrOut = nfg_.size() - ( params_.knownInliers.size() + params_.knownOutliers.size() );
// ^^ number of measurements that are not known to be inliers or outliers (GNC will need to figure them out)
if (mu <= 0 || nrUnknownInOrOut == 0) { // no need to even call GNC in this case
if (mu <= 0 && params_.verbosity >= GncParameters::Verbosity::SUMMARY) {
std::cout << "GNC Optimizer stopped because maximum residual at "
"initialization is small."
<< std::endl;
}
if (nrUnknownInOrOut==0 && params_.verbosity >= GncParameters::Verbosity::SUMMARY) {
std::cout << "GNC Optimizer stopped because all measurements are already known to be inliers or outliers"
<< std::endl;
}
if (params_.verbosity >= GncParameters::Verbosity::VALUES) {
result.print("result\n");
std::cout << "mu: " << mu << std::endl;
@ -350,17 +403,21 @@ class GncOptimizer {
/// Calculate gnc weights.
Vector calculateWeights(const Values& currentEstimate, const double mu) {
Vector weights = Vector::Ones(nfg_.size());
Vector weights = initializeWeightsFromKnownInliersAndOutliers();
// do not update the weights that the user has decided are known inliers
std::vector<size_t> allWeights;
for (size_t k = 0; k < nfg_.size(); k++) {
allWeights.push_back(k);
}
std::vector<size_t> knownWeights;
std::set_union(params_.knownInliers.begin(), params_.knownInliers.end(),
params_.knownOutliers.begin(), params_.knownOutliers.end(),
std::inserter(knownWeights, knownWeights.begin()));
std::vector<size_t> unknownWeights;
std::set_difference(allWeights.begin(), allWeights.end(),
params_.knownInliers.begin(),
params_.knownInliers.end(),
knownWeights.begin(), knownWeights.end(),
std::inserter(unknownWeights, unknownWeights.begin()));
// update weights of known inlier/outlier measurements

21
gtsam/nonlinear/GncParams.h
View File

@ -71,6 +71,7 @@ class GncParams {
double weightsTol = 1e-4; ///< If the weights are within weightsTol from being binary, stop iterating (only for TLS)
Verbosity verbosity = SILENT; ///< Verbosity level
std::vector<size_t> knownInliers = std::vector<size_t>(); ///< Slots in the factor graph corresponding to measurements that we know are inliers
std::vector<size_t> knownOutliers = std::vector<size_t>(); ///< Slots in the factor graph corresponding to measurements that we know are outliers
/// Set the robust loss function to be used in GNC (chosen among the ones in GncLossType).
void setLossType(const GncLossType type) {
@ -112,8 +113,21 @@ class GncParams {
* only apply GNC to prune outliers from the loop closures.
* */
void setKnownInliers(const std::vector<size_t>& knownIn) {
for (size_t i = 0; i < knownIn.size(); i++)
for (size_t i = 0; i < knownIn.size(); i++){
knownInliers.push_back(knownIn[i]);
}
std::sort(knownInliers.begin(), knownInliers.end());
}
/** (Optional) Provide a vector of measurements that must be considered outliers. The enties in the vector
* corresponds to the slots in the factor graph. For instance, if you have a nonlinear factor graph nfg,
* and you provide knownOut = {0, 2, 15}, GNC will not apply outlier rejection to nfg[0], nfg[2], and nfg[15].
* */
void setKnownOutliers(const std::vector<size_t>& knownOut) {
for (size_t i = 0; i < knownOut.size(); i++){
knownOutliers.push_back(knownOut[i]);
}
std::sort(knownOutliers.begin(), knownOutliers.end());
}
/// Equals.
@ -121,7 +135,8 @@ class GncParams {
return baseOptimizerParams.equals(other.baseOptimizerParams)
&& lossType == other.lossType && maxIterations == other.maxIterations
&& std::fabs(muStep - other.muStep) <= tol
&& verbosity == other.verbosity && knownInliers == other.knownInliers;
&& verbosity == other.verbosity && knownInliers == other.knownInliers
&& knownOutliers == other.knownOutliers;
}
/// Print.
@ -144,6 +159,8 @@ class GncParams {
std::cout << "verbosity: " << verbosity << "\n";
for (size_t i = 0; i < knownInliers.size(); i++)
std::cout << "knownInliers: " << knownInliers[i] << "\n";
for (size_t i = 0; i < knownOutliers.size(); i++)
std::cout << "knownOutliers: " << knownOutliers[i] << "\n";
baseOptimizerParams.print(str);
}
};

Some files were not shown because too many files have changed in this diff Show More