Merge remote-tracking branch 'origin/develop' into feature/fixedSizeDerivatives
Conflicts: .cprojectrelease/4.3a0
dellaert
commit
96016edf85
48
.cproject
48
.cproject
|
|
@ -2409,6 +2409,46 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testType.run" path="build/wrap/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j4</buildArguments>
|
||||
<buildTarget>testType.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testArgument.run" path="build/wrap/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j4</buildArguments>
|
||||
<buildTarget>testArgument.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testReturnValue.run" path="build/wrap/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j4</buildArguments>
|
||||
<buildTarget>testReturnValue.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testTemplate.run" path="build/wrap/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j4</buildArguments>
|
||||
<buildTarget>testTemplate.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testGlobalFunction.run" path="build/wrap/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j4</buildArguments>
|
||||
<buildTarget>testGlobalFunction.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="schedulingExample.run" path="build/gtsam_unstable/discrete/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
|
|
@ -2657,6 +2697,14 @@
|
|||
<useDefaultCommand>false</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testSymbolicBayesNetB.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testSymbolicBayesNetB.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testGaussianISAM.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
|
|
|
|||
|
|
@ -40,8 +40,10 @@ Optional prerequisites - used automatically if findable by CMake:
|
|||
Additional Information
|
||||
----------------------
|
||||
|
||||
See the [`INSTALL`](https://bitbucket.org/gtborg/gtsam/src/develop/INSTALL) file for more detailed installation instructions.
|
||||
Read about important [`GTSAM-Concepts`] here.
|
||||
|
||||
See the [`INSTALL`] file for more detailed installation instructions.
|
||||
|
||||
GTSAM is open source under the BSD license, see the [`LICENSE`](https://bitbucket.org/gtborg/gtsam/src/develop/LICENSE) and [`LICENSE.BSD`](https://bitbucket.org/gtborg/gtsam/src/develop/LICENSE.BSD) files.
|
||||
|
||||
Please see the [`examples/`](https://bitbucket.org/gtborg/gtsam/src/develop/examples) directory and the [`USAGE`](https://bitbucket.org/gtborg/gtsam/src/develop/USAGE) file for examples on how to use GTSAM.
|
||||
Please see the [`examples/`](examples) directory and the [`USAGE`] file for examples on how to use GTSAM.
|
||||
|
|
@ -15,8 +15,8 @@ option(GTSAM_BUILD_TYPE_POSTFIXES "Enable/Disable appending the build typ
|
|||
# Add debugging flags but only on the first pass
|
||||
if(NOT FIRST_PASS_DONE)
|
||||
if(MSVC)
|
||||
set(CMAKE_C_FLAGS_DEBUG "/D_DEBUG /MDd /Zi /Ob0 /Od /RTC1 /W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during debug builds." FORCE)
|
||||
set(CMAKE_CXX_FLAGS_DEBUG "/D_DEBUG /MDd /Zi /Ob0 /Od /RTC1 /W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during debug builds." FORCE)
|
||||
set(CMAKE_C_FLAGS_DEBUG "/D_DEBUG /MDd /Zi /Ob0 /Od /RTC1 /W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN /DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Flags used by the compiler during debug builds." FORCE)
|
||||
set(CMAKE_CXX_FLAGS_DEBUG "/D_DEBUG /MDd /Zi /Ob0 /Od /RTC1 /W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN /DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Flags used by the compiler during debug builds." FORCE)
|
||||
set(CMAKE_C_FLAGS_RELWITHDEBINFO "/MD /O2 /DNDEBUG /W3 /GR /EHsc /MP /Zi /d2Zi+ /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during relwithdebinfo builds." FORCE)
|
||||
set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "/MD /O2 /DNDEBUG /W3 /GR /EHsc /MP /Zi /d2Zi+ /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during relwithdebinfo builds." FORCE)
|
||||
set(CMAKE_C_FLAGS_RELEASE "/MD /O2 /DNDEBUG /W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during release builds." FORCE)
|
||||
|
|
@ -34,8 +34,8 @@ if(NOT FIRST_PASS_DONE)
|
|||
set(CMAKE_MODULE_LINKER_FLAGS_PROFILING "${CMAKE_MODULE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during profiling builds." FORCE)
|
||||
mark_as_advanced(CMAKE_C_FLAGS_PROFILING CMAKE_CXX_FLAGS_PROFILING CMAKE_EXE_LINKER_FLAGS_PROFILING CMAKE_SHARED_LINKER_FLAGS_PROFILING CMAKE_MODULE_LINKER_FLAGS_PROFILING)
|
||||
else()
|
||||
set(CMAKE_C_FLAGS_DEBUG "${CMAKE_C_FLAGS_DEBUG} -fno-inline -Wall" CACHE STRING "Flags used by the compiler during debug builds." FORCE)
|
||||
set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -fno-inline -Wall" CACHE STRING "Flags used by the compiler during debug builds." FORCE)
|
||||
set(CMAKE_C_FLAGS_DEBUG "${CMAKE_C_FLAGS_DEBUG} -fno-inline -Wall -DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Flags used by the compiler during debug builds." FORCE)
|
||||
set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -fno-inline -Wall -DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Flags used by the compiler during debug builds." FORCE)
|
||||
set(CMAKE_C_FLAGS_RELWITHDEBINFO "-g -O3 -Wall -DNDEBUG" CACHE STRING "Flags used by the compiler during relwithdebinfo builds." FORCE)
|
||||
set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "-g -O3 -Wall -DNDEBUG" CACHE STRING "Flags used by the compiler during relwithdebinfo builds." FORCE)
|
||||
set(CMAKE_C_FLAGS_RELEASE "-O3 -Wall -DNDEBUG -Wall" CACHE STRING "Flags used by the compiler during release builds." FORCE)
|
||||
|
|
|
|||
9
gtsam.h
9
gtsam.h
|
|
@ -1734,6 +1734,8 @@ class Values {
|
|||
void insert(size_t j, const gtsam::Cal3Bundler& t);
|
||||
void insert(size_t j, const gtsam::EssentialMatrix& t);
|
||||
void insert(size_t j, const gtsam::imuBias::ConstantBias& t);
|
||||
void insert(size_t j, Vector t);
|
||||
void insert(size_t j, Matrix t);
|
||||
|
||||
void update(size_t j, const gtsam::Point2& t);
|
||||
void update(size_t j, const gtsam::Point3& t);
|
||||
|
|
@ -1746,9 +1748,10 @@ class Values {
|
|||
void update(size_t j, const gtsam::Cal3Bundler& t);
|
||||
void update(size_t j, const gtsam::EssentialMatrix& t);
|
||||
void update(size_t j, const gtsam::imuBias::ConstantBias& t);
|
||||
void update(size_t j, Vector t);
|
||||
void update(size_t j, Matrix t);
|
||||
|
||||
template<T = {gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Pose2,
|
||||
gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::imuBias::ConstantBias}>
|
||||
template<T = {gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Pose2, gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, gtsam::EssentialMatrix, gtsam::imuBias::ConstantBias, Vector, Matrix}>
|
||||
T at(size_t j);
|
||||
};
|
||||
|
||||
|
|
@ -2146,7 +2149,7 @@ class NonlinearISAM {
|
|||
#include <gtsam/geometry/StereoPoint2.h>
|
||||
|
||||
#include <gtsam/slam/PriorFactor.h>
|
||||
template<T = {gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2, gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}>
|
||||
template<T = { gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2,gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}>
|
||||
virtual class PriorFactor : gtsam::NoiseModelFactor {
|
||||
PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel);
|
||||
T prior() const;
|
||||
|
|
|
|||
|
|
@ -45,6 +45,8 @@ endif()
|
|||
option(GTSAM_BUILD_METIS_EXECUTABLES "Build metis library executables" OFF)
|
||||
add_subdirectory(metis)
|
||||
|
||||
add_subdirectory(ceres)
|
||||
|
||||
############ NOTE: When updating GeographicLib be sure to disable building their examples
|
||||
############ and unit tests by commenting out their lines:
|
||||
# add_subdirectory (examples)
|
||||
|
|
|
|||
|
|
@ -0,0 +1,2 @@
|
|||
file(GLOB ceres_headers "${CMAKE_CURRENT_SOURCE_DIR}/*.h")
|
||||
install(FILES ${ceres_headers} DESTINATION include/gtsam/3rdparty/ceres)
|
||||
|
|
@ -142,10 +142,10 @@
|
|||
|
||||
#include <stddef.h>
|
||||
|
||||
#include <gtsam_unstable/nonlinear/ceres_jet.h>
|
||||
#include <gtsam_unstable/nonlinear/ceres_eigen.h>
|
||||
#include <gtsam_unstable/nonlinear/ceres_fixed_array.h>
|
||||
#include <gtsam_unstable/nonlinear/ceres_variadic_evaluate.h>
|
||||
#include <gtsam/3rdparty/ceres/jet.h>
|
||||
#include <gtsam/3rdparty/ceres/eigen.h>
|
||||
#include <gtsam/3rdparty/ceres/fixed_array.h>
|
||||
#include <gtsam/3rdparty/ceres/variadic_evaluate.h>
|
||||
#define DCHECK assert
|
||||
#define DCHECK_GT(a,b) assert((a)>(b))
|
||||
|
||||
|
|
@ -33,7 +33,7 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam_unstable/nonlinear/ceres_rotation.h>
|
||||
#include <gtsam/3rdparty/ceres/rotation.h>
|
||||
|
||||
// Templated pinhole camera model for used with Ceres. The camera is
|
||||
// parameterized using 9 parameters: 3 for rotation, 3 for translation, 1 for
|
||||
|
|
@ -34,8 +34,8 @@
|
|||
|
||||
#include <cstddef>
|
||||
#include <gtsam/3rdparty/gtsam_eigen_includes.h>
|
||||
#include <gtsam_unstable/nonlinear/ceres_macros.h>
|
||||
#include <gtsam_unstable/nonlinear/ceres_manual_constructor.h>
|
||||
#include <gtsam/3rdparty/ceres/macros.h>
|
||||
#include <gtsam/3rdparty/ceres/manual_constructor.h>
|
||||
|
||||
namespace ceres {
|
||||
namespace internal {
|
||||
|
|
@ -163,7 +163,7 @@
|
|||
#include <string>
|
||||
|
||||
#include <gtsam/3rdparty/gtsam_eigen_includes.h>
|
||||
#include <gtsam_unstable/nonlinear/ceres_fpclassify.h>
|
||||
#include <gtsam/3rdparty/ceres/fpclassify.h>
|
||||
|
||||
namespace ceres {
|
||||
|
||||
|
|
@ -47,6 +47,7 @@
|
|||
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <limits>
|
||||
#include <assert.h>
|
||||
#define DCHECK assert
|
||||
|
||||
|
|
@ -34,9 +34,9 @@
|
|||
|
||||
#include <stddef.h>
|
||||
|
||||
#include <gtsam_unstable/nonlinear/ceres_jet.h>
|
||||
#include <gtsam_unstable/nonlinear/ceres_eigen.h>
|
||||
#include <gtsam_unstable/nonlinear/ceres_fixed_array.h>
|
||||
#include <gtsam/3rdparty/ceres/jet.h>
|
||||
#include <gtsam/3rdparty/ceres/eigen.h>
|
||||
#include <gtsam/3rdparty/ceres/fixed_array.h>
|
||||
|
||||
namespace ceres {
|
||||
namespace internal {
|
||||
|
|
@ -73,7 +73,6 @@ template<typename T>
|
|||
struct dimension: public Dynamic {
|
||||
};
|
||||
|
||||
|
||||
/**
|
||||
* zero<T>::value is intended to be the origin of a canonical coordinate system
|
||||
* with canonical(t) == DefaultChart<T>::local(zero<T>::value, t)
|
||||
|
|
@ -111,12 +110,14 @@ struct is_group<Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > : publi
|
|||
};
|
||||
|
||||
template<int M, int N, int Options, int MaxRows, int MaxCols>
|
||||
struct is_manifold<Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > : public boost::true_type{
|
||||
struct is_manifold<Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > : public boost::true_type {
|
||||
};
|
||||
|
||||
template<int M, int N, int Options, int MaxRows, int MaxCols>
|
||||
struct dimension<Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > : public boost::integral_constant<int,
|
||||
M == Eigen::Dynamic ? Eigen::Dynamic : (N == Eigen::Dynamic ? Eigen::Dynamic : M * N)> {
|
||||
struct dimension<Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > : public boost::integral_constant<
|
||||
int,
|
||||
M == Eigen::Dynamic ? Eigen::Dynamic :
|
||||
(N == Eigen::Dynamic ? Eigen::Dynamic : M * N)> {
|
||||
//TODO after switch to c++11 : the above should should be extracted to a constexpr function
|
||||
// for readability and to reduce code duplication
|
||||
};
|
||||
|
|
@ -131,10 +132,10 @@ struct zero<Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > {
|
|||
};
|
||||
|
||||
template<int M, int N, int Options>
|
||||
struct identity<Eigen::Matrix<double, M, N, Options> > : public zero<Eigen::Matrix<double, M, N, Options> > {
|
||||
struct identity<Eigen::Matrix<double, M, N, Options> > : public zero<
|
||||
Eigen::Matrix<double, M, N, Options> > {
|
||||
};
|
||||
|
||||
|
||||
template<typename T> struct is_chart: public boost::false_type {
|
||||
};
|
||||
|
||||
|
|
@ -250,10 +251,14 @@ struct DefaultChart<Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > {
|
|||
*/
|
||||
typedef Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> type;
|
||||
typedef type T;
|
||||
typedef Eigen::Matrix<double, traits::dimension<T>::value, 1> vector;BOOST_STATIC_ASSERT_MSG((M!=Eigen::Dynamic && N!=Eigen::Dynamic),
|
||||
"DefaultChart has not been implemented yet for dynamically sized matrices");
|
||||
typedef Eigen::Matrix<double, traits::dimension<T>::value, 1> vector;
|
||||
|
||||
BOOST_STATIC_ASSERT_MSG((M!=Eigen::Dynamic && N!=Eigen::Dynamic),
|
||||
"Internal error: DefaultChart for Dynamic should be handled by template below");
|
||||
|
||||
static vector local(const T& origin, const T& other) {
|
||||
return reshape<vector::RowsAtCompileTime, 1, vector::Options>(other) - reshape<vector::RowsAtCompileTime, 1, vector::Options>(origin);
|
||||
return reshape<vector::RowsAtCompileTime, 1, vector::Options>(other)
|
||||
- reshape<vector::RowsAtCompileTime, 1, vector::Options>(origin);
|
||||
}
|
||||
static T retract(const T& origin, const vector& d) {
|
||||
return origin + reshape<M, N, Options>(d);
|
||||
|
|
@ -266,20 +271,36 @@ struct DefaultChart<Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > {
|
|||
// Dynamically sized Vector
|
||||
template<>
|
||||
struct DefaultChart<Vector> {
|
||||
typedef Vector T;
|
||||
typedef T type;
|
||||
typedef T vector;
|
||||
static vector local(const T& origin, const T& other) {
|
||||
typedef Vector type;
|
||||
typedef Vector vector;
|
||||
static vector local(const Vector& origin, const Vector& other) {
|
||||
return other - origin;
|
||||
}
|
||||
static T retract(const T& origin, const vector& d) {
|
||||
static Vector retract(const Vector& origin, const vector& d) {
|
||||
return origin + d;
|
||||
}
|
||||
static int getDimension(const T& origin) {
|
||||
static int getDimension(const Vector& origin) {
|
||||
return origin.size();
|
||||
}
|
||||
};
|
||||
|
||||
// Dynamically sized Matrix
|
||||
template<>
|
||||
struct DefaultChart<Matrix> {
|
||||
typedef Matrix type;
|
||||
typedef Vector vector;
|
||||
static vector local(const Matrix& origin, const Matrix& other) {
|
||||
Matrix d = other - origin;
|
||||
return Eigen::Map<Vector>(d.data(),getDimension(d));
|
||||
}
|
||||
static Matrix retract(const Matrix& origin, const vector& d) {
|
||||
return origin + Eigen::Map<const Matrix>(d.data(),origin.rows(),origin.cols());
|
||||
}
|
||||
static int getDimension(const Matrix& m) {
|
||||
return m.size();
|
||||
}
|
||||
};
|
||||
|
||||
/**
|
||||
* Old Concept check class for Manifold types
|
||||
* Requires a mapping between a linear tangent space and the underlying
|
||||
|
|
|
|||
|
|
@ -26,8 +26,8 @@ namespace gtsam {
|
|||
/* ************************************************************************* */
|
||||
template<class FACTOR>
|
||||
void MetisIndex::augment(const FactorGraph<FACTOR>& factors) {
|
||||
std::map<idx_t, FastSet<idx_t> > iAdjMap; // Stores a set of keys that are adjacent to key x, with adjMap.first
|
||||
std::map<idx_t, FastSet<idx_t> >::iterator iAdjMapIt;
|
||||
std::map<int32_t, FastSet<int32_t> > iAdjMap; // Stores a set of keys that are adjacent to key x, with adjMap.first
|
||||
std::map<int32_t, FastSet<int32_t> >::iterator iAdjMapIt;
|
||||
std::set<Key> keySet;
|
||||
|
||||
/* ********** Convert to CSR format ********** */
|
||||
|
|
@ -36,7 +36,7 @@ void MetisIndex::augment(const FactorGraph<FACTOR>& factors) {
|
|||
// starting at index xadj[i] and ending at(but not including)
|
||||
// index xadj[i + 1](i.e., adjncy[xadj[i]] through
|
||||
// and including adjncy[xadj[i + 1] - 1]).
|
||||
idx_t keyCounter = 0;
|
||||
int32_t keyCounter = 0;
|
||||
|
||||
// First: Record a copy of each key inside the factorgraph and create a
|
||||
// key to integer mapping. This is referenced during the adjaceny step
|
||||
|
|
@ -58,7 +58,7 @@ void MetisIndex::augment(const FactorGraph<FACTOR>& factors) {
|
|||
BOOST_FOREACH(const Key& k1, *factors[i])
|
||||
BOOST_FOREACH(const Key& k2, *factors[i])
|
||||
if (k1 != k2) {
|
||||
// Store both in Key and idx_t format
|
||||
// Store both in Key and int32_t format
|
||||
int i = intKeyBMap_.left.at(k1);
|
||||
int j = intKeyBMap_.left.at(k2);
|
||||
iAdjMap[i].insert(iAdjMap[i].end(), j);
|
||||
|
|
@ -71,14 +71,14 @@ void MetisIndex::augment(const FactorGraph<FACTOR>& factors) {
|
|||
|
||||
xadj_.push_back(0); // Always set the first index to zero
|
||||
for (iAdjMapIt = iAdjMap.begin(); iAdjMapIt != iAdjMap.end(); ++iAdjMapIt) {
|
||||
std::vector<idx_t> temp;
|
||||
std::vector<int32_t> temp;
|
||||
// Copy from the FastSet into a temporary vector
|
||||
std::copy(iAdjMapIt->second.begin(), iAdjMapIt->second.end(),
|
||||
std::back_inserter(temp));
|
||||
// Insert each index's set in order by appending them to the end of adj_
|
||||
adj_.insert(adj_.end(), temp.begin(), temp.end());
|
||||
//adj_.push_back(temp);
|
||||
xadj_.push_back((idx_t) adj_.size());
|
||||
xadj_.push_back((int32_t) adj_.size());
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -22,7 +22,6 @@
|
|||
#include <gtsam/base/FastVector.h>
|
||||
#include <gtsam/base/types.h>
|
||||
#include <gtsam/base/timing.h>
|
||||
#include <gtsam/3rdparty/metis/metis.h>
|
||||
|
||||
// Boost bimap generates many ugly warnings in CLANG
|
||||
#ifdef __clang__
|
||||
|
|
@ -47,13 +46,13 @@ namespace gtsam {
|
|||
class GTSAM_EXPORT MetisIndex {
|
||||
public:
|
||||
typedef boost::shared_ptr<MetisIndex> shared_ptr;
|
||||
typedef boost::bimap<Key, idx_t> bm_type;
|
||||
typedef boost::bimap<Key, int32_t> bm_type;
|
||||
|
||||
private:
|
||||
FastVector<idx_t> xadj_; // Index of node's adjacency list in adj
|
||||
FastVector<idx_t> adj_; // Stores ajacency lists of all nodes, appended into a single vector
|
||||
FastVector<idx_t> iadj_; // Integer keys for passing into metis. One to one mapping with adj_;
|
||||
boost::bimap<Key, idx_t> intKeyBMap_; // Stores Key <-> integer value relationship
|
||||
FastVector<int32_t> xadj_; // Index of node's adjacency list in adj
|
||||
FastVector<int32_t> adj_; // Stores ajacency lists of all nodes, appended into a single vector
|
||||
FastVector<int32_t> iadj_; // Integer keys for passing into metis. One to one mapping with adj_;
|
||||
boost::bimap<Key, int32_t> intKeyBMap_; // Stores Key <-> integer value relationship
|
||||
size_t nKeys_;
|
||||
|
||||
public:
|
||||
|
|
@ -84,16 +83,16 @@ public:
|
|||
template<class FACTOR>
|
||||
void augment(const FactorGraph<FACTOR>& factors);
|
||||
|
||||
std::vector<idx_t> xadj() const {
|
||||
std::vector<int32_t> xadj() const {
|
||||
return xadj_;
|
||||
}
|
||||
std::vector<idx_t> adj() const {
|
||||
std::vector<int32_t> adj() const {
|
||||
return adj_;
|
||||
}
|
||||
size_t nValues() const {
|
||||
return nKeys_;
|
||||
}
|
||||
Key intToKey(idx_t value) const {
|
||||
Key intToKey(int32_t value) const {
|
||||
assert(value >= 0);
|
||||
return intKeyBMap_.right.find(value)->second;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -106,9 +106,9 @@ namespace gtsam {
|
|||
typedef boost::tuple<Key,size_t,Key> Base;
|
||||
KeyInfoEntry(){}
|
||||
KeyInfoEntry(size_t idx, size_t d, Key start) : Base(idx, d, start) {}
|
||||
const size_t index() const { return this->get<0>(); }
|
||||
const size_t dim() const { return this->get<1>(); }
|
||||
const size_t colstart() const { return this->get<2>(); }
|
||||
size_t index() const { return this->get<0>(); }
|
||||
size_t dim() const { return this->get<1>(); }
|
||||
size_t colstart() const { return this->get<2>(); }
|
||||
};
|
||||
|
||||
/************************************************************************************/
|
||||
|
|
|
|||
|
|
@ -0,0 +1,497 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 CombinedImuFactor.cpp
|
||||
* @author Luca Carlone
|
||||
* @author Stephen Williams
|
||||
* @author Richard Roberts
|
||||
* @author Vadim Indelman
|
||||
* @author David Jensen
|
||||
* @author Frank Dellaert
|
||||
**/
|
||||
|
||||
#include <gtsam/navigation/CombinedImuFactor.h>
|
||||
|
||||
/* External or standard includes */
|
||||
#include <ostream>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
using namespace std;
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
// Inner class CombinedPreintegratedMeasurements
|
||||
//------------------------------------------------------------------------------
|
||||
CombinedImuFactor::CombinedPreintegratedMeasurements::CombinedPreintegratedMeasurements(
|
||||
const imuBias::ConstantBias& bias, const Matrix3& measuredAccCovariance,
|
||||
const Matrix3& measuredOmegaCovariance, const Matrix3& integrationErrorCovariance,
|
||||
const Matrix3& biasAccCovariance, const Matrix3& biasOmegaCovariance,
|
||||
const Matrix& biasAccOmegaInit, const bool use2ndOrderIntegration) :
|
||||
biasHat_(bias), deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()),
|
||||
deltaRij_(Rot3()), deltaTij_(0.0),
|
||||
delPdelBiasAcc_(Z_3x3), delPdelBiasOmega_(Z_3x3),
|
||||
delVdelBiasAcc_(Z_3x3), delVdelBiasOmega_(Z_3x3),
|
||||
delRdelBiasOmega_(Z_3x3), use2ndOrderIntegration_(use2ndOrderIntegration)
|
||||
{
|
||||
measurementCovariance_.setZero();
|
||||
measurementCovariance_.block<3,3>(0,0) = integrationErrorCovariance;
|
||||
measurementCovariance_.block<3,3>(3,3) = measuredAccCovariance;
|
||||
measurementCovariance_.block<3,3>(6,6) = measuredOmegaCovariance;
|
||||
measurementCovariance_.block<3,3>(9,9) = biasAccCovariance;
|
||||
measurementCovariance_.block<3,3>(12,12) = biasOmegaCovariance;
|
||||
measurementCovariance_.block<6,6>(15,15) = biasAccOmegaInit;
|
||||
PreintMeasCov_.setZero();
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
void CombinedImuFactor::CombinedPreintegratedMeasurements::print(const string& s) const{
|
||||
cout << s << endl;
|
||||
biasHat_.print(" biasHat");
|
||||
cout << " deltaTij " << deltaTij_ << endl;
|
||||
cout << " deltaPij [ " << deltaPij_.transpose() << " ]" << endl;
|
||||
cout << " deltaVij [ " << deltaVij_.transpose() << " ]" << endl;
|
||||
deltaRij_.print(" deltaRij ");
|
||||
cout << " measurementCovariance [ " << measurementCovariance_ << " ]" << endl;
|
||||
cout << " PreintMeasCov [ " << PreintMeasCov_ << " ]" << endl;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
bool CombinedImuFactor::CombinedPreintegratedMeasurements::equals(const CombinedPreintegratedMeasurements& expected, double tol) const{
|
||||
return biasHat_.equals(expected.biasHat_, tol)
|
||||
&& equal_with_abs_tol(measurementCovariance_, expected.measurementCovariance_, tol)
|
||||
&& equal_with_abs_tol(deltaPij_, expected.deltaPij_, tol)
|
||||
&& equal_with_abs_tol(deltaVij_, expected.deltaVij_, tol)
|
||||
&& deltaRij_.equals(expected.deltaRij_, tol)
|
||||
&& fabs(deltaTij_ - expected.deltaTij_) < tol
|
||||
&& equal_with_abs_tol(delPdelBiasAcc_, expected.delPdelBiasAcc_, tol)
|
||||
&& equal_with_abs_tol(delPdelBiasOmega_, expected.delPdelBiasOmega_, tol)
|
||||
&& equal_with_abs_tol(delVdelBiasAcc_, expected.delVdelBiasAcc_, tol)
|
||||
&& equal_with_abs_tol(delVdelBiasOmega_, expected.delVdelBiasOmega_, tol)
|
||||
&& equal_with_abs_tol(delRdelBiasOmega_, expected.delRdelBiasOmega_, tol);
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
void CombinedImuFactor::CombinedPreintegratedMeasurements::resetIntegration(){
|
||||
deltaPij_ = Vector3::Zero();
|
||||
deltaVij_ = Vector3::Zero();
|
||||
deltaRij_ = Rot3();
|
||||
deltaTij_ = 0.0;
|
||||
delPdelBiasAcc_ = Z_3x3;
|
||||
delPdelBiasOmega_ = Z_3x3;
|
||||
delVdelBiasAcc_ = Z_3x3;
|
||||
delVdelBiasOmega_ = Z_3x3;
|
||||
delRdelBiasOmega_ = Z_3x3;
|
||||
PreintMeasCov_.setZero();
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement(
|
||||
const Vector3& measuredAcc, const Vector3& measuredOmega,
|
||||
double deltaT, boost::optional<const Pose3&> body_P_sensor) {
|
||||
|
||||
// NOTE: order is important here because each update uses old values, e.g., velocity and position updates are based on previous rotation estimate.
|
||||
// (i.e., we have to update jacobians and covariances before updating preintegrated measurements).
|
||||
|
||||
// First we compensate the measurements for the bias: since we have only an estimate of the bias, the covariance includes the corresponding uncertainty
|
||||
Vector3 correctedAcc = biasHat_.correctAccelerometer(measuredAcc);
|
||||
Vector3 correctedOmega = biasHat_.correctGyroscope(measuredOmega);
|
||||
|
||||
// Then compensate for sensor-body displacement: we express the quantities (originally in the IMU frame) into the body frame
|
||||
if(body_P_sensor){
|
||||
Matrix3 body_R_sensor = body_P_sensor->rotation().matrix();
|
||||
correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame
|
||||
Matrix3 body_omega_body__cross = skewSymmetric(correctedOmega);
|
||||
correctedAcc = body_R_sensor * correctedAcc - body_omega_body__cross * body_omega_body__cross * body_P_sensor->translation().vector();
|
||||
// linear acceleration vector in the body frame
|
||||
}
|
||||
|
||||
const Vector3 theta_incr = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement
|
||||
const Rot3 Rincr = Rot3::Expmap(theta_incr); // rotation increment computed from the current rotation rate measurement
|
||||
const Matrix3 Jr_theta_incr = Rot3::rightJacobianExpMapSO3(theta_incr); // Right jacobian computed at theta_incr
|
||||
|
||||
// Update Jacobians
|
||||
/* ----------------------------------------------------------------------------------------------------------------------- */
|
||||
if(!use2ndOrderIntegration_){
|
||||
delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT;
|
||||
delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT;
|
||||
}else{
|
||||
delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - 0.5 * deltaRij_.matrix() * deltaT*deltaT;
|
||||
delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT - 0.5 * deltaRij_.matrix()
|
||||
* skewSymmetric(biasHat_.correctAccelerometer(measuredAcc)) * deltaT*deltaT * delRdelBiasOmega_;
|
||||
}
|
||||
|
||||
delVdelBiasAcc_ += -deltaRij_.matrix() * deltaT;
|
||||
delVdelBiasOmega_ += -deltaRij_.matrix() * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega_;
|
||||
delRdelBiasOmega_ = Rincr.inverse().matrix() * delRdelBiasOmega_ - Jr_theta_incr * deltaT;
|
||||
|
||||
// Update preintegrated measurements covariance: as in [2] we consider a first order propagation that
|
||||
// can be seen as a prediction phase in an EKF framework. In this implementation, contrarily to [2] we
|
||||
// consider the uncertainty of the bias selection and we keep correlation between biases and preintegrated measurements
|
||||
/* ----------------------------------------------------------------------------------------------------------------------- */
|
||||
const Vector3 theta_i = Rot3::Logmap(deltaRij_); // parametrization of so(3)
|
||||
const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3(theta_i);
|
||||
|
||||
Rot3 Rot_j = deltaRij_ * Rincr;
|
||||
const Vector3 theta_j = Rot3::Logmap(Rot_j); // parametrization of so(3)
|
||||
const Matrix3 Jrinv_theta_j = Rot3::rightJacobianExpMapSO3inverse(theta_j);
|
||||
|
||||
// Single Jacobians to propagate covariance
|
||||
Matrix3 H_pos_pos = I_3x3;
|
||||
Matrix3 H_pos_vel = I_3x3 * deltaT;
|
||||
Matrix3 H_pos_angles = Z_3x3;
|
||||
|
||||
Matrix3 H_vel_pos = Z_3x3;
|
||||
Matrix3 H_vel_vel = I_3x3;
|
||||
Matrix3 H_vel_angles = - deltaRij_.matrix() * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT;
|
||||
// analytic expression corresponding to the following numerical derivative
|
||||
// Matrix H_vel_angles = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_vel, correctedOmega, correctedAcc, deltaT, _1, deltaVij), theta_i);
|
||||
Matrix3 H_vel_biasacc = - deltaRij_.matrix() * deltaT;
|
||||
|
||||
Matrix3 H_angles_pos = Z_3x3;
|
||||
Matrix3 H_angles_vel = Z_3x3;
|
||||
Matrix3 H_angles_angles = Jrinv_theta_j * Rincr.inverse().matrix() * Jr_theta_i;
|
||||
Matrix3 H_angles_biasomega =- Jrinv_theta_j * Jr_theta_incr * deltaT;
|
||||
// analytic expression corresponding to the following numerical derivative
|
||||
// Matrix H_angles_angles = numericalDerivative11<Vector3, Vector3>(boost::bind(&PreIntegrateIMUObservations_delta_angles, correctedOmega, deltaT, _1), thetaij);
|
||||
|
||||
// overall Jacobian wrt preintegrated measurements (df/dx)
|
||||
Matrix F(15,15);
|
||||
F << H_pos_pos, H_pos_vel, H_pos_angles, Z_3x3, Z_3x3,
|
||||
H_vel_pos, H_vel_vel, H_vel_angles, H_vel_biasacc, Z_3x3,
|
||||
H_angles_pos, H_angles_vel, H_angles_angles, Z_3x3, H_angles_biasomega,
|
||||
Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3,
|
||||
Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3;
|
||||
|
||||
// first order uncertainty propagation
|
||||
// Optimized matrix multiplication (1/deltaT) * G * measurementCovariance * G.transpose()
|
||||
|
||||
Matrix G_measCov_Gt = Matrix::Zero(15,15);
|
||||
// BLOCK DIAGONAL TERMS
|
||||
G_measCov_Gt.block<3,3>(0,0) = deltaT * measurementCovariance_.block<3,3>(0,0);
|
||||
|
||||
G_measCov_Gt.block<3,3>(3,3) = (1/deltaT) * (H_vel_biasacc) *
|
||||
(measurementCovariance_.block<3,3>(3,3) + measurementCovariance_.block<3,3>(15,15) ) *
|
||||
(H_vel_biasacc.transpose());
|
||||
|
||||
G_measCov_Gt.block<3,3>(6,6) = (1/deltaT) * (H_angles_biasomega) *
|
||||
(measurementCovariance_.block<3,3>(6,6) + measurementCovariance_.block<3,3>(18,18) ) *
|
||||
(H_angles_biasomega.transpose());
|
||||
|
||||
G_measCov_Gt.block<3,3>(9,9) = deltaT * measurementCovariance_.block<3,3>(9,9);
|
||||
|
||||
G_measCov_Gt.block<3,3>(12,12) = deltaT * measurementCovariance_.block<3,3>(12,12);
|
||||
|
||||
// NEW OFF BLOCK DIAGONAL TERMS
|
||||
Matrix3 block23 = H_vel_biasacc * measurementCovariance_.block<3,3>(18,15) * H_angles_biasomega.transpose();
|
||||
G_measCov_Gt.block<3,3>(3,6) = block23;
|
||||
G_measCov_Gt.block<3,3>(6,3) = block23.transpose();
|
||||
|
||||
PreintMeasCov_ = F * PreintMeasCov_ * F.transpose() + G_measCov_Gt;
|
||||
|
||||
// Update preintegrated measurements
|
||||
/* ----------------------------------------------------------------------------------------------------------------------- */
|
||||
if(!use2ndOrderIntegration_){
|
||||
deltaPij_ += deltaVij_ * deltaT;
|
||||
}else{
|
||||
deltaPij_ += deltaVij_ * deltaT + 0.5 * deltaRij_.matrix() * biasHat_.correctAccelerometer(measuredAcc) * deltaT*deltaT;
|
||||
}
|
||||
deltaVij_ += deltaRij_.matrix() * correctedAcc * deltaT;
|
||||
deltaRij_ = deltaRij_ * Rincr;
|
||||
deltaTij_ += deltaT;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
// CombinedImuFactor methods
|
||||
//------------------------------------------------------------------------------
|
||||
CombinedImuFactor::CombinedImuFactor() :
|
||||
preintegratedMeasurements_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Matrix::Zero(6,6)) {}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
CombinedImuFactor::CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j,
|
||||
const CombinedPreintegratedMeasurements& preintegratedMeasurements,
|
||||
const Vector3& gravity, const Vector3& omegaCoriolis,
|
||||
boost::optional<const Pose3&> body_P_sensor, const bool use2ndOrderCoriolis) :
|
||||
Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.PreintMeasCov_), pose_i, vel_i, pose_j, vel_j, bias_i, bias_j),
|
||||
preintegratedMeasurements_(preintegratedMeasurements),
|
||||
gravity_(gravity),
|
||||
omegaCoriolis_(omegaCoriolis),
|
||||
body_P_sensor_(body_P_sensor),
|
||||
use2ndOrderCoriolis_(use2ndOrderCoriolis){
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
gtsam::NonlinearFactor::shared_ptr CombinedImuFactor::clone() const {
|
||||
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
|
||||
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
void CombinedImuFactor::print(const string& s, const KeyFormatter& keyFormatter) const {
|
||||
cout << s << "CombinedImuFactor("
|
||||
<< keyFormatter(this->key1()) << ","
|
||||
<< keyFormatter(this->key2()) << ","
|
||||
<< keyFormatter(this->key3()) << ","
|
||||
<< keyFormatter(this->key4()) << ","
|
||||
<< keyFormatter(this->key5()) << ","
|
||||
<< keyFormatter(this->key6()) << ")\n";
|
||||
preintegratedMeasurements_.print(" preintegrated measurements:");
|
||||
cout << " gravity: [ " << gravity_.transpose() << " ]" << endl;
|
||||
cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << endl;
|
||||
this->noiseModel_->print(" noise model: ");
|
||||
if(this->body_P_sensor_)
|
||||
this->body_P_sensor_->print(" sensor pose in body frame: ");
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
bool CombinedImuFactor::equals(const NonlinearFactor& expected, double tol) const {
|
||||
const This *e = dynamic_cast<const This*> (&expected);
|
||||
return e != NULL && Base::equals(*e, tol)
|
||||
&& preintegratedMeasurements_.equals(e->preintegratedMeasurements_, tol)
|
||||
&& equal_with_abs_tol(gravity_, e->gravity_, tol)
|
||||
&& equal_with_abs_tol(omegaCoriolis_, e->omegaCoriolis_, tol)
|
||||
&& ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_)));
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
Vector CombinedImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j,
|
||||
const imuBias::ConstantBias& bias_i, const imuBias::ConstantBias& bias_j,
|
||||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2,
|
||||
boost::optional<Matrix&> H3, boost::optional<Matrix&> H4,
|
||||
boost::optional<Matrix&> H5, boost::optional<Matrix&> H6) const {
|
||||
|
||||
const double& deltaTij = preintegratedMeasurements_.deltaTij_;
|
||||
const Vector3 biasAccIncr = bias_i.accelerometer() - preintegratedMeasurements_.biasHat_.accelerometer();
|
||||
const Vector3 biasOmegaIncr = bias_i.gyroscope() - preintegratedMeasurements_.biasHat_.gyroscope();
|
||||
|
||||
// we give some shorter name to rotations and translations
|
||||
const Rot3 Rot_i = pose_i.rotation();
|
||||
const Rot3 Rot_j = pose_j.rotation();
|
||||
const Vector3 pos_i = pose_i.translation().vector();
|
||||
const Vector3 pos_j = pose_j.translation().vector();
|
||||
|
||||
// We compute factor's Jacobians, according to [3]
|
||||
/* ---------------------------------------------------------------------------------------------------- */
|
||||
const Rot3 deltaRij_biascorrected = preintegratedMeasurements_.deltaRij_.retract(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP);
|
||||
// deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr)
|
||||
|
||||
Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
|
||||
|
||||
Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected -
|
||||
Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij; // Coriolis term
|
||||
|
||||
const Rot3 deltaRij_biascorrected_corioliscorrected =
|
||||
Rot3::Expmap( theta_biascorrected_corioliscorrected );
|
||||
|
||||
const Rot3 fRhat = deltaRij_biascorrected_corioliscorrected.between(Rot_i.between(Rot_j));
|
||||
|
||||
const Matrix3 Jr_theta_bcc = Rot3::rightJacobianExpMapSO3(theta_biascorrected_corioliscorrected);
|
||||
|
||||
const Matrix3 Jtheta = -Jr_theta_bcc * skewSymmetric(Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij);
|
||||
|
||||
const Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat));
|
||||
|
||||
if(H1) {
|
||||
H1->resize(15,6);
|
||||
|
||||
Matrix3 dfPdPi;
|
||||
Matrix3 dfVdPi;
|
||||
if(use2ndOrderCoriolis_){
|
||||
dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij;
|
||||
dfVdPi = skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij;
|
||||
}
|
||||
else{
|
||||
dfPdPi = - Rot_i.matrix();
|
||||
dfVdPi = Z_3x3;
|
||||
}
|
||||
|
||||
(*H1) <<
|
||||
// dfP/dRi
|
||||
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij_
|
||||
+ preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr),
|
||||
// dfP/dPi
|
||||
dfPdPi,
|
||||
// dfV/dRi
|
||||
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij_
|
||||
+ preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr),
|
||||
// dfV/dPi
|
||||
dfVdPi,
|
||||
// dfR/dRi
|
||||
Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta),
|
||||
// dfR/dPi
|
||||
Z_3x3,
|
||||
//dBiasAcc/dPi
|
||||
Z_3x3, Z_3x3,
|
||||
//dBiasOmega/dPi
|
||||
Z_3x3, Z_3x3;
|
||||
}
|
||||
|
||||
if(H2) {
|
||||
H2->resize(15,3);
|
||||
(*H2) <<
|
||||
// dfP/dVi
|
||||
- I_3x3 * deltaTij
|
||||
+ skewSymmetric(omegaCoriolis_) * deltaTij * deltaTij, // Coriolis term - we got rid of the 2 wrt ins paper
|
||||
// dfV/dVi
|
||||
- I_3x3
|
||||
+ 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term
|
||||
// dfR/dVi
|
||||
Z_3x3,
|
||||
//dBiasAcc/dVi
|
||||
Z_3x3,
|
||||
//dBiasOmega/dVi
|
||||
Z_3x3;
|
||||
}
|
||||
|
||||
if(H3) {
|
||||
H3->resize(15,6);
|
||||
(*H3) <<
|
||||
// dfP/dPosej
|
||||
Z_3x3, Rot_j.matrix(),
|
||||
// dfV/dPosej
|
||||
Matrix::Zero(3,6),
|
||||
// dfR/dPosej
|
||||
Jrinv_fRhat * ( I_3x3 ), Z_3x3,
|
||||
//dBiasAcc/dPosej
|
||||
Z_3x3, Z_3x3,
|
||||
//dBiasOmega/dPosej
|
||||
Z_3x3, Z_3x3;
|
||||
}
|
||||
|
||||
if(H4) {
|
||||
H4->resize(15,3);
|
||||
(*H4) <<
|
||||
// dfP/dVj
|
||||
Z_3x3,
|
||||
// dfV/dVj
|
||||
I_3x3,
|
||||
// dfR/dVj
|
||||
Z_3x3,
|
||||
//dBiasAcc/dVj
|
||||
Z_3x3,
|
||||
//dBiasOmega/dVj
|
||||
Z_3x3;
|
||||
}
|
||||
|
||||
if(H5) {
|
||||
const Matrix3 Jrinv_theta_bc = Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected);
|
||||
const Matrix3 Jr_JbiasOmegaIncr = Rot3::rightJacobianExpMapSO3(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr);
|
||||
const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega_;
|
||||
|
||||
H5->resize(15,6);
|
||||
(*H5) <<
|
||||
// dfP/dBias_i
|
||||
- Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasAcc_,
|
||||
- Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasOmega_,
|
||||
// dfV/dBias_i
|
||||
- Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasAcc_,
|
||||
- Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasOmega_,
|
||||
// dfR/dBias_i
|
||||
Matrix::Zero(3,3),
|
||||
Jrinv_fRhat * ( - fRhat.inverse().matrix() * JbiasOmega),
|
||||
//dBiasAcc/dBias_i
|
||||
-I_3x3, Z_3x3,
|
||||
//dBiasOmega/dBias_i
|
||||
Z_3x3, -I_3x3;
|
||||
}
|
||||
|
||||
if(H6) {
|
||||
H6->resize(15,6);
|
||||
(*H6) <<
|
||||
// dfP/dBias_j
|
||||
Z_3x3, Z_3x3,
|
||||
// dfV/dBias_j
|
||||
Z_3x3, Z_3x3,
|
||||
// dfR/dBias_j
|
||||
Z_3x3, Z_3x3,
|
||||
//dBiasAcc/dBias_j
|
||||
I_3x3, Z_3x3,
|
||||
//dBiasOmega/dBias_j
|
||||
Z_3x3, I_3x3;
|
||||
}
|
||||
|
||||
// Evaluate residual error, according to [3]
|
||||
/* ---------------------------------------------------------------------------------------------------- */
|
||||
const Vector3 fp =
|
||||
pos_j - pos_i
|
||||
- Rot_i.matrix() * (preintegratedMeasurements_.deltaPij_
|
||||
+ preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr
|
||||
+ preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr)
|
||||
- vel_i * deltaTij
|
||||
+ skewSymmetric(omegaCoriolis_) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper
|
||||
- 0.5 * gravity_ * deltaTij*deltaTij;
|
||||
|
||||
const Vector3 fv =
|
||||
vel_j - vel_i - Rot_i.matrix() * (preintegratedMeasurements_.deltaVij_
|
||||
+ preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr
|
||||
+ preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr)
|
||||
+ 2 * skewSymmetric(omegaCoriolis_) * vel_i * deltaTij // Coriolis term
|
||||
- gravity_ * deltaTij;
|
||||
|
||||
const Vector3 fR = Rot3::Logmap(fRhat);
|
||||
|
||||
const Vector3 fbiasAcc = bias_j.accelerometer() - bias_i.accelerometer();
|
||||
|
||||
const Vector3 fbiasOmega = bias_j.gyroscope() - bias_i.gyroscope();
|
||||
|
||||
Vector r(15); r << fp, fv, fR, fbiasAcc, fbiasOmega; // vector of size 15
|
||||
|
||||
return r;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
PoseVelocityBias CombinedImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i,
|
||||
const imuBias::ConstantBias& bias_i,
|
||||
const CombinedPreintegratedMeasurements& preintegratedMeasurements,
|
||||
const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis){
|
||||
|
||||
const double& deltaTij = preintegratedMeasurements.deltaTij_;
|
||||
const Vector3 biasAccIncr = bias_i.accelerometer() - preintegratedMeasurements.biasHat_.accelerometer();
|
||||
const Vector3 biasOmegaIncr = bias_i.gyroscope() - preintegratedMeasurements.biasHat_.gyroscope();
|
||||
|
||||
const Rot3 Rot_i = pose_i.rotation();
|
||||
const Vector3 pos_i = pose_i.translation().vector();
|
||||
|
||||
// Predict state at time j
|
||||
/* ---------------------------------------------------------------------------------------------------- */
|
||||
Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij_
|
||||
+ preintegratedMeasurements.delPdelBiasAcc_ * biasAccIncr
|
||||
+ preintegratedMeasurements.delPdelBiasOmega_ * biasOmegaIncr)
|
||||
+ vel_i * deltaTij
|
||||
- skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper
|
||||
+ 0.5 * gravity * deltaTij*deltaTij;
|
||||
|
||||
Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij_
|
||||
+ preintegratedMeasurements.delVdelBiasAcc_ * biasAccIncr
|
||||
+ preintegratedMeasurements.delVdelBiasOmega_ * biasOmegaIncr)
|
||||
- 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term
|
||||
+ gravity * deltaTij);
|
||||
|
||||
if(use2ndOrderCoriolis){
|
||||
pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij; // 2nd order coriolis term for position
|
||||
vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity
|
||||
}
|
||||
|
||||
const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij_.retract(preintegratedMeasurements.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP);
|
||||
// deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr)
|
||||
Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
|
||||
Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected -
|
||||
Rot_i.inverse().matrix() * omegaCoriolis * deltaTij; // Coriolis term
|
||||
const Rot3 deltaRij_biascorrected_corioliscorrected =
|
||||
Rot3::Expmap( theta_biascorrected_corioliscorrected );
|
||||
const Rot3 Rot_j = Rot_i.compose( deltaRij_biascorrected_corioliscorrected );
|
||||
|
||||
Pose3 pose_j = Pose3( Rot_j, Point3(pos_j) );
|
||||
|
||||
return PoseVelocityBias(pose_j, vel_j, bias_i);
|
||||
}
|
||||
|
||||
} /// namespace gtsam
|
||||
|
|
@ -11,40 +11,24 @@
|
|||
|
||||
/**
|
||||
* @file CombinedImuFactor.h
|
||||
* @author Luca Carlone, Stephen Williams, Richard Roberts, Vadim Indelman, David Jensen
|
||||
* @author Luca Carlone
|
||||
* @author Stephen Williams
|
||||
* @author Richard Roberts
|
||||
* @author Vadim Indelman
|
||||
* @author David Jensen
|
||||
* @author Frank Dellaert
|
||||
**/
|
||||
|
||||
#pragma once
|
||||
|
||||
/* GTSAM includes */
|
||||
#include <gtsam/navigation/ImuBias.h>
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactor.h>
|
||||
#include <gtsam/linear/GaussianFactor.h>
|
||||
#include <gtsam/navigation/ImuBias.h>
|
||||
#include <gtsam/base/debug.h>
|
||||
|
||||
/* External or standard includes */
|
||||
#include <ostream>
|
||||
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* Struct to hold all state variables of CombinedImuFactor
|
||||
* returned by predict function
|
||||
*/
|
||||
struct PoseVelocityBias {
|
||||
Pose3 pose;
|
||||
Vector3 velocity;
|
||||
imuBias::ConstantBias bias;
|
||||
|
||||
PoseVelocityBias(const Pose3& _pose, const Vector3& _velocity,
|
||||
const imuBias::ConstantBias _bias) :
|
||||
pose(_pose), velocity(_velocity), bias(_bias) {
|
||||
}
|
||||
};
|
||||
|
||||
/**
|
||||
/**
|
||||
*
|
||||
* @addtogroup SLAM
|
||||
*
|
||||
|
|
@ -60,20 +44,47 @@ namespace gtsam {
|
|||
* [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: Computation of the Jacobian Matrices", Tech. Report, 2013.
|
||||
*/
|
||||
|
||||
class CombinedImuFactor: public NoiseModelFactor6<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias,imuBias::ConstantBias> {
|
||||
/**
|
||||
* Struct to hold all state variables of CombinedImuFactor returned by Predict function
|
||||
*/
|
||||
struct PoseVelocityBias {
|
||||
Pose3 pose;
|
||||
Vector3 velocity;
|
||||
imuBias::ConstantBias bias;
|
||||
|
||||
public:
|
||||
PoseVelocityBias(const Pose3& _pose, const Vector3& _velocity,
|
||||
const imuBias::ConstantBias _bias) :
|
||||
pose(_pose), velocity(_velocity), bias(_bias) {
|
||||
}
|
||||
};
|
||||
|
||||
/** Struct to store results of preintegrating IMU measurements. Can be build
|
||||
* incrementally so as to avoid costly integration at time of factor construction. */
|
||||
/**
|
||||
* CombinedImuFactor is a 6-ways factor involving previous state (pose and velocity of the vehicle, as well as bias
|
||||
* at previous time step), and current state (pose, velocity, bias at current time step). According to the
|
||||
* preintegration scheme proposed in [2], the CombinedImuFactor includes many IMU measurements, which are
|
||||
* "summarized" using the CombinedPreintegratedMeasurements class. There are 3 main differences wrt ImuFactor:
|
||||
* 1) The factor is 6-ways, meaning that it also involves both biases (previous and current time step).
|
||||
* Therefore, the factor internally imposes the biases to be slowly varying; in particular, the matrices
|
||||
* "biasAccCovariance" and "biasOmegaCovariance" described the random walk that models bias evolution.
|
||||
* 2) The preintegration covariance takes into account the noise in the bias estimate used for integration.
|
||||
* 3) The covariance matrix of the CombinedPreintegratedMeasurements preserves the correlation between the bias uncertainty
|
||||
* and the preintegrated measurements uncertainty.
|
||||
*/
|
||||
class CombinedImuFactor: public NoiseModelFactor6<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias,imuBias::ConstantBias> {
|
||||
public:
|
||||
|
||||
/** CombinedPreintegratedMeasurements accumulates (integrates) the IMU measurements (rotation rates and accelerations)
|
||||
* and the corresponding covariance matrix. The measurements are then used to build the Preintegrated IMU factor*/
|
||||
* and the corresponding covariance matrix. The measurements are then used to build the CombinedPreintegrated IMU factor (CombinedImuFactor).
|
||||
* Integration is done incrementally (ideally, one integrates the measurement as soon as it is received
|
||||
* from the IMU) so as to avoid costly integration at time of factor construction.
|
||||
*/
|
||||
class CombinedPreintegratedMeasurements {
|
||||
|
||||
friend class CombinedImuFactor;
|
||||
|
||||
protected:
|
||||
imuBias::ConstantBias biasHat_; ///< Acceleration and angular rate bias values used during preintegration
|
||||
Matrix measurementCovariance_; ///< (Raw measurements uncertainty) Covariance of the vector
|
||||
Eigen::Matrix<double,21,21> measurementCovariance_; ///< (Raw measurements uncertainty) Covariance of the vector
|
||||
///< [integrationError measuredAcc measuredOmega biasAccRandomWalk biasOmegaRandomWalk biasAccInit biasOmegaInit] in R^(21 x 21)
|
||||
|
||||
Vector3 deltaPij_; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, in [2]) (in frame i)
|
||||
|
|
@ -86,236 +97,53 @@ namespace gtsam {
|
|||
Matrix3 delVdelBiasAcc_; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias
|
||||
Matrix3 delVdelBiasOmega_; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias
|
||||
Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias
|
||||
Matrix PreintMeasCov_; ///< Covariance matrix of the preintegrated measurements (first-order propagation from *measurementCovariance*)
|
||||
bool use2ndOrderIntegration_; ///< Controls the order of integration
|
||||
// public:
|
||||
///< In the combined factor is also includes the biases and keeps the correlation between the preintegrated measurements and the biases
|
||||
|
||||
Eigen::Matrix<double,15,15> PreintMeasCov_; ///< Covariance matrix of the preintegrated measurements
|
||||
///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION BiasAcc BiasOmega]
|
||||
/** Default constructor, initialize with no IMU measurements */
|
||||
///< (first-order propagation from *measurementCovariance*). CombinedPreintegratedMeasurements also include the biases and keep the correlation
|
||||
///< between the preintegrated measurements and the biases
|
||||
|
||||
bool use2ndOrderIntegration_; ///< Controls the order of integration
|
||||
|
||||
public:
|
||||
CombinedPreintegratedMeasurements(
|
||||
const imuBias::ConstantBias& bias, ///< Current estimate of acceleration and rotation rate biases
|
||||
const Matrix3& measuredAccCovariance, ///< Covariance matrix of measuredAcc
|
||||
const Matrix3& measuredOmegaCovariance, ///< Covariance matrix of measuredAcc
|
||||
const Matrix3& integrationErrorCovariance, ///< Covariance matrix of measuredAcc
|
||||
const Matrix3& biasAccCovariance, ///< Covariance matrix of biasAcc (random walk describing BIAS evolution)
|
||||
const Matrix3& biasOmegaCovariance, ///< Covariance matrix of biasOmega (random walk describing BIAS evolution)
|
||||
const Matrix& biasAccOmegaInit, ///< Covariance of biasAcc & biasOmega when preintegrating measurements
|
||||
const bool use2ndOrderIntegration = false ///< Controls the order of integration
|
||||
///< (this allows to consider the uncertainty of the BIAS choice when integrating the measurements)
|
||||
) : biasHat_(bias), measurementCovariance_(21,21), deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()), deltaTij_(0.0),
|
||||
delPdelBiasAcc_(Matrix3::Zero()), delPdelBiasOmega_(Matrix3::Zero()),
|
||||
delVdelBiasAcc_(Matrix3::Zero()), delVdelBiasOmega_(Matrix3::Zero()),
|
||||
delRdelBiasOmega_(Matrix3::Zero()), PreintMeasCov_(Matrix::Zero(15,15)),
|
||||
use2ndOrderIntegration_(use2ndOrderIntegration)
|
||||
{
|
||||
// COVARIANCE OF: [Integration AccMeasurement OmegaMeasurement BiasAccRandomWalk BiasOmegaRandomWalk (BiasAccInit BiasOmegaInit)] SIZE (21x21)
|
||||
measurementCovariance_ << integrationErrorCovariance , Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(),
|
||||
Matrix3::Zero(), measuredAccCovariance, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(),
|
||||
Matrix3::Zero(), Matrix3::Zero(), measuredOmegaCovariance, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(),
|
||||
Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), biasAccCovariance, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(),
|
||||
Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), biasOmegaCovariance, Matrix3::Zero(), Matrix3::Zero(),
|
||||
Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), biasAccOmegaInit.block(0,0,3,3), biasAccOmegaInit.block(0,3,3,3),
|
||||
Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), biasAccOmegaInit.block(3,0,3,3), biasAccOmegaInit.block(3,3,3,3);
|
||||
|
||||
}
|
||||
/**
|
||||
* Default constructor, initializes the class with no measurements
|
||||
* @param bias Current estimate of acceleration and rotation rate biases
|
||||
* @param measuredAccCovariance Covariance matrix of measuredAcc
|
||||
* @param measuredOmegaCovariance Covariance matrix of measured Angular Rate
|
||||
* @param integrationErrorCovariance Covariance matrix of integration errors (velocity -> position)
|
||||
* @param biasAccCovariance Covariance matrix of biasAcc (random walk describing BIAS evolution)
|
||||
* @param biasOmegaCovariance Covariance matrix of biasOmega (random walk describing BIAS evolution)
|
||||
* @param biasAccOmegaInit Covariance of biasAcc & biasOmega when preintegrating measurements
|
||||
* @param use2ndOrderIntegration Controls the order of integration
|
||||
* (if false: p(t+1) = p(t) + v(t) deltaT ; if true: p(t+1) = p(t) + v(t) deltaT + 0.5 * acc(t) deltaT^2)
|
||||
*/
|
||||
CombinedPreintegratedMeasurements(const imuBias::ConstantBias& bias, const Matrix3& measuredAccCovariance,
|
||||
const Matrix3& measuredOmegaCovariance, const Matrix3& integrationErrorCovariance,
|
||||
const Matrix3& biasAccCovariance, const Matrix3& biasOmegaCovariance,
|
||||
const Matrix& biasAccOmegaInit, const bool use2ndOrderIntegration = false);
|
||||
|
||||
CombinedPreintegratedMeasurements() :
|
||||
biasHat_(imuBias::ConstantBias()), measurementCovariance_(21,21), deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()), deltaTij_(0.0),
|
||||
delPdelBiasAcc_(Matrix3::Zero()), delPdelBiasOmega_(Matrix3::Zero()),
|
||||
delVdelBiasAcc_(Matrix3::Zero()), delVdelBiasOmega_(Matrix3::Zero()),
|
||||
delRdelBiasOmega_(Matrix3::Zero()), PreintMeasCov_(Matrix::Zero(15,15)),
|
||||
use2ndOrderIntegration_(false) ///< Controls the order of integration
|
||||
{
|
||||
}
|
||||
/// print
|
||||
void print(const std::string& s = "Preintegrated Measurements:") const;
|
||||
|
||||
/** print */
|
||||
void print(const std::string& s = "Preintegrated Measurements:") const {
|
||||
std::cout << s << std::endl;
|
||||
biasHat_.print(" biasHat");
|
||||
std::cout << " deltaTij " << deltaTij_ << std::endl;
|
||||
std::cout << " deltaPij [ " << deltaPij_.transpose() << " ]" << std::endl;
|
||||
std::cout << " deltaVij [ " << deltaVij_.transpose() << " ]" << std::endl;
|
||||
deltaRij_.print(" deltaRij ");
|
||||
std::cout << " measurementCovariance [ " << measurementCovariance_ << " ]" << std::endl;
|
||||
std::cout << " PreintMeasCov [ " << PreintMeasCov_ << " ]" << std::endl;
|
||||
}
|
||||
/// equals
|
||||
bool equals(const CombinedPreintegratedMeasurements& expected, double tol=1e-9) const;
|
||||
|
||||
/** equals */
|
||||
bool equals(const CombinedPreintegratedMeasurements& expected, double tol=1e-9) const {
|
||||
return biasHat_.equals(expected.biasHat_, tol)
|
||||
&& equal_with_abs_tol(measurementCovariance_, expected.measurementCovariance_, tol)
|
||||
&& equal_with_abs_tol(deltaPij_, expected.deltaPij_, tol)
|
||||
&& equal_with_abs_tol(deltaVij_, expected.deltaVij_, tol)
|
||||
&& deltaRij_.equals(expected.deltaRij_, tol)
|
||||
&& std::fabs(deltaTij_ - expected.deltaTij_) < tol
|
||||
&& equal_with_abs_tol(delPdelBiasAcc_, expected.delPdelBiasAcc_, tol)
|
||||
&& equal_with_abs_tol(delPdelBiasOmega_, expected.delPdelBiasOmega_, tol)
|
||||
&& equal_with_abs_tol(delVdelBiasAcc_, expected.delVdelBiasAcc_, tol)
|
||||
&& equal_with_abs_tol(delVdelBiasOmega_, expected.delVdelBiasOmega_, tol)
|
||||
&& equal_with_abs_tol(delRdelBiasOmega_, expected.delRdelBiasOmega_, tol);
|
||||
}
|
||||
/// Re-initialize CombinedPreintegratedMeasurements
|
||||
void resetIntegration();
|
||||
|
||||
void resetIntegration(){
|
||||
deltaPij_ = Vector3::Zero();
|
||||
deltaVij_ = Vector3::Zero();
|
||||
deltaRij_ = Rot3();
|
||||
deltaTij_ = 0.0;
|
||||
delPdelBiasAcc_ = Matrix3::Zero();
|
||||
delPdelBiasOmega_ = Matrix3::Zero();
|
||||
delVdelBiasAcc_ = Matrix3::Zero();
|
||||
delVdelBiasOmega_ = Matrix3::Zero();
|
||||
delRdelBiasOmega_ = Matrix3::Zero();
|
||||
PreintMeasCov_ = Matrix::Zero(15,15);
|
||||
}
|
||||
/**
|
||||
* Add a single IMU measurement to the preintegration.
|
||||
* @param measuredAcc Measured acceleration (in body frame, as given by the sensor)
|
||||
* @param measuredOmega Measured angular velocity (as given by the sensor)
|
||||
* @param deltaT Time interval between two consecutive IMU measurements
|
||||
* @param body_P_sensor Optional sensor frame (pose of the IMU in the body frame)
|
||||
*/
|
||||
void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT,
|
||||
boost::optional<const Pose3&> body_P_sensor = boost::none);
|
||||
|
||||
/** Add a single IMU measurement to the preintegration. */
|
||||
void integrateMeasurement(
|
||||
const Vector3& measuredAcc, ///< Measured linear acceleration (in body frame)
|
||||
const Vector3& measuredOmega, ///< Measured angular velocity (in body frame)
|
||||
double deltaT, ///< Time step
|
||||
boost::optional<const Pose3&> body_P_sensor = boost::none ///< Sensor frame
|
||||
) {
|
||||
// NOTE: order is important here because each update uses old values, e.g., velocity and position updates are based on previous rotation estimate.
|
||||
// First we compensate the measurements for the bias: since we have only an estimate of the bias, the covariance includes the corresponding uncertainty
|
||||
Vector3 correctedAcc = biasHat_.correctAccelerometer(measuredAcc);
|
||||
Vector3 correctedOmega = biasHat_.correctGyroscope(measuredOmega);
|
||||
|
||||
// Then compensate for sensor-body displacement: we express the quantities (originally in the IMU frame) into the body frame
|
||||
if(body_P_sensor){
|
||||
Matrix3 body_R_sensor = body_P_sensor->rotation().matrix();
|
||||
correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame
|
||||
Matrix3 body_omega_body__cross = skewSymmetric(correctedOmega);
|
||||
correctedAcc = body_R_sensor * correctedAcc - body_omega_body__cross * body_omega_body__cross * body_P_sensor->translation().vector();
|
||||
// linear acceleration vector in the body frame
|
||||
}
|
||||
|
||||
const Vector3 theta_incr = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement
|
||||
const Rot3 Rincr = Rot3::Expmap(theta_incr); // rotation increment computed from the current rotation rate measurement
|
||||
const Matrix3 Jr_theta_incr = Rot3::rightJacobianExpMapSO3(theta_incr); // Right jacobian computed at theta_incr
|
||||
|
||||
// Update Jacobians
|
||||
/* ----------------------------------------------------------------------------------------------------------------------- */
|
||||
if(!use2ndOrderIntegration_){
|
||||
delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT;
|
||||
delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT;
|
||||
}else{
|
||||
delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - 0.5 * deltaRij_.matrix() * deltaT*deltaT;
|
||||
delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT - 0.5 * deltaRij_.matrix()
|
||||
* skewSymmetric(biasHat_.correctAccelerometer(measuredAcc)) * deltaT*deltaT * delRdelBiasOmega_;
|
||||
}
|
||||
|
||||
delVdelBiasAcc_ += -deltaRij_.matrix() * deltaT;
|
||||
delVdelBiasOmega_ += -deltaRij_.matrix() * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega_;
|
||||
delRdelBiasOmega_ = Rincr.inverse().matrix() * delRdelBiasOmega_ - Jr_theta_incr * deltaT;
|
||||
|
||||
// Update preintegrated measurements covariance: as in [2] we consider a first order propagation that
|
||||
// can be seen as a prediction phase in an EKF framework. In this implementation, contrarily to [2] we
|
||||
// consider the uncertainty of the bias selection and we keep correlation between biases and preintegrated measurements
|
||||
/* ----------------------------------------------------------------------------------------------------------------------- */
|
||||
Matrix3 Z_3x3 = Matrix3::Zero();
|
||||
Matrix3 I_3x3 = Matrix3::Identity();
|
||||
const Vector3 theta_i = Rot3::Logmap(deltaRij_); // parametrization of so(3)
|
||||
const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3(theta_i);
|
||||
|
||||
Rot3 Rot_j = deltaRij_ * Rincr;
|
||||
const Vector3 theta_j = Rot3::Logmap(Rot_j); // parametrization of so(3)
|
||||
const Matrix3 Jrinv_theta_j = Rot3::rightJacobianExpMapSO3inverse(theta_j);
|
||||
|
||||
// Single Jacobians to propagate covariance
|
||||
Matrix3 H_pos_pos = I_3x3;
|
||||
Matrix3 H_pos_vel = I_3x3 * deltaT;
|
||||
Matrix3 H_pos_angles = Z_3x3;
|
||||
|
||||
Matrix3 H_vel_pos = Z_3x3;
|
||||
Matrix3 H_vel_vel = I_3x3;
|
||||
Matrix3 H_vel_angles = - deltaRij_.matrix() * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT;
|
||||
// analytic expression corresponding to the following numerical derivative
|
||||
// Matrix H_vel_angles = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_vel, correctedOmega, correctedAcc, deltaT, _1, deltaVij), theta_i);
|
||||
Matrix3 H_vel_biasacc = - deltaRij_.matrix() * deltaT;
|
||||
|
||||
Matrix3 H_angles_pos = Z_3x3;
|
||||
Matrix3 H_angles_vel = Z_3x3;
|
||||
Matrix3 H_angles_angles = Jrinv_theta_j * Rincr.inverse().matrix() * Jr_theta_i;
|
||||
Matrix3 H_angles_biasomega =- Jrinv_theta_j * Jr_theta_incr * deltaT;
|
||||
// analytic expression corresponding to the following numerical derivative
|
||||
// Matrix H_angles_angles = numericalDerivative11<Vector3, Vector3>(boost::bind(&PreIntegrateIMUObservations_delta_angles, correctedOmega, deltaT, _1), thetaij);
|
||||
|
||||
// overall Jacobian wrt preintegrated measurements (df/dx)
|
||||
Matrix F(15,15);
|
||||
F << H_pos_pos, H_pos_vel, H_pos_angles, Z_3x3, Z_3x3,
|
||||
H_vel_pos, H_vel_vel, H_vel_angles, H_vel_biasacc, Z_3x3,
|
||||
H_angles_pos, H_angles_vel, H_angles_angles, Z_3x3, H_angles_biasomega,
|
||||
Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3,
|
||||
Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3;
|
||||
|
||||
|
||||
// first order uncertainty propagation
|
||||
// Optimized matrix multiplication (1/deltaT) * G * measurementCovariance * G.transpose()
|
||||
|
||||
Matrix G_measCov_Gt = Matrix::Zero(15,15);
|
||||
// BLOCK DIAGONAL TERMS
|
||||
G_measCov_Gt.block(0,0,3,3) = deltaT * measurementCovariance_.block(0,0,3,3);
|
||||
|
||||
G_measCov_Gt.block(3,3,3,3) = (1/deltaT) * (H_vel_biasacc) *
|
||||
(measurementCovariance_.block(3,3,3,3) + measurementCovariance_.block(15,15,3,3) ) *
|
||||
(H_vel_biasacc.transpose());
|
||||
|
||||
G_measCov_Gt.block(6,6,3,3) = (1/deltaT) * (H_angles_biasomega) *
|
||||
(measurementCovariance_.block(6,6,3,3) + measurementCovariance_.block(18,18,3,3) ) *
|
||||
(H_angles_biasomega.transpose());
|
||||
|
||||
G_measCov_Gt.block(9,9,3,3) = deltaT * measurementCovariance_.block(9,9,3,3);
|
||||
|
||||
G_measCov_Gt.block(12,12,3,3) = deltaT * measurementCovariance_.block(12,12,3,3);
|
||||
|
||||
// NEW OFF BLOCK DIAGONAL TERMS
|
||||
Matrix3 block23 = H_vel_biasacc * measurementCovariance_.block(18,15,3,3) * H_angles_biasomega.transpose();
|
||||
G_measCov_Gt.block(3,6,3,3) = block23;
|
||||
G_measCov_Gt.block(6,3,3,3) = block23.transpose();
|
||||
|
||||
PreintMeasCov_ = F * PreintMeasCov_ * F.transpose() + G_measCov_Gt;
|
||||
|
||||
// Update preintegrated measurements
|
||||
/* ----------------------------------------------------------------------------------------------------------------------- */
|
||||
if(!use2ndOrderIntegration_){
|
||||
deltaPij_ += deltaVij_ * deltaT;
|
||||
}else{
|
||||
deltaPij_ += deltaVij_ * deltaT + 0.5 * deltaRij_.matrix() * biasHat_.correctAccelerometer(measuredAcc) * deltaT*deltaT;
|
||||
}
|
||||
deltaVij_ += deltaRij_.matrix() * correctedAcc * deltaT;
|
||||
deltaRij_ = deltaRij_ * Rincr;
|
||||
deltaTij_ += deltaT;
|
||||
}
|
||||
|
||||
/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
|
||||
// This function is only used for test purposes (compare numerical derivatives wrt analytic ones)
|
||||
static inline Vector PreIntegrateIMUObservations_delta_vel(const Vector& msr_gyro_t, const Vector& msr_acc_t, const double msr_dt,
|
||||
const Vector3& delta_angles, const Vector& delta_vel_in_t0){
|
||||
|
||||
// Note: all delta terms refer to an IMU\sensor system at t0
|
||||
|
||||
Vector body_t_a_body = msr_acc_t;
|
||||
Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles);
|
||||
|
||||
return delta_vel_in_t0 + R_t_to_t0.matrix() * body_t_a_body * msr_dt;
|
||||
}
|
||||
|
||||
// This function is only used for test purposes (compare numerical derivatives wrt analytic ones)
|
||||
static inline Vector PreIntegrateIMUObservations_delta_angles(const Vector& msr_gyro_t, const double msr_dt,
|
||||
const Vector3& delta_angles){
|
||||
|
||||
// Note: all delta terms refer to an IMU\sensor system at t0
|
||||
|
||||
// Calculate the corrected measurements using the Bias object
|
||||
Vector body_t_omega_body= msr_gyro_t;
|
||||
|
||||
Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles);
|
||||
|
||||
R_t_to_t0 = R_t_to_t0 * Rot3::Expmap( body_t_omega_body*msr_dt );
|
||||
return Rot3::Logmap(R_t_to_t0);
|
||||
}
|
||||
/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
|
||||
/// methods to access class variables
|
||||
Matrix measurementCovariance() const {return measurementCovariance_;}
|
||||
Matrix deltaRij() const {return deltaRij_.matrix();}
|
||||
double deltaTij() const{return deltaTij_;}
|
||||
|
|
@ -329,6 +157,28 @@ namespace gtsam {
|
|||
Matrix delRdelBiasOmega() const{ return delRdelBiasOmega_;}
|
||||
Matrix PreintMeasCov() const { return PreintMeasCov_;}
|
||||
|
||||
/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
|
||||
// This function is only used for test purposes (compare numerical derivatives wrt analytic ones)
|
||||
static inline Vector PreIntegrateIMUObservations_delta_vel(const Vector& msr_gyro_t, const Vector& msr_acc_t, const double msr_dt,
|
||||
const Vector3& delta_angles, const Vector& delta_vel_in_t0){
|
||||
// Note: all delta terms refer to an IMU\sensor system at t0
|
||||
Vector body_t_a_body = msr_acc_t;
|
||||
Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles);
|
||||
return delta_vel_in_t0 + R_t_to_t0.matrix() * body_t_a_body * msr_dt;
|
||||
}
|
||||
|
||||
// This function is only used for test purposes (compare numerical derivatives wrt analytic ones)
|
||||
static inline Vector PreIntegrateIMUObservations_delta_angles(const Vector& msr_gyro_t, const double msr_dt,
|
||||
const Vector3& delta_angles){
|
||||
// Note: all delta terms refer to an IMU\sensor system at t0
|
||||
// Calculate the corrected measurements using the Bias object
|
||||
Vector body_t_omega_body= msr_gyro_t;
|
||||
Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles);
|
||||
R_t_to_t0 = R_t_to_t0 * Rot3::Expmap( body_t_omega_body*msr_dt );
|
||||
return Rot3::Logmap(R_t_to_t0);
|
||||
}
|
||||
/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
|
|
@ -348,7 +198,7 @@ namespace gtsam {
|
|||
}
|
||||
};
|
||||
|
||||
private:
|
||||
private:
|
||||
|
||||
typedef CombinedImuFactor This;
|
||||
typedef NoiseModelFactor6<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias,imuBias::ConstantBias> Base;
|
||||
|
|
@ -360,7 +210,7 @@ namespace gtsam {
|
|||
|
||||
bool use2ndOrderCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect
|
||||
|
||||
public:
|
||||
public:
|
||||
|
||||
/** Shorthand for a smart pointer to a factor */
|
||||
#if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5
|
||||
|
|
@ -370,67 +220,42 @@ namespace gtsam {
|
|||
#endif
|
||||
|
||||
/** Default constructor - only use for serialization */
|
||||
CombinedImuFactor() : preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix::Zero(6,6)) {}
|
||||
CombinedImuFactor();
|
||||
|
||||
/** Constructor */
|
||||
CombinedImuFactor(
|
||||
Key pose_i, ///< previous pose key
|
||||
Key vel_i, ///< previous velocity key
|
||||
Key pose_j, ///< current pose key
|
||||
Key vel_j, ///< current velocity key
|
||||
Key bias_i, ///< previous bias key
|
||||
Key bias_j, ///< current bias key
|
||||
const CombinedPreintegratedMeasurements& preintegratedMeasurements, ///< Preintegrated IMU measurements
|
||||
const Vector3& gravity, ///< gravity vector
|
||||
const Vector3& omegaCoriolis, ///< rotation rate of inertial frame
|
||||
boost::optional<const Pose3&> body_P_sensor = boost::none, ///< The Pose of the sensor frame in the body frame
|
||||
const bool use2ndOrderCoriolis = false ///< When true, the second-order term is used in the calculation of the Coriolis Effect
|
||||
) :
|
||||
Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.PreintMeasCov_), pose_i, vel_i, pose_j, vel_j, bias_i, bias_j),
|
||||
preintegratedMeasurements_(preintegratedMeasurements),
|
||||
gravity_(gravity),
|
||||
omegaCoriolis_(omegaCoriolis),
|
||||
body_P_sensor_(body_P_sensor),
|
||||
use2ndOrderCoriolis_(use2ndOrderCoriolis){
|
||||
}
|
||||
/**
|
||||
* Constructor
|
||||
* @param pose_i Previous pose key
|
||||
* @param vel_i Previous velocity key
|
||||
* @param pose_j Current pose key
|
||||
* @param vel_j Current velocity key
|
||||
* @param bias_i Previous bias key
|
||||
* @param bias_j Current bias key
|
||||
* @param CombinedPreintegratedMeasurements CombinedPreintegratedMeasurements IMU measurements
|
||||
* @param gravity Gravity vector expressed in the global frame
|
||||
* @param omegaCoriolis Rotation rate of the global frame w.r.t. an inertial frame
|
||||
* @param body_P_sensor Optional pose of the sensor frame in the body frame
|
||||
* @param use2ndOrderCoriolis When true, the second-order term is used in the calculation of the Coriolis Effect
|
||||
*/
|
||||
CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j,
|
||||
const CombinedPreintegratedMeasurements& preintegratedMeasurements,
|
||||
const Vector3& gravity, const Vector3& omegaCoriolis,
|
||||
boost::optional<const Pose3&> body_P_sensor = boost::none, const bool use2ndOrderCoriolis = false);
|
||||
|
||||
virtual ~CombinedImuFactor() {}
|
||||
|
||||
/// @return a deep copy of this factor
|
||||
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
|
||||
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
|
||||
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
|
||||
virtual gtsam::NonlinearFactor::shared_ptr clone() const;
|
||||
|
||||
/** implement functions needed for Testable */
|
||||
|
||||
/** print */
|
||||
virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
|
||||
std::cout << s << "CombinedImuFactor("
|
||||
<< keyFormatter(this->key1()) << ","
|
||||
<< keyFormatter(this->key2()) << ","
|
||||
<< keyFormatter(this->key3()) << ","
|
||||
<< keyFormatter(this->key4()) << ","
|
||||
<< keyFormatter(this->key5()) << ","
|
||||
<< keyFormatter(this->key6()) << ")\n";
|
||||
preintegratedMeasurements_.print(" preintegrated measurements:");
|
||||
std::cout << " gravity: [ " << gravity_.transpose() << " ]" << std::endl;
|
||||
std::cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << std::endl;
|
||||
this->noiseModel_->print(" noise model: ");
|
||||
if(this->body_P_sensor_)
|
||||
this->body_P_sensor_->print(" sensor pose in body frame: ");
|
||||
}
|
||||
/// print
|
||||
virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
|
||||
|
||||
/** equals */
|
||||
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
|
||||
const This *e = dynamic_cast<const This*> (&expected);
|
||||
return e != NULL && Base::equals(*e, tol)
|
||||
&& preintegratedMeasurements_.equals(e->preintegratedMeasurements_, tol)
|
||||
&& equal_with_abs_tol(gravity_, e->gravity_, tol)
|
||||
&& equal_with_abs_tol(omegaCoriolis_, e->omegaCoriolis_, tol)
|
||||
&& ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_)));
|
||||
}
|
||||
/// equals
|
||||
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const;
|
||||
|
||||
/** Access the preintegrated measurements. */
|
||||
|
||||
const CombinedPreintegratedMeasurements& preintegratedMeasurements() const {
|
||||
return preintegratedMeasurements_; }
|
||||
|
||||
|
|
@ -440,7 +265,7 @@ namespace gtsam {
|
|||
|
||||
/** implement functions needed to derive from Factor */
|
||||
|
||||
/** vector of errors */
|
||||
/// vector of errors
|
||||
Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j,
|
||||
const imuBias::ConstantBias& bias_i, const imuBias::ConstantBias& bias_j,
|
||||
boost::optional<Matrix&> H1 = boost::none,
|
||||
|
|
@ -448,263 +273,15 @@ namespace gtsam {
|
|||
boost::optional<Matrix&> H3 = boost::none,
|
||||
boost::optional<Matrix&> H4 = boost::none,
|
||||
boost::optional<Matrix&> H5 = boost::none,
|
||||
boost::optional<Matrix&> H6 = boost::none) const
|
||||
{
|
||||
boost::optional<Matrix&> H6 = boost::none) const;
|
||||
|
||||
const double& deltaTij = preintegratedMeasurements_.deltaTij_;
|
||||
const Vector3 biasAccIncr = bias_i.accelerometer() - preintegratedMeasurements_.biasHat_.accelerometer();
|
||||
const Vector3 biasOmegaIncr = bias_i.gyroscope() - preintegratedMeasurements_.biasHat_.gyroscope();
|
||||
|
||||
// we give some shorter name to rotations and translations
|
||||
const Rot3 Rot_i = pose_i.rotation();
|
||||
const Rot3 Rot_j = pose_j.rotation();
|
||||
const Vector3 pos_i = pose_i.translation().vector();
|
||||
const Vector3 pos_j = pose_j.translation().vector();
|
||||
|
||||
// We compute factor's Jacobians, according to [3]
|
||||
/* ---------------------------------------------------------------------------------------------------- */
|
||||
const Rot3 deltaRij_biascorrected = preintegratedMeasurements_.deltaRij_.retract(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP);
|
||||
// deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr)
|
||||
|
||||
Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
|
||||
|
||||
Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected -
|
||||
Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij; // Coriolis term
|
||||
|
||||
const Rot3 deltaRij_biascorrected_corioliscorrected =
|
||||
Rot3::Expmap( theta_biascorrected_corioliscorrected );
|
||||
|
||||
const Rot3 fRhat = deltaRij_biascorrected_corioliscorrected.between(Rot_i.between(Rot_j));
|
||||
|
||||
const Matrix3 Jr_theta_bcc = Rot3::rightJacobianExpMapSO3(theta_biascorrected_corioliscorrected);
|
||||
|
||||
const Matrix3 Jtheta = -Jr_theta_bcc * skewSymmetric(Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij);
|
||||
|
||||
const Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat));
|
||||
|
||||
/*
|
||||
(*H1) <<
|
||||
// dfP/dRi
|
||||
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij
|
||||
+ preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr),
|
||||
// dfP/dPi
|
||||
- Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij,
|
||||
// dfV/dRi
|
||||
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij
|
||||
+ preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr),
|
||||
// dfV/dPi
|
||||
skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij,
|
||||
// dfR/dRi
|
||||
Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta),
|
||||
// dfR/dPi
|
||||
Matrix3::Zero(),
|
||||
//dBiasAcc/dPi
|
||||
Matrix3::Zero(), Matrix3::Zero(),
|
||||
//dBiasOmega/dPi
|
||||
Matrix3::Zero(), Matrix3::Zero();
|
||||
*/
|
||||
if(H1) {
|
||||
H1->resize(15,6);
|
||||
|
||||
Matrix3 dfPdPi;
|
||||
Matrix3 dfVdPi;
|
||||
if(use2ndOrderCoriolis_){
|
||||
dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij;
|
||||
dfVdPi = skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij;
|
||||
}
|
||||
else{
|
||||
dfPdPi = - Rot_i.matrix();
|
||||
dfVdPi = Matrix3::Zero();
|
||||
}
|
||||
|
||||
(*H1) <<
|
||||
// dfP/dRi
|
||||
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij_
|
||||
+ preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr),
|
||||
// dfP/dPi
|
||||
dfPdPi,
|
||||
// dfV/dRi
|
||||
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij_
|
||||
+ preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr),
|
||||
// dfV/dPi
|
||||
dfVdPi,
|
||||
// dfR/dRi
|
||||
Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta),
|
||||
// dfR/dPi
|
||||
Matrix3::Zero(),
|
||||
//dBiasAcc/dPi
|
||||
Matrix3::Zero(), Matrix3::Zero(),
|
||||
//dBiasOmega/dPi
|
||||
Matrix3::Zero(), Matrix3::Zero();
|
||||
}
|
||||
|
||||
if(H2) {
|
||||
H2->resize(15,3);
|
||||
(*H2) <<
|
||||
// dfP/dVi
|
||||
- Matrix3::Identity() * deltaTij
|
||||
+ skewSymmetric(omegaCoriolis_) * deltaTij * deltaTij, // Coriolis term - we got rid of the 2 wrt ins paper
|
||||
// dfV/dVi
|
||||
- Matrix3::Identity()
|
||||
+ 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term
|
||||
// dfR/dVi
|
||||
Matrix3::Zero(),
|
||||
//dBiasAcc/dVi
|
||||
Matrix3::Zero(),
|
||||
//dBiasOmega/dVi
|
||||
Matrix3::Zero();
|
||||
}
|
||||
|
||||
if(H3) {
|
||||
|
||||
H3->resize(15,6);
|
||||
(*H3) <<
|
||||
// dfP/dPosej
|
||||
Matrix3::Zero(), Rot_j.matrix(),
|
||||
// dfV/dPosej
|
||||
Matrix::Zero(3,6),
|
||||
// dfR/dPosej
|
||||
Jrinv_fRhat * ( Matrix3::Identity() ), Matrix3::Zero(),
|
||||
//dBiasAcc/dPosej
|
||||
Matrix3::Zero(), Matrix3::Zero(),
|
||||
//dBiasOmega/dPosej
|
||||
Matrix3::Zero(), Matrix3::Zero();
|
||||
}
|
||||
|
||||
if(H4) {
|
||||
H4->resize(15,3);
|
||||
(*H4) <<
|
||||
// dfP/dVj
|
||||
Matrix3::Zero(),
|
||||
// dfV/dVj
|
||||
Matrix3::Identity(),
|
||||
// dfR/dVj
|
||||
Matrix3::Zero(),
|
||||
//dBiasAcc/dVj
|
||||
Matrix3::Zero(),
|
||||
//dBiasOmega/dVj
|
||||
Matrix3::Zero();
|
||||
}
|
||||
|
||||
if(H5) {
|
||||
const Matrix3 Jrinv_theta_bc = Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected);
|
||||
const Matrix3 Jr_JbiasOmegaIncr = Rot3::rightJacobianExpMapSO3(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr);
|
||||
const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega_;
|
||||
|
||||
H5->resize(15,6);
|
||||
(*H5) <<
|
||||
// dfP/dBias_i
|
||||
- Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasAcc_,
|
||||
- Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasOmega_,
|
||||
// dfV/dBias_i
|
||||
- Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasAcc_,
|
||||
- Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasOmega_,
|
||||
// dfR/dBias_i
|
||||
Matrix::Zero(3,3),
|
||||
Jrinv_fRhat * ( - fRhat.inverse().matrix() * JbiasOmega),
|
||||
//dBiasAcc/dBias_i
|
||||
-Matrix3::Identity(), Matrix3::Zero(),
|
||||
//dBiasOmega/dBias_i
|
||||
Matrix3::Zero(), -Matrix3::Identity();
|
||||
}
|
||||
|
||||
if(H6) {
|
||||
|
||||
H6->resize(15,6);
|
||||
(*H6) <<
|
||||
// dfP/dBias_j
|
||||
Matrix3::Zero(), Matrix3::Zero(),
|
||||
// dfV/dBias_j
|
||||
Matrix3::Zero(), Matrix3::Zero(),
|
||||
// dfR/dBias_j
|
||||
Matrix3::Zero(), Matrix3::Zero(),
|
||||
//dBiasAcc/dBias_j
|
||||
Matrix3::Identity(), Matrix3::Zero(),
|
||||
//dBiasOmega/dBias_j
|
||||
Matrix3::Zero(), Matrix3::Identity();
|
||||
}
|
||||
|
||||
// Evaluate residual error, according to [3]
|
||||
/* ---------------------------------------------------------------------------------------------------- */
|
||||
const Vector3 fp =
|
||||
pos_j - pos_i
|
||||
- Rot_i.matrix() * (preintegratedMeasurements_.deltaPij_
|
||||
+ preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr
|
||||
+ preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr)
|
||||
- vel_i * deltaTij
|
||||
+ skewSymmetric(omegaCoriolis_) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper
|
||||
- 0.5 * gravity_ * deltaTij*deltaTij;
|
||||
|
||||
const Vector3 fv =
|
||||
vel_j - vel_i - Rot_i.matrix() * (preintegratedMeasurements_.deltaVij_
|
||||
+ preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr
|
||||
+ preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr)
|
||||
+ 2 * skewSymmetric(omegaCoriolis_) * vel_i * deltaTij // Coriolis term
|
||||
- gravity_ * deltaTij;
|
||||
|
||||
const Vector3 fR = Rot3::Logmap(fRhat);
|
||||
|
||||
const Vector3 fbiasAcc = bias_j.accelerometer() - bias_i.accelerometer();
|
||||
|
||||
const Vector3 fbiasOmega = bias_j.gyroscope() - bias_i.gyroscope();
|
||||
|
||||
Vector r(15); r << fp, fv, fR, fbiasAcc, fbiasOmega; // vector of size 15
|
||||
|
||||
return r;
|
||||
}
|
||||
|
||||
|
||||
/** predicted states from IMU */
|
||||
/// predicted states from IMU
|
||||
static PoseVelocityBias Predict(const Pose3& pose_i, const Vector3& vel_i,
|
||||
const imuBias::ConstantBias& bias_i,
|
||||
const CombinedPreintegratedMeasurements& preintegratedMeasurements,
|
||||
const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional<const Pose3&> body_P_sensor = boost::none,
|
||||
const bool use2ndOrderCoriolis = false)
|
||||
{
|
||||
const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false);
|
||||
|
||||
const double& deltaTij = preintegratedMeasurements.deltaTij_;
|
||||
const Vector3 biasAccIncr = bias_i.accelerometer() - preintegratedMeasurements.biasHat_.accelerometer();
|
||||
const Vector3 biasOmegaIncr = bias_i.gyroscope() - preintegratedMeasurements.biasHat_.gyroscope();
|
||||
|
||||
const Rot3 Rot_i = pose_i.rotation();
|
||||
const Vector3 pos_i = pose_i.translation().vector();
|
||||
|
||||
// Predict state at time j
|
||||
/* ---------------------------------------------------------------------------------------------------- */
|
||||
Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij_
|
||||
+ preintegratedMeasurements.delPdelBiasAcc_ * biasAccIncr
|
||||
+ preintegratedMeasurements.delPdelBiasOmega_ * biasOmegaIncr)
|
||||
+ vel_i * deltaTij
|
||||
- skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper
|
||||
+ 0.5 * gravity * deltaTij*deltaTij;
|
||||
|
||||
Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij_
|
||||
+ preintegratedMeasurements.delVdelBiasAcc_ * biasAccIncr
|
||||
+ preintegratedMeasurements.delVdelBiasOmega_ * biasOmegaIncr)
|
||||
- 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term
|
||||
+ gravity * deltaTij);
|
||||
|
||||
if(use2ndOrderCoriolis){
|
||||
pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij; // 2nd order coriolis term for position
|
||||
vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity
|
||||
}
|
||||
|
||||
const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij_.retract(preintegratedMeasurements.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP);
|
||||
// deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr)
|
||||
Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
|
||||
Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected -
|
||||
Rot_i.inverse().matrix() * omegaCoriolis * deltaTij; // Coriolis term
|
||||
const Rot3 deltaRij_biascorrected_corioliscorrected =
|
||||
Rot3::Expmap( theta_biascorrected_corioliscorrected );
|
||||
const Rot3 Rot_j = Rot_i.compose( deltaRij_biascorrected_corioliscorrected );
|
||||
|
||||
Pose3 pose_j = Pose3( Rot_j, Point3(pos_j) );
|
||||
|
||||
return PoseVelocityBias(pose_j, vel_j, bias_i);
|
||||
}
|
||||
|
||||
|
||||
private:
|
||||
private:
|
||||
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
|
|
@ -717,8 +294,8 @@ namespace gtsam {
|
|||
ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_);
|
||||
ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
|
||||
}
|
||||
}; // \class CombinedImuFactor
|
||||
}; // class CombinedImuFactor
|
||||
|
||||
typedef CombinedImuFactor::CombinedPreintegratedMeasurements CombinedImuFactorPreintegratedMeasurements;
|
||||
typedef CombinedImuFactor::CombinedPreintegratedMeasurements CombinedImuFactorPreintegratedMeasurements;
|
||||
|
||||
} /// namespace gtsam
|
||||
|
|
|
|||
|
|
@ -0,0 +1,438 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 ImuFactor.cpp
|
||||
* @author Luca Carlone
|
||||
* @author Stephen Williams
|
||||
* @author Richard Roberts
|
||||
* @author Vadim Indelman
|
||||
* @author David Jensen
|
||||
* @author Frank Dellaert
|
||||
**/
|
||||
|
||||
#include <gtsam/navigation/ImuFactor.h>
|
||||
|
||||
/* External or standard includes */
|
||||
#include <ostream>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
using namespace std;
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
// Inner class PreintegratedMeasurements
|
||||
//------------------------------------------------------------------------------
|
||||
ImuFactor::PreintegratedMeasurements::PreintegratedMeasurements(
|
||||
const imuBias::ConstantBias& bias, const Matrix3& measuredAccCovariance,
|
||||
const Matrix3& measuredOmegaCovariance, const Matrix3& integrationErrorCovariance,
|
||||
const bool use2ndOrderIntegration) :
|
||||
biasHat_(bias), deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()),
|
||||
deltaRij_(Rot3()), deltaTij_(0.0),
|
||||
delPdelBiasAcc_(Z_3x3), delPdelBiasOmega_(Z_3x3),
|
||||
delVdelBiasAcc_(Z_3x3), delVdelBiasOmega_(Z_3x3),
|
||||
delRdelBiasOmega_(Z_3x3), use2ndOrderIntegration_(use2ndOrderIntegration)
|
||||
{
|
||||
measurementCovariance_.setZero();
|
||||
measurementCovariance_.block<3,3>(0,0) = integrationErrorCovariance;
|
||||
measurementCovariance_.block<3,3>(3,3) = measuredAccCovariance;
|
||||
measurementCovariance_.block<3,3>(6,6) = measuredOmegaCovariance;
|
||||
PreintMeasCov_.setZero(9,9);
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
void ImuFactor::PreintegratedMeasurements::print(const string& s) const {
|
||||
cout << s << endl;
|
||||
biasHat_.print(" biasHat");
|
||||
cout << " deltaTij " << deltaTij_ << endl;
|
||||
cout << " deltaPij [ " << deltaPij_.transpose() << " ]" << endl;
|
||||
cout << " deltaVij [ " << deltaVij_.transpose() << " ]" << endl;
|
||||
deltaRij_.print(" deltaRij ");
|
||||
cout << " measurementCovariance = \n [ " << measurementCovariance_ << " ]" << endl;
|
||||
cout << " PreintMeasCov = \n [ " << PreintMeasCov_ << " ]" << endl;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
bool ImuFactor::PreintegratedMeasurements::equals(const PreintegratedMeasurements& expected, double tol) const {
|
||||
return biasHat_.equals(expected.biasHat_, tol)
|
||||
&& equal_with_abs_tol(measurementCovariance_, expected.measurementCovariance_, tol)
|
||||
&& equal_with_abs_tol(deltaPij_, expected.deltaPij_, tol)
|
||||
&& equal_with_abs_tol(deltaVij_, expected.deltaVij_, tol)
|
||||
&& deltaRij_.equals(expected.deltaRij_, tol)
|
||||
&& fabs(deltaTij_ - expected.deltaTij_) < tol
|
||||
&& equal_with_abs_tol(delPdelBiasAcc_, expected.delPdelBiasAcc_, tol)
|
||||
&& equal_with_abs_tol(delPdelBiasOmega_, expected.delPdelBiasOmega_, tol)
|
||||
&& equal_with_abs_tol(delVdelBiasAcc_, expected.delVdelBiasAcc_, tol)
|
||||
&& equal_with_abs_tol(delVdelBiasOmega_, expected.delVdelBiasOmega_, tol)
|
||||
&& equal_with_abs_tol(delRdelBiasOmega_, expected.delRdelBiasOmega_, tol);
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
void ImuFactor::PreintegratedMeasurements::resetIntegration(){
|
||||
deltaPij_ = Vector3::Zero();
|
||||
deltaVij_ = Vector3::Zero();
|
||||
deltaRij_ = Rot3();
|
||||
deltaTij_ = 0.0;
|
||||
delPdelBiasAcc_ = Z_3x3;
|
||||
delPdelBiasOmega_ = Z_3x3;
|
||||
delVdelBiasAcc_ = Z_3x3;
|
||||
delVdelBiasOmega_ = Z_3x3;
|
||||
delRdelBiasOmega_ = Z_3x3;
|
||||
PreintMeasCov_.setZero();
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
void ImuFactor::PreintegratedMeasurements::integrateMeasurement(
|
||||
const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT,
|
||||
boost::optional<const Pose3&> body_P_sensor) {
|
||||
|
||||
// NOTE: order is important here because each update uses old values (i.e., we have to update
|
||||
// jacobians and covariances before updating preintegrated measurements).
|
||||
|
||||
// First we compensate the measurements for the bias
|
||||
Vector3 correctedAcc = biasHat_.correctAccelerometer(measuredAcc);
|
||||
Vector3 correctedOmega = biasHat_.correctGyroscope(measuredOmega);
|
||||
|
||||
// Then compensate for sensor-body displacement: we express the quantities (originally in the IMU frame) into the body frame
|
||||
if(body_P_sensor){
|
||||
Matrix3 body_R_sensor = body_P_sensor->rotation().matrix();
|
||||
correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame
|
||||
Matrix3 body_omega_body__cross = skewSymmetric(correctedOmega);
|
||||
correctedAcc = body_R_sensor * correctedAcc - body_omega_body__cross * body_omega_body__cross * body_P_sensor->translation().vector();
|
||||
// linear acceleration vector in the body frame
|
||||
}
|
||||
|
||||
const Vector3 theta_incr = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement
|
||||
const Rot3 Rincr = Rot3::Expmap(theta_incr); // rotation increment computed from the current rotation rate measurement
|
||||
|
||||
const Matrix3 Jr_theta_incr = Rot3::rightJacobianExpMapSO3(theta_incr); // Right jacobian computed at theta_incr
|
||||
|
||||
// Update Jacobians
|
||||
/* ----------------------------------------------------------------------------------------------------------------------- */
|
||||
if(!use2ndOrderIntegration_){
|
||||
delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT;
|
||||
delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT;
|
||||
}else{
|
||||
delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - 0.5 * deltaRij_.matrix() * deltaT*deltaT;
|
||||
delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT - 0.5 * deltaRij_.matrix()
|
||||
* skewSymmetric(biasHat_.correctAccelerometer(measuredAcc)) * deltaT*deltaT * delRdelBiasOmega_;
|
||||
}
|
||||
delVdelBiasAcc_ += -deltaRij_.matrix() * deltaT;
|
||||
delVdelBiasOmega_ += -deltaRij_.matrix() * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega_;
|
||||
delRdelBiasOmega_ = Rincr.inverse().matrix() * delRdelBiasOmega_ - Jr_theta_incr * deltaT;
|
||||
|
||||
// Update preintegrated measurements covariance
|
||||
// as in [2] we consider a first order propagation that can be seen as a prediction phase in an EKF framework
|
||||
/* ----------------------------------------------------------------------------------------------------------------------- */
|
||||
const Vector3 theta_i = Rot3::Logmap(deltaRij_); // parametrization of so(3)
|
||||
const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3(theta_i);
|
||||
|
||||
Rot3 Rot_j = deltaRij_ * Rincr;
|
||||
const Vector3 theta_j = Rot3::Logmap(Rot_j); // parametrization of so(3)
|
||||
const Matrix3 Jrinv_theta_j = Rot3::rightJacobianExpMapSO3inverse(theta_j);
|
||||
|
||||
Matrix H_pos_pos = I_3x3;
|
||||
Matrix H_pos_vel = I_3x3 * deltaT;
|
||||
Matrix H_pos_angles = Z_3x3;
|
||||
|
||||
Matrix H_vel_pos = Z_3x3;
|
||||
Matrix H_vel_vel = I_3x3;
|
||||
Matrix H_vel_angles = - deltaRij_.matrix() * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT;
|
||||
// analytic expression corresponding to the following numerical derivative
|
||||
// Matrix H_vel_angles = numericalDerivative11<Vector3, Vector3>(boost::bind(&PreIntegrateIMUObservations_delta_vel, correctedOmega, correctedAcc, deltaT, _1, deltaVij), theta_i);
|
||||
|
||||
Matrix H_angles_pos = Z_3x3;
|
||||
Matrix H_angles_vel = Z_3x3;
|
||||
Matrix H_angles_angles = Jrinv_theta_j * Rincr.inverse().matrix() * Jr_theta_i;
|
||||
// analytic expression corresponding to the following numerical derivative
|
||||
// Matrix H_angles_angles = numericalDerivative11<Vector3, Vector3>(boost::bind(&PreIntegrateIMUObservations_delta_angles, correctedOmega, deltaT, _1), thetaij);
|
||||
|
||||
// overall Jacobian wrt preintegrated measurements (df/dx)
|
||||
Matrix F(9,9);
|
||||
F << H_pos_pos, H_pos_vel, H_pos_angles,
|
||||
H_vel_pos, H_vel_vel, H_vel_angles,
|
||||
H_angles_pos, H_angles_vel, H_angles_angles;
|
||||
|
||||
// first order uncertainty propagation:
|
||||
// the deltaT allows to pass from continuous time noise to discrete time noise
|
||||
// measurementCovariance_discrete = measurementCovariance_contTime * (1/deltaT)
|
||||
// Gt * Qt * G =(approx)= measurementCovariance_discrete * deltaT^2 = measurementCovariance_contTime * deltaT
|
||||
PreintMeasCov_ = F * PreintMeasCov_ * F.transpose() + measurementCovariance_ * deltaT ;
|
||||
|
||||
// Extended version, without approximation: Gt * Qt * G =(approx)= measurementCovariance_contTime * deltaT
|
||||
// This in only kept for documentation.
|
||||
//
|
||||
// Matrix G(9,9);
|
||||
// G << I_3x3 * deltaT, Z_3x3, Z_3x3,
|
||||
// Z_3x3, deltaRij.matrix() * deltaT, Z_3x3,
|
||||
// Z_3x3, Z_3x3, Jrinv_theta_j * Jr_theta_incr * deltaT;
|
||||
//
|
||||
// PreintMeasCov = F * PreintMeasCov * F.transpose() + G * (1/deltaT) * measurementCovariance * G.transpose();
|
||||
|
||||
// Update preintegrated measurements (this has to be done after the update of covariances and jacobians!)
|
||||
/* ----------------------------------------------------------------------------------------------------------------------- */
|
||||
if(!use2ndOrderIntegration_){
|
||||
deltaPij_ += deltaVij_ * deltaT;
|
||||
}else{
|
||||
deltaPij_ += deltaVij_ * deltaT + 0.5 * deltaRij_.matrix() * biasHat_.correctAccelerometer(measuredAcc) * deltaT*deltaT;
|
||||
}
|
||||
deltaVij_ += deltaRij_.matrix() * correctedAcc * deltaT;
|
||||
deltaRij_ = deltaRij_ * Rincr;
|
||||
deltaTij_ += deltaT;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
// ImuFactor methods
|
||||
//------------------------------------------------------------------------------
|
||||
ImuFactor::ImuFactor() :
|
||||
preintegratedMeasurements_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3), use2ndOrderCoriolis_(false){}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
ImuFactor::ImuFactor(
|
||||
Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
|
||||
const PreintegratedMeasurements& preintegratedMeasurements,
|
||||
const Vector3& gravity, const Vector3& omegaCoriolis,
|
||||
boost::optional<const Pose3&> body_P_sensor,
|
||||
const bool use2ndOrderCoriolis) :
|
||||
Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.PreintMeasCov_), pose_i, vel_i, pose_j, vel_j, bias),
|
||||
preintegratedMeasurements_(preintegratedMeasurements),
|
||||
gravity_(gravity),
|
||||
omegaCoriolis_(omegaCoriolis),
|
||||
body_P_sensor_(body_P_sensor),
|
||||
use2ndOrderCoriolis_(use2ndOrderCoriolis){
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
gtsam::NonlinearFactor::shared_ptr ImuFactor::clone() const {
|
||||
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
|
||||
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
void ImuFactor::print(const string& s, const KeyFormatter& keyFormatter) const {
|
||||
cout << s << "ImuFactor("
|
||||
<< keyFormatter(this->key1()) << ","
|
||||
<< keyFormatter(this->key2()) << ","
|
||||
<< keyFormatter(this->key3()) << ","
|
||||
<< keyFormatter(this->key4()) << ","
|
||||
<< keyFormatter(this->key5()) << ")\n";
|
||||
preintegratedMeasurements_.print(" preintegrated measurements:");
|
||||
cout << " gravity: [ " << gravity_.transpose() << " ]" << endl;
|
||||
cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << endl;
|
||||
this->noiseModel_->print(" noise model: ");
|
||||
if(this->body_P_sensor_)
|
||||
this->body_P_sensor_->print(" sensor pose in body frame: ");
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
bool ImuFactor::equals(const NonlinearFactor& expected, double tol) const {
|
||||
const This *e = dynamic_cast<const This*> (&expected);
|
||||
return e != NULL && Base::equals(*e, tol)
|
||||
&& preintegratedMeasurements_.equals(e->preintegratedMeasurements_, tol)
|
||||
&& equal_with_abs_tol(gravity_, e->gravity_, tol)
|
||||
&& equal_with_abs_tol(omegaCoriolis_, e->omegaCoriolis_, tol)
|
||||
&& ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_)));
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j,
|
||||
const imuBias::ConstantBias& bias,
|
||||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2,
|
||||
boost::optional<Matrix&> H3, boost::optional<Matrix&> H4,
|
||||
boost::optional<Matrix&> H5) const
|
||||
{
|
||||
|
||||
const double& deltaTij = preintegratedMeasurements_.deltaTij_;
|
||||
const Vector3 biasAccIncr = bias.accelerometer() - preintegratedMeasurements_.biasHat_.accelerometer();
|
||||
const Vector3 biasOmegaIncr = bias.gyroscope() - preintegratedMeasurements_.biasHat_.gyroscope();
|
||||
|
||||
// we give some shorter name to rotations and translations
|
||||
const Rot3 Rot_i = pose_i.rotation();
|
||||
const Rot3 Rot_j = pose_j.rotation();
|
||||
const Vector3 pos_i = pose_i.translation().vector();
|
||||
const Vector3 pos_j = pose_j.translation().vector();
|
||||
|
||||
// We compute factor's Jacobians
|
||||
/* ---------------------------------------------------------------------------------------------------- */
|
||||
const Rot3 deltaRij_biascorrected = preintegratedMeasurements_.deltaRij_.retract(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP);
|
||||
// deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr)
|
||||
|
||||
Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
|
||||
|
||||
Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected -
|
||||
Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij; // Coriolis term
|
||||
|
||||
const Rot3 deltaRij_biascorrected_corioliscorrected =
|
||||
Rot3::Expmap( theta_biascorrected_corioliscorrected );
|
||||
|
||||
const Rot3 fRhat = deltaRij_biascorrected_corioliscorrected.between(Rot_i.between(Rot_j));
|
||||
|
||||
const Matrix3 Jr_theta_bcc = Rot3::rightJacobianExpMapSO3(theta_biascorrected_corioliscorrected);
|
||||
|
||||
const Matrix3 Jtheta = -Jr_theta_bcc * skewSymmetric(Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij);
|
||||
|
||||
const Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat));
|
||||
|
||||
if(H1) {
|
||||
H1->resize(9,6);
|
||||
|
||||
Matrix3 dfPdPi;
|
||||
Matrix3 dfVdPi;
|
||||
if(use2ndOrderCoriolis_){
|
||||
dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij;
|
||||
dfVdPi = skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij;
|
||||
}
|
||||
else{
|
||||
dfPdPi = - Rot_i.matrix();
|
||||
dfVdPi = Z_3x3;
|
||||
}
|
||||
|
||||
(*H1) <<
|
||||
// dfP/dRi
|
||||
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij_
|
||||
+ preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr),
|
||||
// dfP/dPi
|
||||
dfPdPi,
|
||||
// dfV/dRi
|
||||
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij_
|
||||
+ preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr),
|
||||
// dfV/dPi
|
||||
dfVdPi,
|
||||
// dfR/dRi
|
||||
Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta),
|
||||
// dfR/dPi
|
||||
Z_3x3;
|
||||
}
|
||||
|
||||
if(H2) {
|
||||
H2->resize(9,3);
|
||||
(*H2) <<
|
||||
// dfP/dVi
|
||||
- I_3x3 * deltaTij
|
||||
+ skewSymmetric(omegaCoriolis_) * deltaTij * deltaTij, // Coriolis term - we got rid of the 2 wrt ins paper
|
||||
// dfV/dVi
|
||||
- I_3x3
|
||||
+ 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term
|
||||
// dfR/dVi
|
||||
Z_3x3;
|
||||
}
|
||||
|
||||
if(H3) {
|
||||
H3->resize(9,6);
|
||||
(*H3) <<
|
||||
// dfP/dPosej
|
||||
Z_3x3, Rot_j.matrix(),
|
||||
// dfV/dPosej
|
||||
Matrix::Zero(3,6),
|
||||
// dfR/dPosej
|
||||
Jrinv_fRhat * ( I_3x3 ), Z_3x3;
|
||||
}
|
||||
|
||||
if(H4) {
|
||||
H4->resize(9,3);
|
||||
(*H4) <<
|
||||
// dfP/dVj
|
||||
Z_3x3,
|
||||
// dfV/dVj
|
||||
I_3x3,
|
||||
// dfR/dVj
|
||||
Z_3x3;
|
||||
}
|
||||
|
||||
if(H5) {
|
||||
const Matrix3 Jrinv_theta_bc = Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected);
|
||||
const Matrix3 Jr_JbiasOmegaIncr = Rot3::rightJacobianExpMapSO3(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr);
|
||||
const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega_;
|
||||
|
||||
H5->resize(9,6);
|
||||
(*H5) <<
|
||||
// dfP/dBias
|
||||
- Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasAcc_,
|
||||
- Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasOmega_,
|
||||
// dfV/dBias
|
||||
- Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasAcc_,
|
||||
- Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasOmega_,
|
||||
// dfR/dBias
|
||||
Matrix::Zero(3,3),
|
||||
Jrinv_fRhat * ( - fRhat.inverse().matrix() * JbiasOmega);
|
||||
}
|
||||
|
||||
// Evaluate residual error, according to [3]
|
||||
/* ---------------------------------------------------------------------------------------------------- */
|
||||
const Vector3 fp =
|
||||
pos_j - pos_i
|
||||
- Rot_i.matrix() * (preintegratedMeasurements_.deltaPij_
|
||||
+ preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr
|
||||
+ preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr)
|
||||
- vel_i * deltaTij
|
||||
+ skewSymmetric(omegaCoriolis_) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper
|
||||
- 0.5 * gravity_ * deltaTij*deltaTij;
|
||||
|
||||
const Vector3 fv =
|
||||
vel_j - vel_i - Rot_i.matrix() * (preintegratedMeasurements_.deltaVij_
|
||||
+ preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr
|
||||
+ preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr)
|
||||
+ 2 * skewSymmetric(omegaCoriolis_) * vel_i * deltaTij // Coriolis term
|
||||
- gravity_ * deltaTij;
|
||||
|
||||
const Vector3 fR = Rot3::Logmap(fRhat);
|
||||
|
||||
Vector r(9); r << fp, fv, fR;
|
||||
return r;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
PoseVelocity ImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i,
|
||||
const imuBias::ConstantBias& bias, const PreintegratedMeasurements preintegratedMeasurements,
|
||||
const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis)
|
||||
{
|
||||
|
||||
const double& deltaTij = preintegratedMeasurements.deltaTij_;
|
||||
const Vector3 biasAccIncr = bias.accelerometer() - preintegratedMeasurements.biasHat_.accelerometer();
|
||||
const Vector3 biasOmegaIncr = bias.gyroscope() - preintegratedMeasurements.biasHat_.gyroscope();
|
||||
|
||||
const Rot3 Rot_i = pose_i.rotation();
|
||||
const Vector3 pos_i = pose_i.translation().vector();
|
||||
|
||||
// Predict state at time j
|
||||
/* ---------------------------------------------------------------------------------------------------- */
|
||||
Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij_
|
||||
+ preintegratedMeasurements.delPdelBiasAcc_ * biasAccIncr
|
||||
+ preintegratedMeasurements.delPdelBiasOmega_ * biasOmegaIncr)
|
||||
+ vel_i * deltaTij
|
||||
- skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper
|
||||
+ 0.5 * gravity * deltaTij*deltaTij;
|
||||
|
||||
Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij_
|
||||
+ preintegratedMeasurements.delVdelBiasAcc_ * biasAccIncr
|
||||
+ preintegratedMeasurements.delVdelBiasOmega_ * biasOmegaIncr)
|
||||
- 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term
|
||||
+ gravity * deltaTij);
|
||||
|
||||
if(use2ndOrderCoriolis){
|
||||
pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij; // 2nd order coriolis term for position
|
||||
vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity
|
||||
}
|
||||
|
||||
const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij_.retract(preintegratedMeasurements.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP);
|
||||
// deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr)
|
||||
Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
|
||||
Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected -
|
||||
Rot_i.inverse().matrix() * omegaCoriolis * deltaTij; // Coriolis term
|
||||
const Rot3 deltaRij_biascorrected_corioliscorrected =
|
||||
Rot3::Expmap( theta_biascorrected_corioliscorrected );
|
||||
const Rot3 Rot_j = Rot_i.compose( deltaRij_biascorrected_corioliscorrected );
|
||||
|
||||
Pose3 pose_j = Pose3( Rot_j, Point3(pos_j) );
|
||||
return PoseVelocity(pose_j, vel_j);
|
||||
}
|
||||
|
||||
} /// namespace gtsam
|
||||
|
|
@ -11,35 +11,24 @@
|
|||
|
||||
/**
|
||||
* @file ImuFactor.h
|
||||
* @author Luca Carlone, Stephen Williams, Richard Roberts, Vadim Indelman, David Jensen
|
||||
* @author Luca Carlone
|
||||
* @author Stephen Williams
|
||||
* @author Richard Roberts
|
||||
* @author Vadim Indelman
|
||||
* @author David Jensen
|
||||
* @author Frank Dellaert
|
||||
**/
|
||||
|
||||
#pragma once
|
||||
|
||||
/* GTSAM includes */
|
||||
#include <gtsam/nonlinear/NonlinearFactor.h>
|
||||
#include <gtsam/linear/GaussianFactor.h>
|
||||
#include <gtsam/navigation/ImuBias.h>
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/base/debug.h>
|
||||
|
||||
/* External or standard includes */
|
||||
#include <ostream>
|
||||
|
||||
|
||||
namespace gtsam {
|
||||
/**
|
||||
* Struct to hold return variables by the Predict Function
|
||||
*/
|
||||
struct PoseVelocity {
|
||||
Pose3 pose;
|
||||
Vector3 velocity;
|
||||
PoseVelocity(const Pose3& _pose, const Vector3& _velocity) :
|
||||
pose(_pose), velocity(_velocity) {
|
||||
}
|
||||
};
|
||||
|
||||
/**
|
||||
/**
|
||||
*
|
||||
* @addtogroup SLAM
|
||||
*
|
||||
|
|
@ -55,19 +44,42 @@ struct PoseVelocity {
|
|||
* [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: Computation of the Jacobian Matrices", Tech. Report, 2013.
|
||||
*/
|
||||
|
||||
class ImuFactor: public NoiseModelFactor5<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias> {
|
||||
public:
|
||||
/**
|
||||
* Struct to hold return variables by the Predict Function
|
||||
*/
|
||||
struct PoseVelocity {
|
||||
Pose3 pose;
|
||||
Vector3 velocity;
|
||||
PoseVelocity(const Pose3& _pose, const Vector3& _velocity) :
|
||||
pose(_pose), velocity(_velocity) {
|
||||
}
|
||||
};
|
||||
|
||||
/** Struct to store results of preintegrating IMU measurements. Can be build
|
||||
* incrementally so as to avoid costly integration at time of factor construction. */
|
||||
/**
|
||||
* ImuFactor is a 5-ways factor involving previous state (pose and velocity of the vehicle at previous time step),
|
||||
* current state (pose and velocity at current time step), and the bias estimate. According to the
|
||||
* preintegration scheme proposed in [2], the ImuFactor includes many IMU measurements, which are
|
||||
* "summarized" using the PreintegratedMeasurements class.
|
||||
* Note that this factor does not force "temporal consistency" of the biases (which are usually
|
||||
* slowly varying quantities), see also CombinedImuFactor for more details.
|
||||
*/
|
||||
class ImuFactor: public NoiseModelFactor5<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias> {
|
||||
public:
|
||||
|
||||
/** CombinedPreintegratedMeasurements accumulates (integrates) the IMU measurements (rotation rates and accelerations)
|
||||
* and the corresponding covariance matrix. The measurements are then used to build the Preintegrated IMU factor*/
|
||||
/**
|
||||
* PreintegratedMeasurements accumulates (integrates) the IMU measurements
|
||||
* (rotation rates and accelerations) and the corresponding covariance matrix.
|
||||
* The measurements are then used to build the Preintegrated IMU factor (ImuFactor).
|
||||
* Integration is done incrementally (ideally, one integrates the measurement as soon as it is received
|
||||
* from the IMU) so as to avoid costly integration at time of factor construction.
|
||||
*/
|
||||
class PreintegratedMeasurements {
|
||||
|
||||
friend class ImuFactor;
|
||||
|
||||
protected:
|
||||
imuBias::ConstantBias biasHat_; ///< Acceleration and angular rate bias values used during preintegration
|
||||
Matrix measurementCovariance_; ///< (continuous-time uncertainty) Covariance of the vector [integrationError measuredAcc measuredOmega] in R^(9X9)
|
||||
Eigen::Matrix<double,9,9> measurementCovariance_; ///< (continuous-time uncertainty) "Covariance" of the vector [integrationError measuredAcc measuredOmega] in R^(9X9)
|
||||
|
||||
Vector3 deltaPij_; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, in [2]) (in frame i)
|
||||
Vector3 deltaVij_; ///< Preintegrated relative velocity (in global frame)
|
||||
|
|
@ -79,63 +91,47 @@ struct PoseVelocity {
|
|||
Matrix3 delVdelBiasAcc_; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias
|
||||
Matrix3 delVdelBiasOmega_; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias
|
||||
Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias
|
||||
Matrix PreintMeasCov_; ///< Covariance matrix of the preintegrated measurements (first-order propagation from *measurementCovariance*)
|
||||
|
||||
Eigen::Matrix<double,9,9> PreintMeasCov_; ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION]
|
||||
///< (first-order propagation from *measurementCovariance*).
|
||||
|
||||
bool use2ndOrderIntegration_; ///< Controls the order of integration
|
||||
|
||||
public:
|
||||
/** Default constructor, initialize with no IMU measurements */
|
||||
PreintegratedMeasurements(
|
||||
const imuBias::ConstantBias& bias, ///< Current estimate of acceleration and rotation rate biases
|
||||
const Matrix3& measuredAccCovariance, ///< Covariance matrix of measuredAcc
|
||||
const Matrix3& measuredOmegaCovariance, ///< Covariance matrix of measured Angular Rate
|
||||
const Matrix3& integrationErrorCovariance, ///< Covariance matrix of integration errors
|
||||
const bool use2ndOrderIntegration = false ///< Controls the order of integration
|
||||
) : biasHat_(bias), measurementCovariance_(9,9), deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()), deltaTij_(0.0),
|
||||
delPdelBiasAcc_(Matrix3::Zero()), delPdelBiasOmega_(Matrix3::Zero()),
|
||||
delVdelBiasAcc_(Matrix3::Zero()), delVdelBiasOmega_(Matrix3::Zero()),
|
||||
delRdelBiasOmega_(Matrix3::Zero()), PreintMeasCov_(9,9), use2ndOrderIntegration_(use2ndOrderIntegration)
|
||||
{
|
||||
measurementCovariance_ << integrationErrorCovariance , Matrix3::Zero(), Matrix3::Zero(),
|
||||
Matrix3::Zero(), measuredAccCovariance, Matrix3::Zero(),
|
||||
Matrix3::Zero(), Matrix3::Zero(), measuredOmegaCovariance;
|
||||
PreintMeasCov_ = Matrix::Zero(9,9);
|
||||
}
|
||||
|
||||
PreintegratedMeasurements() :
|
||||
biasHat_(imuBias::ConstantBias()), measurementCovariance_(9,9), deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()), deltaTij_(0.0),
|
||||
delPdelBiasAcc_(Matrix3::Zero()), delPdelBiasOmega_(Matrix3::Zero()),
|
||||
delVdelBiasAcc_(Matrix3::Zero()), delVdelBiasOmega_(Matrix3::Zero()),
|
||||
delRdelBiasOmega_(Matrix3::Zero()), PreintMeasCov_(9,9), use2ndOrderIntegration_(false)
|
||||
{
|
||||
measurementCovariance_ = Matrix::Zero(9,9);
|
||||
PreintMeasCov_ = Matrix::Zero(9,9);
|
||||
}
|
||||
/**
|
||||
* Default constructor, initializes the class with no measurements
|
||||
* @param bias Current estimate of acceleration and rotation rate biases
|
||||
* @param measuredAccCovariance Covariance matrix of measuredAcc
|
||||
* @param measuredOmegaCovariance Covariance matrix of measured Angular Rate
|
||||
* @param integrationErrorCovariance Covariance matrix of integration errors (velocity -> position)
|
||||
* @param use2ndOrderIntegration Controls the order of integration
|
||||
* (if false: p(t+1) = p(t) + v(t) deltaT ; if true: p(t+1) = p(t) + v(t) deltaT + 0.5 * acc(t) deltaT^2)
|
||||
*/
|
||||
PreintegratedMeasurements(const imuBias::ConstantBias& bias,
|
||||
const Matrix3& measuredAccCovariance, const Matrix3& measuredOmegaCovariance,
|
||||
const Matrix3& integrationErrorCovariance, const bool use2ndOrderIntegration = false);
|
||||
|
||||
/** print */
|
||||
void print(const std::string& s = "Preintegrated Measurements:") const {
|
||||
std::cout << s << std::endl;
|
||||
biasHat_.print(" biasHat");
|
||||
std::cout << " deltaTij " << deltaTij_ << std::endl;
|
||||
std::cout << " deltaPij [ " << deltaPij_.transpose() << " ]" << std::endl;
|
||||
std::cout << " deltaVij [ " << deltaVij_.transpose() << " ]" << std::endl;
|
||||
deltaRij_.print(" deltaRij ");
|
||||
std::cout << " measurementCovariance [ " << measurementCovariance_ << " ]" << std::endl;
|
||||
std::cout << " PreintMeasCov [ " << PreintMeasCov_ << " ]" << std::endl;
|
||||
}
|
||||
/// print
|
||||
void print(const std::string& s = "Preintegrated Measurements:") const;
|
||||
|
||||
/** equals */
|
||||
bool equals(const PreintegratedMeasurements& expected, double tol=1e-9) const {
|
||||
return biasHat_.equals(expected.biasHat_, tol)
|
||||
&& equal_with_abs_tol(measurementCovariance_, expected.measurementCovariance_, tol)
|
||||
&& equal_with_abs_tol(deltaPij_, expected.deltaPij_, tol)
|
||||
&& equal_with_abs_tol(deltaVij_, expected.deltaVij_, tol)
|
||||
&& deltaRij_.equals(expected.deltaRij_, tol)
|
||||
&& std::fabs(deltaTij_ - expected.deltaTij_) < tol
|
||||
&& equal_with_abs_tol(delPdelBiasAcc_, expected.delPdelBiasAcc_, tol)
|
||||
&& equal_with_abs_tol(delPdelBiasOmega_, expected.delPdelBiasOmega_, tol)
|
||||
&& equal_with_abs_tol(delVdelBiasAcc_, expected.delVdelBiasAcc_, tol)
|
||||
&& equal_with_abs_tol(delVdelBiasOmega_, expected.delVdelBiasOmega_, tol)
|
||||
&& equal_with_abs_tol(delRdelBiasOmega_, expected.delRdelBiasOmega_, tol);
|
||||
}
|
||||
/// equals
|
||||
bool equals(const PreintegratedMeasurements& expected, double tol=1e-9) const;
|
||||
|
||||
/// Re-initialize PreintegratedMeasurements
|
||||
void resetIntegration();
|
||||
|
||||
/**
|
||||
* Add a single IMU measurement to the preintegration.
|
||||
* @param measuredAcc Measured acceleration (in body frame, as given by the sensor)
|
||||
* @param measuredOmega Measured angular velocity (as given by the sensor)
|
||||
* @param deltaT Time interval between two consecutive IMU measurements
|
||||
* @param body_P_sensor Optional sensor frame (pose of the IMU in the body frame)
|
||||
*/
|
||||
void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT,
|
||||
boost::optional<const Pose3&> body_P_sensor = boost::none);
|
||||
|
||||
/// methods to access class variables
|
||||
Matrix measurementCovariance() const {return measurementCovariance_;}
|
||||
Matrix deltaRij() const {return deltaRij_.matrix();}
|
||||
double deltaTij() const{return deltaTij_;}
|
||||
|
|
@ -149,147 +145,23 @@ struct PoseVelocity {
|
|||
Matrix delRdelBiasOmega() const{ return delRdelBiasOmega_;}
|
||||
Matrix preintMeasCov() const { return PreintMeasCov_;}
|
||||
|
||||
|
||||
void resetIntegration(){
|
||||
deltaPij_ = Vector3::Zero();
|
||||
deltaVij_ = Vector3::Zero();
|
||||
deltaRij_ = Rot3();
|
||||
deltaTij_ = 0.0;
|
||||
delPdelBiasAcc_ = Matrix3::Zero();
|
||||
delPdelBiasOmega_ = Matrix3::Zero();
|
||||
delVdelBiasAcc_ = Matrix3::Zero();
|
||||
delVdelBiasOmega_ = Matrix3::Zero();
|
||||
delRdelBiasOmega_ = Matrix3::Zero();
|
||||
PreintMeasCov_ = Matrix::Zero(9,9);
|
||||
}
|
||||
|
||||
/** Add a single IMU measurement to the preintegration. */
|
||||
void integrateMeasurement(
|
||||
const Vector3& measuredAcc, ///< Measured linear acceleration (in body frame)
|
||||
const Vector3& measuredOmega, ///< Measured angular velocity (in body frame)
|
||||
double deltaT, ///< Time step
|
||||
boost::optional<const Pose3&> body_P_sensor = boost::none ///< Sensor frame
|
||||
) {
|
||||
|
||||
// NOTE: order is important here because each update uses old values.
|
||||
// First we compensate the measurements for the bias
|
||||
Vector3 correctedAcc = biasHat_.correctAccelerometer(measuredAcc);
|
||||
Vector3 correctedOmega = biasHat_.correctGyroscope(measuredOmega);
|
||||
|
||||
// Then compensate for sensor-body displacement: we express the quantities (originally in the IMU frame) into the body frame
|
||||
if(body_P_sensor){
|
||||
Matrix3 body_R_sensor = body_P_sensor->rotation().matrix();
|
||||
correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame
|
||||
Matrix3 body_omega_body__cross = skewSymmetric(correctedOmega);
|
||||
correctedAcc = body_R_sensor * correctedAcc - body_omega_body__cross * body_omega_body__cross * body_P_sensor->translation().vector();
|
||||
// linear acceleration vector in the body frame
|
||||
}
|
||||
|
||||
const Vector3 theta_incr = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement
|
||||
const Rot3 Rincr = Rot3::Expmap(theta_incr); // rotation increment computed from the current rotation rate measurement
|
||||
|
||||
const Matrix3 Jr_theta_incr = Rot3::rightJacobianExpMapSO3(theta_incr); // Right jacobian computed at theta_incr
|
||||
|
||||
// Update Jacobians
|
||||
/* ----------------------------------------------------------------------------------------------------------------------- */
|
||||
if(!use2ndOrderIntegration_){
|
||||
delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT;
|
||||
delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT;
|
||||
}else{
|
||||
delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - 0.5 * deltaRij_.matrix() * deltaT*deltaT;
|
||||
delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT - 0.5 * deltaRij_.matrix()
|
||||
* skewSymmetric(biasHat_.correctAccelerometer(measuredAcc)) * deltaT*deltaT * delRdelBiasOmega_;
|
||||
}
|
||||
delVdelBiasAcc_ += -deltaRij_.matrix() * deltaT;
|
||||
delVdelBiasOmega_ += -deltaRij_.matrix() * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega_;
|
||||
delRdelBiasOmega_ = Rincr.inverse().matrix() * delRdelBiasOmega_ - Jr_theta_incr * deltaT;
|
||||
|
||||
// Update preintegrated measurements covariance
|
||||
/* ----------------------------------------------------------------------------------------------------------------------- */
|
||||
Matrix3 Z_3x3 = Matrix3::Zero();
|
||||
Matrix3 I_3x3 = Matrix3::Identity();
|
||||
const Vector3 theta_i = Rot3::Logmap(deltaRij_); // parametrization of so(3)
|
||||
const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3(theta_i);
|
||||
|
||||
Rot3 Rot_j = deltaRij_ * Rincr;
|
||||
const Vector3 theta_j = Rot3::Logmap(Rot_j); // parametrization of so(3)
|
||||
const Matrix3 Jrinv_theta_j = Rot3::rightJacobianExpMapSO3inverse(theta_j);
|
||||
|
||||
// Update preintegrated measurements covariance: as in [2] we consider a first order propagation that
|
||||
// can be seen as a prediction phase in an EKF framework
|
||||
Matrix H_pos_pos = I_3x3;
|
||||
Matrix H_pos_vel = I_3x3 * deltaT;
|
||||
Matrix H_pos_angles = Z_3x3;
|
||||
|
||||
Matrix H_vel_pos = Z_3x3;
|
||||
Matrix H_vel_vel = I_3x3;
|
||||
Matrix H_vel_angles = - deltaRij_.matrix() * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT;
|
||||
// analytic expression corresponding to the following numerical derivative
|
||||
// Matrix H_vel_angles = numericalDerivative11<Vector3, Vector3>(boost::bind(&PreIntegrateIMUObservations_delta_vel, correctedOmega, correctedAcc, deltaT, _1, deltaVij), theta_i);
|
||||
|
||||
Matrix H_angles_pos = Z_3x3;
|
||||
Matrix H_angles_vel = Z_3x3;
|
||||
Matrix H_angles_angles = Jrinv_theta_j * Rincr.inverse().matrix() * Jr_theta_i;
|
||||
// analytic expression corresponding to the following numerical derivative
|
||||
// Matrix H_angles_angles = numericalDerivative11<Vector3, Vector3>(boost::bind(&PreIntegrateIMUObservations_delta_angles, correctedOmega, deltaT, _1), thetaij);
|
||||
|
||||
// overall Jacobian wrt preintegrated measurements (df/dx)
|
||||
Matrix F(9,9);
|
||||
F << H_pos_pos, H_pos_vel, H_pos_angles,
|
||||
H_vel_pos, H_vel_vel, H_vel_angles,
|
||||
H_angles_pos, H_angles_vel, H_angles_angles;
|
||||
|
||||
// first order uncertainty propagation:
|
||||
// the deltaT allows to pass from continuous time noise to discrete time noise
|
||||
// measurementCovariance_discrete = measurementCovariance_contTime * (1/deltaT)
|
||||
// Gt * Qt * G =(approx)= measurementCovariance_discrete * deltaT^2 = measurementCovariance_contTime * deltaT
|
||||
PreintMeasCov_ = F * PreintMeasCov_ * F.transpose() + measurementCovariance_ * deltaT ;
|
||||
|
||||
// Extended version, without approximation: Gt * Qt * G =(approx)= measurementCovariance_contTime * deltaT
|
||||
//
|
||||
// Matrix G(9,9);
|
||||
// G << I_3x3 * deltaT, Z_3x3, Z_3x3,
|
||||
// Z_3x3, deltaRij.matrix() * deltaT, Z_3x3,
|
||||
// Z_3x3, Z_3x3, Jrinv_theta_j * Jr_theta_incr * deltaT;
|
||||
//
|
||||
// PreintMeasCov = F * PreintMeasCov * F.transpose() + G * (1/deltaT) * measurementCovariance * G.transpose();
|
||||
|
||||
// Update preintegrated measurements
|
||||
/* ----------------------------------------------------------------------------------------------------------------------- */
|
||||
if(!use2ndOrderIntegration_){
|
||||
deltaPij_ += deltaVij_ * deltaT;
|
||||
}else{
|
||||
deltaPij_ += deltaVij_ * deltaT + 0.5 * deltaRij_.matrix() * biasHat_.correctAccelerometer(measuredAcc) * deltaT*deltaT;
|
||||
}
|
||||
deltaVij_ += deltaRij_.matrix() * correctedAcc * deltaT;
|
||||
deltaRij_ = deltaRij_ * Rincr;
|
||||
deltaTij_ += deltaT;
|
||||
}
|
||||
|
||||
/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
|
||||
// This function is only used for test purposes (compare numerical derivatives wrt analytic ones)
|
||||
static inline Vector PreIntegrateIMUObservations_delta_vel(const Vector& msr_gyro_t, const Vector& msr_acc_t, const double msr_dt,
|
||||
const Vector3& delta_angles, const Vector& delta_vel_in_t0){
|
||||
|
||||
// Note: all delta terms refer to an IMU\sensor system at t0
|
||||
|
||||
Vector body_t_a_body = msr_acc_t;
|
||||
Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles);
|
||||
|
||||
return delta_vel_in_t0 + R_t_to_t0.matrix() * body_t_a_body * msr_dt;
|
||||
}
|
||||
|
||||
// This function is only used for test purposes (compare numerical derivatives wrt analytic ones)
|
||||
static inline Vector PreIntegrateIMUObservations_delta_angles(const Vector& msr_gyro_t, const double msr_dt,
|
||||
const Vector3& delta_angles){
|
||||
|
||||
// Note: all delta terms refer to an IMU\sensor system at t0
|
||||
|
||||
// Calculate the corrected measurements using the Bias object
|
||||
Vector body_t_omega_body= msr_gyro_t;
|
||||
|
||||
Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles);
|
||||
|
||||
R_t_to_t0 = R_t_to_t0 * Rot3::Expmap( body_t_omega_body*msr_dt );
|
||||
return Rot3::Logmap(R_t_to_t0);
|
||||
}
|
||||
|
|
@ -336,65 +208,41 @@ struct PoseVelocity {
|
|||
#endif
|
||||
|
||||
/** Default constructor - only use for serialization */
|
||||
ImuFactor() : preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()), use2ndOrderCoriolis_(false){}
|
||||
ImuFactor();
|
||||
|
||||
/** Constructor */
|
||||
ImuFactor(
|
||||
Key pose_i, ///< previous pose key
|
||||
Key vel_i, ///< previous velocity key
|
||||
Key pose_j, ///< current pose key
|
||||
Key vel_j, ///< current velocity key
|
||||
Key bias, ///< previous bias key
|
||||
const PreintegratedMeasurements& preintegratedMeasurements, ///< preintegrated IMU measurements
|
||||
const Vector3& gravity, ///< gravity vector
|
||||
const Vector3& omegaCoriolis, ///< rotation rate of the inertial frame
|
||||
boost::optional<const Pose3&> body_P_sensor = boost::none, ///< The Pose of the sensor frame in the body frame
|
||||
const bool use2ndOrderCoriolis = false ///< When true, the second-order term is used in the calculation of the Coriolis Effect
|
||||
) :
|
||||
Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.PreintMeasCov_), pose_i, vel_i, pose_j, vel_j, bias),
|
||||
preintegratedMeasurements_(preintegratedMeasurements),
|
||||
gravity_(gravity),
|
||||
omegaCoriolis_(omegaCoriolis),
|
||||
body_P_sensor_(body_P_sensor),
|
||||
use2ndOrderCoriolis_(use2ndOrderCoriolis){
|
||||
}
|
||||
/**
|
||||
* Constructor
|
||||
* @param pose_i Previous pose key
|
||||
* @param vel_i Previous velocity key
|
||||
* @param pose_j Current pose key
|
||||
* @param vel_j Current velocity key
|
||||
* @param bias Previous bias key
|
||||
* @param preintegratedMeasurements Preintegrated IMU measurements
|
||||
* @param gravity Gravity vector expressed in the global frame
|
||||
* @param omegaCoriolis Rotation rate of the global frame w.r.t. an inertial frame
|
||||
* @param body_P_sensor Optional pose of the sensor frame in the body frame
|
||||
* @param use2ndOrderCoriolis When true, the second-order term is used in the calculation of the Coriolis Effect
|
||||
*/
|
||||
ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
|
||||
const PreintegratedMeasurements& preintegratedMeasurements,
|
||||
const Vector3& gravity, const Vector3& omegaCoriolis,
|
||||
boost::optional<const Pose3&> body_P_sensor = boost::none, const bool use2ndOrderCoriolis = false);
|
||||
|
||||
virtual ~ImuFactor() {}
|
||||
|
||||
/// @return a deep copy of this factor
|
||||
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
|
||||
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
|
||||
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
|
||||
virtual gtsam::NonlinearFactor::shared_ptr clone() const;
|
||||
|
||||
/** implement functions needed for Testable */
|
||||
|
||||
/** print */
|
||||
virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
|
||||
std::cout << s << "ImuFactor("
|
||||
<< keyFormatter(this->key1()) << ","
|
||||
<< keyFormatter(this->key2()) << ","
|
||||
<< keyFormatter(this->key3()) << ","
|
||||
<< keyFormatter(this->key4()) << ","
|
||||
<< keyFormatter(this->key5()) << ")\n";
|
||||
preintegratedMeasurements_.print(" preintegrated measurements:");
|
||||
std::cout << " gravity: [ " << gravity_.transpose() << " ]" << std::endl;
|
||||
std::cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << std::endl;
|
||||
this->noiseModel_->print(" noise model: ");
|
||||
if(this->body_P_sensor_)
|
||||
this->body_P_sensor_->print(" sensor pose in body frame: ");
|
||||
}
|
||||
/// print
|
||||
virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
|
||||
|
||||
/** equals */
|
||||
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
|
||||
const This *e = dynamic_cast<const This*> (&expected);
|
||||
return e != NULL && Base::equals(*e, tol)
|
||||
&& preintegratedMeasurements_.equals(e->preintegratedMeasurements_, tol)
|
||||
&& equal_with_abs_tol(gravity_, e->gravity_, tol)
|
||||
&& equal_with_abs_tol(omegaCoriolis_, e->omegaCoriolis_, tol)
|
||||
&& ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_)));
|
||||
}
|
||||
/// equals
|
||||
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const;
|
||||
|
||||
/** Access the preintegrated measurements. */
|
||||
|
||||
const PreintegratedMeasurements& preintegratedMeasurements() const {
|
||||
return preintegratedMeasurements_; }
|
||||
|
||||
|
|
@ -404,205 +252,19 @@ struct PoseVelocity {
|
|||
|
||||
/** implement functions needed to derive from Factor */
|
||||
|
||||
/** vector of errors */
|
||||
/// vector of errors
|
||||
Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j,
|
||||
const imuBias::ConstantBias& bias,
|
||||
boost::optional<Matrix&> H1 = boost::none,
|
||||
boost::optional<Matrix&> H2 = boost::none,
|
||||
boost::optional<Matrix&> H3 = boost::none,
|
||||
boost::optional<Matrix&> H4 = boost::none,
|
||||
boost::optional<Matrix&> H5 = boost::none) const
|
||||
{
|
||||
boost::optional<Matrix&> H5 = boost::none) const;
|
||||
|
||||
const double& deltaTij = preintegratedMeasurements_.deltaTij_;
|
||||
const Vector3 biasAccIncr = bias.accelerometer() - preintegratedMeasurements_.biasHat_.accelerometer();
|
||||
const Vector3 biasOmegaIncr = bias.gyroscope() - preintegratedMeasurements_.biasHat_.gyroscope();
|
||||
|
||||
// we give some shorter name to rotations and translations
|
||||
const Rot3 Rot_i = pose_i.rotation();
|
||||
const Rot3 Rot_j = pose_j.rotation();
|
||||
const Vector3 pos_i = pose_i.translation().vector();
|
||||
const Vector3 pos_j = pose_j.translation().vector();
|
||||
|
||||
// We compute factor's Jacobians
|
||||
/* ---------------------------------------------------------------------------------------------------- */
|
||||
const Rot3 deltaRij_biascorrected = preintegratedMeasurements_.deltaRij_.retract(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP);
|
||||
// deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr)
|
||||
|
||||
Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
|
||||
|
||||
Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected -
|
||||
Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij; // Coriolis term
|
||||
|
||||
const Rot3 deltaRij_biascorrected_corioliscorrected =
|
||||
Rot3::Expmap( theta_biascorrected_corioliscorrected );
|
||||
|
||||
const Rot3 fRhat = deltaRij_biascorrected_corioliscorrected.between(Rot_i.between(Rot_j));
|
||||
|
||||
const Matrix3 Jr_theta_bcc = Rot3::rightJacobianExpMapSO3(theta_biascorrected_corioliscorrected);
|
||||
|
||||
const Matrix3 Jtheta = -Jr_theta_bcc * skewSymmetric(Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij);
|
||||
|
||||
const Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat));
|
||||
|
||||
if(H1) {
|
||||
H1->resize(9,6);
|
||||
|
||||
Matrix3 dfPdPi;
|
||||
Matrix3 dfVdPi;
|
||||
if(use2ndOrderCoriolis_){
|
||||
dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij;
|
||||
dfVdPi = skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij;
|
||||
}
|
||||
else{
|
||||
dfPdPi = - Rot_i.matrix();
|
||||
dfVdPi = Matrix3::Zero();
|
||||
}
|
||||
|
||||
(*H1) <<
|
||||
// dfP/dRi
|
||||
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij_
|
||||
+ preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr),
|
||||
// dfP/dPi
|
||||
dfPdPi,
|
||||
// dfV/dRi
|
||||
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij_
|
||||
+ preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr),
|
||||
// dfV/dPi
|
||||
dfVdPi,
|
||||
// dfR/dRi
|
||||
Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta),
|
||||
// dfR/dPi
|
||||
Matrix3::Zero();
|
||||
}
|
||||
|
||||
if(H2) {
|
||||
H2->resize(9,3);
|
||||
(*H2) <<
|
||||
// dfP/dVi
|
||||
- Matrix3::Identity() * deltaTij
|
||||
+ skewSymmetric(omegaCoriolis_) * deltaTij * deltaTij, // Coriolis term - we got rid of the 2 wrt ins paper
|
||||
// dfV/dVi
|
||||
- Matrix3::Identity()
|
||||
+ 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term
|
||||
// dfR/dVi
|
||||
Matrix3::Zero();
|
||||
}
|
||||
|
||||
if(H3) {
|
||||
|
||||
H3->resize(9,6);
|
||||
(*H3) <<
|
||||
// dfP/dPosej
|
||||
Matrix3::Zero(), Rot_j.matrix(),
|
||||
// dfV/dPosej
|
||||
Matrix::Zero(3,6),
|
||||
// dfR/dPosej
|
||||
Jrinv_fRhat * ( Matrix3::Identity() ), Matrix3::Zero();
|
||||
}
|
||||
|
||||
if(H4) {
|
||||
H4->resize(9,3);
|
||||
(*H4) <<
|
||||
// dfP/dVj
|
||||
Matrix3::Zero(),
|
||||
// dfV/dVj
|
||||
Matrix3::Identity(),
|
||||
// dfR/dVj
|
||||
Matrix3::Zero();
|
||||
}
|
||||
|
||||
if(H5) {
|
||||
|
||||
const Matrix3 Jrinv_theta_bc = Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected);
|
||||
const Matrix3 Jr_JbiasOmegaIncr = Rot3::rightJacobianExpMapSO3(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr);
|
||||
const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega_;
|
||||
|
||||
H5->resize(9,6);
|
||||
(*H5) <<
|
||||
// dfP/dBias
|
||||
- Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasAcc_,
|
||||
- Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasOmega_,
|
||||
// dfV/dBias
|
||||
- Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasAcc_,
|
||||
- Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasOmega_,
|
||||
// dfR/dBias
|
||||
Matrix::Zero(3,3),
|
||||
Jrinv_fRhat * ( - fRhat.inverse().matrix() * JbiasOmega);
|
||||
}
|
||||
|
||||
// Evaluate residual error, according to [3]
|
||||
/* ---------------------------------------------------------------------------------------------------- */
|
||||
const Vector3 fp =
|
||||
pos_j - pos_i
|
||||
- Rot_i.matrix() * (preintegratedMeasurements_.deltaPij_
|
||||
+ preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr
|
||||
+ preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr)
|
||||
- vel_i * deltaTij
|
||||
+ skewSymmetric(omegaCoriolis_) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper
|
||||
- 0.5 * gravity_ * deltaTij*deltaTij;
|
||||
|
||||
const Vector3 fv =
|
||||
vel_j - vel_i - Rot_i.matrix() * (preintegratedMeasurements_.deltaVij_
|
||||
+ preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr
|
||||
+ preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr)
|
||||
+ 2 * skewSymmetric(omegaCoriolis_) * vel_i * deltaTij // Coriolis term
|
||||
- gravity_ * deltaTij;
|
||||
|
||||
const Vector3 fR = Rot3::Logmap(fRhat);
|
||||
|
||||
Vector r(9); r << fp, fv, fR;
|
||||
return r;
|
||||
}
|
||||
|
||||
|
||||
/** predicted states from IMU */
|
||||
/// predicted states from IMU
|
||||
static PoseVelocity Predict(const Pose3& pose_i, const Vector3& vel_i,
|
||||
const imuBias::ConstantBias& bias, const PreintegratedMeasurements preintegratedMeasurements,
|
||||
const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional<const Pose3&> body_P_sensor = boost::none,
|
||||
const bool use2ndOrderCoriolis = false)
|
||||
{
|
||||
|
||||
const double& deltaTij = preintegratedMeasurements.deltaTij_;
|
||||
const Vector3 biasAccIncr = bias.accelerometer() - preintegratedMeasurements.biasHat_.accelerometer();
|
||||
const Vector3 biasOmegaIncr = bias.gyroscope() - preintegratedMeasurements.biasHat_.gyroscope();
|
||||
|
||||
const Rot3 Rot_i = pose_i.rotation();
|
||||
const Vector3 pos_i = pose_i.translation().vector();
|
||||
|
||||
// Predict state at time j
|
||||
/* ---------------------------------------------------------------------------------------------------- */
|
||||
Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij_
|
||||
+ preintegratedMeasurements.delPdelBiasAcc_ * biasAccIncr
|
||||
+ preintegratedMeasurements.delPdelBiasOmega_ * biasOmegaIncr)
|
||||
+ vel_i * deltaTij
|
||||
- skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper
|
||||
+ 0.5 * gravity * deltaTij*deltaTij;
|
||||
|
||||
Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij_
|
||||
+ preintegratedMeasurements.delVdelBiasAcc_ * biasAccIncr
|
||||
+ preintegratedMeasurements.delVdelBiasOmega_ * biasOmegaIncr)
|
||||
- 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term
|
||||
+ gravity * deltaTij);
|
||||
|
||||
if(use2ndOrderCoriolis){
|
||||
pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij; // 2nd order coriolis term for position
|
||||
vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity
|
||||
}
|
||||
|
||||
const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij_.retract(preintegratedMeasurements.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP);
|
||||
// deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr)
|
||||
Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
|
||||
Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected -
|
||||
Rot_i.inverse().matrix() * omegaCoriolis * deltaTij; // Coriolis term
|
||||
const Rot3 deltaRij_biascorrected_corioliscorrected =
|
||||
Rot3::Expmap( theta_biascorrected_corioliscorrected );
|
||||
const Rot3 Rot_j = Rot_i.compose( deltaRij_biascorrected_corioliscorrected );
|
||||
|
||||
Pose3 pose_j = Pose3( Rot_j, Point3(pos_j) );
|
||||
return PoseVelocity(pose_j, vel_j);
|
||||
}
|
||||
|
||||
const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false);
|
||||
|
||||
private:
|
||||
|
||||
|
|
@ -617,7 +279,7 @@ struct PoseVelocity {
|
|||
ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_);
|
||||
ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
|
||||
}
|
||||
}; // \class ImuFactor
|
||||
}; // class ImuFactor
|
||||
|
||||
typedef ImuFactor::PreintegratedMeasurements ImuFactorPreintegratedMeasurements;
|
||||
|
||||
|
|
|
|||
|
|
@ -10,19 +10,22 @@
|
|||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file testImuFactor.cpp
|
||||
* @brief Unit test for ImuFactor
|
||||
* @author Luca Carlone, Stephen Williams, Richard Roberts
|
||||
* @file testCombinedImuFactor.cpp
|
||||
* @brief Unit test for Lupton-style combined IMU factor
|
||||
* @author Luca Carlone
|
||||
* @author Stephen Williams
|
||||
* @author Richard Roberts
|
||||
*/
|
||||
|
||||
#include <gtsam/nonlinear/Values.h>
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
#include <gtsam/navigation/ImuFactor.h>
|
||||
#include <gtsam/navigation/CombinedImuFactor.h>
|
||||
#include <gtsam/navigation/ImuBias.h>
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/nonlinear/Values.h>
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
#include <gtsam/base/TestableAssertions.h>
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
#include <boost/bind.hpp>
|
||||
|
|
|
|||
|
|
@ -39,15 +39,13 @@ using symbol_shorthand::B;
|
|||
namespace {
|
||||
Vector callEvaluateError(const ImuFactor& factor,
|
||||
const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j,
|
||||
const imuBias::ConstantBias& bias)
|
||||
{
|
||||
const imuBias::ConstantBias& bias){
|
||||
return factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias);
|
||||
}
|
||||
|
||||
Rot3 evaluateRotationError(const ImuFactor& factor,
|
||||
const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j,
|
||||
const imuBias::ConstantBias& bias)
|
||||
{
|
||||
const imuBias::ConstantBias& bias){
|
||||
return Rot3::Expmap(factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias).tail(3) ) ;
|
||||
}
|
||||
|
||||
|
|
@ -56,9 +54,7 @@ ImuFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements(
|
|||
const list<Vector3>& measuredAccs,
|
||||
const list<Vector3>& measuredOmegas,
|
||||
const list<double>& deltaTs,
|
||||
const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0)
|
||||
)
|
||||
{
|
||||
const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) ){
|
||||
ImuFactor::PreintegratedMeasurements result(bias, Matrix3::Identity(),
|
||||
Matrix3::Identity(), Matrix3::Identity());
|
||||
|
||||
|
|
@ -68,7 +64,6 @@ ImuFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements(
|
|||
for( ; itAcc != measuredAccs.end(); ++itAcc, ++itOmega, ++itDeltaT) {
|
||||
result.integrateMeasurement(*itAcc, *itOmega, *itDeltaT);
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
|
|
@ -77,8 +72,7 @@ Vector3 evaluatePreintegratedMeasurementsPosition(
|
|||
const list<Vector3>& measuredAccs,
|
||||
const list<Vector3>& measuredOmegas,
|
||||
const list<double>& deltaTs,
|
||||
const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) )
|
||||
{
|
||||
const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) ){
|
||||
return evaluatePreintegratedMeasurements(bias,
|
||||
measuredAccs, measuredOmegas, deltaTs).deltaPij();
|
||||
}
|
||||
|
|
@ -99,20 +93,16 @@ Rot3 evaluatePreintegratedMeasurementsRotation(
|
|||
const list<Vector3>& measuredAccs,
|
||||
const list<Vector3>& measuredOmegas,
|
||||
const list<double>& deltaTs,
|
||||
const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) )
|
||||
{
|
||||
const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) ){
|
||||
return Rot3(evaluatePreintegratedMeasurements(bias,
|
||||
measuredAccs, measuredOmegas, deltaTs, initialRotationRate).deltaRij());
|
||||
}
|
||||
|
||||
Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega, const double deltaT)
|
||||
{
|
||||
Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega, const double deltaT){
|
||||
return Rot3::Expmap((measuredOmega - biasOmega) * deltaT);
|
||||
}
|
||||
|
||||
|
||||
Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta)
|
||||
{
|
||||
Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta){
|
||||
return Rot3::Logmap( Rot3::Expmap(thetahat).compose( Rot3::Expmap(deltatheta) ) );
|
||||
}
|
||||
|
||||
|
|
@ -212,7 +202,6 @@ TEST( ImuFactor, Error )
|
|||
Matrix H1a, H2a, H3a, H4a, H5a;
|
||||
(void) factor.evaluateError(x1, v1, x2, v2, bias, H1a, H2a, H3a, H4a, H5a);
|
||||
|
||||
|
||||
// positions and velocities
|
||||
Matrix H1etop6 = H1e.topRows(6);
|
||||
Matrix H1atop6 = H1a.topRows(6);
|
||||
|
|
@ -230,7 +219,7 @@ TEST( ImuFactor, Error )
|
|||
EXPECT(assert_equal(RH3e, H3a.bottomRows(3), 1e-5)); // 1e-5 needs to be added only when using quaternions for rotations
|
||||
|
||||
EXPECT(assert_equal(H4e, H4a));
|
||||
// EXPECT(assert_equal(H5e, H5a));
|
||||
// EXPECT(assert_equal(H5e, H5a));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -243,7 +232,6 @@ TEST( ImuFactor, ErrorWithBiases )
|
|||
// Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/10.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0));
|
||||
// Vector3 v2(Vector3(0.5, 0.0, 0.0));
|
||||
|
||||
|
||||
imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot)
|
||||
Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0));
|
||||
Vector3 v1(Vector3(0.5, 0.0, 0.0));
|
||||
|
|
@ -260,8 +248,8 @@ TEST( ImuFactor, ErrorWithBiases )
|
|||
ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero());
|
||||
pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
|
||||
|
||||
// ImuFactor::PreintegratedMeasurements pre_int_data(bias.head(3), bias.tail(3));
|
||||
// pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
|
||||
// ImuFactor::PreintegratedMeasurements pre_int_data(bias.head(3), bias.tail(3));
|
||||
// pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
|
||||
|
||||
// Create factor
|
||||
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis);
|
||||
|
|
@ -272,7 +260,7 @@ TEST( ImuFactor, ErrorWithBiases )
|
|||
|
||||
// Expected error
|
||||
Vector errorExpected(9); errorExpected << 0, 0, 0, 0, 0, 0, 0, 0, 0;
|
||||
// EXPECT(assert_equal(errorExpected, errorActual, 1e-6));
|
||||
// EXPECT(assert_equal(errorExpected, errorActual, 1e-6));
|
||||
|
||||
// Expected Jacobians
|
||||
Matrix H1e = numericalDerivative11<Vector,Pose3>(
|
||||
|
|
@ -315,7 +303,6 @@ TEST( ImuFactor, PartialDerivativeExpmap )
|
|||
Vector3 measuredOmega; measuredOmega << 0.1, 0, 0;
|
||||
double deltaT = 0.5;
|
||||
|
||||
|
||||
// Compute numerical derivatives
|
||||
Matrix expectedDelRdelBiasOmega = numericalDerivative11<Rot3, Vector3>(boost::bind(
|
||||
&evaluateRotation, measuredOmega, _1, deltaT), Vector3(biasOmega));
|
||||
|
|
@ -326,7 +313,6 @@ TEST( ImuFactor, PartialDerivativeExpmap )
|
|||
|
||||
// Compare Jacobians
|
||||
EXPECT(assert_equal(expectedDelRdelBiasOmega, actualdelRdelBiasOmega, 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations
|
||||
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -349,9 +335,6 @@ TEST( ImuFactor, PartialDerivativeLogmap )
|
|||
const Matrix3 actualDelFdeltheta = Matrix3::Identity() +
|
||||
0.5 * X + (1/(normx*normx) - (1+cos(normx))/(2*normx * sin(normx)) ) * X * X;
|
||||
|
||||
// std::cout << "actualDelFdeltheta" << actualDelFdeltheta << std::endl;
|
||||
// std::cout << "expectedDelFdeltheta" << expectedDelFdeltheta << std::endl;
|
||||
|
||||
// Compare Jacobians
|
||||
EXPECT(assert_equal(expectedDelFdeltheta, actualDelFdeltheta));
|
||||
|
||||
|
|
@ -495,7 +478,6 @@ TEST( ImuFactor, FirstOrderPreIntegratedMeasurements )
|
|||
// tictoc_print_();
|
||||
//}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement )
|
||||
{
|
||||
|
|
@ -515,15 +497,9 @@ TEST( ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement )
|
|||
|
||||
const Pose3 body_P_sensor(Rot3::Expmap(Vector3(0,0.10,0.10)), Point3(1,0,0));
|
||||
|
||||
// ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0),
|
||||
// Vector3(0.0, 0.0, 0.0)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), measuredOmega);
|
||||
|
||||
|
||||
ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0),
|
||||
Vector3(0.0, 0.0, 0.0)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero());
|
||||
|
||||
|
||||
|
||||
pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
|
||||
|
||||
// Create factor
|
||||
|
|
@ -560,6 +536,7 @@ TEST( ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement )
|
|||
EXPECT(assert_equal(H5e, H5a));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(ImuFactor, PredictPositionAndVelocity){
|
||||
imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot)
|
||||
|
||||
|
|
@ -593,6 +570,7 @@ TEST(ImuFactor, PredictPositionAndVelocity){
|
|||
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(ImuFactor, PredictRotation) {
|
||||
imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot)
|
||||
|
||||
|
|
|
|||
|
|
@ -18,7 +18,7 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam_unstable/nonlinear/ceres_autodiff.h>
|
||||
#include <gtsam/3rdparty/ceres/autodiff.h>
|
||||
#include <gtsam/base/Manifold.h>
|
||||
#include <gtsam/base/OptionalJacobian.h>
|
||||
|
||||
|
|
@ -109,7 +109,7 @@ struct UseBlockIf<false, Derived> {
|
|||
};
|
||||
}
|
||||
|
||||
/// Handle Leaf Case: reverseAD ends here, by writing a matrix into Jacobians
|
||||
/// Handle Leaf Case: reverse AD ends here, by writing a matrix into Jacobians
|
||||
template<typename Derived>
|
||||
void handleLeafCase(const Eigen::MatrixBase<Derived>& dTdA,
|
||||
JacobianMap& jacobians, Key key) {
|
||||
|
|
@ -188,28 +188,28 @@ public:
|
|||
}
|
||||
}
|
||||
/**
|
||||
* *** This is the main entry point for reverseAD, called from Expression ***
|
||||
* *** This is the main entry point for reverse AD, called from Expression ***
|
||||
* Called only once, either inserts I into Jacobians (Leaf) or starts AD (Function)
|
||||
*/
|
||||
typedef Eigen::Matrix<double, Dim, Dim> JacobianTT;
|
||||
void startReverseAD(JacobianMap& jacobians) const {
|
||||
void startReverseAD1(JacobianMap& jacobians) const {
|
||||
if (kind == Leaf) {
|
||||
// This branch will only be called on trivial Leaf expressions, i.e. Priors
|
||||
static const JacobianTT I = JacobianTT::Identity();
|
||||
handleLeafCase(I, jacobians, content.key);
|
||||
} else if (kind == Function)
|
||||
// This is the more typical entry point, starting the AD pipeline
|
||||
// Inside the startReverseAD that the correctly dimensioned pipeline is chosen.
|
||||
content.ptr->startReverseAD(jacobians);
|
||||
// Inside startReverseAD2 the correctly dimensioned pipeline is chosen.
|
||||
content.ptr->startReverseAD2(jacobians);
|
||||
}
|
||||
// Either add to Jacobians (Leaf) or propagate (Function)
|
||||
template<typename DerivedMatrix>
|
||||
void reverseAD(const Eigen::MatrixBase<DerivedMatrix> & dTdA,
|
||||
void reverseAD1(const Eigen::MatrixBase<DerivedMatrix> & dTdA,
|
||||
JacobianMap& jacobians) const {
|
||||
if (kind == Leaf)
|
||||
handleLeafCase(dTdA, jacobians, content.key);
|
||||
else if (kind == Function)
|
||||
content.ptr->reverseAD(dTdA, jacobians);
|
||||
content.ptr->reverseAD2(dTdA, jacobians);
|
||||
}
|
||||
|
||||
/// Define type so we can apply it as a meta-function
|
||||
|
|
@ -471,10 +471,10 @@ struct FunctionalBase: ExpressionNode<T> {
|
|||
struct Record {
|
||||
void print(const std::string& indent) const {
|
||||
}
|
||||
void startReverseAD(JacobianMap& jacobians) const {
|
||||
void startReverseAD4(JacobianMap& jacobians) const {
|
||||
}
|
||||
template<typename SomeMatrix>
|
||||
void reverseAD(const SomeMatrix & dFdT, JacobianMap& jacobians) const {
|
||||
void reverseAD4(const SomeMatrix & dFdT, JacobianMap& jacobians) const {
|
||||
}
|
||||
};
|
||||
/// Construct an execution trace for reverse AD
|
||||
|
|
@ -506,9 +506,9 @@ struct JacobianTrace {
|
|||
typename Jacobian<T, A>::type dTdA;
|
||||
};
|
||||
|
||||
/**
|
||||
* Recursive Definition of Functional ExpressionNode
|
||||
*/
|
||||
// Recursive Definition of Functional ExpressionNode
|
||||
// The reason we inherit from Argument<T, A, N> is because we can then
|
||||
// case to this unique signature to retrieve the expression at any level
|
||||
template<class T, class A, class Base>
|
||||
struct GenerateFunctionalNode: Argument<T, A, Base::N + 1>, Base {
|
||||
|
||||
|
|
@ -529,7 +529,9 @@ struct GenerateFunctionalNode: Argument<T, A, Base::N + 1>, Base {
|
|||
This::expression->dims(map);
|
||||
}
|
||||
|
||||
/// Recursive Record Class for Functional Expressions
|
||||
// Recursive Record Class for Functional Expressions
|
||||
// The reason we inherit from JacobianTrace<T, A, N> is because we can then
|
||||
// case to this unique signature to retrieve the value/trace at any level
|
||||
struct Record: JacobianTrace<T, A, N>, Base::Record {
|
||||
|
||||
typedef T return_type;
|
||||
|
|
@ -544,17 +546,26 @@ struct GenerateFunctionalNode: Argument<T, A, Base::N + 1>, Base {
|
|||
}
|
||||
|
||||
/// Start the reverse AD process
|
||||
void startReverseAD(JacobianMap& jacobians) const {
|
||||
Base::Record::startReverseAD(jacobians);
|
||||
This::trace.reverseAD(This::dTdA, jacobians);
|
||||
void startReverseAD4(JacobianMap& jacobians) const {
|
||||
Base::Record::startReverseAD4(jacobians);
|
||||
// This is the crucial point where the size of the AD pipeline is selected.
|
||||
// One pipeline is started for each argument, but the number of rows in each
|
||||
// pipeline is the same, namely the dimension of the output argument T.
|
||||
// For example, if the entire expression is rooted by a binary function
|
||||
// yielding a 2D result, then the matrix dTdA will have 2 rows.
|
||||
// ExecutionTrace::reverseAD1 just passes this on to CallRecord::reverseAD2
|
||||
// which calls the correctly sized CallRecord::reverseAD3, which in turn
|
||||
// calls reverseAD4 below.
|
||||
This::trace.reverseAD1(This::dTdA, jacobians);
|
||||
}
|
||||
|
||||
/// Given df/dT, multiply in dT/dA and continue reverse AD process
|
||||
// Cols is always known at compile time
|
||||
template<int Rows, int Cols>
|
||||
void reverseAD(const Eigen::Matrix<double, Rows, Cols> & dFdT,
|
||||
void reverseAD4(const Eigen::Matrix<double, Rows, Cols> & dFdT,
|
||||
JacobianMap& jacobians) const {
|
||||
Base::Record::reverseAD(dFdT, jacobians);
|
||||
This::trace.reverseAD(dFdT * This::dTdA, jacobians);
|
||||
Base::Record::reverseAD4(dFdT, jacobians);
|
||||
This::trace.reverseAD1(dFdT * This::dTdA, jacobians);
|
||||
}
|
||||
};
|
||||
|
||||
|
|
@ -615,8 +626,8 @@ struct FunctionalNode {
|
|||
struct Record: public internal::CallRecordImplementor<Record,
|
||||
traits::dimension<T>::value>, public Base::Record {
|
||||
using Base::Record::print;
|
||||
using Base::Record::startReverseAD;
|
||||
using Base::Record::reverseAD;
|
||||
using Base::Record::startReverseAD4;
|
||||
using Base::Record::reverseAD4;
|
||||
|
||||
virtual ~Record() {
|
||||
}
|
||||
|
|
|
|||
|
|
@ -209,7 +209,7 @@ private:
|
|||
ExecutionTraceStorage traceStorage[size];
|
||||
ExecutionTrace<T> trace;
|
||||
T value(traceExecution(values, trace, traceStorage));
|
||||
trace.startReverseAD(jacobians);
|
||||
trace.startReverseAD1(jacobians);
|
||||
return value;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -17,8 +17,8 @@
|
|||
* @brief unit tests for Block Automatic Differentiation
|
||||
*/
|
||||
|
||||
#include <gtsam_unstable/nonlinear/ceres_example.h>
|
||||
#include <gtsam_unstable/nonlinear/AdaptAutoDiff.h>
|
||||
#include <gtsam/3rdparty/ceres/example.h>
|
||||
#include <gtsam/nonlinear/AdaptAutoDiff.h>
|
||||
#include <gtsam/nonlinear/Expression.h>
|
||||
#include <gtsam/geometry/PinholeCamera.h>
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
|
|
@ -12,6 +12,8 @@
|
|||
/**
|
||||
* @file testValues.cpp
|
||||
* @author Richard Roberts
|
||||
* @author Frank Dellaert
|
||||
* @author Mike Bosse
|
||||
*/
|
||||
|
||||
#include <gtsam/nonlinear/Values.h>
|
||||
|
|
@ -168,9 +170,9 @@ TEST(Values, basic_functions)
|
|||
Values values;
|
||||
const Values& values_c = values;
|
||||
values.insert(2, Vector3());
|
||||
values.insert(4, Vector3());
|
||||
values.insert(6, Vector3());
|
||||
values.insert(8, Vector3());
|
||||
values.insert(4, Vector(3));
|
||||
values.insert(6, Matrix23());
|
||||
values.insert(8, Matrix(2,3));
|
||||
|
||||
// find
|
||||
EXPECT_LONGS_EQUAL(4, values.find(4)->key);
|
||||
|
|
|
|||
|
|
@ -74,25 +74,26 @@ namespace gtsam {
|
|||
std::cout << s << "BetweenFactor("
|
||||
<< keyFormatter(this->key1()) << ","
|
||||
<< keyFormatter(this->key2()) << ")\n";
|
||||
measured_.print(" measured: ");
|
||||
traits::print<T>()(measured_, " measured: ");
|
||||
this->noiseModel_->print(" noise model: ");
|
||||
}
|
||||
|
||||
/** equals */
|
||||
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
|
||||
const This *e = dynamic_cast<const This*> (&expected);
|
||||
return e != NULL && Base::equals(*e, tol) && this->measured_.equals(e->measured_, tol);
|
||||
return e != NULL && Base::equals(*e, tol) && traits::equals<T>()(this->measured_, e->measured_, tol);
|
||||
}
|
||||
|
||||
/** implement functions needed to derive from Factor */
|
||||
|
||||
/** vector of errors */
|
||||
Vector evaluateError(const T& p1, const T& p2,
|
||||
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
|
||||
boost::optional<Matrix&> H1 = boost::none,boost::optional<Matrix&> H2 =
|
||||
boost::none) const {
|
||||
T hx = p1.between(p2, H1, H2); // h(x)
|
||||
DefaultChart<T> chart;
|
||||
// manifold equivalent of h(x)-z -> log(z,h(x))
|
||||
return measured_.localCoordinates(hx);
|
||||
return chart.local(measured_, hx);
|
||||
}
|
||||
|
||||
/** return the measured */
|
||||
|
|
@ -129,7 +130,9 @@ namespace gtsam {
|
|||
|
||||
/** Syntactic sugar for constrained version */
|
||||
BetweenConstraint(const VALUE& measured, Key key1, Key key2, double mu = 1000.0) :
|
||||
BetweenFactor<VALUE>(key1, key2, measured, noiseModel::Constrained::All(VALUE::Dim(), fabs(mu))) {}
|
||||
BetweenFactor<VALUE>(key1, key2, measured,
|
||||
noiseModel::Constrained::All(DefaultChart<VALUE>::getDimension(measured), fabs(mu)))
|
||||
{}
|
||||
|
||||
private:
|
||||
|
||||
|
|
|
|||
|
|
@ -67,23 +67,24 @@ namespace gtsam {
|
|||
/** print */
|
||||
virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
|
||||
std::cout << s << "PriorFactor on " << keyFormatter(this->key()) << "\n";
|
||||
prior_.print(" prior mean: ");
|
||||
traits::print<T>()(prior_, " prior mean: ");
|
||||
this->noiseModel_->print(" noise model: ");
|
||||
}
|
||||
|
||||
/** equals */
|
||||
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
|
||||
const This* e = dynamic_cast<const This*> (&expected);
|
||||
return e != NULL && Base::equals(*e, tol) && this->prior_.equals(e->prior_, tol);
|
||||
return e != NULL && Base::equals(*e, tol) && traits::equals<T>()(prior_, e->prior_, tol);
|
||||
}
|
||||
|
||||
/** implement functions needed to derive from Factor */
|
||||
|
||||
/** vector of errors */
|
||||
Vector evaluateError(const T& p, boost::optional<Matrix&> H = boost::none) const {
|
||||
if (H) (*H) = eye(p.dim());
|
||||
DefaultChart<T> chart;
|
||||
if (H) (*H) = eye(chart.getDimension(p));
|
||||
// manifold equivalent of h(x)-z -> log(z,h(x))
|
||||
return prior_.localCoordinates(p);
|
||||
return chart.local(prior_,p);
|
||||
}
|
||||
|
||||
const VALUE & prior() const { return prior_; }
|
||||
|
|
|
|||
|
|
@ -44,6 +44,30 @@ TEST(BetweenFactor, Rot3) {
|
|||
EXPECT(assert_equal(numericalH2,actualH2, 1E-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
/*
|
||||
// Constructor scalar
|
||||
TEST(BetweenFactor, ConstructorScalar) {
|
||||
SharedNoiseModel model;
|
||||
double measured_value = 0.0;
|
||||
BetweenFactor<double> factor(1, 2, measured_value, model);
|
||||
}
|
||||
|
||||
// Constructor vector3
|
||||
TEST(BetweenFactor, ConstructorVector3) {
|
||||
SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 1.0);
|
||||
Vector3 measured_value(1, 2, 3);
|
||||
BetweenFactor<Vector3> factor(1, 2, measured_value, model);
|
||||
}
|
||||
|
||||
// Constructor dynamic sized vector
|
||||
TEST(BetweenFactor, ConstructorDynamicSizeVector) {
|
||||
SharedNoiseModel model = noiseModel::Isotropic::Sigma(5, 1.0);
|
||||
Vector measured_value(5); measured_value << 1, 2, 3, 4, 5;
|
||||
BetweenFactor<Vector> factor(1, 2, measured_value, model);
|
||||
}
|
||||
*/
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
|
|
|
|||
|
|
@ -5,8 +5,8 @@
|
|||
* @date Nov 4, 2014
|
||||
*/
|
||||
|
||||
#include <gtsam/base/Vector.h>
|
||||
#include <gtsam/slam/PriorFactor.h>
|
||||
#include <gtsam/base/LieScalar.h>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
using namespace std;
|
||||
|
|
@ -14,10 +14,23 @@ using namespace gtsam;
|
|||
|
||||
/* ************************************************************************* */
|
||||
|
||||
// Constructor
|
||||
TEST(PriorFactor, Constructor) {
|
||||
// Constructor scalar
|
||||
TEST(PriorFactor, ConstructorScalar) {
|
||||
SharedNoiseModel model;
|
||||
PriorFactor<LieScalar> factor(1, LieScalar(1.0), model);
|
||||
PriorFactor<double> factor(1, 1.0, model);
|
||||
}
|
||||
|
||||
// Constructor vector3
|
||||
TEST(PriorFactor, ConstructorVector3) {
|
||||
SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 1.0);
|
||||
PriorFactor<Vector3> factor(1, Vector3(1,2,3), model);
|
||||
}
|
||||
|
||||
// Constructor dynamic sized vector
|
||||
TEST(PriorFactor, ConstructorDynamicSizeVector) {
|
||||
Vector v(5); v << 1, 2, 3, 4, 5;
|
||||
SharedNoiseModel model = noiseModel::Isotropic::Sigma(5, 1.0);
|
||||
PriorFactor<Vector> factor(1, v, model);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -32,12 +32,6 @@ class JacobianMap;
|
|||
// forward declaration
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
/**
|
||||
* MaxVirtualStaticRows defines how many separate virtual reverseAD with specific
|
||||
* static rows (1..MaxVirtualStaticRows) methods will be part of the CallRecord interface.
|
||||
*/
|
||||
const int MaxVirtualStaticRows = 4;
|
||||
|
||||
namespace internal {
|
||||
|
||||
/**
|
||||
|
|
@ -57,7 +51,8 @@ struct ConvertToVirtualFunctionSupportedMatrixType {
|
|||
template<>
|
||||
struct ConvertToVirtualFunctionSupportedMatrixType<false> {
|
||||
template<typename Derived>
|
||||
static const Eigen::Matrix<double, Derived::RowsAtCompileTime, Derived::ColsAtCompileTime> convert(
|
||||
static const Eigen::Matrix<double, Derived::RowsAtCompileTime,
|
||||
Derived::ColsAtCompileTime> convert(
|
||||
const Eigen::MatrixBase<Derived> & x) {
|
||||
return x;
|
||||
}
|
||||
|
|
@ -69,126 +64,132 @@ struct ConvertToVirtualFunctionSupportedMatrixType<false> {
|
|||
}
|
||||
};
|
||||
|
||||
/**
|
||||
* Recursive definition of an interface having several purely
|
||||
* virtual _reverseAD(const Eigen::Matrix<double, Rows, Cols> &, JacobianMap&)
|
||||
* with Rows in 1..MaxSupportedStaticRows
|
||||
*/
|
||||
template<int MaxSupportedStaticRows, int Cols>
|
||||
struct ReverseADInterface: ReverseADInterface<MaxSupportedStaticRows - 1, Cols> {
|
||||
using ReverseADInterface<MaxSupportedStaticRows - 1, Cols>::_reverseAD;
|
||||
virtual void _reverseAD(
|
||||
const Eigen::Matrix<double, MaxSupportedStaticRows, Cols> & dFdT,
|
||||
JacobianMap& jacobians) const = 0;
|
||||
};
|
||||
|
||||
template<int Cols>
|
||||
struct ReverseADInterface<0, Cols> {
|
||||
virtual void _reverseAD(
|
||||
const Eigen::Matrix<double, Eigen::Dynamic, Cols> & dFdT,
|
||||
JacobianMap& jacobians) const = 0;
|
||||
virtual void _reverseAD(const Matrix & dFdT,
|
||||
JacobianMap& jacobians) const = 0;
|
||||
};
|
||||
|
||||
/**
|
||||
* ReverseADImplementor is a utility class used by CallRecordImplementor to
|
||||
* implementing the recursive ReverseADInterface interface.
|
||||
*/
|
||||
template<typename Derived, int MaxSupportedStaticRows, int Cols>
|
||||
struct ReverseADImplementor: ReverseADImplementor<Derived,
|
||||
MaxSupportedStaticRows - 1, Cols> {
|
||||
private:
|
||||
using ReverseADImplementor<Derived, MaxSupportedStaticRows - 1, Cols>::_reverseAD;
|
||||
virtual void _reverseAD(
|
||||
const Eigen::Matrix<double, MaxSupportedStaticRows, Cols> & dFdT,
|
||||
JacobianMap& jacobians) const {
|
||||
static_cast<const Derived *>(this)->reverseAD(dFdT, jacobians);
|
||||
}
|
||||
friend struct internal::ReverseADImplementor<Derived,
|
||||
MaxSupportedStaticRows + 1, Cols>;
|
||||
};
|
||||
|
||||
template<typename Derived, int Cols>
|
||||
struct ReverseADImplementor<Derived, 0, Cols> : virtual internal::ReverseADInterface<
|
||||
MaxVirtualStaticRows, Cols> {
|
||||
private:
|
||||
using internal::ReverseADInterface<MaxVirtualStaticRows, Cols>::_reverseAD;
|
||||
const Derived & derived() const {
|
||||
return static_cast<const Derived&>(*this);
|
||||
}
|
||||
virtual void _reverseAD(
|
||||
const Eigen::Matrix<double, Eigen::Dynamic, Cols> & dFdT,
|
||||
JacobianMap& jacobians) const {
|
||||
derived().reverseAD(dFdT, jacobians);
|
||||
}
|
||||
virtual void _reverseAD(const Matrix & dFdT, JacobianMap& jacobians) const {
|
||||
derived().reverseAD(dFdT, jacobians);
|
||||
}
|
||||
friend struct internal::ReverseADImplementor<Derived, 1, Cols>;
|
||||
};
|
||||
|
||||
} // namespace internal
|
||||
|
||||
/**
|
||||
* The CallRecord class stores the Jacobians of applying a function
|
||||
* with respect to each of its arguments. It also stores an execution trace
|
||||
* (defined below) for each of its arguments.
|
||||
*
|
||||
* It is implemented in the function-style ExpressionNode's nested Record class below.
|
||||
* The CallRecord is an abstract base class for the any class that stores
|
||||
* the Jacobians of applying a function with respect to each of its arguments,
|
||||
* as well as an execution trace for each of its arguments.
|
||||
*/
|
||||
template<int Cols>
|
||||
struct CallRecord: virtual private internal::ReverseADInterface<
|
||||
MaxVirtualStaticRows, Cols> {
|
||||
struct CallRecord {
|
||||
|
||||
// Print entire record, recursively
|
||||
inline void print(const std::string& indent) const {
|
||||
_print(indent);
|
||||
}
|
||||
|
||||
inline void startReverseAD(JacobianMap& jacobians) const {
|
||||
_startReverseAD(jacobians);
|
||||
// Main entry point for the reverse AD process of a functional expression.
|
||||
// Called *once* by the main AD entry point, ExecutionTrace::startReverseAD1
|
||||
// This function then calls ExecutionTrace::reverseAD for every argument
|
||||
// which will in turn call the reverseAD method below.
|
||||
// This non-virtual function _startReverseAD3, implemented in derived
|
||||
inline void startReverseAD2(JacobianMap& jacobians) const {
|
||||
_startReverseAD3(jacobians);
|
||||
}
|
||||
|
||||
// Dispatch the reverseAD2 calls issued by ExecutionTrace::reverseAD1
|
||||
// Here we convert to dynamic if the
|
||||
template<typename Derived>
|
||||
inline void reverseAD(const Eigen::MatrixBase<Derived> & dFdT,
|
||||
inline void reverseAD2(const Eigen::MatrixBase<Derived> & dFdT,
|
||||
JacobianMap& jacobians) const {
|
||||
_reverseAD(
|
||||
internal::ConvertToVirtualFunctionSupportedMatrixType<(Derived::RowsAtCompileTime > MaxVirtualStaticRows)>::convert(
|
||||
dFdT), jacobians);
|
||||
_reverseAD3(
|
||||
internal::ConvertToVirtualFunctionSupportedMatrixType<
|
||||
(Derived::RowsAtCompileTime > 5)>::convert(dFdT),
|
||||
jacobians);
|
||||
}
|
||||
|
||||
inline void reverseAD(const Matrix & dFdT, JacobianMap& jacobians) const {
|
||||
_reverseAD(dFdT, jacobians);
|
||||
// This overload supports matrices with both rows and columns dynamically sized.
|
||||
// The template version above would be slower by introducing an extra conversion
|
||||
// to statically sized columns.
|
||||
inline void reverseAD2(const Matrix & dFdT, JacobianMap& jacobians) const {
|
||||
_reverseAD3(dFdT, jacobians);
|
||||
}
|
||||
|
||||
virtual ~CallRecord() {
|
||||
}
|
||||
|
||||
private:
|
||||
|
||||
virtual void _print(const std::string& indent) const = 0;
|
||||
virtual void _startReverseAD(JacobianMap& jacobians) const = 0;
|
||||
using internal::ReverseADInterface<MaxVirtualStaticRows, Cols>::_reverseAD;
|
||||
virtual void _startReverseAD3(JacobianMap& jacobians) const = 0;
|
||||
|
||||
virtual void _reverseAD3(const Matrix & dFdT,
|
||||
JacobianMap& jacobians) const = 0;
|
||||
|
||||
virtual void _reverseAD3(
|
||||
const Eigen::Matrix<double, Eigen::Dynamic, Cols> & dFdT,
|
||||
JacobianMap& jacobians) const = 0;
|
||||
|
||||
virtual void _reverseAD3(const Eigen::Matrix<double, 1, Cols> & dFdT,
|
||||
JacobianMap& jacobians) const = 0;
|
||||
virtual void _reverseAD3(const Eigen::Matrix<double, 2, Cols> & dFdT,
|
||||
JacobianMap& jacobians) const = 0;
|
||||
virtual void _reverseAD3(const Eigen::Matrix<double, 3, Cols> & dFdT,
|
||||
JacobianMap& jacobians) const = 0;
|
||||
virtual void _reverseAD3(const Eigen::Matrix<double, 4, Cols> & dFdT,
|
||||
JacobianMap& jacobians) const = 0;
|
||||
virtual void _reverseAD3(const Eigen::Matrix<double, 5, Cols> & dFdT,
|
||||
JacobianMap& jacobians) const = 0;
|
||||
};
|
||||
|
||||
/**
|
||||
* CallRecordMaxVirtualStaticRows tells which separate virtual reverseAD with specific
|
||||
* static rows (1..CallRecordMaxVirtualStaticRows) methods are part of the CallRecord
|
||||
* interface. It is used to keep the testCallRecord unit test in sync.
|
||||
*/
|
||||
const int CallRecordMaxVirtualStaticRows = 5;
|
||||
|
||||
namespace internal {
|
||||
/**
|
||||
* The CallRecordImplementor implements the CallRecord interface for a Derived class by
|
||||
* delegating to its corresponding (templated) non-virtual methods.
|
||||
*/
|
||||
template<typename Derived, int Cols>
|
||||
struct CallRecordImplementor: public CallRecord<Cols>,
|
||||
private ReverseADImplementor<Derived, MaxVirtualStaticRows, Cols> {
|
||||
struct CallRecordImplementor: public CallRecord<Cols> {
|
||||
private:
|
||||
|
||||
const Derived & derived() const {
|
||||
return static_cast<const Derived&>(*this);
|
||||
}
|
||||
|
||||
virtual void _print(const std::string& indent) const {
|
||||
derived().print(indent);
|
||||
}
|
||||
virtual void _startReverseAD(JacobianMap& jacobians) const {
|
||||
derived().startReverseAD(jacobians);
|
||||
|
||||
virtual void _startReverseAD3(JacobianMap& jacobians) const {
|
||||
derived().startReverseAD4(jacobians);
|
||||
}
|
||||
|
||||
virtual void _reverseAD3(const Matrix & dFdT, JacobianMap& jacobians) const {
|
||||
derived().reverseAD4(dFdT, jacobians);
|
||||
}
|
||||
|
||||
virtual void _reverseAD3(
|
||||
const Eigen::Matrix<double, Eigen::Dynamic, Cols> & dFdT,
|
||||
JacobianMap& jacobians) const {
|
||||
derived().reverseAD4(dFdT, jacobians);
|
||||
}
|
||||
virtual void _reverseAD3(const Eigen::Matrix<double, 1, Cols> & dFdT,
|
||||
JacobianMap& jacobians) const {
|
||||
derived().reverseAD4(dFdT, jacobians);
|
||||
}
|
||||
virtual void _reverseAD3(const Eigen::Matrix<double, 2, Cols> & dFdT,
|
||||
JacobianMap& jacobians) const {
|
||||
derived().reverseAD4(dFdT, jacobians);
|
||||
}
|
||||
virtual void _reverseAD3(const Eigen::Matrix<double, 3, Cols> & dFdT,
|
||||
JacobianMap& jacobians) const {
|
||||
derived().reverseAD4(dFdT, jacobians);
|
||||
}
|
||||
virtual void _reverseAD3(const Eigen::Matrix<double, 4, Cols> & dFdT,
|
||||
JacobianMap& jacobians) const {
|
||||
derived().reverseAD4(dFdT, jacobians);
|
||||
}
|
||||
virtual void _reverseAD3(const Eigen::Matrix<double, 5, Cols> & dFdT,
|
||||
JacobianMap& jacobians) const {
|
||||
derived().reverseAD4(dFdT, jacobians);
|
||||
}
|
||||
template<typename D, int R, int C> friend struct ReverseADImplementor;
|
||||
};
|
||||
|
||||
} // namespace internal
|
||||
|
|
|
|||
|
|
@ -35,7 +35,6 @@ class ExpressionFactor: public NoiseModelFactor {
|
|||
T measurement_; ///< the measurement to be compared with the expression
|
||||
Expression<T> expression_; ///< the expression that is AD enabled
|
||||
FastVector<int> dims_; ///< dimensions of the Jacobian matrices
|
||||
size_t augmentedCols_; ///< total number of columns + 1 (for RHS)
|
||||
|
||||
static const int Dim = traits::dimension<T>::value;
|
||||
|
||||
|
|
@ -55,15 +54,6 @@ public:
|
|||
// Get keys and dimensions for Jacobian matrices
|
||||
// An Expression is assumed unmutable, so we do this now
|
||||
boost::tie(keys_, dims_) = expression_.keysAndDims();
|
||||
|
||||
// Add sizes to know how much memory to allocate on stack in linearize
|
||||
augmentedCols_ = std::accumulate(dims_.begin(), dims_.end(), 1);
|
||||
|
||||
#ifdef DEBUG_ExpressionFactor
|
||||
BOOST_FOREACH(size_t d, dims_)
|
||||
std::cout << d << " ";
|
||||
std::cout << " -> " << Dim << "x" << augmentedCols_ << std::endl;
|
||||
#endif
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -16,22 +16,9 @@
|
|||
* @brief unit tests for Basis Decompositions w Expressions
|
||||
*/
|
||||
|
||||
#include <gtsam_unstable/nonlinear/expressions.h>
|
||||
#include <gtsam_unstable/nonlinear/ExpressionFactor.h>
|
||||
#include <gtsam/linear/VectorValues.h>
|
||||
#include <gtsam/linear/GaussianFactorGraph.h>
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/base/Matrix.h>
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
#include <boost/assign/list_of.hpp>
|
||||
using boost::assign::list_of;
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
noiseModel::Diagonal::shared_ptr model = noiseModel::Unit::Create(1);
|
||||
namespace gtsam {
|
||||
|
||||
/// Fourier
|
||||
template<int N>
|
||||
|
|
@ -67,6 +54,80 @@ public:
|
|||
}
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#include <gtsam_unstable/nonlinear/ExpressionFactor.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/linear/GaussianFactorGraph.h>
|
||||
#include <gtsam/linear/VectorValues.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/// For now, this is our sequence representation
|
||||
typedef std::map<double, double> Sequence;
|
||||
|
||||
/**
|
||||
* Class that does Fourier Decomposition via least squares
|
||||
*/
|
||||
class FourierDecomposition {
|
||||
public:
|
||||
|
||||
typedef Vector3 Coefficients; ///< Fourier coefficients
|
||||
|
||||
private:
|
||||
Coefficients c_;
|
||||
|
||||
public:
|
||||
|
||||
/// Create nonlinear FG from Sequence
|
||||
static NonlinearFactorGraph NonlinearGraph(const Sequence& sequence,
|
||||
const SharedNoiseModel& model) {
|
||||
NonlinearFactorGraph graph;
|
||||
Expression<Coefficients> c(0);
|
||||
typedef std::pair<double, double> Sample;
|
||||
BOOST_FOREACH(Sample sample, sequence) {
|
||||
Expression<double> expression(Fourier<3>(sample.first), c);
|
||||
ExpressionFactor<double> factor(model, sample.second, expression);
|
||||
graph.add(factor);
|
||||
}
|
||||
return graph;
|
||||
}
|
||||
|
||||
/// Create linear FG from Sequence
|
||||
static GaussianFactorGraph::shared_ptr LinearGraph(const Sequence& sequence,
|
||||
const SharedNoiseModel& model) {
|
||||
NonlinearFactorGraph graph = NonlinearGraph(sequence, model);
|
||||
Values values;
|
||||
values.insert<Coefficients>(0, Coefficients::Zero()); // does not matter
|
||||
GaussianFactorGraph::shared_ptr gfg = graph.linearize(values);
|
||||
return gfg;
|
||||
}
|
||||
|
||||
/// Constructor
|
||||
FourierDecomposition(const Sequence& sequence,
|
||||
const SharedNoiseModel& model) {
|
||||
GaussianFactorGraph::shared_ptr gfg = LinearGraph(sequence, model);
|
||||
VectorValues solution = gfg->optimize();
|
||||
c_ = solution.at(0);
|
||||
}
|
||||
|
||||
/// Return Fourier coefficients
|
||||
Coefficients coefficients() {
|
||||
return c_;
|
||||
}
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#include <gtsam_unstable/nonlinear/expressionTesting.h>
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
noiseModel::Diagonal::shared_ptr model = noiseModel::Unit::Create(1);
|
||||
|
||||
//******************************************************************************
|
||||
TEST(BasisDecompositions, Fourier) {
|
||||
Fourier<3> fx(0);
|
||||
|
|
@ -79,12 +140,14 @@ TEST(BasisDecompositions, Fourier) {
|
|||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(BasisDecompositions, FourierExpression) {
|
||||
TEST(BasisDecompositions, ManualFourier) {
|
||||
|
||||
// Create linear factor graph
|
||||
GaussianFactorGraph g;
|
||||
Key key(1);
|
||||
Vector3_ c(key);
|
||||
Expression<Vector3> c(key);
|
||||
Values values;
|
||||
values.insert<Vector3>(key, Vector3::Zero()); // does not matter
|
||||
for (size_t i = 0; i < 16; i++) {
|
||||
double x = i * M_PI / 8, y = exp(sin(x) + cos(x));
|
||||
|
||||
|
|
@ -93,13 +156,44 @@ TEST(BasisDecompositions, FourierExpression) {
|
|||
A << 1, cos(x), sin(x);
|
||||
Vector b(1);
|
||||
b << y;
|
||||
JacobianFactor f1(key, A, b, model);
|
||||
JacobianFactor f1(key, A, b);
|
||||
g.add(f1);
|
||||
|
||||
// With ExpressionFactor
|
||||
Expression<double> expression(Fourier<3>(x), c);
|
||||
EXPECT_CORRECT_EXPRESSION_JACOBIANS(expression, values, 1e-5, 1e-9);
|
||||
{
|
||||
ExpressionFactor<double> f2(model, y, expression);
|
||||
|
||||
g.add(f1);
|
||||
boost::shared_ptr<GaussianFactor> gf = f2.linearize(values);
|
||||
boost::shared_ptr<JacobianFactor> jf = //
|
||||
boost::dynamic_pointer_cast<JacobianFactor>(gf);
|
||||
CHECK(jf);
|
||||
EXPECT( assert_equal(f1, *jf, 1e-9));
|
||||
}
|
||||
{
|
||||
ExpressionFactor<double> f2(model, y, expression);
|
||||
boost::shared_ptr<GaussianFactor> gf = f2.linearize(values);
|
||||
boost::shared_ptr<JacobianFactor> jf = //
|
||||
boost::dynamic_pointer_cast<JacobianFactor>(gf);
|
||||
CHECK(jf);
|
||||
EXPECT( assert_equal(f1, *jf, 1e-9));
|
||||
}
|
||||
{
|
||||
ExpressionFactor<double> f2(model, y, expression);
|
||||
boost::shared_ptr<GaussianFactor> gf = f2.linearize(values);
|
||||
boost::shared_ptr<JacobianFactor> jf = //
|
||||
boost::dynamic_pointer_cast<JacobianFactor>(gf);
|
||||
CHECK(jf);
|
||||
EXPECT( assert_equal(f1, *jf, 1e-9));
|
||||
}
|
||||
{
|
||||
ExpressionFactor<double> f2(model, y, expression);
|
||||
boost::shared_ptr<GaussianFactor> gf = f2.linearize(values);
|
||||
boost::shared_ptr<JacobianFactor> jf = //
|
||||
boost::dynamic_pointer_cast<JacobianFactor>(gf);
|
||||
CHECK(jf);
|
||||
EXPECT( assert_equal(f1, *jf, 1e-9));
|
||||
}
|
||||
}
|
||||
|
||||
// Solve
|
||||
|
|
@ -110,6 +204,24 @@ TEST(BasisDecompositions, FourierExpression) {
|
|||
EXPECT(assert_equal((Vector) expected, actual.at(key),1e-4));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(BasisDecompositions, FourierDecomposition) {
|
||||
|
||||
// Create example sequence
|
||||
Sequence sequence;
|
||||
for (size_t i = 0; i < 16; i++) {
|
||||
double x = i * M_PI / 8, y = exp(sin(x) + cos(x));
|
||||
sequence[x] = y;
|
||||
}
|
||||
|
||||
// Do Fourier Decomposition
|
||||
FourierDecomposition actual(sequence, model);
|
||||
|
||||
// Check
|
||||
Vector3 expected(1.5661, 1.2717, 1.2717);
|
||||
EXPECT(assert_equal((Vector) expected, actual.coefficients(),1e-4));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
int main() {
|
||||
TestResult tr;
|
||||
|
|
|
|||
|
|
@ -33,7 +33,7 @@ static const int Cols = 3;
|
|||
|
||||
|
||||
int dynamicIfAboveMax(int i){
|
||||
if(i > MaxVirtualStaticRows){
|
||||
if(i > CallRecordMaxVirtualStaticRows){
|
||||
return Eigen::Dynamic;
|
||||
}
|
||||
else return i;
|
||||
|
|
@ -43,7 +43,6 @@ struct CallConfig {
|
|||
int compTimeCols;
|
||||
int runTimeRows;
|
||||
int runTimeCols;
|
||||
CallConfig() {}
|
||||
CallConfig(int rows, int cols):
|
||||
compTimeRows(dynamicIfAboveMax(rows)),
|
||||
compTimeCols(cols),
|
||||
|
|
@ -72,25 +71,26 @@ struct CallConfig {
|
|||
};
|
||||
|
||||
struct Record: public internal::CallRecordImplementor<Record, Cols> {
|
||||
Record() : cc(0, 0) {}
|
||||
virtual ~Record() {
|
||||
}
|
||||
void print(const std::string& indent) const {
|
||||
}
|
||||
void startReverseAD(JacobianMap& jacobians) const {
|
||||
void startReverseAD4(JacobianMap& jacobians) const {
|
||||
}
|
||||
|
||||
mutable CallConfig cc;
|
||||
private:
|
||||
template<typename SomeMatrix>
|
||||
void reverseAD(const SomeMatrix & dFdT, JacobianMap& jacobians) const {
|
||||
void reverseAD4(const SomeMatrix & dFdT, JacobianMap& jacobians) const {
|
||||
cc.compTimeRows = SomeMatrix::RowsAtCompileTime;
|
||||
cc.compTimeCols = SomeMatrix::ColsAtCompileTime;
|
||||
cc.runTimeRows = dFdT.rows();
|
||||
cc.runTimeCols = dFdT.cols();
|
||||
}
|
||||
|
||||
template<typename Derived, int Rows, int OtherCols>
|
||||
friend struct internal::ReverseADImplementor;
|
||||
template<typename Derived, int Rows>
|
||||
friend struct internal::CallRecordImplementor;
|
||||
};
|
||||
|
||||
JacobianMap & NJM= *static_cast<JacobianMap *>(NULL);
|
||||
|
|
@ -102,56 +102,56 @@ TEST(CallRecord, virtualReverseAdDispatching) {
|
|||
Record record;
|
||||
{
|
||||
const int Rows = 1;
|
||||
record.CallRecord::reverseAD(Eigen::Matrix<double, Rows, Cols>(), NJM);
|
||||
record.CallRecord::reverseAD2(Eigen::Matrix<double, Rows, Cols>(), NJM);
|
||||
EXPECT((assert_equal(record.cc, CallConfig(Rows, Cols))));
|
||||
record.CallRecord::reverseAD(DynRowMat(Rows, Cols), NJM);
|
||||
record.CallRecord::reverseAD2(DynRowMat(Rows, Cols), NJM);
|
||||
EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Cols, Rows, Cols))));
|
||||
record.CallRecord::reverseAD(Eigen::MatrixXd(Rows, Cols), NJM);
|
||||
record.CallRecord::reverseAD2(Eigen::MatrixXd(Rows, Cols), NJM);
|
||||
EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Eigen::Dynamic, Rows, Cols))));
|
||||
}
|
||||
{
|
||||
const int Rows = 2;
|
||||
record.CallRecord::reverseAD(Eigen::Matrix<double, Rows, Cols>(), NJM);
|
||||
record.CallRecord::reverseAD2(Eigen::Matrix<double, Rows, Cols>(), NJM);
|
||||
EXPECT((assert_equal(record.cc, CallConfig(Rows, Cols))));
|
||||
record.CallRecord::reverseAD(DynRowMat(Rows, Cols), NJM);
|
||||
record.CallRecord::reverseAD2(DynRowMat(Rows, Cols), NJM);
|
||||
EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Cols, Rows, Cols))));
|
||||
record.CallRecord::reverseAD(Eigen::MatrixXd(Rows, Cols), NJM);
|
||||
record.CallRecord::reverseAD2(Eigen::MatrixXd(Rows, Cols), NJM);
|
||||
EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Eigen::Dynamic, Rows, Cols))));
|
||||
}
|
||||
{
|
||||
const int Rows = 3;
|
||||
record.CallRecord::reverseAD(Eigen::Matrix<double, Rows, Cols>(), NJM);
|
||||
record.CallRecord::reverseAD2(Eigen::Matrix<double, Rows, Cols>(), NJM);
|
||||
EXPECT((assert_equal(record.cc, CallConfig(Rows, Cols))));
|
||||
record.CallRecord::reverseAD(DynRowMat(Rows, Cols), NJM);
|
||||
record.CallRecord::reverseAD2(DynRowMat(Rows, Cols), NJM);
|
||||
EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Cols, Rows, Cols))));
|
||||
record.CallRecord::reverseAD(Eigen::MatrixXd(Rows, Cols), NJM);
|
||||
record.CallRecord::reverseAD2(Eigen::MatrixXd(Rows, Cols), NJM);
|
||||
EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Eigen::Dynamic, Rows, Cols))));
|
||||
}
|
||||
{
|
||||
const int Rows = MaxVirtualStaticRows;
|
||||
record.CallRecord::reverseAD(Eigen::Matrix<double, Rows, Cols>(), NJM);
|
||||
const int Rows = 4;
|
||||
record.CallRecord::reverseAD2(Eigen::Matrix<double, Rows, Cols>(), NJM);
|
||||
EXPECT((assert_equal(record.cc, CallConfig(Rows, Cols))));
|
||||
record.CallRecord::reverseAD(DynRowMat(Rows, Cols), NJM);
|
||||
record.CallRecord::reverseAD2(DynRowMat(Rows, Cols), NJM);
|
||||
EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Cols, Rows, Cols))));
|
||||
record.CallRecord::reverseAD(Eigen::MatrixXd(Rows, Cols), NJM);
|
||||
record.CallRecord::reverseAD2(Eigen::MatrixXd(Rows, Cols), NJM);
|
||||
EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Eigen::Dynamic, Rows, Cols))));
|
||||
}
|
||||
{
|
||||
const int Rows = MaxVirtualStaticRows + 1;
|
||||
record.CallRecord::reverseAD(Eigen::Matrix<double, Rows, Cols>(), NJM);
|
||||
const int Rows = 5;
|
||||
record.CallRecord::reverseAD2(Eigen::Matrix<double, Rows, Cols>(), NJM);
|
||||
EXPECT((assert_equal(record.cc, CallConfig(Rows, Cols))));
|
||||
record.CallRecord::reverseAD(DynRowMat(Rows, Cols), NJM);
|
||||
record.CallRecord::reverseAD2(DynRowMat(Rows, Cols), NJM);
|
||||
EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Cols, Rows, Cols))));
|
||||
record.CallRecord::reverseAD(Eigen::MatrixXd(Rows, Cols), NJM);
|
||||
record.CallRecord::reverseAD2(Eigen::MatrixXd(Rows, Cols), NJM);
|
||||
EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Eigen::Dynamic, Rows, Cols))));
|
||||
}
|
||||
{
|
||||
const int Rows = MaxVirtualStaticRows + 2;
|
||||
record.CallRecord::reverseAD(Eigen::Matrix<double, Rows, Cols>(), NJM);
|
||||
const int Rows = 6;
|
||||
record.CallRecord::reverseAD2(Eigen::Matrix<double, Rows, Cols>(), NJM);
|
||||
EXPECT((assert_equal(record.cc, CallConfig(Rows, Cols))));
|
||||
record.CallRecord::reverseAD(DynRowMat(Rows, Cols), NJM);
|
||||
record.CallRecord::reverseAD2(DynRowMat(Rows, Cols), NJM);
|
||||
EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Cols, Rows, Cols))));
|
||||
record.CallRecord::reverseAD(Eigen::MatrixXd(Rows, Cols), NJM);
|
||||
record.CallRecord::reverseAD2(Eigen::MatrixXd(Rows, Cols), NJM);
|
||||
EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Eigen::Dynamic, Rows, Cols))));
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -76,10 +76,13 @@ TEST(ExpressionFactor, Model) {
|
|||
|
||||
// Concise version
|
||||
ExpressionFactor<Point2> f(model, Point2(0, 0), p);
|
||||
|
||||
// Check values and derivatives
|
||||
EXPECT_DOUBLES_EQUAL(old.error(values), f.error(values), 1e-9);
|
||||
EXPECT_LONGS_EQUAL(2, f.dim());
|
||||
boost::shared_ptr<GaussianFactor> gf2 = f.linearize(values);
|
||||
EXPECT( assert_equal(*old.linearize(values), *gf2, 1e-9));
|
||||
EXPECT_CORRECT_FACTOR_JACOBIANS(f, values, 1e-5, 1e-5); // another way
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -124,6 +127,38 @@ TEST(ExpressionFactor, Unary) {
|
|||
EXPECT( assert_equal(expected, *jf, 1e-9));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Unary(Leaf)) and Unary(Unary(Leaf)))
|
||||
// wide version (not handled in fixed-size pipeline)
|
||||
typedef Eigen::Matrix<double,9,3> Matrix93;
|
||||
Vector9 wide(const Point3& p, boost::optional<Matrix93&> H) {
|
||||
Vector9 v;
|
||||
v << p.vector(), p.vector(), p.vector();
|
||||
if (H) *H << eye(3), eye(3), eye(3);
|
||||
return v;
|
||||
}
|
||||
typedef Eigen::Matrix<double,9,9> Matrix9;
|
||||
Vector9 id9(const Vector9& v, boost::optional<Matrix9&> H) {
|
||||
if (H) *H = Matrix9::Identity();
|
||||
return v;
|
||||
}
|
||||
TEST(ExpressionFactor, Wide) {
|
||||
// Create some values
|
||||
Values values;
|
||||
values.insert(2, Point3(0, 0, 1));
|
||||
Point3_ point(2);
|
||||
Vector9 measured;
|
||||
Expression<Vector9> expression(wide,point);
|
||||
SharedNoiseModel model = noiseModel::Unit::Create(9);
|
||||
|
||||
ExpressionFactor<Vector9> f1(model, measured, expression);
|
||||
EXPECT_CORRECT_FACTOR_JACOBIANS(f1, values, 1e-5, 1e-9);
|
||||
|
||||
Expression<Vector9> expression2(id9,expression);
|
||||
ExpressionFactor<Vector9> f2(model, measured, expression2);
|
||||
EXPECT_CORRECT_FACTOR_JACOBIANS(f2, values, 1e-5, 1e-9);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
static Point2 myUncal(const Cal3_S2& K, const Point2& p,
|
||||
OptionalJacobian<2,5> Dcal, OptionalJacobian<2,2> Dp) {
|
||||
|
|
|
|||
|
|
@ -17,7 +17,7 @@
|
|||
*/
|
||||
|
||||
#include "timeLinearize.h"
|
||||
#include <gtsam_unstable/nonlinear/ceres_example.h>
|
||||
#include <gtsam/3rdparty/ceres/example.h>
|
||||
#include <gtsam_unstable/nonlinear/AdaptAutoDiff.h>
|
||||
#include <gtsam_unstable/nonlinear/ExpressionFactor.h>
|
||||
#include <gtsam/slam/GeneralSFMFactor.h>
|
||||
|
|
|
|||
|
|
@ -0,0 +1 @@
|
|||
*.m~
|
||||
|
|
@ -0,0 +1,40 @@
|
|||
% test wrapping of Values
|
||||
import gtsam.*;
|
||||
|
||||
values = Values;
|
||||
E = EssentialMatrix(Rot3,Unit3);
|
||||
tol = 1e-9;
|
||||
|
||||
values.insert(0, Point2);
|
||||
values.insert(1, Point3);
|
||||
values.insert(2, Rot2);
|
||||
values.insert(3, Pose2);
|
||||
values.insert(4, Rot3);
|
||||
values.insert(5, Pose3);
|
||||
values.insert(6, Cal3_S2);
|
||||
values.insert(7, Cal3DS2);
|
||||
values.insert(8, Cal3Bundler);
|
||||
values.insert(9, E);
|
||||
values.insert(10, imuBias.ConstantBias);
|
||||
|
||||
% special cases for Vector and Matrix:
|
||||
values.insert(11, [1;2;3]);
|
||||
values.insert(12, [1 2;3 4]);
|
||||
|
||||
EXPECT('at',values.atPoint2(0).equals(Point2,tol));
|
||||
EXPECT('at',values.atPoint3(1).equals(Point3,tol));
|
||||
EXPECT('at',values.atRot2(2).equals(Rot2,tol));
|
||||
EXPECT('at',values.atPose2(3).equals(Pose2,tol));
|
||||
EXPECT('at',values.atRot3(4).equals(Rot3,tol));
|
||||
EXPECT('at',values.atPose3(5).equals(Pose3,tol));
|
||||
EXPECT('at',values.atCal3_S2(6).equals(Cal3_S2,tol));
|
||||
EXPECT('at',values.atCal3DS2(7).equals(Cal3DS2,tol));
|
||||
EXPECT('at',values.atCal3Bundler(8).equals(Cal3Bundler,tol));
|
||||
EXPECT('at',values.atEssentialMatrix(9).equals(E,tol));
|
||||
EXPECT('at',values.atConstantBias(10).equals(imuBias.ConstantBias,tol));
|
||||
|
||||
% special cases for Vector and Matrix:
|
||||
actualVector = values.atVector(11);
|
||||
EQUALITY('at',[1;2;3],actualVector,tol);
|
||||
actualMatrix = values.atMatrix(12);
|
||||
EQUALITY('at',[1 2;3 4],actualMatrix,tol);
|
||||
|
|
@ -1,5 +1,8 @@
|
|||
% Test runner script - runs each test
|
||||
|
||||
display 'Starting: testValues'
|
||||
testValues
|
||||
|
||||
display 'Starting: testJacobianFactor'
|
||||
testJacobianFactor
|
||||
|
||||
|
|
|
|||
|
|
@ -20,6 +20,7 @@
|
|||
|
||||
#include <boost/foreach.hpp>
|
||||
#include <boost/regex.hpp>
|
||||
#include <boost/lexical_cast.hpp>
|
||||
|
||||
#include <iostream>
|
||||
#include <fstream>
|
||||
|
|
@ -31,12 +32,13 @@ using namespace wrap;
|
|||
/* ************************************************************************* */
|
||||
Argument Argument::expandTemplate(const TemplateSubstitution& ts) const {
|
||||
Argument instArg = *this;
|
||||
instArg.type = ts(type);
|
||||
instArg.type = ts.tryToSubstitite(type);
|
||||
return instArg;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
ArgumentList ArgumentList::expandTemplate(const TemplateSubstitution& ts) const {
|
||||
ArgumentList ArgumentList::expandTemplate(
|
||||
const TemplateSubstitution& ts) const {
|
||||
ArgumentList instArgList;
|
||||
BOOST_FOREACH(const Argument& arg, *this) {
|
||||
Argument instArg = arg.expandTemplate(ts);
|
||||
|
|
@ -48,25 +50,25 @@ ArgumentList ArgumentList::expandTemplate(const TemplateSubstitution& ts) const
|
|||
/* ************************************************************************* */
|
||||
string Argument::matlabClass(const string& delim) const {
|
||||
string result;
|
||||
BOOST_FOREACH(const string& ns, type.namespaces)
|
||||
BOOST_FOREACH(const string& ns, type.namespaces())
|
||||
result += ns + delim;
|
||||
if (type.name == "string" || type.name == "unsigned char"
|
||||
|| type.name == "char")
|
||||
if (type.name() == "string" || type.name() == "unsigned char"
|
||||
|| type.name() == "char")
|
||||
return result + "char";
|
||||
if (type.name == "Vector" || type.name == "Matrix")
|
||||
if (type.name() == "Vector" || type.name() == "Matrix")
|
||||
return result + "double";
|
||||
if (type.name == "int" || type.name == "size_t")
|
||||
if (type.name() == "int" || type.name() == "size_t")
|
||||
return result + "numeric";
|
||||
if (type.name == "bool")
|
||||
if (type.name() == "bool")
|
||||
return result + "logical";
|
||||
return result + type.name;
|
||||
return result + type.name();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool Argument::isScalar() const {
|
||||
return (type.name == "bool" || type.name == "char"
|
||||
|| type.name == "unsigned char" || type.name == "int"
|
||||
|| type.name == "size_t" || type.name == "double");
|
||||
return (type.name() == "bool" || type.name() == "char"
|
||||
|| type.name() == "unsigned char" || type.name() == "int"
|
||||
|| type.name() == "size_t" || type.name() == "double");
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -97,6 +99,13 @@ void Argument::matlab_unwrap(FileWriter& file, const string& matlabName) const {
|
|||
file.oss << ");" << endl;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void Argument::proxy_check(FileWriter& proxyFile, const string& s) const {
|
||||
proxyFile.oss << "isa(" << s << ",'" << matlabClass(".") << "')";
|
||||
if (type.name() == "Vector")
|
||||
proxyFile.oss << " && size(" << s << ",2)==1";
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
string ArgumentList::types() const {
|
||||
string str;
|
||||
|
|
@ -104,7 +113,7 @@ string ArgumentList::types() const {
|
|||
BOOST_FOREACH(Argument arg, *this) {
|
||||
if (!first)
|
||||
str += ",";
|
||||
str += arg.type.name;
|
||||
str += arg.type.name();
|
||||
first = false;
|
||||
}
|
||||
return str;
|
||||
|
|
@ -116,14 +125,14 @@ string ArgumentList::signature() const {
|
|||
bool cap = false;
|
||||
|
||||
BOOST_FOREACH(Argument arg, *this) {
|
||||
BOOST_FOREACH(char ch, arg.type.name)
|
||||
BOOST_FOREACH(char ch, arg.type.name())
|
||||
if (isupper(ch)) {
|
||||
sig += ch;
|
||||
//If there is a capital letter, we don't want to read it below
|
||||
cap = true;
|
||||
}
|
||||
if (!cap)
|
||||
sig += arg.type.name[0];
|
||||
sig += arg.type.name()[0];
|
||||
//Reset to default
|
||||
cap = false;
|
||||
}
|
||||
|
|
@ -170,25 +179,14 @@ void ArgumentList::emit_prototype(FileWriter& file, const string& name) const {
|
|||
BOOST_FOREACH(Argument arg, *this) {
|
||||
if (!first)
|
||||
file.oss << ", ";
|
||||
file.oss << arg.type.name << " " << arg.name;
|
||||
file.oss << arg.type.name() << " " << arg.name;
|
||||
first = false;
|
||||
}
|
||||
file.oss << ")";
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void ArgumentList::emit_call(FileWriter& proxyFile,
|
||||
const ReturnValue& returnVal, const string& wrapperName, int id,
|
||||
bool staticMethod) const {
|
||||
returnVal.emit_matlab(proxyFile);
|
||||
proxyFile.oss << wrapperName << "(" << id;
|
||||
if (!staticMethod)
|
||||
proxyFile.oss << ", this";
|
||||
proxyFile.oss << ", varargin{:});\n";
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
void ArgumentList::emit_conditional_call(FileWriter& proxyFile,
|
||||
const ReturnValue& returnVal, const string& wrapperName, int id,
|
||||
bool staticMethod) const {
|
||||
void ArgumentList::proxy_check(FileWriter& proxyFile) const {
|
||||
// Check nr of arguments
|
||||
proxyFile.oss << "if length(varargin) == " << size();
|
||||
if (size() > 0)
|
||||
|
|
@ -198,15 +196,12 @@ void ArgumentList::emit_conditional_call(FileWriter& proxyFile,
|
|||
for (size_t i = 0; i < size(); i++) {
|
||||
if (!first)
|
||||
proxyFile.oss << " && ";
|
||||
proxyFile.oss << "isa(varargin{" << i + 1 << "},'"
|
||||
<< (*this)[i].matlabClass(".") << "')";
|
||||
string s = "varargin{" + boost::lexical_cast<string>(i + 1) + "}";
|
||||
(*this)[i].proxy_check(proxyFile, s);
|
||||
first = false;
|
||||
}
|
||||
proxyFile.oss << "\n";
|
||||
|
||||
// output call to C++ wrapper
|
||||
proxyFile.oss << " ";
|
||||
emit_call(proxyFile, returnVal, wrapperName, id, staticMethod);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
|
|
|||
123
wrap/Argument.h
123
wrap/Argument.h
|
|
@ -28,13 +28,23 @@ namespace wrap {
|
|||
/// Argument class
|
||||
struct Argument {
|
||||
Qualified type;
|
||||
bool is_const, is_ref, is_ptr;
|
||||
std::string name;
|
||||
bool is_const, is_ref, is_ptr;
|
||||
|
||||
Argument() :
|
||||
is_const(false), is_ref(false), is_ptr(false) {
|
||||
}
|
||||
|
||||
Argument(const Qualified& t, const std::string& n) :
|
||||
type(t), name(n), is_const(false), is_ref(false), is_ptr(false) {
|
||||
}
|
||||
|
||||
bool operator==(const Argument& other) const {
|
||||
return type == other.type && name == other.name
|
||||
&& is_const == other.is_const && is_ref == other.is_ref
|
||||
&& is_ptr == other.is_ptr;
|
||||
}
|
||||
|
||||
Argument expandTemplate(const TemplateSubstitution& ts) const;
|
||||
|
||||
/// return MATLAB class for use in isa(x,class)
|
||||
|
|
@ -46,6 +56,12 @@ struct Argument {
|
|||
/// MATLAB code generation, MATLAB to C++
|
||||
void matlab_unwrap(FileWriter& file, const std::string& matlabName) const;
|
||||
|
||||
/**
|
||||
* emit checking argument to MATLAB proxy
|
||||
* @param proxyFile output stream
|
||||
*/
|
||||
void proxy_check(FileWriter& proxyFile, const std::string& s) const;
|
||||
|
||||
friend std::ostream& operator<<(std::ostream& os, const Argument& arg) {
|
||||
os << (arg.is_const ? "const " : "") << arg.type << (arg.is_ptr ? "*" : "")
|
||||
<< (arg.is_ref ? "&" : "");
|
||||
|
|
@ -88,26 +104,12 @@ struct ArgumentList: public std::vector<Argument> {
|
|||
void emit_prototype(FileWriter& file, const std::string& name) const;
|
||||
|
||||
/**
|
||||
* emit emit MATLAB call to proxy
|
||||
* emit checking arguments to MATLAB proxy
|
||||
* @param proxyFile output stream
|
||||
* @param returnVal the return value
|
||||
* @param wrapperName of method or function
|
||||
* @param staticMethod flag to emit "this" in call
|
||||
*/
|
||||
void emit_call(FileWriter& proxyFile, const ReturnValue& returnVal,
|
||||
const std::string& wrapperName, int id, bool staticMethod = false) const;
|
||||
|
||||
/**
|
||||
* emit conditional MATLAB call to proxy (checking arguments first)
|
||||
* @param proxyFile output stream
|
||||
* @param returnVal the return value
|
||||
* @param wrapperName of method or function
|
||||
* @param staticMethod flag to emit "this" in call
|
||||
*/
|
||||
void emit_conditional_call(FileWriter& proxyFile,
|
||||
const ReturnValue& returnVal, const std::string& wrapperName, int id,
|
||||
bool staticMethod = false) const;
|
||||
void proxy_check(FileWriter& proxyFile) const;
|
||||
|
||||
/// Output stream operator
|
||||
friend std::ostream& operator<<(std::ostream& os,
|
||||
const ArgumentList& argList) {
|
||||
os << "(";
|
||||
|
|
@ -122,5 +124,88 @@ struct ArgumentList: public std::vector<Argument> {
|
|||
|
||||
};
|
||||
|
||||
} // \namespace wrap
|
||||
/* ************************************************************************* */
|
||||
// http://boost-spirit.com/distrib/spirit_1_8_2/libs/spirit/doc/grammar.html
|
||||
struct ArgumentGrammar: public classic::grammar<ArgumentGrammar> {
|
||||
|
||||
wrap::Argument& result_; ///< successful parse will be placed in here
|
||||
TypeGrammar argument_type_g; ///< Type parser for Argument::type
|
||||
|
||||
/// Construct type grammar and specify where result is placed
|
||||
ArgumentGrammar(wrap::Argument& result) :
|
||||
result_(result), argument_type_g(result.type) {
|
||||
}
|
||||
|
||||
/// Definition of type grammar
|
||||
template<typename ScannerT>
|
||||
struct definition: BasicRules<ScannerT> {
|
||||
|
||||
typedef classic::rule<ScannerT> Rule;
|
||||
|
||||
Rule argument_p;
|
||||
|
||||
definition(ArgumentGrammar const& self) {
|
||||
using namespace classic;
|
||||
|
||||
// NOTE: allows for pointers to all types
|
||||
// Slightly more permissive than before on basis/eigen type qualification
|
||||
// Also, currently parses Point2*&, can't make it work otherwise :-(
|
||||
argument_p = !str_p("const")[assign_a(self.result_.is_const, T)] //
|
||||
>> self.argument_type_g //
|
||||
>> !ch_p('*')[assign_a(self.result_.is_ptr, T)]
|
||||
>> !ch_p('&')[assign_a(self.result_.is_ref, T)]
|
||||
>> BasicRules<ScannerT>::name_p[assign_a(self.result_.name)];
|
||||
}
|
||||
|
||||
Rule const& start() const {
|
||||
return argument_p;
|
||||
}
|
||||
|
||||
};
|
||||
};
|
||||
// ArgumentGrammar
|
||||
|
||||
/* ************************************************************************* */
|
||||
// http://boost-spirit.com/distrib/spirit_1_8_2/libs/spirit/doc/grammar.html
|
||||
struct ArgumentListGrammar: public classic::grammar<ArgumentListGrammar> {
|
||||
|
||||
wrap::ArgumentList& result_; ///< successful parse will be placed in here
|
||||
|
||||
/// Construct type grammar and specify where result is placed
|
||||
ArgumentListGrammar(wrap::ArgumentList& result) :
|
||||
result_(result) {
|
||||
}
|
||||
|
||||
/// Definition of type grammar
|
||||
template<typename ScannerT>
|
||||
struct definition {
|
||||
|
||||
const Argument arg0; ///< used to reset arg
|
||||
Argument arg; ///< temporary argument for use during parsing
|
||||
ArgumentGrammar argument_g; ///< single Argument parser
|
||||
|
||||
classic::rule<ScannerT> argument_p, argumentList_p;
|
||||
|
||||
definition(ArgumentListGrammar const& self) :
|
||||
argument_g(arg) {
|
||||
using namespace classic;
|
||||
|
||||
argument_p = argument_g //
|
||||
[classic::push_back_a(self.result_, arg)] //
|
||||
[assign_a(arg, arg0)];
|
||||
|
||||
argumentList_p = '(' >> !argument_p >> *(',' >> argument_p) >> ')';
|
||||
}
|
||||
|
||||
classic::rule<ScannerT> const& start() const {
|
||||
return argumentList_p;
|
||||
}
|
||||
|
||||
};
|
||||
};
|
||||
// ArgumentListGrammar
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
}// \namespace wrap
|
||||
|
||||
|
|
|
|||
157
wrap/Class.cpp
157
wrap/Class.cpp
|
|
@ -22,23 +22,68 @@
|
|||
|
||||
#include <boost/foreach.hpp>
|
||||
#include <boost/lexical_cast.hpp>
|
||||
#include <boost/range/adaptor/map.hpp>
|
||||
#include <boost/range/algorithm/copy.hpp>
|
||||
|
||||
#include <vector>
|
||||
#include <iostream>
|
||||
#include <fstream>
|
||||
#include <iterator> // std::ostream_iterator
|
||||
//#include <cstdint> // on Linux GCC: fails with error regarding needing C++0x std flags
|
||||
//#include <cinttypes> // same failure as above
|
||||
#include <stdint.h> // works on Linux GCC
|
||||
using namespace std;
|
||||
using namespace wrap;
|
||||
|
||||
/* ************************************************************************* */
|
||||
void Class::assignParent(const Qualified& parent) {
|
||||
parentClass.reset(parent);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
boost::optional<string> Class::qualifiedParent() const {
|
||||
boost::optional<string> result = boost::none;
|
||||
if (parentClass)
|
||||
result = parentClass->qualifiedName("::");
|
||||
return result;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
static void handleException(const out_of_range& oor,
|
||||
const Class::Methods& methods) {
|
||||
cerr << "Class::method: key not found: " << oor.what() << ", methods are:\n";
|
||||
using boost::adaptors::map_keys;
|
||||
ostream_iterator<string> out_it(cerr, "\n");
|
||||
boost::copy(methods | map_keys, out_it);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Method& Class::mutableMethod(Str key) {
|
||||
try {
|
||||
return methods_.at(key);
|
||||
} catch (const out_of_range& oor) {
|
||||
handleException(oor, methods_);
|
||||
throw runtime_error("Internal error in wrap");
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
const Method& Class::method(Str key) const {
|
||||
try {
|
||||
return methods_.at(key);
|
||||
} catch (const out_of_range& oor) {
|
||||
handleException(oor, methods_);
|
||||
throw runtime_error("Internal error in wrap");
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void Class::matlab_proxy(Str toolboxPath, Str wrapperName,
|
||||
const TypeAttributesTable& typeAttributes, FileWriter& wrapperFile,
|
||||
vector<string>& functionNames) const {
|
||||
|
||||
// Create namespace folders
|
||||
createNamespaceStructure(namespaces, toolboxPath);
|
||||
createNamespaceStructure(namespaces(), toolboxPath);
|
||||
|
||||
// open destination classFile
|
||||
string classFile = matlabName(toolboxPath);
|
||||
|
|
@ -48,21 +93,20 @@ void Class::matlab_proxy(Str toolboxPath, Str wrapperName,
|
|||
const string matlabQualName = qualifiedName(".");
|
||||
const string matlabUniqueName = qualifiedName();
|
||||
const string cppName = qualifiedName("::");
|
||||
const string matlabBaseName = qualifiedParent.qualifiedName(".");
|
||||
const string cppBaseName = qualifiedParent.qualifiedName("::");
|
||||
|
||||
// emit class proxy code
|
||||
// we want our class to inherit the handle class for memory purposes
|
||||
const string parent = qualifiedParent.empty() ? "handle" : matlabBaseName;
|
||||
const string parent =
|
||||
parentClass ? parentClass->qualifiedName(".") : "handle";
|
||||
comment_fragment(proxyFile);
|
||||
proxyFile.oss << "classdef " << name << " < " << parent << endl;
|
||||
proxyFile.oss << "classdef " << name() << " < " << parent << endl;
|
||||
proxyFile.oss << " properties\n";
|
||||
proxyFile.oss << " ptr_" << matlabUniqueName << " = 0\n";
|
||||
proxyFile.oss << " end\n";
|
||||
proxyFile.oss << " methods\n";
|
||||
|
||||
// Constructor
|
||||
proxyFile.oss << " function obj = " << name << "(varargin)\n";
|
||||
proxyFile.oss << " function obj = " << name() << "(varargin)\n";
|
||||
// Special pointer constructors - one in MATLAB to create an object and
|
||||
// assign a pointer returned from a C++ function. In turn this MATLAB
|
||||
// constructor calls a special C++ function that just adds the object to
|
||||
|
|
@ -75,11 +119,12 @@ void Class::matlab_proxy(Str toolboxPath, Str wrapperName,
|
|||
wrapperFile.oss << "\n";
|
||||
|
||||
// Regular constructors
|
||||
boost::optional<string> cppBaseName = qualifiedParent();
|
||||
for (size_t i = 0; i < constructor.nrOverloads(); i++) {
|
||||
ArgumentList args = constructor.argumentList(i);
|
||||
const int id = (int) functionNames.size();
|
||||
constructor.proxy_fragment(proxyFile, wrapperName, !qualifiedParent.empty(),
|
||||
id, args);
|
||||
constructor.proxy_fragment(proxyFile, wrapperName, (bool) parentClass, id,
|
||||
args);
|
||||
const string wrapFunctionName = constructor.wrapper_fragment(wrapperFile,
|
||||
cppName, matlabUniqueName, cppBaseName, id, args);
|
||||
wrapperFile.oss << "\n";
|
||||
|
|
@ -89,9 +134,9 @@ void Class::matlab_proxy(Str toolboxPath, Str wrapperName,
|
|||
proxyFile.oss << " error('Arguments do not match any overload of "
|
||||
<< matlabQualName << " constructor');\n";
|
||||
proxyFile.oss << " end\n";
|
||||
if (!qualifiedParent.empty())
|
||||
proxyFile.oss << " obj = obj@" << matlabBaseName << "(uint64("
|
||||
<< ptr_constructor_key << "), base_ptr);\n";
|
||||
if (parentClass)
|
||||
proxyFile.oss << " obj = obj@" << parentClass->qualifiedName(".")
|
||||
<< "(uint64(" << ptr_constructor_key << "), base_ptr);\n";
|
||||
proxyFile.oss << " obj.ptr_" << matlabUniqueName << " = my_ptr;\n";
|
||||
proxyFile.oss << " end\n\n";
|
||||
|
||||
|
|
@ -111,7 +156,7 @@ void Class::matlab_proxy(Str toolboxPath, Str wrapperName,
|
|||
<< " function disp(obj), obj.display; end\n %DISP Calls print on the object\n";
|
||||
|
||||
// Methods
|
||||
BOOST_FOREACH(const Methods::value_type& name_m, methods) {
|
||||
BOOST_FOREACH(const Methods::value_type& name_m, methods_) {
|
||||
const Method& m = name_m.second;
|
||||
m.proxy_wrapper_fragments(proxyFile, wrapperFile, cppName, matlabQualName,
|
||||
matlabUniqueName, wrapperName, typeAttributes, functionNames);
|
||||
|
|
@ -151,7 +196,6 @@ void Class::pointer_constructor_fragments(FileWriter& proxyFile,
|
|||
|
||||
const string matlabUniqueName = qualifiedName();
|
||||
const string cppName = qualifiedName("::");
|
||||
const string baseCppName = qualifiedParent.qualifiedName("::");
|
||||
|
||||
const int collectorInsertId = (int) functionNames.size();
|
||||
const string collectorInsertFunctionName = matlabUniqueName
|
||||
|
|
@ -188,7 +232,7 @@ void Class::pointer_constructor_fragments(FileWriter& proxyFile,
|
|||
} else {
|
||||
proxyFile.oss << " my_ptr = varargin{2};\n";
|
||||
}
|
||||
if (qualifiedParent.empty()) // If this class has a base class, we'll get a base class pointer back
|
||||
if (!parentClass) // If this class has a base class, we'll get a base class pointer back
|
||||
proxyFile.oss << " ";
|
||||
else
|
||||
proxyFile.oss << " base_ptr = ";
|
||||
|
|
@ -211,9 +255,10 @@ void Class::pointer_constructor_fragments(FileWriter& proxyFile,
|
|||
// Add to collector
|
||||
wrapperFile.oss << " collector_" << matlabUniqueName << ".insert(self);\n";
|
||||
// If we have a base class, return the base class pointer (MATLAB will call the base class collectorInsertAndMakeBase to add this to the collector and recurse the heirarchy)
|
||||
if (!qualifiedParent.empty()) {
|
||||
boost::optional<string> cppBaseName = qualifiedParent();
|
||||
if (cppBaseName) {
|
||||
wrapperFile.oss << "\n";
|
||||
wrapperFile.oss << " typedef boost::shared_ptr<" << baseCppName
|
||||
wrapperFile.oss << " typedef boost::shared_ptr<" << *cppBaseName
|
||||
<< "> SharedBase;\n";
|
||||
wrapperFile.oss
|
||||
<< " out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);\n";
|
||||
|
|
@ -244,10 +289,10 @@ void Class::pointer_constructor_fragments(FileWriter& proxyFile,
|
|||
/* ************************************************************************* */
|
||||
Class Class::expandTemplate(const TemplateSubstitution& ts) const {
|
||||
Class inst = *this;
|
||||
inst.methods = expandMethodTemplate(methods, ts);
|
||||
inst.methods_ = expandMethodTemplate(methods_, ts);
|
||||
inst.static_methods = expandMethodTemplate(static_methods, ts);
|
||||
inst.constructor = constructor.expandTemplate(ts);
|
||||
inst.deconstructor.name = inst.name;
|
||||
inst.deconstructor.name = inst.name();
|
||||
return inst;
|
||||
}
|
||||
|
||||
|
|
@ -257,10 +302,10 @@ vector<Class> Class::expandTemplate(Str templateArg,
|
|||
vector<Class> result;
|
||||
BOOST_FOREACH(const Qualified& instName, instantiations) {
|
||||
Qualified expandedClass = (Qualified) (*this);
|
||||
expandedClass.name += instName.name;
|
||||
expandedClass.expand(instName.name());
|
||||
const TemplateSubstitution ts(templateArg, instName, expandedClass);
|
||||
Class inst = expandTemplate(ts);
|
||||
inst.name = expandedClass.name;
|
||||
inst.name_ = expandedClass.name();
|
||||
inst.templateArgs.clear();
|
||||
inst.typedefName = qualifiedName("::") + "<" + instName.qualifiedName("::")
|
||||
+ ">";
|
||||
|
|
@ -272,50 +317,50 @@ vector<Class> Class::expandTemplate(Str templateArg,
|
|||
/* ************************************************************************* */
|
||||
void Class::addMethod(bool verbose, bool is_const, Str methodName,
|
||||
const ArgumentList& argumentList, const ReturnValue& returnValue,
|
||||
Str templateArgName, const vector<Qualified>& templateArgValues) {
|
||||
const Template& tmplate) {
|
||||
// Check if templated
|
||||
if (!templateArgName.empty() && templateArgValues.size() > 0) {
|
||||
if (tmplate.valid()) {
|
||||
// Create method to expand
|
||||
// For all values of the template argument, create a new method
|
||||
BOOST_FOREACH(const Qualified& instName, templateArgValues) {
|
||||
const TemplateSubstitution ts(templateArgName, instName, this->name);
|
||||
BOOST_FOREACH(const Qualified& instName, tmplate.argValues()) {
|
||||
const TemplateSubstitution ts(tmplate.argName(), instName, *this);
|
||||
// substitute template in arguments
|
||||
ArgumentList expandedArgs = argumentList.expandTemplate(ts);
|
||||
// do the same for return type
|
||||
ReturnValue expandedRetVal = returnValue.expandTemplate(ts);
|
||||
// Now stick in new overload stack with expandedMethodName key
|
||||
// but note we use the same, unexpanded methodName in overload
|
||||
string expandedMethodName = methodName + instName.name;
|
||||
methods[expandedMethodName].addOverload(methodName, expandedArgs,
|
||||
string expandedMethodName = methodName + instName.name();
|
||||
methods_[expandedMethodName].addOverload(methodName, expandedArgs,
|
||||
expandedRetVal, is_const, instName, verbose);
|
||||
}
|
||||
} else
|
||||
// just add overload
|
||||
methods[methodName].addOverload(methodName, argumentList, returnValue,
|
||||
is_const, Qualified(), verbose);
|
||||
methods_[methodName].addOverload(methodName, argumentList, returnValue,
|
||||
is_const, boost::none, verbose);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void Class::erase_serialization() {
|
||||
Methods::iterator it = methods.find("serializable");
|
||||
if (it != methods.end()) {
|
||||
Methods::iterator it = methods_.find("serializable");
|
||||
if (it != methods_.end()) {
|
||||
#ifndef WRAP_DISABLE_SERIALIZE
|
||||
isSerializable = true;
|
||||
#else
|
||||
// cout << "Ignoring serializable() flag in class " << name << endl;
|
||||
#endif
|
||||
methods.erase(it);
|
||||
methods_.erase(it);
|
||||
}
|
||||
|
||||
it = methods.find("serialize");
|
||||
if (it != methods.end()) {
|
||||
it = methods_.find("serialize");
|
||||
if (it != methods_.end()) {
|
||||
#ifndef WRAP_DISABLE_SERIALIZE
|
||||
isSerializable = true;
|
||||
hasSerialization = true;
|
||||
#else
|
||||
// cout << "Ignoring serialize() flag in class " << name << endl;
|
||||
#endif
|
||||
methods.erase(it);
|
||||
methods_.erase(it);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -327,31 +372,31 @@ void Class::verifyAll(vector<string>& validTypes, bool& hasSerialiable) const {
|
|||
// verify all of the function arguments
|
||||
//TODO:verifyArguments<ArgumentList>(validTypes, constructor.args_list);
|
||||
verifyArguments<StaticMethod>(validTypes, static_methods);
|
||||
verifyArguments<Method>(validTypes, methods);
|
||||
verifyArguments<Method>(validTypes, methods_);
|
||||
|
||||
// verify function return types
|
||||
verifyReturnTypes<StaticMethod>(validTypes, static_methods);
|
||||
verifyReturnTypes<Method>(validTypes, methods);
|
||||
verifyReturnTypes<Method>(validTypes, methods_);
|
||||
|
||||
// verify parents
|
||||
if (!qualifiedParent.empty()
|
||||
&& find(validTypes.begin(), validTypes.end(),
|
||||
qualifiedParent.qualifiedName("::")) == validTypes.end())
|
||||
throw DependencyMissing(qualifiedParent.qualifiedName("::"),
|
||||
qualifiedName("::"));
|
||||
boost::optional<string> parent = qualifiedParent();
|
||||
if (parent
|
||||
&& find(validTypes.begin(), validTypes.end(), *parent)
|
||||
== validTypes.end())
|
||||
throw DependencyMissing(*parent, qualifiedName("::"));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void Class::appendInheritedMethods(const Class& cls,
|
||||
const vector<Class>& classes) {
|
||||
|
||||
if (!cls.qualifiedParent.empty()) {
|
||||
if (cls.parentClass) {
|
||||
|
||||
// Find parent
|
||||
BOOST_FOREACH(const Class& parent, classes) {
|
||||
// We found a parent class for our parent, TODO improve !
|
||||
if (parent.name == cls.qualifiedParent.name) {
|
||||
methods.insert(parent.methods.begin(), parent.methods.end());
|
||||
if (parent.name() == cls.parentClass->name()) {
|
||||
methods_.insert(parent.methods_.begin(), parent.methods_.end());
|
||||
appendInheritedMethods(parent, classes);
|
||||
}
|
||||
}
|
||||
|
|
@ -361,11 +406,11 @@ void Class::appendInheritedMethods(const Class& cls,
|
|||
/* ************************************************************************* */
|
||||
string Class::getTypedef() const {
|
||||
string result;
|
||||
BOOST_FOREACH(Str namesp, namespaces) {
|
||||
BOOST_FOREACH(Str namesp, namespaces()) {
|
||||
result += ("namespace " + namesp + " { ");
|
||||
}
|
||||
result += ("typedef " + typedefName + " " + name + ";");
|
||||
for (size_t i = 0; i < namespaces.size(); ++i) {
|
||||
result += ("typedef " + typedefName + " " + name() + ";");
|
||||
for (size_t i = 0; i < namespaces().size(); ++i) {
|
||||
result += " }";
|
||||
}
|
||||
return result;
|
||||
|
|
@ -373,15 +418,15 @@ string Class::getTypedef() const {
|
|||
|
||||
/* ************************************************************************* */
|
||||
void Class::comment_fragment(FileWriter& proxyFile) const {
|
||||
proxyFile.oss << "%class " << name << ", see Doxygen page for details\n";
|
||||
proxyFile.oss << "%class " << name() << ", see Doxygen page for details\n";
|
||||
proxyFile.oss
|
||||
<< "%at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html\n";
|
||||
|
||||
constructor.comment_fragment(proxyFile);
|
||||
|
||||
if (!methods.empty())
|
||||
if (!methods_.empty())
|
||||
proxyFile.oss << "%\n%-------Methods-------\n";
|
||||
BOOST_FOREACH(const Methods::value_type& name_m, methods)
|
||||
BOOST_FOREACH(const Methods::value_type& name_m, methods_)
|
||||
name_m.second.comment_fragment(proxyFile);
|
||||
|
||||
if (!static_methods.empty())
|
||||
|
|
@ -393,7 +438,7 @@ void Class::comment_fragment(FileWriter& proxyFile) const {
|
|||
proxyFile.oss << "%\n%-------Serialization Interface-------\n";
|
||||
proxyFile.oss << "%string_serialize() : returns string\n";
|
||||
proxyFile.oss << "%string_deserialize(string serialized) : returns "
|
||||
<< this->name << "\n";
|
||||
<< name() << "\n";
|
||||
}
|
||||
|
||||
proxyFile.oss << "%\n";
|
||||
|
|
@ -586,12 +631,12 @@ string Class::getSerializationExport() const {
|
|||
|
||||
/* ************************************************************************* */
|
||||
void Class::python_wrapper(FileWriter& wrapperFile) const {
|
||||
wrapperFile.oss << "class_<" << name << ">(\"" << name << "\")\n";
|
||||
constructor.python_wrapper(wrapperFile, name);
|
||||
wrapperFile.oss << "class_<" << name() << ">(\"" << name() << "\")\n";
|
||||
constructor.python_wrapper(wrapperFile, name());
|
||||
BOOST_FOREACH(const StaticMethod& m, static_methods | boost::adaptors::map_values)
|
||||
m.python_wrapper(wrapperFile, name);
|
||||
BOOST_FOREACH(const Method& m, methods | boost::adaptors::map_values)
|
||||
m.python_wrapper(wrapperFile, name);
|
||||
m.python_wrapper(wrapperFile, name());
|
||||
BOOST_FOREACH(const Method& m, methods_ | boost::adaptors::map_values)
|
||||
m.python_wrapper(wrapperFile, name());
|
||||
wrapperFile.oss << ";\n\n";
|
||||
}
|
||||
|
||||
|
|
|
|||
173
wrap/Class.h
173
wrap/Class.h
|
|
@ -19,14 +19,29 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include "spirit.h"
|
||||
#include "Template.h"
|
||||
#include "Constructor.h"
|
||||
#include "Deconstructor.h"
|
||||
#include "Method.h"
|
||||
#include "StaticMethod.h"
|
||||
#include "TypeAttributesTable.h"
|
||||
|
||||
#ifdef __GNUC__
|
||||
#pragma GCC diagnostic push
|
||||
#pragma GCC diagnostic ignored "-Wunused-variable"
|
||||
#endif
|
||||
#include <boost/lambda/bind.hpp>
|
||||
#include <boost/lambda/lambda.hpp>
|
||||
#ifdef __GNUC__
|
||||
#pragma GCC diagnostic pop
|
||||
#endif
|
||||
|
||||
namespace bl = boost::lambda;
|
||||
|
||||
#include <boost/foreach.hpp>
|
||||
#include <boost/range/adaptor/map.hpp>
|
||||
#include <boost/optional.hpp>
|
||||
|
||||
#include <string>
|
||||
#include <map>
|
||||
|
|
@ -36,13 +51,20 @@ namespace wrap {
|
|||
/// Class has name, constructors, methods
|
||||
class Class: public Qualified {
|
||||
|
||||
public:
|
||||
typedef const std::string& Str;
|
||||
typedef std::map<std::string, Method> Methods;
|
||||
Methods methods; ///< Class methods
|
||||
typedef std::map<std::string, StaticMethod> StaticMethods;
|
||||
|
||||
private:
|
||||
|
||||
boost::optional<Qualified> parentClass; ///< The *single* parent
|
||||
Methods methods_; ///< Class methods
|
||||
Method& mutableMethod(Str key);
|
||||
|
||||
public:
|
||||
|
||||
typedef const std::string& Str;
|
||||
typedef std::map<std::string, StaticMethod> StaticMethods;
|
||||
StaticMethods static_methods; ///< Static methods
|
||||
|
||||
// Then the instance variables are set directly by the Module constructor
|
||||
std::vector<std::string> templateArgs; ///< Template arguments
|
||||
|
|
@ -50,26 +72,28 @@ public:
|
|||
bool isVirtual; ///< Whether the class is part of a virtual inheritance chain
|
||||
bool isSerializable; ///< Whether we can use boost.serialization to serialize the class - creates exports
|
||||
bool hasSerialization; ///< Whether we should create the serialization functions
|
||||
Qualified qualifiedParent; ///< The *single* parent
|
||||
StaticMethods static_methods; ///< Static methods
|
||||
Constructor constructor; ///< Class constructors
|
||||
Deconstructor deconstructor; ///< Deconstructor to deallocate C++ object
|
||||
bool verbose_; ///< verbose flag
|
||||
|
||||
/// Constructor creates an empty class
|
||||
Class(bool verbose = true) :
|
||||
isVirtual(false), isSerializable(false), hasSerialization(false), deconstructor(
|
||||
verbose), verbose_(verbose) {
|
||||
parentClass(boost::none), isVirtual(false), isSerializable(false), hasSerialization(
|
||||
false), deconstructor(verbose), verbose_(verbose) {
|
||||
}
|
||||
|
||||
void assignParent(const Qualified& parent);
|
||||
|
||||
boost::optional<std::string> qualifiedParent() const;
|
||||
|
||||
size_t nrMethods() const {
|
||||
return methods.size();
|
||||
}
|
||||
Method& method(Str name) {
|
||||
return methods.at(name);
|
||||
return methods_.size();
|
||||
}
|
||||
|
||||
const Method& method(Str key) const;
|
||||
|
||||
bool exists(Str name) const {
|
||||
return methods.find(name) != methods.end();
|
||||
return methods_.find(name) != methods_.end();
|
||||
}
|
||||
|
||||
// And finally MATLAB code is emitted, methods below called by Module::matlab_code
|
||||
|
|
@ -85,7 +109,7 @@ public:
|
|||
/// Add potentially overloaded, potentially templated method
|
||||
void addMethod(bool verbose, bool is_const, Str methodName,
|
||||
const ArgumentList& argumentList, const ReturnValue& returnValue,
|
||||
Str templateArgName, const std::vector<Qualified>& templateArgValues);
|
||||
const Template& tmplate);
|
||||
|
||||
/// Post-process classes for serialization markers
|
||||
void erase_serialization(); // non-const !
|
||||
|
|
@ -115,11 +139,11 @@ public:
|
|||
void python_wrapper(FileWriter& wrapperFile) const;
|
||||
|
||||
friend std::ostream& operator<<(std::ostream& os, const Class& cls) {
|
||||
os << "class " << cls.name << "{\n";
|
||||
os << "class " << cls.name() << "{\n";
|
||||
os << cls.constructor << ";\n";
|
||||
BOOST_FOREACH(const StaticMethod& m, cls.static_methods | boost::adaptors::map_values)
|
||||
os << m << ";\n";
|
||||
BOOST_FOREACH(const Method& m, cls.methods | boost::adaptors::map_values)
|
||||
BOOST_FOREACH(const Method& m, cls.methods_ | boost::adaptors::map_values)
|
||||
os << m << ";\n";
|
||||
os << "};" << std::endl;
|
||||
return os;
|
||||
|
|
@ -134,5 +158,122 @@ private:
|
|||
void comment_fragment(FileWriter& proxyFile) const;
|
||||
};
|
||||
|
||||
} // \namespace wrap
|
||||
/* ************************************************************************* */
|
||||
// http://boost-spirit.com/distrib/spirit_1_8_2/libs/spirit/doc/grammar.html
|
||||
struct ClassGrammar: public classic::grammar<ClassGrammar> {
|
||||
|
||||
Class& cls_; ///< successful parse will be placed in here
|
||||
Template& template_; ///< result needs to be visible outside
|
||||
|
||||
/// Construct type grammar and specify where result is placed
|
||||
ClassGrammar(Class& cls, Template& t) :
|
||||
cls_(cls), template_(t) {
|
||||
}
|
||||
|
||||
/// Definition of type grammar
|
||||
template<typename ScannerT>
|
||||
struct definition: BasicRules<ScannerT> {
|
||||
|
||||
using BasicRules<ScannerT>::name_p;
|
||||
using BasicRules<ScannerT>::className_p;
|
||||
using BasicRules<ScannerT>::comments_p;
|
||||
|
||||
// NOTE: allows for pointers to all types
|
||||
ArgumentList args;
|
||||
ArgumentListGrammar argumentList_g;
|
||||
|
||||
Constructor constructor0, constructor;
|
||||
|
||||
ReturnValue retVal0, retVal;
|
||||
ReturnValueGrammar returnValue_g;
|
||||
|
||||
Template methodTemplate;
|
||||
TemplateGrammar methodTemplate_g, classTemplate_g;
|
||||
|
||||
std::string methodName;
|
||||
bool isConst, T, F;
|
||||
|
||||
// Parent class
|
||||
Qualified possibleParent;
|
||||
TypeGrammar classParent_g;
|
||||
|
||||
classic::rule<ScannerT> constructor_p, methodName_p, method_p,
|
||||
staticMethodName_p, static_method_p, templateList_p, classParent_p,
|
||||
functions_p, class_p;
|
||||
|
||||
definition(ClassGrammar const& self) :
|
||||
argumentList_g(args), returnValue_g(retVal), //
|
||||
methodTemplate_g(methodTemplate), classTemplate_g(self.template_), //
|
||||
T(true), F(false), classParent_g(possibleParent) {
|
||||
|
||||
using namespace classic;
|
||||
bool verbose = false; // TODO
|
||||
|
||||
// ConstructorGrammar
|
||||
constructor_p = (className_p >> argumentList_g >> ';' >> !comments_p) //
|
||||
[bl::bind(&Constructor::push_back, bl::var(constructor),
|
||||
bl::var(args))] //
|
||||
[clear_a(args)];
|
||||
|
||||
// MethodGrammar
|
||||
methodName_p = lexeme_d[(upper_p | lower_p) >> *(alnum_p | '_')];
|
||||
|
||||
// gtsam::Values retract(const gtsam::VectorValues& delta) const;
|
||||
method_p = !methodTemplate_g
|
||||
>> (returnValue_g >> methodName_p[assign_a(methodName)]
|
||||
>> argumentList_g >> !str_p("const")[assign_a(isConst, T)] >> ';'
|
||||
>> *comments_p) //
|
||||
[bl::bind(&Class::addMethod, bl::var(self.cls_), verbose,
|
||||
bl::var(isConst), bl::var(methodName), bl::var(args),
|
||||
bl::var(retVal), bl::var(methodTemplate))] //
|
||||
[assign_a(retVal, retVal0)][clear_a(args)] //
|
||||
[clear_a(methodTemplate)][assign_a(isConst, F)];
|
||||
|
||||
// StaticMethodGrammar
|
||||
staticMethodName_p = lexeme_d[(upper_p | lower_p) >> *(alnum_p | '_')];
|
||||
|
||||
static_method_p = (str_p("static") >> returnValue_g
|
||||
>> staticMethodName_p[assign_a(methodName)] >> argumentList_g >> ';'
|
||||
>> *comments_p) //
|
||||
[bl::bind(&StaticMethod::addOverload,
|
||||
bl::var(self.cls_.static_methods)[bl::var(methodName)],
|
||||
bl::var(methodName), bl::var(args), bl::var(retVal), boost::none,
|
||||
verbose)] //
|
||||
[assign_a(retVal, retVal0)][clear_a(args)];
|
||||
|
||||
// template<POSE, POINT>
|
||||
templateList_p = (str_p("template") >> '<'
|
||||
>> name_p[push_back_a(self.cls_.templateArgs)]
|
||||
>> *(',' >> name_p[push_back_a(self.cls_.templateArgs)]) >> '>');
|
||||
|
||||
// parse a full class
|
||||
classParent_p = (':' >> classParent_g >> '{') //
|
||||
[bl::bind(&Class::assignParent, bl::var(self.cls_),
|
||||
bl::var(possibleParent))][clear_a(possibleParent)];
|
||||
|
||||
functions_p = constructor_p | method_p | static_method_p;
|
||||
|
||||
// parse a full class
|
||||
class_p = (!(classTemplate_g[push_back_a(self.cls_.templateArgs,
|
||||
self.template_.argName())] | templateList_p)
|
||||
>> !(str_p("virtual")[assign_a(self.cls_.isVirtual, T)])
|
||||
>> str_p("class") >> className_p[assign_a(self.cls_.name_)]
|
||||
>> (classParent_p | '{') >> //
|
||||
*(functions_p | comments_p) >> str_p("};")) //
|
||||
[bl::bind(&Constructor::initializeOrCheck, bl::var(constructor),
|
||||
bl::var(self.cls_.name_), boost::none, verbose)][assign_a(
|
||||
self.cls_.constructor, constructor)] //
|
||||
[assign_a(self.cls_.deconstructor.name, self.cls_.name_)] //
|
||||
[assign_a(constructor, constructor0)];
|
||||
}
|
||||
|
||||
classic::rule<ScannerT> const& start() const {
|
||||
return class_p;
|
||||
}
|
||||
|
||||
};
|
||||
};
|
||||
// ClassGrammar
|
||||
|
||||
}// \namespace wrap
|
||||
|
||||
|
|
|
|||
|
|
@ -69,7 +69,7 @@ void Constructor::proxy_fragment(FileWriter& file,
|
|||
|
||||
/* ************************************************************************* */
|
||||
string Constructor::wrapper_fragment(FileWriter& file, Str cppClassName,
|
||||
Str matlabUniqueName, Str cppBaseClassName, int id,
|
||||
Str matlabUniqueName, boost::optional<string> cppBaseClassName, int id,
|
||||
const ArgumentList& al) const {
|
||||
|
||||
const string wrapFunctionName = matlabUniqueName + "_constructor_"
|
||||
|
|
@ -100,9 +100,9 @@ string Constructor::wrapper_fragment(FileWriter& file, Str cppClassName,
|
|||
<< endl;
|
||||
|
||||
// If we have a base class, return the base class pointer (MATLAB will call the base class collectorInsertAndMakeBase to add this to the collector and recurse the heirarchy)
|
||||
if (!cppBaseClassName.empty()) {
|
||||
if (cppBaseClassName) {
|
||||
file.oss << "\n";
|
||||
file.oss << " typedef boost::shared_ptr<" << cppBaseClassName
|
||||
file.oss << " typedef boost::shared_ptr<" << *cppBaseClassName
|
||||
<< "> SharedBase;\n";
|
||||
file.oss
|
||||
<< " out[1] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);\n";
|
||||
|
|
|
|||
|
|
@ -19,7 +19,7 @@
|
|||
#pragma once
|
||||
|
||||
#include "OverloadedFunction.h"
|
||||
|
||||
#include <boost/optional.hpp>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
|
|
@ -68,7 +68,7 @@ struct Constructor: public OverloadedFunction {
|
|||
|
||||
/// cpp wrapper
|
||||
std::string wrapper_fragment(FileWriter& file, Str cppClassName,
|
||||
Str matlabUniqueName, Str cppBaseClassName, int id,
|
||||
Str matlabUniqueName, boost::optional<std::string> cppBaseClassName, int id,
|
||||
const ArgumentList& al) const;
|
||||
|
||||
/// constructor function
|
||||
|
|
|
|||
|
|
@ -27,6 +27,7 @@ namespace wrap {
|
|||
std::string name;
|
||||
bool isVirtual;
|
||||
ForwardDeclaration() : isVirtual(false) {}
|
||||
ForwardDeclaration(const std::string& s) : name(s), isVirtual(false) {}
|
||||
};
|
||||
|
||||
}
|
||||
|
|
@ -109,8 +109,8 @@ class FullyOverloadedFunction: public Function, public SignatureOverloads {
|
|||
public:
|
||||
|
||||
bool addOverload(const std::string& name, const ArgumentList& args,
|
||||
const ReturnValue& retVal, const Qualified& instName = Qualified(),
|
||||
bool verbose = false) {
|
||||
const ReturnValue& retVal, boost::optional<const Qualified> instName =
|
||||
boost::none, bool verbose = false) {
|
||||
bool first = initializeOrCheck(name, instName, verbose);
|
||||
SignatureOverloads::push_back(args, retVal);
|
||||
return first;
|
||||
|
|
|
|||
|
|
@ -29,12 +29,11 @@ using namespace std;
|
|||
using namespace wrap;
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool Function::initializeOrCheck(const std::string& name,
|
||||
const Qualified& instName, bool verbose) {
|
||||
bool Function::initializeOrCheck(const string& name,
|
||||
boost::optional<const Qualified> instName, bool verbose) {
|
||||
|
||||
if (name.empty())
|
||||
throw std::runtime_error(
|
||||
"Function::initializeOrCheck called with empty name");
|
||||
throw runtime_error("Function::initializeOrCheck called with empty name");
|
||||
|
||||
// Check if this overload is give to the correct method
|
||||
if (name_.empty()) {
|
||||
|
|
@ -43,15 +42,38 @@ bool Function::initializeOrCheck(const std::string& name,
|
|||
verbose_ = verbose;
|
||||
return true;
|
||||
} else {
|
||||
if (name_ != name || templateArgValue_ != instName || verbose_ != verbose)
|
||||
throw std::runtime_error(
|
||||
"Function::initializeOrCheck called with different arguments: with name "
|
||||
+ name + " instead of expected " + name_
|
||||
+ ", or with template argument " + instName.qualifiedName(":")
|
||||
+ " instead of expected " + templateArgValue_.qualifiedName(":"));
|
||||
if (name_ != name || verbose_ != verbose
|
||||
|| ((bool) templateArgValue_ != (bool) instName)
|
||||
|| ((bool) templateArgValue_ && (bool) instName
|
||||
&& !(*templateArgValue_ == *instName)))
|
||||
throw runtime_error(
|
||||
"Function::initializeOrCheck called with different arguments");
|
||||
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void Function::emit_call(FileWriter& proxyFile, const ReturnValue& returnVal,
|
||||
const string& wrapperName, int id) const {
|
||||
returnVal.emit_matlab(proxyFile);
|
||||
proxyFile.oss << wrapperName << "(" << id;
|
||||
if (!isStatic())
|
||||
proxyFile.oss << ", this";
|
||||
proxyFile.oss << ", varargin{:});\n";
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void Function::emit_conditional_call(FileWriter& proxyFile,
|
||||
const ReturnValue& returnVal, const ArgumentList& args,
|
||||
const string& wrapperName, int id) const {
|
||||
|
||||
// Check all arguments
|
||||
args.proxy_check(proxyFile);
|
||||
|
||||
// output call to C++ wrapper
|
||||
proxyFile.oss << " ";
|
||||
emit_call(proxyFile, returnVal, wrapperName, id);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -19,6 +19,7 @@
|
|||
#pragma once
|
||||
|
||||
#include "Argument.h"
|
||||
#include <boost/optional.hpp>
|
||||
|
||||
namespace wrap {
|
||||
|
||||
|
|
@ -28,7 +29,7 @@ class Function {
|
|||
protected:
|
||||
|
||||
std::string name_; ///< name of method
|
||||
Qualified templateArgValue_; ///< value of template argument if applicable
|
||||
boost::optional<Qualified> templateArgValue_; ///< value of template argument if applicable
|
||||
bool verbose_;
|
||||
|
||||
public:
|
||||
|
|
@ -37,19 +38,35 @@ public:
|
|||
* @brief first time, fill in instance variables, otherwise check if same
|
||||
* @return true if first time, false thereafter
|
||||
*/
|
||||
bool initializeOrCheck(const std::string& name, const Qualified& instName =
|
||||
Qualified(), bool verbose = false);
|
||||
bool initializeOrCheck(const std::string& name,
|
||||
boost::optional<const Qualified> instName = boost::none, bool verbose =
|
||||
false);
|
||||
|
||||
std::string name() const {
|
||||
return name_;
|
||||
}
|
||||
|
||||
std::string matlabName() const {
|
||||
if (templateArgValue_.empty())
|
||||
return name_;
|
||||
else
|
||||
return name_ + templateArgValue_.name;
|
||||
/// Only Methods are non-static
|
||||
virtual bool isStatic() const {
|
||||
return true;
|
||||
}
|
||||
|
||||
std::string matlabName() const {
|
||||
if (templateArgValue_)
|
||||
return name_ + templateArgValue_->name();
|
||||
else
|
||||
return name_;
|
||||
}
|
||||
|
||||
/// Emit function call to MATLAB (no argument checking)
|
||||
void emit_call(FileWriter& proxyFile, const ReturnValue& returnVal,
|
||||
const std::string& wrapperName, int id) const;
|
||||
|
||||
/// Emit checking arguments and function call to MATLAB
|
||||
void emit_conditional_call(FileWriter& proxyFile,
|
||||
const ReturnValue& returnVal, const ArgumentList& args,
|
||||
const std::string& wrapperName, int id) const;
|
||||
|
||||
};
|
||||
|
||||
} // \namespace wrap
|
||||
|
|
|
|||
|
|
@ -18,9 +18,9 @@ using namespace std;
|
|||
/* ************************************************************************* */
|
||||
void GlobalFunction::addOverload(const Qualified& overload,
|
||||
const ArgumentList& args, const ReturnValue& retVal,
|
||||
const Qualified& instName, bool verbose) {
|
||||
string name(overload.name);
|
||||
FullyOverloadedFunction::addOverload(name, args, retVal, instName, verbose);
|
||||
boost::optional<const Qualified> instName, bool verbose) {
|
||||
FullyOverloadedFunction::addOverload(overload.name(), args, retVal, instName,
|
||||
verbose);
|
||||
overloads.push_back(overload);
|
||||
}
|
||||
|
||||
|
|
@ -37,7 +37,7 @@ void GlobalFunction::matlab_proxy(const string& toolboxPath,
|
|||
for (size_t i = 0; i < overloads.size(); ++i) {
|
||||
Qualified overload = overloads.at(i);
|
||||
// use concatenated namespaces as key
|
||||
string str_ns = qualifiedName("", overload.namespaces);
|
||||
string str_ns = qualifiedName("", overload.namespaces());
|
||||
const ReturnValue& ret = returnValue(i);
|
||||
const ArgumentList& args = argumentList(i);
|
||||
grouped_functions[str_ns].addOverload(overload, args, ret);
|
||||
|
|
@ -59,7 +59,7 @@ void GlobalFunction::generateSingleFunction(const string& toolboxPath,
|
|||
|
||||
// create the folder for the namespace
|
||||
const Qualified& overload1 = overloads.front();
|
||||
createNamespaceStructure(overload1.namespaces, toolboxPath);
|
||||
createNamespaceStructure(overload1.namespaces(), toolboxPath);
|
||||
|
||||
// open destination mfunctionFileName
|
||||
string mfunctionFileName = overload1.matlabName(toolboxPath);
|
||||
|
|
@ -80,7 +80,7 @@ void GlobalFunction::generateSingleFunction(const string& toolboxPath,
|
|||
|
||||
// Output proxy matlab code
|
||||
mfunctionFile.oss << " " << (i == 0 ? "" : "else");
|
||||
args.emit_conditional_call(mfunctionFile, returnVal, wrapperName, id, true); // true omits "this"
|
||||
emit_conditional_call(mfunctionFile, returnVal, args, wrapperName, id);
|
||||
|
||||
// Output C++ wrapper code
|
||||
|
||||
|
|
@ -104,7 +104,7 @@ void GlobalFunction::generateSingleFunction(const string& toolboxPath,
|
|||
args.matlab_unwrap(file, 0); // We start at 0 because there is no self object
|
||||
|
||||
// call method with default type and wrap result
|
||||
if (returnVal.type1.name != "void")
|
||||
if (returnVal.type1.name() != "void")
|
||||
returnVal.wrap_result(cppName + "(" + args.names() + ")", file,
|
||||
typeAttributes);
|
||||
else
|
||||
|
|
|
|||
|
|
@ -11,6 +11,18 @@
|
|||
|
||||
#include "FullyOverloadedFunction.h"
|
||||
|
||||
#ifdef __GNUC__
|
||||
#pragma GCC diagnostic push
|
||||
#pragma GCC diagnostic ignored "-Wunused-variable"
|
||||
#endif
|
||||
#include <boost/lambda/bind.hpp>
|
||||
#include <boost/lambda/lambda.hpp>
|
||||
#ifdef __GNUC__
|
||||
#pragma GCC diagnostic pop
|
||||
#endif
|
||||
|
||||
namespace bl = boost::lambda;
|
||||
|
||||
namespace wrap {
|
||||
|
||||
struct GlobalFunction: public FullyOverloadedFunction {
|
||||
|
|
@ -19,8 +31,8 @@ struct GlobalFunction: public FullyOverloadedFunction {
|
|||
|
||||
// adds an overloaded version of this function,
|
||||
void addOverload(const Qualified& overload, const ArgumentList& args,
|
||||
const ReturnValue& retVal, const Qualified& instName = Qualified(),
|
||||
bool verbose = false);
|
||||
const ReturnValue& retVal, boost::optional<const Qualified> instName =
|
||||
boost::none, bool verbose = false);
|
||||
|
||||
void verifyArguments(const std::vector<std::string>& validArgs) const {
|
||||
SignatureOverloads::verifyArguments(validArgs, name_);
|
||||
|
|
@ -47,5 +59,67 @@ private:
|
|||
|
||||
};
|
||||
|
||||
} // \namespace wrap
|
||||
typedef std::map<std::string, GlobalFunction> GlobalFunctions;
|
||||
|
||||
/* ************************************************************************* */
|
||||
// http://boost-spirit.com/distrib/spirit_1_8_2/libs/spirit/doc/grammar.html
|
||||
struct GlobalFunctionGrammar: public classic::grammar<GlobalFunctionGrammar> {
|
||||
|
||||
GlobalFunctions& global_functions_; ///< successful parse will be placed in here
|
||||
std::vector<std::string>& namespaces_;
|
||||
|
||||
/// Construct type grammar and specify where result is placed
|
||||
GlobalFunctionGrammar(GlobalFunctions& global_functions,
|
||||
std::vector<std::string>& namespaces) :
|
||||
global_functions_(global_functions), namespaces_(namespaces) {
|
||||
}
|
||||
|
||||
/// Definition of type grammar
|
||||
template<typename ScannerT>
|
||||
struct definition: BasicRules<ScannerT> {
|
||||
|
||||
// using BasicRules<ScannerT>::name_p;
|
||||
// using BasicRules<ScannerT>::className_p;
|
||||
using BasicRules<ScannerT>::comments_p;
|
||||
|
||||
ArgumentList args;
|
||||
ArgumentListGrammar argumentList_g;
|
||||
|
||||
ReturnValue retVal0, retVal;
|
||||
ReturnValueGrammar returnValue_g;
|
||||
|
||||
Qualified globalFunction;
|
||||
|
||||
classic::rule<ScannerT> globalFunctionName_p, global_function_p;
|
||||
|
||||
definition(GlobalFunctionGrammar const& self) :
|
||||
argumentList_g(args), returnValue_g(retVal) {
|
||||
|
||||
using namespace classic;
|
||||
bool verbose = false; // TODO
|
||||
|
||||
globalFunctionName_p = lexeme_d[(upper_p | lower_p) >> *(alnum_p | '_')];
|
||||
|
||||
// parse a global function
|
||||
global_function_p = (returnValue_g
|
||||
>> globalFunctionName_p[assign_a(globalFunction.name_)]
|
||||
>> argumentList_g >> ';' >> *comments_p) //
|
||||
[assign_a(globalFunction.namespaces_, self.namespaces_)][bl::bind(
|
||||
&GlobalFunction::addOverload,
|
||||
bl::var(self.global_functions_)[bl::var(globalFunction.name_)],
|
||||
bl::var(globalFunction), bl::var(args), bl::var(retVal),
|
||||
boost::none, verbose)] //
|
||||
[assign_a(retVal, retVal0)][clear_a(globalFunction)][clear_a(args)];
|
||||
|
||||
}
|
||||
|
||||
classic::rule<ScannerT> const& start() const {
|
||||
return global_function_p;
|
||||
}
|
||||
|
||||
};
|
||||
};
|
||||
// GlobalFunctionGrammar
|
||||
|
||||
}// \namespace wrap
|
||||
|
||||
|
|
|
|||
|
|
@ -30,9 +30,9 @@ using namespace wrap;
|
|||
|
||||
/* ************************************************************************* */
|
||||
bool Method::addOverload(Str name, const ArgumentList& args,
|
||||
const ReturnValue& retVal, bool is_const, const Qualified& instName,
|
||||
bool verbose) {
|
||||
bool first = StaticMethod::addOverload(name, args, retVal, instName, verbose);
|
||||
const ReturnValue& retVal, bool is_const,
|
||||
boost::optional<const Qualified> instName, bool verbose) {
|
||||
bool first = MethodBase::addOverload(name, args, retVal, instName, verbose);
|
||||
if (first)
|
||||
is_const_ = is_const;
|
||||
else if (is_const && !is_const_)
|
||||
|
|
@ -52,14 +52,12 @@ void Method::proxy_header(FileWriter& proxyFile) const {
|
|||
|
||||
/* ************************************************************************* */
|
||||
string Method::wrapper_call(FileWriter& wrapperFile, Str cppClassName,
|
||||
Str matlabUniqueName, const ArgumentList& args,
|
||||
const ReturnValue& returnVal, const TypeAttributesTable& typeAttributes,
|
||||
const Qualified& instName) const {
|
||||
Str matlabUniqueName, const ArgumentList& args) const {
|
||||
// check arguments
|
||||
// extra argument obj -> nargin-1 is passed !
|
||||
// example: checkArguments("equals",nargout,nargin-1,2);
|
||||
wrapperFile.oss << " checkArguments(\"" << name_ << "\",nargout,nargin-1,"
|
||||
<< args.size() << ");\n";
|
||||
wrapperFile.oss << " checkArguments(\"" << matlabName()
|
||||
<< "\",nargout,nargin-1," << args.size() << ");\n";
|
||||
|
||||
// get class pointer
|
||||
// example: shared_ptr<Test> = unwrap_shared_ptr< Test >(in[0], "Test");
|
||||
|
|
@ -72,8 +70,8 @@ string Method::wrapper_call(FileWriter& wrapperFile, Str cppClassName,
|
|||
// call method and wrap result
|
||||
// example: out[0]=wrap<bool>(obj->return_field(t));
|
||||
string expanded = "obj->" + name_;
|
||||
if (!instName.empty())
|
||||
expanded += ("<" + instName.qualifiedName("::") + ">");
|
||||
if (templateArgValue_)
|
||||
expanded += ("<" + templateArgValue_->qualifiedName("::") + ">");
|
||||
|
||||
return expanded;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -18,12 +18,12 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include "StaticMethod.h"
|
||||
#include "MethodBase.h"
|
||||
|
||||
namespace wrap {
|
||||
|
||||
/// Method class
|
||||
class Method: public StaticMethod {
|
||||
class Method: public MethodBase {
|
||||
|
||||
bool is_const_;
|
||||
|
||||
|
|
@ -32,8 +32,9 @@ public:
|
|||
typedef const std::string& Str;
|
||||
|
||||
bool addOverload(Str name, const ArgumentList& args,
|
||||
const ReturnValue& retVal, bool is_const, const Qualified& instName =
|
||||
Qualified(), bool verbose = false);
|
||||
const ReturnValue& retVal, bool is_const,
|
||||
boost::optional<const Qualified> instName = boost::none, bool verbose =
|
||||
false);
|
||||
|
||||
virtual bool isStatic() const {
|
||||
return false;
|
||||
|
|
@ -55,9 +56,7 @@ private:
|
|||
void proxy_header(FileWriter& proxyFile) const;
|
||||
|
||||
virtual std::string wrapper_call(FileWriter& wrapperFile, Str cppClassName,
|
||||
Str matlabUniqueName, const ArgumentList& args,
|
||||
const ReturnValue& returnVal, const TypeAttributesTable& typeAttributes,
|
||||
const Qualified& instName) const;
|
||||
Str matlabUniqueName, const ArgumentList& args) const;
|
||||
};
|
||||
|
||||
} // \namespace wrap
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file MethodBase.ccp
|
||||
* @author Frank Dellaert
|
||||
* @author Andrew Melim
|
||||
* @author Richard Roberts
|
||||
**/
|
||||
|
||||
#include "Method.h"
|
||||
#include "utilities.h"
|
||||
|
||||
#include <boost/foreach.hpp>
|
||||
#include <boost/lexical_cast.hpp>
|
||||
#include <boost/algorithm/string.hpp>
|
||||
|
||||
#include <iostream>
|
||||
#include <fstream>
|
||||
|
||||
using namespace std;
|
||||
using namespace wrap;
|
||||
|
||||
/* ************************************************************************* */
|
||||
void MethodBase::proxy_wrapper_fragments(FileWriter& proxyFile,
|
||||
FileWriter& wrapperFile, Str cppClassName, Str matlabQualName,
|
||||
Str matlabUniqueName, Str wrapperName,
|
||||
const TypeAttributesTable& typeAttributes,
|
||||
vector<string>& functionNames) const {
|
||||
|
||||
// emit header, e.g., function varargout = templatedMethod(this, varargin)
|
||||
proxy_header(proxyFile);
|
||||
|
||||
// Emit comments for documentation
|
||||
string up_name = boost::to_upper_copy(matlabName());
|
||||
proxyFile.oss << " % " << up_name << " usage: ";
|
||||
usage_fragment(proxyFile, matlabName());
|
||||
|
||||
// Emit URL to Doxygen page
|
||||
proxyFile.oss << " % "
|
||||
<< "Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html"
|
||||
<< endl;
|
||||
|
||||
// Handle special case of single overload with all numeric arguments
|
||||
if (nrOverloads() == 1 && argumentList(0).allScalar()) {
|
||||
// Output proxy matlab code
|
||||
// TODO: document why is it OK to not check arguments in this case
|
||||
proxyFile.oss << " ";
|
||||
const int id = (int) functionNames.size();
|
||||
emit_call(proxyFile, returnValue(0), wrapperName, id);
|
||||
|
||||
// Output C++ wrapper code
|
||||
const string wrapFunctionName = wrapper_fragment(wrapperFile, cppClassName,
|
||||
matlabUniqueName, 0, id, typeAttributes);
|
||||
|
||||
// Add to function list
|
||||
functionNames.push_back(wrapFunctionName);
|
||||
} else {
|
||||
// Check arguments for all overloads
|
||||
for (size_t i = 0; i < nrOverloads(); ++i) {
|
||||
|
||||
// Output proxy matlab code
|
||||
proxyFile.oss << " " << (i == 0 ? "" : "else");
|
||||
const int id = (int) functionNames.size();
|
||||
emit_conditional_call(proxyFile, returnValue(i), argumentList(i),
|
||||
wrapperName, id);
|
||||
|
||||
// Output C++ wrapper code
|
||||
const string wrapFunctionName = wrapper_fragment(wrapperFile,
|
||||
cppClassName, matlabUniqueName, i, id, typeAttributes);
|
||||
|
||||
// Add to function list
|
||||
functionNames.push_back(wrapFunctionName);
|
||||
}
|
||||
proxyFile.oss << " else\n";
|
||||
proxyFile.oss
|
||||
<< " error('Arguments do not match any overload of function "
|
||||
<< matlabQualName << "." << name_ << "');" << endl;
|
||||
proxyFile.oss << " end\n";
|
||||
}
|
||||
|
||||
proxyFile.oss << " end\n";
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
string MethodBase::wrapper_fragment(FileWriter& wrapperFile, Str cppClassName,
|
||||
Str matlabUniqueName, int overload, int id,
|
||||
const TypeAttributesTable& typeAttributes) const {
|
||||
|
||||
// generate code
|
||||
|
||||
const string wrapFunctionName = matlabUniqueName + "_" + name_ + "_"
|
||||
+ boost::lexical_cast<string>(id);
|
||||
|
||||
const ArgumentList& args = argumentList(overload);
|
||||
const ReturnValue& returnVal = returnValue(overload);
|
||||
|
||||
// call
|
||||
wrapperFile.oss << "void " << wrapFunctionName
|
||||
<< "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n";
|
||||
// start
|
||||
wrapperFile.oss << "{\n";
|
||||
|
||||
returnVal.wrapTypeUnwrap(wrapperFile);
|
||||
|
||||
wrapperFile.oss << " typedef boost::shared_ptr<" << cppClassName
|
||||
<< "> Shared;" << endl;
|
||||
|
||||
// get call
|
||||
// for static methods: cppClassName::staticMethod<TemplateVal>
|
||||
// for instance methods: obj->instanceMethod<TemplateVal>
|
||||
string expanded = wrapper_call(wrapperFile, cppClassName, matlabUniqueName,
|
||||
args);
|
||||
|
||||
expanded += ("(" + args.names() + ")");
|
||||
if (returnVal.type1.name() != "void")
|
||||
returnVal.wrap_result(expanded, wrapperFile, typeAttributes);
|
||||
else
|
||||
wrapperFile.oss << " " + expanded + ";\n";
|
||||
|
||||
// finish
|
||||
wrapperFile.oss << "}\n";
|
||||
|
||||
return wrapFunctionName;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void MethodBase::python_wrapper(FileWriter& wrapperFile, Str className) const {
|
||||
wrapperFile.oss << " .def(\"" << name_ << "\", &" << className << "::"
|
||||
<< name_ << ");\n";
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -0,0 +1,67 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 MethodBase.h
|
||||
* @brief describes and generates code for static methods
|
||||
* @author Frank Dellaert
|
||||
* @author Alex Cunningham
|
||||
* @author Richard Roberts
|
||||
**/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "FullyOverloadedFunction.h"
|
||||
|
||||
namespace wrap {
|
||||
|
||||
/// MethodBase class
|
||||
struct MethodBase: public FullyOverloadedFunction {
|
||||
|
||||
typedef const std::string& Str;
|
||||
|
||||
// emit a list of comments, one for each overload
|
||||
void comment_fragment(FileWriter& proxyFile) const {
|
||||
SignatureOverloads::comment_fragment(proxyFile, matlabName());
|
||||
}
|
||||
|
||||
void verifyArguments(const std::vector<std::string>& validArgs) const {
|
||||
SignatureOverloads::verifyArguments(validArgs, name_);
|
||||
}
|
||||
|
||||
void verifyReturnTypes(const std::vector<std::string>& validtypes) const {
|
||||
SignatureOverloads::verifyReturnTypes(validtypes, name_);
|
||||
}
|
||||
|
||||
// MATLAB code generation
|
||||
// classPath is class directory, e.g., ../matlab/@Point2
|
||||
void proxy_wrapper_fragments(FileWriter& proxyFile, FileWriter& wrapperFile,
|
||||
Str cppClassName, Str matlabQualName, Str matlabUniqueName,
|
||||
Str wrapperName, const TypeAttributesTable& typeAttributes,
|
||||
std::vector<std::string>& functionNames) const;
|
||||
|
||||
// emit python wrapper
|
||||
void python_wrapper(FileWriter& wrapperFile, Str className) const;
|
||||
|
||||
protected:
|
||||
|
||||
virtual void proxy_header(FileWriter& proxyFile) const = 0;
|
||||
|
||||
std::string wrapper_fragment(FileWriter& wrapperFile, Str cppClassName,
|
||||
Str matlabUniqueName, int overload, int id,
|
||||
const TypeAttributesTable& typeAttributes) const; ///< cpp wrapper
|
||||
|
||||
virtual std::string wrapper_call(FileWriter& wrapperFile, Str cppClassName,
|
||||
Str matlabUniqueName, const ArgumentList& args) const = 0;
|
||||
};
|
||||
|
||||
} // \namespace wrap
|
||||
|
||||
279
wrap/Module.cpp
279
wrap/Module.cpp
|
|
@ -22,22 +22,6 @@
|
|||
#include "TypeAttributesTable.h"
|
||||
#include "utilities.h"
|
||||
|
||||
//#define BOOST_SPIRIT_DEBUG
|
||||
#include "spirit_actors.h"
|
||||
|
||||
#include <boost/spirit/include/classic_confix.hpp>
|
||||
#include <boost/spirit/include/classic_clear_actor.hpp>
|
||||
#include <boost/spirit/include/classic_insert_at_actor.hpp>
|
||||
#ifdef __GNUC__
|
||||
#pragma GCC diagnostic push
|
||||
#pragma GCC diagnostic ignored "-Wunused-variable"
|
||||
#endif
|
||||
#include <boost/lambda/bind.hpp>
|
||||
#include <boost/lambda/lambda.hpp>
|
||||
#ifdef __GNUC__
|
||||
#pragma GCC diagnostic pop
|
||||
#endif
|
||||
#include <boost/lambda/construct.hpp>
|
||||
#include <boost/foreach.hpp>
|
||||
#include <boost/filesystem.hpp>
|
||||
#include <boost/lexical_cast.hpp>
|
||||
|
|
@ -51,8 +35,6 @@ using namespace BOOST_SPIRIT_CLASSIC_NS;
|
|||
namespace bl = boost::lambda;
|
||||
namespace fs = boost::filesystem;
|
||||
|
||||
typedef rule<BOOST_SPIRIT_CLASSIC_NS::phrase_scanner_t> Rule;
|
||||
|
||||
/* ************************************************************************* */
|
||||
// We parse an interface file into a Module object.
|
||||
// The grammar is defined using the boost/spirit combinatorial parser.
|
||||
|
|
@ -102,7 +84,6 @@ Module::Module(const string& interfacePath,
|
|||
void Module::parseMarkup(const std::string& data) {
|
||||
// The parse imperatively :-( updates variables gradually during parse
|
||||
// The one with postfix 0 are used to reset the variables after parse.
|
||||
vector<string> namespaces; // current namespace tag
|
||||
|
||||
//----------------------------------------------------------------------------
|
||||
// Grammar with actions that build the Class object. Actions are
|
||||
|
|
@ -113,211 +94,40 @@ void Module::parseMarkup(const std::string& data) {
|
|||
// http://www.boost.org/doc/libs/1_37_0/libs/spirit/classic/doc/directives.html
|
||||
// ----------------------------------------------------------------------------
|
||||
|
||||
Rule comments_p = comment_p("/*", "*/") | comment_p("//", eol_p);
|
||||
// Define Rule and instantiate basic rules
|
||||
typedef rule<phrase_scanner_t> Rule;
|
||||
BasicRules<phrase_scanner_t> basic;
|
||||
|
||||
Rule basisType_p =
|
||||
(str_p("string") | "bool" | "size_t" | "int" | "double" | "char" | "unsigned char");
|
||||
vector<string> namespaces; // current namespace tag
|
||||
|
||||
Rule keywords_p =
|
||||
(str_p("const") | "static" | "namespace" | "void" | basisType_p);
|
||||
|
||||
Rule eigenType_p =
|
||||
(str_p("Vector") | "Matrix");
|
||||
|
||||
//Rule for STL Containers (class names are lowercase)
|
||||
Rule stlType_p = (str_p("vector") | "list");
|
||||
|
||||
Rule className_p = (lexeme_d[upper_p >> *(alnum_p | '_')] - eigenType_p - keywords_p) | stlType_p;
|
||||
|
||||
Rule namespace_name_p = lexeme_d[lower_p >> *(alnum_p | '_')] - keywords_p;
|
||||
|
||||
Argument arg0, arg;
|
||||
Rule namespace_arg_p = namespace_name_p
|
||||
[push_back_a(arg.type.namespaces)] >> str_p("::");
|
||||
|
||||
Rule argEigenType_p =
|
||||
eigenType_p[assign_a(arg.type.name)];
|
||||
|
||||
Rule eigenRef_p =
|
||||
!str_p("const") [assign_a(arg.is_const,true)] >>
|
||||
eigenType_p [assign_a(arg.type.name)] >>
|
||||
ch_p('&') [assign_a(arg.is_ref,true)];
|
||||
|
||||
Rule classArg_p =
|
||||
!str_p("const") [assign_a(arg.is_const,true)] >>
|
||||
*namespace_arg_p >>
|
||||
className_p[assign_a(arg.type.name)] >>
|
||||
!ch_p('&')[assign_a(arg.is_ref,true)];
|
||||
|
||||
Rule name_p = lexeme_d[alpha_p >> *(alnum_p | '_')];
|
||||
|
||||
// TODO, do we really need cls here? Non-local
|
||||
// parse a full class
|
||||
Class cls0(verbose),cls(verbose);
|
||||
Rule classParent_p =
|
||||
*(namespace_name_p[push_back_a(cls.qualifiedParent.namespaces)] >> str_p("::")) >>
|
||||
className_p[assign_a(cls.qualifiedParent.name)];
|
||||
|
||||
// parse "gtsam::Pose2" and add to templateArgValues
|
||||
Qualified templateArgValue;
|
||||
vector<Qualified> templateArgValues;
|
||||
Rule templateArgValue_p =
|
||||
(*(namespace_name_p[push_back_a(templateArgValue.namespaces)] >> str_p("::")) >>
|
||||
className_p[assign_a(templateArgValue.name)])
|
||||
[push_back_a(templateArgValues, templateArgValue)]
|
||||
[clear_a(templateArgValue)];
|
||||
|
||||
// template<CALIBRATION = {gtsam::Cal3DS2}>
|
||||
string templateArgName;
|
||||
Rule templateArgValues_p =
|
||||
(str_p("template") >>
|
||||
'<' >> name_p[assign_a(templateArgName)] >> '=' >>
|
||||
'{' >> !(templateArgValue_p >> *(',' >> templateArgValue_p)) >> '}' >>
|
||||
'>');
|
||||
Template classTemplate;
|
||||
ClassGrammar class_g(cls,classTemplate);
|
||||
Rule class_p = class_g //
|
||||
[assign_a(cls.namespaces_, namespaces)]
|
||||
[bl::bind(&handle_possible_template, bl::var(classes), bl::var(cls),
|
||||
bl::var(classTemplate.argValues()))]
|
||||
[clear_a(classTemplate)] //
|
||||
[assign_a(cls,cls0)];
|
||||
|
||||
// parse "gtsam::Pose2" and add to singleInstantiation.typeList
|
||||
TemplateInstantiationTypedef singleInstantiation;
|
||||
Rule templateSingleInstantiationArg_p =
|
||||
(*(namespace_name_p[push_back_a(templateArgValue.namespaces)] >> str_p("::")) >>
|
||||
className_p[assign_a(templateArgValue.name)])
|
||||
[push_back_a(singleInstantiation.typeList, templateArgValue)]
|
||||
[clear_a(templateArgValue)];
|
||||
TemplateInstantiationTypedef singleInstantiation, singleInstantiation0;
|
||||
TypeListGrammar<'<','>'> typelist_g(singleInstantiation.typeList);
|
||||
|
||||
// typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Point2> RangeFactorPosePoint2;
|
||||
TemplateInstantiationTypedef singleInstantiation0;
|
||||
TypeGrammar instantiationClass_g(singleInstantiation.class_);
|
||||
Rule templateSingleInstantiation_p =
|
||||
(str_p("typedef") >>
|
||||
*(namespace_name_p[push_back_a(singleInstantiation.class_.namespaces)] >> str_p("::")) >>
|
||||
className_p[assign_a(singleInstantiation.class_.name)] >>
|
||||
'<' >> templateSingleInstantiationArg_p >> *(',' >> templateSingleInstantiationArg_p) >>
|
||||
'>' >>
|
||||
className_p[assign_a(singleInstantiation.name)] >>
|
||||
(str_p("typedef") >> instantiationClass_g >>
|
||||
typelist_g >>
|
||||
basic.className_p[assign_a(singleInstantiation.name_)] >>
|
||||
';')
|
||||
[assign_a(singleInstantiation.namespaces, namespaces)]
|
||||
[assign_a(singleInstantiation.namespaces_, namespaces)]
|
||||
[push_back_a(templateInstantiationTypedefs, singleInstantiation)]
|
||||
[assign_a(singleInstantiation, singleInstantiation0)];
|
||||
|
||||
// template<POSE, POINT>
|
||||
Rule templateList_p =
|
||||
(str_p("template") >>
|
||||
'<' >> name_p[push_back_a(cls.templateArgs)] >> *(',' >> name_p[push_back_a(cls.templateArgs)]) >>
|
||||
'>');
|
||||
|
||||
// NOTE: allows for pointers to all types
|
||||
ArgumentList args;
|
||||
Rule argument_p =
|
||||
((basisType_p[assign_a(arg.type.name)] | argEigenType_p | eigenRef_p | classArg_p)
|
||||
>> !ch_p('*')[assign_a(arg.is_ptr,true)]
|
||||
>> name_p[assign_a(arg.name)])
|
||||
[push_back_a(args, arg)]
|
||||
[assign_a(arg,arg0)];
|
||||
|
||||
Rule argumentList_p = !argument_p >> * (',' >> argument_p);
|
||||
|
||||
// parse class constructor
|
||||
Constructor constructor0(verbose), constructor(verbose);
|
||||
Rule constructor_p =
|
||||
(className_p >> '(' >> argumentList_p >> ')' >> ';' >> !comments_p)
|
||||
[bl::bind(&Constructor::push_back, bl::var(constructor), bl::var(args))]
|
||||
[clear_a(args)];
|
||||
|
||||
vector<string> namespaces_return; /// namespace for current return type
|
||||
Rule namespace_ret_p = namespace_name_p[push_back_a(namespaces_return)] >> str_p("::");
|
||||
|
||||
// HACK: use const values instead of using enums themselves - somehow this doesn't result in values getting assigned to gibberish
|
||||
static const ReturnType::return_category RETURN_EIGEN = ReturnType::EIGEN;
|
||||
static const ReturnType::return_category RETURN_BASIS = ReturnType::BASIS;
|
||||
static const ReturnType::return_category RETURN_CLASS = ReturnType::CLASS;
|
||||
static const ReturnType::return_category RETURN_VOID = ReturnType::VOID;
|
||||
|
||||
ReturnType retType0, retType;
|
||||
Rule returnType_p =
|
||||
(basisType_p[assign_a(retType.name)][assign_a(retType.category, RETURN_BASIS)]) |
|
||||
((*namespace_ret_p)[assign_a(retType.namespaces, namespaces_return)][clear_a(namespaces_return)]
|
||||
>> (className_p[assign_a(retType.name)][assign_a(retType.category, RETURN_CLASS)]) >>
|
||||
!ch_p('*')[assign_a(retType.isPtr,true)]) |
|
||||
(eigenType_p[assign_a(retType.name)][assign_a(retType.category, RETURN_EIGEN)]);
|
||||
|
||||
ReturnValue retVal0, retVal;
|
||||
Rule returnType1_p = returnType_p[assign_a(retVal.type1,retType)][assign_a(retType,retType0)];
|
||||
Rule returnType2_p = returnType_p[assign_a(retVal.type2,retType)][assign_a(retType,retType0)];
|
||||
|
||||
Rule pair_p =
|
||||
(str_p("pair") >> '<' >> returnType1_p >> ',' >> returnType2_p >> '>')
|
||||
[assign_a(retVal.isPair,true)];
|
||||
|
||||
Rule void_p = str_p("void")[assign_a(retVal.type1.name)][assign_a(retVal.type1.category, RETURN_VOID)];
|
||||
|
||||
Rule returnValue_p = void_p | pair_p | returnType1_p;
|
||||
|
||||
Rule methodName_p = lexeme_d[(upper_p | lower_p) >> *(alnum_p | '_')];
|
||||
|
||||
// gtsam::Values retract(const gtsam::VectorValues& delta) const;
|
||||
string methodName;
|
||||
bool isConst, isConst0 = false;
|
||||
Rule method_p =
|
||||
!templateArgValues_p >>
|
||||
(returnValue_p >> methodName_p[assign_a(methodName)] >>
|
||||
'(' >> argumentList_p >> ')' >>
|
||||
!str_p("const")[assign_a(isConst,true)] >> ';' >> *comments_p)
|
||||
[bl::bind(&Class::addMethod, bl::var(cls), verbose, bl::var(isConst),
|
||||
bl::var(methodName), bl::var(args), bl::var(retVal),
|
||||
bl::var(templateArgName), bl::var(templateArgValues))]
|
||||
[assign_a(retVal,retVal0)]
|
||||
[clear_a(args)]
|
||||
[clear_a(templateArgValues)]
|
||||
[assign_a(isConst,isConst0)];
|
||||
|
||||
Rule staticMethodName_p = lexeme_d[(upper_p | lower_p) >> *(alnum_p | '_')];
|
||||
|
||||
Rule static_method_p =
|
||||
(str_p("static") >> returnValue_p >> staticMethodName_p[assign_a(methodName)] >>
|
||||
'(' >> argumentList_p >> ')' >> ';' >> *comments_p)
|
||||
[bl::bind(&StaticMethod::addOverload,
|
||||
bl::var(cls.static_methods)[bl::var(methodName)],
|
||||
bl::var(methodName), bl::var(args), bl::var(retVal), Qualified(),verbose)]
|
||||
[assign_a(retVal,retVal0)]
|
||||
[clear_a(args)];
|
||||
|
||||
Rule functions_p = constructor_p | method_p | static_method_p;
|
||||
|
||||
// parse a full class
|
||||
vector<Qualified> templateInstantiations;
|
||||
Rule class_p =
|
||||
eps_p[assign_a(cls,cls0)]
|
||||
>> (!(templateArgValues_p
|
||||
[push_back_a(cls.templateArgs, templateArgName)]
|
||||
[assign_a(templateInstantiations,templateArgValues)]
|
||||
[clear_a(templateArgValues)]
|
||||
| templateList_p)
|
||||
>> !(str_p("virtual")[assign_a(cls.isVirtual, true)])
|
||||
>> str_p("class")
|
||||
>> className_p[assign_a(cls.name)]
|
||||
>> ((':' >> classParent_p >> '{') | '{')
|
||||
>> *(functions_p | comments_p)
|
||||
>> str_p("};"))
|
||||
[bl::bind(&Constructor::initializeOrCheck, bl::var(constructor),
|
||||
bl::var(cls.name), Qualified(), verbose)]
|
||||
[assign_a(cls.constructor, constructor)]
|
||||
[assign_a(cls.namespaces, namespaces)]
|
||||
[assign_a(cls.deconstructor.name,cls.name)]
|
||||
[bl::bind(&handle_possible_template, bl::var(classes), bl::var(cls),
|
||||
bl::var(templateInstantiations))]
|
||||
[clear_a(templateInstantiations)]
|
||||
[assign_a(constructor, constructor0)]
|
||||
[assign_a(cls,cls0)];
|
||||
|
||||
// parse a global function
|
||||
Qualified globalFunction;
|
||||
Rule global_function_p =
|
||||
(returnValue_p >> staticMethodName_p[assign_a(globalFunction.name)] >>
|
||||
'(' >> argumentList_p >> ')' >> ';' >> *comments_p)
|
||||
[assign_a(globalFunction.namespaces,namespaces)]
|
||||
[bl::bind(&GlobalFunction::addOverload,
|
||||
bl::var(global_functions)[bl::var(globalFunction.name)],
|
||||
bl::var(globalFunction), bl::var(args), bl::var(retVal), Qualified(),verbose)]
|
||||
[assign_a(retVal,retVal0)]
|
||||
[clear_a(globalFunction)]
|
||||
[clear_a(args)];
|
||||
// Create grammar for global functions
|
||||
GlobalFunctionGrammar global_function_g(global_functions,namespaces);
|
||||
|
||||
Rule include_p = str_p("#include") >> ch_p('<') >> (*(anychar_p - '>'))[push_back_a(includes)] >> ch_p('>');
|
||||
|
||||
|
|
@ -328,9 +138,9 @@ void Module::parseMarkup(const std::string& data) {
|
|||
|
||||
Rule namespace_def_p =
|
||||
(str_p("namespace")
|
||||
>> namespace_name_p[push_back_a(namespaces)]
|
||||
>> basic.namespace_p[push_back_a(namespaces)]
|
||||
>> ch_p('{')
|
||||
>> *(include_p | class_p | templateSingleInstantiation_p | global_function_p | namespace_def_p | comments_p)
|
||||
>> *(include_p | class_p | templateSingleInstantiation_p | global_function_g | namespace_def_p | basic.comments_p)
|
||||
>> ch_p('}'))
|
||||
[pop_a(namespaces)];
|
||||
|
||||
|
|
@ -343,47 +153,23 @@ void Module::parseMarkup(const std::string& data) {
|
|||
Rule forward_declaration_p =
|
||||
!(str_p("virtual")[assign_a(fwDec.isVirtual, true)])
|
||||
>> str_p("class")
|
||||
>> (*(namespace_name_p >> str_p("::")) >> className_p)[assign_a(fwDec.name)]
|
||||
>> (*(basic.namespace_p >> str_p("::")) >> basic.className_p)[assign_a(fwDec.name)]
|
||||
>> ch_p(';')
|
||||
[push_back_a(forward_declarations, fwDec)]
|
||||
[assign_a(fwDec, fwDec0)];
|
||||
|
||||
Rule module_content_p = comments_p | include_p | class_p | templateSingleInstantiation_p | forward_declaration_p | global_function_p | namespace_def_p;
|
||||
Rule module_content_p = basic.comments_p | include_p | class_p
|
||||
| templateSingleInstantiation_p | forward_declaration_p
|
||||
| global_function_g | namespace_def_p;
|
||||
|
||||
Rule module_p = *module_content_p >> !end_p;
|
||||
|
||||
//----------------------------------------------------------------------------
|
||||
// for debugging, define BOOST_SPIRIT_DEBUG
|
||||
# ifdef BOOST_SPIRIT_DEBUG
|
||||
BOOST_SPIRIT_DEBUG_NODE(className_p);
|
||||
BOOST_SPIRIT_DEBUG_NODE(classPtr_p);
|
||||
BOOST_SPIRIT_DEBUG_NODE(classRef_p);
|
||||
BOOST_SPIRIT_DEBUG_NODE(basisType_p);
|
||||
BOOST_SPIRIT_DEBUG_NODE(name_p);
|
||||
BOOST_SPIRIT_DEBUG_NODE(argument_p);
|
||||
BOOST_SPIRIT_DEBUG_NODE(argumentList_p);
|
||||
BOOST_SPIRIT_DEBUG_NODE(constructor_p);
|
||||
BOOST_SPIRIT_DEBUG_NODE(returnType1_p);
|
||||
BOOST_SPIRIT_DEBUG_NODE(returnType2_p);
|
||||
BOOST_SPIRIT_DEBUG_NODE(pair_p);
|
||||
BOOST_SPIRIT_DEBUG_NODE(void_p);
|
||||
BOOST_SPIRIT_DEBUG_NODE(returnValue_p);
|
||||
BOOST_SPIRIT_DEBUG_NODE(methodName_p);
|
||||
BOOST_SPIRIT_DEBUG_NODE(method_p);
|
||||
BOOST_SPIRIT_DEBUG_NODE(class_p);
|
||||
BOOST_SPIRIT_DEBUG_NODE(namespace_def_p);
|
||||
BOOST_SPIRIT_DEBUG_NODE(module_p);
|
||||
# endif
|
||||
//----------------------------------------------------------------------------
|
||||
|
||||
// and parse contents
|
||||
parse_info<const char*> info = parse(data.c_str(), module_p, space_p);
|
||||
if(!info.full) {
|
||||
printf("parsing stopped at \n%.20s\n",info.stop);
|
||||
cout << "Stopped near:\n"
|
||||
"class '" << cls.name << "'\n"
|
||||
"method '" << methodName << "'\n"
|
||||
"argument '" << arg.name << "'" << endl;
|
||||
cout << "Stopped in:\n"
|
||||
"class '" << cls.name_ << "'" << endl;
|
||||
throw ParseFailed((int)info.length);
|
||||
}
|
||||
|
||||
|
|
@ -416,6 +202,11 @@ void Module::parseMarkup(const std::string& data) {
|
|||
// Create type attributes table and check validity
|
||||
typeAttributes.addClasses(expandedClasses);
|
||||
typeAttributes.addForwardDeclarations(forward_declarations);
|
||||
// add Eigen types as template arguments are also checked ?
|
||||
vector<ForwardDeclaration> eigen;
|
||||
eigen.push_back(ForwardDeclaration("Vector"));
|
||||
eigen.push_back(ForwardDeclaration("Matrix"));
|
||||
typeAttributes.addForwardDeclarations(eigen);
|
||||
typeAttributes.checkValidity(expandedClasses);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -18,15 +18,15 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <map>
|
||||
|
||||
#include "Class.h"
|
||||
#include "GlobalFunction.h"
|
||||
#include "TemplateInstantiationTypedef.h"
|
||||
#include "ForwardDeclaration.h"
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <map>
|
||||
|
||||
namespace wrap {
|
||||
|
||||
/**
|
||||
|
|
@ -34,9 +34,6 @@ namespace wrap {
|
|||
*/
|
||||
struct Module {
|
||||
|
||||
typedef std::map<std::string, GlobalFunction> GlobalFunctions;
|
||||
typedef std::map<std::string, Method> Methods;
|
||||
|
||||
// Filled during parsing:
|
||||
std::string name; ///< module name
|
||||
bool verbose; ///< verbose flag
|
||||
|
|
|
|||
|
|
@ -87,7 +87,8 @@ class OverloadedFunction: public Function, public ArgumentOverloads {
|
|||
public:
|
||||
|
||||
bool addOverload(const std::string& name, const ArgumentList& args,
|
||||
const Qualified& instName = Qualified(), bool verbose = false) {
|
||||
boost::optional<const Qualified> instName = boost::none, bool verbose =
|
||||
false) {
|
||||
bool first = initializeOrCheck(name, instName, verbose);
|
||||
ArgumentOverloads::push_back(args);
|
||||
return first;
|
||||
|
|
|
|||
218
wrap/Qualified.h
218
wrap/Qualified.h
|
|
@ -18,6 +18,7 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <wrap/spirit.h>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
|
|
@ -26,43 +27,122 @@ namespace wrap {
|
|||
/**
|
||||
* Class to encapuslate a qualified name, i.e., with (nested) namespaces
|
||||
*/
|
||||
struct Qualified {
|
||||
class Qualified {
|
||||
|
||||
std::vector<std::string> namespaces; ///< Stack of namespaces
|
||||
std::string name; ///< type name
|
||||
//protected:
|
||||
public:
|
||||
|
||||
Qualified(const std::string& name_ = "") :
|
||||
name(name_) {
|
||||
std::vector<std::string> namespaces_; ///< Stack of namespaces
|
||||
std::string name_; ///< type name
|
||||
|
||||
friend struct TypeGrammar;
|
||||
friend class TemplateSubstitution;
|
||||
|
||||
public:
|
||||
|
||||
/// the different categories
|
||||
typedef enum {
|
||||
CLASS = 1, EIGEN = 2, BASIS = 3, VOID = 4
|
||||
} Category;
|
||||
Category category;
|
||||
|
||||
Qualified() :
|
||||
category(VOID) {
|
||||
}
|
||||
|
||||
Qualified(const std::string& n, Category c = CLASS) :
|
||||
name_(n), category(c) {
|
||||
}
|
||||
|
||||
Qualified(const std::string& ns1, const std::string& ns2,
|
||||
const std::string& n, Category c = CLASS) :
|
||||
name_(n), category(c) {
|
||||
namespaces_.push_back(ns1);
|
||||
namespaces_.push_back(ns2);
|
||||
}
|
||||
|
||||
Qualified(const std::string& ns1, const std::string& n, Category c = CLASS) :
|
||||
name_(n), category(c) {
|
||||
namespaces_.push_back(ns1);
|
||||
}
|
||||
|
||||
Qualified(std::vector<std::string> ns, const std::string& name) :
|
||||
namespaces_(ns), name_(name), category(CLASS) {
|
||||
}
|
||||
|
||||
std::string name() const {
|
||||
return name_;
|
||||
}
|
||||
|
||||
std::vector<std::string> namespaces() const {
|
||||
return namespaces_;
|
||||
}
|
||||
|
||||
// Qualified is 'abused' as template argument name as well
|
||||
// this function checks whether *this matches with templateArg
|
||||
bool match(const std::string& templateArg) const {
|
||||
return (name_ == templateArg && namespaces_.empty()); //TODO && category == CLASS);
|
||||
}
|
||||
|
||||
void rename(const Qualified& q) {
|
||||
namespaces_ = q.namespaces_;
|
||||
name_ = q.name_;
|
||||
category = q.category;
|
||||
}
|
||||
|
||||
void expand(const std::string& expansion) {
|
||||
name_ += expansion;
|
||||
}
|
||||
|
||||
bool operator==(const Qualified& other) const {
|
||||
return namespaces_ == other.namespaces_ && name_ == other.name_
|
||||
&& category == other.category;
|
||||
}
|
||||
|
||||
bool empty() const {
|
||||
return namespaces.empty() && name.empty();
|
||||
return namespaces_.empty() && name_.empty();
|
||||
}
|
||||
|
||||
void clear() {
|
||||
namespaces.clear();
|
||||
name.clear();
|
||||
virtual void clear() {
|
||||
namespaces_.clear();
|
||||
name_.clear();
|
||||
category = VOID;
|
||||
}
|
||||
|
||||
bool operator!=(const Qualified& other) const {
|
||||
return other.name != name || other.namespaces != namespaces;
|
||||
public:
|
||||
|
||||
static Qualified MakeClass(std::vector<std::string> namespaces,
|
||||
const std::string& name) {
|
||||
return Qualified(namespaces, name);
|
||||
}
|
||||
|
||||
static Qualified MakeEigen(const std::string& name) {
|
||||
return Qualified(name, EIGEN);
|
||||
}
|
||||
|
||||
static Qualified MakeBasis(const std::string& name) {
|
||||
return Qualified(name, BASIS);
|
||||
}
|
||||
|
||||
static Qualified MakeVoid() {
|
||||
return Qualified("void", VOID);
|
||||
}
|
||||
|
||||
/// Return a qualified string using given delimiter
|
||||
std::string qualifiedName(const std::string& delimiter = "") const {
|
||||
std::string result;
|
||||
for (std::size_t i = 0; i < namespaces.size(); ++i)
|
||||
result += (namespaces[i] + delimiter);
|
||||
result += name;
|
||||
for (std::size_t i = 0; i < namespaces_.size(); ++i)
|
||||
result += (namespaces_[i] + delimiter);
|
||||
result += name_;
|
||||
return result;
|
||||
}
|
||||
|
||||
/// Return a matlab file name, i.e. "toolboxPath/+ns1/+ns2/name.m"
|
||||
std::string matlabName(const std::string& toolboxPath) const {
|
||||
std::string result = toolboxPath;
|
||||
for (std::size_t i = 0; i < namespaces.size(); ++i)
|
||||
result += ("/+" + namespaces[i]);
|
||||
result += "/" + name + ".m";
|
||||
for (std::size_t i = 0; i < namespaces_.size(); ++i)
|
||||
result += ("/+" + namespaces_[i]);
|
||||
result += "/" + name_ + ".m";
|
||||
return result;
|
||||
}
|
||||
|
||||
|
|
@ -73,5 +153,109 @@ struct Qualified {
|
|||
|
||||
};
|
||||
|
||||
/* ************************************************************************* */
|
||||
// http://boost-spirit.com/distrib/spirit_1_8_2/libs/spirit/doc/grammar.html
|
||||
struct TypeGrammar: classic::grammar<TypeGrammar> {
|
||||
|
||||
wrap::Qualified& result_; ///< successful parse will be placed in here
|
||||
|
||||
/// Construct type grammar and specify where result is placed
|
||||
TypeGrammar(wrap::Qualified& result) :
|
||||
result_(result) {
|
||||
}
|
||||
|
||||
/// Definition of type grammar
|
||||
template<typename ScannerT>
|
||||
struct definition: BasicRules<ScannerT> {
|
||||
|
||||
typedef classic::rule<ScannerT> Rule;
|
||||
|
||||
Rule void_p, basisType_p, eigenType_p, namespace_del_p, class_p, type_p;
|
||||
|
||||
definition(TypeGrammar const& self) {
|
||||
|
||||
using namespace wrap;
|
||||
using namespace classic;
|
||||
typedef BasicRules<ScannerT> Basic;
|
||||
|
||||
// HACK: use const values instead of using enums themselves - somehow this doesn't result in values getting assigned to gibberish
|
||||
static const Qualified::Category EIGEN = Qualified::EIGEN;
|
||||
static const Qualified::Category BASIS = Qualified::BASIS;
|
||||
static const Qualified::Category CLASS = Qualified::CLASS;
|
||||
static const Qualified::Category VOID = Qualified::VOID;
|
||||
|
||||
void_p = str_p("void") //
|
||||
[assign_a(self.result_.name_)] //
|
||||
[assign_a(self.result_.category, VOID)];
|
||||
|
||||
basisType_p = Basic::basisType_p //
|
||||
[assign_a(self.result_.name_)] //
|
||||
[assign_a(self.result_.category, BASIS)];
|
||||
|
||||
eigenType_p = Basic::eigenType_p //
|
||||
[assign_a(self.result_.name_)] //
|
||||
[assign_a(self.result_.category, EIGEN)];
|
||||
|
||||
namespace_del_p = Basic::namespace_p //
|
||||
[push_back_a(self.result_.namespaces_)] >> str_p("::");
|
||||
|
||||
class_p = *namespace_del_p >> Basic::className_p //
|
||||
[assign_a(self.result_.name_)] //
|
||||
[assign_a(self.result_.category, CLASS)];
|
||||
|
||||
type_p = void_p | basisType_p | class_p | eigenType_p;
|
||||
}
|
||||
|
||||
Rule const& start() const {
|
||||
return type_p;
|
||||
}
|
||||
|
||||
};
|
||||
};
|
||||
// type_grammar
|
||||
|
||||
/* ************************************************************************* */
|
||||
// http://boost-spirit.com/distrib/spirit_1_8_2/libs/spirit/doc/grammar.html
|
||||
template<char OPEN, char CLOSE>
|
||||
struct TypeListGrammar: public classic::grammar<TypeListGrammar<OPEN, CLOSE> > {
|
||||
|
||||
typedef std::vector<wrap::Qualified> TypeList;
|
||||
TypeList& result_; ///< successful parse will be placed in here
|
||||
|
||||
/// Construct type grammar and specify where result is placed
|
||||
TypeListGrammar(TypeList& result) :
|
||||
result_(result) {
|
||||
}
|
||||
|
||||
/// Definition of type grammar
|
||||
template<typename ScannerT>
|
||||
struct definition {
|
||||
|
||||
wrap::Qualified type; ///< temporary for use during parsing
|
||||
TypeGrammar type_g; ///< Individual Type grammars
|
||||
|
||||
classic::rule<ScannerT> type_p, typeList_p;
|
||||
|
||||
definition(TypeListGrammar const& self) :
|
||||
type_g(type) {
|
||||
using namespace classic;
|
||||
|
||||
type_p = type_g[push_back_a(self.result_, type)][clear_a(type)];
|
||||
|
||||
typeList_p = OPEN >> !type_p >> *(',' >> type_p) >> CLOSE;
|
||||
}
|
||||
|
||||
classic::rule<ScannerT> const& start() const {
|
||||
return typeList_p;
|
||||
}
|
||||
|
||||
};
|
||||
};
|
||||
// TypeListGrammar
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Needed for other parsers in Argument.h and ReturnType.h
|
||||
static const bool T = true;
|
||||
|
||||
} // \namespace wrap
|
||||
|
||||
|
|
|
|||
|
|
@ -14,7 +14,7 @@ using namespace wrap;
|
|||
|
||||
/* ************************************************************************* */
|
||||
string ReturnType::str(bool add_ptr) const {
|
||||
return maybe_shared_ptr(add_ptr && isPtr, qualifiedName("::"), name);
|
||||
return maybe_shared_ptr(add_ptr && isPtr, qualifiedName("::"), name());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -24,37 +24,46 @@ void ReturnType::wrap_result(const string& out, const string& result,
|
|||
string cppType = qualifiedName("::"), matlabType = qualifiedName(".");
|
||||
|
||||
if (category == CLASS) {
|
||||
|
||||
// Handle Classes
|
||||
string objCopy, ptrType;
|
||||
ptrType = "Shared" + name;
|
||||
const bool isVirtual = typeAttributes.at(cppType).isVirtual;
|
||||
if (isVirtual) {
|
||||
const bool isVirtual = typeAttributes.attributes(cppType).isVirtual;
|
||||
if (isPtr)
|
||||
objCopy = result;
|
||||
else
|
||||
objCopy = result; // a shared pointer can always be passed as is
|
||||
else {
|
||||
// but if we want an actual new object, things get more complex
|
||||
if (isVirtual)
|
||||
// A virtual class needs to be cloned, so the whole hierarchy is returned
|
||||
objCopy = result + ".clone()";
|
||||
} else {
|
||||
if (isPtr)
|
||||
objCopy = result;
|
||||
else
|
||||
objCopy = ptrType + "(new " + cppType + "(" + result + "))";
|
||||
// ...but a non-virtual class can just be copied
|
||||
objCopy = "Shared" + name() + "(new " + cppType + "(" + result + "))";
|
||||
}
|
||||
// e.g. out[1] = wrap_shared_ptr(pairResult.second,"gtsam.Point3", false);
|
||||
wrapperFile.oss << out << " = wrap_shared_ptr(" << objCopy << ",\""
|
||||
<< matlabType << "\", " << (isVirtual ? "true" : "false") << ");\n";
|
||||
|
||||
} else if (isPtr) {
|
||||
wrapperFile.oss << " Shared" << name << "* ret = new Shared" << name << "("
|
||||
<< result << ");" << endl;
|
||||
|
||||
// Handle shared pointer case for BASIS/EIGEN/VOID
|
||||
wrapperFile.oss << " {\n Shared" << name() << "* ret = new Shared" << name()
|
||||
<< "(" << result << ");" << endl;
|
||||
wrapperFile.oss << out << " = wrap_shared_ptr(ret,\"" << matlabType
|
||||
<< "\");\n";
|
||||
<< "\");\n }\n";
|
||||
|
||||
} else if (matlabType != "void")
|
||||
|
||||
// Handle normal case case for BASIS/EIGEN
|
||||
wrapperFile.oss << out << " = wrap< " << str(false) << " >(" << result
|
||||
<< ");\n";
|
||||
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void ReturnType::wrapTypeUnwrap(FileWriter& wrapperFile) const {
|
||||
if (category == CLASS)
|
||||
wrapperFile.oss << " typedef boost::shared_ptr<" << qualifiedName("::")
|
||||
<< "> Shared" << name << ";" << endl;
|
||||
<< "> Shared" << name() << ";" << endl;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -9,6 +9,7 @@
|
|||
#include "FileWriter.h"
|
||||
#include "TypeAttributesTable.h"
|
||||
#include "utilities.h"
|
||||
#include <iostream>
|
||||
|
||||
#pragma once
|
||||
|
||||
|
|
@ -17,28 +18,25 @@ namespace wrap {
|
|||
/**
|
||||
* Encapsulates return value of a method or function
|
||||
*/
|
||||
struct ReturnType: Qualified {
|
||||
|
||||
/// the different supported return value categories
|
||||
typedef enum {
|
||||
CLASS = 1, EIGEN = 2, BASIS = 3, VOID = 4
|
||||
} return_category;
|
||||
struct ReturnType: public Qualified {
|
||||
|
||||
bool isPtr;
|
||||
return_category category;
|
||||
|
||||
friend struct ReturnValueGrammar;
|
||||
|
||||
/// Makes a void type
|
||||
ReturnType() :
|
||||
isPtr(false), category(CLASS) {
|
||||
isPtr(false) {
|
||||
}
|
||||
|
||||
ReturnType(const std::string& name) :
|
||||
isPtr(false), category(CLASS) {
|
||||
Qualified::name = name;
|
||||
/// Constructor, no namespaces
|
||||
ReturnType(const std::string& name, Category c = CLASS, bool ptr = false) :
|
||||
Qualified(name, c), isPtr(ptr) {
|
||||
}
|
||||
|
||||
void rename(const Qualified& q) {
|
||||
name = q.name;
|
||||
namespaces = q.namespaces;
|
||||
virtual void clear() {
|
||||
Qualified::clear();
|
||||
isPtr = false;
|
||||
}
|
||||
|
||||
/// Check if this type is in a set of valid types
|
||||
|
|
@ -64,4 +62,36 @@ private:
|
|||
|
||||
};
|
||||
|
||||
//******************************************************************************
|
||||
// http://boost-spirit.com/distrib/spirit_1_8_2/libs/spirit/doc/grammar.html
|
||||
struct ReturnTypeGrammar: public classic::grammar<ReturnTypeGrammar> {
|
||||
|
||||
wrap::ReturnType& result_; ///< successful parse will be placed in here
|
||||
|
||||
TypeGrammar type_g;
|
||||
|
||||
/// Construct ReturnType grammar and specify where result is placed
|
||||
ReturnTypeGrammar(wrap::ReturnType& result) :
|
||||
result_(result), type_g(result_) {
|
||||
}
|
||||
|
||||
/// Definition of type grammar
|
||||
template<typename ScannerT>
|
||||
struct definition {
|
||||
|
||||
classic::rule<ScannerT> type_p;
|
||||
|
||||
definition(ReturnTypeGrammar const& self) {
|
||||
using namespace classic;
|
||||
type_p = self.type_g >> !ch_p('*')[assign_a(self.result_.isPtr, T)];
|
||||
}
|
||||
|
||||
classic::rule<ScannerT> const& start() const {
|
||||
return type_p;
|
||||
}
|
||||
|
||||
};
|
||||
};
|
||||
// ReturnTypeGrammar
|
||||
|
||||
} // \namespace wrap
|
||||
|
|
|
|||
|
|
@ -17,9 +17,9 @@ using namespace wrap;
|
|||
/* ************************************************************************* */
|
||||
ReturnValue ReturnValue::expandTemplate(const TemplateSubstitution& ts) const {
|
||||
ReturnValue instRetVal = *this;
|
||||
instRetVal.type1 = ts(type1);
|
||||
instRetVal.type1 = ts.tryToSubstitite(type1);
|
||||
if (isPair)
|
||||
instRetVal.type2 = ts(type2);
|
||||
instRetVal.type2 = ts.tryToSubstitite(type2);
|
||||
return instRetVal;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -25,6 +25,8 @@ struct ReturnValue {
|
|||
bool isPair;
|
||||
ReturnType type1, type2;
|
||||
|
||||
friend struct ReturnValueGrammar;
|
||||
|
||||
/// Constructor
|
||||
ReturnValue() :
|
||||
isPair(false) {
|
||||
|
|
@ -35,6 +37,22 @@ struct ReturnValue {
|
|||
isPair(false), type1(type) {
|
||||
}
|
||||
|
||||
/// Constructor
|
||||
ReturnValue(const ReturnType& t1, const ReturnType& t2) :
|
||||
isPair(true), type1(t1), type2(t2) {
|
||||
}
|
||||
|
||||
virtual void clear() {
|
||||
type1.clear();
|
||||
type2.clear();
|
||||
isPair = false;
|
||||
}
|
||||
|
||||
bool operator==(const ReturnValue& other) const {
|
||||
return isPair == other.isPair && type1 == other.type1
|
||||
&& type2 == other.type2;
|
||||
}
|
||||
|
||||
/// Substitute template argument
|
||||
ReturnValue expandTemplate(const TemplateSubstitution& ts) const;
|
||||
|
||||
|
|
@ -59,4 +77,39 @@ struct ReturnValue {
|
|||
|
||||
};
|
||||
|
||||
} // \namespace wrap
|
||||
//******************************************************************************
|
||||
// http://boost-spirit.com/distrib/spirit_1_8_2/libs/spirit/doc/grammar.html
|
||||
struct ReturnValueGrammar: public classic::grammar<ReturnValueGrammar> {
|
||||
|
||||
wrap::ReturnValue& result_; ///< successful parse will be placed in here
|
||||
ReturnTypeGrammar returnType1_g, returnType2_g; ///< Type parsers
|
||||
|
||||
/// Construct type grammar and specify where result is placed
|
||||
ReturnValueGrammar(wrap::ReturnValue& result) :
|
||||
result_(result), returnType1_g(result.type1), returnType2_g(result.type2) {
|
||||
}
|
||||
|
||||
/// Definition of type grammar
|
||||
template<typename ScannerT>
|
||||
struct definition {
|
||||
|
||||
classic::rule<ScannerT> pair_p, returnValue_p;
|
||||
|
||||
definition(ReturnValueGrammar const& self) {
|
||||
using namespace classic;
|
||||
|
||||
pair_p = (str_p("pair") >> '<' >> self.returnType1_g >> ','
|
||||
>> self.returnType2_g >> '>')[assign_a(self.result_.isPair, T)];
|
||||
|
||||
returnValue_p = pair_p | self.returnType1_g;
|
||||
}
|
||||
|
||||
classic::rule<ScannerT> const& start() const {
|
||||
return returnValue_p;
|
||||
}
|
||||
|
||||
};
|
||||
};
|
||||
// ReturnValueGrammar
|
||||
|
||||
}// \namespace wrap
|
||||
|
|
|
|||
|
|
@ -16,7 +16,7 @@
|
|||
* @author Richard Roberts
|
||||
**/
|
||||
|
||||
#include "Method.h"
|
||||
#include "StaticMethod.h"
|
||||
#include "utilities.h"
|
||||
|
||||
#include <boost/foreach.hpp>
|
||||
|
|
@ -36,117 +36,9 @@ void StaticMethod::proxy_header(FileWriter& proxyFile) const {
|
|||
proxyFile.oss << " function varargout = " << upperName << "(varargin)\n";
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void StaticMethod::proxy_wrapper_fragments(FileWriter& proxyFile,
|
||||
FileWriter& wrapperFile, Str cppClassName, Str matlabQualName,
|
||||
Str matlabUniqueName, Str wrapperName,
|
||||
const TypeAttributesTable& typeAttributes,
|
||||
vector<string>& functionNames) const {
|
||||
|
||||
// emit header, e.g., function varargout = templatedMethod(this, varargin)
|
||||
proxy_header(proxyFile);
|
||||
|
||||
// Emit comments for documentation
|
||||
string up_name = boost::to_upper_copy(matlabName());
|
||||
proxyFile.oss << " % " << up_name << " usage: ";
|
||||
usage_fragment(proxyFile, matlabName());
|
||||
|
||||
// Emit URL to Doxygen page
|
||||
proxyFile.oss << " % "
|
||||
<< "Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html"
|
||||
<< endl;
|
||||
|
||||
// Handle special case of single overload with all numeric arguments
|
||||
if (nrOverloads() == 1 && argumentList(0).allScalar()) {
|
||||
// Output proxy matlab code
|
||||
// TODO: document why is it OK to not check arguments in this case
|
||||
proxyFile.oss << " ";
|
||||
const int id = (int) functionNames.size();
|
||||
argumentList(0).emit_call(proxyFile, returnValue(0), wrapperName, id,
|
||||
isStatic());
|
||||
|
||||
// Output C++ wrapper code
|
||||
const string wrapFunctionName = wrapper_fragment(wrapperFile, cppClassName,
|
||||
matlabUniqueName, 0, id, typeAttributes, templateArgValue_);
|
||||
|
||||
// Add to function list
|
||||
functionNames.push_back(wrapFunctionName);
|
||||
} else {
|
||||
// Check arguments for all overloads
|
||||
for (size_t i = 0; i < nrOverloads(); ++i) {
|
||||
|
||||
// Output proxy matlab code
|
||||
proxyFile.oss << " " << (i == 0 ? "" : "else");
|
||||
const int id = (int) functionNames.size();
|
||||
argumentList(i).emit_conditional_call(proxyFile, returnValue(i),
|
||||
wrapperName, id, isStatic());
|
||||
|
||||
// Output C++ wrapper code
|
||||
const string wrapFunctionName = wrapper_fragment(wrapperFile,
|
||||
cppClassName, matlabUniqueName, i, id, typeAttributes,
|
||||
templateArgValue_);
|
||||
|
||||
// Add to function list
|
||||
functionNames.push_back(wrapFunctionName);
|
||||
}
|
||||
proxyFile.oss << " else\n";
|
||||
proxyFile.oss
|
||||
<< " error('Arguments do not match any overload of function "
|
||||
<< matlabQualName << "." << name_ << "');" << endl;
|
||||
proxyFile.oss << " end\n";
|
||||
}
|
||||
|
||||
proxyFile.oss << " end\n";
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
string StaticMethod::wrapper_fragment(FileWriter& wrapperFile, Str cppClassName,
|
||||
Str matlabUniqueName, int overload, int id,
|
||||
const TypeAttributesTable& typeAttributes,
|
||||
const Qualified& instName) const {
|
||||
|
||||
// generate code
|
||||
|
||||
const string wrapFunctionName = matlabUniqueName + "_" + name_ + "_"
|
||||
+ boost::lexical_cast<string>(id);
|
||||
|
||||
const ArgumentList& args = argumentList(overload);
|
||||
const ReturnValue& returnVal = returnValue(overload);
|
||||
|
||||
// call
|
||||
wrapperFile.oss << "void " << wrapFunctionName
|
||||
<< "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n";
|
||||
// start
|
||||
wrapperFile.oss << "{\n";
|
||||
|
||||
returnVal.wrapTypeUnwrap(wrapperFile);
|
||||
|
||||
wrapperFile.oss << " typedef boost::shared_ptr<" << cppClassName
|
||||
<< "> Shared;" << endl;
|
||||
|
||||
// get call
|
||||
// for static methods: cppClassName::staticMethod<TemplateVal>
|
||||
// for instance methods: obj->instanceMethod<TemplateVal>
|
||||
string expanded = wrapper_call(wrapperFile, cppClassName, matlabUniqueName,
|
||||
args, returnVal, typeAttributes, instName);
|
||||
|
||||
expanded += ("(" + args.names() + ")");
|
||||
if (returnVal.type1.name != "void")
|
||||
returnVal.wrap_result(expanded, wrapperFile, typeAttributes);
|
||||
else
|
||||
wrapperFile.oss << " " + expanded + ";\n";
|
||||
|
||||
// finish
|
||||
wrapperFile.oss << "}\n";
|
||||
|
||||
return wrapFunctionName;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
string StaticMethod::wrapper_call(FileWriter& wrapperFile, Str cppClassName,
|
||||
Str matlabUniqueName, const ArgumentList& args,
|
||||
const ReturnValue& returnVal, const TypeAttributesTable& typeAttributes,
|
||||
const Qualified& instName) const {
|
||||
Str matlabUniqueName, const ArgumentList& args) const {
|
||||
// check arguments
|
||||
// NOTE: for static functions, there is no object passed
|
||||
wrapperFile.oss << " checkArguments(\"" << matlabUniqueName << "." << name_
|
||||
|
|
@ -158,17 +50,10 @@ string StaticMethod::wrapper_call(FileWriter& wrapperFile, Str cppClassName,
|
|||
// call method and wrap result
|
||||
// example: out[0]=wrap<bool>(staticMethod(t));
|
||||
string expanded = cppClassName + "::" + name_;
|
||||
if (!instName.empty())
|
||||
expanded += ("<" + instName.qualifiedName("::") + ">");
|
||||
if (templateArgValue_)
|
||||
expanded += ("<" + templateArgValue_->qualifiedName("::") + ">");
|
||||
|
||||
return expanded;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void StaticMethod::python_wrapper(FileWriter& wrapperFile,
|
||||
Str className) const {
|
||||
wrapperFile.oss << " .def(\"" << name_ << "\", &" << className << "::" << name_
|
||||
<< ");\n";
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -19,42 +19,15 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include "FullyOverloadedFunction.h"
|
||||
#include "MethodBase.h"
|
||||
|
||||
namespace wrap {
|
||||
|
||||
/// StaticMethod class
|
||||
struct StaticMethod: public FullyOverloadedFunction {
|
||||
struct StaticMethod: public MethodBase {
|
||||
|
||||
typedef const std::string& Str;
|
||||
|
||||
virtual bool isStatic() const {
|
||||
return true;
|
||||
}
|
||||
|
||||
// emit a list of comments, one for each overload
|
||||
void comment_fragment(FileWriter& proxyFile) const {
|
||||
SignatureOverloads::comment_fragment(proxyFile, matlabName());
|
||||
}
|
||||
|
||||
void verifyArguments(const std::vector<std::string>& validArgs) const {
|
||||
SignatureOverloads::verifyArguments(validArgs, name_);
|
||||
}
|
||||
|
||||
void verifyReturnTypes(const std::vector<std::string>& validtypes) const {
|
||||
SignatureOverloads::verifyReturnTypes(validtypes, name_);
|
||||
}
|
||||
|
||||
// MATLAB code generation
|
||||
// classPath is class directory, e.g., ../matlab/@Point2
|
||||
void proxy_wrapper_fragments(FileWriter& proxyFile, FileWriter& wrapperFile,
|
||||
Str cppClassName, Str matlabQualName, Str matlabUniqueName,
|
||||
Str wrapperName, const TypeAttributesTable& typeAttributes,
|
||||
std::vector<std::string>& functionNames) const;
|
||||
|
||||
// emit python wrapper
|
||||
void python_wrapper(FileWriter& wrapperFile, Str className) const;
|
||||
|
||||
friend std::ostream& operator<<(std::ostream& os, const StaticMethod& m) {
|
||||
for (size_t i = 0; i < m.nrOverloads(); i++)
|
||||
os << "static " << m.returnVals_[i] << " " << m.name_ << m.argLists_[i];
|
||||
|
|
@ -65,15 +38,8 @@ protected:
|
|||
|
||||
virtual void proxy_header(FileWriter& proxyFile) const;
|
||||
|
||||
std::string wrapper_fragment(FileWriter& wrapperFile, Str cppClassName,
|
||||
Str matlabUniqueName, int overload, int id,
|
||||
const TypeAttributesTable& typeAttributes, const Qualified& instName =
|
||||
Qualified()) const; ///< cpp wrapper
|
||||
|
||||
virtual std::string wrapper_call(FileWriter& wrapperFile, Str cppClassName,
|
||||
Str matlabUniqueName, const ArgumentList& args,
|
||||
const ReturnValue& returnVal, const TypeAttributesTable& typeAttributes,
|
||||
const Qualified& instName) const;
|
||||
Str matlabUniqueName, const ArgumentList& args) const;
|
||||
};
|
||||
|
||||
} // \namespace wrap
|
||||
|
|
|
|||
|
|
@ -0,0 +1,102 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 Template.h
|
||||
* @brief Template name
|
||||
* @author Frank Dellaert
|
||||
* @date Nov 11, 2014
|
||||
**/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <wrap/Qualified.h>
|
||||
|
||||
namespace wrap {
|
||||
|
||||
/// The template specification that goes before a method or a class
|
||||
class Template {
|
||||
std::string argName_;
|
||||
std::vector<Qualified> argValues_;
|
||||
friend struct TemplateGrammar;
|
||||
public:
|
||||
/// The only way to get values into a Template is via our friendly Grammar
|
||||
Template() {
|
||||
}
|
||||
void clear() {
|
||||
argName_.clear();
|
||||
argValues_.clear();
|
||||
}
|
||||
const std::string& argName() const {
|
||||
return argName_;
|
||||
}
|
||||
const std::vector<Qualified>& argValues() const {
|
||||
return argValues_;
|
||||
}
|
||||
size_t nrValues() const {
|
||||
return argValues_.size();
|
||||
}
|
||||
const Qualified& operator[](size_t i) const {
|
||||
return argValues_[i];
|
||||
}
|
||||
bool valid() const {
|
||||
return !argName_.empty() && argValues_.size() > 0;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
/* ************************************************************************* */
|
||||
// http://boost-spirit.com/distrib/spirit_1_8_2/libs/spirit/doc/grammar.html
|
||||
struct TemplateGrammar: public classic::grammar<TemplateGrammar> {
|
||||
|
||||
Template& result_; ///< successful parse will be placed in here
|
||||
TypeListGrammar<'{', '}'> argValues_g; ///< TypeList parser
|
||||
|
||||
/// Construct type grammar and specify where result is placed
|
||||
TemplateGrammar(Template& result) :
|
||||
result_(result), argValues_g(result.argValues_) {
|
||||
}
|
||||
|
||||
/// Definition of type grammar
|
||||
template<typename ScannerT>
|
||||
struct definition: BasicRules<ScannerT> {
|
||||
|
||||
classic::rule<ScannerT> templateArgValues_p;
|
||||
|
||||
definition(TemplateGrammar const& self) {
|
||||
using classic::str_p;
|
||||
using classic::assign_a;
|
||||
templateArgValues_p = (str_p("template") >> '<'
|
||||
>> (BasicRules<ScannerT>::name_p)[assign_a(self.result_.argName_)]
|
||||
>> '=' >> self.argValues_g >> '>');
|
||||
}
|
||||
|
||||
classic::rule<ScannerT> const& start() const {
|
||||
return templateArgValues_p;
|
||||
}
|
||||
|
||||
};
|
||||
};
|
||||
// TemplateGrammar
|
||||
|
||||
/// Cool initializer for tests
|
||||
static inline boost::optional<Template> CreateTemplate(const std::string& s) {
|
||||
Template result;
|
||||
TemplateGrammar g(result);
|
||||
bool success = parse(s.c_str(), g, classic::space_p).full;
|
||||
if (success)
|
||||
return result;
|
||||
else
|
||||
return boost::none;
|
||||
}
|
||||
|
||||
} // \namespace wrap
|
||||
|
||||
|
|
@ -32,7 +32,7 @@ Class TemplateInstantiationTypedef::findAndExpand(
|
|||
// Find matching class
|
||||
boost::optional<Class const &> matchedClass;
|
||||
BOOST_FOREACH(const Class& cls, classes) {
|
||||
if (cls.name == class_.name && cls.namespaces == class_.namespaces
|
||||
if (cls.name() == class_.name() && cls.namespaces() == class_.namespaces()
|
||||
&& cls.templateArgs.size() == typeList.size()) {
|
||||
matchedClass.reset(cls);
|
||||
break;
|
||||
|
|
@ -52,7 +52,8 @@ Class TemplateInstantiationTypedef::findAndExpand(
|
|||
}
|
||||
|
||||
// Fix class properties
|
||||
classInst.name = name;
|
||||
classInst.name_ = name();
|
||||
classInst.namespaces_ = namespaces();
|
||||
classInst.templateArgs.clear();
|
||||
classInst.typedefName = matchedClass->qualifiedName("::") + "<";
|
||||
if (typeList.size() > 0)
|
||||
|
|
@ -60,7 +61,6 @@ Class TemplateInstantiationTypedef::findAndExpand(
|
|||
for (size_t i = 1; i < typeList.size(); ++i)
|
||||
classInst.typedefName += (", " + typeList[i].qualifiedName("::"));
|
||||
classInst.typedefName += ">";
|
||||
classInst.namespaces = namespaces;
|
||||
|
||||
return classInst;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -40,25 +40,25 @@ public:
|
|||
}
|
||||
|
||||
std::string expandedClassName() const {
|
||||
return expandedClass_.name;
|
||||
return expandedClass_.name();
|
||||
}
|
||||
|
||||
// Substitute if needed
|
||||
Qualified operator()(const Qualified& type) const {
|
||||
if (type.name == templateArg_ && type.namespaces.empty())
|
||||
Qualified tryToSubstitite(const Qualified& type) const {
|
||||
if (type.match(templateArg_))
|
||||
return qualifiedType_;
|
||||
else if (type.name == "This")
|
||||
else if (type.match("This"))
|
||||
return expandedClass_;
|
||||
else
|
||||
return type;
|
||||
}
|
||||
|
||||
// Substitute if needed
|
||||
ReturnType operator()(const ReturnType& type) const {
|
||||
ReturnType tryToSubstitite(const ReturnType& type) const {
|
||||
ReturnType instType = type;
|
||||
if (type.name == templateArg_ && type.namespaces.empty())
|
||||
if (type.match(templateArg_))
|
||||
instType.rename(qualifiedType_);
|
||||
else if (type.name == "This")
|
||||
else if (type.match("This"))
|
||||
instType.rename(expandedClass_);
|
||||
return instType;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -21,42 +21,66 @@
|
|||
#include "utilities.h"
|
||||
|
||||
#include <boost/foreach.hpp>
|
||||
#include <boost/range/adaptor/map.hpp>
|
||||
#include <boost/range/algorithm/copy.hpp>
|
||||
|
||||
#include <iterator> // std::ostream_iterator
|
||||
using namespace std;
|
||||
|
||||
namespace wrap {
|
||||
|
||||
/* ************************************************************************* */
|
||||
void TypeAttributesTable::addClasses(const vector<Class>& classes) {
|
||||
/* ************************************************************************* */
|
||||
const TypeAttributes& TypeAttributesTable::attributes(const string& key) const {
|
||||
try {
|
||||
return table_.at(key);
|
||||
} catch (const out_of_range& oor) {
|
||||
cerr << "TypeAttributesTable::attributes: key " << key
|
||||
<< " not found. Valid keys are:\n";
|
||||
using boost::adaptors::map_keys;
|
||||
ostream_iterator<string> out_it(cerr, "\n");
|
||||
boost::copy(table_ | map_keys, out_it);
|
||||
throw runtime_error("Internal error in wrap");
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void TypeAttributesTable::addClasses(const vector<Class>& classes) {
|
||||
BOOST_FOREACH(const Class& cls, classes) {
|
||||
if(!insert(make_pair(cls.qualifiedName("::"), TypeAttributes(cls.isVirtual))).second)
|
||||
if (!table_.insert(
|
||||
make_pair(cls.qualifiedName("::"), TypeAttributes(cls.isVirtual))).second)
|
||||
throw DuplicateDefinition("class " + cls.qualifiedName("::"));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void TypeAttributesTable::addForwardDeclarations(const vector<ForwardDeclaration>& forwardDecls) {
|
||||
/* ************************************************************************* */
|
||||
void TypeAttributesTable::addForwardDeclarations(
|
||||
const vector<ForwardDeclaration>& forwardDecls) {
|
||||
BOOST_FOREACH(const ForwardDeclaration& fwDec, forwardDecls) {
|
||||
if(!insert(make_pair(fwDec.name, TypeAttributes(fwDec.isVirtual))).second)
|
||||
if (!table_.insert(make_pair(fwDec.name, TypeAttributes(fwDec.isVirtual))).second)
|
||||
throw DuplicateDefinition("class " + fwDec.name);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void TypeAttributesTable::checkValidity(const vector<Class>& classes) const {
|
||||
/* ************************************************************************* */
|
||||
void TypeAttributesTable::checkValidity(const vector<Class>& classes) const {
|
||||
BOOST_FOREACH(const Class& cls, classes) {
|
||||
|
||||
boost::optional<string> parent = cls.qualifiedParent();
|
||||
if (parent) {
|
||||
|
||||
// Check that class is virtual if it has a parent
|
||||
if (!cls.qualifiedParent.empty() && !cls.isVirtual)
|
||||
if (!cls.isVirtual)
|
||||
throw AttributeError(cls.qualifiedName("::"),
|
||||
"Has a base class so needs to be declared virtual, change to 'virtual class "
|
||||
+ cls.name + " ...'");
|
||||
+ cls.name() + " ...'");
|
||||
|
||||
// Check that parent is virtual as well
|
||||
Qualified parent = cls.qualifiedParent;
|
||||
if (!parent.empty() && !at(parent.qualifiedName("::")).isVirtual)
|
||||
throw AttributeError(parent.qualifiedName("::"),
|
||||
if (!table_.at(*parent).isVirtual)
|
||||
throw AttributeError(*parent,
|
||||
"Is the base class of " + cls.qualifiedName("::")
|
||||
+ ", so needs to be declared virtual");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -27,8 +27,8 @@
|
|||
|
||||
namespace wrap {
|
||||
|
||||
// Forward declarations
|
||||
class Class;
|
||||
// Forward declarations
|
||||
class Class;
|
||||
|
||||
/** Attributes about valid classes, both for classes defined in this module and
|
||||
* also those forward-declared from others. At the moment this only contains
|
||||
|
|
@ -37,18 +37,33 @@ namespace wrap {
|
|||
*/
|
||||
struct TypeAttributes {
|
||||
bool isVirtual;
|
||||
TypeAttributes() : isVirtual(false) {}
|
||||
TypeAttributes(bool isVirtual) : isVirtual(isVirtual) {}
|
||||
TypeAttributes() :
|
||||
isVirtual(false) {
|
||||
}
|
||||
TypeAttributes(bool isVirtual) :
|
||||
isVirtual(isVirtual) {
|
||||
}
|
||||
};
|
||||
|
||||
/** Map of type names to attributes. */
|
||||
class TypeAttributesTable : public std::map<std::string, TypeAttributes> {
|
||||
class TypeAttributesTable {
|
||||
|
||||
std::map<std::string, TypeAttributes> table_;
|
||||
|
||||
public:
|
||||
TypeAttributesTable() {}
|
||||
|
||||
/// Constructor
|
||||
TypeAttributesTable() {
|
||||
}
|
||||
|
||||
void addClasses(const std::vector<Class>& classes);
|
||||
void addForwardDeclarations(const std::vector<ForwardDeclaration>& forwardDecls);
|
||||
void addForwardDeclarations(
|
||||
const std::vector<ForwardDeclaration>& forwardDecls);
|
||||
|
||||
/// Access attributes associated with key, informative failure
|
||||
const TypeAttributes& attributes(const std::string& key) const;
|
||||
|
||||
/// Check that all virtual classes are properly defined
|
||||
void checkValidity(const std::vector<Class>& classes) const;
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -1,16 +1,20 @@
|
|||
/**
|
||||
* @file spirit_actors.h
|
||||
*
|
||||
* @brief Additional actors for the wrap parser
|
||||
* @brief Additional utilities and actors for the wrap parser
|
||||
*
|
||||
* @date Dec 8, 2011
|
||||
* @author Alex Cunningham
|
||||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <boost/spirit/include/classic_core.hpp>
|
||||
#include <boost/spirit/include/classic_ref_actor.hpp>
|
||||
#include <boost/spirit/include/classic_push_back_actor.hpp>
|
||||
#include <boost/spirit/include/classic_clear_actor.hpp>
|
||||
#include <boost/spirit/include/classic_assign_actor.hpp>
|
||||
#include <boost/spirit/include/classic_confix.hpp>
|
||||
|
||||
namespace boost {
|
||||
namespace spirit {
|
||||
|
|
@ -121,3 +125,47 @@ BOOST_SPIRIT_CLASSIC_NAMESPACE_END
|
|||
}
|
||||
}
|
||||
|
||||
namespace wrap {
|
||||
|
||||
namespace classic = BOOST_SPIRIT_CLASSIC_NS;
|
||||
|
||||
/// Some basic rules used by all parsers
|
||||
template<typename ScannerT>
|
||||
struct BasicRules {
|
||||
|
||||
classic::rule<ScannerT> comments_p, basisType_p, eigenType_p, keywords_p,
|
||||
stlType_p, name_p, className_p, namespace_p;
|
||||
|
||||
BasicRules() {
|
||||
|
||||
using classic::comment_p;
|
||||
using classic::eol_p;
|
||||
using classic::str_p;
|
||||
using classic::alpha_p;
|
||||
using classic::lexeme_d;
|
||||
using classic::upper_p;
|
||||
using classic::lower_p;
|
||||
using classic::alnum_p;
|
||||
|
||||
comments_p = comment_p("/*", "*/") | comment_p("//", eol_p);
|
||||
|
||||
basisType_p = (str_p("string") | "bool" | "size_t" | "int" | "double"
|
||||
| "char" | "unsigned char");
|
||||
|
||||
eigenType_p = (str_p("Vector") | "Matrix");
|
||||
|
||||
keywords_p =
|
||||
(str_p("const") | "static" | "namespace" | "void" | basisType_p);
|
||||
|
||||
stlType_p = (str_p("vector") | "list");
|
||||
|
||||
name_p = lexeme_d[alpha_p >> *(alnum_p | '_')];
|
||||
|
||||
className_p = (lexeme_d[upper_p >> *(alnum_p | '_')] - eigenType_p
|
||||
- keywords_p) | stlType_p;
|
||||
|
||||
namespace_p = lexeme_d[lower_p >> *(alnum_p | '_')] - keywords_p;
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
|
|
@ -8,6 +8,7 @@ class_<Point2>("Point2")
|
|||
.def("argChar", &Point2::argChar);
|
||||
.def("argUChar", &Point2::argUChar);
|
||||
.def("dim", &Point2::dim);
|
||||
.def("eigenArguments", &Point2::eigenArguments);
|
||||
.def("returnChar", &Point2::returnChar);
|
||||
.def("vectorConfusion", &Point2::vectorConfusion);
|
||||
.def("x", &Point2::x);
|
||||
|
|
@ -59,19 +60,23 @@ class_<MyTemplatePoint2>("MyTemplatePoint2")
|
|||
.def("return_ptrs", &MyTemplatePoint2::return_ptrs);
|
||||
.def("templatedMethod", &MyTemplatePoint2::templatedMethod);
|
||||
.def("templatedMethod", &MyTemplatePoint2::templatedMethod);
|
||||
.def("templatedMethod", &MyTemplatePoint2::templatedMethod);
|
||||
.def("templatedMethod", &MyTemplatePoint2::templatedMethod);
|
||||
;
|
||||
|
||||
class_<MyTemplatePoint3>("MyTemplatePoint3")
|
||||
.def("MyTemplatePoint3", &MyTemplatePoint3::MyTemplatePoint3);
|
||||
.def("accept_T", &MyTemplatePoint3::accept_T);
|
||||
.def("accept_Tptr", &MyTemplatePoint3::accept_Tptr);
|
||||
.def("create_MixedPtrs", &MyTemplatePoint3::create_MixedPtrs);
|
||||
.def("create_ptrs", &MyTemplatePoint3::create_ptrs);
|
||||
.def("return_T", &MyTemplatePoint3::return_T);
|
||||
.def("return_Tptr", &MyTemplatePoint3::return_Tptr);
|
||||
.def("return_ptrs", &MyTemplatePoint3::return_ptrs);
|
||||
.def("templatedMethod", &MyTemplatePoint3::templatedMethod);
|
||||
.def("templatedMethod", &MyTemplatePoint3::templatedMethod);
|
||||
class_<MyTemplateMatrix>("MyTemplateMatrix")
|
||||
.def("MyTemplateMatrix", &MyTemplateMatrix::MyTemplateMatrix);
|
||||
.def("accept_T", &MyTemplateMatrix::accept_T);
|
||||
.def("accept_Tptr", &MyTemplateMatrix::accept_Tptr);
|
||||
.def("create_MixedPtrs", &MyTemplateMatrix::create_MixedPtrs);
|
||||
.def("create_ptrs", &MyTemplateMatrix::create_ptrs);
|
||||
.def("return_T", &MyTemplateMatrix::return_T);
|
||||
.def("return_Tptr", &MyTemplateMatrix::return_Tptr);
|
||||
.def("return_ptrs", &MyTemplateMatrix::return_ptrs);
|
||||
.def("templatedMethod", &MyTemplateMatrix::templatedMethod);
|
||||
.def("templatedMethod", &MyTemplateMatrix::templatedMethod);
|
||||
.def("templatedMethod", &MyTemplateMatrix::templatedMethod);
|
||||
.def("templatedMethod", &MyTemplateMatrix::templatedMethod);
|
||||
;
|
||||
|
||||
class_<MyFactorPosePoint2>("MyFactorPosePoint2")
|
||||
|
|
|
|||
|
|
@ -0,0 +1,101 @@
|
|||
%class Point2, see Doxygen page for details
|
||||
%at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
%
|
||||
%-------Constructors-------
|
||||
%Point2()
|
||||
%Point2(double x, double y)
|
||||
%
|
||||
%-------Methods-------
|
||||
%argChar(char a) : returns void
|
||||
%argUChar(unsigned char a) : returns void
|
||||
%dim() : returns int
|
||||
%eigenArguments(Vector v, Matrix m) : returns void
|
||||
%returnChar() : returns char
|
||||
%vectorConfusion() : returns VectorNotEigen
|
||||
%x() : returns double
|
||||
%y() : returns double
|
||||
%
|
||||
classdef Point2 < handle
|
||||
properties
|
||||
ptr_gtsamPoint2 = 0
|
||||
end
|
||||
methods
|
||||
function obj = Point2(varargin)
|
||||
if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
|
||||
my_ptr = varargin{2};
|
||||
geometry_wrapper(0, my_ptr);
|
||||
elseif nargin == 0
|
||||
my_ptr = geometry_wrapper(1);
|
||||
elseif nargin == 2 && isa(varargin{1},'double') && isa(varargin{2},'double')
|
||||
my_ptr = geometry_wrapper(2, varargin{1}, varargin{2});
|
||||
else
|
||||
error('Arguments do not match any overload of gtsam.Point2 constructor');
|
||||
end
|
||||
obj.ptr_gtsamPoint2 = my_ptr;
|
||||
end
|
||||
|
||||
function delete(obj)
|
||||
geometry_wrapper(3, obj.ptr_gtsamPoint2);
|
||||
end
|
||||
|
||||
function display(obj), obj.print(''); end
|
||||
%DISPLAY Calls print on the object
|
||||
function disp(obj), obj.display; end
|
||||
%DISP Calls print on the object
|
||||
function varargout = argChar(this, varargin)
|
||||
% ARGCHAR usage: argChar(char a) : returns void
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
geometry_wrapper(4, this, varargin{:});
|
||||
end
|
||||
|
||||
function varargout = argUChar(this, varargin)
|
||||
% ARGUCHAR usage: argUChar(unsigned char a) : returns void
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
geometry_wrapper(5, this, varargin{:});
|
||||
end
|
||||
|
||||
function varargout = dim(this, varargin)
|
||||
% DIM usage: dim() : returns int
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
varargout{1} = geometry_wrapper(6, this, varargin{:});
|
||||
end
|
||||
|
||||
function varargout = eigenArguments(this, varargin)
|
||||
% EIGENARGUMENTS usage: eigenArguments(Vector v, Matrix m) : returns void
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
if length(varargin) == 2 && isa(varargin{1},'double') && size(varargin{1},2)==1 && isa(varargin{2},'double')
|
||||
geometry_wrapper(7, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function gtsam.Point2.eigenArguments');
|
||||
end
|
||||
end
|
||||
|
||||
function varargout = returnChar(this, varargin)
|
||||
% RETURNCHAR usage: returnChar() : returns char
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
varargout{1} = geometry_wrapper(8, this, varargin{:});
|
||||
end
|
||||
|
||||
function varargout = vectorConfusion(this, varargin)
|
||||
% VECTORCONFUSION usage: vectorConfusion() : returns VectorNotEigen
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
varargout{1} = geometry_wrapper(9, this, varargin{:});
|
||||
end
|
||||
|
||||
function varargout = x(this, varargin)
|
||||
% X usage: x() : returns double
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
varargout{1} = geometry_wrapper(10, this, varargin{:});
|
||||
end
|
||||
|
||||
function varargout = y(this, varargin)
|
||||
% Y usage: y() : returns double
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
varargout{1} = geometry_wrapper(11, this, varargin{:});
|
||||
end
|
||||
|
||||
end
|
||||
|
||||
methods(Static = true)
|
||||
end
|
||||
end
|
||||
|
|
@ -0,0 +1,93 @@
|
|||
%class Point3, see Doxygen page for details
|
||||
%at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
%
|
||||
%-------Constructors-------
|
||||
%Point3(double x, double y, double z)
|
||||
%
|
||||
%-------Methods-------
|
||||
%norm() : returns double
|
||||
%
|
||||
%-------Static Methods-------
|
||||
%StaticFunctionRet(double z) : returns gtsam::Point3
|
||||
%staticFunction() : returns double
|
||||
%
|
||||
%-------Serialization Interface-------
|
||||
%string_serialize() : returns string
|
||||
%string_deserialize(string serialized) : returns Point3
|
||||
%
|
||||
classdef Point3 < handle
|
||||
properties
|
||||
ptr_gtsamPoint3 = 0
|
||||
end
|
||||
methods
|
||||
function obj = Point3(varargin)
|
||||
if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
|
||||
my_ptr = varargin{2};
|
||||
geometry_wrapper(12, my_ptr);
|
||||
elseif nargin == 3 && isa(varargin{1},'double') && isa(varargin{2},'double') && isa(varargin{3},'double')
|
||||
my_ptr = geometry_wrapper(13, varargin{1}, varargin{2}, varargin{3});
|
||||
else
|
||||
error('Arguments do not match any overload of gtsam.Point3 constructor');
|
||||
end
|
||||
obj.ptr_gtsamPoint3 = my_ptr;
|
||||
end
|
||||
|
||||
function delete(obj)
|
||||
geometry_wrapper(14, obj.ptr_gtsamPoint3);
|
||||
end
|
||||
|
||||
function display(obj), obj.print(''); end
|
||||
%DISPLAY Calls print on the object
|
||||
function disp(obj), obj.display; end
|
||||
%DISP Calls print on the object
|
||||
function varargout = norm(this, varargin)
|
||||
% NORM usage: norm() : returns double
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
varargout{1} = geometry_wrapper(15, this, varargin{:});
|
||||
end
|
||||
|
||||
function varargout = string_serialize(this, varargin)
|
||||
% STRING_SERIALIZE usage: string_serialize() : returns string
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
if length(varargin) == 0
|
||||
varargout{1} = geometry_wrapper(16, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function gtsam.Point3.string_serialize');
|
||||
end
|
||||
end
|
||||
|
||||
function sobj = saveobj(obj)
|
||||
% SAVEOBJ Saves the object to a matlab-readable format
|
||||
sobj = obj.string_serialize();
|
||||
end
|
||||
end
|
||||
|
||||
methods(Static = true)
|
||||
function varargout = StaticFunctionRet(varargin)
|
||||
% STATICFUNCTIONRET usage: StaticFunctionRet(double z) : returns gtsam::Point3
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
varargout{1} = geometry_wrapper(17, varargin{:});
|
||||
end
|
||||
|
||||
function varargout = StaticFunction(varargin)
|
||||
% STATICFUNCTION usage: staticFunction() : returns double
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
varargout{1} = geometry_wrapper(18, varargin{:});
|
||||
end
|
||||
|
||||
function varargout = string_deserialize(varargin)
|
||||
% STRING_DESERIALIZE usage: string_deserialize() : returns gtsam.Point3
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
if length(varargin) == 1
|
||||
varargout{1} = geometry_wrapper(19, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function gtsam.Point3.string_deserialize');
|
||||
end
|
||||
end
|
||||
|
||||
function obj = loadobj(sobj)
|
||||
% LOADOBJ Saves the object to a matlab-readable format
|
||||
obj = gtsam.Point3.string_deserialize(sobj);
|
||||
end
|
||||
end
|
||||
end
|
||||
|
|
@ -0,0 +1,35 @@
|
|||
%class MyBase, see Doxygen page for details
|
||||
%at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
%
|
||||
classdef MyBase < handle
|
||||
properties
|
||||
ptr_MyBase = 0
|
||||
end
|
||||
methods
|
||||
function obj = MyBase(varargin)
|
||||
if (nargin == 2 || (nargin == 3 && strcmp(varargin{3}, 'void'))) && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
|
||||
if nargin == 2
|
||||
my_ptr = varargin{2};
|
||||
else
|
||||
my_ptr = geometry_wrapper(44, varargin{2});
|
||||
end
|
||||
geometry_wrapper(43, my_ptr);
|
||||
else
|
||||
error('Arguments do not match any overload of MyBase constructor');
|
||||
end
|
||||
obj.ptr_MyBase = my_ptr;
|
||||
end
|
||||
|
||||
function delete(obj)
|
||||
geometry_wrapper(45, obj.ptr_MyBase);
|
||||
end
|
||||
|
||||
function display(obj), obj.print(''); end
|
||||
%DISPLAY Calls print on the object
|
||||
function disp(obj), obj.display; end
|
||||
%DISP Calls print on the object
|
||||
end
|
||||
|
||||
methods(Static = true)
|
||||
end
|
||||
end
|
||||
|
|
@ -0,0 +1,36 @@
|
|||
%class MyFactorPosePoint2, see Doxygen page for details
|
||||
%at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
%
|
||||
%-------Constructors-------
|
||||
%MyFactorPosePoint2(size_t key1, size_t key2, double measured, Base noiseModel)
|
||||
%
|
||||
classdef MyFactorPosePoint2 < handle
|
||||
properties
|
||||
ptr_MyFactorPosePoint2 = 0
|
||||
end
|
||||
methods
|
||||
function obj = MyFactorPosePoint2(varargin)
|
||||
if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
|
||||
my_ptr = varargin{2};
|
||||
geometry_wrapper(76, my_ptr);
|
||||
elseif nargin == 4 && isa(varargin{1},'numeric') && isa(varargin{2},'numeric') && isa(varargin{3},'double') && isa(varargin{4},'gtsam.noiseModel.Base')
|
||||
my_ptr = geometry_wrapper(77, varargin{1}, varargin{2}, varargin{3}, varargin{4});
|
||||
else
|
||||
error('Arguments do not match any overload of MyFactorPosePoint2 constructor');
|
||||
end
|
||||
obj.ptr_MyFactorPosePoint2 = my_ptr;
|
||||
end
|
||||
|
||||
function delete(obj)
|
||||
geometry_wrapper(78, obj.ptr_MyFactorPosePoint2);
|
||||
end
|
||||
|
||||
function display(obj), obj.print(''); end
|
||||
%DISPLAY Calls print on the object
|
||||
function disp(obj), obj.display; end
|
||||
%DISP Calls print on the object
|
||||
end
|
||||
|
||||
methods(Static = true)
|
||||
end
|
||||
end
|
||||
|
|
@ -0,0 +1,156 @@
|
|||
%class MyTemplateMatrix, see Doxygen page for details
|
||||
%at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
%
|
||||
%-------Constructors-------
|
||||
%MyTemplateMatrix()
|
||||
%
|
||||
%-------Methods-------
|
||||
%accept_T(Matrix value) : returns void
|
||||
%accept_Tptr(Matrix value) : returns void
|
||||
%create_MixedPtrs() : returns pair< Matrix, Matrix >
|
||||
%create_ptrs() : returns pair< Matrix, Matrix >
|
||||
%return_T(Matrix value) : returns Matrix
|
||||
%return_Tptr(Matrix value) : returns Matrix
|
||||
%return_ptrs(Matrix p1, Matrix p2) : returns pair< Matrix, Matrix >
|
||||
%templatedMethodMatrix(Matrix t) : returns Matrix
|
||||
%templatedMethodPoint2(Point2 t) : returns gtsam::Point2
|
||||
%templatedMethodPoint3(Point3 t) : returns gtsam::Point3
|
||||
%templatedMethodVector(Vector t) : returns Vector
|
||||
%
|
||||
classdef MyTemplateMatrix < MyBase
|
||||
properties
|
||||
ptr_MyTemplateMatrix = 0
|
||||
end
|
||||
methods
|
||||
function obj = MyTemplateMatrix(varargin)
|
||||
if (nargin == 2 || (nargin == 3 && strcmp(varargin{3}, 'void'))) && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
|
||||
if nargin == 2
|
||||
my_ptr = varargin{2};
|
||||
else
|
||||
my_ptr = geometry_wrapper(62, varargin{2});
|
||||
end
|
||||
base_ptr = geometry_wrapper(61, my_ptr);
|
||||
elseif nargin == 0
|
||||
[ my_ptr, base_ptr ] = geometry_wrapper(63);
|
||||
else
|
||||
error('Arguments do not match any overload of MyTemplateMatrix constructor');
|
||||
end
|
||||
obj = obj@MyBase(uint64(5139824614673773682), base_ptr);
|
||||
obj.ptr_MyTemplateMatrix = my_ptr;
|
||||
end
|
||||
|
||||
function delete(obj)
|
||||
geometry_wrapper(64, obj.ptr_MyTemplateMatrix);
|
||||
end
|
||||
|
||||
function display(obj), obj.print(''); end
|
||||
%DISPLAY Calls print on the object
|
||||
function disp(obj), obj.display; end
|
||||
%DISP Calls print on the object
|
||||
function varargout = accept_T(this, varargin)
|
||||
% ACCEPT_T usage: accept_T(Matrix value) : returns void
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
if length(varargin) == 1 && isa(varargin{1},'double')
|
||||
geometry_wrapper(65, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function MyTemplateMatrix.accept_T');
|
||||
end
|
||||
end
|
||||
|
||||
function varargout = accept_Tptr(this, varargin)
|
||||
% ACCEPT_TPTR usage: accept_Tptr(Matrix value) : returns void
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
if length(varargin) == 1 && isa(varargin{1},'double')
|
||||
geometry_wrapper(66, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function MyTemplateMatrix.accept_Tptr');
|
||||
end
|
||||
end
|
||||
|
||||
function varargout = create_MixedPtrs(this, varargin)
|
||||
% CREATE_MIXEDPTRS usage: create_MixedPtrs() : returns pair< Matrix, Matrix >
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
[ varargout{1} varargout{2} ] = geometry_wrapper(67, this, varargin{:});
|
||||
end
|
||||
|
||||
function varargout = create_ptrs(this, varargin)
|
||||
% CREATE_PTRS usage: create_ptrs() : returns pair< Matrix, Matrix >
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
[ varargout{1} varargout{2} ] = geometry_wrapper(68, this, varargin{:});
|
||||
end
|
||||
|
||||
function varargout = return_T(this, varargin)
|
||||
% RETURN_T usage: return_T(Matrix value) : returns Matrix
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
if length(varargin) == 1 && isa(varargin{1},'double')
|
||||
varargout{1} = geometry_wrapper(69, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function MyTemplateMatrix.return_T');
|
||||
end
|
||||
end
|
||||
|
||||
function varargout = return_Tptr(this, varargin)
|
||||
% RETURN_TPTR usage: return_Tptr(Matrix value) : returns Matrix
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
if length(varargin) == 1 && isa(varargin{1},'double')
|
||||
varargout{1} = geometry_wrapper(70, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function MyTemplateMatrix.return_Tptr');
|
||||
end
|
||||
end
|
||||
|
||||
function varargout = return_ptrs(this, varargin)
|
||||
% RETURN_PTRS usage: return_ptrs(Matrix p1, Matrix p2) : returns pair< Matrix, Matrix >
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
if length(varargin) == 2 && isa(varargin{1},'double') && isa(varargin{2},'double')
|
||||
[ varargout{1} varargout{2} ] = geometry_wrapper(71, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function MyTemplateMatrix.return_ptrs');
|
||||
end
|
||||
end
|
||||
|
||||
function varargout = templatedMethodMatrix(this, varargin)
|
||||
% TEMPLATEDMETHODMATRIX usage: templatedMethodMatrix(Matrix t) : returns Matrix
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
if length(varargin) == 1 && isa(varargin{1},'double')
|
||||
varargout{1} = geometry_wrapper(72, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function MyTemplateMatrix.templatedMethod');
|
||||
end
|
||||
end
|
||||
|
||||
function varargout = templatedMethodPoint2(this, varargin)
|
||||
% TEMPLATEDMETHODPOINT2 usage: templatedMethodPoint2(Point2 t) : returns gtsam::Point2
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
if length(varargin) == 1 && isa(varargin{1},'gtsam.Point2')
|
||||
varargout{1} = geometry_wrapper(73, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function MyTemplateMatrix.templatedMethod');
|
||||
end
|
||||
end
|
||||
|
||||
function varargout = templatedMethodPoint3(this, varargin)
|
||||
% TEMPLATEDMETHODPOINT3 usage: templatedMethodPoint3(Point3 t) : returns gtsam::Point3
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
if length(varargin) == 1 && isa(varargin{1},'gtsam.Point3')
|
||||
varargout{1} = geometry_wrapper(74, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function MyTemplateMatrix.templatedMethod');
|
||||
end
|
||||
end
|
||||
|
||||
function varargout = templatedMethodVector(this, varargin)
|
||||
% TEMPLATEDMETHODVECTOR usage: templatedMethodVector(Vector t) : returns Vector
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
if length(varargin) == 1 && isa(varargin{1},'double') && size(varargin{1},2)==1
|
||||
varargout{1} = geometry_wrapper(75, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function MyTemplateMatrix.templatedMethod');
|
||||
end
|
||||
end
|
||||
|
||||
end
|
||||
|
||||
methods(Static = true)
|
||||
end
|
||||
end
|
||||
|
|
@ -0,0 +1,156 @@
|
|||
%class MyTemplatePoint2, see Doxygen page for details
|
||||
%at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
%
|
||||
%-------Constructors-------
|
||||
%MyTemplatePoint2()
|
||||
%
|
||||
%-------Methods-------
|
||||
%accept_T(Point2 value) : returns void
|
||||
%accept_Tptr(Point2 value) : returns void
|
||||
%create_MixedPtrs() : returns pair< gtsam::Point2, gtsam::Point2 >
|
||||
%create_ptrs() : returns pair< gtsam::Point2, gtsam::Point2 >
|
||||
%return_T(Point2 value) : returns gtsam::Point2
|
||||
%return_Tptr(Point2 value) : returns gtsam::Point2
|
||||
%return_ptrs(Point2 p1, Point2 p2) : returns pair< gtsam::Point2, gtsam::Point2 >
|
||||
%templatedMethodMatrix(Matrix t) : returns Matrix
|
||||
%templatedMethodPoint2(Point2 t) : returns gtsam::Point2
|
||||
%templatedMethodPoint3(Point3 t) : returns gtsam::Point3
|
||||
%templatedMethodVector(Vector t) : returns Vector
|
||||
%
|
||||
classdef MyTemplatePoint2 < MyBase
|
||||
properties
|
||||
ptr_MyTemplatePoint2 = 0
|
||||
end
|
||||
methods
|
||||
function obj = MyTemplatePoint2(varargin)
|
||||
if (nargin == 2 || (nargin == 3 && strcmp(varargin{3}, 'void'))) && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
|
||||
if nargin == 2
|
||||
my_ptr = varargin{2};
|
||||
else
|
||||
my_ptr = geometry_wrapper(47, varargin{2});
|
||||
end
|
||||
base_ptr = geometry_wrapper(46, my_ptr);
|
||||
elseif nargin == 0
|
||||
[ my_ptr, base_ptr ] = geometry_wrapper(48);
|
||||
else
|
||||
error('Arguments do not match any overload of MyTemplatePoint2 constructor');
|
||||
end
|
||||
obj = obj@MyBase(uint64(5139824614673773682), base_ptr);
|
||||
obj.ptr_MyTemplatePoint2 = my_ptr;
|
||||
end
|
||||
|
||||
function delete(obj)
|
||||
geometry_wrapper(49, obj.ptr_MyTemplatePoint2);
|
||||
end
|
||||
|
||||
function display(obj), obj.print(''); end
|
||||
%DISPLAY Calls print on the object
|
||||
function disp(obj), obj.display; end
|
||||
%DISP Calls print on the object
|
||||
function varargout = accept_T(this, varargin)
|
||||
% ACCEPT_T usage: accept_T(Point2 value) : returns void
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
if length(varargin) == 1 && isa(varargin{1},'gtsam.Point2')
|
||||
geometry_wrapper(50, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function MyTemplatePoint2.accept_T');
|
||||
end
|
||||
end
|
||||
|
||||
function varargout = accept_Tptr(this, varargin)
|
||||
% ACCEPT_TPTR usage: accept_Tptr(Point2 value) : returns void
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
if length(varargin) == 1 && isa(varargin{1},'gtsam.Point2')
|
||||
geometry_wrapper(51, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function MyTemplatePoint2.accept_Tptr');
|
||||
end
|
||||
end
|
||||
|
||||
function varargout = create_MixedPtrs(this, varargin)
|
||||
% CREATE_MIXEDPTRS usage: create_MixedPtrs() : returns pair< gtsam::Point2, gtsam::Point2 >
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
[ varargout{1} varargout{2} ] = geometry_wrapper(52, this, varargin{:});
|
||||
end
|
||||
|
||||
function varargout = create_ptrs(this, varargin)
|
||||
% CREATE_PTRS usage: create_ptrs() : returns pair< gtsam::Point2, gtsam::Point2 >
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
[ varargout{1} varargout{2} ] = geometry_wrapper(53, this, varargin{:});
|
||||
end
|
||||
|
||||
function varargout = return_T(this, varargin)
|
||||
% RETURN_T usage: return_T(Point2 value) : returns gtsam::Point2
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
if length(varargin) == 1 && isa(varargin{1},'gtsam.Point2')
|
||||
varargout{1} = geometry_wrapper(54, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function MyTemplatePoint2.return_T');
|
||||
end
|
||||
end
|
||||
|
||||
function varargout = return_Tptr(this, varargin)
|
||||
% RETURN_TPTR usage: return_Tptr(Point2 value) : returns gtsam::Point2
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
if length(varargin) == 1 && isa(varargin{1},'gtsam.Point2')
|
||||
varargout{1} = geometry_wrapper(55, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function MyTemplatePoint2.return_Tptr');
|
||||
end
|
||||
end
|
||||
|
||||
function varargout = return_ptrs(this, varargin)
|
||||
% RETURN_PTRS usage: return_ptrs(Point2 p1, Point2 p2) : returns pair< gtsam::Point2, gtsam::Point2 >
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
if length(varargin) == 2 && isa(varargin{1},'gtsam.Point2') && isa(varargin{2},'gtsam.Point2')
|
||||
[ varargout{1} varargout{2} ] = geometry_wrapper(56, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function MyTemplatePoint2.return_ptrs');
|
||||
end
|
||||
end
|
||||
|
||||
function varargout = templatedMethodMatrix(this, varargin)
|
||||
% TEMPLATEDMETHODMATRIX usage: templatedMethodMatrix(Matrix t) : returns Matrix
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
if length(varargin) == 1 && isa(varargin{1},'double')
|
||||
varargout{1} = geometry_wrapper(57, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function MyTemplatePoint2.templatedMethod');
|
||||
end
|
||||
end
|
||||
|
||||
function varargout = templatedMethodPoint2(this, varargin)
|
||||
% TEMPLATEDMETHODPOINT2 usage: templatedMethodPoint2(Point2 t) : returns gtsam::Point2
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
if length(varargin) == 1 && isa(varargin{1},'gtsam.Point2')
|
||||
varargout{1} = geometry_wrapper(58, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function MyTemplatePoint2.templatedMethod');
|
||||
end
|
||||
end
|
||||
|
||||
function varargout = templatedMethodPoint3(this, varargin)
|
||||
% TEMPLATEDMETHODPOINT3 usage: templatedMethodPoint3(Point3 t) : returns gtsam::Point3
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
if length(varargin) == 1 && isa(varargin{1},'gtsam.Point3')
|
||||
varargout{1} = geometry_wrapper(59, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function MyTemplatePoint2.templatedMethod');
|
||||
end
|
||||
end
|
||||
|
||||
function varargout = templatedMethodVector(this, varargin)
|
||||
% TEMPLATEDMETHODVECTOR usage: templatedMethodVector(Vector t) : returns Vector
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
if length(varargin) == 1 && isa(varargin{1},'double') && size(varargin{1},2)==1
|
||||
varargout{1} = geometry_wrapper(60, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function MyTemplatePoint2.templatedMethod');
|
||||
end
|
||||
end
|
||||
|
||||
end
|
||||
|
||||
methods(Static = true)
|
||||
end
|
||||
end
|
||||
|
|
@ -7,10 +7,10 @@
|
|||
%
|
||||
%-------Methods-------
|
||||
%arg_EigenConstRef(Matrix value) : returns void
|
||||
%create_MixedPtrs() : returns pair< Test, SharedTest >
|
||||
%create_ptrs() : returns pair< SharedTest, SharedTest >
|
||||
%create_MixedPtrs() : returns pair< Test, Test >
|
||||
%create_ptrs() : returns pair< Test, Test >
|
||||
%print() : returns void
|
||||
%return_Point2Ptr(bool value) : returns Point2
|
||||
%return_Point2Ptr(bool value) : returns gtsam::Point2
|
||||
%return_Test(Test value) : returns Test
|
||||
%return_TestPtr(Test value) : returns Test
|
||||
%return_bool(bool value) : returns bool
|
||||
|
|
@ -20,7 +20,7 @@
|
|||
%return_matrix1(Matrix value) : returns Matrix
|
||||
%return_matrix2(Matrix value) : returns Matrix
|
||||
%return_pair(Vector v, Matrix A) : returns pair< Vector, Matrix >
|
||||
%return_ptrs(Test p1, Test p2) : returns pair< SharedTest, SharedTest >
|
||||
%return_ptrs(Test p1, Test p2) : returns pair< Test, Test >
|
||||
%return_size_t(size_t value) : returns size_t
|
||||
%return_string(string value) : returns string
|
||||
%return_vector1(Vector value) : returns Vector
|
||||
|
|
@ -34,11 +34,11 @@ classdef Test < handle
|
|||
function obj = Test(varargin)
|
||||
if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
|
||||
my_ptr = varargin{2};
|
||||
geometry_wrapper(19, my_ptr);
|
||||
geometry_wrapper(20, my_ptr);
|
||||
elseif nargin == 0
|
||||
my_ptr = geometry_wrapper(20);
|
||||
my_ptr = geometry_wrapper(21);
|
||||
elseif nargin == 2 && isa(varargin{1},'double') && isa(varargin{2},'double')
|
||||
my_ptr = geometry_wrapper(21, varargin{1}, varargin{2});
|
||||
my_ptr = geometry_wrapper(22, varargin{1}, varargin{2});
|
||||
else
|
||||
error('Arguments do not match any overload of Test constructor');
|
||||
end
|
||||
|
|
@ -46,7 +46,7 @@ classdef Test < handle
|
|||
end
|
||||
|
||||
function delete(obj)
|
||||
geometry_wrapper(22, obj.ptr_Test);
|
||||
geometry_wrapper(23, obj.ptr_Test);
|
||||
end
|
||||
|
||||
function display(obj), obj.print(''); end
|
||||
|
|
@ -57,41 +57,41 @@ classdef Test < handle
|
|||
% ARG_EIGENCONSTREF usage: arg_EigenConstRef(Matrix value) : returns void
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
if length(varargin) == 1 && isa(varargin{1},'double')
|
||||
geometry_wrapper(23, this, varargin{:});
|
||||
geometry_wrapper(24, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function Test.arg_EigenConstRef');
|
||||
end
|
||||
end
|
||||
|
||||
function varargout = create_MixedPtrs(this, varargin)
|
||||
% CREATE_MIXEDPTRS usage: create_MixedPtrs() : returns pair< Test, SharedTest >
|
||||
% CREATE_MIXEDPTRS usage: create_MixedPtrs() : returns pair< Test, Test >
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
[ varargout{1} varargout{2} ] = geometry_wrapper(24, this, varargin{:});
|
||||
[ varargout{1} varargout{2} ] = geometry_wrapper(25, this, varargin{:});
|
||||
end
|
||||
|
||||
function varargout = create_ptrs(this, varargin)
|
||||
% CREATE_PTRS usage: create_ptrs() : returns pair< SharedTest, SharedTest >
|
||||
% CREATE_PTRS usage: create_ptrs() : returns pair< Test, Test >
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
[ varargout{1} varargout{2} ] = geometry_wrapper(25, this, varargin{:});
|
||||
[ varargout{1} varargout{2} ] = geometry_wrapper(26, this, varargin{:});
|
||||
end
|
||||
|
||||
function varargout = print(this, varargin)
|
||||
% PRINT usage: print() : returns void
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
geometry_wrapper(26, this, varargin{:});
|
||||
geometry_wrapper(27, this, varargin{:});
|
||||
end
|
||||
|
||||
function varargout = return_Point2Ptr(this, varargin)
|
||||
% RETURN_POINT2PTR usage: return_Point2Ptr(bool value) : returns Point2
|
||||
% RETURN_POINT2PTR usage: return_Point2Ptr(bool value) : returns gtsam::Point2
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
varargout{1} = geometry_wrapper(27, this, varargin{:});
|
||||
varargout{1} = geometry_wrapper(28, this, varargin{:});
|
||||
end
|
||||
|
||||
function varargout = return_Test(this, varargin)
|
||||
% RETURN_TEST usage: return_Test(Test value) : returns Test
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
if length(varargin) == 1 && isa(varargin{1},'Test')
|
||||
varargout{1} = geometry_wrapper(28, this, varargin{:});
|
||||
varargout{1} = geometry_wrapper(29, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function Test.return_Test');
|
||||
end
|
||||
|
|
@ -101,7 +101,7 @@ classdef Test < handle
|
|||
% RETURN_TESTPTR usage: return_TestPtr(Test value) : returns Test
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
if length(varargin) == 1 && isa(varargin{1},'Test')
|
||||
varargout{1} = geometry_wrapper(29, this, varargin{:});
|
||||
varargout{1} = geometry_wrapper(30, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function Test.return_TestPtr');
|
||||
end
|
||||
|
|
@ -110,20 +110,20 @@ classdef Test < handle
|
|||
function varargout = return_bool(this, varargin)
|
||||
% RETURN_BOOL usage: return_bool(bool value) : returns bool
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
varargout{1} = geometry_wrapper(30, this, varargin{:});
|
||||
varargout{1} = geometry_wrapper(31, this, varargin{:});
|
||||
end
|
||||
|
||||
function varargout = return_double(this, varargin)
|
||||
% RETURN_DOUBLE usage: return_double(double value) : returns double
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
varargout{1} = geometry_wrapper(31, this, varargin{:});
|
||||
varargout{1} = geometry_wrapper(32, this, varargin{:});
|
||||
end
|
||||
|
||||
function varargout = return_field(this, varargin)
|
||||
% RETURN_FIELD usage: return_field(Test t) : returns bool
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
if length(varargin) == 1 && isa(varargin{1},'Test')
|
||||
varargout{1} = geometry_wrapper(32, this, varargin{:});
|
||||
varargout{1} = geometry_wrapper(33, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function Test.return_field');
|
||||
end
|
||||
|
|
@ -132,14 +132,14 @@ classdef Test < handle
|
|||
function varargout = return_int(this, varargin)
|
||||
% RETURN_INT usage: return_int(int value) : returns int
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
varargout{1} = geometry_wrapper(33, this, varargin{:});
|
||||
varargout{1} = geometry_wrapper(34, this, varargin{:});
|
||||
end
|
||||
|
||||
function varargout = return_matrix1(this, varargin)
|
||||
% RETURN_MATRIX1 usage: return_matrix1(Matrix value) : returns Matrix
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
if length(varargin) == 1 && isa(varargin{1},'double')
|
||||
varargout{1} = geometry_wrapper(34, this, varargin{:});
|
||||
varargout{1} = geometry_wrapper(35, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function Test.return_matrix1');
|
||||
end
|
||||
|
|
@ -149,7 +149,7 @@ classdef Test < handle
|
|||
% RETURN_MATRIX2 usage: return_matrix2(Matrix value) : returns Matrix
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
if length(varargin) == 1 && isa(varargin{1},'double')
|
||||
varargout{1} = geometry_wrapper(35, this, varargin{:});
|
||||
varargout{1} = geometry_wrapper(36, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function Test.return_matrix2');
|
||||
end
|
||||
|
|
@ -158,18 +158,18 @@ classdef Test < handle
|
|||
function varargout = return_pair(this, varargin)
|
||||
% RETURN_PAIR usage: return_pair(Vector v, Matrix A) : returns pair< Vector, Matrix >
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
if length(varargin) == 2 && isa(varargin{1},'double') && isa(varargin{2},'double')
|
||||
[ varargout{1} varargout{2} ] = geometry_wrapper(36, this, varargin{:});
|
||||
if length(varargin) == 2 && isa(varargin{1},'double') && size(varargin{1},2)==1 && isa(varargin{2},'double')
|
||||
[ varargout{1} varargout{2} ] = geometry_wrapper(37, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function Test.return_pair');
|
||||
end
|
||||
end
|
||||
|
||||
function varargout = return_ptrs(this, varargin)
|
||||
% RETURN_PTRS usage: return_ptrs(Test p1, Test p2) : returns pair< SharedTest, SharedTest >
|
||||
% RETURN_PTRS usage: return_ptrs(Test p1, Test p2) : returns pair< Test, Test >
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
if length(varargin) == 2 && isa(varargin{1},'Test') && isa(varargin{2},'Test')
|
||||
[ varargout{1} varargout{2} ] = geometry_wrapper(37, this, varargin{:});
|
||||
[ varargout{1} varargout{2} ] = geometry_wrapper(38, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function Test.return_ptrs');
|
||||
end
|
||||
|
|
@ -178,14 +178,14 @@ classdef Test < handle
|
|||
function varargout = return_size_t(this, varargin)
|
||||
% RETURN_SIZE_T usage: return_size_t(size_t value) : returns size_t
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
varargout{1} = geometry_wrapper(38, this, varargin{:});
|
||||
varargout{1} = geometry_wrapper(39, this, varargin{:});
|
||||
end
|
||||
|
||||
function varargout = return_string(this, varargin)
|
||||
% RETURN_STRING usage: return_string(string value) : returns string
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
if length(varargin) == 1 && isa(varargin{1},'char')
|
||||
varargout{1} = geometry_wrapper(39, this, varargin{:});
|
||||
varargout{1} = geometry_wrapper(40, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function Test.return_string');
|
||||
end
|
||||
|
|
@ -194,8 +194,8 @@ classdef Test < handle
|
|||
function varargout = return_vector1(this, varargin)
|
||||
% RETURN_VECTOR1 usage: return_vector1(Vector value) : returns Vector
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
if length(varargin) == 1 && isa(varargin{1},'double')
|
||||
varargout{1} = geometry_wrapper(40, this, varargin{:});
|
||||
if length(varargin) == 1 && isa(varargin{1},'double') && size(varargin{1},2)==1
|
||||
varargout{1} = geometry_wrapper(41, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function Test.return_vector1');
|
||||
end
|
||||
|
|
@ -204,8 +204,8 @@ classdef Test < handle
|
|||
function varargout = return_vector2(this, varargin)
|
||||
% RETURN_VECTOR2 usage: return_vector2(Vector value) : returns Vector
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
if length(varargin) == 1 && isa(varargin{1},'double')
|
||||
varargout{1} = geometry_wrapper(41, this, varargin{:});
|
||||
if length(varargin) == 1 && isa(varargin{1},'double') && size(varargin{1},2)==1
|
||||
varargout{1} = geometry_wrapper(42, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function Test.return_vector2');
|
||||
end
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
function varargout = aGlobalFunction(varargin)
|
||||
if length(varargin) == 0
|
||||
varargout{1} = geometry_wrapper(42, varargin{:});
|
||||
varargout{1} = geometry_wrapper(79, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function aGlobalFunction');
|
||||
end
|
||||
|
|
|
|||
File diff suppressed because it is too large
Load Diff
|
|
@ -1,8 +1,8 @@
|
|||
function varargout = overloadedGlobalFunction(varargin)
|
||||
if length(varargin) == 1 && isa(varargin{1},'numeric')
|
||||
varargout{1} = geometry_wrapper(43, varargin{:});
|
||||
varargout{1} = geometry_wrapper(80, varargin{:});
|
||||
elseif length(varargin) == 2 && isa(varargin{1},'numeric') && isa(varargin{2},'double')
|
||||
varargout{1} = geometry_wrapper(44, varargin{:});
|
||||
varargout{1} = geometry_wrapper(81, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function overloadedGlobalFunction');
|
||||
end
|
||||
|
|
|
|||
|
|
@ -9,6 +9,7 @@
|
|||
%argChar(char a) : returns void
|
||||
%argUChar(unsigned char a) : returns void
|
||||
%dim() : returns int
|
||||
%eigenArguments(Vector v, Matrix m) : returns void
|
||||
%returnChar() : returns char
|
||||
%vectorConfusion() : returns VectorNotEigen
|
||||
%x() : returns double
|
||||
|
|
@ -59,28 +60,38 @@ classdef Point2 < handle
|
|||
varargout{1} = geometry_wrapper(6, this, varargin{:});
|
||||
end
|
||||
|
||||
function varargout = eigenArguments(this, varargin)
|
||||
% EIGENARGUMENTS usage: eigenArguments(Vector v, Matrix m) : returns void
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
if length(varargin) == 2 && isa(varargin{1},'double') && size(varargin{1},2)==1 && isa(varargin{2},'double')
|
||||
geometry_wrapper(7, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function gtsam.Point2.eigenArguments');
|
||||
end
|
||||
end
|
||||
|
||||
function varargout = returnChar(this, varargin)
|
||||
% RETURNCHAR usage: returnChar() : returns char
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
varargout{1} = geometry_wrapper(7, this, varargin{:});
|
||||
varargout{1} = geometry_wrapper(8, this, varargin{:});
|
||||
end
|
||||
|
||||
function varargout = vectorConfusion(this, varargin)
|
||||
% VECTORCONFUSION usage: vectorConfusion() : returns VectorNotEigen
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
varargout{1} = geometry_wrapper(8, this, varargin{:});
|
||||
varargout{1} = geometry_wrapper(9, this, varargin{:});
|
||||
end
|
||||
|
||||
function varargout = x(this, varargin)
|
||||
% X usage: x() : returns double
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
varargout{1} = geometry_wrapper(9, this, varargin{:});
|
||||
varargout{1} = geometry_wrapper(10, this, varargin{:});
|
||||
end
|
||||
|
||||
function varargout = y(this, varargin)
|
||||
% Y usage: y() : returns double
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
varargout{1} = geometry_wrapper(10, this, varargin{:});
|
||||
varargout{1} = geometry_wrapper(11, this, varargin{:});
|
||||
end
|
||||
|
||||
end
|
||||
|
|
|
|||
|
|
@ -19,9 +19,9 @@ classdef Point3 < handle
|
|||
function obj = Point3(varargin)
|
||||
if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
|
||||
my_ptr = varargin{2};
|
||||
geometry_wrapper(11, my_ptr);
|
||||
geometry_wrapper(12, my_ptr);
|
||||
elseif nargin == 3 && isa(varargin{1},'double') && isa(varargin{2},'double') && isa(varargin{3},'double')
|
||||
my_ptr = geometry_wrapper(12, varargin{1}, varargin{2}, varargin{3});
|
||||
my_ptr = geometry_wrapper(13, varargin{1}, varargin{2}, varargin{3});
|
||||
else
|
||||
error('Arguments do not match any overload of gtsam.Point3 constructor');
|
||||
end
|
||||
|
|
@ -29,7 +29,7 @@ classdef Point3 < handle
|
|||
end
|
||||
|
||||
function delete(obj)
|
||||
geometry_wrapper(13, obj.ptr_gtsamPoint3);
|
||||
geometry_wrapper(14, obj.ptr_gtsamPoint3);
|
||||
end
|
||||
|
||||
function display(obj), obj.print(''); end
|
||||
|
|
@ -39,7 +39,7 @@ classdef Point3 < handle
|
|||
function varargout = norm(this, varargin)
|
||||
% NORM usage: norm() : returns double
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
varargout{1} = geometry_wrapper(14, this, varargin{:});
|
||||
varargout{1} = geometry_wrapper(15, this, varargin{:});
|
||||
end
|
||||
|
||||
end
|
||||
|
|
@ -48,13 +48,13 @@ classdef Point3 < handle
|
|||
function varargout = StaticFunctionRet(varargin)
|
||||
% STATICFUNCTIONRET usage: StaticFunctionRet(double z) : returns gtsam::Point3
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
varargout{1} = geometry_wrapper(15, varargin{:});
|
||||
varargout{1} = geometry_wrapper(16, varargin{:});
|
||||
end
|
||||
|
||||
function varargout = StaticFunction(varargin)
|
||||
% STATICFUNCTION usage: staticFunction() : returns double
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
varargout{1} = geometry_wrapper(16, varargin{:});
|
||||
varargout{1} = geometry_wrapper(17, varargin{:});
|
||||
end
|
||||
|
||||
end
|
||||
|
|
|
|||
|
|
@ -11,9 +11,9 @@ classdef MyBase < handle
|
|||
if nargin == 2
|
||||
my_ptr = varargin{2};
|
||||
else
|
||||
my_ptr = geometry_wrapper(41, varargin{2});
|
||||
my_ptr = geometry_wrapper(42, varargin{2});
|
||||
end
|
||||
geometry_wrapper(40, my_ptr);
|
||||
geometry_wrapper(41, my_ptr);
|
||||
else
|
||||
error('Arguments do not match any overload of MyBase constructor');
|
||||
end
|
||||
|
|
@ -21,7 +21,7 @@ classdef MyBase < handle
|
|||
end
|
||||
|
||||
function delete(obj)
|
||||
geometry_wrapper(42, obj.ptr_MyBase);
|
||||
geometry_wrapper(43, obj.ptr_MyBase);
|
||||
end
|
||||
|
||||
function display(obj), obj.print(''); end
|
||||
|
|
|
|||
|
|
@ -12,9 +12,9 @@ classdef MyFactorPosePoint2 < handle
|
|||
function obj = MyFactorPosePoint2(varargin)
|
||||
if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
|
||||
my_ptr = varargin{2};
|
||||
geometry_wrapper(69, my_ptr);
|
||||
geometry_wrapper(74, my_ptr);
|
||||
elseif nargin == 4 && isa(varargin{1},'numeric') && isa(varargin{2},'numeric') && isa(varargin{3},'double') && isa(varargin{4},'gtsam.noiseModel.Base')
|
||||
my_ptr = geometry_wrapper(70, varargin{1}, varargin{2}, varargin{3}, varargin{4});
|
||||
my_ptr = geometry_wrapper(75, varargin{1}, varargin{2}, varargin{3}, varargin{4});
|
||||
else
|
||||
error('Arguments do not match any overload of MyFactorPosePoint2 constructor');
|
||||
end
|
||||
|
|
@ -22,7 +22,7 @@ classdef MyFactorPosePoint2 < handle
|
|||
end
|
||||
|
||||
function delete(obj)
|
||||
geometry_wrapper(71, obj.ptr_MyFactorPosePoint2);
|
||||
geometry_wrapper(76, obj.ptr_MyFactorPosePoint2);
|
||||
end
|
||||
|
||||
function display(obj), obj.print(''); end
|
||||
|
|
|
|||
|
|
@ -0,0 +1,156 @@
|
|||
%class MyTemplateMatrix, see Doxygen page for details
|
||||
%at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
%
|
||||
%-------Constructors-------
|
||||
%MyTemplateMatrix()
|
||||
%
|
||||
%-------Methods-------
|
||||
%accept_T(Matrix value) : returns void
|
||||
%accept_Tptr(Matrix value) : returns void
|
||||
%create_MixedPtrs() : returns pair< Matrix, Matrix >
|
||||
%create_ptrs() : returns pair< Matrix, Matrix >
|
||||
%return_T(Matrix value) : returns Matrix
|
||||
%return_Tptr(Matrix value) : returns Matrix
|
||||
%return_ptrs(Matrix p1, Matrix p2) : returns pair< Matrix, Matrix >
|
||||
%templatedMethodMatrix(Matrix t) : returns Matrix
|
||||
%templatedMethodPoint2(Point2 t) : returns gtsam::Point2
|
||||
%templatedMethodPoint3(Point3 t) : returns gtsam::Point3
|
||||
%templatedMethodVector(Vector t) : returns Vector
|
||||
%
|
||||
classdef MyTemplateMatrix < MyBase
|
||||
properties
|
||||
ptr_MyTemplateMatrix = 0
|
||||
end
|
||||
methods
|
||||
function obj = MyTemplateMatrix(varargin)
|
||||
if (nargin == 2 || (nargin == 3 && strcmp(varargin{3}, 'void'))) && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
|
||||
if nargin == 2
|
||||
my_ptr = varargin{2};
|
||||
else
|
||||
my_ptr = geometry_wrapper(60, varargin{2});
|
||||
end
|
||||
base_ptr = geometry_wrapper(59, my_ptr);
|
||||
elseif nargin == 0
|
||||
[ my_ptr, base_ptr ] = geometry_wrapper(61);
|
||||
else
|
||||
error('Arguments do not match any overload of MyTemplateMatrix constructor');
|
||||
end
|
||||
obj = obj@MyBase(uint64(5139824614673773682), base_ptr);
|
||||
obj.ptr_MyTemplateMatrix = my_ptr;
|
||||
end
|
||||
|
||||
function delete(obj)
|
||||
geometry_wrapper(62, obj.ptr_MyTemplateMatrix);
|
||||
end
|
||||
|
||||
function display(obj), obj.print(''); end
|
||||
%DISPLAY Calls print on the object
|
||||
function disp(obj), obj.display; end
|
||||
%DISP Calls print on the object
|
||||
function varargout = accept_T(this, varargin)
|
||||
% ACCEPT_T usage: accept_T(Matrix value) : returns void
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
if length(varargin) == 1 && isa(varargin{1},'double')
|
||||
geometry_wrapper(63, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function MyTemplateMatrix.accept_T');
|
||||
end
|
||||
end
|
||||
|
||||
function varargout = accept_Tptr(this, varargin)
|
||||
% ACCEPT_TPTR usage: accept_Tptr(Matrix value) : returns void
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
if length(varargin) == 1 && isa(varargin{1},'double')
|
||||
geometry_wrapper(64, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function MyTemplateMatrix.accept_Tptr');
|
||||
end
|
||||
end
|
||||
|
||||
function varargout = create_MixedPtrs(this, varargin)
|
||||
% CREATE_MIXEDPTRS usage: create_MixedPtrs() : returns pair< Matrix, Matrix >
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
[ varargout{1} varargout{2} ] = geometry_wrapper(65, this, varargin{:});
|
||||
end
|
||||
|
||||
function varargout = create_ptrs(this, varargin)
|
||||
% CREATE_PTRS usage: create_ptrs() : returns pair< Matrix, Matrix >
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
[ varargout{1} varargout{2} ] = geometry_wrapper(66, this, varargin{:});
|
||||
end
|
||||
|
||||
function varargout = return_T(this, varargin)
|
||||
% RETURN_T usage: return_T(Matrix value) : returns Matrix
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
if length(varargin) == 1 && isa(varargin{1},'double')
|
||||
varargout{1} = geometry_wrapper(67, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function MyTemplateMatrix.return_T');
|
||||
end
|
||||
end
|
||||
|
||||
function varargout = return_Tptr(this, varargin)
|
||||
% RETURN_TPTR usage: return_Tptr(Matrix value) : returns Matrix
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
if length(varargin) == 1 && isa(varargin{1},'double')
|
||||
varargout{1} = geometry_wrapper(68, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function MyTemplateMatrix.return_Tptr');
|
||||
end
|
||||
end
|
||||
|
||||
function varargout = return_ptrs(this, varargin)
|
||||
% RETURN_PTRS usage: return_ptrs(Matrix p1, Matrix p2) : returns pair< Matrix, Matrix >
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
if length(varargin) == 2 && isa(varargin{1},'double') && isa(varargin{2},'double')
|
||||
[ varargout{1} varargout{2} ] = geometry_wrapper(69, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function MyTemplateMatrix.return_ptrs');
|
||||
end
|
||||
end
|
||||
|
||||
function varargout = templatedMethodMatrix(this, varargin)
|
||||
% TEMPLATEDMETHODMATRIX usage: templatedMethodMatrix(Matrix t) : returns Matrix
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
if length(varargin) == 1 && isa(varargin{1},'double')
|
||||
varargout{1} = geometry_wrapper(70, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function MyTemplateMatrix.templatedMethod');
|
||||
end
|
||||
end
|
||||
|
||||
function varargout = templatedMethodPoint2(this, varargin)
|
||||
% TEMPLATEDMETHODPOINT2 usage: templatedMethodPoint2(Point2 t) : returns gtsam::Point2
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
if length(varargin) == 1 && isa(varargin{1},'gtsam.Point2')
|
||||
varargout{1} = geometry_wrapper(71, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function MyTemplateMatrix.templatedMethod');
|
||||
end
|
||||
end
|
||||
|
||||
function varargout = templatedMethodPoint3(this, varargin)
|
||||
% TEMPLATEDMETHODPOINT3 usage: templatedMethodPoint3(Point3 t) : returns gtsam::Point3
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
if length(varargin) == 1 && isa(varargin{1},'gtsam.Point3')
|
||||
varargout{1} = geometry_wrapper(72, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function MyTemplateMatrix.templatedMethod');
|
||||
end
|
||||
end
|
||||
|
||||
function varargout = templatedMethodVector(this, varargin)
|
||||
% TEMPLATEDMETHODVECTOR usage: templatedMethodVector(Vector t) : returns Vector
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
if length(varargin) == 1 && isa(varargin{1},'double') && size(varargin{1},2)==1
|
||||
varargout{1} = geometry_wrapper(73, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function MyTemplateMatrix.templatedMethod');
|
||||
end
|
||||
end
|
||||
|
||||
end
|
||||
|
||||
methods(Static = true)
|
||||
end
|
||||
end
|
||||
|
|
@ -12,8 +12,10 @@
|
|||
%return_T(Point2 value) : returns gtsam::Point2
|
||||
%return_Tptr(Point2 value) : returns gtsam::Point2
|
||||
%return_ptrs(Point2 p1, Point2 p2) : returns pair< gtsam::Point2, gtsam::Point2 >
|
||||
%templatedMethodPoint2(Point2 t) : returns void
|
||||
%templatedMethodPoint3(Point3 t) : returns void
|
||||
%templatedMethodMatrix(Matrix t) : returns Matrix
|
||||
%templatedMethodPoint2(Point2 t) : returns gtsam::Point2
|
||||
%templatedMethodPoint3(Point3 t) : returns gtsam::Point3
|
||||
%templatedMethodVector(Vector t) : returns Vector
|
||||
%
|
||||
classdef MyTemplatePoint2 < MyBase
|
||||
properties
|
||||
|
|
@ -25,11 +27,11 @@ classdef MyTemplatePoint2 < MyBase
|
|||
if nargin == 2
|
||||
my_ptr = varargin{2};
|
||||
else
|
||||
my_ptr = geometry_wrapper(44, varargin{2});
|
||||
my_ptr = geometry_wrapper(45, varargin{2});
|
||||
end
|
||||
base_ptr = geometry_wrapper(43, my_ptr);
|
||||
base_ptr = geometry_wrapper(44, my_ptr);
|
||||
elseif nargin == 0
|
||||
[ my_ptr, base_ptr ] = geometry_wrapper(45);
|
||||
[ my_ptr, base_ptr ] = geometry_wrapper(46);
|
||||
else
|
||||
error('Arguments do not match any overload of MyTemplatePoint2 constructor');
|
||||
end
|
||||
|
|
@ -38,7 +40,7 @@ classdef MyTemplatePoint2 < MyBase
|
|||
end
|
||||
|
||||
function delete(obj)
|
||||
geometry_wrapper(46, obj.ptr_MyTemplatePoint2);
|
||||
geometry_wrapper(47, obj.ptr_MyTemplatePoint2);
|
||||
end
|
||||
|
||||
function display(obj), obj.print(''); end
|
||||
|
|
@ -49,7 +51,7 @@ classdef MyTemplatePoint2 < MyBase
|
|||
% ACCEPT_T usage: accept_T(Point2 value) : returns void
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
if length(varargin) == 1 && isa(varargin{1},'gtsam.Point2')
|
||||
geometry_wrapper(47, this, varargin{:});
|
||||
geometry_wrapper(48, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function MyTemplatePoint2.accept_T');
|
||||
end
|
||||
|
|
@ -59,7 +61,7 @@ classdef MyTemplatePoint2 < MyBase
|
|||
% ACCEPT_TPTR usage: accept_Tptr(Point2 value) : returns void
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
if length(varargin) == 1 && isa(varargin{1},'gtsam.Point2')
|
||||
geometry_wrapper(48, this, varargin{:});
|
||||
geometry_wrapper(49, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function MyTemplatePoint2.accept_Tptr');
|
||||
end
|
||||
|
|
@ -68,20 +70,20 @@ classdef MyTemplatePoint2 < MyBase
|
|||
function varargout = create_MixedPtrs(this, varargin)
|
||||
% CREATE_MIXEDPTRS usage: create_MixedPtrs() : returns pair< gtsam::Point2, gtsam::Point2 >
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
[ varargout{1} varargout{2} ] = geometry_wrapper(49, this, varargin{:});
|
||||
[ varargout{1} varargout{2} ] = geometry_wrapper(50, this, varargin{:});
|
||||
end
|
||||
|
||||
function varargout = create_ptrs(this, varargin)
|
||||
% CREATE_PTRS usage: create_ptrs() : returns pair< gtsam::Point2, gtsam::Point2 >
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
[ varargout{1} varargout{2} ] = geometry_wrapper(50, this, varargin{:});
|
||||
[ varargout{1} varargout{2} ] = geometry_wrapper(51, this, varargin{:});
|
||||
end
|
||||
|
||||
function varargout = return_T(this, varargin)
|
||||
% RETURN_T usage: return_T(Point2 value) : returns gtsam::Point2
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
if length(varargin) == 1 && isa(varargin{1},'gtsam.Point2')
|
||||
varargout{1} = geometry_wrapper(51, this, varargin{:});
|
||||
varargout{1} = geometry_wrapper(52, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function MyTemplatePoint2.return_T');
|
||||
end
|
||||
|
|
@ -91,7 +93,7 @@ classdef MyTemplatePoint2 < MyBase
|
|||
% RETURN_TPTR usage: return_Tptr(Point2 value) : returns gtsam::Point2
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
if length(varargin) == 1 && isa(varargin{1},'gtsam.Point2')
|
||||
varargout{1} = geometry_wrapper(52, this, varargin{:});
|
||||
varargout{1} = geometry_wrapper(53, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function MyTemplatePoint2.return_Tptr');
|
||||
end
|
||||
|
|
@ -101,27 +103,47 @@ classdef MyTemplatePoint2 < MyBase
|
|||
% RETURN_PTRS usage: return_ptrs(Point2 p1, Point2 p2) : returns pair< gtsam::Point2, gtsam::Point2 >
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
if length(varargin) == 2 && isa(varargin{1},'gtsam.Point2') && isa(varargin{2},'gtsam.Point2')
|
||||
[ varargout{1} varargout{2} ] = geometry_wrapper(53, this, varargin{:});
|
||||
[ varargout{1} varargout{2} ] = geometry_wrapper(54, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function MyTemplatePoint2.return_ptrs');
|
||||
end
|
||||
end
|
||||
|
||||
function varargout = templatedMethodMatrix(this, varargin)
|
||||
% TEMPLATEDMETHODMATRIX usage: templatedMethodMatrix(Matrix t) : returns Matrix
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
if length(varargin) == 1 && isa(varargin{1},'double')
|
||||
varargout{1} = geometry_wrapper(55, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function MyTemplatePoint2.templatedMethod');
|
||||
end
|
||||
end
|
||||
|
||||
function varargout = templatedMethodPoint2(this, varargin)
|
||||
% TEMPLATEDMETHODPOINT2 usage: templatedMethodPoint2(Point2 t) : returns void
|
||||
% TEMPLATEDMETHODPOINT2 usage: templatedMethodPoint2(Point2 t) : returns gtsam::Point2
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
if length(varargin) == 1 && isa(varargin{1},'gtsam.Point2')
|
||||
geometry_wrapper(54, this, varargin{:});
|
||||
varargout{1} = geometry_wrapper(56, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function MyTemplatePoint2.templatedMethod');
|
||||
end
|
||||
end
|
||||
|
||||
function varargout = templatedMethodPoint3(this, varargin)
|
||||
% TEMPLATEDMETHODPOINT3 usage: templatedMethodPoint3(Point3 t) : returns void
|
||||
% TEMPLATEDMETHODPOINT3 usage: templatedMethodPoint3(Point3 t) : returns gtsam::Point3
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
if length(varargin) == 1 && isa(varargin{1},'gtsam.Point3')
|
||||
geometry_wrapper(55, this, varargin{:});
|
||||
varargout{1} = geometry_wrapper(57, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function MyTemplatePoint2.templatedMethod');
|
||||
end
|
||||
end
|
||||
|
||||
function varargout = templatedMethodVector(this, varargin)
|
||||
% TEMPLATEDMETHODVECTOR usage: templatedMethodVector(Vector t) : returns Vector
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
if length(varargin) == 1 && isa(varargin{1},'double') && size(varargin{1},2)==1
|
||||
varargout{1} = geometry_wrapper(58, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function MyTemplatePoint2.templatedMethod');
|
||||
end
|
||||
|
|
|
|||
|
|
@ -1,134 +0,0 @@
|
|||
%class MyTemplatePoint3, see Doxygen page for details
|
||||
%at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
%
|
||||
%-------Constructors-------
|
||||
%MyTemplatePoint3()
|
||||
%
|
||||
%-------Methods-------
|
||||
%accept_T(Point3 value) : returns void
|
||||
%accept_Tptr(Point3 value) : returns void
|
||||
%create_MixedPtrs() : returns pair< gtsam::Point3, gtsam::Point3 >
|
||||
%create_ptrs() : returns pair< gtsam::Point3, gtsam::Point3 >
|
||||
%return_T(Point3 value) : returns gtsam::Point3
|
||||
%return_Tptr(Point3 value) : returns gtsam::Point3
|
||||
%return_ptrs(Point3 p1, Point3 p2) : returns pair< gtsam::Point3, gtsam::Point3 >
|
||||
%templatedMethodPoint2(Point2 t) : returns void
|
||||
%templatedMethodPoint3(Point3 t) : returns void
|
||||
%
|
||||
classdef MyTemplatePoint3 < MyBase
|
||||
properties
|
||||
ptr_MyTemplatePoint3 = 0
|
||||
end
|
||||
methods
|
||||
function obj = MyTemplatePoint3(varargin)
|
||||
if (nargin == 2 || (nargin == 3 && strcmp(varargin{3}, 'void'))) && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
|
||||
if nargin == 2
|
||||
my_ptr = varargin{2};
|
||||
else
|
||||
my_ptr = geometry_wrapper(57, varargin{2});
|
||||
end
|
||||
base_ptr = geometry_wrapper(56, my_ptr);
|
||||
elseif nargin == 0
|
||||
[ my_ptr, base_ptr ] = geometry_wrapper(58);
|
||||
else
|
||||
error('Arguments do not match any overload of MyTemplatePoint3 constructor');
|
||||
end
|
||||
obj = obj@MyBase(uint64(5139824614673773682), base_ptr);
|
||||
obj.ptr_MyTemplatePoint3 = my_ptr;
|
||||
end
|
||||
|
||||
function delete(obj)
|
||||
geometry_wrapper(59, obj.ptr_MyTemplatePoint3);
|
||||
end
|
||||
|
||||
function display(obj), obj.print(''); end
|
||||
%DISPLAY Calls print on the object
|
||||
function disp(obj), obj.display; end
|
||||
%DISP Calls print on the object
|
||||
function varargout = accept_T(this, varargin)
|
||||
% ACCEPT_T usage: accept_T(Point3 value) : returns void
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
if length(varargin) == 1 && isa(varargin{1},'gtsam.Point3')
|
||||
geometry_wrapper(60, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function MyTemplatePoint3.accept_T');
|
||||
end
|
||||
end
|
||||
|
||||
function varargout = accept_Tptr(this, varargin)
|
||||
% ACCEPT_TPTR usage: accept_Tptr(Point3 value) : returns void
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
if length(varargin) == 1 && isa(varargin{1},'gtsam.Point3')
|
||||
geometry_wrapper(61, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function MyTemplatePoint3.accept_Tptr');
|
||||
end
|
||||
end
|
||||
|
||||
function varargout = create_MixedPtrs(this, varargin)
|
||||
% CREATE_MIXEDPTRS usage: create_MixedPtrs() : returns pair< gtsam::Point3, gtsam::Point3 >
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
[ varargout{1} varargout{2} ] = geometry_wrapper(62, this, varargin{:});
|
||||
end
|
||||
|
||||
function varargout = create_ptrs(this, varargin)
|
||||
% CREATE_PTRS usage: create_ptrs() : returns pair< gtsam::Point3, gtsam::Point3 >
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
[ varargout{1} varargout{2} ] = geometry_wrapper(63, this, varargin{:});
|
||||
end
|
||||
|
||||
function varargout = return_T(this, varargin)
|
||||
% RETURN_T usage: return_T(Point3 value) : returns gtsam::Point3
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
if length(varargin) == 1 && isa(varargin{1},'gtsam.Point3')
|
||||
varargout{1} = geometry_wrapper(64, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function MyTemplatePoint3.return_T');
|
||||
end
|
||||
end
|
||||
|
||||
function varargout = return_Tptr(this, varargin)
|
||||
% RETURN_TPTR usage: return_Tptr(Point3 value) : returns gtsam::Point3
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
if length(varargin) == 1 && isa(varargin{1},'gtsam.Point3')
|
||||
varargout{1} = geometry_wrapper(65, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function MyTemplatePoint3.return_Tptr');
|
||||
end
|
||||
end
|
||||
|
||||
function varargout = return_ptrs(this, varargin)
|
||||
% RETURN_PTRS usage: return_ptrs(Point3 p1, Point3 p2) : returns pair< gtsam::Point3, gtsam::Point3 >
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
if length(varargin) == 2 && isa(varargin{1},'gtsam.Point3') && isa(varargin{2},'gtsam.Point3')
|
||||
[ varargout{1} varargout{2} ] = geometry_wrapper(66, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function MyTemplatePoint3.return_ptrs');
|
||||
end
|
||||
end
|
||||
|
||||
function varargout = templatedMethodPoint2(this, varargin)
|
||||
% TEMPLATEDMETHODPOINT2 usage: templatedMethodPoint2(Point2 t) : returns void
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
if length(varargin) == 1 && isa(varargin{1},'gtsam.Point2')
|
||||
geometry_wrapper(67, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function MyTemplatePoint3.templatedMethod');
|
||||
end
|
||||
end
|
||||
|
||||
function varargout = templatedMethodPoint3(this, varargin)
|
||||
% TEMPLATEDMETHODPOINT3 usage: templatedMethodPoint3(Point3 t) : returns void
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
if length(varargin) == 1 && isa(varargin{1},'gtsam.Point3')
|
||||
geometry_wrapper(68, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function MyTemplatePoint3.templatedMethod');
|
||||
end
|
||||
end
|
||||
|
||||
end
|
||||
|
||||
methods(Static = true)
|
||||
end
|
||||
end
|
||||
|
|
@ -34,11 +34,11 @@ classdef Test < handle
|
|||
function obj = Test(varargin)
|
||||
if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
|
||||
my_ptr = varargin{2};
|
||||
geometry_wrapper(17, my_ptr);
|
||||
geometry_wrapper(18, my_ptr);
|
||||
elseif nargin == 0
|
||||
my_ptr = geometry_wrapper(18);
|
||||
my_ptr = geometry_wrapper(19);
|
||||
elseif nargin == 2 && isa(varargin{1},'double') && isa(varargin{2},'double')
|
||||
my_ptr = geometry_wrapper(19, varargin{1}, varargin{2});
|
||||
my_ptr = geometry_wrapper(20, varargin{1}, varargin{2});
|
||||
else
|
||||
error('Arguments do not match any overload of Test constructor');
|
||||
end
|
||||
|
|
@ -46,7 +46,7 @@ classdef Test < handle
|
|||
end
|
||||
|
||||
function delete(obj)
|
||||
geometry_wrapper(20, obj.ptr_Test);
|
||||
geometry_wrapper(21, obj.ptr_Test);
|
||||
end
|
||||
|
||||
function display(obj), obj.print(''); end
|
||||
|
|
@ -57,7 +57,7 @@ classdef Test < handle
|
|||
% ARG_EIGENCONSTREF usage: arg_EigenConstRef(Matrix value) : returns void
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
if length(varargin) == 1 && isa(varargin{1},'double')
|
||||
geometry_wrapper(21, this, varargin{:});
|
||||
geometry_wrapper(22, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function Test.arg_EigenConstRef');
|
||||
end
|
||||
|
|
@ -66,32 +66,32 @@ classdef Test < handle
|
|||
function varargout = create_MixedPtrs(this, varargin)
|
||||
% CREATE_MIXEDPTRS usage: create_MixedPtrs() : returns pair< Test, Test >
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
[ varargout{1} varargout{2} ] = geometry_wrapper(22, this, varargin{:});
|
||||
[ varargout{1} varargout{2} ] = geometry_wrapper(23, this, varargin{:});
|
||||
end
|
||||
|
||||
function varargout = create_ptrs(this, varargin)
|
||||
% CREATE_PTRS usage: create_ptrs() : returns pair< Test, Test >
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
[ varargout{1} varargout{2} ] = geometry_wrapper(23, this, varargin{:});
|
||||
[ varargout{1} varargout{2} ] = geometry_wrapper(24, this, varargin{:});
|
||||
end
|
||||
|
||||
function varargout = print(this, varargin)
|
||||
% PRINT usage: print() : returns void
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
geometry_wrapper(24, this, varargin{:});
|
||||
geometry_wrapper(25, this, varargin{:});
|
||||
end
|
||||
|
||||
function varargout = return_Point2Ptr(this, varargin)
|
||||
% RETURN_POINT2PTR usage: return_Point2Ptr(bool value) : returns gtsam::Point2
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
varargout{1} = geometry_wrapper(25, this, varargin{:});
|
||||
varargout{1} = geometry_wrapper(26, this, varargin{:});
|
||||
end
|
||||
|
||||
function varargout = return_Test(this, varargin)
|
||||
% RETURN_TEST usage: return_Test(Test value) : returns Test
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
if length(varargin) == 1 && isa(varargin{1},'Test')
|
||||
varargout{1} = geometry_wrapper(26, this, varargin{:});
|
||||
varargout{1} = geometry_wrapper(27, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function Test.return_Test');
|
||||
end
|
||||
|
|
@ -101,7 +101,7 @@ classdef Test < handle
|
|||
% RETURN_TESTPTR usage: return_TestPtr(Test value) : returns Test
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
if length(varargin) == 1 && isa(varargin{1},'Test')
|
||||
varargout{1} = geometry_wrapper(27, this, varargin{:});
|
||||
varargout{1} = geometry_wrapper(28, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function Test.return_TestPtr');
|
||||
end
|
||||
|
|
@ -110,20 +110,20 @@ classdef Test < handle
|
|||
function varargout = return_bool(this, varargin)
|
||||
% RETURN_BOOL usage: return_bool(bool value) : returns bool
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
varargout{1} = geometry_wrapper(28, this, varargin{:});
|
||||
varargout{1} = geometry_wrapper(29, this, varargin{:});
|
||||
end
|
||||
|
||||
function varargout = return_double(this, varargin)
|
||||
% RETURN_DOUBLE usage: return_double(double value) : returns double
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
varargout{1} = geometry_wrapper(29, this, varargin{:});
|
||||
varargout{1} = geometry_wrapper(30, this, varargin{:});
|
||||
end
|
||||
|
||||
function varargout = return_field(this, varargin)
|
||||
% RETURN_FIELD usage: return_field(Test t) : returns bool
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
if length(varargin) == 1 && isa(varargin{1},'Test')
|
||||
varargout{1} = geometry_wrapper(30, this, varargin{:});
|
||||
varargout{1} = geometry_wrapper(31, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function Test.return_field');
|
||||
end
|
||||
|
|
@ -132,14 +132,14 @@ classdef Test < handle
|
|||
function varargout = return_int(this, varargin)
|
||||
% RETURN_INT usage: return_int(int value) : returns int
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
varargout{1} = geometry_wrapper(31, this, varargin{:});
|
||||
varargout{1} = geometry_wrapper(32, this, varargin{:});
|
||||
end
|
||||
|
||||
function varargout = return_matrix1(this, varargin)
|
||||
% RETURN_MATRIX1 usage: return_matrix1(Matrix value) : returns Matrix
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
if length(varargin) == 1 && isa(varargin{1},'double')
|
||||
varargout{1} = geometry_wrapper(32, this, varargin{:});
|
||||
varargout{1} = geometry_wrapper(33, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function Test.return_matrix1');
|
||||
end
|
||||
|
|
@ -149,7 +149,7 @@ classdef Test < handle
|
|||
% RETURN_MATRIX2 usage: return_matrix2(Matrix value) : returns Matrix
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
if length(varargin) == 1 && isa(varargin{1},'double')
|
||||
varargout{1} = geometry_wrapper(33, this, varargin{:});
|
||||
varargout{1} = geometry_wrapper(34, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function Test.return_matrix2');
|
||||
end
|
||||
|
|
@ -158,8 +158,8 @@ classdef Test < handle
|
|||
function varargout = return_pair(this, varargin)
|
||||
% RETURN_PAIR usage: return_pair(Vector v, Matrix A) : returns pair< Vector, Matrix >
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
if length(varargin) == 2 && isa(varargin{1},'double') && isa(varargin{2},'double')
|
||||
[ varargout{1} varargout{2} ] = geometry_wrapper(34, this, varargin{:});
|
||||
if length(varargin) == 2 && isa(varargin{1},'double') && size(varargin{1},2)==1 && isa(varargin{2},'double')
|
||||
[ varargout{1} varargout{2} ] = geometry_wrapper(35, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function Test.return_pair');
|
||||
end
|
||||
|
|
@ -169,7 +169,7 @@ classdef Test < handle
|
|||
% RETURN_PTRS usage: return_ptrs(Test p1, Test p2) : returns pair< Test, Test >
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
if length(varargin) == 2 && isa(varargin{1},'Test') && isa(varargin{2},'Test')
|
||||
[ varargout{1} varargout{2} ] = geometry_wrapper(35, this, varargin{:});
|
||||
[ varargout{1} varargout{2} ] = geometry_wrapper(36, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function Test.return_ptrs');
|
||||
end
|
||||
|
|
@ -178,14 +178,14 @@ classdef Test < handle
|
|||
function varargout = return_size_t(this, varargin)
|
||||
% RETURN_SIZE_T usage: return_size_t(size_t value) : returns size_t
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
varargout{1} = geometry_wrapper(36, this, varargin{:});
|
||||
varargout{1} = geometry_wrapper(37, this, varargin{:});
|
||||
end
|
||||
|
||||
function varargout = return_string(this, varargin)
|
||||
% RETURN_STRING usage: return_string(string value) : returns string
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
if length(varargin) == 1 && isa(varargin{1},'char')
|
||||
varargout{1} = geometry_wrapper(37, this, varargin{:});
|
||||
varargout{1} = geometry_wrapper(38, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function Test.return_string');
|
||||
end
|
||||
|
|
@ -194,8 +194,8 @@ classdef Test < handle
|
|||
function varargout = return_vector1(this, varargin)
|
||||
% RETURN_VECTOR1 usage: return_vector1(Vector value) : returns Vector
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
if length(varargin) == 1 && isa(varargin{1},'double')
|
||||
varargout{1} = geometry_wrapper(38, this, varargin{:});
|
||||
if length(varargin) == 1 && isa(varargin{1},'double') && size(varargin{1},2)==1
|
||||
varargout{1} = geometry_wrapper(39, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function Test.return_vector1');
|
||||
end
|
||||
|
|
@ -204,8 +204,8 @@ classdef Test < handle
|
|||
function varargout = return_vector2(this, varargin)
|
||||
% RETURN_VECTOR2 usage: return_vector2(Vector value) : returns Vector
|
||||
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
|
||||
if length(varargin) == 1 && isa(varargin{1},'double')
|
||||
varargout{1} = geometry_wrapper(39, this, varargin{:});
|
||||
if length(varargin) == 1 && isa(varargin{1},'double') && size(varargin{1},2)==1
|
||||
varargout{1} = geometry_wrapper(40, this, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function Test.return_vector2');
|
||||
end
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
function varargout = aGlobalFunction(varargin)
|
||||
if length(varargin) == 0
|
||||
varargout{1} = geometry_wrapper(72, varargin{:});
|
||||
varargout{1} = geometry_wrapper(77, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function aGlobalFunction');
|
||||
end
|
||||
|
|
|
|||
File diff suppressed because it is too large
Load Diff
|
|
@ -1,8 +1,8 @@
|
|||
function varargout = overloadedGlobalFunction(varargin)
|
||||
if length(varargin) == 1 && isa(varargin{1},'numeric')
|
||||
varargout{1} = geometry_wrapper(73, varargin{:});
|
||||
varargout{1} = geometry_wrapper(78, varargin{:});
|
||||
elseif length(varargin) == 2 && isa(varargin{1},'numeric') && isa(varargin{2},'double')
|
||||
varargout{1} = geometry_wrapper(74, varargin{:});
|
||||
varargout{1} = geometry_wrapper(79, varargin{:});
|
||||
else
|
||||
error('Arguments do not match any overload of function overloadedGlobalFunction');
|
||||
end
|
||||
|
|
|
|||
|
|
@ -14,6 +14,7 @@ class Point2 {
|
|||
char returnChar() const;
|
||||
void argChar(char a) const;
|
||||
void argUChar(unsigned char a) const;
|
||||
void eigenArguments(Vector v, Matrix m) const;
|
||||
VectorNotEigen vectorConfusion();
|
||||
|
||||
void serializable() const; // Sets flag and creates export, but does not make serialization functions
|
||||
|
|
@ -100,12 +101,12 @@ virtual class MyBase {
|
|||
};
|
||||
|
||||
// A templated class
|
||||
template<T = {gtsam::Point2, gtsam::Point3}>
|
||||
template<T = {gtsam::Point2, Matrix}>
|
||||
virtual class MyTemplate : MyBase {
|
||||
MyTemplate();
|
||||
|
||||
template<ARG = {gtsam::Point2, gtsam::Point3}>
|
||||
void templatedMethod(const ARG& t);
|
||||
template<ARG = {gtsam::Point2, gtsam::Point3, Vector, Matrix}>
|
||||
ARG templatedMethod(const ARG& t);
|
||||
|
||||
// Stress test templates and pointer combinations
|
||||
void accept_T(const T& value) const;
|
||||
|
|
@ -124,7 +125,7 @@ class MyFactor {
|
|||
};
|
||||
|
||||
// and a typedef specializing it
|
||||
typedef MyFactor<gtsam::Pose2, gtsam::Point2> MyFactorPosePoint2;
|
||||
typedef MyFactor<gtsam::Pose2, Matrix> MyFactorPosePoint2;
|
||||
|
||||
// comments at the end!
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,131 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 testArgument.cpp
|
||||
* @brief Unit test for Argument class
|
||||
* @author Frank Dellaert
|
||||
* @date Nov 12, 2014
|
||||
**/
|
||||
|
||||
#include <wrap/Argument.h>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <iostream>
|
||||
|
||||
using namespace std;
|
||||
using namespace wrap;
|
||||
|
||||
//******************************************************************************
|
||||
TEST( Argument, grammar ) {
|
||||
|
||||
using classic::space_p;
|
||||
|
||||
// Create type grammar that will place result in actual
|
||||
Argument actual, arg0;
|
||||
ArgumentGrammar g(actual);
|
||||
|
||||
EXPECT(parse("const gtsam::Point2& p4", g, space_p).full);
|
||||
EXPECT(actual.type==Qualified("gtsam","Point2",Qualified::CLASS));
|
||||
EXPECT(actual.name=="p4");
|
||||
EXPECT(actual.is_const);
|
||||
EXPECT(actual.is_ref);
|
||||
EXPECT(!actual.is_ptr);
|
||||
actual = arg0;
|
||||
|
||||
EXPECT(parse("Point2& p", g, space_p).full);
|
||||
EXPECT(actual.type==Qualified("Point2",Qualified::CLASS));
|
||||
EXPECT(actual.name=="p");
|
||||
EXPECT(!actual.is_const);
|
||||
EXPECT(actual.is_ref);
|
||||
EXPECT(!actual.is_ptr);
|
||||
actual = arg0;
|
||||
|
||||
EXPECT(parse("gtsam::Point2* p3", g, space_p).full);
|
||||
EXPECT(actual.type==Qualified("gtsam","Point2",Qualified::CLASS));
|
||||
EXPECT(actual.name=="p3");
|
||||
EXPECT(!actual.is_const);
|
||||
EXPECT(!actual.is_ref);
|
||||
EXPECT(actual.is_ptr);
|
||||
actual = arg0;
|
||||
|
||||
EXPECT(parse("char a", g, space_p).full);
|
||||
EXPECT(actual==Argument(Qualified("char",Qualified::BASIS),"a"));
|
||||
actual = arg0;
|
||||
|
||||
EXPECT(parse("unsigned char a", g, space_p).full);
|
||||
EXPECT(actual==Argument(Qualified("unsigned char",Qualified::BASIS),"a"));
|
||||
actual = arg0;
|
||||
|
||||
EXPECT(parse("Vector v", g, space_p).full);
|
||||
EXPECT(actual==Argument(Qualified("Vector",Qualified::EIGEN),"v"));
|
||||
actual = arg0;
|
||||
|
||||
EXPECT(parse("const Matrix& m", g, space_p).full);
|
||||
EXPECT(actual.type==Qualified("Matrix",Qualified::EIGEN));
|
||||
EXPECT(actual.name=="m");
|
||||
EXPECT(actual.is_const);
|
||||
EXPECT(actual.is_ref);
|
||||
EXPECT(!actual.is_ptr);
|
||||
actual = arg0;
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST( ArgumentList, grammar ) {
|
||||
|
||||
using classic::space_p;
|
||||
|
||||
// Create type grammar that will place result in actual
|
||||
ArgumentList actual;
|
||||
ArgumentListGrammar g(actual);
|
||||
|
||||
EXPECT(parse("(const gtsam::Point2& p4)", g, space_p).full);
|
||||
EXPECT_LONGS_EQUAL(1, actual.size());
|
||||
actual.clear();
|
||||
|
||||
EXPECT(parse("()", g, space_p).full);
|
||||
EXPECT_LONGS_EQUAL(0, actual.size());
|
||||
actual.clear();
|
||||
|
||||
EXPECT(parse("(char a)", g, space_p).full);
|
||||
EXPECT_LONGS_EQUAL(1, actual.size());
|
||||
actual.clear();
|
||||
|
||||
EXPECT(parse("(unsigned char a)", g, space_p).full);
|
||||
EXPECT_LONGS_EQUAL(1, actual.size());
|
||||
actual.clear();
|
||||
|
||||
EXPECT(parse("(Vector v, Matrix m)", g, space_p).full);
|
||||
EXPECT_LONGS_EQUAL(2, actual.size());
|
||||
EXPECT(actual[0]==Argument(Qualified("Vector",Qualified::EIGEN),"v"));
|
||||
EXPECT(actual[1]==Argument(Qualified("Matrix",Qualified::EIGEN),"m"));
|
||||
actual.clear();
|
||||
|
||||
EXPECT(parse("(Point2 p)", g, space_p).full);
|
||||
EXPECT_LONGS_EQUAL(1, actual.size());
|
||||
actual.clear();
|
||||
|
||||
EXPECT(parse("(Point2 p1, Point3 p2)", g, space_p).full);
|
||||
EXPECT_LONGS_EQUAL(2, actual.size());
|
||||
EXPECT(actual[0]==Argument(Qualified("Point2"),"p1"));
|
||||
EXPECT(actual[1]==Argument(Qualified("Point3"),"p2"));
|
||||
actual.clear();
|
||||
|
||||
EXPECT(parse("(gtsam::Point2 p3)", g, space_p).full);
|
||||
EXPECT_LONGS_EQUAL(1, actual.size());
|
||||
actual.clear();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in New Issue