diff --git a/.cproject b/.cproject index 5176792c2..7b5b769be 100644 --- a/.cproject +++ b/.cproject @@ -1,17 +1,19 @@ - + + + + + - - @@ -60,13 +62,13 @@ + + - - @@ -116,13 +118,13 @@ + + - - @@ -516,6 +518,22 @@ true true + + make + -j5 + SFMExampleExpressions.run + true + true + true + + + make + -j5 + Pose2SLAMExampleExpressions.run + true + true + true + make -j5 @@ -718,14 +736,6 @@ true true - - make - -j5 - testImuFactor.run - true - true - true - make -j5 @@ -782,7 +792,71 @@ true true - + + make + -j5 + testInertialNavFactor_GlobalVelocity.run + true + true + true + + + make + -j5 + testInvDepthFactorVariant3.run + true + true + true + + + make + -j5 + testInvDepthFactorVariant1.run + true + true + true + + + make + -j5 + testEquivInertialNavFactor_GlobalVel.run + true + true + true + + + make + -j5 + testInvDepthFactorVariant2.run + true + true + true + + + make + -j5 + testRelativeElevationFactor.run + true + true + true + + + make + -j5 + testPoseBetweenFactor.run + true + true + true + + + make + -j5 + testGaussMarkov1stOrderFactor.run + true + true + true + + make -j5 testGaussianFactorGraphUnordered.run @@ -878,6 +952,46 @@ true true + + make + -j5 + testGaussianBayesTree.run + true + true + true + + + make + -j5 + timeCameraExpression.run + true + true + true + + + make + -j5 + timeOneCameraExpression.run + true + true + true + + + make + -j5 + timeSFMExpressions.run + true + true + true + + + make + -j5 + timeAdaptAutoDiff.run + true + true + true + make -j5 @@ -2055,6 +2169,30 @@ true true + + make + -j5 + testRot2.run + true + true + true + + + make + -j5 + testRot3Q.run + true + true + true + + + make + -j5 + testRot3.run + true + true + true + make -j2 @@ -2151,6 +2289,22 @@ true true + + make + -j5 + testVelocityConstraint.run + true + true + true + + + make + -j5 + testVelocityConstraint3.run + true + true + true + make -j1 @@ -2231,6 +2385,38 @@ true true + + make + -j5 + testSpirit.run + true + true + true + + + make + -j5 + check.wrap + true + true + true + + + make + -j5 + testMethod.run + true + true + true + + + make + -j5 + testClass.run + true + true + true + make -j5 @@ -2311,6 +2497,22 @@ true true + + make + -j5 + testNumericalDerivative.run + true + true + true + + + make + -j5 + testVerticalBlockMatrix.run + true + true + true + make -j5 @@ -2524,6 +2726,14 @@ true true + + make + -j5 + testManifold.run + true + true + true + make -j5 @@ -2532,6 +2742,38 @@ true true + + make + -j5 + testExpressionFactor.run + true + true + true + + + make + -j5 + testExpression.run + true + true + true + + + make + -j5 + testExpressionMeta.run + true + true + true + + + make + -j5 + testAdaptAutoDiff.run + true + true + true + make -j2 @@ -2596,10 +2838,10 @@ true true - + make -j5 - testBetweenFactor.run + testPriorFactor.run true true true @@ -2652,18 +2894,10 @@ true true - + make -j5 - testGPSFactor.run - true - true - true - - - make - -j5 - testGaussMarkov1stOrderFactor.run + testPoseRotationPrior.run true true true @@ -3052,22 +3286,6 @@ true true - - make - -j5 - testSpirit.run - true - true - true - - - make - -j5 - check.wrap - true - true - true - make -j5 diff --git a/CMakeLists.txt b/CMakeLists.txt index bc7d8aecd..754279e53 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -9,8 +9,8 @@ if(NOT DEFINED CMAKE_MACOSX_RPATH) endif() # Set the version number for the library -set (GTSAM_VERSION_MAJOR 3) -set (GTSAM_VERSION_MINOR 1) +set (GTSAM_VERSION_MAJOR 4) +set (GTSAM_VERSION_MINOR 0) set (GTSAM_VERSION_PATCH 0) math (EXPR GTSAM_VERSION_NUMERIC "10000 * ${GTSAM_VERSION_MAJOR} + 100 * ${GTSAM_VERSION_MINOR} + ${GTSAM_VERSION_PATCH}") set (GTSAM_VERSION_STRING "${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH}") diff --git a/examples/CameraResectioning.cpp b/examples/CameraResectioning.cpp index 2048a84c8..f1e1a0010 100644 --- a/examples/CameraResectioning.cpp +++ b/examples/CameraResectioning.cpp @@ -48,7 +48,7 @@ public: virtual Vector evaluateError(const Pose3& pose, boost::optional H = boost::none) const { SimpleCamera camera(pose, *K_); - Point2 reprojectionError(camera.project(P_, H) - p_); + Point2 reprojectionError(camera.project(P_, H, boost::none, boost::none) - p_); return reprojectionError.vector(); } }; diff --git a/examples/SFMExample.cpp b/examples/SFMExample.cpp index 9fa4bd026..75d14cd72 100644 --- a/examples/SFMExample.cpp +++ b/examples/SFMExample.cpp @@ -85,8 +85,8 @@ int main(int argc, char* argv[]) { // Simulated measurements from each camera pose, adding them to the factor graph for (size_t i = 0; i < poses.size(); ++i) { + SimpleCamera camera(poses[i], *K); for (size_t j = 0; j < points.size(); ++j) { - SimpleCamera camera(poses[i], *K); Point2 measurement = camera.project(points[j]); graph.push_back(GenericProjectionFactor(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K)); } @@ -111,6 +111,8 @@ int main(int argc, char* argv[]) { /* Optimize the graph and print results */ Values result = DoglegOptimizer(graph, initialEstimate).optimize(); result.print("Final results:\n"); + cout << "initial error = " << graph.error(initialEstimate) << endl; + cout << "final error = " << graph.error(result) << endl; return 0; } diff --git a/gtsam.h b/gtsam.h index 4438bb363..acbbb1bc7 100644 --- a/gtsam.h +++ b/gtsam.h @@ -156,8 +156,14 @@ virtual class Value { size_t dim() const; }; +class Vector3 { + Vector3(Vector v); +}; +class Vector6 { + Vector6(Vector v); +}; #include -virtual class LieScalar : gtsam::Value { +class LieScalar { // Standard constructors LieScalar(); LieScalar(double d); @@ -186,7 +192,7 @@ virtual class LieScalar : gtsam::Value { }; #include -virtual class LieVector : gtsam::Value { +class LieVector { // Standard constructors LieVector(); LieVector(Vector v); @@ -218,7 +224,7 @@ virtual class LieVector : gtsam::Value { }; #include -virtual class LieMatrix : gtsam::Value { +class LieMatrix { // Standard constructors LieMatrix(); LieMatrix(Matrix v); @@ -253,7 +259,7 @@ virtual class LieMatrix : gtsam::Value { // geometry //************************************************************************* -virtual class Point2 : gtsam::Value { +class Point2 { // Standard Constructors Point2(); Point2(double x, double y); @@ -290,7 +296,7 @@ virtual class Point2 : gtsam::Value { void serialize() const; }; -virtual class StereoPoint2 : gtsam::Value { +class StereoPoint2 { // Standard Constructors StereoPoint2(); StereoPoint2(double uL, double uR, double v); @@ -325,7 +331,7 @@ virtual class StereoPoint2 : gtsam::Value { void serialize() const; }; -virtual class Point3 : gtsam::Value { +class Point3 { // Standard Constructors Point3(); Point3(double x, double y, double z); @@ -361,7 +367,7 @@ virtual class Point3 : gtsam::Value { void serialize() const; }; -virtual class Rot2 : gtsam::Value { +class Rot2 { // Standard Constructors and Named Constructors Rot2(); Rot2(double theta); @@ -406,7 +412,7 @@ virtual class Rot2 : gtsam::Value { void serialize() const; }; -virtual class Rot3 : gtsam::Value { +class Rot3 { // Standard Constructors and Named Constructors Rot3(); Rot3(Matrix R); @@ -462,7 +468,7 @@ virtual class Rot3 : gtsam::Value { void serialize() const; }; -virtual class Pose2 : gtsam::Value { +class Pose2 { // Standard Constructor Pose2(); Pose2(const gtsam::Pose2& pose); @@ -512,7 +518,7 @@ virtual class Pose2 : gtsam::Value { void serialize() const; }; -virtual class Pose3 : gtsam::Value { +class Pose3 { // Standard Constructors Pose3(); Pose3(const gtsam::Pose3& pose); @@ -564,7 +570,7 @@ virtual class Pose3 : gtsam::Value { }; #include -virtual class Unit3 : gtsam::Value { +class Unit3 { // Standard Constructors Unit3(); Unit3(const gtsam::Point3& pose); @@ -585,7 +591,7 @@ virtual class Unit3 : gtsam::Value { }; #include -virtual class EssentialMatrix : gtsam::Value { +class EssentialMatrix { // Standard Constructors EssentialMatrix(const gtsam::Rot3& aRb, const gtsam::Unit3& aTb); @@ -606,7 +612,7 @@ virtual class EssentialMatrix : gtsam::Value { double error(Vector vA, Vector vB); }; -virtual class Cal3_S2 : gtsam::Value { +class Cal3_S2 { // Standard Constructors Cal3_S2(); Cal3_S2(double fx, double fy, double s, double u0, double v0); @@ -643,7 +649,7 @@ virtual class Cal3_S2 : gtsam::Value { }; #include -virtual class Cal3DS2 : gtsam::Value { +class Cal3DS2 { // Standard Constructors Cal3DS2(); Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2, double k3, double k4); @@ -699,7 +705,43 @@ class Cal3_S2Stereo { double baseline() const; }; -virtual class CalibratedCamera : gtsam::Value { +#include +class Cal3Bundler { + // Standard Constructors + Cal3Bundler(); + Cal3Bundler(double fx, double k1, double k2, double u0, double v0); + + // Testable + void print(string s) const; + bool equals(const gtsam::Cal3Bundler& rhs, double tol) const; + + // Manifold + static size_t Dim(); + size_t dim() const; + gtsam::Cal3Bundler retract(Vector v) const; + Vector localCoordinates(const gtsam::Cal3Bundler& c) const; + + // Action on Point2 + gtsam::Point2 calibrate(const gtsam::Point2& p, double tol) const; + gtsam::Point2 calibrate(const gtsam::Point2& p) const; + gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; + + // Standard Interface + double fx() const; + double fy() const; + double k1() const; + double k2() const; + double u0() const; + double v0() const; + Vector vector() const; + Vector k() const; + //Matrix K() const; //FIXME: Uppercase + + // enabling serialization functionality + void serialize() const; +}; + +class CalibratedCamera { // Standard Constructors and Named Constructors CalibratedCamera(); CalibratedCamera(const gtsam::Pose3& pose); @@ -732,7 +774,7 @@ virtual class CalibratedCamera : gtsam::Value { void serialize() const; }; -virtual class SimpleCamera : gtsam::Value { +class SimpleCamera { // Standard Constructors and Named Constructors SimpleCamera(); SimpleCamera(const gtsam::Pose3& pose); @@ -771,7 +813,7 @@ virtual class SimpleCamera : gtsam::Value { }; template -virtual class PinholeCamera : gtsam::Value { +class PinholeCamera { // Standard Constructors and Named Constructors PinholeCamera(); PinholeCamera(const gtsam::Pose3& pose); @@ -809,7 +851,7 @@ virtual class PinholeCamera : gtsam::Value { void serialize() const; }; -virtual class StereoCamera : gtsam::Value { +class StereoCamera { // Standard Constructors and Named Constructors StereoCamera(); StereoCamera(const gtsam::Pose3& pose, const gtsam::Cal3_S2Stereo* K); @@ -862,7 +904,7 @@ virtual class SymbolicFactor { }; #include -class SymbolicFactorGraph { +virtual class SymbolicFactorGraph { SymbolicFactorGraph(); SymbolicFactorGraph(const gtsam::SymbolicBayesNet& bayesNet); SymbolicFactorGraph(const gtsam::SymbolicBayesTree& bayesTree); @@ -1664,15 +1706,12 @@ class Values { void print(string s) const; bool equals(const gtsam::Values& other, double tol) const; - void insert(size_t j, const gtsam::Value& value); void insert(const gtsam::Values& values); - void update(size_t j, const gtsam::Value& val); void update(const gtsam::Values& values); void erase(size_t j); void swap(gtsam::Values& values); bool exists(size_t j) const; - gtsam::Value at(size_t j) const; gtsam::KeyList keys() const; gtsam::VectorValues zeroVectors() const; @@ -1682,6 +1721,37 @@ class Values { // enabling serialization functionality void serialize() const; + + // New in 4.0, we have to specialize every insert/update/at to generate wrappers + // Instead of the old: + // void insert(size_t j, const gtsam::Value& value); + // void update(size_t j, const gtsam::Value& val); + // gtsam::Value at(size_t j) const; + void insert(size_t j, const gtsam::Point2& t); + void insert(size_t j, const gtsam::Point3& t); + void insert(size_t j, const gtsam::Rot2& t); + void insert(size_t j, const gtsam::Pose2& t); + void insert(size_t j, const gtsam::Rot3& t); + void insert(size_t j, const gtsam::Pose3& t); + void insert(size_t j, const gtsam::Cal3_S2& t); + void insert(size_t j, const gtsam::Cal3DS2& t); + void insert(size_t j, const gtsam::Cal3Bundler& t); + void insert(size_t j, const gtsam::EssentialMatrix& t); + + void update(size_t j, const gtsam::Point2& t); + void update(size_t j, const gtsam::Point3& t); + void update(size_t j, const gtsam::Rot2& t); + void update(size_t j, const gtsam::Pose2& t); + void update(size_t j, const gtsam::Rot3& t); + void update(size_t j, const gtsam::Pose3& t); + void update(size_t j, const gtsam::Cal3_S2& t); + void update(size_t j, const gtsam::Cal3DS2& t); + void update(size_t j, const gtsam::Cal3Bundler& t); + void update(size_t j, const gtsam::EssentialMatrix& t); + + template + T at(size_t j); }; // Actually a FastList @@ -2080,7 +2150,7 @@ class NonlinearISAM { #include #include -template +template virtual class PriorFactor : gtsam::NoiseModelFactor { PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); T prior() const; @@ -2091,7 +2161,7 @@ virtual class PriorFactor : gtsam::NoiseModelFactor { #include -template +template virtual class BetweenFactor : gtsam::NoiseModelFactor { BetweenFactor(size_t key1, size_t key2, const T& relativePose, const gtsam::noiseModel::Base* noiseModel); T measured() const; @@ -2103,7 +2173,7 @@ virtual class BetweenFactor : gtsam::NoiseModelFactor { #include -template +template virtual class NonlinearEquality : gtsam::NoiseModelFactor { // Constructor - forces exact evaluation NonlinearEquality(size_t j, const T& feasible); @@ -2284,7 +2354,7 @@ void writeG2o(const gtsam::NonlinearFactorGraph& graph, namespace imuBias { #include -virtual class ConstantBias : gtsam::Value { +class ConstantBias { // Standard Constructor ConstantBias(); ConstantBias(Vector biasAcc, Vector biasGyro); @@ -2351,7 +2421,7 @@ virtual class ImuFactor : gtsam::NonlinearFactor { // Standard Interface gtsam::ImuFactorPreintegratedMeasurements preintegratedMeasurements() const; - void predict(const gtsam::Pose3& pose_i, const gtsam::LieVector& vel_i, gtsam::Pose3& pose_j, gtsam::LieVector& vel_j, + void Predict(const gtsam::Pose3& pose_i, const gtsam::Vector3& vel_i, gtsam::Pose3& pose_j, gtsam::Vector3& vel_j, const gtsam::imuBias::ConstantBias& bias, const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis) const; @@ -2440,9 +2510,7 @@ virtual class CombinedImuFactor : gtsam::NonlinearFactor { // Standard Interface gtsam::CombinedImuFactorPreintegratedMeasurements preintegratedMeasurements() const; - - - static void predict(const gtsam::Pose3& pose_i, const gtsam::LieVector& vel_i, gtsam::Pose3& pose_j, gtsam::LieVector& vel_j, + void Predict(const gtsam::Pose3& pose_i, const gtsam::Vector3& vel_i, gtsam::Pose3& pose_j, gtsam::Vector3& vel_j, const gtsam::imuBias::ConstantBias& bias_i, const gtsam::imuBias::ConstantBias& bias_j, const gtsam::CombinedImuFactorPreintegratedMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis); diff --git a/gtsam/3rdparty/CCOLAMD/Lib/ccolamd.o b/gtsam/3rdparty/CCOLAMD/Lib/ccolamd.o deleted file mode 100644 index 0e2eddee2..000000000 Binary files a/gtsam/3rdparty/CCOLAMD/Lib/ccolamd.o and /dev/null differ diff --git a/gtsam/3rdparty/CCOLAMD/Lib/ccolamd_global.o b/gtsam/3rdparty/CCOLAMD/Lib/ccolamd_global.o deleted file mode 100644 index ea1e61b09..000000000 Binary files a/gtsam/3rdparty/CCOLAMD/Lib/ccolamd_global.o and /dev/null differ diff --git a/gtsam/3rdparty/CCOLAMD/Lib/ccolamd_l.o b/gtsam/3rdparty/CCOLAMD/Lib/ccolamd_l.o deleted file mode 100644 index 213b1cc0c..000000000 Binary files a/gtsam/3rdparty/CCOLAMD/Lib/ccolamd_l.o and /dev/null differ diff --git a/gtsam/3rdparty/gtsam_eigen_includes.h.in b/gtsam/3rdparty/gtsam_eigen_includes.h.in index fa5a51c70..f53e37f07 100644 --- a/gtsam/3rdparty/gtsam_eigen_includes.h.in +++ b/gtsam/3rdparty/gtsam_eigen_includes.h.in @@ -27,3 +27,7 @@ #include <@GTSAM_EIGEN_INCLUDE_PREFIX@Eigen/LU> #include <@GTSAM_EIGEN_INCLUDE_PREFIX@Eigen/SVD> #include <@GTSAM_EIGEN_INCLUDE_PREFIX@Eigen/Geometry> + + + + diff --git a/gtsam/base/ChartValue.h b/gtsam/base/ChartValue.h new file mode 100644 index 000000000..3cc6a041c --- /dev/null +++ b/gtsam/base/ChartValue.h @@ -0,0 +1,221 @@ +/* ---------------------------------------------------------------------------- + + * 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 ChartValue.h + * @brief + * @date October, 2014 + * @author Michael Bosse, Abel Gawel, Renaud Dube + * based on DerivedValue.h by Duy Nguyen Ta + */ + +#pragma once + +#include +#include +#include + +////////////////// +// The following includes windows.h in some MSVC versions, so we undef min, max, and ERROR +#include + +#ifdef min +#undef min +#endif + +#ifdef max +#undef max +#endif + +#ifdef ERROR +#undef ERROR +#endif +////////////////// + +namespace gtsam { + +/** + * ChartValue is derived from GenericValue and Chart so that + * Chart can be zero sized (as in DefaultChart) + * if the Chart is a member variable then it won't ever be zero sized. + */ +template > +class ChartValue: public GenericValue, public Chart_ { + + BOOST_CONCEPT_ASSERT((ChartConcept)); + +public: + + typedef T type; + typedef Chart_ Chart; + +public: + + /// Default Constructor. TODO might not make sense for some types + ChartValue() : + GenericValue(T()) { + } + + /// Construct froma value + ChartValue(const T& value) : + GenericValue(value) { + } + + /// Construct from a value and initialize the chart + template + ChartValue(const T& value, C chart_initializer) : + GenericValue(value), Chart(chart_initializer) { + } + + /// Destructor + virtual ~ChartValue() { + } + + /** + * Create a duplicate object returned as a pointer to the generic Value interface. + * For the sake of performance, this function use singleton pool allocator instead of the normal heap allocator. + * The result must be deleted with Value::deallocate_, not with the 'delete' operator. + */ + virtual Value* clone_() const { + void *place = boost::singleton_pool::malloc(); + ChartValue* ptr = new (place) ChartValue(*this); // calls copy constructor to fill in + return ptr; + } + + /** + * Destroy and deallocate this object, only if it was originally allocated using clone_(). + */ + virtual void deallocate_() const { + this->~ChartValue(); // Virtual destructor cleans up the derived object + boost::singleton_pool::free((void*) this); // Release memory from pool + } + + /** + * Clone this value (normal clone on the heap, delete with 'delete' operator) + */ + virtual boost::shared_ptr clone() const { + return boost::make_shared(*this); + } + + /// Chart Value interface version of retract + virtual Value* retract_(const Vector& delta) const { + // Call retract on the derived class using the retract trait function + const T retractResult = Chart::retract(GenericValue::value(), delta); + + // Create a Value pointer copy of the result + void* resultAsValuePlace = + boost::singleton_pool::malloc(); + Value* resultAsValue = new (resultAsValuePlace) ChartValue(retractResult, + static_cast(*this)); + + // Return the pointer to the Value base class + return resultAsValue; + } + + /// Generic Value interface version of localCoordinates + virtual Vector localCoordinates_(const Value& value2) const { + // Cast the base class Value pointer to a templated generic class pointer + const GenericValue& genericValue2 = + static_cast&>(value2); + + // Return the result of calling localCoordinates trait on the derived class + return Chart::local(GenericValue::value(), genericValue2.value()); + } + + /// Non-virtual version of retract + ChartValue retract(const Vector& delta) const { + return ChartValue(Chart::retract(GenericValue::value(), delta), + static_cast(*this)); + } + + /// Non-virtual version of localCoordinates + Vector localCoordinates(const ChartValue& value2) const { + return localCoordinates_(value2); + } + + /// Return run-time dimensionality + virtual size_t dim() const { + // need functional form here since the dimension may be dynamic + return Chart::getDimension(GenericValue::value()); + } + + /// Assignment operator + virtual Value& operator=(const Value& rhs) { + // Cast the base class Value pointer to a derived class pointer + const ChartValue& derivedRhs = static_cast(rhs); + + // Do the assignment and return the result + *this = ChartValue(derivedRhs); // calls copy constructor + return *this; + } + +protected: + + // implicit assignment operator for (const ChartValue& rhs) works fine here + /// Assignment operator, protected because only the Value or DERIVED + /// assignment operators should be used. + // DerivedValue& operator=(const DerivedValue& rhs) { + // // Nothing to do, do not call base class assignment operator + // return *this; + // } + +private: + + /// Fake Tag struct for singleton pool allocator. In fact, it is never used! + struct PoolTag { + }; + +private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + // ar & boost::serialization::make_nvp("value",); + // todo: implement a serialization for charts + ar + & boost::serialization::make_nvp("GenericValue", + boost::serialization::base_object >(*this)); + } + +}; + +// Define +namespace traits { + +/// The dimension of a ChartValue is the dimension of the chart +template +struct dimension > : public dimension { + // TODO Frank thinks dimension is a property of type, chart should conform +}; + +} // \ traits + +/// Get the chart from a Value +template +const Chart& Value::getChart() const { + return dynamic_cast(*this); +} + +/// Convenience function that can be used to make an expression to convert a value to a chart +template +ChartValue convertToChartValue(const T& value, + boost::optional< + Eigen::Matrix::value, + traits::dimension::value>&> H = boost::none) { + if (H) { + *H = Eigen::Matrix::value, + traits::dimension::value>::Identity(); + } + return ChartValue(value); +} + +} /* namespace gtsam */ diff --git a/gtsam/base/GenericValue.h b/gtsam/base/GenericValue.h new file mode 100644 index 000000000..6c109675d --- /dev/null +++ b/gtsam/base/GenericValue.h @@ -0,0 +1,168 @@ +/* ---------------------------------------------------------------------------- + + * 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 GenericValue.h + * @brief Wraps any type T so it can play as a Value + * @date October, 2014 + * @author Michael Bosse, Abel Gawel, Renaud Dube + * based on DerivedValue.h by Duy Nguyen Ta + */ + +#pragma once + +#include +#include +#include +#include + +namespace gtsam { + +// To play as a GenericValue, we need the following traits +namespace traits { + +// trait to wrap the default equals of types +template +struct equals { + typedef T type; + typedef bool result_type; + bool operator()(const T& a, const T& b, double tol) { + return a.equals(b, tol); + } +}; + +// trait to wrap the default print of types +template +struct print { + typedef T type; + typedef void result_type; + void operator()(const T& obj, const std::string& str) { + obj.print(str); + } +}; + +// equals for scalars +template<> +struct equals { + typedef double type; + typedef bool result_type; + bool operator()(double a, double b, double tol) { + return std::abs(a - b) <= tol; + } +}; + +// print for scalars +template<> +struct print { + typedef double type; + typedef void result_type; + void operator()(double a, const std::string& str) { + std::cout << str << ": " << a << std::endl; + } +}; + +// equals for Matrix types +template +struct equals > { + typedef Eigen::Matrix type; + typedef bool result_type; + bool operator()(const type& A, const type& B, double tol) { + return equal_with_abs_tol(A, B, tol); + } +}; + +// print for Matrix types +template +struct print > { + typedef Eigen::Matrix type; + typedef void result_type; + void operator()(const type& A, const std::string& str) { + std::cout << str << ": " << A << std::endl; + } +}; + +} + +/** + * Wraps any type T so it can play as a Value + */ +template +class GenericValue: public Value { + +public: + + typedef T type; + +protected: + + T value_; ///< The wrapped value + +public: + + /// Construct from value + GenericValue(const T& value) : + value_(value) { + } + + /// Return a constant value + const T& value() const { + return value_; + } + + /// Return the value + T& value() { + return value_; + } + + /// Destructor + virtual ~GenericValue() { + } + + /// equals implementing generic Value interface + virtual bool equals_(const Value& p, double tol = 1e-9) const { + // Cast the base class Value pointer to a templated generic class pointer + const GenericValue& genericValue2 = static_cast(p); + // Return the result of using the equals traits for the derived class + return traits::equals()(this->value_, genericValue2.value_, tol); + } + + /// non virtual equals function, uses traits + bool equals(const GenericValue &other, double tol = 1e-9) const { + return traits::equals()(this->value(), other.value(), tol); + } + + /// Virtual print function, uses traits + virtual void print(const std::string& str) const { + traits::print()(value_, str); + } + + // Serialization below: + + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Value); + ar & BOOST_SERIALIZATION_NVP(value_); + } + +protected: + + // Assignment operator for this class not needed since GenericValue is an abstract class + +}; + +// define Value::cast here since now GenericValue has been declared +template +const ValueType& Value::cast() const { + return dynamic_cast&>(*this).value(); +} + +} /* namespace gtsam */ diff --git a/gtsam/base/LieMatrix.h b/gtsam/base/LieMatrix.h index ec2dec815..f1fc69e71 100644 --- a/gtsam/base/LieMatrix.h +++ b/gtsam/base/LieMatrix.h @@ -29,7 +29,7 @@ namespace gtsam { /** * LieVector is a wrapper around vector to allow it to be a Lie type */ -struct LieMatrix : public Matrix, public DerivedValue { +struct LieMatrix : public Matrix { /// @name Constructors /// @{ @@ -166,12 +166,24 @@ private: friend class boost::serialization::access; template void serialize(Archive & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("LieMatrix", - boost::serialization::base_object(*this)); ar & boost::serialization::make_nvp("Matrix", boost::serialization::base_object(*this)); } }; + +// Define GTSAM traits +namespace traits { + +template<> +struct is_manifold : public boost::true_type { +}; + +template<> +struct dimension : public Dynamic { +}; + +} + } // \namespace gtsam diff --git a/gtsam/base/LieScalar.h b/gtsam/base/LieScalar.h index 21a2e10ad..1d4aa86e2 100644 --- a/gtsam/base/LieScalar.h +++ b/gtsam/base/LieScalar.h @@ -26,7 +26,7 @@ namespace gtsam { /** * LieScalar is a wrapper around double to allow it to be a Lie type */ - struct GTSAM_EXPORT LieScalar : public DerivedValue { + struct GTSAM_EXPORT LieScalar { /** default constructor */ LieScalar() : d_(0.0) {} @@ -111,4 +111,22 @@ namespace gtsam { private: double d_; }; + + // Define GTSAM traits + namespace traits { + + template<> + struct is_group : public boost::true_type { + }; + + template<> + struct is_manifold : public boost::true_type { + }; + + template<> + struct dimension : public boost::integral_constant { + }; + + } + } // \namespace gtsam diff --git a/gtsam/base/LieVector.h b/gtsam/base/LieVector.h index a8bfe3007..99b351ff5 100644 --- a/gtsam/base/LieVector.h +++ b/gtsam/base/LieVector.h @@ -26,7 +26,7 @@ namespace gtsam { /** * LieVector is a wrapper around vector to allow it to be a Lie type */ -struct LieVector : public Vector, public DerivedValue { +struct LieVector : public Vector { /** default constructor - should be unnecessary */ LieVector() {} @@ -123,11 +123,22 @@ private: friend class boost::serialization::access; template void serialize(Archive & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("LieVector", - boost::serialization::base_object(*this)); ar & boost::serialization::make_nvp("Vector", boost::serialization::base_object(*this)); } - }; + +// Define GTSAM traits +namespace traits { + +template<> +struct is_manifold : public boost::true_type { +}; + +template<> +struct dimension : public Dynamic { +}; + +} + } // \namespace gtsam diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h index ceebf6bad..a5a3d5f34 100644 --- a/gtsam/base/Manifold.h +++ b/gtsam/base/Manifold.h @@ -13,25 +13,19 @@ * @file Manifold.h * @brief Base class and basic functions for Manifold types * @author Alex Cunningham + * @author Frank Dellaert */ #pragma once -#include #include +#include +#include +#include namespace gtsam { /** - * Concept check class for Manifold types - * Requires a mapping between a linear tangent space and the underlying - * manifold, of which Lie is a specialization. - * - * The necessary functions to implement for Manifold are defined - * below with additional details as to the interface. The - * concept checking function in class Manifold will check whether or not - * the function exists and throw compile-time errors. - * * A manifold defines a space in which there is a notion of a linear tangent space * that can be centered around a given point on the manifold. These nonlinear * spaces may have such properties as wrapping around (as is the case with rotations), @@ -45,7 +39,256 @@ namespace gtsam { * There may be multiple possible retractions for a given manifold, which can be chosen * between depending on the computational complexity. The important criteria for * the creation for the retract and localCoordinates functions is that they be - * inverse operations. + * inverse operations. The new notion of a Chart guarantees that. + * + */ + +// Traits, same style as Boost.TypeTraits +// All meta-functions below ever only declare a single type +// or a type/value/value_type +namespace traits { + +// is group, by default this is false +template +struct is_group: public boost::false_type { +}; + +// identity, no default provided, by default given by default constructor +template +struct identity { + static T value() { + return T(); + } +}; + +// is manifold, by default this is false +template +struct is_manifold: public boost::false_type { +}; + +// dimension, can return Eigen::Dynamic (-1) if not known at compile time +// defaults to dynamic, TODO makes sense ? +typedef boost::integral_constant Dynamic; +template +struct dimension: public Dynamic { +}; + + +/** + * zero::value is intended to be the origin of a canonical coordinate system + * with canonical(t) == DefaultChart::local(zero::value, t) + * Below we provide the group identity as zero *in case* it is a group + */ +template struct zero: public identity { + BOOST_STATIC_ASSERT(is_group::value); +}; + +// double + +template<> +struct is_group : public boost::true_type { +}; + +template<> +struct is_manifold : public boost::true_type { +}; + +template<> +struct dimension : public boost::integral_constant { +}; + +template<> +struct zero { + static double value() { + return 0; + } +}; + +// Fixed size Eigen::Matrix type + +template +struct is_group > : public boost::true_type { +}; + +template +struct is_manifold > : public boost::true_type { +}; + +template +struct dimension > : public boost::integral_constant { + //TODO after switch to c++11 : the above should should be extracted to a constexpr function + // for readability and to reduce code duplication +}; + +template +struct zero > { + BOOST_STATIC_ASSERT_MSG((M!=Eigen::Dynamic && N!=Eigen::Dynamic), + "traits::zero is only supported for fixed-size matrices"); + static Eigen::Matrix value() { + return Eigen::Matrix::Zero(); + } +}; + +template +struct identity > : public zero > { +}; + + +template struct is_chart: public boost::false_type { +}; + +} // \ namespace traits + +// Chart is a map from T -> vector, retract is its inverse +template +struct DefaultChart { + //BOOST_STATIC_ASSERT(traits::is_manifold::value); + typedef T type; + typedef Eigen::Matrix::value, 1> vector; + + static vector local(const T& origin, const T& other) { + return origin.localCoordinates(other); + } + static T retract(const T& origin, const vector& d) { + return origin.retract(d); + } + static int getDimension(const T& origin) { + return origin.dim(); + } +}; + +namespace traits { +// populate default traits +template struct is_chart > : public boost::true_type { +}; +template struct dimension > : public dimension { +}; +} + +template +struct ChartConcept { +public: + typedef typename C::type type; + typedef typename C::vector vector; + + BOOST_CONCEPT_USAGE(ChartConcept) { + // is_chart trait should be true + BOOST_STATIC_ASSERT((traits::is_chart::value)); + + /** + * Returns Retraction update of val_ + */ + type retract_ret = C::retract(val_, vec_); + + /** + * Returns local coordinates of another object + */ + vec_ = C::local(val_, retract_ret); + + // a way to get the dimension that is compatible with dynamically sized types + dim_ = C::getDimension(val_); + } + +private: + type val_; + vector vec_; + int dim_; +}; + +/** + * CanonicalChart > is a chart around zero::value + * Canonical is CanonicalChart > + * An example is Canonical + */ +template struct CanonicalChart { + BOOST_CONCEPT_ASSERT((ChartConcept)); + + typedef C Chart; + typedef typename Chart::type type; + typedef typename Chart::vector vector; + + // Convert t of type T into canonical coordinates + vector local(const type& t) { + return Chart::local(traits::zero::value(), t); + } + // Convert back from canonical coordinates to T + type retract(const vector& v) { + return Chart::retract(traits::zero::value(), v); + } +}; +template struct Canonical: public CanonicalChart > { +}; + +// double + +template<> +struct DefaultChart { + typedef double type; + typedef Eigen::Matrix vector; + + static vector local(double origin, double other) { + vector d; + d << other - origin; + return d; + } + static double retract(double origin, const vector& d) { + return origin + d[0]; + } + static const int getDimension(double) { + return 1; + } +}; + +// Fixed size Eigen::Matrix type + +template +struct DefaultChart > { + /** + * This chart for the vector space of M x N matrices (represented by Eigen matrices) chooses as basis the one with respect to which the coordinates are exactly the matrix entries as laid out in memory (as determined by Options). + * Computing coordinates for a matrix is then simply a reshape to the row vector of appropriate size. + */ + typedef Eigen::Matrix type; + typedef type T; + typedef Eigen::Matrix::value, 1> vector;BOOST_STATIC_ASSERT_MSG((M!=Eigen::Dynamic && N!=Eigen::Dynamic), + "DefaultChart has not been implemented yet for dynamically sized matrices"); + static vector local(const T& origin, const T& other) { + return reshape(other) - reshape(origin); + } + static T retract(const T& origin, const vector& d) { + return origin + reshape(d); + } + static int getDimension(const T&origin) { + return origin.rows() * origin.cols(); + } +}; + +// Dynamically sized Vector +template<> +struct DefaultChart { + typedef Vector T; + typedef T type; + typedef T vector; + static vector local(const T& origin, const T& other) { + return other - origin; + } + static T retract(const T& origin, const vector& d) { + return origin + d; + } + static int getDimension(const T& origin) { + return origin.size(); + } +}; + +/** + * Old Concept check class for Manifold types + * Requires a mapping between a linear tangent space and the underlying + * manifold, of which Lie is a specialization. + * + * The necessary functions to implement for Manifold are defined + * below with additional details as to the interface. The + * concept checking function in class Manifold will check whether or not + * the function exists and throw compile-time errors. * * Returns dimensionality of the tangent space, which may be smaller than the number * of nonlinear parameters. @@ -61,7 +304,7 @@ namespace gtsam { * By convention, we use capital letters to designate a static function * @tparam T is a Lie type, like Point2, Pose3, etc. */ -template +template class ManifoldConcept { private: /** concept checking function - implement the functions this demands */ @@ -89,7 +332,7 @@ private: } }; -} // namespace gtsam +} // \ namespace gtsam /** * Macros for using the ManifoldConcept diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp index dd17c00ef..1469e265d 100644 --- a/gtsam/base/Matrix.cpp +++ b/gtsam/base/Matrix.cpp @@ -667,7 +667,7 @@ Matrix expm(const Matrix& A, size_t K) { /* ************************************************************************* */ Matrix Cayley(const Matrix& A) { - size_t n = A.cols(); + Matrix::Index n = A.cols(); assert(A.rows() == n); // original diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index 5ded8ddfc..132bf79ad 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -37,10 +37,28 @@ namespace gtsam { typedef Eigen::MatrixXd Matrix; typedef Eigen::Matrix MatrixRowMajor; +typedef Eigen::Matrix2d Matrix2; typedef Eigen::Matrix3d Matrix3; typedef Eigen::Matrix4d Matrix4; +typedef Eigen::Matrix Matrix5; typedef Eigen::Matrix Matrix6; +typedef Eigen::Matrix Matrix23; +typedef Eigen::Matrix Matrix24; +typedef Eigen::Matrix Matrix25; +typedef Eigen::Matrix Matrix26; +typedef Eigen::Matrix Matrix27; +typedef Eigen::Matrix Matrix28; +typedef Eigen::Matrix Matrix29; + +typedef Eigen::Matrix Matrix32; +typedef Eigen::Matrix Matrix34; +typedef Eigen::Matrix Matrix35; +typedef Eigen::Matrix Matrix36; +typedef Eigen::Matrix Matrix37; +typedef Eigen::Matrix Matrix38; +typedef Eigen::Matrix Matrix39; + // Matrix expressions for accessing parts of matrices typedef Eigen::Block SubMatrix; typedef Eigen::Block ConstSubMatrix; @@ -275,6 +293,49 @@ void zeroBelowDiagonal(MATRIX& A, size_t cols=0) { */ inline Matrix trans(const Matrix& A) { return A.transpose(); } +/// Reshape functor +template +struct Reshape { + //TODO replace this with Eigen's reshape function as soon as available. (There is a PR already pending : https://bitbucket.org/eigen/eigen/pull-request/41/reshape/diff) + typedef Eigen::Map > ReshapedType; + static inline ReshapedType reshape(const Eigen::Matrix & in) { + return in.data(); + } +}; + +/// Reshape specialization that does nothing as shape stays the same (needed to not be ambiguous for square input equals square output) +template +struct Reshape { + typedef const Eigen::Matrix & ReshapedType; + static inline ReshapedType reshape(const Eigen::Matrix & in) { + return in; + } +}; + +/// Reshape specialization that does nothing as shape stays the same +template +struct Reshape { + typedef const Eigen::Matrix & ReshapedType; + static inline ReshapedType reshape(const Eigen::Matrix & in) { + return in; + } +}; + +/// Reshape specialization that does transpose +template +struct Reshape { + typedef typename Eigen::Matrix::ConstTransposeReturnType ReshapedType; + static inline ReshapedType reshape(const Eigen::Matrix & in) { + return in.transpose(); + } +}; + +template +inline typename Reshape::ReshapedType reshape(const Eigen::Matrix & m){ + BOOST_STATIC_ASSERT(InM * InN == OutM * OutN); + return Reshape::reshape(m); +} + /** * solve AX=B via in-place Lu factorization and backsubstitution * After calling, A contains LU, B the solved RHS vectors diff --git a/gtsam/base/Value.h b/gtsam/base/Value.h index ab23e47fa..afc244d6c 100644 --- a/gtsam/base/Value.h +++ b/gtsam/base/Value.h @@ -120,7 +120,17 @@ namespace gtsam { virtual Vector localCoordinates_(const Value& value) const = 0; /** Assignment operator */ - virtual Value& operator=(const Value& rhs) = 0; + virtual Value& operator=(const Value& rhs) { + //needs a empty definition so recursion in implicit derived assignment operators work + return *this; + } + + /** Cast to known ValueType */ + template + const ValueType& cast() const; + + template + const Chart& getChart() const; /** Virutal destructor */ virtual ~Value() {} diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index 72d502550..1be8408d2 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -36,7 +36,12 @@ typedef Eigen::VectorXd Vector; // Commonly used fixed size vectors typedef Eigen::Vector2d Vector2; typedef Eigen::Vector3d Vector3; +typedef Eigen::Matrix Vector4; +typedef Eigen::Matrix Vector5; typedef Eigen::Matrix Vector6; +typedef Eigen::Matrix Vector7; +typedef Eigen::Matrix Vector8; +typedef Eigen::Matrix Vector9; typedef Eigen::VectorBlock SubVector; typedef Eigen::VectorBlock ConstSubVector; diff --git a/gtsam/base/VerticalBlockMatrix.h b/gtsam/base/VerticalBlockMatrix.h index 029f55c58..b075d73b3 100644 --- a/gtsam/base/VerticalBlockMatrix.h +++ b/gtsam/base/VerticalBlockMatrix.h @@ -65,36 +65,39 @@ namespace gtsam { /** Construct from a container of the sizes of each vertical block. */ template - VerticalBlockMatrix(const CONTAINER& dimensions, DenseIndex height, bool appendOneDimension = false) : - rowStart_(0), rowEnd_(height), blockStart_(0) - { + VerticalBlockMatrix(const CONTAINER& dimensions, DenseIndex height, + bool appendOneDimension = false) : + variableColOffsets_(dimensions.size() + (appendOneDimension ? 2 : 1)), + rowStart_(0), rowEnd_(height), blockStart_(0) { fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension); matrix_.resize(height, variableColOffsets_.back()); assertInvariants(); } /** Construct from a container of the sizes of each vertical block and a pre-prepared matrix. */ - template - VerticalBlockMatrix(const CONTAINER& dimensions, const Matrix& matrix, bool appendOneDimension = false) : - matrix_(matrix), rowStart_(0), rowEnd_(matrix.rows()), blockStart_(0) - { + template + VerticalBlockMatrix(const CONTAINER& dimensions, + const Eigen::MatrixBase& matrix, bool appendOneDimension = false) : + matrix_(matrix), variableColOffsets_(dimensions.size() + (appendOneDimension ? 2 : 1)), + rowStart_(0), rowEnd_(matrix.rows()), blockStart_(0) { fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension); - if(variableColOffsets_.back() != matrix_.cols()) - throw std::invalid_argument("Requested to create a VerticalBlockMatrix with dimensions that do not sum to the total columns of the provided matrix."); + if (variableColOffsets_.back() != matrix_.cols()) + throw std::invalid_argument( + "Requested to create a VerticalBlockMatrix with dimensions that do not sum to the total columns of the provided matrix."); assertInvariants(); } - /** - * Construct from iterator over the sizes of each vertical block. */ + /** Construct from iterator over the sizes of each vertical block. */ template - VerticalBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim, DenseIndex height, bool appendOneDimension = false) : - rowStart_(0), rowEnd_(height), blockStart_(0) - { + VerticalBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim, + DenseIndex height, bool appendOneDimension = false) : + variableColOffsets_((lastBlockDim-firstBlockDim) + (appendOneDimension ? 2 : 1)), + rowStart_(0), rowEnd_(height), blockStart_(0) { fillOffsets(firstBlockDim, lastBlockDim, appendOneDimension); matrix_.resize(height, variableColOffsets_.back()); assertInvariants(); } - + /** Copy the block structure and resize the underlying matrix, but do not copy the matrix data. * If blockStart(), rowStart(), and/or rowEnd() have been modified, this copies the structure of * the corresponding matrix view. In the destination VerticalBlockView, blockStart() and @@ -203,18 +206,12 @@ namespace gtsam { template void fillOffsets(ITERATOR firstBlockDim, ITERATOR lastBlockDim, bool appendOneDimension) { - variableColOffsets_.resize((lastBlockDim-firstBlockDim) + 1 + (appendOneDimension ? 1 : 0)); variableColOffsets_[0] = 0; DenseIndex j=0; - for(ITERATOR dim=firstBlockDim; dim!=lastBlockDim; ++dim) { + for(ITERATOR dim=firstBlockDim; dim!=lastBlockDim; ++dim, ++j) variableColOffsets_[j+1] = variableColOffsets_[j] + *dim; - ++ j; - } if(appendOneDimension) - { variableColOffsets_[j+1] = variableColOffsets_[j] + 1; - ++ j; - } } friend class SymmetricBlockMatrix; diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h index 036f23f68..9339c9c7b 100644 --- a/gtsam/base/numericalDerivative.h +++ b/gtsam/base/numericalDerivative.h @@ -16,7 +16,6 @@ */ // \callgraph - #pragma once #include @@ -29,597 +28,492 @@ #pragma GCC diagnostic pop #endif -#include #include +#include namespace gtsam { - /* - * Note that all of these functions have two versions, a boost.function version and a - * standard C++ function pointer version. This allows reformulating the arguments of - * a function to fit the correct structure, which is useful for situations like - * member functions and functions with arguments not involved in the derivative: - * - * Usage of the boost bind version to rearrange arguments: - * for a function with one relevant param and an optional derivative: - * Foo bar(const Obj& a, boost::optional H1) - * Use boost.bind to restructure: - * boost::bind(bar, _1, boost::none) - * This syntax will fix the optional argument to boost::none, while using the first argument provided - * - * For member functions, such as below, with an instantiated copy instanceOfSomeClass - * Foo SomeClass::bar(const Obj& a) - * Use boost bind as follows to create a function pointer that uses the member function: - * boost::bind(&SomeClass::bar, ref(instanceOfSomeClass), _1) - * - * For additional details, see the documentation: - * http://www.boost.org/doc/libs/release/libs/bind/bind.html - */ +/* + * Note that all of these functions have two versions, a boost.function version and a + * standard C++ function pointer version. This allows reformulating the arguments of + * a function to fit the correct structure, which is useful for situations like + * member functions and functions with arguments not involved in the derivative: + * + * Usage of the boost bind version to rearrange arguments: + * for a function with one relevant param and an optional derivative: + * Foo bar(const Obj& a, boost::optional H1) + * Use boost.bind to restructure: + * boost::bind(bar, _1, boost::none) + * This syntax will fix the optional argument to boost::none, while using the first argument provided + * + * For member functions, such as below, with an instantiated copy instanceOfSomeClass + * Foo SomeClass::bar(const Obj& a) + * Use boost bind as follows to create a function pointer that uses the member function: + * boost::bind(&SomeClass::bar, ref(instanceOfSomeClass), _1) + * + * For additional details, see the documentation: + * http://www.boost.org/doc/libs/release/libs/bind/bind.html + */ +/** + * Numerically compute gradient of scalar function + * Class X is the input argument + * The class X needs to have dim, expmap, logmap + */ +template +Vector numericalGradient(boost::function h, const X& x, + double delta = 1e-5) { + double factor = 1.0 / (2.0 * delta); - /** global functions for converting to a LieVector for use with numericalDerivative */ - inline LieVector makeLieVector(const Vector& v) { return LieVector(v); } - inline LieVector makeLieVectorD(double d) { return LieVector((Vector) (Vector(1) << d)); } + BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + "Template argument X must be a manifold type."); + static const int N = traits::dimension::value; + BOOST_STATIC_ASSERT_MSG(N>0, "Template argument X must be fixed-size type."); + typedef DefaultChart ChartX; + typedef typename ChartX::vector TangentX; - /** - * Numerically compute gradient of scalar function - * Class X is the input argument - * The class X needs to have dim, expmap, logmap - */ - template - Vector numericalGradient(boost::function h, const X& x, double delta=1e-5) { - double factor = 1.0/(2.0*delta); - const size_t n = x.dim(); - Vector d = zero(n), g = zero(n); - for (size_t j=0;j - Vector numericalGradient(double (*h)(const X&), const X& x, double delta=1e-5) { - return numericalGradient(boost::bind(h, _1), x, delta); - } - - /** - * Compute numerical derivative in argument 1 of unary function - * @param h unary function yielding m-vector - * @param x n-dimensional value at which to evaluate h - * @param delta increment for numerical derivative - * Class Y is the output argument - * Class X is the input argument - * @return m*n Jacobian computed via central differencing - * Both classes X,Y need dim, expmap, logmap - */ - template - Matrix numericalDerivative11(boost::function h, const X& x, double delta=1e-5) { - Y hx = h(x); - double factor = 1.0/(2.0*delta); - const size_t m = hx.dim(), n = x.dim(); - Vector d = zero(n); - Matrix H = zeros(m,n); - for (size_t j=0;j - Matrix numericalDerivative11(Y (*h)(const X&), const X& x, double delta=1e-5) { - return numericalDerivative11(boost::bind(h, _1), x, delta); - } - -// /** remapping for double valued functions */ -// template -// Matrix numericalDerivative11(boost::function h, const X& x, double delta=1e-5) { -// return numericalDerivative11(boost::bind(makeLieVectorD, boost::bind(h, _1)), x, delta); -// } - - template - Matrix numericalDerivative11(double (*h)(const X&), const X& x, double delta=1e-5) { - return numericalDerivative11(boost::bind(makeLieVectorD, boost::bind(h, _1)), x, delta); - } - - /** remapping for vector valued functions */ - template - Matrix numericalDerivative11(boost::function h, const X& x, double delta=1e-5) { - return numericalDerivative11(boost::bind(makeLieVector, boost::bind(h, _1)), x, delta); - } - - template - Matrix numericalDerivative11(Vector (*h)(const X&), const X& x, double delta=1e-5) { - return numericalDerivative11(boost::bind(makeLieVector, boost::bind(h, _1)), x, delta); - } - - /** - * Compute numerical derivative in argument 1 of binary function - * @param h binary function yielding m-vector - * @param x1 n-dimensional first argument value - * @param x2 second argument value - * @param delta increment for numerical derivative - * @return m*n Jacobian computed via central differencing - * All classes Y,X1,X2 need dim, expmap, logmap - */ - template - Matrix numericalDerivative21(boost::function h, - const X1& x1, const X2& x2, double delta=1e-5) { - Y hx = h(x1,x2); - double factor = 1.0/(2.0*delta); - const size_t m = hx.dim(), n = x1.dim(); - Vector d = zero(n); - Matrix H = zeros(m,n); - for (size_t j=0;j - inline Matrix numericalDerivative21(Y (*h)(const X1&, const X2&), - const X1& x1, const X2& x2, double delta=1e-5) { - return numericalDerivative21(boost::bind(h, _1, _2), x1, x2, delta); - } - - /** pseudo-partial template specialization for double return values */ - template - Matrix numericalDerivative21(boost::function h, - const X1& x1, const X2& x2, double delta=1e-5) { - return numericalDerivative21( - boost::bind(makeLieVectorD, boost::bind(h, _1, _2)), x1, x2, delta); - } - - template - Matrix numericalDerivative21(double (*h)(const X1&, const X2&), - const X1& x1, const X2& x2, double delta=1e-5) { - return numericalDerivative21( - boost::bind(makeLieVectorD, boost::bind(h, _1, _2)), x1, x2, delta); - } - - /** pseudo-partial template specialization for vector return values */ - template - Matrix numericalDerivative21(boost::function h, - const X1& x1, const X2& x2, double delta=1e-5) { - return numericalDerivative21( - boost::bind(makeLieVector, boost::bind(h, _1, _2)), x1, x2, delta); - } - - template - inline Matrix numericalDerivative21(Vector (*h)(const X1&, const X2&), - const X1& x1, const X2& x2, double delta=1e-5) { - return numericalDerivative21( - boost::bind(makeLieVector, boost::bind(h, _1, _2)), x1, x2, delta); - } - - /** - * Compute numerical derivative in argument 2 of binary function - * @param h binary function yielding m-vector - * @param x1 first argument value - * @param x2 n-dimensional second argument value - * @param delta increment for numerical derivative - * @return m*n Jacobian computed via central differencing - * All classes Y,X1,X2 need dim, expmap, logmap - */ - template - Matrix numericalDerivative22 - (boost::function h, - const X1& x1, const X2& x2, double delta=1e-5) { - Y hx = h(x1,x2); - double factor = 1.0/(2.0*delta); - const size_t m = hx.dim(), n = x2.dim(); - Vector d = zero(n); - Matrix H = zeros(m,n); - for (size_t j=0;j - inline Matrix numericalDerivative22 - (Y (*h)(const X1&, const X2&), const X1& x1, const X2& x2, double delta=1e-5) { - return numericalDerivative22(boost::bind(h, _1, _2), x1, x2, delta); - } - - /** pseudo-partial template specialization for double return values */ - template - Matrix numericalDerivative22(boost::function h, - const X1& x1, const X2& x2, double delta=1e-5) { - return numericalDerivative22( - boost::bind(makeLieVectorD, boost::bind(h, _1, _2)), x1, x2, delta); - } - - template - inline Matrix numericalDerivative22(double (*h)(const X1&, const X2&), - const X1& x1, const X2& x2, double delta=1e-5) { - return numericalDerivative22( - boost::bind(makeLieVectorD, boost::bind(h, _1, _2)), x1, x2, delta); - } - - /** pseudo-partial template specialization for vector return values */ - template - Matrix numericalDerivative22(boost::function h, - const X1& x1, const X2& x2, double delta=1e-5) { - return numericalDerivative22( - boost::bind(makeLieVector, boost::bind(h, _1, _2)), x1, x2, delta); - } - - template - inline Matrix numericalDerivative22(Vector (*h)(const X1&, const X2&), - const X1& x1, const X2& x2, double delta=1e-5) { - return numericalDerivative22( - boost::bind(makeLieVector, boost::bind(h, _1, _2)), x1, x2, delta); - } - - /** - * Compute numerical derivative in argument 1 of ternary function - * @param h ternary function yielding m-vector - * @param x1 n-dimensional first argument value - * @param x2 second argument value - * @param x3 third argument value - * @param delta increment for numerical derivative - * @return m*n Jacobian computed via central differencing - * All classes Y,X1,X2,X3 need dim, expmap, logmap - */ - template - Matrix numericalDerivative31 - (boost::function h, - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) - { - Y hx = h(x1,x2,x3); - double factor = 1.0/(2.0*delta); - const size_t m = hx.dim(), n = x1.dim(); - Vector d = zero(n); - Matrix H = zeros(m,n); - for (size_t j=0;j - inline Matrix numericalDerivative31 - (Y (*h)(const X1&, const X2&, const X3&), - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative31(boost::bind(h, _1, _2, _3), x1, x2, x3, delta); - } - - /** pseudo-partial template specialization for double return values */ - template - Matrix numericalDerivative31(boost::function h, - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative31( - boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); - } - - template - inline Matrix numericalDerivative31(double (*h)(const X1&, const X2&, const X3&), - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative31( - boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); - } - - /** pseudo-partial template specialization for vector return values */ - template - Matrix numericalDerivative31(boost::function h, - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative31( - boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); - } - - template - inline Matrix numericalDerivative31(Vector (*h)(const X1&, const X2&, const X3&), - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative31( - boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); - } - - /** - * Compute numerical derivative in argument 2 of ternary function - * @param h ternary function yielding m-vector - * @param x1 n-dimensional first argument value - * @param x2 second argument value - * @param x3 third argument value - * @param delta increment for numerical derivative - * @return m*n Jacobian computed via central differencing - * All classes Y,X1,X2,X3 need dim, expmap, logmap - */ - template - Matrix numericalDerivative32 - (boost::function h, - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) - { - Y hx = h(x1,x2,x3); - double factor = 1.0/(2.0*delta); - const size_t m = hx.dim(), n = x2.dim(); - Vector d = zero(n); - Matrix H = zeros(m,n); - for (size_t j=0;j - inline Matrix numericalDerivative32 - (Y (*h)(const X1&, const X2&, const X3&), - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative32(boost::bind(h, _1, _2, _3), x1, x2, x3, delta); - } - - /** pseudo-partial template specialization for double return values */ - template - Matrix numericalDerivative32(boost::function h, - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative32( - boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); - } - - template - inline Matrix numericalDerivative32(double (*h)(const X1&, const X2&, const X3&), - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative32( - boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); - } - - /** pseudo-partial template specialization for vector return values */ - template - Matrix numericalDerivative32(boost::function h, - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative32( - boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); - } - - template - inline Matrix numericalDerivative32(Vector (*h)(const X1&, const X2&, const X3&), - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative32( - boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); - } - - /** - * Compute numerical derivative in argument 3 of ternary function - * @param h ternary function yielding m-vector - * @param x1 n-dimensional first argument value - * @param x2 second argument value - * @param x3 third argument value - * @param delta increment for numerical derivative - * @return m*n Jacobian computed via central differencing - * All classes Y,X1,X2,X3 need dim, expmap, logmap - */ - template - Matrix numericalDerivative33 - (boost::function h, - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) - { - Y hx = h(x1,x2,x3); - double factor = 1.0/(2.0*delta); - const size_t m = hx.dim(), n = x3.dim(); - Vector d = zero(n); - Matrix H = zeros(m,n); - for (size_t j=0;j - inline Matrix numericalDerivative33 - (Y (*h)(const X1&, const X2&, const X3&), - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative33(boost::bind(h, _1, _2, _3), x1, x2, x3, delta); - } - - /** pseudo-partial template specialization for double return values */ - template - Matrix numericalDerivative33(boost::function h, - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative33( - boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); - } - - template - inline Matrix numericalDerivative33(double (*h)(const X1&, const X2&, const X3&), - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative33( - boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); - } - - /** pseudo-partial template specialization for vector return values */ - template - Matrix numericalDerivative33(boost::function h, - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative33( - boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); - } - - template - inline Matrix numericalDerivative33(Vector (*h)(const X1&, const X2&, const X3&), - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative33( - boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); - } - - /** - * Compute numerical Hessian matrix. Requires a single-argument Lie->scalar - * function. This is implemented simply as the derivative of the gradient. - * @param f A function taking a Lie object as input and returning a scalar - * @param x The center point for computing the Hessian - * @param delta The numerical derivative step size - * @return n*n Hessian matrix computed via central differencing - */ - template - inline Matrix numericalHessian(boost::function f, const X& x, double delta=1e-5) { - return numericalDerivative11(boost::function(boost::bind( - static_cast,const X&, double)>(&numericalGradient), - f, _1, delta)), x, delta); - } - - template - inline Matrix numericalHessian(double (*f)(const X&), const X& x, double delta=1e-5) { - return numericalHessian(boost::function(f), x, delta); - } - - - /** Helper class that computes the derivative of f w.r.t. x1, centered about - * x1_, as a function of x2 - */ - template - class G_x1 { - const boost::function& f_; - const X1& x1_; - double delta_; - public: - G_x1(const boost::function& f, const X1& x1, double delta) : f_(f), x1_(x1), delta_(delta) {} - Vector operator()(const X2& x2) { - return numericalGradient(boost::function(boost::bind(f_, _1, x2)), x1_, delta_); - } - }; - - template - inline Matrix numericalHessian212(boost::function f, const X1& x1, const X2& x2, double delta=1e-5) { - G_x1 g_x1(f, x1, delta); - return numericalDerivative11(boost::function(boost::bind(boost::ref(g_x1), _1)), x2, delta); - } - - - template - inline Matrix numericalHessian212(double (*f)(const X1&, const X2&), const X1& x1, const X2& x2, double delta=1e-5) { - return numericalHessian212(boost::function(f), x1, x2, delta); - } - - - template - inline Matrix numericalHessian211(boost::function f, const X1& x1, const X2& x2, double delta=1e-5) { - - Vector (*numGrad)(boost::function, const X1&, double) = &numericalGradient; - boost::function f2(boost::bind(f, _1, x2)); - - return numericalDerivative11(boost::function(boost::bind(numGrad, f2, _1, delta)), x1, delta); - } - - - template - inline Matrix numericalHessian211(double (*f)(const X1&, const X2&), const X1& x1, const X2& x2, double delta=1e-5) { - return numericalHessian211(boost::function(f), x1, x2, delta); - } - - - template - inline Matrix numericalHessian222(boost::function f, const X1& x1, const X2& x2, double delta=1e-5) { - - Vector (*numGrad)(boost::function, const X2&, double) = &numericalGradient; - boost::function f2(boost::bind(f, x1, _1)); - - return numericalDerivative11(boost::function(boost::bind(numGrad, f2, _1, delta)), x2, delta); - } - - - template - inline Matrix numericalHessian222(double (*f)(const X1&, const X2&), const X1& x1, const X2& x2, double delta=1e-5) { - return numericalHessian222(boost::function(f), x1, x2, delta); - } - - /** - * Numerical Hessian for tenary functions - */ - /* **************************************************************** */ - template - inline Matrix numericalHessian311(boost::function f, - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - - Vector (*numGrad)(boost::function, const X1&, double) = &numericalGradient; - boost::function f2(boost::bind(f, _1, x2, x3)); - - return numericalDerivative11(boost::function(boost::bind(numGrad, f2, _1, delta)), x1, delta); - } - - template - inline Matrix numericalHessian311(double (*f)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalHessian311(boost::function(f), x1, x2, x3, delta); - } - - /* **************************************************************** */ - template - inline Matrix numericalHessian322(boost::function f, - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - - Vector (*numGrad)(boost::function, const X2&, double) = &numericalGradient; - boost::function f2(boost::bind(f, x1, _1, x3)); - - return numericalDerivative11(boost::function(boost::bind(numGrad, f2, _1, delta)), x2, delta); - } - - template - inline Matrix numericalHessian322(double (*f)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalHessian322(boost::function(f), x1, x2, x3, delta); - } - - /* **************************************************************** */ - template - inline Matrix numericalHessian333(boost::function f, - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - - Vector (*numGrad)(boost::function, const X3&, double) = &numericalGradient; - boost::function f2(boost::bind(f, x1, x2, _1)); - - return numericalDerivative11(boost::function(boost::bind(numGrad, f2, _1, delta)), x3, delta); - } - - template - inline Matrix numericalHessian333(double (*f)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalHessian333(boost::function(f), x1, x2, x3, delta); - } - - /* **************************************************************** */ - template - inline Matrix numericalHessian312(boost::function f, const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalHessian212(boost::function(boost::bind(f, _1, _2, x3)), x1, x2, delta ); - } - - template - inline Matrix numericalHessian313(boost::function f, const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalHessian212(boost::function(boost::bind(f, _1, x2, _2)), x1, x3, delta ); - } - - template - inline Matrix numericalHessian323(boost::function f, const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalHessian212(boost::function(boost::bind(f, x1, _1, _2)), x2, x3, delta ); - } - - /* **************************************************************** */ - template - inline Matrix numericalHessian312(double (*f)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalHessian312(boost::function(f), x1, x2, x3, delta); - } - - template - inline Matrix numericalHessian313(double (*f)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalHessian313(boost::function(f), x1, x2, x3, delta); - } - - template - inline Matrix numericalHessian323(double (*f)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalHessian323(boost::function(f), x1, x2, x3, delta); + // get chart at x + ChartX chartX; + + // Prepare a tangent vector to perturb x with, only works for fixed size + TangentX d; + d.setZero(); + + Vector g = zero(N); // Can be fixed size + for (int j = 0; j < N; j++) { + d(j) = delta; + double hxplus = h(chartX.retract(x, d)); + d(j) = -delta; + double hxmin = h(chartX.retract(x, d)); + d(j) = 0; + g(j) = (hxplus - hxmin) * factor; } + return g; +} + +/** + * @brief New-style numerical derivatives using manifold_traits + * @brief Computes numerical derivative in argument 1 of unary function + * @param h unary function yielding m-vector + * @param x n-dimensional value at which to evaluate h + * @param delta increment for numerical derivative + * Class Y is the output argument + * Class X is the input argument + * @return m*n Jacobian computed via central differencing + */ +template +// TODO Should compute fixed-size matrix +Matrix numericalDerivative11(boost::function h, const X& x, + double delta = 1e-5) { + using namespace traits; + + BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + "Template argument Y must be a manifold type."); + typedef DefaultChart ChartY; + typedef typename ChartY::vector TangentY; + + BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + "Template argument X must be a manifold type."); + static const int N = traits::dimension::value; + BOOST_STATIC_ASSERT_MSG(N>0, "Template argument X must be fixed-size type."); + typedef DefaultChart ChartX; + typedef typename ChartX::vector TangentX; + + // get value at x, and corresponding chart + Y hx = h(x); + ChartY chartY; + + // Bit of a hack for now to find number of rows + TangentY zeroY = chartY.local(hx, hx); + size_t m = zeroY.size(); + + // get chart at x + ChartX chartX; + + // Prepare a tangent vector to perturb x with, only works for fixed size + TangentX dx; + dx.setZero(); + + // Fill in Jacobian H + Matrix H = zeros(m, N); + double factor = 1.0 / (2.0 * delta); + for (int j = 0; j < N; j++) { + dx(j) = delta; + TangentY dy1 = chartY.local(hx, h(chartX.retract(x, dx))); + dx(j) = -delta; + TangentY dy2 = chartY.local(hx, h(chartX.retract(x, dx))); + dx(j) = 0; + H.col(j) << (dy1 - dy2) * factor; + } + return H; +} + +/** use a raw C++ function pointer */ +template +Matrix numericalDerivative11(Y (*h)(const X&), const X& x, + double delta = 1e-5) { + return numericalDerivative11(boost::bind(h, _1), x, delta); +} + +/** + * Compute numerical derivative in argument 1 of binary function + * @param h binary function yielding m-vector + * @param x1 n-dimensional first argument value + * @param x2 second argument value + * @param delta increment for numerical derivative + * @return m*n Jacobian computed via central differencing + */ +template +Matrix numericalDerivative21(const boost::function& h, + const X1& x1, const X2& x2, double delta = 1e-5) { + BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + "Template argument Y must be a manifold type."); + BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + "Template argument X1 must be a manifold type."); + return numericalDerivative11(boost::bind(h, _1, x2), x1, delta); +} + +/** use a raw C++ function pointer */ +template +inline Matrix numericalDerivative21(Y (*h)(const X1&, const X2&), const X1& x1, + const X2& x2, double delta = 1e-5) { + return numericalDerivative21(boost::bind(h, _1, _2), x1, x2, delta); +} + +/** + * Compute numerical derivative in argument 2 of binary function + * @param h binary function yielding m-vector + * @param x1 first argument value + * @param x2 n-dimensional second argument value + * @param delta increment for numerical derivative + * @return m*n Jacobian computed via central differencing + */ +template +Matrix numericalDerivative22(boost::function h, + const X1& x1, const X2& x2, double delta = 1e-5) { + BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + "Template argument Y must be a manifold type."); + BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + "Template argument X2 must be a manifold type."); + return numericalDerivative11(boost::bind(h, x1, _1), x2, delta); +} + +/** use a raw C++ function pointer */ +template +inline Matrix numericalDerivative22(Y (*h)(const X1&, const X2&), const X1& x1, + const X2& x2, double delta = 1e-5) { + return numericalDerivative22(boost::bind(h, _1, _2), x1, x2, delta); +} + +/** + * Compute numerical derivative in argument 1 of ternary function + * @param h ternary function yielding m-vector + * @param x1 n-dimensional first argument value + * @param x2 second argument value + * @param x3 third argument value + * @param delta increment for numerical derivative + * @return m*n Jacobian computed via central differencing + * All classes Y,X1,X2,X3 need dim, expmap, logmap + */ +template +Matrix numericalDerivative31( + boost::function h, const X1& x1, + const X2& x2, const X3& x3, double delta = 1e-5) { + BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + "Template argument Y must be a manifold type."); + BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + "Template argument X1 must be a manifold type."); + return numericalDerivative11(boost::bind(h, _1, x2, x3), x1, delta); +} + +template +inline Matrix numericalDerivative31(Y (*h)(const X1&, const X2&, const X3&), + const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { + return numericalDerivative31(boost::bind(h, _1, _2, _3), x1, + x2, x3, delta); +} + +/** + * Compute numerical derivative in argument 2 of ternary function + * @param h ternary function yielding m-vector + * @param x1 n-dimensional first argument value + * @param x2 second argument value + * @param x3 third argument value + * @param delta increment for numerical derivative + * @return m*n Jacobian computed via central differencing + * All classes Y,X1,X2,X3 need dim, expmap, logmap + */ +template +Matrix numericalDerivative32( + boost::function h, const X1& x1, + const X2& x2, const X3& x3, double delta = 1e-5) { + BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + "Template argument Y must be a manifold type."); + BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + "Template argument X2 must be a manifold type."); + return numericalDerivative11(boost::bind(h, x1, _1, x3), x2, delta); +} + +template +inline Matrix numericalDerivative32(Y (*h)(const X1&, const X2&, const X3&), + const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { + return numericalDerivative32(boost::bind(h, _1, _2, _3), x1, + x2, x3, delta); +} + +/** + * Compute numerical derivative in argument 3 of ternary function + * @param h ternary function yielding m-vector + * @param x1 n-dimensional first argument value + * @param x2 second argument value + * @param x3 third argument value + * @param delta increment for numerical derivative + * @return m*n Jacobian computed via central differencing + * All classes Y,X1,X2,X3 need dim, expmap, logmap + */ +template +Matrix numericalDerivative33( + boost::function h, const X1& x1, + const X2& x2, const X3& x3, double delta = 1e-5) { + BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + "Template argument Y must be a manifold type."); + BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + "Template argument X3 must be a manifold type."); + return numericalDerivative11(boost::bind(h, x1, x2, _1), x3, delta); +} + +template +inline Matrix numericalDerivative33(Y (*h)(const X1&, const X2&, const X3&), + const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { + return numericalDerivative33(boost::bind(h, _1, _2, _3), x1, + x2, x3, delta); +} + +/** + * Compute numerical Hessian matrix. Requires a single-argument Lie->scalar + * function. This is implemented simply as the derivative of the gradient. + * @param f A function taking a Lie object as input and returning a scalar + * @param x The center point for computing the Hessian + * @param delta The numerical derivative step size + * @return n*n Hessian matrix computed via central differencing + */ +template +inline Matrix numericalHessian(boost::function f, const X& x, + double delta = 1e-5) { + BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + "Template argument X must be a manifold type."); + typedef boost::function F; + typedef boost::function G; + G ng = static_cast(numericalGradient ); + return numericalDerivative11(boost::bind(ng, f, _1, delta), x, + delta); +} + +template +inline Matrix numericalHessian(double (*f)(const X&), const X& x, double delta = + 1e-5) { + return numericalHessian(boost::function(f), x, delta); +} + +/** Helper class that computes the derivative of f w.r.t. x1, centered about + * x1_, as a function of x2 + */ +template +class G_x1 { + const boost::function& f_; + X1 x1_; + double delta_; +public: + G_x1(const boost::function& f, const X1& x1, + double delta) : + f_(f), x1_(x1), delta_(delta) { + } + Vector operator()(const X2& x2) { + return numericalGradient(boost::bind(f_, _1, x2), x1_, delta_); + } +}; + +template +inline Matrix numericalHessian212( + boost::function f, const X1& x1, const X2& x2, + double delta = 1e-5) { + G_x1 g_x1(f, x1, delta); + return numericalDerivative11( + boost::function( + boost::bind(boost::ref(g_x1), _1)), x2, delta); +} + +template +inline Matrix numericalHessian212(double (*f)(const X1&, const X2&), + const X1& x1, const X2& x2, double delta = 1e-5) { + return numericalHessian212(boost::function(f), + x1, x2, delta); +} + +template +inline Matrix numericalHessian211( + boost::function f, const X1& x1, const X2& x2, + double delta = 1e-5) { + + Vector (*numGrad)(boost::function, const X1&, + double) = &numericalGradient; + boost::function f2(boost::bind(f, _1, x2)); + + return numericalDerivative11( + boost::function(boost::bind(numGrad, f2, _1, delta)), + x1, delta); +} + +template +inline Matrix numericalHessian211(double (*f)(const X1&, const X2&), + const X1& x1, const X2& x2, double delta = 1e-5) { + return numericalHessian211(boost::function(f), + x1, x2, delta); +} + +template +inline Matrix numericalHessian222( + boost::function f, const X1& x1, const X2& x2, + double delta = 1e-5) { + + Vector (*numGrad)(boost::function, const X2&, + double) = &numericalGradient; + boost::function f2(boost::bind(f, x1, _1)); + + return numericalDerivative11( + boost::function(boost::bind(numGrad, f2, _1, delta)), + x2, delta); +} + +template +inline Matrix numericalHessian222(double (*f)(const X1&, const X2&), + const X1& x1, const X2& x2, double delta = 1e-5) { + return numericalHessian222(boost::function(f), + x1, x2, delta); +} + +/** + * Numerical Hessian for tenary functions + */ +/* **************************************************************** */ +template +inline Matrix numericalHessian311( + boost::function f, const X1& x1, + const X2& x2, const X3& x3, double delta = 1e-5) { + + Vector (*numGrad)(boost::function, const X1&, + double) = &numericalGradient; + boost::function f2(boost::bind(f, _1, x2, x3)); + + return numericalDerivative11( + boost::function(boost::bind(numGrad, f2, _1, delta)), + x1, delta); +} + +template +inline Matrix numericalHessian311(double (*f)(const X1&, const X2&, const X3&), + const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { + return numericalHessian311( + boost::function(f), x1, x2, x3, + delta); +} + +/* **************************************************************** */ +template +inline Matrix numericalHessian322( + boost::function f, const X1& x1, + const X2& x2, const X3& x3, double delta = 1e-5) { + + Vector (*numGrad)(boost::function, const X2&, + double) = &numericalGradient; + boost::function f2(boost::bind(f, x1, _1, x3)); + + return numericalDerivative11( + boost::function(boost::bind(numGrad, f2, _1, delta)), + x2, delta); +} + +template +inline Matrix numericalHessian322(double (*f)(const X1&, const X2&, const X3&), + const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { + return numericalHessian322( + boost::function(f), x1, x2, x3, + delta); +} + +/* **************************************************************** */ +template +inline Matrix numericalHessian333( + boost::function f, const X1& x1, + const X2& x2, const X3& x3, double delta = 1e-5) { + + Vector (*numGrad)(boost::function, const X3&, + double) = &numericalGradient; + boost::function f2(boost::bind(f, x1, x2, _1)); + + return numericalDerivative11( + boost::function(boost::bind(numGrad, f2, _1, delta)), + x3, delta); +} + +template +inline Matrix numericalHessian333(double (*f)(const X1&, const X2&, const X3&), + const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { + return numericalHessian333( + boost::function(f), x1, x2, x3, + delta); +} + +/* **************************************************************** */ +template +inline Matrix numericalHessian312( + boost::function f, const X1& x1, + const X2& x2, const X3& x3, double delta = 1e-5) { + return numericalHessian212( + boost::function(boost::bind(f, _1, _2, x3)), + x1, x2, delta); +} + +template +inline Matrix numericalHessian313( + boost::function f, const X1& x1, + const X2& x2, const X3& x3, double delta = 1e-5) { + return numericalHessian212( + boost::function(boost::bind(f, _1, x2, _2)), + x1, x3, delta); +} + +template +inline Matrix numericalHessian323( + boost::function f, const X1& x1, + const X2& x2, const X3& x3, double delta = 1e-5) { + return numericalHessian212( + boost::function(boost::bind(f, x1, _1, _2)), + x2, x3, delta); +} + +/* **************************************************************** */ +template +inline Matrix numericalHessian312(double (*f)(const X1&, const X2&, const X3&), + const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { + return numericalHessian312( + boost::function(f), x1, x2, x3, + delta); +} + +template +inline Matrix numericalHessian313(double (*f)(const X1&, const X2&, const X3&), + const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { + return numericalHessian313( + boost::function(f), x1, x2, x3, + delta); +} + +template +inline Matrix numericalHessian323(double (*f)(const X1&, const X2&, const X3&), + const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { + return numericalHessian323( + boost::function(f), x1, x2, x3, + delta); +} } diff --git a/gtsam/base/tests/testNumericalDerivative.cpp b/gtsam/base/tests/testNumericalDerivative.cpp index f7e4d3baa..b4a9b91d6 100644 --- a/gtsam/base/tests/testNumericalDerivative.cpp +++ b/gtsam/base/tests/testNumericalDerivative.cpp @@ -15,115 +15,123 @@ * @date Apr 8, 2011 */ +#include #include -#include - +using namespace std; using namespace gtsam; /* ************************************************************************* */ -double f(const LieVector& x) { +double f(const Vector2& x) { assert(x.size() == 2); return sin(x(0)) + cos(x(1)); } /* ************************************************************************* */ -TEST(testNumericalDerivative, numericalHessian) { - LieVector center = ones(2); +// +TEST(testNumericalDerivative, numericalGradient) { + Vector2 x(1, 1); - Matrix expected = (Matrix(2,2) << - -sin(center(0)), 0.0, - 0.0, -cos(center(1))); + Vector expected(2); + expected << cos(x(1)), -sin(x(0)); - Matrix actual = numericalHessian(f, center); + Vector actual = numericalGradient(f, x); EXPECT(assert_equal(expected, actual, 1e-5)); } /* ************************************************************************* */ -double f2(const LieVector& x) { +TEST(testNumericalDerivative, numericalHessian) { + Vector2 x(1, 1); + + Matrix expected(2, 2); + expected << -sin(x(0)), 0.0, 0.0, -cos(x(1)); + + Matrix actual = numericalHessian(f, x); + + EXPECT(assert_equal(expected, actual, 1e-5)); +} + +/* ************************************************************************* */ +double f2(const Vector2& x) { assert(x.size() == 2); return sin(x(0)) * cos(x(1)); } /* ************************************************************************* */ +// TEST(testNumericalDerivative, numericalHessian2) { - Vector v_center = (Vector(2) << 0.5, 1.0); - LieVector center(v_center); + Vector2 v(0.5, 1.0); + Vector2 x(v); - Matrix expected = (Matrix(2,2) << - -cos(center(1))*sin(center(0)), -sin(center(1))*cos(center(0)), - -cos(center(0))*sin(center(1)), -sin(center(0))*cos(center(1))); + Matrix expected = (Matrix(2, 2) << -cos(x(1)) * sin(x(0)), -sin(x(1)) + * cos(x(0)), -cos(x(0)) * sin(x(1)), -sin(x(0)) * cos(x(1))); - Matrix actual = numericalHessian(f2, center); + Matrix actual = numericalHessian(f2, x); EXPECT(assert_equal(expected, actual, 1e-5)); } /* ************************************************************************* */ -double f3(const LieVector& x1, const LieVector& x2) { - assert(x1.size() == 1 && x2.size() == 1); - return sin(x1(0)) * cos(x2(0)); +double f3(double x1, double x2) { + return sin(x1) * cos(x2); } /* ************************************************************************* */ +// TEST(testNumericalDerivative, numericalHessian211) { - Vector v_center1 = (Vector(1) << 1.0); - Vector v_center2 = (Vector(1) << 5.0); - LieVector center1(v_center1), center2(v_center2); + double x1 = 1, x2 = 5; - Matrix expected11 = (Matrix(1, 1) << -sin(center1(0))*cos(center2(0))); - Matrix actual11 = numericalHessian211(f3, center1, center2); + Matrix expected11 = (Matrix(1, 1) << -sin(x1) * cos(x2)); + Matrix actual11 = numericalHessian211(f3, x1, x2); EXPECT(assert_equal(expected11, actual11, 1e-5)); - Matrix expected12 = (Matrix(1, 1) <<-cos(center1(0))*sin(center2(0))); - Matrix actual12 = numericalHessian212(f3, center1, center2); + Matrix expected12 = (Matrix(1, 1) << -cos(x1) * sin(x2)); + Matrix actual12 = numericalHessian212(f3, x1, x2); EXPECT(assert_equal(expected12, actual12, 1e-5)); - Matrix expected22 = (Matrix(1, 1) <<-sin(center1(0))*cos(center2(0))); - Matrix actual22 = numericalHessian222(f3, center1, center2); + Matrix expected22 = (Matrix(1, 1) << -sin(x1) * cos(x2)); + Matrix actual22 = numericalHessian222(f3, x1, x2); EXPECT(assert_equal(expected22, actual22, 1e-5)); } /* ************************************************************************* */ -double f4(const LieVector& x, const LieVector& y, const LieVector& z) { - assert(x.size() == 1 && y.size() == 1 && z.size() == 1); - return sin(x(0)) * cos(y(0)) * z(0)*z(0); +double f4(double x, double y, double z) { + return sin(x) * cos(y) * z * z; } /* ************************************************************************* */ +// TEST(testNumericalDerivative, numericalHessian311) { - Vector v_center1 = (Vector(1) << 1.0); - Vector v_center2 = (Vector(1) << 2.0); - Vector v_center3 = (Vector(1) << 3.0); - LieVector center1(v_center1), center2(v_center2), center3(v_center3); - - double x = center1(0), y = center2(0), z = center3(0); - Matrix expected11 = (Matrix(1, 1) << -sin(x)*cos(y)*z*z); - Matrix actual11 = numericalHessian311(f4, center1, center2, center3); + double x = 1, y = 2, z = 3; + Matrix expected11 = (Matrix(1, 1) << -sin(x) * cos(y) * z * z); + Matrix actual11 = numericalHessian311(f4, x, y, z); EXPECT(assert_equal(expected11, actual11, 1e-5)); - Matrix expected12 = (Matrix(1, 1) << -cos(x)*sin(y)*z*z); - Matrix actual12 = numericalHessian312(f4, center1, center2, center3); + Matrix expected12 = (Matrix(1, 1) << -cos(x) * sin(y) * z * z); + Matrix actual12 = numericalHessian312(f4, x, y, z); EXPECT(assert_equal(expected12, actual12, 1e-5)); - Matrix expected13 = (Matrix(1, 1) << cos(x)*cos(y)*2*z); - Matrix actual13 = numericalHessian313(f4, center1, center2, center3); + Matrix expected13 = (Matrix(1, 1) << cos(x) * cos(y) * 2 * z); + Matrix actual13 = numericalHessian313(f4, x, y, z); EXPECT(assert_equal(expected13, actual13, 1e-5)); - Matrix expected22 = (Matrix(1, 1) << -sin(x)*cos(y)*z*z); - Matrix actual22 = numericalHessian322(f4, center1, center2, center3); + Matrix expected22 = (Matrix(1, 1) << -sin(x) * cos(y) * z * z); + Matrix actual22 = numericalHessian322(f4, x, y, z); EXPECT(assert_equal(expected22, actual22, 1e-5)); - Matrix expected23 = (Matrix(1, 1) << -sin(x)*sin(y)*2*z); - Matrix actual23 = numericalHessian323(f4, center1, center2, center3); + Matrix expected23 = (Matrix(1, 1) << -sin(x) * sin(y) * 2 * z); + Matrix actual23 = numericalHessian323(f4, x, y, z); EXPECT(assert_equal(expected23, actual23, 1e-5)); - Matrix expected33 = (Matrix(1, 1) << sin(x)*cos(y)*2); - Matrix actual33 = numericalHessian333(f4, center1, center2, center3); + Matrix expected33 = (Matrix(1, 1) << sin(x) * cos(y) * 2); + Matrix actual33 = numericalHessian333(f4, x, y, z); EXPECT(assert_equal(expected33, actual33, 1e-5)); } /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ diff --git a/gtsam/base/tests/testVerticalBlockMatrix.cpp b/gtsam/base/tests/testVerticalBlockMatrix.cpp index fad23fa7d..c504752aa 100644 --- a/gtsam/base/tests/testVerticalBlockMatrix.cpp +++ b/gtsam/base/tests/testVerticalBlockMatrix.cpp @@ -24,9 +24,20 @@ using namespace std; using namespace gtsam; using boost::assign::list_of; +list L = list_of(3)(2)(1); +vector dimensions(L.begin(),L.end()); + //***************************************************************************** -TEST(VerticalBlockMatrix, constructor) { - VerticalBlockMatrix actual(list_of(3)(2)(1), +TEST(VerticalBlockMatrix, Constructor1) { + VerticalBlockMatrix actual(dimensions,6); + EXPECT_LONGS_EQUAL(6,actual.rows()); + EXPECT_LONGS_EQUAL(6,actual.cols()); + EXPECT_LONGS_EQUAL(3,actual.nBlocks()); +} + +//***************************************************************************** +TEST(VerticalBlockMatrix, Constructor2) { + VerticalBlockMatrix actual(dimensions, (Matrix(6, 6) << 1, 2, 3, 4, 5, 6, // 2, 8, 9, 10, 11, 12, // 3, 9, 15, 16, 17, 18, // @@ -38,6 +49,14 @@ TEST(VerticalBlockMatrix, constructor) { EXPECT_LONGS_EQUAL(3,actual.nBlocks()); } +//***************************************************************************** +TEST(VerticalBlockMatrix, Constructor3) { + VerticalBlockMatrix actual(dimensions.begin(),dimensions.end(),6); + EXPECT_LONGS_EQUAL(6,actual.rows()); + EXPECT_LONGS_EQUAL(6,actual.cols()); + EXPECT_LONGS_EQUAL(3,actual.nBlocks()); +} + //***************************************************************************** int main() { TestResult tr; diff --git a/gtsam/geometry/Cal3Bundler.cpp b/gtsam/geometry/Cal3Bundler.cpp index 587d0ea63..ab5fde629 100644 --- a/gtsam/geometry/Cal3Bundler.cpp +++ b/gtsam/geometry/Cal3Bundler.cpp @@ -46,7 +46,7 @@ Vector Cal3Bundler::k() const { } /* ************************************************************************* */ -Vector Cal3Bundler::vector() const { +Vector3 Cal3Bundler::vector() const { return (Vector(3) << f_, k1_, k2_); } @@ -64,6 +64,45 @@ bool Cal3Bundler::equals(const Cal3Bundler& K, double tol) const { return true; } +/* ************************************************************************* */ +Point2 Cal3Bundler::uncalibrate(const Point2& p) const { + // r = x^2 + y^2; + // g = (1 + k(1)*r + k(2)*r^2); + // pi(:,i) = g * pn(:,i) + const double x = p.x(), y = p.y(); + const double r = x * x + y * y; + const double g = 1. + (k1_ + k2_ * r) * r; + const double u = g * x, v = g * y; + return Point2(u0_ + f_ * u, v0_ + f_ * v); +} + +/* ************************************************************************* */ +Point2 Cal3Bundler::uncalibrate(const Point2& p, // + boost::optional Dcal, boost::optional Dp) const { + // r = x^2 + y^2; + // g = (1 + k(1)*r + k(2)*r^2); + // pi(:,i) = g * pn(:,i) + const double x = p.x(), y = p.y(); + const double r = x * x + y * y; + const double g = 1. + (k1_ + k2_ * r) * r; + const double u = g * x, v = g * y; + + // Derivatives make use of intermediate variables above + if (Dcal) { + double rx = r * x, ry = r * y; + *Dcal << u, f_ * rx, f_ * r * rx, v, f_ * ry, f_ * r * ry; + } + + if (Dp) { + const double a = 2. * (k1_ + 2. * k2_ * r); + const double axx = a * x * x, axy = a * x * y, ayy = a * y * y; + *Dp << g + axx, axy, axy, g + ayy; + *Dp *= f_; + } + + return Point2(u0_ + f_ * u, v0_ + f_ * v); +} + /* ************************************************************************* */ Point2 Cal3Bundler::uncalibrate(const Point2& p, // boost::optional Dcal, boost::optional Dp) const { @@ -150,7 +189,7 @@ Cal3Bundler Cal3Bundler::retract(const Vector& d) const { } /* ************************************************************************* */ -Vector Cal3Bundler::localCoordinates(const Cal3Bundler& T2) const { +Vector3 Cal3Bundler::localCoordinates(const Cal3Bundler& T2) const { return T2.vector() - vector(); } diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h index 53529ba46..396659582 100644 --- a/gtsam/geometry/Cal3Bundler.h +++ b/gtsam/geometry/Cal3Bundler.h @@ -28,7 +28,7 @@ namespace gtsam { * @addtogroup geometry * \nosubgrouping */ -class GTSAM_EXPORT Cal3Bundler: public DerivedValue { +class GTSAM_EXPORT Cal3Bundler { private: double f_; ///< focal length @@ -72,7 +72,7 @@ public: Matrix K() const; ///< Standard 3*3 calibration matrix Vector k() const; ///< Radial distortion parameters (4 of them, 2 0) - Vector vector() const; + Vector3 vector() const; /// focal length x inline double fx() const { @@ -108,12 +108,29 @@ public: /** * convert intrinsic coordinates xy to image coordinates uv * @param p point in intrinsic coordinates + * @return point in image coordinates + */ + Point2 uncalibrate(const Point2& p) const; + + /** + * Version of uncalibrate with fixed derivatives + * @param p point in intrinsic coordinates * @param Dcal optional 2*3 Jacobian wrpt CalBundler parameters * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates * @return point in image coordinates */ - Point2 uncalibrate(const Point2& p, boost::optional Dcal = - boost::none, boost::optional Dp = boost::none) const; + Point2 uncalibrate(const Point2& p, boost::optional Dcal, + boost::optional Dp) const; + + /** + * Version of uncalibrate with dynamic derivatives + * @param p point in intrinsic coordinates + * @param Dcal optional 2*3 Jacobian wrpt CalBundler parameters + * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates + * @return point in image coordinates + */ + Point2 uncalibrate(const Point2& p, boost::optional Dcal, + boost::optional Dp) const; /// Conver a pixel coordinate to ideal coordinate Point2 calibrate(const Point2& pi, const double tol = 1e-5) const; @@ -135,7 +152,7 @@ public: Cal3Bundler retract(const Vector& d) const; /// Calculate local coordinates to another calibration - Vector localCoordinates(const Cal3Bundler& T2) const; + Vector3 localCoordinates(const Cal3Bundler& T2) const; /// dimensionality virtual size_t dim() const { @@ -157,9 +174,6 @@ private: friend class boost::serialization::access; template void serialize(Archive & ar, const unsigned int version) { - ar - & boost::serialization::make_nvp("Cal3Bundler", - boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(f_); ar & BOOST_SERIALIZATION_NVP(k1_); ar & BOOST_SERIALIZATION_NVP(k2_); @@ -169,6 +183,26 @@ private: /// @} - }; +}; - } // namespace gtsam +// Define GTSAM traits +namespace traits { + +template<> +struct is_manifold : public boost::true_type { +}; + +template<> +struct dimension : public boost::integral_constant { +}; + +template<> +struct zero { + static Cal3Bundler value() { + return Cal3Bundler(0, 0, 0); + } +}; + +} + +} // namespace gtsam diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h index 617fcf5ff..95524e115 100644 --- a/gtsam/geometry/Cal3DS2.h +++ b/gtsam/geometry/Cal3DS2.h @@ -37,7 +37,7 @@ namespace gtsam { * k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y ] * pi = K*pn */ -class GTSAM_EXPORT Cal3DS2 : public Cal3DS2_Base, public DerivedValue { +class GTSAM_EXPORT Cal3DS2 : public Cal3DS2_Base { typedef Cal3DS2_Base Base; @@ -87,6 +87,7 @@ public: /// Return dimensions of calibration manifold object static size_t Dim() { return 9; } //TODO: make a final dimension variable + private: /// @} @@ -98,8 +99,6 @@ private: template void serialize(Archive & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("Cal3DS2", - boost::serialization::base_object(*this)); ar & boost::serialization::make_nvp("Cal3DS2", boost::serialization::base_object(*this)); } @@ -108,5 +107,18 @@ private: }; +// Define GTSAM traits +namespace traits { + +template<> +struct is_manifold : public boost::true_type { +}; + +template<> +struct dimension : public boost::integral_constant { +}; + +} + } diff --git a/gtsam/geometry/Cal3DS2_Base.cpp b/gtsam/geometry/Cal3DS2_Base.cpp index b8181ab4d..e4397a449 100644 --- a/gtsam/geometry/Cal3DS2_Base.cpp +++ b/gtsam/geometry/Cal3DS2_Base.cpp @@ -53,23 +53,23 @@ bool Cal3DS2_Base::equals(const Cal3DS2_Base& K, double tol) const { } /* ************************************************************************* */ -static Eigen::Matrix D2dcalibration(double x, double y, double xx, +static Matrix29 D2dcalibration(double x, double y, double xx, double yy, double xy, double rr, double r4, double pnx, double pny, - const Eigen::Matrix& DK) { - Eigen::Matrix DR1; + const Matrix2& DK) { + Matrix25 DR1; DR1 << pnx, 0.0, pny, 1.0, 0.0, 0.0, pny, 0.0, 0.0, 1.0; - Eigen::Matrix DR2; + Matrix24 DR2; DR2 << x * rr, x * r4, 2 * xy, rr + 2 * xx, // y * rr, y * r4, rr + 2 * yy, 2 * xy; - Eigen::Matrix D; + Matrix29 D; D << DR1, DK * DR2; return D; } /* ************************************************************************* */ -static Eigen::Matrix D2dintrinsic(double x, double y, double rr, +static Matrix2 D2dintrinsic(double x, double y, double rr, double g, double k1, double k2, double p1, double p2, - const Eigen::Matrix& DK) { + const Matrix2& DK) { const double drdx = 2. * x; const double drdy = 2. * y; const double dgdx = k1 * drdx + k2 * 2. * rr * drdx; @@ -82,7 +82,7 @@ static Eigen::Matrix D2dintrinsic(double x, double y, double rr, const double dDydx = 2. * p2 * y + p1 * drdx; const double dDydy = 2. * p2 * x + p1 * (drdy + 4. * y); - Eigen::Matrix DR; + Matrix2 DR; DR << g + x * dgdx + dDxdx, x * dgdy + dDxdy, // y * dgdx + dDydx, g + y * dgdy + dDydy; @@ -90,8 +90,9 @@ static Eigen::Matrix D2dintrinsic(double x, double y, double rr, } /* ************************************************************************* */ -Point2 Cal3DS2_Base::uncalibrate(const Point2& p, boost::optional H1, - boost::optional H2) const { +Point2 Cal3DS2_Base::uncalibrate(const Point2& p, + boost::optional H1, + boost::optional H2) const { // rr = x^2 + y^2; // g = (1 + k(1)*rr + k(2)*rr^2); @@ -110,7 +111,7 @@ Point2 Cal3DS2_Base::uncalibrate(const Point2& p, boost::optional H1, const double pnx = g * x + dx; const double pny = g * y + dy; - Eigen::Matrix DK; + Matrix2 DK; if (H1 || H2) DK << fx_, s_, 0.0, fy_; // Derivative for calibration @@ -125,6 +126,26 @@ Point2 Cal3DS2_Base::uncalibrate(const Point2& p, boost::optional H1, return Point2(fx_ * pnx + s_ * pny + u0_, fy_ * pny + v0_); } +/* ************************************************************************* */ +Point2 Cal3DS2_Base::uncalibrate(const Point2& p, + boost::optional H1, + boost::optional H2) const { + + if (H1 || H2) { + Matrix29 D1; + Matrix2 D2; + Point2 pu = uncalibrate(p, D1, D2); + if (H1) + *H1 = D1; + if (H2) + *H2 = D2; + return pu; + + } else { + return uncalibrate(p); + } +} + /* ************************************************************************* */ Point2 Cal3DS2_Base::calibrate(const Point2& pi, const double tol) const { // Use the following fixed point iteration to invert the radial distortion. @@ -161,7 +182,7 @@ Matrix Cal3DS2_Base::D2d_intrinsic(const Point2& p) const { const double rr = xx + yy; const double r4 = rr * rr; const double g = (1 + k1_ * rr + k2_ * r4); - Eigen::Matrix DK; + Matrix2 DK; DK << fx_, s_, 0.0, fy_; return D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK); } @@ -176,7 +197,7 @@ Matrix Cal3DS2_Base::D2d_calibration(const Point2& p) const { const double dy = 2 * p2_ * xy + p1_ * (rr + 2 * yy); const double pnx = g * x + dx; const double pny = g * y + dy; - Eigen::Matrix DK; + Matrix2 DK; DK << fx_, s_, 0.0, fy_; return D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK); } diff --git a/gtsam/geometry/Cal3DS2_Base.h b/gtsam/geometry/Cal3DS2_Base.h index 7be5a6874..61c01e349 100644 --- a/gtsam/geometry/Cal3DS2_Base.h +++ b/gtsam/geometry/Cal3DS2_Base.h @@ -18,7 +18,6 @@ #pragma once -#include #include namespace gtsam { @@ -114,9 +113,14 @@ public: * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates * @return point in (distorted) image coordinates */ + Point2 uncalibrate(const Point2& p, - boost::optional Dcal = boost::none, - boost::optional Dp = boost::none) const ; + boost::optional Dcal = boost::none, + boost::optional Dp = boost::none) const ; + + Point2 uncalibrate(const Point2& p, + boost::optional Dcal, + boost::optional Dp) const ; /// Convert (distorted) image coordinates uv to intrinsic coordinates xy Point2 calibrate(const Point2& p, const double tol=1e-5) const; @@ -127,7 +131,6 @@ public: /// Derivative of uncalibrate wrpt the calibration parameters Matrix D2d_calibration(const Point2& p) const ; - private: /// @} diff --git a/gtsam/geometry/Cal3Unified.cpp b/gtsam/geometry/Cal3Unified.cpp index e7b408982..6113714a1 100644 --- a/gtsam/geometry/Cal3Unified.cpp +++ b/gtsam/geometry/Cal3Unified.cpp @@ -50,6 +50,7 @@ bool Cal3Unified::equals(const Cal3Unified& K, double tol) const { } /* ************************************************************************* */ +// todo: make a fixed sized jacobian version of this Point2 Cal3Unified::uncalibrate(const Point2& p, boost::optional H1, boost::optional H2) const { @@ -70,26 +71,30 @@ Point2 Cal3Unified::uncalibrate(const Point2& p, // Part2: project NPlane point to pixel plane: use Cal3DS2 Point2 m(x,y); - Matrix H1base, H2base; // jacobians from Base class + Matrix29 H1base; + Matrix2 H2base; // jacobians from Base class Point2 puncalib = Base::uncalibrate(m, H1base, H2base); // Inlined derivative for calibration if (H1) { // part1 - Matrix DU = (Matrix(2,1) << -xs * sqrt_nx * xi_sqrt_nx2, - -ys * sqrt_nx * xi_sqrt_nx2); - Matrix DDS2U = H2base * DU; + Vector2 DU; + DU << -xs * sqrt_nx * xi_sqrt_nx2, // + -ys * sqrt_nx * xi_sqrt_nx2; - *H1 = collect(2, &H1base, &DDS2U); + H1->resize(2,10); + H1->block<2,9>(0,0) = H1base; + H1->block<2,1>(0,9) = H2base * DU; } + // Inlined derivative for points if (H2) { // part1 const double denom = 1.0 * xi_sqrt_nx2 / sqrt_nx; const double mid = -(xi * xs*ys) * denom; - Matrix DU = (Matrix(2, 2) << - (sqrt_nx + xi*(ys*ys + 1)) * denom, mid, - mid, (sqrt_nx + xi*(xs*xs + 1)) * denom); + Matrix2 DU; + DU << (sqrt_nx + xi*(ys*ys + 1)) * denom, mid, // + mid, (sqrt_nx + xi*(xs*xs + 1)) * denom; *H2 = H2base * DU; } diff --git a/gtsam/geometry/Cal3Unified.h b/gtsam/geometry/Cal3Unified.h index 03db6af9a..fb99592f7 100644 --- a/gtsam/geometry/Cal3Unified.h +++ b/gtsam/geometry/Cal3Unified.h @@ -40,7 +40,7 @@ namespace gtsam { * k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y ] * pi = K*pn */ -class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base, public DerivedValue { +class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base { typedef Cal3Unified This; typedef Cal3DS2_Base Base; @@ -50,9 +50,8 @@ private: double xi_; // mirror parameter public: - //Matrix K() const ; - //Eigen::Vector4d k() const { return Base::k(); } - Vector vector() const ; + + Vector vector() const ; /// @name Standard Constructors /// @{ @@ -125,25 +124,35 @@ public: private: - /// @} - /// @name Advanced Interface - /// @{ - /** Serialization function */ friend class boost::serialization::access; template void serialize(Archive & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("Cal3Unified", - boost::serialization::base_object(*this)); ar & boost::serialization::make_nvp("Cal3Unified", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(xi_); } - /// @} +}; +// Define GTSAM traits +namespace traits { + +template<> +struct is_manifold : public boost::true_type { +}; + +template<> +struct dimension : public boost::integral_constant { +}; + +template<> +struct zero { + static Cal3Unified value() { return Cal3Unified();} }; } +} + diff --git a/gtsam/geometry/Cal3_S2.cpp b/gtsam/geometry/Cal3_S2.cpp index c8c0255ea..aece1ded1 100644 --- a/gtsam/geometry/Cal3_S2.cpp +++ b/gtsam/geometry/Cal3_S2.cpp @@ -75,10 +75,29 @@ bool Cal3_S2::equals(const Cal3_S2& K, double tol) const { Point2 Cal3_S2::uncalibrate(const Point2& p, boost::optional Dcal, boost::optional Dp) const { const double x = p.x(), y = p.y(); - if (Dcal) - *Dcal = (Matrix(2, 5) << x, 0.0, y, 1.0, 0.0, 0.0, y, 0.0, 0.0, 1.0); - if (Dp) - *Dp = (Matrix(2, 2) << fx_, s_, 0.000, fy_); + if (Dcal) { + Dcal->resize(2,5); + *Dcal << x, 0.0, y, 1.0, 0.0, 0.0, y, 0.0, 0.0, 1.0; + } + if (Dp) { + Dp->resize(2,2); + *Dp << fx_, s_, 0.0, fy_; + } + return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_); +} + +/* ************************************************************************* */ +Point2 Cal3_S2::uncalibrate(const Point2& p, boost::optional Dcal, + boost::optional Dp) const { + const double x = p.x(), y = p.y(); + if (Dcal) *Dcal << x, 0.0, y, 1.0, 0.0, 0.0, y, 0.0, 0.0, 1.0; + if (Dp) *Dp << fx_, s_, 0.0, fy_; + return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_); +} + +/* ************************************************************************* */ +Point2 Cal3_S2::uncalibrate(const Point2& p) const { + const double x = p.x(), y = p.y(); return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_); } diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index 03c6bff3f..cd1add116 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -31,7 +31,7 @@ namespace gtsam { * @addtogroup geometry * \nosubgrouping */ -class GTSAM_EXPORT Cal3_S2: public DerivedValue { +class GTSAM_EXPORT Cal3_S2 { private: double fx_, fy_, s_, u0_, v0_; @@ -144,12 +144,29 @@ public: /** * convert intrinsic coordinates xy to image coordinates uv * @param p point in intrinsic coordinates + * @return point in image coordinates + */ + Point2 uncalibrate(const Point2& p) const; + + /** + * convert intrinsic coordinates xy to image coordinates uv, fixed derivaitves + * @param p point in intrinsic coordinates * @param Dcal optional 2*5 Jacobian wrpt Cal3_S2 parameters * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates * @return point in image coordinates */ - Point2 uncalibrate(const Point2& p, boost::optional Dcal = - boost::none, boost::optional Dp = boost::none) const; + Point2 uncalibrate(const Point2& p, boost::optional Dcal, + boost::optional Dp) const; + + /** + * convert intrinsic coordinates xy to image coordinates uv, dynamic derivaitves + * @param p point in intrinsic coordinates + * @param Dcal optional 2*5 Jacobian wrpt Cal3_S2 parameters + * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates + * @return point in image coordinates + */ + Point2 uncalibrate(const Point2& p, boost::optional Dcal, + boost::optional Dp) const; /** * convert image coordinates uv to intrinsic coordinates xy @@ -209,9 +226,6 @@ private: friend class boost::serialization::access; template void serialize(Archive & ar, const unsigned int version) { - ar - & boost::serialization::make_nvp("Cal3_S2", - boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(fx_); ar & BOOST_SERIALIZATION_NVP(fy_); ar & BOOST_SERIALIZATION_NVP(s_); @@ -223,4 +237,22 @@ private: }; +// Define GTSAM traits +namespace traits { + +template<> +struct is_manifold : public boost::true_type { +}; + +template<> +struct dimension : public boost::integral_constant { +}; + +template<> +struct zero { + static Cal3_S2 value() { return Cal3_S2();} +}; + +} + } // \ namespace gtsam diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index 6e7b5a114..66d1c5125 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -39,7 +39,7 @@ public: * @addtogroup geometry * \nosubgrouping */ -class GTSAM_EXPORT CalibratedCamera: public DerivedValue { +class GTSAM_EXPORT CalibratedCamera { private: Pose3 pose_; // 6DOF pose @@ -214,12 +214,28 @@ private: friend class boost::serialization::access; template void serialize(Archive & ar, const unsigned int version) { - ar - & boost::serialization::make_nvp("CalibratedCamera", - boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(pose_); } /// @} - };} +}; + +// Define GTSAM traits +namespace traits { + +template<> +struct is_group : public boost::true_type { +}; + +template<> +struct is_manifold : public boost::true_type { +}; + +template<> +struct dimension : public boost::integral_constant { +}; + +} + +} diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h index 32b966261..8763504c0 100644 --- a/gtsam/geometry/EssentialMatrix.h +++ b/gtsam/geometry/EssentialMatrix.h @@ -20,7 +20,7 @@ namespace gtsam { * but here we choose instead to parameterize it as a (Rot3,Unit3) pair. * We can then non-linearly optimize immediately on this 5-dimensional manifold. */ -class GTSAM_EXPORT EssentialMatrix: public DerivedValue { +class GTSAM_EXPORT EssentialMatrix { private: @@ -58,6 +58,8 @@ public: return EssentialMatrix(Rot3::Random(rng), Unit3::Random(rng)); } + virtual ~EssentialMatrix() {} + /// @} /// @name Testable @@ -176,8 +178,6 @@ private: friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("EssentialMatrix", - boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(aRb_); ar & BOOST_SERIALIZATION_NVP(aTb_); @@ -196,5 +196,23 @@ private: }; +// Define GTSAM traits +namespace traits { + +template<> +struct is_manifold : public boost::true_type { +}; + +template<> +struct dimension : public boost::integral_constant { +}; + +template<> +struct zero { + static EssentialMatrix value() { return EssentialMatrix();} +}; + +} + } // gtsam diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index 709b95181..83851e8c0 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -37,11 +37,13 @@ namespace gtsam { * \nosubgrouping */ template -class PinholeCamera: public DerivedValue > { +class PinholeCamera { private: Pose3 pose_; Calibration K_; + static const int DimK = traits::dimension::value; + public: /// @name Standard Constructors @@ -240,12 +242,13 @@ public: calibration().retract(d.tail(calibration().dim()))); } + typedef Eigen::Matrix VectorK6; + /// return canonical coordinate - Vector localCoordinates(const PinholeCamera& T2) const { - Vector d(dim()); - d.head(pose().dim()) = pose().localCoordinates(T2.pose()); - d.tail(calibration().dim()) = calibration().localCoordinates( - T2.calibration()); + VectorK6 localCoordinates(const PinholeCamera& T2) const { + VectorK6 d; // TODO: why does d.head<6>() not compile?? + d.head(6) = pose().localCoordinates(T2.pose()); + d.tail(DimK) = calibration().localCoordinates(T2.calibration()); return d; } @@ -270,16 +273,15 @@ public: * @param Dpoint is the 2*3 Jacobian w.r.t. P */ inline static Point2 project_to_camera(const Point3& P, - boost::optional Dpoint = boost::none) { + boost::optional Dpoint = boost::none) { #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION if (P.z() <= 0) throw CheiralityException(); #endif double d = 1.0 / P.z(); const double u = P.x() * d, v = P.y() * d; - if (Dpoint) { - *Dpoint = (Matrix(2, 3) << d, 0.0, -u * d, 0.0, d, -v * d); - } + if (Dpoint) + *Dpoint << d, 0.0, -u * d, 0.0, d, -v * d; return Point2(u, v); } @@ -290,6 +292,22 @@ public: return std::make_pair(K_.uncalibrate(pn), pc.z() > 0); } + /** project a point from world coordinate to the image + * @param pw is a point in world coordinates + */ + inline Point2 project(const Point3& pw) const { + + // Transform to camera coordinates and check cheirality + const Point3 pc = pose_.transform_to(pw); + + // Project to normalized image coordinates + const Point2 pn = project_to_camera(pc); + + return K_.uncalibrate(pn); + } + + typedef Eigen::Matrix Matrix2K; + /** project a point from world coordinate to the image * @param pw is a point in world coordinates * @param Dpose is the Jacobian w.r.t. pose3 @@ -298,9 +316,44 @@ public: */ inline Point2 project( const Point3& pw, // - boost::optional Dpose = boost::none, - boost::optional Dpoint = boost::none, - boost::optional Dcal = boost::none) const { + boost::optional Dpose, + boost::optional Dpoint, + boost::optional Dcal) const { + + // Transform to camera coordinates and check cheirality + const Point3 pc = pose_.transform_to(pw); + + // Project to normalized image coordinates + const Point2 pn = project_to_camera(pc); + + if (Dpose || Dpoint) { + const double z = pc.z(), d = 1.0 / z; + + // uncalibration + Matrix2 Dpi_pn; + const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn); + + // chain the Jacobian matrices + if (Dpose) + calculateDpose(pn, d, Dpi_pn, *Dpose); + if (Dpoint) + calculateDpoint(pn, d, pose_.rotation().matrix(), Dpi_pn, *Dpoint); + return pi; + } else + return K_.uncalibrate(pn, Dcal, boost::none); + } + + /** project a point from world coordinate to the image + * @param pw is a point in world coordinates + * @param Dpose is the Jacobian w.r.t. pose3 + * @param Dpoint is the Jacobian w.r.t. point3 + * @param Dcal is the Jacobian w.r.t. calibration + */ + inline Point2 project( + const Point3& pw, // + boost::optional Dpose, + boost::optional Dpoint, + boost::optional Dcal) const { // Transform to camera coordinates and check cheirality const Point3 pc = pose_.transform_to(pw); @@ -326,7 +379,7 @@ public: } return pi; } else - return K_.uncalibrate(pn, Dcal); + return K_.uncalibrate(pn, Dcal, boost::none); } /** project a point at infinity from world coordinate to the image @@ -355,7 +408,7 @@ public: Dpc_pose.block(0, 0, 3, 3) = Dpc_rot; // camera to normalized image coordinate - Matrix Dpn_pc; // 2*3 + Matrix23 Dpn_pc; // 2*3 const Point2 pn = project_to_camera(pc, Dpn_pc); // uncalibration @@ -371,15 +424,26 @@ public: return pi; } + typedef Eigen::Matrix Matrix2K6; + /** project a point from world coordinate to the image + * @param pw is a point in the world coordinate + */ + Point2 project2(const Point3& pw) const { + const Point3 pc = pose_.transform_to(pw); + const Point2 pn = project_to_camera(pc); + return K_.uncalibrate(pn); + } + + /** project a point from world coordinate to the image, fixed Jacobians * @param pw is a point in the world coordinate * @param Dcamera is the Jacobian w.r.t. [pose3 calibration] * @param Dpoint is the Jacobian w.r.t. point3 */ - inline Point2 project2( + Point2 project2( const Point3& pw, // - boost::optional Dcamera = boost::none, - boost::optional Dpoint = boost::none) const { + boost::optional Dcamera, + boost::optional Dpoint) const { const Point3 pc = pose_.transform_to(pw); const Point2 pn = project_to_camera(pc); @@ -390,7 +454,40 @@ public: const double z = pc.z(), d = 1.0 / z; // uncalibration - Matrix Dcal, Dpi_pn(2, 2); + Matrix2K Dcal; + Matrix2 Dpi_pn; + const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn); + + if (Dcamera) { // TODO why does leftCols<6>() not compile ?? + calculateDpose(pn, d, Dpi_pn, Dcamera->leftCols(6)); + Dcamera->rightCols(K_.dim()) = Dcal; // Jacobian wrt calib + } + if (Dpoint) { + calculateDpoint(pn, d, pose_.rotation().matrix(), Dpi_pn, *Dpoint); + } + return pi; + } + } + + /** project a point from world coordinate to the image + * @param pw is a point in the world coordinate + * @param Dcamera is the Jacobian w.r.t. [pose3 calibration] + * @param Dpoint is the Jacobian w.r.t. point3 + */ + Point2 project2(const Point3& pw, // + boost::optional Dcamera, boost::optional Dpoint) const { + + const Point3 pc = pose_.transform_to(pw); + const Point2 pn = project_to_camera(pc); + + if (!Dcamera && !Dpoint) { + return K_.uncalibrate(pn); + } else { + const double z = pc.z(), d = 1.0 / z; + + // uncalibration + Matrix2K Dcal; + Matrix2 Dpi_pn; const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn); if (Dcamera) { @@ -517,16 +614,16 @@ private: * See http://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html */ template - static void calculateDpose(const Point2& pn, double d, const Matrix& Dpi_pn, + static void calculateDpose(const Point2& pn, double d, const Matrix2& Dpi_pn, Eigen::MatrixBase const & Dpose) { // optimized version of derivatives, see CalibratedCamera.nb const double u = pn.x(), v = pn.y(); double uv = u * v, uu = u * u, vv = v * v; - Eigen::Matrix Dpn_pose; + Matrix26 Dpn_pose; Dpn_pose << uv, -1 - uu, v, -d, 0, d * u, 1 + vv, -uv, -u, 0, -d, d * v; assert(Dpose.rows()==2 && Dpose.cols()==6); const_cast&>(Dpose) = // - Dpi_pn.block<2, 2>(0, 0) * Dpn_pose; + Dpi_pn * Dpn_pose; } /** @@ -538,32 +635,50 @@ private: * See http://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html */ template - static void calculateDpoint(const Point2& pn, double d, const Matrix& R, - const Matrix& Dpi_pn, Eigen::MatrixBase const & Dpoint) { + static void calculateDpoint(const Point2& pn, double d, const Matrix3& R, + const Matrix2& Dpi_pn, Eigen::MatrixBase const & Dpoint) { // optimized version of derivatives, see CalibratedCamera.nb const double u = pn.x(), v = pn.y(); - Eigen::Matrix Dpn_point; + Matrix23 Dpn_point; Dpn_point << // R(0, 0) - u * R(0, 2), R(1, 0) - u * R(1, 2), R(2, 0) - u * R(2, 2), // /**/R(0, 1) - v * R(0, 2), R(1, 1) - v * R(1, 2), R(2, 1) - v * R(2, 2); Dpn_point *= d; assert(Dpoint.rows()==2 && Dpoint.cols()==3); const_cast&>(Dpoint) = // - Dpi_pn.block<2, 2>(0, 0) * Dpn_point; + Dpi_pn * Dpn_point; } - /// @} - /// @name Advanced Interface - /// @{ - /** Serialization function */ friend class boost::serialization::access; template void serialize(Archive & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Value); ar & BOOST_SERIALIZATION_NVP(pose_); ar & BOOST_SERIALIZATION_NVP(K_); } - /// @} - } - ;} + +}; + +// Define GTSAM traits +namespace traits { + +template +struct is_manifold > : public boost::true_type { +}; + +template +struct dimension > : public boost::integral_constant< + int, dimension::value + dimension::value> { +}; + +template +struct zero > { + static PinholeCamera value() { + return PinholeCamera(zero::value(), + zero::value()); + } +}; + +} + +} // \ gtsam diff --git a/gtsam/geometry/Point2.cpp b/gtsam/geometry/Point2.cpp index 6fc9330ad..8b90aed63 100644 --- a/gtsam/geometry/Point2.cpp +++ b/gtsam/geometry/Point2.cpp @@ -27,8 +27,6 @@ namespace gtsam { /** Explicit instantiation of base class to export members */ INSTANTIATE_LIE(Point2); -static const Matrix oneOne = (Matrix(1, 2) << 1.0, 1.0); - /* ************************************************************************* */ void Point2::print(const string& s) const { cout << s << *this << endl; @@ -39,16 +37,20 @@ bool Point2::equals(const Point2& q, double tol) const { return (fabs(x_ - q.x()) < tol && fabs(y_ - q.y()) < tol); } +/* ************************************************************************* */ +double Point2::norm() const { + return sqrt(x_ * x_ + y_ * y_); +} + /* ************************************************************************* */ double Point2::norm(boost::optional H) const { - double r = sqrt(x_ * x_ + y_ * y_); + double r = norm(); if (H) { - Matrix D_result_d; + H->resize(1,2); if (fabs(r) > 1e-10) - D_result_d = (Matrix(1, 2) << x_ / r, y_ / r); + *H << x_ / r, y_ / r; else - D_result_d = oneOne; // TODO: really infinity, why 1 here?? - *H = D_result_d; + *H << 1, 1; // really infinity, why 1 ? } return r; } diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index dc9a1dac8..59a18aed7 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -32,11 +32,10 @@ namespace gtsam { * @addtogroup geometry * \nosubgrouping */ -class GTSAM_EXPORT Point2 : public DerivedValue { -public: - /// dimension of the variable - used to autodetect sizes - static const size_t dimension = 2; +class GTSAM_EXPORT Point2 { + private: + double x_, y_; public: @@ -153,10 +152,10 @@ public: /// @{ /// dimension of the variable - used to autodetect sizes - inline static size_t Dim() { return dimension; } + inline static size_t Dim() { return 2; } /// Dimensionality of tangent space = 2 DOF - inline size_t dim() const { return dimension; } + inline size_t dim() const { return 2; } /// Updates a with tangent space delta inline Point2 retract(const Vector& v) const { return *this + Point2(v); } @@ -182,7 +181,10 @@ public: Point2 unit() const { return *this/norm(); } /** norm of point */ - double norm(boost::optional H = boost::none) const; + double norm() const; + + /** norm of point, with derivative */ + double norm(boost::optional H) const; /** distance between two points */ double distance(const Point2& p2, boost::optional H1 = boost::none, @@ -235,8 +237,6 @@ private: template void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("Point2", - boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(x_); ar & BOOST_SERIALIZATION_NVP(y_); } @@ -248,5 +248,22 @@ private: /// multiply with scalar inline Point2 operator*(double s, const Point2& p) {return p*s;} +// Define GTSAM traits +namespace traits { + +template<> +struct is_group : public boost::true_type { +}; + +template<> +struct is_manifold : public boost::true_type { +}; + +template<> +struct dimension : public boost::integral_constant { +}; + +} + } diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp index b6244630e..42ecd8c74 100644 --- a/gtsam/geometry/Point3.cpp +++ b/gtsam/geometry/Point3.cpp @@ -98,6 +98,19 @@ double Point3::norm() const { return sqrt(x_ * x_ + y_ * y_ + z_ * z_); } +/* ************************************************************************* */ +double Point3::norm(boost::optional H) const { + double r = norm(); + if (H) { + H->resize(1,3); + if (fabs(r) > 1e-10) + *H << x_ / r, y_ / r, z_ / r; + else + *H << 1, 1, 1; // really infinity, why 1 ? + } + return r; +} + /* ************************************************************************* */ Point3 Point3::normalize(boost::optional H) const { Point3 normalized = *this / norm(); diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 96add6c32..d490ccf8d 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -36,12 +36,10 @@ namespace gtsam { * @addtogroup geometry * \nosubgrouping */ - class GTSAM_EXPORT Point3 : public DerivedValue { - public: - /// dimension of the variable - used to autodetect sizes - static const size_t dimension = 3; + class GTSAM_EXPORT Point3 { private: + double x_, y_, z_; public: @@ -122,10 +120,10 @@ namespace gtsam { /// @{ /// dimension of the variable - used to autodetect sizes - inline static size_t Dim() { return dimension; } + inline static size_t Dim() { return 3; } /// return dimensionality of tangent space, DOF = 3 - inline size_t dim() const { return dimension; } + inline size_t dim() const { return 3; } /// Updates a with tangent space delta inline Point3 retract(const Vector& v) const { return Point3(*this + v); } @@ -185,6 +183,9 @@ namespace gtsam { /** Distance of the point from the origin */ double norm() const; + /** Distance of the point from the origin, with Jacobian */ + double norm(boost::optional H) const; + /** normalize, with optional Jacobian */ Point3 normalize(boost::optional H = boost::none) const; @@ -236,8 +237,6 @@ namespace gtsam { template void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("Point3", - boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(x_); ar & BOOST_SERIALIZATION_NVP(y_); ar & BOOST_SERIALIZATION_NVP(z_); @@ -250,4 +249,20 @@ namespace gtsam { /// Syntactic sugar for multiplying coordinates by a scalar s*p inline Point3 operator*(double s, const Point3& p) { return p*s;} + // Define GTSAM traits + namespace traits { + + template<> + struct is_group : public boost::true_type { + }; + + template<> + struct is_manifold : public boost::true_type { + }; + + template<> + struct dimension : public boost::integral_constant { + }; + + } } diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index 85307e322..90c3f5f8c 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -123,6 +123,27 @@ Pose2 Pose2::inverse(boost::optional H1) const { return Pose2(r_.inverse(), r_.unrotate(Point2(-t_.x(), -t_.y()))); } +/* ************************************************************************* */ +// see doc/math.lyx, SE(2) section +Point2 Pose2::transform_to(const Point2& point) const { + Point2 d = point - t_; + return r_.unrotate(d); +} + +/* ************************************************************************* */ +// see doc/math.lyx, SE(2) section +Point2 Pose2::transform_to(const Point2& point, + boost::optional H1, boost::optional H2) const { + Point2 d = point - t_; + Point2 q = r_.unrotate(d); + if (!H1 && !H2) return q; + if (H1) *H1 << + -1.0, 0.0, q.y(), + 0.0, -1.0, -q.x(); + if (H2) *H2 = r_.transpose(); + return q; +} + /* ************************************************************************* */ // see doc/math.lyx, SE(2) section Point2 Pose2::transform_to(const Point2& point, @@ -161,6 +182,62 @@ Point2 Pose2::transform_from(const Point2& p, return q + t_; } +/* ************************************************************************* */ +Pose2 Pose2::between(const Pose2& p2) const { + // get cosines and sines from rotation matrices + const Rot2& R1 = r_, R2 = p2.r(); + double c1=R1.c(), s1=R1.s(), c2=R2.c(), s2=R2.s(); + + // Assert that R1 and R2 are normalized + assert(std::abs(c1*c1 + s1*s1 - 1.0) < 1e-5 && std::abs(c2*c2 + s2*s2 - 1.0) < 1e-5); + + // Calculate delta rotation = between(R1,R2) + double c = c1 * c2 + s1 * s2, s = -s1 * c2 + c1 * s2; + Rot2 R(Rot2::atan2(s,c)); // normalizes + + // Calculate delta translation = unrotate(R1, dt); + Point2 dt = p2.t() - t_; + double x = dt.x(), y = dt.y(); + // t = R1' * (t2-t1) + Point2 t(c1 * x + s1 * y, -s1 * x + c1 * y); + + return Pose2(R,t); +} + +/* ************************************************************************* */ +Pose2 Pose2::between(const Pose2& p2, boost::optional H1, + boost::optional H2) const { + // get cosines and sines from rotation matrices + const Rot2& R1 = r_, R2 = p2.r(); + double c1=R1.c(), s1=R1.s(), c2=R2.c(), s2=R2.s(); + + // Assert that R1 and R2 are normalized + assert(std::abs(c1*c1 + s1*s1 - 1.0) < 1e-5 && std::abs(c2*c2 + s2*s2 - 1.0) < 1e-5); + + // Calculate delta rotation = between(R1,R2) + double c = c1 * c2 + s1 * s2, s = -s1 * c2 + c1 * s2; + Rot2 R(Rot2::atan2(s,c)); // normalizes + + // Calculate delta translation = unrotate(R1, dt); + Point2 dt = p2.t() - t_; + double x = dt.x(), y = dt.y(); + // t = R1' * (t2-t1) + Point2 t(c1 * x + s1 * y, -s1 * x + c1 * y); + + // FD: This is just -AdjointMap(between(p2,p1)) inlined and re-using above + if (H1) { + double dt1 = -s2 * x + c2 * y; + double dt2 = -c2 * x - s2 * y; + *H1 << + -c, -s, dt1, + s, -c, dt2, + 0.0, 0.0,-1.0; + } + if (H2) *H2 = I3; + + return Pose2(R,t); +} + /* ************************************************************************* */ Pose2 Pose2::between(const Pose2& p2, boost::optional H1, boost::optional H2) const { diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index 26244877b..7aad66710 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -33,10 +33,9 @@ namespace gtsam { * @addtogroup geometry * \nosubgrouping */ -class GTSAM_EXPORT Pose2 : public DerivedValue { +class GTSAM_EXPORT Pose2 { public: - static const size_t dimension = 3; /** Pose Concept requirements */ typedef Rot2 Rotation; @@ -123,20 +122,29 @@ public: /** * Return relative pose between p1 and p2, in p1 coordinate frame */ - Pose2 between(const Pose2& p2, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const; + Pose2 between(const Pose2& p2) const; + /** + * Return relative pose between p1 and p2, in p1 coordinate frame + */ + Pose2 between(const Pose2& p2, boost::optional H1, + boost::optional H2) const; + + /** + * Return relative pose between p1 and p2, in p1 coordinate frame + */ + Pose2 between(const Pose2& p2, boost::optional H1, + boost::optional H2) const; /// @} /// @name Manifold /// @{ /// Dimensionality of tangent space = 3 DOF - used to autodetect sizes - inline static size_t Dim() { return dimension; } + inline static size_t Dim() { return 3; } /// Dimensionality of tangent space = 3 DOF - inline size_t dim() const { return dimension; } + inline size_t dim() const { return 3; } /// Retraction from R^3 \f$ [T_x,T_y,\theta] \f$ to Pose2 manifold neighborhood around current pose Pose2 retract(const Vector& v) const; @@ -182,10 +190,18 @@ public: /// @name Group Action on Point2 /// @{ + /** Return point coordinates in pose coordinate frame */ + Point2 transform_to(const Point2& point) const; + /** Return point coordinates in pose coordinate frame */ Point2 transform_to(const Point2& point, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const; + boost::optional H1, + boost::optional H2) const; + + /** Return point coordinates in pose coordinate frame */ + Point2 transform_to(const Point2& point, + boost::optional H1, + boost::optional H2) const; /** Return point coordinates in global frame */ Point2 transform_from(const Point2& point, @@ -277,14 +293,14 @@ public: */ static std::pair rotationInterval() { return std::make_pair(2, 2); } + /// @} + private: // Serialization function friend class boost::serialization::access; template void serialize(Archive & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("Pose2", - boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(t_); ar & BOOST_SERIALIZATION_NVP(r_); } @@ -303,7 +319,18 @@ inline Matrix wedge(const Vector& xi) { typedef std::pair Point2Pair; GTSAM_EXPORT boost::optional align(const std::vector& pairs); -/// @} +// Define GTSAM traits +namespace traits { + +template<> +struct is_manifold : public boost::true_type { +}; + +template<> +struct dimension : public boost::integral_constant { +}; + +} } // namespace gtsam diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index bfd2fcb9a..7c4ac34e2 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -243,8 +243,8 @@ Pose3 Pose3::transform_to(const Pose3& pose) const { Point3 Pose3::transform_from(const Point3& p, boost::optional Dpose, boost::optional Dpoint) const { if (Dpose) { - const Matrix R = R_.matrix(); - Matrix DR = R * skewSymmetric(-p.x(), -p.y(), -p.z()); + const Matrix3 R = R_.matrix(); + Matrix3 DR = R * skewSymmetric(-p.x(), -p.y(), -p.z()); Dpose->resize(3, 6); (*Dpose) << DR, R; } @@ -254,18 +254,45 @@ Point3 Pose3::transform_from(const Point3& p, boost::optional Dpose, } /* ************************************************************************* */ -Point3 Pose3::transform_to(const Point3& p, boost::optional Dpose, - boost::optional Dpoint) const { - const Point3 result = R_.unrotate(p - t_); +Point3 Pose3::transform_to(const Point3& p) const { + return R_.unrotate(p - t_); +} + +/* ************************************************************************* */ +Point3 Pose3::transform_to(const Point3& p, boost::optional Dpose, + boost::optional Dpoint) const { + // Only get transpose once, to avoid multiple allocations, + // as well as multiple conversions in the Quaternion case + const Matrix3 Rt = R_.transpose(); + const Point3 q(Rt*(p - t_).vector()); if (Dpose) { - const Point3& q = result; - Matrix DR = skewSymmetric(q.x(), q.y(), q.z()); - Dpose->resize(3, 6); - (*Dpose) << DR, _I3; + const double wx = q.x(), wy = q.y(), wz = q.z(); + (*Dpose) << + 0.0, -wz, +wy,-1.0, 0.0, 0.0, + +wz, 0.0, -wx, 0.0,-1.0, 0.0, + -wy, +wx, 0.0, 0.0, 0.0,-1.0; } if (Dpoint) - *Dpoint = R_.transpose(); - return result; + *Dpoint = Rt; + return q; +} + +/* ************************************************************************* */ +Point3 Pose3::transform_to(const Point3& p, boost::optional Dpose, + boost::optional Dpoint) const { + const Matrix3 Rt = R_.transpose(); + const Point3 q(Rt*(p - t_).vector()); + if (Dpose) { + const double wx = q.x(), wy = q.y(), wz = q.z(); + Dpose->resize(3, 6); + (*Dpose) << + 0.0, -wz, +wy,-1.0, 0.0, 0.0, + +wz, 0.0, -wx, 0.0,-1.0, 0.0, + -wy, +wx, 0.0, 0.0, 0.0,-1.0; + } + if (Dpoint) + *Dpoint = Rt; + return q; } /* ************************************************************************* */ diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 825389243..001edb563 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -39,9 +39,8 @@ class Pose2; * @addtogroup geometry * \nosubgrouping */ -class GTSAM_EXPORT Pose3: public DerivedValue { +class GTSAM_EXPORT Pose3{ public: - static const size_t dimension = 6; /** Pose Concept requirements */ typedef Rot3 Rotation; @@ -132,12 +131,12 @@ public: /// Dimensionality of tangent space = 6 DOF - used to autodetect sizes static size_t Dim() { - return dimension; + return 6; } /// Dimensionality of the tangent space = 6 DOF size_t dim() const { - return dimension; + return 6; } /// Retraction from R^6 \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ from R^ with fast first-order approximation to the exponential map @@ -250,8 +249,13 @@ public: * @param Dpoint optional 3*3 Jacobian wrpt point * @return point in Pose coordinates */ + Point3 transform_to(const Point3& p) const; + Point3 transform_to(const Point3& p, - boost::optional Dpose=boost::none, boost::optional Dpoint=boost::none) const; + boost::optional Dpose, boost::optional Dpoint) const; + + Point3 transform_to(const Point3& p, + boost::optional Dpose, boost::optional Dpoint) const; /// @} /// @name Standard Interface @@ -322,8 +326,6 @@ public: friend class boost::serialization::access; template void serialize(Archive & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("Pose3", - boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(R_); ar & BOOST_SERIALIZATION_NVP(t_); } @@ -350,4 +352,21 @@ inline Matrix wedge(const Vector& xi) { typedef std::pair Point3Pair; GTSAM_EXPORT boost::optional align(const std::vector& pairs); +// Define GTSAM traits +namespace traits { + +template<> +struct is_group : public boost::true_type { +}; + +template<> +struct is_manifold : public boost::true_type { +}; + +template<> +struct dimension : public boost::integral_constant { +}; + +} + } // namespace gtsam diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index d121beb12..c5d6141b6 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -31,7 +31,7 @@ namespace gtsam { * @addtogroup geometry * \nosubgrouping */ - class GTSAM_EXPORT Rot2 : public DerivedValue { + class GTSAM_EXPORT Rot2 { public: /** get the dimension by the type */ @@ -230,23 +230,31 @@ namespace gtsam { /** return 2*2 transpose (inverse) rotation matrix */ Matrix transpose() const; - /// @} - /// @name Advanced Interface - /// @{ - private: /** Serialization function */ friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("Rot2", - boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(c_); ar & BOOST_SERIALIZATION_NVP(s_); } - /// @} - }; + // Define GTSAM traits + namespace traits { + + template<> + struct is_group : public boost::true_type { + }; + + template<> + struct is_manifold : public boost::true_type { + }; + + template<> + struct dimension : public boost::integral_constant { + }; + + } } // gtsam diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index 37aa78a78..846e0e070 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -97,13 +97,33 @@ Unit3 Rot3::operator*(const Unit3& p) const { return rotate(p); } +/* ************************************************************************* */ +// see doc/math.lyx, SO(3) section +Point3 Rot3::unrotate(const Point3& p, boost::optional H1, + boost::optional H2) const { + const Matrix3& Rt = transpose(); + Point3 q(Rt * p.vector()); // q = Rt*p + const double wx = q.x(), wy = q.y(), wz = q.z(); + if (H1) + *H1 << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0; + if (H2) + *H2 = Rt; + return q; +} + /* ************************************************************************* */ // see doc/math.lyx, SO(3) section Point3 Rot3::unrotate(const Point3& p, boost::optional H1, boost::optional H2) const { - Point3 q(transpose()*p.vector()); // q = Rt*p - if (H1) *H1 = skewSymmetric(q.x(), q.y(), q.z()); - if (H2) *H2 = transpose(); + const Matrix3& Rt = transpose(); + Point3 q(Rt * p.vector()); // q = Rt*p + const double wx = q.x(), wy = q.y(), wz = q.z(); + if (H1) { + H1->resize(3,3); + *H1 << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0; + } + if (H2) + *H2 = Rt; return q; } @@ -235,6 +255,20 @@ ostream &operator<<(ostream &os, const Rot3& R) { return os; } +/* ************************************************************************* */ +Point3 Rot3::unrotate(const Point3& p) const { + // Eigen expression + return Point3(transpose()*p.vector()); // q = Rt*p +} + +/* ************************************************************************* */ +Rot3 Rot3::slerp(double t, const Rot3& other) const { + // amazingly simple in GTSAM :-) + assert(t>=0 && t<=1); + Vector3 omega = localCoordinates(other, Rot3::EXPMAP); + return retract(t * omega, Rot3::EXPMAP); +} + /* ************************************************************************* */ } // namespace gtsam diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index c8aeae51b..4f40d164d 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -58,11 +58,10 @@ namespace gtsam { * @addtogroup geometry * \nosubgrouping */ - class GTSAM_EXPORT Rot3 : public DerivedValue { - public: - static const size_t dimension = 3; + class GTSAM_EXPORT Rot3 { private: + #ifdef GTSAM_USE_QUATERNIONS /** Internal Eigen Quaternion */ Quaternion quaternion_; @@ -219,8 +218,15 @@ namespace gtsam { Rot3 inverse(boost::optional H1=boost::none) const; /// Compose two rotations i.e., R= (*this) * R2 - Rot3 compose(const Rot3& R2, - boost::optional H1=boost::none, boost::optional H2=boost::none) const; + Rot3 compose(const Rot3& R2) const; + + /// Compose two rotations i.e., R= (*this) * R2 + Rot3 compose(const Rot3& R2, boost::optional H1, + boost::optional H2) const; + + /// Compose two rotations i.e., R= (*this) * R2 + Rot3 compose(const Rot3& R2, boost::optional H1, + boost::optional H2) const; /** compose two rotations */ Rot3 operator*(const Rot3& R2) const; @@ -247,10 +253,10 @@ namespace gtsam { /// @{ /// dimension of the variable - used to autodetect sizes - static size_t Dim() { return dimension; } + static size_t Dim() { return 3; } /// return dimensionality of tangent space, DOF = 3 - size_t dim() const { return dimension; } + size_t dim() const { return 3; } /** * The method retract() is used to map from the tangent space back to the manifold. @@ -328,11 +334,16 @@ namespace gtsam { /// rotate point from rotated coordinate frame to world = R*p Point3 operator*(const Point3& p) const; - /** - * rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$ - */ - Point3 unrotate(const Point3& p, boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const; + /// rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$ + Point3 unrotate(const Point3& p) const; + + /// rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$ + Point3 unrotate(const Point3& p, boost::optional H1, + boost::optional H2) const; + + /// rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$ + Point3 unrotate(const Point3& p, boost::optional H1, + boost::optional H2) const; /// @} /// @name Group Action on Unit3 @@ -356,8 +367,11 @@ namespace gtsam { /** return 3*3 rotation matrix */ Matrix3 matrix() const; - /** return 3*3 transpose (inverse) rotation matrix */ + /** + * Return 3*3 transpose (inverse) rotation matrix + */ Matrix3 transpose() const; + // TODO: const Eigen::Transpose transpose() const; /// @deprecated, this is base 1, and was just confusing Point3 column(int index) const; @@ -423,17 +437,25 @@ namespace gtsam { */ Vector quaternion() const; + /** + * @brief Spherical Linear intERPolation between *this and other + * @param s a value between 0 and 1 + * @param other final point of iterpolation geodesic on manifold + */ + Rot3 slerp(double t, const Rot3& other) const; + /// Output stream operator GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Rot3& p); + /// @} + private: + /** Serialization function */ friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("Rot3", - boost::serialization::base_object(*this)); #ifndef GTSAM_USE_QUATERNIONS ar & boost::serialization::make_nvp("rot11", rot_(0,0)); ar & boost::serialization::make_nvp("rot12", rot_(0,1)); @@ -451,9 +473,8 @@ namespace gtsam { ar & boost::serialization::make_nvp("z", quaternion_.z()); #endif } - }; - /// @} + }; /** * [RQ] receives a 3 by 3 matrix and returns an upper triangular matrix R @@ -466,4 +487,21 @@ namespace gtsam { * @return a vector [thetax, thetay, thetaz] in radians. */ GTSAM_EXPORT std::pair RQ(const Matrix3& A); + + // Define GTSAM traits + namespace traits { + + template<> + struct is_group : public boost::true_type { + }; + + template<> + struct is_manifold : public boost::true_type { + }; + + template<> + struct dimension : public boost::integral_constant { + }; + + } } diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index 118d8546e..d0c7e95f3 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -63,11 +63,9 @@ Rot3::Rot3(const Matrix& R) { rot_ = R; } -///* ************************************************************************* */ -//Rot3::Rot3(const Matrix3& R) : rot_(R) {} - /* ************************************************************************* */ -Rot3::Rot3(const Quaternion& q) : rot_(q.toRotationMatrix()) {} +Rot3::Rot3(const Quaternion& q) : rot_(q.toRotationMatrix()) { +} /* ************************************************************************* */ Rot3 Rot3::Rx(double t) { @@ -143,6 +141,19 @@ Rot3 Rot3::rodriguez(const Vector& w, double theta) { -swy + C02, swx + C12, c + C22); } +/* ************************************************************************* */ +Rot3 Rot3::compose (const Rot3& R2) const { + return *this * R2; +} + +/* ************************************************************************* */ +Rot3 Rot3::compose (const Rot3& R2, + boost::optional H1, boost::optional H2) const { + if (H1) *H1 = R2.transpose(); + if (H2) *H2 = I3; + return *this * R2; +} + /* ************************************************************************* */ Rot3 Rot3::compose (const Rot3& R2, boost::optional H1, boost::optional H2) const { @@ -156,10 +167,16 @@ Rot3 Rot3::operator*(const Rot3& R2) const { return Rot3(Matrix3(rot_*R2.rot_)); } +/* ************************************************************************* */ +// TODO const Eigen::Transpose Rot3::transpose() const { +Matrix3 Rot3::transpose() const { + return rot_.transpose(); +} + /* ************************************************************************* */ Rot3 Rot3::inverse(boost::optional H1) const { if (H1) *H1 = -rot_; - return Rot3(Matrix3(rot_.transpose())); + return Rot3(Matrix3(transpose())); } /* ************************************************************************* */ @@ -167,7 +184,8 @@ Rot3 Rot3::between (const Rot3& R2, boost::optional H1, boost::optional H2) const { if (H1) *H1 = -(R2.transpose()*rot_); if (H2) *H2 = I3; - return Rot3(Matrix3(rot_.transpose()*R2.rot_)); + Matrix3 R12 = transpose()*R2.rot_; + return Rot3(R12); } /* ************************************************************************* */ @@ -253,23 +271,23 @@ Vector3 Rot3::localCoordinates(const Rot3& T, Rot3::CoordinatesMode mode) const return Logmap(between(T)); } else if(mode == Rot3::CAYLEY) { // Create a fixed-size matrix - Eigen::Matrix3d A(between(T).matrix()); + Matrix3 A = rot_.transpose() * T.matrix(); // Mathematica closed form optimization (procrastination?) gone wild: const double a=A(0,0),b=A(0,1),c=A(0,2); const double d=A(1,0),e=A(1,1),f=A(1,2); const double g=A(2,0),h=A(2,1),i=A(2,2); const double di = d*i, ce = c*e, cd = c*d, fg=f*g; const double M = 1 + e - f*h + i + e*i; - const double K = 2.0 / (cd*h + M + a*M -g*(c + ce) - b*(d + di - fg)); - const double x = (a * f - cd + f) * K; - const double y = (b * f - ce - c) * K; - const double z = (fg - di - d) * K; - return -2 * Vector3(x, y, z); + const double K = - 4.0 / (cd*h + M + a*M -g*(c + ce) - b*(d + di - fg)); + const double x = a * f - cd + f; + const double y = b * f - ce - c; + const double z = fg - di - d; + return K * Vector3(x, y, z); } else if(mode == Rot3::SLOW_CAYLEY) { // Create a fixed-size matrix - Eigen::Matrix3d A(between(T).matrix()); + Matrix3 A(between(T).matrix()); // using templated version of Cayley - Eigen::Matrix3d Omega = CayleyFixed<3>(A); + Matrix3 Omega = CayleyFixed<3>(A); return -2*Vector3(Omega(2,1),Omega(0,2),Omega(1,0)); } else { assert(false); @@ -282,11 +300,6 @@ Matrix3 Rot3::matrix() const { return rot_; } -/* ************************************************************************* */ -Matrix3 Rot3::transpose() const { - return rot_.transpose(); -} - /* ************************************************************************* */ Point3 Rot3::r1() const { return Point3(rot_.col(0)); } diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp index 6b7a4e0ce..19de92ca2 100644 --- a/gtsam/geometry/Rot3Q.cpp +++ b/gtsam/geometry/Rot3Q.cpp @@ -56,21 +56,25 @@ namespace gtsam { Rot3::Rot3(const Matrix& R) : quaternion_(Matrix3(R)) {} -// /* ************************************************************************* */ -// Rot3::Rot3(const Matrix3& R) : -// quaternion_(R) {} + /* ************************************************************************* */ + Rot3::Rot3(const Quaternion& q) : + quaternion_(q) { + } /* ************************************************************************* */ - Rot3::Rot3(const Quaternion& q) : quaternion_(q) {} + Rot3 Rot3::Rx(double t) { + return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitX())); + } /* ************************************************************************* */ - Rot3 Rot3::Rx(double t) { return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitX())); } + Rot3 Rot3::Ry(double t) { + return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitY())); + } /* ************************************************************************* */ - Rot3 Rot3::Ry(double t) { return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitY())); } - - /* ************************************************************************* */ - Rot3 Rot3::Rz(double t) { return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitZ())); } + Rot3 Rot3::Rz(double t) { + return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitZ())); + } /* ************************************************************************* */ Rot3 Rot3::RzRyRx(double x, double y, double z) { return Rot3( @@ -83,6 +87,19 @@ namespace gtsam { Rot3 Rot3::rodriguez(const Vector& w, double theta) { return Quaternion(Eigen::AngleAxisd(theta, w)); } + /* ************************************************************************* */ + Rot3 Rot3::compose(const Rot3& R2) const { + return Rot3(quaternion_ * R2.quaternion_); + } + + /* ************************************************************************* */ + Rot3 Rot3::compose(const Rot3& R2, + boost::optional H1, boost::optional H2) const { + if (H1) *H1 = R2.transpose(); + if (H2) *H2 = I3; + return Rot3(quaternion_ * R2.quaternion_); + } + /* ************************************************************************* */ Rot3 Rot3::compose(const Rot3& R2, boost::optional H1, boost::optional H2) const { @@ -102,6 +119,14 @@ namespace gtsam { return Rot3(quaternion_.inverse()); } + /* ************************************************************************* */ + // TODO: Could we do this? It works in Rot3M but not here, probably because + // here we create an intermediate value by calling matrix() + // const Eigen::Transpose Rot3::transpose() const { + Matrix3 Rot3::transpose() const { + return matrix().transpose(); + } + /* ************************************************************************* */ Rot3 Rot3::between(const Rot3& R2, boost::optional H1, boost::optional H2) const { @@ -161,9 +186,6 @@ namespace gtsam { /* ************************************************************************* */ Matrix3 Rot3::matrix() const {return quaternion_.toRotationMatrix();} - /* ************************************************************************* */ - Matrix3 Rot3::transpose() const {return quaternion_.toRotationMatrix().transpose();} - /* ************************************************************************* */ Point3 Rot3::r1() const { return Point3(quaternion_.toRotationMatrix().col(0)); } diff --git a/gtsam/geometry/StereoCamera.h b/gtsam/geometry/StereoCamera.h index 70917b2c4..6b290edac 100644 --- a/gtsam/geometry/StereoCamera.h +++ b/gtsam/geometry/StereoCamera.h @@ -36,7 +36,7 @@ public: * A stereo camera class, parameterize by left camera pose and stereo calibration * @addtogroup geometry */ -class GTSAM_EXPORT StereoCamera : public DerivedValue { +class GTSAM_EXPORT StereoCamera { private: Pose3 leftCamPose_; @@ -147,12 +147,28 @@ private: friend class boost::serialization::access; template void serialize(Archive & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("StereoCamera", - boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(leftCamPose_); ar & BOOST_SERIALIZATION_NVP(K_); } }; +// Define GTSAM traits +namespace traits { + +template<> +struct is_manifold : public boost::true_type { +}; + +template<> +struct dimension : public boost::integral_constant { +}; + +template<> +struct zero { + static StereoCamera value() { return StereoCamera();} +}; + +} + } diff --git a/gtsam/geometry/StereoPoint2.h b/gtsam/geometry/StereoPoint2.h index 8ce2e49bf..d62a3f067 100644 --- a/gtsam/geometry/StereoPoint2.h +++ b/gtsam/geometry/StereoPoint2.h @@ -28,7 +28,7 @@ namespace gtsam { * @addtogroup geometry * \nosubgrouping */ - class GTSAM_EXPORT StereoPoint2 : public DerivedValue { + class GTSAM_EXPORT StereoPoint2 { public: static const size_t dimension = 3; private: @@ -162,8 +162,6 @@ namespace gtsam { friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("StereoPoint2", - boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(uL_); ar & BOOST_SERIALIZATION_NVP(uR_); ar & BOOST_SERIALIZATION_NVP(v_); @@ -173,4 +171,20 @@ namespace gtsam { }; + // Define GTSAM traits + namespace traits { + + template<> + struct is_group : public boost::true_type { + }; + + template<> + struct is_manifold : public boost::true_type { + }; + + template<> + struct dimension : public boost::integral_constant { + }; + + } } diff --git a/gtsam/geometry/TriangulationFactor.h b/gtsam/geometry/TriangulationFactor.h index 24b7918e3..fc8d546d3 100644 --- a/gtsam/geometry/TriangulationFactor.h +++ b/gtsam/geometry/TriangulationFactor.h @@ -115,7 +115,7 @@ public: Vector evaluateError(const Point3& point, boost::optional H2 = boost::none) const { try { - Point2 error(camera_.project(point, boost::none, H2) - measured_); + Point2 error(camera_.project(point, boost::none, H2, boost::none) - measured_); return error.vector(); } catch (CheiralityException& e) { if (H2) @@ -155,7 +155,7 @@ public: // Would be even better if we could pass blocks to project const Point3& point = x.at(key()); - b = -(camera_.project(point, boost::none, A) - measured_).vector(); + b = -(camera_.project(point, boost::none, A, boost::none) - measured_).vector(); if (noiseModel_) this->noiseModel_->WhitenSystem(A, b); diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h index 23dc535e8..d28c9b4a8 100644 --- a/gtsam/geometry/Unit3.h +++ b/gtsam/geometry/Unit3.h @@ -28,7 +28,7 @@ namespace gtsam { /// Represents a 3D point on a unit sphere. -class GTSAM_EXPORT Unit3: public DerivedValue { +class GTSAM_EXPORT Unit3{ private: @@ -149,8 +149,6 @@ private: friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("Unit3", - boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(p_); ar & BOOST_SERIALIZATION_NVP(B_); } @@ -159,5 +157,25 @@ private: }; +// Define GTSAM traits +namespace traits { + +template<> +struct is_manifold : public boost::true_type { +}; + +template<> +struct dimension : public boost::integral_constant { +}; + +template<> +struct zero { + static Unit3 value() { + return Unit3(); + } +}; + +} + } // namespace gtsam diff --git a/gtsam/geometry/tests/testCal3Bundler.cpp b/gtsam/geometry/tests/testCal3Bundler.cpp index 6e62d4be0..e9eb689e1 100644 --- a/gtsam/geometry/tests/testCal3Bundler.cpp +++ b/gtsam/geometry/tests/testCal3Bundler.cpp @@ -14,7 +14,6 @@ * @brief Unit tests for transform derivatives */ - #include #include #include @@ -28,6 +27,15 @@ GTSAM_CONCEPT_MANIFOLD_INST(Cal3Bundler) static Cal3Bundler K(500, 1e-3, 1e-3, 1000, 2000); static Point2 p(2,3); +/* ************************************************************************* */ +TEST( Cal3Bundler, vector) +{ + Cal3Bundler K; + Vector expected(3); + expected << 1, 0, 0; + CHECK(assert_equal(expected,K.vector())); +} + /* ************************************************************************* */ TEST( Cal3Bundler, uncalibrate) { @@ -36,7 +44,7 @@ TEST( Cal3Bundler, uncalibrate) double g = v[0]*(1+v[1]*r+v[2]*r*r) ; Point2 expected (1000+g*p.x(), 2000+g*p.y()) ; Point2 actual = K.uncalibrate(p); - CHECK(assert_equal(actual,expected)); + CHECK(assert_equal(expected,actual)); } TEST( Cal3Bundler, calibrate ) diff --git a/gtsam/geometry/tests/testPoint2.cpp b/gtsam/geometry/tests/testPoint2.cpp index 66ee5a387..215f376f6 100644 --- a/gtsam/geometry/tests/testPoint2.cpp +++ b/gtsam/geometry/tests/testPoint2.cpp @@ -84,8 +84,8 @@ namespace { Point2 l1(1, 0), l2(1, 1), l3(2, 2), l4(1, 3); /* ************************************************************************* */ - LieVector norm_proxy(const Point2& point) { - return LieVector(point.norm()); + double norm_proxy(const Point2& point) { + return point.norm(); } } TEST( Point2, norm ) { @@ -112,8 +112,8 @@ TEST( Point2, norm ) { /* ************************************************************************* */ namespace { - LieVector distance_proxy(const Point2& location, const Point2& point) { - return LieVector(location.distance(point)); + double distance_proxy(const Point2& location, const Point2& point) { + return location.distance(point); } } TEST( Point2, distance ) { diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp index 836487c1f..a46031209 100644 --- a/gtsam/geometry/tests/testPoint3.cpp +++ b/gtsam/geometry/tests/testPoint3.cpp @@ -88,6 +88,20 @@ TEST (Point3, normalize) { EXPECT(assert_equal(expectedH, actualH, 1e-8)); } +//************************************************************************* +double norm_proxy(const Point3& point) { + return double(point.norm()); +} + +TEST (Point3, norm) { + Matrix actualH; + Point3 point(3,4,5); // arbitrary point + double expected = sqrt(50); + EXPECT_DOUBLES_EQUAL(expected, point.norm(actualH), 1e-8); + Matrix expectedH = numericalDerivative11(norm_proxy, point); + EXPECT(assert_equal(expectedH, actualH, 1e-8)); +} + /* ************************************************************************* */ double testFunc(const Point3& P, const Point3& Q) { return P.distance(Q); diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index 42b548f5a..12266c16f 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -574,8 +574,8 @@ TEST( Pose2, bearing_pose ) /* ************************************************************************* */ namespace { - LieVector range_proxy(const Pose2& pose, const Point2& point) { - return LieVector(pose.range(point)); + double range_proxy(const Pose2& pose, const Point2& point) { + return pose.range(point); } } TEST( Pose2, range ) @@ -611,8 +611,8 @@ TEST( Pose2, range ) /* ************************************************************************* */ namespace { - LieVector range_pose_proxy(const Pose2& pose, const Pose2& point) { - return LieVector(pose.range(point)); + double range_pose_proxy(const Pose2& pose, const Pose2& point) { + return pose.range(point); } } TEST( Pose2, range_pose ) diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 175a11ff1..2a775379d 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -547,8 +547,8 @@ Pose3 xl4(Rot3::ypr(0.0, 0.0, 1.0), Point3(1, 4,-4)); /* ************************************************************************* */ -LieVector range_proxy(const Pose3& pose, const Point3& point) { - return LieVector(pose.range(point)); +double range_proxy(const Pose3& pose, const Point3& point) { + return pose.range(point); } TEST( Pose3, range ) { @@ -582,8 +582,8 @@ TEST( Pose3, range ) } /* ************************************************************************* */ -LieVector range_pose_proxy(const Pose3& pose, const Pose3& point) { - return LieVector(pose.range(point)); +double range_pose_proxy(const Pose3& pose, const Pose3& point) { + return pose.range(point); } TEST( Pose3, range_pose ) { @@ -674,30 +674,24 @@ TEST(Pose3, align_2) { /* ************************************************************************* */ /// exp(xi) exp(y) = exp(xi + x) /// Hence, y = log (exp(-xi)*exp(xi+x)) - Vector xi = (Vector(6) << 0.1, 0.2, 0.3, 1.0, 2.0, 3.0); -Vector testDerivExpmapInv(const LieVector& dxi) { - Vector y = Pose3::Logmap(Pose3::Expmap(-xi)*Pose3::Expmap(xi+dxi)); - return y; +Vector testDerivExpmapInv(const Vector6& dxi) { + return Pose3::Logmap(Pose3::Expmap(-xi) * Pose3::Expmap(xi + dxi)); } TEST( Pose3, dExpInv_TLN) { Matrix res = Pose3::dExpInv_exp(xi); - Matrix numericalDerivExpmapInv = numericalDerivative11( - boost::function( - boost::bind(testDerivExpmapInv, _1) - ), - LieVector(Vector::Zero(6)), 1e-5 - ); + Matrix numericalDerivExpmapInv = numericalDerivative11( + testDerivExpmapInv, Vector6::Zero(), 1e-5); EXPECT(assert_equal(numericalDerivExpmapInv,res,3e-1)); } /* ************************************************************************* */ -Vector testDerivAdjoint(const LieVector& xi, const LieVector& v) { - return Pose3::adjointMap(xi)*v; +Vector testDerivAdjoint(const Vector6& xi, const Vector6& v) { + return Pose3::adjointMap(xi) * v; } TEST( Pose3, adjoint) { @@ -706,20 +700,16 @@ TEST( Pose3, adjoint) { Matrix actualH; Vector actual = Pose3::adjoint(screw::xi, screw::xi, actualH); - Matrix numericalH = numericalDerivative21( - boost::function( - boost::bind(testDerivAdjoint, _1, _2) - ), - LieVector(screw::xi), LieVector(screw::xi), 1e-5 - ); + Matrix numericalH = numericalDerivative21( + testDerivAdjoint, screw::xi, screw::xi, 1e-5); EXPECT(assert_equal(expected,actual,1e-5)); EXPECT(assert_equal(numericalH,actualH,1e-5)); } /* ************************************************************************* */ -Vector testDerivAdjointTranspose(const LieVector& xi, const LieVector& v) { - return Pose3::adjointMap(xi).transpose()*v; +Vector testDerivAdjointTranspose(const Vector6& xi, const Vector6& v) { + return Pose3::adjointMap(xi).transpose() * v; } TEST( Pose3, adjointTranspose) { @@ -730,12 +720,8 @@ TEST( Pose3, adjointTranspose) { Matrix actualH; Vector actual = Pose3::adjointTranspose(xi, v, actualH); - Matrix numericalH = numericalDerivative21( - boost::function( - boost::bind(testDerivAdjointTranspose, _1, _2) - ), - LieVector(xi), LieVector(v), 1e-5 - ); + Matrix numericalH = numericalDerivative21( + testDerivAdjointTranspose, xi, v, 1e-5); EXPECT(assert_equal(expected,actual,1e-15)); EXPECT(assert_equal(numericalH,actualH,1e-5)); diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index ca1e2220c..63dc75876 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -130,15 +130,20 @@ TEST( Rot3, rodriguez4) Rot3 expected(c,-s, 0, s, c, 0, 0, 0, 1); - CHECK(assert_equal(expected,actual,1e-5)); - CHECK(assert_equal(slow_but_correct_rodriguez(axis*angle),actual,1e-5)); + CHECK(assert_equal(expected,actual)); + CHECK(assert_equal(slow_but_correct_rodriguez(axis*angle),actual)); } /* ************************************************************************* */ TEST( Rot3, retract) { Vector v = zero(3); - CHECK(assert_equal(R.retract(v), R)); + CHECK(assert_equal(R, R.retract(v))); + + // test Canonical coordinates + Canonical chart; + Vector v2 = chart.local(R); + CHECK(assert_equal(R, chart.retract(v2))); } /* ************************************************************************* */ @@ -209,9 +214,9 @@ Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta){ TEST( Rot3, rightJacobianExpMapSO3 ) { // Linearization point - Vector thetahat = (Vector(3) << 0.1, 0, 0); + Vector3 thetahat; thetahat << 0.1, 0, 0; - Matrix expectedJacobian = numericalDerivative11( + Matrix expectedJacobian = numericalDerivative11( boost::bind(&Rot3::Expmap, _1), thetahat); Matrix actualJacobian = Rot3::rightJacobianExpMapSO3(thetahat); CHECK(assert_equal(expectedJacobian, actualJacobian)); @@ -221,10 +226,10 @@ TEST( Rot3, rightJacobianExpMapSO3 ) TEST( Rot3, rightJacobianExpMapSO3inverse ) { // Linearization point - Vector thetahat = (Vector(3) << 0.1,0.1,0); ///< Current estimate of rotation rate bias - Vector deltatheta = (Vector(3) << 0, 0, 0); + Vector3 thetahat; thetahat << 0.1,0.1,0; ///< Current estimate of rotation rate bias + Vector3 deltatheta; deltatheta << 0, 0, 0; - Matrix expectedJacobian = numericalDerivative11( + Matrix expectedJacobian = numericalDerivative11( boost::bind(&evaluateLogRotation, thetahat, _1), deltatheta); Matrix actualJacobian = Rot3::rightJacobianExpMapSO3inverse(thetahat); EXPECT(assert_equal(expectedJacobian, actualJacobian)); @@ -381,10 +386,10 @@ TEST( Rot3, between ) EXPECT(assert_equal(expected,actual)); Matrix numericalH1 = numericalDerivative21(testing::between , R1, R2); - CHECK(assert_equal(numericalH1,actualH1, 1e-4)); + CHECK(assert_equal(numericalH1,actualH1)); Matrix numericalH2 = numericalDerivative22(testing::between , R1, R2); - CHECK(assert_equal(numericalH2,actualH2, 1e-4)); + CHECK(assert_equal(numericalH2,actualH2)); } /* ************************************************************************* */ @@ -400,12 +405,12 @@ Vector3 testDexpL(const Vector3& dw) { TEST( Rot3, dexpL) { Matrix actualDexpL = Rot3::dexpL(w); - Matrix expectedDexpL = numericalDerivative11(testDexpL, - LieVector(zero(3)), 1e-2); - EXPECT(assert_equal(expectedDexpL, actualDexpL, 1e-5)); + Matrix expectedDexpL = numericalDerivative11(testDexpL, + Vector3::Zero(), 1e-2); + EXPECT(assert_equal(expectedDexpL, actualDexpL,1e-7)); Matrix actualDexpInvL = Rot3::dexpInvL(w); - EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL, 1e-5)); + EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL,1e-7)); } /* ************************************************************************* */ @@ -518,7 +523,7 @@ TEST( Rot3, logmapStability ) { // std::cout << "trace: " << tr << std::endl; // R.print("R = "); Vector actualw = Rot3::Logmap(R); - CHECK(assert_equal(w, actualw, 1e-15)); // this should be fixed for Quaternions!!! + CHECK(assert_equal(w, actualw, 1e-15)); } /* ************************************************************************* */ @@ -577,6 +582,18 @@ TEST( Rot3, stream) EXPECT(os.str() == "\n|1, 0, 0|\n|0, 1, 0|\n|0, 0, 1|\n"); } +/* ************************************************************************* */ +TEST( Rot3, slerp) +{ + // A first simple test + Rot3 R1 = Rot3::Rz(1), R2 = Rot3::Rz(2), R3 = Rot3::Rz(1.5); + EXPECT(assert_equal(R1, R1.slerp(0.0,R2))); + EXPECT(assert_equal(R2, R1.slerp(1.0,R2))); + EXPECT(assert_equal(R3, R1.slerp(0.5,R2))); + // Make sure other can be *this + EXPECT(assert_equal(R1, R1.slerp(0.5,R1))); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/geometry/tests/testRot3M.cpp b/gtsam/geometry/tests/testRot3M.cpp index c3db476b5..876b3845f 100644 --- a/gtsam/geometry/tests/testRot3M.cpp +++ b/gtsam/geometry/tests/testRot3M.cpp @@ -32,6 +32,9 @@ using namespace std; using namespace gtsam; +GTSAM_CONCEPT_TESTABLE_INST(Rot3) +GTSAM_CONCEPT_LIE_INST(Rot3) + static Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2); static Point3 P(0.2, 0.7, -2.0); static const Matrix I3 = eye(3); diff --git a/gtsam/geometry/tests/testSimpleCamera.cpp b/gtsam/geometry/tests/testSimpleCamera.cpp index 9ced28dca..366d09d49 100644 --- a/gtsam/geometry/tests/testSimpleCamera.cpp +++ b/gtsam/geometry/tests/testSimpleCamera.cpp @@ -125,7 +125,7 @@ static Point2 project2(const Pose3& pose, const Point3& point) { TEST( SimpleCamera, Dproject_point_pose) { Matrix Dpose, Dpoint; - Point2 result = camera.project(point1, Dpose, Dpoint); + Point2 result = camera.project(point1, Dpose, Dpoint, boost::none); Matrix numerical_pose = numericalDerivative21(project2, pose1, point1); Matrix numerical_point = numericalDerivative22(project2, pose1, point1); CHECK(assert_equal(result, Point2(-100, 100) )); diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp index 51c195d32..0bd553a40 100644 --- a/gtsam/geometry/tests/testTriangulation.cpp +++ b/gtsam/geometry/tests/testTriangulation.cpp @@ -274,11 +274,7 @@ TEST( triangulation, TriangulationFactor ) { Matrix HActual; factor.evaluateError(landmark, HActual); -// Matrix expectedH1 = numericalDerivative11( -// boost::bind(&EssentialMatrixConstraint::evaluateError, &factor, _1, pose2, -// boost::none, boost::none), pose1); - // The expected Jacobian - Matrix HExpected = numericalDerivative11( + Matrix HExpected = numericalDerivative11( boost::bind(&Factor::evaluateError, &factor, _1, boost::none), landmark); // Verify the Jacobians are correct diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp index 4aa553df2..53ae2fc58 100644 --- a/gtsam/geometry/tests/testUnit3.cpp +++ b/gtsam/geometry/tests/testUnit3.cpp @@ -115,13 +115,13 @@ TEST(Unit3, error) { Matrix actual, expected; // Use numerical derivatives to calculate the expected Jacobian { - expected = numericalDerivative11( + expected = numericalDerivative11( boost::bind(&Unit3::error, &p, _1, boost::none), q); p.error(q, actual); EXPECT(assert_equal(expected.transpose(), actual, 1e-9)); } { - expected = numericalDerivative11( + expected = numericalDerivative11( boost::bind(&Unit3::error, &p, _1, boost::none), r); p.error(r, actual); EXPECT(assert_equal(expected.transpose(), actual, 1e-9)); diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index 99ea91cd9..92f43b6be 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -82,20 +82,22 @@ namespace gtsam { class GTSAM_EXPORT JacobianFactor : public GaussianFactor { public: + typedef JacobianFactor This; ///< Typedef to this class typedef GaussianFactor Base; ///< Typedef to base class typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class - protected: - VerticalBlockMatrix Ab_; // the block view of the full matrix - noiseModel::Diagonal::shared_ptr model_; // Gaussian noise model with diagonal covariance matrix - - public: typedef VerticalBlockMatrix::Block ABlock; typedef VerticalBlockMatrix::constBlock constABlock; typedef ABlock::ColXpr BVector; typedef constABlock::ConstColXpr constBVector; + protected: + + VerticalBlockMatrix Ab_; // the block view of the full matrix + noiseModel::Diagonal::shared_ptr model_; // Gaussian noise model with diagonal covariance matrix + + public: /** Convert from other GaussianFactor */ explicit JacobianFactor(const GaussianFactor& gf); @@ -328,6 +330,21 @@ namespace gtsam { private: + /** Unsafe Constructor that creates an uninitialized Jacobian of right size + * @param keys in some order + * @param diemnsions of the variables in same order + * @param m output dimension + * @param model noise model (default NULL) + */ + template + JacobianFactor(const KEYS& keys, const DIMENSIONS& dims, DenseIndex m, + const SharedDiagonal& model = SharedDiagonal()) : + Base(keys), Ab_(dims.begin(), dims.end(), m, true), model_(model) { + } + + // be very selective on who can access these private methods: + template friend class ExpressionFactor; + /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 069f2c0aa..caad6b01a 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -61,6 +61,11 @@ namespace gtsam { Base(size_t dim = 1):dim_(dim) {} virtual ~Base() {} + /** true if a constrained noise mode, saves slow/clumsy dynamic casting */ + virtual bool is_constrained() const { + return false; + } + /// Dimensionality inline size_t dim() const { return dim_;} @@ -385,6 +390,11 @@ namespace gtsam { virtual ~Constrained() {} + /** true if a constrained noise mode, saves slow/clumsy dynamic casting */ + virtual bool is_constrained() const { + return true; + } + /// Access mu as a vector const Vector& mu() const { return mu_; } diff --git a/gtsam/linear/tests/testGaussianBayesNet.cpp b/gtsam/linear/tests/testGaussianBayesNet.cpp index d65f96f7f..cd5231e49 100644 --- a/gtsam/linear/tests/testGaussianBayesNet.cpp +++ b/gtsam/linear/tests/testGaussianBayesNet.cpp @@ -19,7 +19,6 @@ #include #include #include -#include #include #include @@ -149,8 +148,9 @@ TEST( GaussianBayesNet, DeterminantTest ) } /* ************************************************************************* */ +typedef Eigen::Matrix Vector10; namespace { - double computeError(const GaussianBayesNet& gbn, const LieVector& values) + double computeError(const GaussianBayesNet& gbn, const Vector10& values) { pair Rd = GaussianFactorGraph(gbn).jacobian(); return 0.5 * (Rd.first * values - Rd.second).squaredNorm(); @@ -180,14 +180,12 @@ TEST(GaussianBayesNet, ComputeSteepestDescentPoint) { 4, (Vector(2) << 49.0,50.0), (Matrix(2, 2) << 51.0,52.0,0.0,54.0))); // Compute the Hessian numerically - Matrix hessian = numericalHessian( - boost::function(boost::bind(&computeError, gbn, _1)), - LieVector(Vector::Zero(GaussianFactorGraph(gbn).jacobian().first.cols()))); + Matrix hessian = numericalHessian( + boost::bind(&computeError, gbn, _1), Vector10::Zero()); // Compute the gradient numerically - Vector gradient = numericalGradient( - boost::function(boost::bind(&computeError, gbn, _1)), - LieVector(Vector::Zero(GaussianFactorGraph(gbn).jacobian().first.cols()))); + Vector gradient = numericalGradient( + boost::bind(&computeError, gbn, _1), Vector10::Zero()); // Compute the gradient using dense matrices Matrix augmentedHessian = GaussianFactorGraph(gbn).augmentedHessian(); diff --git a/gtsam/linear/tests/testGaussianBayesTree.cpp b/gtsam/linear/tests/testGaussianBayesTree.cpp index 8f37cac0c..217fab543 100644 --- a/gtsam/linear/tests/testGaussianBayesTree.cpp +++ b/gtsam/linear/tests/testGaussianBayesTree.cpp @@ -24,7 +24,6 @@ using namespace boost::assign; #include -#include #include #include #include @@ -175,13 +174,13 @@ TEST(GaussianBayesTree, complicatedMarginal) { EXPECT(assert_equal(expectedCov, actualCov, 1e1)); } +/* ************************************************************************* */ +typedef Eigen::Matrix Vector10; namespace { - /* ************************************************************************* */ - double computeError(const GaussianBayesTree& gbt, const LieVector& values) - { - pair Rd = GaussianFactorGraph(gbt).jacobian(); - return 0.5 * (Rd.first * values - Rd.second).squaredNorm(); - } +double computeError(const GaussianBayesTree& gbt, const Vector10& values) { + pair Rd = GaussianFactorGraph(gbt).jacobian(); + return 0.5 * (Rd.first * values - Rd.second).squaredNorm(); +} } /* ************************************************************************* */ @@ -243,14 +242,12 @@ TEST(GaussianBayesTree, ComputeSteepestDescentPointBT) { 2, (Vector(4) << 1.0,2.0,15.0,16.0)))))); // Compute the Hessian numerically - Matrix hessian = numericalHessian( - boost::function(boost::bind(&computeError, bt, _1)), - LieVector(Vector::Zero(GaussianFactorGraph(bt).jacobian().first.cols()))); + Matrix hessian = numericalHessian( + boost::bind(&computeError, bt, _1), Vector10::Zero()); // Compute the gradient numerically - Vector gradient = numericalGradient( - boost::function(boost::bind(&computeError, bt, _1)), - LieVector(Vector::Zero(GaussianFactorGraph(bt).jacobian().first.cols()))); + Vector gradient = numericalGradient( + boost::bind(&computeError, bt, _1), Vector10::Zero()); // Compute the gradient using dense matrices Matrix augmentedHessian = GaussianFactorGraph(bt).augmentedHessian(); diff --git a/gtsam/linear/tests/testHessianFactor.cpp b/gtsam/linear/tests/testHessianFactor.cpp index 17ad0bf42..90a9cceda 100644 --- a/gtsam/linear/tests/testHessianFactor.cpp +++ b/gtsam/linear/tests/testHessianFactor.cpp @@ -276,8 +276,8 @@ TEST(HessianFactor, CombineAndEliminate) 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0); - Vector b0 = (Vector(3) << 1.5, 1.5, 1.5); - Vector s0 = (Vector(3) << 1.6, 1.6, 1.6); + Vector3 b0(1.5, 1.5, 1.5); + Vector3 s0(1.6, 1.6, 1.6); Matrix A10 = (Matrix(3,3) << 2.0, 0.0, 0.0, @@ -287,15 +287,15 @@ TEST(HessianFactor, CombineAndEliminate) -2.0, 0.0, 0.0, 0.0, -2.0, 0.0, 0.0, 0.0, -2.0); - Vector b1 = (Vector(3) << 2.5, 2.5, 2.5); - Vector s1 = (Vector(3) << 2.6, 2.6, 2.6); + Vector3 b1(2.5, 2.5, 2.5); + Vector3 s1(2.6, 2.6, 2.6); Matrix A21 = (Matrix(3,3) << 3.0, 0.0, 0.0, 0.0, 3.0, 0.0, 0.0, 0.0, 3.0); - Vector b2 = (Vector(3) << 3.5, 3.5, 3.5); - Vector s2 = (Vector(3) << 3.6, 3.6, 3.6); + Vector3 b2(3.5, 3.5, 3.5); + Vector3 s2(3.6, 3.6, 3.6); GaussianFactorGraph gfg; gfg.add(1, A01, b0, noiseModel::Diagonal::Sigmas(s0, true)); @@ -305,8 +305,8 @@ TEST(HessianFactor, CombineAndEliminate) Matrix zero3x3 = zeros(3,3); Matrix A0 = gtsam::stack(3, &A10, &zero3x3, &zero3x3); Matrix A1 = gtsam::stack(3, &A11, &A01, &A21); - Vector b = gtsam::concatVectors(3, &b1, &b0, &b2); - Vector sigmas = gtsam::concatVectors(3, &s1, &s0, &s2); + Vector9 b; b << b1, b0, b2; + Vector9 sigmas; sigmas << s1, s0, s2; // create a full, uneliminated version of the factor JacobianFactor expectedFactor(0, A0, 1, A1, b, noiseModel::Diagonal::Sigmas(sigmas, true)); diff --git a/gtsam/linear/tests/testJacobianFactor.cpp b/gtsam/linear/tests/testJacobianFactor.cpp index f70c3496a..1650df0ec 100644 --- a/gtsam/linear/tests/testJacobianFactor.cpp +++ b/gtsam/linear/tests/testJacobianFactor.cpp @@ -369,8 +369,8 @@ TEST(JacobianFactor, eliminate) 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0); - Vector b0 = (Vector(3) << 1.5, 1.5, 1.5); - Vector s0 = (Vector(3) << 1.6, 1.6, 1.6); + Vector3 b0(1.5, 1.5, 1.5); + Vector3 s0(1.6, 1.6, 1.6); Matrix A10 = (Matrix(3, 3) << 2.0, 0.0, 0.0, @@ -380,15 +380,15 @@ TEST(JacobianFactor, eliminate) -2.0, 0.0, 0.0, 0.0, -2.0, 0.0, 0.0, 0.0, -2.0); - Vector b1 = (Vector(3) << 2.5, 2.5, 2.5); - Vector s1 = (Vector(3) << 2.6, 2.6, 2.6); + Vector3 b1(2.5, 2.5, 2.5); + Vector3 s1(2.6, 2.6, 2.6); Matrix A21 = (Matrix(3, 3) << 3.0, 0.0, 0.0, 0.0, 3.0, 0.0, 0.0, 0.0, 3.0); - Vector b2 = (Vector(3) << 3.5, 3.5, 3.5); - Vector s2 = (Vector(3) << 3.6, 3.6, 3.6); + Vector3 b2(3.5, 3.5, 3.5); + Vector3 s2(3.6, 3.6, 3.6); GaussianFactorGraph gfg; gfg.add(1, A01, b0, noiseModel::Diagonal::Sigmas(s0, true)); @@ -398,8 +398,8 @@ TEST(JacobianFactor, eliminate) Matrix zero3x3 = zeros(3,3); Matrix A0 = gtsam::stack(3, &A10, &zero3x3, &zero3x3); Matrix A1 = gtsam::stack(3, &A11, &A01, &A21); - Vector b = gtsam::concatVectors(3, &b1, &b0, &b2); - Vector sigmas = gtsam::concatVectors(3, &s1, &s0, &s2); + Vector9 b; b << b1, b0, b2; + Vector9 sigmas; sigmas << s1, s0, s2; JacobianFactor combinedFactor(0, A0, 1, A1, b, noiseModel::Diagonal::Sigmas(sigmas, true)); GaussianFactorGraph::EliminationResult expected = combinedFactor.eliminate(list_of(0)); diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index ca0af7d27..590149e25 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -17,11 +17,10 @@ #pragma once /* GTSAM includes */ -#include -#include #include #include -#include +#include +#include #include /* External or standard includes */ @@ -46,7 +45,7 @@ namespace gtsam { * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: Computation of the Jacobian Matrices", Tech. Report, 2013. */ - class CombinedImuFactor: public NoiseModelFactor6 { + class CombinedImuFactor: public NoiseModelFactor6 { public: @@ -222,7 +221,7 @@ namespace gtsam { 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(boost::bind(&PreIntegrateIMUObservations_delta_angles, correctedOmega, deltaT, _1), thetaij); + // Matrix H_angles_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_angles, correctedOmega, deltaT, _1), thetaij); // overall Jacobian wrt preintegrated measurements (df/dx) Matrix F(15,15); @@ -339,7 +338,7 @@ namespace gtsam { private: typedef CombinedImuFactor This; - typedef NoiseModelFactor6 Base; + typedef NoiseModelFactor6 Base; CombinedPreintegratedMeasurements preintegratedMeasurements_; Vector3 gravity_; @@ -429,7 +428,7 @@ namespace gtsam { /** implement functions needed to derive from Factor */ /** vector of errors */ - Vector evaluateError(const Pose3& pose_i, const LieVector& vel_i, const Pose3& pose_j, const LieVector& vel_j, + 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 H1 = boost::none, boost::optional H2 = boost::none, @@ -643,7 +642,7 @@ namespace gtsam { /** predicted states from IMU */ - static void predict(const Pose3& pose_i, const LieVector& vel_i, Pose3& pose_j, LieVector& vel_j, + static void Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j, Vector3& vel_j, const imuBias::ConstantBias& bias_i, imuBias::ConstantBias& bias_j, const CombinedPreintegratedMeasurements& preintegratedMeasurements, const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional body_P_sensor = boost::none, @@ -666,7 +665,7 @@ namespace gtsam { - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper + 0.5 * gravity * deltaTij*deltaTij; - vel_j = LieVector(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij_ + vel_j = Vector3(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij_ + preintegratedMeasurements.delVdelBiasAcc * biasAccIncr + preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr) - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term diff --git a/gtsam/navigation/ImuBias.h b/gtsam/navigation/ImuBias.h index 32911e589..18713505e 100644 --- a/gtsam/navigation/ImuBias.h +++ b/gtsam/navigation/ImuBias.h @@ -39,7 +39,7 @@ namespace gtsam { /// All bias models live in the imuBias namespace namespace imuBias { - class ConstantBias : public DerivedValue { + class ConstantBias { private: Vector3 biasAcc_; Vector3 biasGyro_; @@ -144,7 +144,7 @@ namespace imuBias { /// return dimensionality of tangent space inline size_t dim() const { return dimension; } - /** Update the LieVector with a tangent space update */ + /** Update the bias with a tangent space update */ inline ConstantBias retract(const Vector& v) const { return ConstantBias(biasAcc_ + v.head(3), biasGyro_ + v.tail(3)); } /** @return the local coordinates of another object */ @@ -205,8 +205,7 @@ namespace imuBias { template void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("imuBias::ConstantBias", - boost::serialization::base_object(*this)); + ar & boost::serialization::make_nvp("imuBias::ConstantBias",*this); ar & BOOST_SERIALIZATION_NVP(biasAcc_); ar & BOOST_SERIALIZATION_NVP(biasGyro_); } @@ -218,6 +217,23 @@ namespace imuBias { } // namespace ImuBias +// Define GTSAM traits +namespace traits { + +template<> +struct is_group : public boost::true_type { +}; + +template<> +struct is_manifold : public boost::true_type { +}; + +template<> +struct dimension : public boost::integral_constant { +}; + +} + } // namespace gtsam diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index 6fe0ac256..f8c0806a5 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -21,7 +21,6 @@ #include #include #include -#include #include /* External or standard includes */ @@ -46,7 +45,7 @@ namespace gtsam { * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: Computation of the Jacobian Matrices", Tech. Report, 2013. */ - class ImuFactor: public NoiseModelFactor5 { + class ImuFactor: public NoiseModelFactor5 { public: @@ -223,13 +222,13 @@ namespace gtsam { 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(boost::bind(&PreIntegrateIMUObservations_delta_vel, correctedOmega, correctedAcc, deltaT, _1, deltaVij), theta_i); + // Matrix H_vel_angles = numericalDerivative11(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(boost::bind(&PreIntegrateIMUObservations_delta_angles, correctedOmega, deltaT, _1), thetaij); + // Matrix H_angles_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_angles, correctedOmega, deltaT, _1), thetaij); // overall Jacobian wrt preintegrated measurements (df/dx) Matrix F(9,9); @@ -315,7 +314,7 @@ namespace gtsam { private: typedef ImuFactor This; - typedef NoiseModelFactor5 Base; + typedef NoiseModelFactor5 Base; PreintegratedMeasurements preintegratedMeasurements_; Vector3 gravity_; @@ -403,7 +402,7 @@ namespace gtsam { /** implement functions needed to derive from Factor */ /** vector of errors */ - Vector evaluateError(const Pose3& pose_i, const LieVector& vel_i, const Pose3& pose_j, const LieVector& vel_j, + Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, const imuBias::ConstantBias& bias, boost::optional H1 = boost::none, boost::optional H2 = boost::none, @@ -555,7 +554,7 @@ namespace gtsam { /** predicted states from IMU */ - static void predict(const Pose3& pose_i, const LieVector& vel_i, Pose3& pose_j, LieVector& vel_j, + static void Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j, Vector3& vel_j, const imuBias::ConstantBias& bias, const PreintegratedMeasurements preintegratedMeasurements, const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional body_P_sensor = boost::none, const bool use2ndOrderCoriolis = false) @@ -577,7 +576,7 @@ namespace gtsam { - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper + 0.5 * gravity * deltaTij*deltaTij; - vel_j = LieVector(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij_ + vel_j = Vector3(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij_ + preintegratedMeasurements.delVdelBiasAcc * biasAccIncr + preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr) - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term diff --git a/gtsam/navigation/MagFactor.h b/gtsam/navigation/MagFactor.h index 0220b9509..96a0971c5 100644 --- a/gtsam/navigation/MagFactor.h +++ b/gtsam/navigation/MagFactor.h @@ -19,7 +19,6 @@ #include #include #include -#include namespace gtsam { @@ -61,7 +60,7 @@ public: static Point3 unrotate(const Rot2& R, const Point3& p, boost::optional HR = boost::none) { - Point3 q = Rot3::yaw(R.theta()).unrotate(p, HR); + Point3 q = Rot3::yaw(R.theta()).unrotate(p, HR, boost::none); if (HR) { // assign to temporary first to avoid error in Win-Debug mode Matrix H = HR->col(2); @@ -114,7 +113,7 @@ public: Vector evaluateError(const Rot3& nRb, boost::optional H = boost::none) const { // measured bM = nRbÕ * nM + b - Point3 hx = nRb.unrotate(nM_, H) + bias_; + Point3 hx = nRb.unrotate(nM_, H, boost::none) + bias_; return (hx - measured_).vector(); } }; @@ -165,7 +164,7 @@ public: * This version uses model measured bM = scale * bRn * direction + bias * and optimizes for both scale, direction, and the bias. */ -class MagFactor3: public NoiseModelFactor3 { +class MagFactor3: public NoiseModelFactor3 { const Point3 measured_; ///< The measured magnetometer values const Rot3 bRn_; ///< The assumed known rotation from nav to body @@ -175,7 +174,7 @@ public: /** Constructor */ MagFactor3(Key key1, Key key2, Key key3, const Point3& measured, const Rot3& nRb, const SharedNoiseModel& model) : - NoiseModelFactor3(model, key1, key2, key3), // + NoiseModelFactor3(model, key1, key2, key3), // measured_(measured), bRn_(nRb.inverse()) { } @@ -190,7 +189,7 @@ public: * @param nM (unknown) local earth magnetic field vector, in nav frame * @param bias (unknown) 3D bias */ - Vector evaluateError(const LieScalar& scale, const Unit3& direction, + Vector evaluateError(double scale, const Unit3& direction, const Point3& bias, boost::optional H1 = boost::none, boost::optional H2 = boost::none, boost::optional H3 = boost::none) const { diff --git a/gtsam/navigation/tests/testAttitudeFactor.cpp b/gtsam/navigation/tests/testAttitudeFactor.cpp index c4155ea16..790080556 100644 --- a/gtsam/navigation/tests/testAttitudeFactor.cpp +++ b/gtsam/navigation/tests/testAttitudeFactor.cpp @@ -45,7 +45,7 @@ TEST( Rot3AttitudeFactor, Constructor ) { EXPECT(assert_equal(zero(2),factor.evaluateError(nRb),1e-5)); // Calculate numerical derivatives - Matrix expectedH = numericalDerivative11( + Matrix expectedH = numericalDerivative11( boost::bind(&Rot3AttitudeFactor::evaluateError, &factor, _1, boost::none), nRb); @@ -78,7 +78,7 @@ TEST( Pose3AttitudeFactor, Constructor ) { EXPECT(assert_equal(zero(2),factor.evaluateError(T),1e-5)); // Calculate numerical derivatives - Matrix expectedH = numericalDerivative11( + Matrix expectedH = numericalDerivative11( boost::bind(&Pose3AttitudeFactor::evaluateError, &factor, _1, boost::none), T); diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index b6fb8442d..d307c1d8e 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -21,7 +21,6 @@ #include #include #include -#include #include #include #include @@ -143,9 +142,9 @@ TEST( CombinedImuFactor, ErrorWithBiases ) imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot) imuBias::ConstantBias bias2(Vector3(0.2, 0.2, 0), Vector3(1, 0, 0.3)); // Biases (acc, rot) Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0)); - LieVector v1((Vector(3) << 0.5, 0.0, 0.0)); + Vector3 v1(0.5, 0.0, 0.0); Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0)); - LieVector v2((Vector(3) << 0.5, 0.0, 0.0)); + Vector3 v2(0.5, 0.0, 0.0); // Measurements Vector3 gravity; gravity << 0, 0, 9.81; @@ -239,12 +238,12 @@ TEST( CombinedImuFactor, FirstOrderPreIntegratedMeasurements ) evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)); // Compute numerical derivatives - Matrix expectedDelPdelBias = numericalDerivative11( + Matrix expectedDelPdelBias = numericalDerivative11( boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias); Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3); Matrix expectedDelPdelBiasOmega = expectedDelPdelBias.rightCols(3); - Matrix expectedDelVdelBias = numericalDerivative11( + Matrix expectedDelVdelBias = numericalDerivative11( boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias); Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3); Matrix expectedDelVdelBiasOmega = expectedDelVdelBias.rightCols(3); diff --git a/gtsam/navigation/tests/testGPSFactor.cpp b/gtsam/navigation/tests/testGPSFactor.cpp index f02047aa9..8c93020c9 100644 --- a/gtsam/navigation/tests/testGPSFactor.cpp +++ b/gtsam/navigation/tests/testGPSFactor.cpp @@ -56,7 +56,7 @@ TEST( GPSFactor, Constructors ) { EXPECT(assert_equal(zero(3),factor.evaluateError(T),1e-5)); // Calculate numerical derivatives - Matrix expectedH = numericalDerivative11( + Matrix expectedH = numericalDerivative11( boost::bind(&GPSFactor::evaluateError, &factor, _1, boost::none), T); // Use the factor to calculate the derivative diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 14c49460a..d6e57521f 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -20,7 +20,6 @@ #include #include #include -#include #include #include #include @@ -39,14 +38,14 @@ using symbol_shorthand::B; /* ************************************************************************* */ namespace { Vector callEvaluateError(const ImuFactor& factor, - const Pose3& pose_i, const LieVector& vel_i, const Pose3& pose_j, const LieVector& vel_j, + const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, 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 LieVector& vel_i, const Pose3& pose_j, const LieVector& vel_j, + const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, const imuBias::ConstantBias& bias) { return Rot3::Expmap(factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias).tail(3) ) ; @@ -168,9 +167,9 @@ TEST( ImuFactor, Error ) // Linearization point imuBias::ConstantBias bias; // Bias Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0)); - LieVector v1((Vector(3) << 0.5, 0.0, 0.0)); + Vector3 v1((Vector(3) << 0.5, 0.0, 0.0)); Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/100.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0)); - LieVector v2((Vector(3) << 0.5, 0.0, 0.0)); + Vector3 v2((Vector(3) << 0.5, 0.0, 0.0)); // Measurements Vector3 gravity; gravity << 0, 0, 9.81; @@ -192,15 +191,15 @@ TEST( ImuFactor, Error ) EXPECT(assert_equal(errorExpected, errorActual, 1e-6)); // Expected Jacobians - Matrix H1e = numericalDerivative11( + Matrix H1e = numericalDerivative11( boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1); - Matrix H2e = numericalDerivative11( + Matrix H2e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1); - Matrix H3e = numericalDerivative11( + Matrix H3e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2); - Matrix H4e = numericalDerivative11( + Matrix H4e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2); - Matrix H5e = numericalDerivative11( + Matrix H5e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias); // Check rotation Jacobians @@ -240,16 +239,16 @@ TEST( ImuFactor, ErrorWithBiases ) // Linearization point // Vector bias(6); bias << 0.2, 0, 0, 0.1, 0, 0; // Biases (acc, rot) // Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0)); -// LieVector v1((Vector(3) << 0.5, 0.0, 0.0)); +// Vector3 v1((Vector(3) << 0.5, 0.0, 0.0)); // 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)); -// LieVector v2((Vector(3) << 0.5, 0.0, 0.0)); +// Vector3 v2((Vector(3) << 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)); - LieVector v1((Vector(3) << 0.5, 0.0, 0.0)); + Vector3 v1((Vector(3) << 0.5, 0.0, 0.0)); Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0)); - LieVector v2((Vector(3) << 0.5, 0.0, 0.0)); + Vector3 v2((Vector(3) << 0.5, 0.0, 0.0)); // Measurements Vector3 gravity; gravity << 0, 0, 9.81; @@ -276,15 +275,15 @@ TEST( ImuFactor, ErrorWithBiases ) // EXPECT(assert_equal(errorExpected, errorActual, 1e-6)); // Expected Jacobians - Matrix H1e = numericalDerivative11( + Matrix H1e = numericalDerivative11( boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1); - Matrix H2e = numericalDerivative11( + Matrix H2e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1); - Matrix H3e = numericalDerivative11( + Matrix H3e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2); - Matrix H4e = numericalDerivative11( + Matrix H4e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2); - Matrix H5e = numericalDerivative11( + Matrix H5e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias); // Check rotation Jacobians @@ -318,8 +317,8 @@ TEST( ImuFactor, PartialDerivativeExpmap ) // Compute numerical derivatives - Matrix expectedDelRdelBiasOmega = numericalDerivative11(boost::bind( - &evaluateRotation, measuredOmega, _1, deltaT), LieVector(biasOmega)); + Matrix expectedDelRdelBiasOmega = numericalDerivative11(boost::bind( + &evaluateRotation, measuredOmega, _1, deltaT), Vector3(biasOmega)); const Matrix3 Jr = Rot3::rightJacobianExpMapSO3((measuredOmega - biasOmega) * deltaT); @@ -341,8 +340,8 @@ TEST( ImuFactor, PartialDerivativeLogmap ) // Compute numerical derivatives - Matrix expectedDelFdeltheta = numericalDerivative11(boost::bind( - &evaluateLogRotation, thetahat, _1), LieVector(deltatheta)); + Matrix expectedDelFdeltheta = numericalDerivative11(boost::bind( + &evaluateLogRotation, thetahat, _1), Vector3(deltatheta)); const Vector3 x = thetahat; // parametrization of so(3) const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^ @@ -417,12 +416,12 @@ TEST( ImuFactor, FirstOrderPreIntegratedMeasurements ) evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)); // Compute numerical derivatives - Matrix expectedDelPdelBias = numericalDerivative11( + Matrix expectedDelPdelBias = numericalDerivative11( boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias); Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3); Matrix expectedDelPdelBiasOmega = expectedDelPdelBias.rightCols(3); - Matrix expectedDelVdelBias = numericalDerivative11( + Matrix expectedDelVdelBias = numericalDerivative11( boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias); Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3); Matrix expectedDelVdelBiasOmega = expectedDelVdelBias.rightCols(3); @@ -447,9 +446,9 @@ TEST( ImuFactor, FirstOrderPreIntegratedMeasurements ) //{ // // Linearization point // Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0)); -// LieVector v1((Vector(3) << 0.5, 0.0, 0.0)); +// Vector3 v1((Vector(3) << 0.5, 0.0, 0.0)); // Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/100.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0)); -// LieVector v2((Vector(3) << 0.5, 0.0, 0.0)); +// Vector3 v2((Vector(3) << 0.5, 0.0, 0.0)); // imuBias::ConstantBias bias(Vector3(0.001, 0.002, 0.008), Vector3(0.002, 0.004, 0.012)); // // // Pre-integrator @@ -503,9 +502,9 @@ TEST( ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement ) 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)); - LieVector v1((Vector(3) << 0.5, 0.0, 0.0)); + Vector3 v1((Vector(3) << 0.5, 0.0, 0.0)); Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0)); - LieVector v2((Vector(3) << 0.5, 0.0, 0.0)); + Vector3 v2((Vector(3) << 0.5, 0.0, 0.0)); // Measurements Vector3 gravity; gravity << 0, 0, 9.81; @@ -531,15 +530,15 @@ TEST( ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement ) ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis); // Expected Jacobians - Matrix H1e = numericalDerivative11( + Matrix H1e = numericalDerivative11( boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1); - Matrix H2e = numericalDerivative11( + Matrix H2e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1); - Matrix H3e = numericalDerivative11( + Matrix H3e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2); - Matrix H4e = numericalDerivative11( + Matrix H4e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2); - Matrix H5e = numericalDerivative11( + Matrix H5e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias); // Check rotation Jacobians diff --git a/gtsam/navigation/tests/testMagFactor.cpp b/gtsam/navigation/tests/testMagFactor.cpp index 1875e4f1c..5599c93d6 100644 --- a/gtsam/navigation/tests/testMagFactor.cpp +++ b/gtsam/navigation/tests/testMagFactor.cpp @@ -72,35 +72,35 @@ TEST( MagFactor, Factors ) { // MagFactor MagFactor f(1, measured, s, dir, bias, model); EXPECT( assert_equal(zero(3),f.evaluateError(theta,H1),1e-5)); - EXPECT( assert_equal(numericalDerivative11 // + EXPECT( assert_equal(numericalDerivative11 // (boost::bind(&MagFactor::evaluateError, &f, _1, none), theta), H1, 1e-7)); // MagFactor1 MagFactor1 f1(1, measured, s, dir, bias, model); EXPECT( assert_equal(zero(3),f1.evaluateError(nRb,H1),1e-5)); - EXPECT( assert_equal(numericalDerivative11 // + EXPECT( assert_equal(numericalDerivative11 // (boost::bind(&MagFactor1::evaluateError, &f1, _1, none), nRb), H1, 1e-7)); // MagFactor2 MagFactor2 f2(1, 2, measured, nRb, model); EXPECT( assert_equal(zero(3),f2.evaluateError(scaled,bias,H1,H2),1e-5)); - EXPECT( assert_equal(numericalDerivative11 // + EXPECT( assert_equal(numericalDerivative11 // (boost::bind(&MagFactor2::evaluateError, &f2, _1, bias, none, none), scaled),// H1, 1e-7)); - EXPECT( assert_equal(numericalDerivative11 // + EXPECT( assert_equal(numericalDerivative11 // (boost::bind(&MagFactor2::evaluateError, &f2, scaled, _1, none, none), bias),// H2, 1e-7)); // MagFactor2 MagFactor3 f3(1, 2, 3, measured, nRb, model); EXPECT(assert_equal(zero(3),f3.evaluateError(s,dir,bias,H1,H2,H3),1e-5)); - EXPECT(assert_equal(numericalDerivative11 // + EXPECT(assert_equal(numericalDerivative11 // (boost::bind(&MagFactor3::evaluateError, &f3, _1, dir, bias, none, none, none), s),// H1, 1e-7)); - EXPECT(assert_equal(numericalDerivative11 // + EXPECT(assert_equal(numericalDerivative11 // (boost::bind(&MagFactor3::evaluateError, &f3, s, _1, bias, none, none, none), dir),// H2, 1e-7)); - EXPECT(assert_equal(numericalDerivative11 // + EXPECT(assert_equal(numericalDerivative11 // (boost::bind(&MagFactor3::evaluateError, &f3, s, dir, _1, none, none, none), bias),// H3, 1e-7)); } diff --git a/gtsam/nonlinear/NonlinearFactor.cpp b/gtsam/nonlinear/NonlinearFactor.cpp new file mode 100644 index 000000000..e9b97d644 --- /dev/null +++ b/gtsam/nonlinear/NonlinearFactor.cpp @@ -0,0 +1,145 @@ +/* ---------------------------------------------------------------------------- + + * 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 NonlinearFactor.cpp + * @brief Nonlinear Factor base classes + * @author Frank Dellaert + * @author Richard Roberts + */ + +#include +#include +#include + +namespace gtsam { + +/* ************************************************************************* */ +void NonlinearFactor::print(const std::string& s, + const KeyFormatter& keyFormatter) const { + std::cout << s << " keys = { "; + BOOST_FOREACH(Key key, keys()) { + std::cout << keyFormatter(key) << " "; + } + std::cout << "}" << std::endl; +} + +/* ************************************************************************* */ +bool NonlinearFactor::equals(const NonlinearFactor& f, double tol) const { + return Base::equals(f); +} + +/* ************************************************************************* */ +NonlinearFactor::shared_ptr NonlinearFactor::rekey( + const std::map& rekey_mapping) const { + shared_ptr new_factor = clone(); + for (size_t i = 0; i < new_factor->size(); ++i) { + Key& cur_key = new_factor->keys()[i]; + std::map::const_iterator mapping = rekey_mapping.find(cur_key); + if (mapping != rekey_mapping.end()) + cur_key = mapping->second; + } + return new_factor; +} + +/* ************************************************************************* */ +NonlinearFactor::shared_ptr NonlinearFactor::rekey( + const std::vector& new_keys) const { + assert(new_keys.size() == keys().size()); + shared_ptr new_factor = clone(); + new_factor->keys() = new_keys; + return new_factor; +} + +/* ************************************************************************* */ +void NoiseModelFactor::print(const std::string& s, + const KeyFormatter& keyFormatter) const { + Base::print(s, keyFormatter); + if (noiseModel_) + noiseModel_->print(" noise model: "); +} + +/* ************************************************************************* */ +bool NoiseModelFactor::equals(const NonlinearFactor& f, double tol) const { + const NoiseModelFactor* e = dynamic_cast(&f); + return e && Base::equals(f, tol) + && ((!noiseModel_ && !e->noiseModel_) + || (noiseModel_ && e->noiseModel_ + && noiseModel_->equals(*e->noiseModel_, tol))); +} + +/* ************************************************************************* */ +static void check(const SharedNoiseModel& noiseModel, size_t m) { + if (noiseModel && m != noiseModel->dim()) + throw std::invalid_argument( + boost::str( + boost::format( + "NoiseModelFactor: NoiseModel has dimension %1% instead of %2%.") + % noiseModel->dim() % m)); +} + +/* ************************************************************************* */ +Vector NoiseModelFactor::whitenedError(const Values& c) const { + const Vector b = unwhitenedError(c); + check(noiseModel_, b.size()); + return noiseModel_ ? noiseModel_->whiten(b) : b; +} + +/* ************************************************************************* */ +double NoiseModelFactor::error(const Values& c) const { + if (active(c)) { + const Vector b = unwhitenedError(c); + check(noiseModel_, b.size()); + if (noiseModel_) + return 0.5 * noiseModel_->distance(b); + else + return 0.5 * b.squaredNorm(); + } else { + return 0.0; + } +} + +/* ************************************************************************* */ +boost::shared_ptr NoiseModelFactor::linearize( + const Values& x) const { + + // Only linearize if the factor is active + if (!active(x)) + return boost::shared_ptr(); + + // Call evaluate error to get Jacobians and RHS vector b + std::vector A(size()); + Vector b = -unwhitenedError(x, A); + check(noiseModel_, b.size()); + + // Whiten the corresponding system now + if (noiseModel_) + noiseModel_->WhitenSystem(A, b); + + // Fill in terms, needed to create JacobianFactor below + std::vector > terms(size()); + for (size_t j = 0; j < size(); ++j) { + terms[j].first = keys()[j]; + terms[j].second.swap(A[j]); + } + + // TODO pass unwhitened + noise model to Gaussian factor + if (noiseModel_ && noiseModel_->is_constrained()) + return GaussianFactor::shared_ptr( + new JacobianFactor(terms, b, + boost::static_pointer_cast(noiseModel_)->unit())); + else + return GaussianFactor::shared_ptr(new JacobianFactor(terms, b)); +} + +/* ************************************************************************* */ + +} // \namespace gtsam diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index e43346ca8..8f50bc91f 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -43,12 +43,10 @@ namespace gtsam { using boost::assign::cref_list_of; /* ************************************************************************* */ + /** * Nonlinear factor base class * - * Templated on a values structure type. The values structures are typically - * more general than just vectors, e.g., Rot3 or Pose3, - * which are objects in non-linear manifolds (Lie groups). * \nosubgrouping */ class NonlinearFactor: public Factor { @@ -82,18 +80,10 @@ public: /** print */ virtual void print(const std::string& s = "", - const KeyFormatter& keyFormatter = DefaultKeyFormatter) const - { - std::cout << s << " keys = { "; - BOOST_FOREACH(Key key, this->keys()) { std::cout << keyFormatter(key) << " "; } - std::cout << "}" << std::endl; - } + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; /** Check if two factors are equal */ - virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const { - return Base::equals(f); - } - + virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const; /// @} /// @name Standard Interface /// @{ @@ -128,17 +118,6 @@ public: virtual boost::shared_ptr linearize(const Values& c) const = 0; - /** - * Create a symbolic factor using the given ordering to determine the - * variable indices. - */ - //virtual IndexFactor::shared_ptr symbolic(const Ordering& ordering) const { - // std::vector indices(this->size()); - // for(size_t j=0; jsize(); ++j) - // indices[j] = ordering[this->keys()[j]]; - // return IndexFactor::shared_ptr(new IndexFactor(indices)); - //} - /** * Creates a shared_ptr clone of the factor - needs to be specialized to allow * for subclasses @@ -155,28 +134,13 @@ public: * Creates a shared_ptr clone of the factor with different keys using * a map from old->new keys */ - shared_ptr rekey(const std::map& rekey_mapping) const { - shared_ptr new_factor = clone(); - for (size_t i=0; isize(); ++i) { - Key& cur_key = new_factor->keys()[i]; - std::map::const_iterator mapping = rekey_mapping.find(cur_key); - if (mapping != rekey_mapping.end()) - cur_key = mapping->second; - } - return new_factor; - } + shared_ptr rekey(const std::map& rekey_mapping) const; /** * Clones a factor and fully replaces its keys * @param new_keys is the full replacement set of keys */ - shared_ptr rekey(const std::vector& new_keys) const { - assert(new_keys.size() == this->keys().size()); - shared_ptr new_factor = clone(); - new_factor->keys() = new_keys; - return new_factor; - } - + shared_ptr rekey(const std::vector& new_keys) const; }; // \class NonlinearFactor @@ -229,19 +193,10 @@ public: /** Print */ virtual void print(const std::string& s = "", - const KeyFormatter& keyFormatter = DefaultKeyFormatter) const - { - Base::print(s, keyFormatter); - this->noiseModel_->print(" noise model: "); - } + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; /** Check if two factors are equal */ - virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const { - const NoiseModelFactor* e = dynamic_cast(&f); - return e && Base::equals(f, tol) && - ((!noiseModel_ && !e->noiseModel_) || - (noiseModel_ && e->noiseModel_ && noiseModel_->equals(*e->noiseModel_, tol))); - } + virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const; /** get the dimension of the factor (number of rows on linearization) */ virtual size_t dim() const { @@ -256,21 +211,17 @@ public: /** * Error function *without* the NoiseModel, \f$ z-h(x) \f$. * Override this method to finish implementing an N-way factor. - * If any of the optional Matrix reference arguments are specified, it should compute - * both the function evaluation and its derivative(s) in X1 (and/or X2, X3...). + * If the optional arguments is specified, it should compute + * both the function evaluation and its derivative(s) in H. */ - virtual Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const = 0; + virtual Vector unwhitenedError(const Values& x, + boost::optional&> H = boost::none) const = 0; /** * Vector of errors, whitened * This is the raw error, i.e., i.e. \f$ (h(x)-z)/\sigma \f$ in case of a Gaussian */ - Vector whitenedError(const Values& c) const { - const Vector unwhitenedErrorVec = unwhitenedError(c); - if((size_t) unwhitenedErrorVec.size() != noiseModel_->dim()) - throw std::invalid_argument("This factor was created with a NoiseModel of incorrect dimension."); - return noiseModel_->whiten(unwhitenedErrorVec); - } + Vector whitenedError(const Values& c) const; /** * Calculate the error of the factor. @@ -278,60 +229,14 @@ public: * In this class, we take the raw prediction error \f$ h(x)-z \f$, ask the noise model * to transform it to \f$ (h(x)-z)^2/\sigma^2 \f$, and then multiply by 0.5. */ - virtual double error(const Values& c) const { - if (this->active(c)) { - const Vector unwhitenedErrorVec = unwhitenedError(c); - if((size_t) unwhitenedErrorVec.size() != noiseModel_->dim()) - throw std::invalid_argument("This factor was created with a NoiseModel of incorrect dimension."); - return 0.5 * noiseModel_->distance(unwhitenedErrorVec); - } else { - return 0.0; - } - } + virtual double error(const Values& c) const; /** * Linearize a non-linearFactorN to get a GaussianFactor, * \f$ Ax-b \approx h(x+\delta x)-z = h(x) + A \delta x - z \f$ * Hence \f$ b = z - h(x) = - \mathtt{error\_vector}(x) \f$ */ - boost::shared_ptr linearize(const Values& x) const { - // Only linearize if the factor is active - if (!this->active(x)) - return boost::shared_ptr(); - - // Create the set of terms - Jacobians for each index - Vector b; - // Call evaluate error to get Jacobians and b vector - std::vector A(this->size()); - b = -unwhitenedError(x, A); - if(noiseModel_) - { - if((size_t) b.size() != noiseModel_->dim()) - throw std::invalid_argument("This factor was created with a NoiseModel of incorrect dimension."); - - this->noiseModel_->WhitenSystem(A,b); - } - - std::vector > terms(this->size()); - // Fill in terms - for(size_t j=0; jsize(); ++j) { - terms[j].first = this->keys()[j]; - terms[j].second.swap(A[j]); - } - - if(noiseModel_) - { - // TODO pass unwhitened + noise model to Gaussian factor - noiseModel::Constrained::shared_ptr constrained = - boost::dynamic_pointer_cast(this->noiseModel_); - if(constrained) - return GaussianFactor::shared_ptr(new JacobianFactor(terms, b, constrained->unit())); - else - return GaussianFactor::shared_ptr(new JacobianFactor(terms, b)); - } - else - return GaussianFactor::shared_ptr(new JacobianFactor(terms, b)); - } + boost::shared_ptr linearize(const Values& x) const; private: @@ -348,8 +253,15 @@ private: /* ************************************************************************* */ -/** A convenient base class for creating your own NoiseModelFactor with 1 - * variable. To derive from this class, implement evaluateError(). */ + +/** + * A convenient base class for creating your own NoiseModelFactor with 1 + * variable. To derive from this class, implement evaluateError(). + * + * Templated on a values structure type. The values structures are typically + * more general than just vectors, e.g., Rot3 or Pose3, + * which are objects in non-linear manifolds (Lie groups). + */ template class NoiseModelFactor1: public NoiseModelFactor { diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index 7b812551e..0d559cfe6 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -33,6 +33,7 @@ namespace gtsam { + /* ************************************************************************* */ template struct _ValuesKeyValuePair { @@ -52,6 +53,36 @@ namespace gtsam { _ValuesConstKeyValuePair(const _ValuesKeyValuePair& rhs) : key(rhs.key), value(rhs.value) {} }; + /* ************************************************************************* */ + + // Cast helpers for making _Values[Const]KeyValuePair's from Values::[Const]KeyValuePair + // need to use a struct here for later partial specialization + template + struct ValuesCastHelper { + static CastedKeyValuePairType cast(KeyValuePairType key_value) { + // Static cast because we already checked the type during filtering + return CastedKeyValuePairType(key_value.key, const_cast&>(static_cast&>(key_value.value)).value()); + } + }; + // partial specialized version for ValueType == Value + template + struct ValuesCastHelper { + static CastedKeyValuePairType cast(KeyValuePairType key_value) { + // Static cast because we already checked the type during filtering + // in this case the casted and keyvalue pair are essentially the same type (key, Value&) so perhaps this could be done with just a cast of the key_value? + return CastedKeyValuePairType(key_value.key, key_value.value); + } + }; + // partial specialized version for ValueType == Value + template + struct ValuesCastHelper { + static CastedKeyValuePairType cast(KeyValuePairType key_value) { + // Static cast because we already checked the type during filtering + // in this case the casted and keyvalue pair are essentially the same type (key, Value&) so perhaps this could be done with just a cast of the key_value? + return CastedKeyValuePairType(key_value.key, key_value.value); + } + }; + /* ************************************************************************* */ template class Values::Filtered { @@ -99,19 +130,19 @@ namespace gtsam { begin_(boost::make_transform_iterator( boost::make_filter_iterator( filter, values.begin(), values.end()), - &castHelper)), + &ValuesCastHelper::cast)), end_(boost::make_transform_iterator( boost::make_filter_iterator( filter, values.end(), values.end()), - &castHelper)), + &ValuesCastHelper::cast)), constBegin_(boost::make_transform_iterator( boost::make_filter_iterator( filter, ((const Values&)values).begin(), ((const Values&)values).end()), - &castHelper)), + &ValuesCastHelper::cast)), constEnd_(boost::make_transform_iterator( boost::make_filter_iterator( filter, ((const Values&)values).end(), ((const Values&)values).end()), - &castHelper)) {} + &ValuesCastHelper::cast)) {} friend class Values; iterator begin_; @@ -175,7 +206,7 @@ namespace gtsam { Values::Values(const Values::Filtered& view) { BOOST_FOREACH(const typename Filtered::KeyValuePair& key_value, view) { Key key = key_value.key; - insert(key, key_value.value); + insert(key, static_cast(key_value.value)); } } @@ -184,7 +215,7 @@ namespace gtsam { Values::Values(const Values::ConstFiltered& view) { BOOST_FOREACH(const typename ConstFiltered::KeyValuePair& key_value, view) { Key key = key_value.key; - insert(key, key_value.value); + insert(key, key_value.value); } } @@ -214,6 +245,13 @@ namespace gtsam { return ConstFiltered(boost::bind(&filterHelper, filterFcn, _1), *this); } + /* ************************************************************************* */ + template<> + inline bool Values::filterHelper(const boost::function filter, const ConstKeyValuePair& key_value) { + // Filter and check the type + return filter(key_value.key); + } + /* ************************************************************************* */ template const ValueType& Values::at(Key j) const { @@ -225,11 +263,11 @@ namespace gtsam { throw ValuesKeyDoesNotExist("retrieve", j); // Check the type and throw exception if incorrect - if(typeid(*item->second) != typeid(ValueType)) + try { + return dynamic_cast&>(*item->second).value(); + } catch (std::bad_cast &) { throw ValuesIncorrectType(j, typeid(*item->second), typeid(ValueType)); - - // We have already checked the type, so do a "blind" static_cast, not dynamic_cast - return static_cast(*item->second); + } } /* ************************************************************************* */ @@ -239,15 +277,52 @@ namespace gtsam { KeyValueMap::const_iterator item = values_.find(j); if(item != values_.end()) { - // Check the type and throw exception if incorrect - if(typeid(*item->second) != typeid(ValueType)) + // dynamic cast the type and throw exception if incorrect + try { + return dynamic_cast&>(*item->second).value(); + } catch (std::bad_cast &) { throw ValuesIncorrectType(j, typeid(*item->second), typeid(ValueType)); - - // We have already checked the type, so do a "blind" static_cast, not dynamic_cast - return static_cast(*item->second); - } else { + } + } else { return boost::none; } } + /* ************************************************************************* */ + // insert a plain value using the default chart + template + void Values::insert(Key j, const ValueType& val) { + insert(j, static_cast(ChartValue >(val))); + } + + // insert with custom chart type + template + void Values::insert(Key j, const ValueType& val) { + insert(j, static_cast(ChartValue(val))); + } + + // overloaded insert with chart initializer + template + void Values::insert(Key j, const ValueType& val, ChartInit chart) { + insert(j, static_cast(ChartValue(val, chart))); + } + + // update with default chart + template + void Values::update(Key j, const ValueType& val) { + update(j, static_cast(ChartValue >(val))); + } + + // update with custom chart + template + void Values::update(Key j, const ValueType& val) { + update(j, static_cast(ChartValue(val))); + } + + // update with chart initializer, /todo: perhaps there is a way to init chart from old value... + template + void Values::update(Key j, const ValueType& val, ChartInit chart) { + update(j, static_cast(ChartValue(val, chart))); + } + } diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index 811846f79..e4a27849d 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -44,7 +44,7 @@ #include #include -#include +#include #include #include @@ -251,6 +251,22 @@ namespace gtsam { /** Add a set of variables, throws KeyAlreadyExists if a key is already present */ void insert(const Values& values); + /** Templated version to add a variable with the given j, + * throws KeyAlreadyExists if j is already present + * if no chart is specified, the DefaultChart is used + */ + template + void insert(Key j, const ValueType& val); + + /// overloaded insert version that also specifies a chart + template + void insert(Key j, const ValueType& val); + + /// overloaded insert version that also specifies a chart initializer + template + void insert(Key j, const ValueType& val, ChartInit chart); + + /** insert that mimics the STL map insert - if the value already exists, the map is not modified * and an iterator to the existing value is returned, along with 'false'. If the value did not * exist, it is inserted and an iterator pointing to the new element, along with 'true', is @@ -260,6 +276,21 @@ namespace gtsam { /** single element change of existing element */ void update(Key j, const Value& val); + /** Templated version to update a variable with the given j, + * throws KeyAlreadyExists if j is already present + * if no chart is specified, the DefaultChart is used + */ + template + void update(Key j, const T& val); + + /// overloaded insert version that also specifies a chart + template + void update(Key j, const T& val); + + /// overloaded insert version that also specifies a chart initializer + template + void update(Key j, const T& val, ChartInit chart); + /** update the current available values without adding new ones */ void update(const Values& values); @@ -369,15 +400,9 @@ namespace gtsam { // supplied \c filter function. template static bool filterHelper(const boost::function filter, const ConstKeyValuePair& key_value) { + BOOST_STATIC_ASSERT((!boost::is_same::value)); // Filter and check the type - return filter(key_value.key) && (typeid(ValueType) == typeid(key_value.value) || typeid(ValueType) == typeid(Value)); - } - - // Cast to the derived ValueType - template - static CastedKeyValuePairType castHelper(KeyValuePairType key_value) { - // Static cast because we already checked the type during filtering - return CastedKeyValuePairType(key_value.key, static_cast(key_value.value)); + return filter(key_value.key) && (dynamic_cast*>(&key_value.value)); } /** Serialization function */ diff --git a/gtsam/nonlinear/WhiteNoiseFactor.h b/gtsam/nonlinear/WhiteNoiseFactor.h index 852ad147c..a7987d73b 100644 --- a/gtsam/nonlinear/WhiteNoiseFactor.h +++ b/gtsam/nonlinear/WhiteNoiseFactor.h @@ -19,7 +19,6 @@ #include #include -#include #include namespace gtsam { @@ -126,7 +125,7 @@ namespace gtsam { /// Calculate the error of the factor, typically equal to log-likelihood inline double error(const Values& x) const { - return f(z_, x.at(meanKey_), x.at(precisionKey_)); + return f(z_, x.at(meanKey_), x.at(precisionKey_)); } /** @@ -155,8 +154,8 @@ namespace gtsam { /// linearize returns a Hessianfactor that is an approximation of error(p) virtual boost::shared_ptr linearize(const Values& x) const { - double u = x.at(meanKey_); - double p = x.at(precisionKey_); + double u = x.at(meanKey_); + double p = x.at(precisionKey_); Key j1 = meanKey_; Key j2 = precisionKey_; return linearize(z_, u, p, j1, j2); diff --git a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp index ee2f0d793..a8f287d1e 100644 --- a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp +++ b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp @@ -32,20 +32,37 @@ using namespace gtsam::serializationTestHelpers; /* ************************************************************************* */ // Export all classes derived from Value -BOOST_CLASS_EXPORT(gtsam::Cal3_S2) -BOOST_CLASS_EXPORT(gtsam::Cal3Bundler) -BOOST_CLASS_EXPORT(gtsam::Point3) -BOOST_CLASS_EXPORT(gtsam::Pose3) -BOOST_CLASS_EXPORT(gtsam::Rot3) -BOOST_CLASS_EXPORT(gtsam::PinholeCamera) -BOOST_CLASS_EXPORT(gtsam::PinholeCamera) -BOOST_CLASS_EXPORT(gtsam::PinholeCamera) +BOOST_CLASS_EXPORT(gtsam::Cal3_S2); +BOOST_CLASS_EXPORT(gtsam::Cal3Bundler); +BOOST_CLASS_EXPORT(gtsam::Point3); +BOOST_CLASS_EXPORT(gtsam::Pose3); +BOOST_CLASS_EXPORT(gtsam::Rot3); +BOOST_CLASS_EXPORT(gtsam::PinholeCamera); +BOOST_CLASS_EXPORT(gtsam::PinholeCamera); +BOOST_CLASS_EXPORT(gtsam::PinholeCamera); + +namespace detail { +template struct pack { + typedef T type; +}; +} +#define CHART_VALUE_EXPORT(UNIQUE_NAME, TYPE) \ + typedef gtsam::ChartValue > UNIQUE_NAME; \ + BOOST_CLASS_EXPORT( UNIQUE_NAME ); + /* ************************************************************************* */ typedef PinholeCamera PinholeCal3S2; typedef PinholeCamera PinholeCal3DS2; typedef PinholeCamera PinholeCal3Bundler; +CHART_VALUE_EXPORT(gtsamPoint3Chart, gtsam::Point3); +CHART_VALUE_EXPORT(Cal3S2Chart, PinholeCal3S2); +CHART_VALUE_EXPORT(Cal3DS2Chart, PinholeCal3DS2); +CHART_VALUE_EXPORT(Cal3BundlerChart, PinholeCal3Bundler); + + + /* ************************************************************************* */ static Point3 pt3(1.0, 2.0, 3.0); static Rot3 rt3 = Rot3::RzRyRx(1.0, 3.0, 2.0); @@ -56,12 +73,27 @@ static Cal3DS2 cal2(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0); static Cal3Bundler cal3(1.0, 2.0, 3.0); TEST (Serialization, TemplatedValues) { + std::cout << __LINE__ << std::endl; + EXPECT(equalsObj(pt3)); + std::cout << __LINE__ << std::endl; + ChartValue chv1(pt3); + std::cout << __LINE__ << std::endl; + EXPECT(equalsObj(chv1)); + std::cout << __LINE__ << std::endl; + PinholeCal3S2 pc(pose3,cal1); + EXPECT(equalsObj(pc)); + std::cout << __LINE__ << std::endl; Values values; + values.insert(1,pt3); + std::cout << __LINE__ << std::endl; + EXPECT(equalsObj(values)); + std::cout << __LINE__ << std::endl; values.insert(Symbol('a',0), PinholeCal3S2(pose3, cal1)); values.insert(Symbol('s',5), PinholeCal3DS2(pose3, cal2)); values.insert(Symbol('d',47), PinholeCal3Bundler(pose3, cal3)); values.insert(Symbol('a',5), PinholeCal3S2(pose3, cal1)); EXPECT(equalsObj(values)); + std::cout << __LINE__ << std::endl; EXPECT(equalsXML(values)); EXPECT(equalsBinary(values)); } diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index 041ea0387..60639e8b7 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -21,7 +21,6 @@ #include #include #include -#include #include #include // for operator += @@ -38,7 +37,7 @@ static double inf = std::numeric_limits::infinity(); using symbol_shorthand::X; using symbol_shorthand::L; -const Symbol key1('v',1), key2('v',2), key3('v',3), key4('v',4); +const Symbol key1('v', 1), key2('v', 2), key3('v', 3), key4('v', 4); class TestValueData { @@ -51,34 +50,43 @@ public: }; int TestValueData::ConstructorCount = 0; int TestValueData::DestructorCount = 0; -class TestValue : public DerivedValue { +class TestValue { TestValueData data_; public: - virtual void print(const std::string& str = "") const {} + void print(const std::string& str = "") const {} bool equals(const TestValue& other, double tol = 1e-9) const { return true; } - virtual size_t dim() const { return 0; } + size_t dim() const { return 0; } TestValue retract(const Vector&) const { return TestValue(); } Vector localCoordinates(const TestValue&) const { return Vector(); } }; +namespace gtsam { +namespace traits { +template <> +struct is_manifold : public boost::true_type {}; +template <> +struct dimension : public boost::integral_constant {}; +} +} + /* ************************************************************************* */ TEST( Values, equals1 ) { Values expected; - LieVector v((Vector(3) << 5.0, 6.0, 7.0)); + Vector3 v(5.0, 6.0, 7.0); - expected.insert(key1,v); + expected.insert(key1, v); Values actual; - actual.insert(key1,v); - CHECK(assert_equal(expected,actual)); + actual.insert(key1, v); + CHECK(assert_equal(expected, actual)); } /* ************************************************************************* */ TEST( Values, equals2 ) { Values cfg1, cfg2; - LieVector v1((Vector(3) << 5.0, 6.0, 7.0)); - LieVector v2((Vector(3) << 5.0, 6.0, 8.0)); + Vector3 v1(5.0, 6.0, 7.0); + Vector3 v2(5.0, 6.0, 8.0); cfg1.insert(key1, v1); cfg2.insert(key1, v2); @@ -90,8 +98,8 @@ TEST( Values, equals2 ) TEST( Values, equals_nan ) { Values cfg1, cfg2; - LieVector v1((Vector(3) << 5.0, 6.0, 7.0)); - LieVector v2((Vector(3) << inf, inf, inf)); + Vector3 v1(5.0, 6.0, 7.0); + Vector3 v2(inf, inf, inf); cfg1.insert(key1, v1); cfg2.insert(key1, v2); @@ -103,10 +111,10 @@ TEST( Values, equals_nan ) TEST( Values, insert_good ) { Values cfg1, cfg2, expected; - LieVector v1((Vector(3) << 5.0, 6.0, 7.0)); - LieVector v2((Vector(3) << 8.0, 9.0, 1.0)); - LieVector v3((Vector(3) << 2.0, 4.0, 3.0)); - LieVector v4((Vector(3) << 8.0, 3.0, 7.0)); + Vector3 v1(5.0, 6.0, 7.0); + Vector3 v2(8.0, 9.0, 1.0); + Vector3 v3(2.0, 4.0, 3.0); + Vector3 v4(8.0, 3.0, 7.0); cfg1.insert(key1, v1); cfg1.insert(key2, v2); @@ -125,10 +133,10 @@ TEST( Values, insert_good ) TEST( Values, insert_bad ) { Values cfg1, cfg2; - LieVector v1((Vector(3) << 5.0, 6.0, 7.0)); - LieVector v2((Vector(3) << 8.0, 9.0, 1.0)); - LieVector v3((Vector(3) << 2.0, 4.0, 3.0)); - LieVector v4((Vector(3) << 8.0, 3.0, 7.0)); + Vector3 v1(5.0, 6.0, 7.0); + Vector3 v2(8.0, 9.0, 1.0); + Vector3 v3(2.0, 4.0, 3.0); + Vector3 v4(8.0, 3.0, 7.0); cfg1.insert(key1, v1); cfg1.insert(key2, v2); @@ -142,16 +150,16 @@ TEST( Values, insert_bad ) TEST( Values, update_element ) { Values cfg; - LieVector v1((Vector(3) << 5.0, 6.0, 7.0)); - LieVector v2((Vector(3) << 8.0, 9.0, 1.0)); + Vector3 v1(5.0, 6.0, 7.0); + Vector3 v2(8.0, 9.0, 1.0); cfg.insert(key1, v1); CHECK(cfg.size() == 1); - CHECK(assert_equal(v1, cfg.at(key1))); + CHECK(assert_equal((Vector)v1, cfg.at(key1))); cfg.update(key1, v2); CHECK(cfg.size() == 1); - CHECK(assert_equal(v2, cfg.at(key1))); + CHECK(assert_equal((Vector)v2, cfg.at(key1))); } /* ************************************************************************* */ @@ -159,10 +167,10 @@ TEST(Values, basic_functions) { Values values; const Values& values_c = values; - values.insert(2, LieVector()); - values.insert(4, LieVector()); - values.insert(6, LieVector()); - values.insert(8, LieVector()); + values.insert(2, Vector3()); + values.insert(4, Vector3()); + values.insert(6, Vector3()); + values.insert(8, Vector3()); // find EXPECT_LONGS_EQUAL(4, values.find(4)->key); @@ -186,8 +194,8 @@ TEST(Values, basic_functions) //TEST(Values, dim_zero) //{ // Values config0; -// config0.insert(key1, LieVector((Vector(2) << 2.0, 3.0)); -// config0.insert(key2, LieVector((Vector(3) << 5.0, 6.0, 7.0)); +// config0.insert(key1, Vector2((Vector(2) << 2.0, 3.0)); +// config0.insert(key2, Vector3(5.0, 6.0, 7.0)); // LONGS_EQUAL(5, config0.dim()); // // VectorValues expected; @@ -200,16 +208,16 @@ TEST(Values, basic_functions) TEST(Values, expmap_a) { Values config0; - config0.insert(key1, LieVector((Vector(3) << 1.0, 2.0, 3.0))); - config0.insert(key2, LieVector((Vector(3) << 5.0, 6.0, 7.0))); + config0.insert(key1, Vector3(1.0, 2.0, 3.0)); + config0.insert(key2, Vector3(5.0, 6.0, 7.0)); VectorValues increment = pair_list_of (key1, (Vector(3) << 1.0, 1.1, 1.2)) (key2, (Vector(3) << 1.3, 1.4, 1.5)); Values expected; - expected.insert(key1, LieVector((Vector(3) << 2.0, 3.1, 4.2))); - expected.insert(key2, LieVector((Vector(3) << 6.3, 7.4, 8.5))); + expected.insert(key1, Vector3(2.0, 3.1, 4.2)); + expected.insert(key2, Vector3(6.3, 7.4, 8.5)); CHECK(assert_equal(expected, config0.retract(increment))); } @@ -218,15 +226,15 @@ TEST(Values, expmap_a) TEST(Values, expmap_b) { Values config0; - config0.insert(key1, LieVector((Vector(3) << 1.0, 2.0, 3.0))); - config0.insert(key2, LieVector((Vector(3) << 5.0, 6.0, 7.0))); + config0.insert(key1, Vector3(1.0, 2.0, 3.0)); + config0.insert(key2, Vector3(5.0, 6.0, 7.0)); VectorValues increment = pair_list_of (key2, (Vector(3) << 1.3, 1.4, 1.5)); Values expected; - expected.insert(key1, LieVector((Vector(3) << 1.0, 2.0, 3.0))); - expected.insert(key2, LieVector((Vector(3) << 6.3, 7.4, 8.5))); + expected.insert(key1, Vector3(1.0, 2.0, 3.0)); + expected.insert(key2, Vector3(6.3, 7.4, 8.5)); CHECK(assert_equal(expected, config0.retract(increment))); } @@ -235,16 +243,16 @@ TEST(Values, expmap_b) //TEST(Values, expmap_c) //{ // Values config0; -// config0.insert(key1, LieVector((Vector(3) << 1.0, 2.0, 3.0))); -// config0.insert(key2, LieVector((Vector(3) << 5.0, 6.0, 7.0))); +// config0.insert(key1, Vector3(1.0, 2.0, 3.0)); +// config0.insert(key2, Vector3(5.0, 6.0, 7.0)); // -// Vector increment = LieVector(6, +// Vector increment = Vector6( // 1.0, 1.1, 1.2, // 1.3, 1.4, 1.5); // // Values expected; -// expected.insert(key1, LieVector((Vector(3) << 2.0, 3.1, 4.2))); -// expected.insert(key2, LieVector((Vector(3) << 6.3, 7.4, 8.5))); +// expected.insert(key1, Vector3(2.0, 3.1, 4.2)); +// expected.insert(key2, Vector3(6.3, 7.4, 8.5)); // // CHECK(assert_equal(expected, config0.retract(increment))); //} @@ -253,14 +261,14 @@ TEST(Values, expmap_b) TEST(Values, expmap_d) { Values config0; - config0.insert(key1, LieVector((Vector(3) << 1.0, 2.0, 3.0))); - config0.insert(key2, LieVector((Vector(3) << 5.0, 6.0, 7.0))); + config0.insert(key1, Vector3(1.0, 2.0, 3.0)); + config0.insert(key2, Vector3(5.0, 6.0, 7.0)); //config0.print("config0"); CHECK(equal(config0, config0)); CHECK(config0.equals(config0)); Values poseconfig; - poseconfig.insert(key1, Pose2(1,2,3)); + poseconfig.insert(key1, Pose2(1, 2, 3)); poseconfig.insert(key2, Pose2(0.3, 0.4, 0.5)); CHECK(equal(config0, config0)); @@ -271,8 +279,8 @@ TEST(Values, expmap_d) TEST(Values, localCoordinates) { Values valuesA; - valuesA.insert(key1, LieVector((Vector(3) << 1.0, 2.0, 3.0))); - valuesA.insert(key2, LieVector((Vector(3) << 5.0, 6.0, 7.0))); + valuesA.insert(key1, Vector3(1.0, 2.0, 3.0)); + valuesA.insert(key2, Vector3(5.0, 6.0, 7.0)); VectorValues expDelta = pair_list_of (key1, (Vector(3) << 0.1, 0.2, 0.3)) @@ -308,29 +316,29 @@ TEST(Values, extract_keys) TEST(Values, exists_) { Values config0; - config0.insert(key1, LieVector((Vector(1) << 1.))); - config0.insert(key2, LieVector((Vector(1) << 2.))); + config0.insert(key1, 1.0); + config0.insert(key2, 2.0); - boost::optional v = config0.exists(key1); - CHECK(assert_equal((Vector(1) << 1.),*v)); + boost::optional v = config0.exists(key1); + DOUBLES_EQUAL(1.0,*v,1e-9); } /* ************************************************************************* */ TEST(Values, update) { Values config0; - config0.insert(key1, LieVector((Vector(1) << 1.))); - config0.insert(key2, LieVector((Vector(1) << 2.))); + config0.insert(key1, 1.0); + config0.insert(key2, 2.0); Values superset; - superset.insert(key1, LieVector((Vector(1) << -1.))); - superset.insert(key2, LieVector((Vector(1) << -2.))); + superset.insert(key1, -1.0); + superset.insert(key2, -2.0); config0.update(superset); Values expected; - expected.insert(key1, LieVector((Vector(1) << -1.))); - expected.insert(key2, LieVector((Vector(1) << -2.))); - CHECK(assert_equal(expected,config0)); + expected.insert(key1, -1.0); + expected.insert(key2, -2.0); + CHECK(assert_equal(expected, config0)); } /* ************************************************************************* */ @@ -353,12 +361,14 @@ TEST(Values, filter) { BOOST_FOREACH(const Values::Filtered<>::KeyValuePair& key_value, filtered) { if(i == 0) { LONGS_EQUAL(2, (long)key_value.key); - EXPECT(typeid(Pose2) == typeid(key_value.value)); - EXPECT(assert_equal(pose2, dynamic_cast(key_value.value))); + try {key_value.value.cast();} catch (const std::bad_cast& e) { FAIL("can't cast Value to Pose2");} + THROWS_EXCEPTION(key_value.value.cast()); + EXPECT(assert_equal(pose2, key_value.value.cast())); } else if(i == 1) { LONGS_EQUAL(3, (long)key_value.key); - EXPECT(typeid(Pose3) == typeid(key_value.value)); - EXPECT(assert_equal(pose3, dynamic_cast(key_value.value))); + try {key_value.value.cast();} catch (const std::bad_cast& e) { FAIL("can't cast Value to Pose3");} + THROWS_EXCEPTION(key_value.value.cast()); + EXPECT(assert_equal(pose3, key_value.value.cast())); } else { EXPECT(false); } @@ -408,18 +418,18 @@ TEST(Values, Symbol_filter) { Values values; values.insert(X(0), pose0); - values.insert(Symbol('y',1), pose1); + values.insert(Symbol('y', 1), pose1); values.insert(X(2), pose2); - values.insert(Symbol('y',3), pose3); + values.insert(Symbol('y', 3), pose3); int i = 0; BOOST_FOREACH(const Values::Filtered::KeyValuePair& key_value, values.filter(Symbol::ChrTest('y'))) { if(i == 0) { LONGS_EQUAL(Symbol('y', 1), (long)key_value.key); - EXPECT(assert_equal(pose1, dynamic_cast(key_value.value))); + EXPECT(assert_equal(pose1, key_value.value.cast())); } else if(i == 1) { LONGS_EQUAL(Symbol('y', 3), (long)key_value.key); - EXPECT(assert_equal(pose3, dynamic_cast(key_value.value))); + EXPECT(assert_equal(pose3, key_value.value.cast())); } else { EXPECT(false); } @@ -441,11 +451,15 @@ TEST(Values, Destructors) { values.insert(0, value1); values.insert(1, value2); } - LONGS_EQUAL(4, (long)TestValueData::ConstructorCount); - LONGS_EQUAL(2, (long)TestValueData::DestructorCount); + // additional 2 con/destructor counts for the temporary + // GenericValue in insert() + // but I'm sure some advanced programmer can figure out + // a way to avoid the temporary, or optimize it out + LONGS_EQUAL(4+2, (long)TestValueData::ConstructorCount); + LONGS_EQUAL(2+2, (long)TestValueData::DestructorCount); } - LONGS_EQUAL(4, (long)TestValueData::ConstructorCount); - LONGS_EQUAL(4, (long)TestValueData::DestructorCount); + LONGS_EQUAL(4+2, (long)TestValueData::ConstructorCount); + LONGS_EQUAL(4+2, (long)TestValueData::DestructorCount); } /* ************************************************************************* */ diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h index a910b3e35..9d4a8e6e5 100644 --- a/gtsam/slam/EssentialMatrixFactor.h +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -10,7 +10,6 @@ #include #include #include -#include #include namespace gtsam { @@ -87,14 +86,13 @@ public: * Binary factor that optimizes for E and inverse depth d: assumes measurement * in image 2 is perfect, and returns re-projection error in image 1 */ -class EssentialMatrixFactor2: public NoiseModelFactor2 { +class EssentialMatrixFactor2: public NoiseModelFactor2 { Point3 dP1_; ///< 3D point corresponding to measurement in image 1 Point2 pn_; ///< Measurement in image 2, in ideal coordinates double f_; ///< approximate conversion factor for error scaling - typedef NoiseModelFactor2 Base; + typedef NoiseModelFactor2 Base; typedef EssentialMatrixFactor2 This; public: @@ -155,7 +153,7 @@ public: * @param E essential matrix * @param d inverse depth d */ - Vector evaluateError(const EssentialMatrix& E, const LieScalar& d, + Vector evaluateError(const EssentialMatrix& E, const double& d, boost::optional DE = boost::none, boost::optional Dd = boost::none) const { @@ -180,12 +178,14 @@ public: } else { // Calculate derivatives. TODO if slow: optimize with Mathematica - // 3*2 3*3 3*3 2*3 - Matrix D_1T2_dir, DdP2_rot, DP2_point, Dpn_dP2; + // 3*2 3*3 3*3 + Matrix D_1T2_dir, DdP2_rot, DP2_point; Point3 _1T2 = E.direction().point3(D_1T2_dir); Point3 d1T2 = d * _1T2; Point3 dP2 = E.rotation().unrotate(dP1_ - d1T2, DdP2_rot, DP2_point); + + Matrix23 Dpn_dP2; pn = SimpleCamera::project_to_camera(dP2, Dpn_dP2); if (DE) { @@ -245,7 +245,8 @@ public: */ template EssentialMatrixFactor3(Key key1, Key key2, const Point2& pA, const Point2& pB, - const Rot3& cRb, const SharedNoiseModel& model, boost::shared_ptr K) : + const Rot3& cRb, const SharedNoiseModel& model, + boost::shared_ptr K) : EssentialMatrixFactor2(key1, key2, pA, pB, model, K), cRb_(cRb) { } @@ -267,7 +268,7 @@ public: * @param E essential matrix * @param d inverse depth d */ - Vector evaluateError(const EssentialMatrix& E, const LieScalar& d, + Vector evaluateError(const EssentialMatrix& E, const double& d, boost::optional DE = boost::none, boost::optional Dd = boost::none) const { if (!DE) { diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h index 3e093c7c4..db37e6b8d 100644 --- a/gtsam/slam/ProjectionFactor.h +++ b/gtsam/slam/ProjectionFactor.h @@ -132,17 +132,17 @@ namespace gtsam { if(H1) { gtsam::Matrix H0; PinholeCamera camera(pose.compose(*body_P_sensor_, H0), *K_); - Point2 reprojectionError(camera.project(point, H1, H2) - measured_); + Point2 reprojectionError(camera.project(point, H1, H2, boost::none) - measured_); *H1 = *H1 * H0; return reprojectionError.vector(); } else { PinholeCamera camera(pose.compose(*body_P_sensor_), *K_); - Point2 reprojectionError(camera.project(point, H1, H2) - measured_); + Point2 reprojectionError(camera.project(point, H1, H2, boost::none) - measured_); return reprojectionError.vector(); } } else { PinholeCamera camera(pose, *K_); - Point2 reprojectionError(camera.project(point, H1, H2) - measured_); + Point2 reprojectionError(camera.project(point, H1, H2, boost::none) - measured_); return reprojectionError.vector(); } } catch( CheiralityException& e) { diff --git a/gtsam/slam/RegularImplicitSchurFactor.h b/gtsam/slam/RegularImplicitSchurFactor.h index f1a11e2cd..ecf8adfec 100644 --- a/gtsam/slam/RegularImplicitSchurFactor.h +++ b/gtsam/slam/RegularImplicitSchurFactor.h @@ -29,7 +29,6 @@ public: protected: typedef Eigen::Matrix Matrix2D; ///< type of an F block - typedef Eigen::Matrix Matrix23; typedef Eigen::Matrix MatrixDD; ///< camera hessian typedef std::pair KeyMatrix2D; ///< named F block @@ -203,7 +202,7 @@ public: // F'*(I - E*P*E')*F, TODO: this should work, but it does not :-( // static const Eigen::Matrix I2 = eye(2); - // Eigen::Matrix Q = // + // Matrix2 Q = // // I2 - E_.block<2, 3>(2 * pos, 0) * PointCovariance_ * E_.block<2, 3>(2 * pos, 0).transpose(); // blocks[j] = Fj.transpose() * Q * Fj; } diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 0bf5f4f5b..b6f04d449 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -1,5 +1,4 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved @@ -349,8 +348,9 @@ void save2D(const NonlinearFactorGraph& graph, const Values& config, fstream stream(filename.c_str(), fstream::out); // save poses + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, config) { - const Pose2& pose = dynamic_cast(key_value.value); + const Pose2& pose = key_value.value.cast(); stream << "VERTEX2 " << key_value.key << " " << pose.x() << " " << pose.y() << " " << pose.theta() << endl; } @@ -392,25 +392,22 @@ GraphAndValues readG2o(const string& g2oFile, const bool is3D, /* ************************************************************************* */ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate, const string& filename) { - fstream stream(filename.c_str(), fstream::out); // save 2D & 3D poses - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, estimate) { + Values::ConstFiltered viewPose2 = estimate.filter(); + BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, viewPose2) { + stream << "VERTEX_SE2 " << key_value.key << " " << key_value.value.x() << " " + << key_value.value.y() << " " << key_value.value.theta() << endl; + } - const Pose2* pose2D = dynamic_cast(&key_value.value); - if(pose2D){ - stream << "VERTEX_SE2 " << key_value.key << " " << pose2D->x() << " " - << pose2D->y() << " " << pose2D->theta() << endl; - } - const Pose3* pose3D = dynamic_cast(&key_value.value); - if(pose3D){ - Point3 p = pose3D->translation(); - Rot3 R = pose3D->rotation(); + Values::ConstFiltered viewPose3 = estimate.filter(); + BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, viewPose3) { + Point3 p = key_value.value.translation(); + Rot3 R = key_value.value.rotation(); stream << "VERTEX_SE3:QUAT " << key_value.key << " " << p.x() << " " << p.y() << " " << p.z() << " " << R.toQuaternion().x() << " " << R.toQuaternion().y() << " " << R.toQuaternion().z() << " " << R.toQuaternion().w() << endl; - } } // save edges (2D or 3D) diff --git a/gtsam/slam/tests/testEssentialMatrixConstraint.cpp b/gtsam/slam/tests/testEssentialMatrixConstraint.cpp index 5184393ac..4c0edf5c9 100644 --- a/gtsam/slam/tests/testEssentialMatrixConstraint.cpp +++ b/gtsam/slam/tests/testEssentialMatrixConstraint.cpp @@ -50,10 +50,10 @@ TEST( EssentialMatrixConstraint, test ) { CHECK(assert_equal(expected, actual, 1e-8)); // Calculate numerical derivatives - Matrix expectedH1 = numericalDerivative11( + Matrix expectedH1 = numericalDerivative11( boost::bind(&EssentialMatrixConstraint::evaluateError, &factor, _1, pose2, boost::none, boost::none), pose1); - Matrix expectedH2 = numericalDerivative11( + Matrix expectedH2 = numericalDerivative11( boost::bind(&EssentialMatrixConstraint::evaluateError, &factor, pose1, _1, boost::none, boost::none), pose2); diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index 1e5674599..46e283d34 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -36,7 +36,7 @@ SfM_data data; bool readOK = readBAL(filename, data); Rot3 c1Rc2 = data.cameras[1].pose().rotation(); Point3 c1Tc2 = data.cameras[1].pose().translation(); -PinholeCamera camera2(data.cameras[1].pose(),Cal3_S2()); +PinholeCamera camera2(data.cameras[1].pose(), Cal3_S2()); EssentialMatrix trueE(c1Rc2, Unit3(c1Tc2)); double baseline = 0.1; // actual baseline of the camera @@ -96,7 +96,7 @@ TEST (EssentialMatrixFactor, factor) { // Use numerical derivatives to calculate the expected Jacobian Matrix Hexpected; - Hexpected = numericalDerivative11( + Hexpected = numericalDerivative11( boost::bind(&EssentialMatrixFactor::evaluateError, &factor, _1, boost::none), trueE); @@ -164,17 +164,17 @@ TEST (EssentialMatrixFactor2, factor) { Vector expected = reprojectionError.vector(); Matrix Hactual1, Hactual2; - LieScalar d(baseline / P1.z()); + double d(baseline / P1.z()); Vector actual = factor.evaluateError(trueE, d, Hactual1, Hactual2); EXPECT(assert_equal(expected, actual, 1e-7)); // Use numerical derivatives to calculate the expected Jacobian Matrix Hexpected1, Hexpected2; - boost::function f = - boost::bind(&EssentialMatrixFactor2::evaluateError, &factor, _1, _2, - boost::none, boost::none); - Hexpected1 = numericalDerivative21(f, trueE, d); - Hexpected2 = numericalDerivative22(f, trueE, d); + boost::function f = boost::bind( + &EssentialMatrixFactor2::evaluateError, &factor, _1, _2, boost::none, + boost::none); + Hexpected1 = numericalDerivative21(f, trueE, d); + Hexpected2 = numericalDerivative22(f, trueE, d); // Verify the Jacobian is correct EXPECT(assert_equal(Hexpected1, Hactual1, 1e-8)); @@ -197,7 +197,7 @@ TEST (EssentialMatrixFactor2, minimization) { truth.insert(100, trueE); for (size_t i = 0; i < 5; i++) { Point3 P1 = data.tracks[i].p; - truth.insert(i, LieScalar(baseline / P1.z())); + truth.insert(i, double(baseline / P1.z())); } EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8); @@ -211,7 +211,7 @@ TEST (EssentialMatrixFactor2, minimization) { EssentialMatrix actual = result.at(100); EXPECT(assert_equal(trueE, actual, 1e-1)); for (size_t i = 0; i < 5; i++) - EXPECT(assert_equal(truth.at(i), result.at(i), 1e-1)); + EXPECT_DOUBLES_EQUAL(truth.at(i), result.at(i), 1e-1); // Check error at result EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4); @@ -238,17 +238,17 @@ TEST (EssentialMatrixFactor3, factor) { Vector expected = reprojectionError.vector(); Matrix Hactual1, Hactual2; - LieScalar d(baseline / P1.z()); + double d(baseline / P1.z()); Vector actual = factor.evaluateError(bodyE, d, Hactual1, Hactual2); EXPECT(assert_equal(expected, actual, 1e-7)); // Use numerical derivatives to calculate the expected Jacobian Matrix Hexpected1, Hexpected2; - boost::function f = - boost::bind(&EssentialMatrixFactor3::evaluateError, &factor, _1, _2, - boost::none, boost::none); - Hexpected1 = numericalDerivative21(f, bodyE, d); - Hexpected2 = numericalDerivative22(f, bodyE, d); + boost::function f = boost::bind( + &EssentialMatrixFactor3::evaluateError, &factor, _1, _2, boost::none, + boost::none); + Hexpected1 = numericalDerivative21(f, bodyE, d); + Hexpected2 = numericalDerivative22(f, bodyE, d); // Verify the Jacobian is correct EXPECT(assert_equal(Hexpected1, Hactual1, 1e-8)); @@ -270,7 +270,7 @@ TEST (EssentialMatrixFactor3, minimization) { truth.insert(100, bodyE); for (size_t i = 0; i < 5; i++) { Point3 P1 = data.tracks[i].p; - truth.insert(i, LieScalar(baseline / P1.z())); + truth.insert(i, double(baseline / P1.z())); } EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8); @@ -284,7 +284,7 @@ TEST (EssentialMatrixFactor3, minimization) { EssentialMatrix actual = result.at(100); EXPECT(assert_equal(bodyE, actual, 1e-1)); for (size_t i = 0; i < 5; i++) - EXPECT(assert_equal(truth.at(i), result.at(i), 1e-1)); + EXPECT_DOUBLES_EQUAL(truth.at(i), result.at(i), 1e-1); // Check error at result EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4); @@ -314,7 +314,7 @@ Point2 pB(size_t i) { boost::shared_ptr // K = boost::make_shared(500, 0, 0); -PinholeCamera camera2(data.cameras[1].pose(),*K); +PinholeCamera camera2(data.cameras[1].pose(), *K); Vector vA(size_t i) { Point2 xy = K->calibrate(pA(i)); @@ -380,17 +380,17 @@ TEST (EssentialMatrixFactor2, extraTest) { Vector expected = reprojectionError.vector(); Matrix Hactual1, Hactual2; - LieScalar d(baseline / P1.z()); + double d(baseline / P1.z()); Vector actual = factor.evaluateError(trueE, d, Hactual1, Hactual2); EXPECT(assert_equal(expected, actual, 1e-7)); // Use numerical derivatives to calculate the expected Jacobian Matrix Hexpected1, Hexpected2; - boost::function f = - boost::bind(&EssentialMatrixFactor2::evaluateError, &factor, _1, _2, - boost::none, boost::none); - Hexpected1 = numericalDerivative21(f, trueE, d); - Hexpected2 = numericalDerivative22(f, trueE, d); + boost::function f = boost::bind( + &EssentialMatrixFactor2::evaluateError, &factor, _1, _2, boost::none, + boost::none); + Hexpected1 = numericalDerivative21(f, trueE, d); + Hexpected2 = numericalDerivative22(f, trueE, d); // Verify the Jacobian is correct EXPECT(assert_equal(Hexpected1, Hactual1, 1e-6)); @@ -413,7 +413,7 @@ TEST (EssentialMatrixFactor2, extraMinimization) { truth.insert(100, trueE); for (size_t i = 0; i < data.number_tracks(); i++) { Point3 P1 = data.tracks[i].p; - truth.insert(i, LieScalar(baseline / P1.z())); + truth.insert(i, double(baseline / P1.z())); } EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8); @@ -427,7 +427,7 @@ TEST (EssentialMatrixFactor2, extraMinimization) { EssentialMatrix actual = result.at(100); EXPECT(assert_equal(trueE, actual, 1e-1)); for (size_t i = 0; i < data.number_tracks(); i++) - EXPECT(assert_equal(truth.at(i), result.at(i), 1e-1)); + EXPECT_DOUBLES_EQUAL(truth.at(i), result.at(i), 1e-1); // Check error at result EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4); @@ -449,17 +449,17 @@ TEST (EssentialMatrixFactor3, extraTest) { Vector expected = reprojectionError.vector(); Matrix Hactual1, Hactual2; - LieScalar d(baseline / P1.z()); + double d(baseline / P1.z()); Vector actual = factor.evaluateError(bodyE, d, Hactual1, Hactual2); EXPECT(assert_equal(expected, actual, 1e-7)); // Use numerical derivatives to calculate the expected Jacobian Matrix Hexpected1, Hexpected2; - boost::function f = - boost::bind(&EssentialMatrixFactor3::evaluateError, &factor, _1, _2, - boost::none, boost::none); - Hexpected1 = numericalDerivative21(f, bodyE, d); - Hexpected2 = numericalDerivative22(f, bodyE, d); + boost::function f = boost::bind( + &EssentialMatrixFactor3::evaluateError, &factor, _1, _2, boost::none, + boost::none); + Hexpected1 = numericalDerivative21(f, bodyE, d); + Hexpected2 = numericalDerivative22(f, bodyE, d); // Verify the Jacobian is correct EXPECT(assert_equal(Hexpected1, Hactual1, 1e-6)); diff --git a/gtsam/slam/tests/testPoseRotationPrior.cpp b/gtsam/slam/tests/testPoseRotationPrior.cpp index c9e4acaf7..cbe8efa30 100644 --- a/gtsam/slam/tests/testPoseRotationPrior.cpp +++ b/gtsam/slam/tests/testPoseRotationPrior.cpp @@ -38,13 +38,13 @@ const Rot2 rot2A, rot2B = Rot2::fromAngle(M_PI_2); const Rot2 rot2C = Rot2::fromAngle(M_PI-0.01), rot2D = Rot2::fromAngle(M_PI+0.01); /* ************************************************************************* */ -LieVector evalFactorError3(const Pose3RotationPrior& factor, const Pose3& x) { - return LieVector(factor.evaluateError(x)); +Vector evalFactorError3(const Pose3RotationPrior& factor, const Pose3& x) { + return factor.evaluateError(x); } /* ************************************************************************* */ -LieVector evalFactorError2(const Pose2RotationPrior& factor, const Pose2& x) { - return LieVector(factor.evaluateError(x)); +Vector evalFactorError2(const Pose2RotationPrior& factor, const Pose2& x) { + return factor.evaluateError(x); } /* ************************************************************************* */ @@ -53,8 +53,7 @@ TEST( testPoseRotationFactor, level3_zero_error ) { Pose3RotationPrior factor(poseKey, rot3A, model3); Matrix actH1; EXPECT(assert_equal(zero(3), factor.evaluateError(pose1, actH1))); - Matrix expH1 = numericalDerivative11( - boost::bind(evalFactorError3, factor, _1), pose1, 1e-5); + Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } @@ -68,13 +67,10 @@ TEST( testPoseRotationFactor, level3_error ) { #else EXPECT(assert_equal((Vector(3) << -0.1, -0.2, -0.3), factor.evaluateError(pose1, actH1),1e-2)); #endif + Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5); // the derivative is more complex, but is close to the identity for Rot3 around the origin - /* - Matrix expH1 = numericalDerivative11( - boost::bind(evalFactorError3, factor, _1), pose1, 1e-2); - EXPECT(assert_equal(expH1, actH1, tol));*/ // If not using true expmap will be close, but not exact around the origin - + // EXPECT(assert_equal(expH1, actH1, tol)); } /* ************************************************************************* */ @@ -83,8 +79,7 @@ TEST( testPoseRotationFactor, level2_zero_error ) { Pose2RotationPrior factor(poseKey, rot2A, model1); Matrix actH1; EXPECT(assert_equal(zero(1), factor.evaluateError(pose1, actH1))); - Matrix expH1 = numericalDerivative11( - boost::bind(evalFactorError2, factor, _1), pose1, 1e-5); + Matrix expH1 = numericalDerivative22(evalFactorError2, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } @@ -94,8 +89,7 @@ TEST( testPoseRotationFactor, level2_error ) { Pose2RotationPrior factor(poseKey, rot2B, model1); Matrix actH1; EXPECT(assert_equal((Vector(1) << -M_PI_2), factor.evaluateError(pose1, actH1))); - Matrix expH1 = numericalDerivative11( - boost::bind(evalFactorError2, factor, _1), pose1, 1e-5); + Matrix expH1 = numericalDerivative22(evalFactorError2, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } @@ -105,8 +99,7 @@ TEST( testPoseRotationFactor, level2_error_wrap ) { Pose2RotationPrior factor(poseKey, rot2D, model1); Matrix actH1; EXPECT(assert_equal((Vector(1) << -0.02), factor.evaluateError(pose1, actH1))); - Matrix expH1 = numericalDerivative11( - boost::bind(evalFactorError2, factor, _1), pose1, 1e-5); + Matrix expH1 = numericalDerivative22(evalFactorError2, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } diff --git a/gtsam/slam/tests/testPoseTranslationPrior.cpp b/gtsam/slam/tests/testPoseTranslationPrior.cpp index 36d94c9a3..47ff708d9 100644 --- a/gtsam/slam/tests/testPoseTranslationPrior.cpp +++ b/gtsam/slam/tests/testPoseTranslationPrior.cpp @@ -34,13 +34,13 @@ const Point2 point2A(1.0, 2.0), point2B(4.0, 6.0); const Rot2 rot2A, rot2B = Rot2::fromAngle(M_PI_2); /* ************************************************************************* */ -LieVector evalFactorError3(const Pose3TranslationPrior& factor, const Pose3& x) { - return LieVector(factor.evaluateError(x)); +Vector evalFactorError3(const Pose3TranslationPrior& factor, const Pose3& x) { + return factor.evaluateError(x); } /* ************************************************************************* */ -LieVector evalFactorError2(const Pose2TranslationPrior& factor, const Pose2& x) { - return LieVector(factor.evaluateError(x)); +Vector evalFactorError2(const Pose2TranslationPrior& factor, const Pose2& x) { + return factor.evaluateError(x); } /* ************************************************************************* */ @@ -49,8 +49,7 @@ TEST( testPoseTranslationFactor, level3_zero_error ) { Pose3TranslationPrior factor(poseKey, point3A, model3); Matrix actH1; EXPECT(assert_equal(zero(3), factor.evaluateError(pose1, actH1))); - Matrix expH1 = numericalDerivative11( - boost::bind(evalFactorError3, factor, _1), pose1, 1e-5); + Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } @@ -60,8 +59,7 @@ TEST( testPoseTranslationFactor, level3_error ) { Pose3TranslationPrior factor(poseKey, point3B, model3); Matrix actH1; EXPECT(assert_equal((Vector(3) << -3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1))); - Matrix expH1 = numericalDerivative11( - boost::bind(evalFactorError3, factor, _1), pose1, 1e-5); + Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } @@ -71,8 +69,7 @@ TEST( testPoseTranslationFactor, pitched3_zero_error ) { Pose3TranslationPrior factor(poseKey, point3A, model3); Matrix actH1; EXPECT(assert_equal(zero(3), factor.evaluateError(pose1, actH1))); - Matrix expH1 = numericalDerivative11( - boost::bind(evalFactorError3, factor, _1), pose1, 1e-5); + Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } @@ -82,8 +79,7 @@ TEST( testPoseTranslationFactor, pitched3_error ) { Pose3TranslationPrior factor(poseKey, point3B, model3); Matrix actH1; EXPECT(assert_equal((Vector(3) << -3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1))); - Matrix expH1 = numericalDerivative11( - boost::bind(evalFactorError3, factor, _1), pose1, 1e-5); + Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } @@ -93,8 +89,7 @@ TEST( testPoseTranslationFactor, smallrot3_zero_error ) { Pose3TranslationPrior factor(poseKey, point3A, model3); Matrix actH1; EXPECT(assert_equal(zero(3), factor.evaluateError(pose1, actH1))); - Matrix expH1 = numericalDerivative11( - boost::bind(evalFactorError3, factor, _1), pose1, 1e-5); + Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } @@ -104,8 +99,7 @@ TEST( testPoseTranslationFactor, smallrot3_error ) { Pose3TranslationPrior factor(poseKey, point3B, model3); Matrix actH1; EXPECT(assert_equal((Vector(3) << -3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1))); - Matrix expH1 = numericalDerivative11( - boost::bind(evalFactorError3, factor, _1), pose1, 1e-5); + Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } @@ -115,8 +109,7 @@ TEST( testPoseTranslationFactor, level2_zero_error ) { Pose2TranslationPrior factor(poseKey, point2A, model2); Matrix actH1; EXPECT(assert_equal(zero(2), factor.evaluateError(pose1, actH1))); - Matrix expH1 = numericalDerivative11( - boost::bind(evalFactorError2, factor, _1), pose1, 1e-5); + Matrix expH1 = numericalDerivative22(evalFactorError2, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } @@ -126,8 +119,7 @@ TEST( testPoseTranslationFactor, level2_error ) { Pose2TranslationPrior factor(poseKey, point2B, model2); Matrix actH1; EXPECT(assert_equal((Vector(2) << -3.0,-4.0), factor.evaluateError(pose1, actH1))); - Matrix expH1 = numericalDerivative11( - boost::bind(evalFactorError2, factor, _1), pose1, 1e-5); + Matrix expH1 = numericalDerivative22(evalFactorError2, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } diff --git a/gtsam/slam/tests/testPriorFactor.cpp b/gtsam/slam/tests/testPriorFactor.cpp new file mode 100644 index 000000000..b3e54bedb --- /dev/null +++ b/gtsam/slam/tests/testPriorFactor.cpp @@ -0,0 +1,28 @@ +/** + * @file testPriorFactor.cpp + * @brief Test PriorFactor + * @author Frank Dellaert + * @date Nov 4, 2014 + */ + +#include +#include +#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ + +// Constructor +TEST(PriorFactor, Constructor) { + SharedNoiseModel model; + PriorFactor factor(1, LieScalar(1.0), model); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/gtsam/slam/tests/testRangeFactor.cpp b/gtsam/slam/tests/testRangeFactor.cpp index 4fa6164a1..cba9300f5 100644 --- a/gtsam/slam/tests/testRangeFactor.cpp +++ b/gtsam/slam/tests/testRangeFactor.cpp @@ -23,7 +23,6 @@ #include #include #include -#include #include #include @@ -37,12 +36,12 @@ typedef RangeFactor RangeFactor2D; typedef RangeFactor RangeFactor3D; /* ************************************************************************* */ -LieVector factorError2D(const Pose2& pose, const Point2& point, const RangeFactor2D& factor) { +Vector factorError2D(const Pose2& pose, const Point2& point, const RangeFactor2D& factor) { return factor.evaluateError(pose, point); } /* ************************************************************************* */ -LieVector factorError3D(const Pose3& pose, const Point3& point, const RangeFactor3D& factor) { +Vector factorError3D(const Pose3& pose, const Point3& point, const RangeFactor3D& factor) { return factor.evaluateError(pose, point); } @@ -214,8 +213,8 @@ TEST( RangeFactor, Jacobian2D ) { // Use numerical derivatives to calculate the Jacobians Matrix H1Expected, H2Expected; - H1Expected = numericalDerivative11(boost::bind(&factorError2D, _1, point, factor), pose); - H2Expected = numericalDerivative11(boost::bind(&factorError2D, pose, _1, factor), point); + H1Expected = numericalDerivative11(boost::bind(&factorError2D, _1, point, factor), pose); + H2Expected = numericalDerivative11(boost::bind(&factorError2D, pose, _1, factor), point); // Verify the Jacobians are correct CHECK(assert_equal(H1Expected, H1Actual, 1e-9)); @@ -243,8 +242,8 @@ TEST( RangeFactor, Jacobian2DWithTransform ) { // Use numerical derivatives to calculate the Jacobians Matrix H1Expected, H2Expected; - H1Expected = numericalDerivative11(boost::bind(&factorError2D, _1, point, factor), pose); - H2Expected = numericalDerivative11(boost::bind(&factorError2D, pose, _1, factor), point); + H1Expected = numericalDerivative11(boost::bind(&factorError2D, _1, point, factor), pose); + H2Expected = numericalDerivative11(boost::bind(&factorError2D, pose, _1, factor), point); // Verify the Jacobians are correct CHECK(assert_equal(H1Expected, H1Actual, 1e-9)); @@ -269,8 +268,8 @@ TEST( RangeFactor, Jacobian3D ) { // Use numerical derivatives to calculate the Jacobians Matrix H1Expected, H2Expected; - H1Expected = numericalDerivative11(boost::bind(&factorError3D, _1, point, factor), pose); - H2Expected = numericalDerivative11(boost::bind(&factorError3D, pose, _1, factor), point); + H1Expected = numericalDerivative11(boost::bind(&factorError3D, _1, point, factor), pose); + H2Expected = numericalDerivative11(boost::bind(&factorError3D, pose, _1, factor), point); // Verify the Jacobians are correct CHECK(assert_equal(H1Expected, H1Actual, 1e-9)); @@ -298,8 +297,8 @@ TEST( RangeFactor, Jacobian3DWithTransform ) { // Use numerical derivatives to calculate the Jacobians Matrix H1Expected, H2Expected; - H1Expected = numericalDerivative11(boost::bind(&factorError3D, _1, point, factor), pose); - H2Expected = numericalDerivative11(boost::bind(&factorError3D, pose, _1, factor), point); + H1Expected = numericalDerivative11(boost::bind(&factorError3D, _1, point, factor), pose); + H2Expected = numericalDerivative11(boost::bind(&factorError3D, pose, _1, factor), point); // Verify the Jacobians are correct CHECK(assert_equal(H1Expected, H1Actual, 1e-9)); diff --git a/gtsam/slam/tests/testReferenceFrameFactor.cpp b/gtsam/slam/tests/testReferenceFrameFactor.cpp index 078bf85cd..309801ccb 100644 --- a/gtsam/slam/tests/testReferenceFrameFactor.cpp +++ b/gtsam/slam/tests/testReferenceFrameFactor.cpp @@ -53,9 +53,9 @@ TEST( ReferenceFrameFactor, equals ) { } /* ************************************************************************* */ -LieVector evaluateError_(const PointReferenceFrameFactor& c, +Vector evaluateError_(const PointReferenceFrameFactor& c, const Point2& global, const Pose2& trans, const Point2& local) { - return LieVector(c.evaluateError(global, trans, local)); + return Vector(c.evaluateError(global, trans, local)); } TEST( ReferenceFrameFactor, jacobians ) { @@ -68,13 +68,13 @@ TEST( ReferenceFrameFactor, jacobians ) { tc.evaluateError(global, trans, local, actualDF, actualDT, actualDL); Matrix numericalDT, numericalDL, numericalDF; - numericalDF = numericalDerivative31( + numericalDF = numericalDerivative31( boost::bind(evaluateError_, tc, _1, _2, _3), global, trans, local, 1e-5); - numericalDT = numericalDerivative32( + numericalDT = numericalDerivative32( boost::bind(evaluateError_, tc, _1, _2, _3), global, trans, local, 1e-5); - numericalDL = numericalDerivative33( + numericalDL = numericalDerivative33( boost::bind(evaluateError_, tc, _1, _2, _3), global, trans, local, 1e-5); @@ -100,13 +100,13 @@ TEST( ReferenceFrameFactor, jacobians_zero ) { tc.evaluateError(global, trans, local, actualDF, actualDT, actualDL); Matrix numericalDT, numericalDL, numericalDF; - numericalDF = numericalDerivative31( + numericalDF = numericalDerivative31( boost::bind(evaluateError_, tc, _1, _2, _3), global, trans, local, 1e-5); - numericalDT = numericalDerivative32( + numericalDT = numericalDerivative32( boost::bind(evaluateError_, tc, _1, _2, _3), global, trans, local, 1e-5); - numericalDL = numericalDerivative33( + numericalDL = numericalDerivative33( boost::bind(evaluateError_, tc, _1, _2, _3), global, trans, local, 1e-5); diff --git a/gtsam/slam/tests/testRotateFactor.cpp b/gtsam/slam/tests/testRotateFactor.cpp index f36405318..15e8cf984 100644 --- a/gtsam/slam/tests/testRotateFactor.cpp +++ b/gtsam/slam/tests/testRotateFactor.cpp @@ -69,13 +69,13 @@ TEST (RotateFactor, test) { Matrix actual, expected; // Use numerical derivatives to calculate the expected Jacobian { - expected = numericalDerivative11( + expected = numericalDerivative11( boost::bind(&RotateFactor::evaluateError, &f, _1, boost::none), iRc); f.evaluateError(iRc, actual); EXPECT(assert_equal(expected, actual, 1e-9)); } { - expected = numericalDerivative11( + expected = numericalDerivative11( boost::bind(&RotateFactor::evaluateError, &f, _1, boost::none), R); f.evaluateError(R, actual); EXPECT(assert_equal(expected, actual, 1e-9)); @@ -141,14 +141,14 @@ TEST (RotateDirectionsFactor, test) { Matrix actual, expected; // Use numerical derivatives to calculate the expected Jacobian { - expected = numericalDerivative11( + expected = numericalDerivative11( boost::bind(&RotateDirectionsFactor::evaluateError, &f, _1, boost::none), iRc); f.evaluateError(iRc, actual); EXPECT(assert_equal(expected, actual, 1e-9)); } { - expected = numericalDerivative11( + expected = numericalDerivative11( boost::bind(&RotateDirectionsFactor::evaluateError, &f, _1, boost::none), R); f.evaluateError(R, actual); diff --git a/gtsam_unstable/dynamics/FullIMUFactor.h b/gtsam_unstable/dynamics/FullIMUFactor.h index b8dba6b61..1c35fc9b8 100644 --- a/gtsam_unstable/dynamics/FullIMUFactor.h +++ b/gtsam_unstable/dynamics/FullIMUFactor.h @@ -7,7 +7,6 @@ #pragma once #include -#include #include #include @@ -29,20 +28,20 @@ public: protected: /** measurements from the IMU */ - Vector accel_, gyro_; + Vector3 accel_, gyro_; double dt_; /// time between measurements public: /** Standard constructor */ - FullIMUFactor(const Vector& accel, const Vector& gyro, + FullIMUFactor(const Vector3& accel, const Vector3& gyro, double dt, const Key& key1, const Key& key2, const SharedNoiseModel& model) : Base(model, key1, key2), accel_(accel), gyro_(gyro), dt_(dt) { assert(model->dim() == 9); } /** Single IMU vector - imu = [accel, gyro] */ - FullIMUFactor(const Vector& imu, + FullIMUFactor(const Vector6& imu, double dt, const Key& key1, const Key& key2, const SharedNoiseModel& model) : Base(model, key1, key2), accel_(imu.head(3)), gyro_(imu.tail(3)), dt_(dt) { assert(imu.size() == 6); @@ -68,15 +67,15 @@ public: void print(const std::string& s="", const gtsam::KeyFormatter& formatter = gtsam::DefaultKeyFormatter) const { std::string a = "FullIMUFactor: " + s; Base::print(a, formatter); - gtsam::print(accel_, "accel"); - gtsam::print(gyro_, "gyro"); + gtsam::print((Vector)accel_, "accel"); + gtsam::print((Vector)gyro_, "gyro"); std::cout << "dt: " << dt_ << std::endl; } // access - const Vector& gyro() const { return gyro_; } - const Vector& accel() const { return accel_; } - Vector z() const { return concatVectors(2, &accel_, &gyro_); } + const Vector3& gyro() const { return gyro_; } + const Vector3& accel() const { return accel_; } + Vector6 z() const { return (Vector6() << accel_, gyro_); } /** * Error evaluation with optional derivatives - calculates @@ -85,13 +84,13 @@ public: virtual Vector evaluateError(const PoseRTV& x1, const PoseRTV& x2, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { - Vector z(9); + Vector9 z; z.head(3).operator=(accel_); // Strange syntax to work around ambiguous operator error with clang z.segment(3, 3).operator=(gyro_); // Strange syntax to work around ambiguous operator error with clang z.tail(3).operator=(x2.t().vector()); // Strange syntax to work around ambiguous operator error with clang - if (H1) *H1 = numericalDerivative21( + if (H1) *H1 = numericalDerivative21( boost::bind(This::predict_proxy, _1, _2, dt_), x1, x2, 1e-5); - if (H2) *H2 = numericalDerivative22( + if (H2) *H2 = numericalDerivative22( boost::bind(This::predict_proxy, _1, _2, dt_), x1, x2, 1e-5); return z - predict_proxy(x1, x2, dt_); } @@ -107,11 +106,11 @@ public: private: /** copy of the measurement function formulated for numerical derivatives */ - static LieVector predict_proxy(const PoseRTV& x1, const PoseRTV& x2, double dt) { - Vector hx(9); + static Vector9 predict_proxy(const PoseRTV& x1, const PoseRTV& x2, double dt) { + Vector9 hx; hx.head(6).operator=(x1.imuPrediction(x2, dt)); // Strange syntax to work around ambiguous operator error with clang hx.tail(3).operator=(x1.translationIntegration(x2, dt).vector()); // Strange syntax to work around ambiguous operator error with clang - return LieVector(hx); + return hx; } }; diff --git a/gtsam_unstable/dynamics/IMUFactor.h b/gtsam_unstable/dynamics/IMUFactor.h index 4ad0635cf..5fb4d77aa 100644 --- a/gtsam_unstable/dynamics/IMUFactor.h +++ b/gtsam_unstable/dynamics/IMUFactor.h @@ -7,7 +7,6 @@ #pragma once #include -#include #include #include @@ -27,18 +26,18 @@ public: protected: /** measurements from the IMU */ - Vector accel_, gyro_; + Vector3 accel_, gyro_; double dt_; /// time between measurements public: /** Standard constructor */ - IMUFactor(const Vector& accel, const Vector& gyro, + IMUFactor(const Vector3& accel, const Vector3& gyro, double dt, const Key& key1, const Key& key2, const SharedNoiseModel& model) : Base(model, key1, key2), accel_(accel), gyro_(gyro), dt_(dt) {} /** Full IMU vector specification */ - IMUFactor(const Vector& imu_vector, + IMUFactor(const Vector6& imu_vector, double dt, const Key& key1, const Key& key2, const SharedNoiseModel& model) : Base(model, key1, key2), accel_(imu_vector.head(3)), gyro_(imu_vector.tail(3)), dt_(dt) {} @@ -61,15 +60,15 @@ public: void print(const std::string& s="", const gtsam::KeyFormatter& formatter = gtsam::DefaultKeyFormatter) const { std::string a = "IMUFactor: " + s; Base::print(a, formatter); - gtsam::print(accel_, "accel"); - gtsam::print(gyro_, "gyro"); + gtsam::print((Vector)accel_, "accel"); + gtsam::print((Vector)gyro_, "gyro"); std::cout << "dt: " << dt_ << std::endl; } // access - const Vector& gyro() const { return gyro_; } - const Vector& accel() const { return accel_; } - Vector z() const { return concatVectors(2, &accel_, &gyro_); } + const Vector3& gyro() const { return gyro_; } + const Vector3& accel() const { return accel_; } + Vector6 z() const { return (Vector6() << accel_, gyro_);} /** * Error evaluation with optional derivatives - calculates @@ -78,10 +77,10 @@ public: virtual Vector evaluateError(const PoseRTV& x1, const PoseRTV& x2, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { - const Vector meas = z(); - if (H1) *H1 = numericalDerivative21( + const Vector6 meas = z(); + if (H1) *H1 = numericalDerivative21( boost::bind(This::predict_proxy, _1, _2, dt_, meas), x1, x2, 1e-5); - if (H2) *H2 = numericalDerivative22( + if (H2) *H2 = numericalDerivative22( boost::bind(This::predict_proxy, _1, _2, dt_, meas), x1, x2, 1e-5); return predict_proxy(x1, x2, dt_, meas); } @@ -96,10 +95,10 @@ public: private: /** copy of the measurement function formulated for numerical derivatives */ - static LieVector predict_proxy(const PoseRTV& x1, const PoseRTV& x2, - double dt, const Vector& meas) { - Vector hx = x1.imuPrediction(x2, dt); - return LieVector(meas - hx); + static Vector6 predict_proxy(const PoseRTV& x1, const PoseRTV& x2, + double dt, const Vector6& meas) { + Vector6 hx = x1.imuPrediction(x2, dt); + return meas - hx; } }; diff --git a/gtsam_unstable/dynamics/Pendulum.h b/gtsam_unstable/dynamics/Pendulum.h index 8cfc3cbcd..0100e17c7 100644 --- a/gtsam_unstable/dynamics/Pendulum.h +++ b/gtsam_unstable/dynamics/Pendulum.h @@ -9,7 +9,6 @@ #pragma once -#include #include namespace gtsam { @@ -21,11 +20,11 @@ namespace gtsam { * - For implicit Euler method: q_{k+1} = q_k + h*v_{k+1} * - For sympletic Euler method: q_{k+1} = q_k + h*v_{k+1} */ -class PendulumFactor1: public NoiseModelFactor3 { +class PendulumFactor1: public NoiseModelFactor3 { public: protected: - typedef NoiseModelFactor3 Base; + typedef NoiseModelFactor3 Base; /** default constructor to allow for serialization */ PendulumFactor1() {} @@ -38,7 +37,7 @@ public: ///Constructor. k1: q_{k+1}, k: q_k, velKey: velocity variable depending on the chosen method, h: time step PendulumFactor1(Key k1, Key k, Key velKey, double h, double mu = 1000.0) - : Base(noiseModel::Constrained::All(LieScalar::Dim(), fabs(mu)), k1, k, velKey), h_(h) {} + : Base(noiseModel::Constrained::All(1, fabs(mu)), k1, k, velKey), h_(h) {} /// @return a deep copy of this factor virtual gtsam::NonlinearFactor::shared_ptr clone() const { @@ -46,15 +45,15 @@ public: gtsam::NonlinearFactor::shared_ptr(new PendulumFactor1(*this))); } /** q_k + h*v - q_k1 = 0, with optional derivatives */ - Vector evaluateError(const LieScalar& qk1, const LieScalar& qk, const LieScalar& v, + Vector evaluateError(const double& qk1, const double& qk, const double& v, boost::optional H1 = boost::none, boost::optional H2 = boost::none, boost::optional H3 = boost::none) const { - const size_t p = LieScalar::Dim(); + const size_t p = 1; if (H1) *H1 = -eye(p); if (H2) *H2 = eye(p); if (H3) *H3 = eye(p)*h_; - return qk1.localCoordinates(qk.compose(LieScalar(v*h_))); + return (Vector(1) << qk+v*h_-qk1); } }; // \PendulumFactor1 @@ -67,11 +66,11 @@ public: * - For implicit Euler method: v_{k+1} = v_k - h*g/L*sin(q_{k+1}) * - For sympletic Euler method: v_{k+1} = v_k - h*g/L*sin(q_k) */ -class PendulumFactor2: public NoiseModelFactor3 { +class PendulumFactor2: public NoiseModelFactor3 { public: protected: - typedef NoiseModelFactor3 Base; + typedef NoiseModelFactor3 Base; /** default constructor to allow for serialization */ PendulumFactor2() {} @@ -86,7 +85,7 @@ public: ///Constructor. vk1: v_{k+1}, vk: v_k, qkey: q's key depending on the chosen method, h: time step PendulumFactor2(Key vk1, Key vk, Key qkey, double h, double r = 1.0, double g = 9.81, double mu = 1000.0) - : Base(noiseModel::Constrained::All(LieScalar::Dim(), fabs(mu)), vk1, vk, qkey), h_(h), g_(g), r_(r) {} + : Base(noiseModel::Constrained::All(1, fabs(mu)), vk1, vk, qkey), h_(h), g_(g), r_(r) {} /// @return a deep copy of this factor virtual gtsam::NonlinearFactor::shared_ptr clone() const { @@ -94,15 +93,15 @@ public: gtsam::NonlinearFactor::shared_ptr(new PendulumFactor2(*this))); } /** v_k - h*g/L*sin(q) - v_k1 = 0, with optional derivatives */ - Vector evaluateError(const LieScalar& vk1, const LieScalar& vk, const LieScalar& q, + Vector evaluateError(const double & vk1, const double & vk, const double & q, boost::optional H1 = boost::none, boost::optional H2 = boost::none, boost::optional H3 = boost::none) const { - const size_t p = LieScalar::Dim(); + const size_t p = 1; if (H1) *H1 = -eye(p); if (H2) *H2 = eye(p); - if (H3) *H3 = -eye(p)*h_*g_/r_*cos(q.value()); - return vk1.localCoordinates(LieScalar(vk - h_*g_/r_*sin(q))); + if (H3) *H3 = -eye(p)*h_*g_/r_*cos(q); + return (Vector(1) << vk - h_ * g_ / r_ * sin(q) - vk1); } }; // \PendulumFactor2 @@ -114,11 +113,11 @@ public: * p_k = -D_1 L_d(q_k,q_{k+1},h) = \frac{1}{h}mr^{2}\left(q_{k+1}-q_{k}\right)+mgrh(1-\alpha)\,\sin\left((1-\alpha)q_{k}+\alpha q_{k+1}\right) * = (1/h)mr^2 (q_{k+1}-q_k) + mgrh(1-alpha) sin ((1-alpha)q_k+\alpha q_{k+1}) */ -class PendulumFactorPk: public NoiseModelFactor3 { +class PendulumFactorPk: public NoiseModelFactor3 { public: protected: - typedef NoiseModelFactor3 Base; + typedef NoiseModelFactor3 Base; /** default constructor to allow for serialization */ PendulumFactorPk() {} @@ -136,7 +135,7 @@ public: ///Constructor PendulumFactorPk(Key pKey, Key qKey, Key qKey1, double h, double m = 1.0, double r = 1.0, double g = 9.81, double alpha = 0.0, double mu = 1000.0) - : Base(noiseModel::Constrained::All(LieScalar::Dim(), fabs(mu)), pKey, qKey, qKey1), + : Base(noiseModel::Constrained::All(1, fabs(mu)), pKey, qKey, qKey1), h_(h), m_(m), r_(r), g_(g), alpha_(alpha) {} /// @return a deep copy of this factor @@ -145,11 +144,11 @@ public: gtsam::NonlinearFactor::shared_ptr(new PendulumFactorPk(*this))); } /** 1/h mr^2 (qk1-qk)+mgrh (1-a) sin((1-a)pk + a*pk1) - pk = 0, with optional derivatives */ - Vector evaluateError(const LieScalar& pk, const LieScalar& qk, const LieScalar& qk1, + Vector evaluateError(const double & pk, const double & qk, const double & qk1, boost::optional H1 = boost::none, boost::optional H2 = boost::none, boost::optional H3 = boost::none) const { - const size_t p = LieScalar::Dim(); + const size_t p = 1; double qmid = (1-alpha_)*qk + alpha_*qk1; double mr2_h = 1/h_*m_*r_*r_; @@ -159,7 +158,7 @@ public: if (H2) *H2 = eye(p)*(-mr2_h + mgrh*(1-alpha_)*(1-alpha_)*cos(qmid)); if (H3) *H3 = eye(p)*( mr2_h + mgrh*(1-alpha_)*(alpha_)*cos(qmid)); - return pk.localCoordinates(LieScalar(mr2_h*(qk1-qk) + mgrh*(1-alpha_)*sin(qmid))); + return (Vector(1) << mr2_h * (qk1 - qk) + mgrh * (1 - alpha_) * sin(qmid) - pk); } }; // \PendulumFactorPk @@ -170,11 +169,11 @@ public: * p_k1 = D_2 L_d(q_k,q_{k+1},h) = \frac{1}{h}mr^{2}\left(q_{k+1}-q_{k}\right)-mgrh\alpha\sin\left((1-\alpha)q_{k}+\alpha q_{k+1}\right) * = (1/h)mr^2 (q_{k+1}-q_k) - mgrh alpha sin ((1-alpha)q_k+\alpha q_{k+1}) */ -class PendulumFactorPk1: public NoiseModelFactor3 { +class PendulumFactorPk1: public NoiseModelFactor3 { public: protected: - typedef NoiseModelFactor3 Base; + typedef NoiseModelFactor3 Base; /** default constructor to allow for serialization */ PendulumFactorPk1() {} @@ -192,7 +191,7 @@ public: ///Constructor PendulumFactorPk1(Key pKey1, Key qKey, Key qKey1, double h, double m = 1.0, double r = 1.0, double g = 9.81, double alpha = 0.0, double mu = 1000.0) - : Base(noiseModel::Constrained::All(LieScalar::Dim(), fabs(mu)), pKey1, qKey, qKey1), + : Base(noiseModel::Constrained::All(1, fabs(mu)), pKey1, qKey, qKey1), h_(h), m_(m), r_(r), g_(g), alpha_(alpha) {} /// @return a deep copy of this factor @@ -201,11 +200,11 @@ public: gtsam::NonlinearFactor::shared_ptr(new PendulumFactorPk1(*this))); } /** 1/h mr^2 (qk1-qk) - mgrh a sin((1-a)pk + a*pk1) - pk1 = 0, with optional derivatives */ - Vector evaluateError(const LieScalar& pk1, const LieScalar& qk, const LieScalar& qk1, + Vector evaluateError(const double & pk1, const double & qk, const double & qk1, boost::optional H1 = boost::none, boost::optional H2 = boost::none, boost::optional H3 = boost::none) const { - const size_t p = LieScalar::Dim(); + const size_t p = 1; double qmid = (1-alpha_)*qk + alpha_*qk1; double mr2_h = 1/h_*m_*r_*r_; @@ -215,7 +214,7 @@ public: if (H2) *H2 = eye(p)*(-mr2_h - mgrh*(1-alpha_)*alpha_*cos(qmid)); if (H3) *H3 = eye(p)*( mr2_h - mgrh*alpha_*alpha_*cos(qmid)); - return pk1.localCoordinates(LieScalar(mr2_h*(qk1-qk) - mgrh*alpha_*sin(qmid))); + return (Vector(1) << mr2_h * (qk1 - qk) - mgrh * alpha_ * sin(qmid) - pk1); } }; // \PendulumFactorPk1 diff --git a/gtsam_unstable/dynamics/PoseRTV.cpp b/gtsam_unstable/dynamics/PoseRTV.cpp index 2246baee1..ed8d54696 100644 --- a/gtsam_unstable/dynamics/PoseRTV.cpp +++ b/gtsam_unstable/dynamics/PoseRTV.cpp @@ -57,18 +57,17 @@ void PoseRTV::print(const string& s) const { } /* ************************************************************************* */ -PoseRTV PoseRTV::Expmap(const Vector& v) { - assert(v.size() == 9); - Pose3 newPose = Pose3::Expmap(sub(v, 0, 6)); - Velocity3 newVel = Velocity3::Expmap(sub(v, 6, 9)); +PoseRTV PoseRTV::Expmap(const Vector9& v) { + Pose3 newPose = Pose3::Expmap(v.head<6>()); + Velocity3 newVel = Velocity3::Expmap(v.tail<3>()); return PoseRTV(newPose, newVel); } /* ************************************************************************* */ -Vector PoseRTV::Logmap(const PoseRTV& p) { - Vector Lx = Pose3::Logmap(p.Rt_); - Vector Lv = Velocity3::Logmap(p.v_); - return concatVectors(2, &Lx, &Lv); +Vector9 PoseRTV::Logmap(const PoseRTV& p) { + Vector6 Lx = Pose3::Logmap(p.Rt_); + Vector3 Lv = Velocity3::Logmap(p.v_); + return (Vector9() << Lx, Lv); } /* ************************************************************************* */ @@ -84,9 +83,9 @@ PoseRTV PoseRTV::retract(const Vector& v) const { Vector PoseRTV::localCoordinates(const PoseRTV& p1) const { const Pose3& x0 = pose(), &x1 = p1.pose(); // First order approximation - Vector poseLogmap = x0.localCoordinates(x1); - Vector lv = rotation().unrotate(p1.velocity() - v_).vector(); - return concatVectors(2, &poseLogmap, &lv); + Vector6 poseLogmap = x0.localCoordinates(x1); + Vector3 lv = rotation().unrotate(p1.velocity() - v_).vector(); + return (Vector(9) << poseLogmap, lv); } /* ************************************************************************* */ @@ -190,16 +189,16 @@ PoseRTV PoseRTV::generalDynamics( } /* ************************************************************************* */ -Vector PoseRTV::imuPrediction(const PoseRTV& x2, double dt) const { +Vector6 PoseRTV::imuPrediction(const PoseRTV& x2, double dt) const { // split out states const Rot3 &r1 = R(), &r2 = x2.R(); const Velocity3 &v1 = v(), &v2 = x2.v(); - Vector imu(6); + Vector6 imu; // acceleration Vector accel = v1.localCoordinates(v2) / dt; - imu.head(3) = r2.transpose() * (accel - g); + imu.head<3>() = r2.transpose() * (accel - g); // rotation rates // just using euler angles based on matlab code @@ -211,7 +210,7 @@ Vector PoseRTV::imuPrediction(const PoseRTV& x2, double dt) const { // normalize yaw in difference (as per Mitch's code) dR(2) = Rot2::fromAngle(dR(2)).theta(); dR /= dt; - imu.tail(3) = Enb * dR; + imu.tail<3>() = Enb * dR; // imu.tail(3) = r1.transpose() * dR; return imu; diff --git a/gtsam_unstable/dynamics/PoseRTV.h b/gtsam_unstable/dynamics/PoseRTV.h index 51e09ca5f..43d018752 100644 --- a/gtsam_unstable/dynamics/PoseRTV.h +++ b/gtsam_unstable/dynamics/PoseRTV.h @@ -19,7 +19,7 @@ typedef Point3 Velocity3; * Robot state for use with IMU measurements * - contains translation, translational velocity and rotation */ -class GTSAM_UNSTABLE_EXPORT PoseRTV : public DerivedValue { +class GTSAM_UNSTABLE_EXPORT PoseRTV { protected: Pose3 Rt_; @@ -86,8 +86,8 @@ public: * expmap/logmap are poor approximations that assume independence of components * Currently implemented using the poor retract/unretract approximations */ - static PoseRTV Expmap(const Vector& v); - static Vector Logmap(const PoseRTV& p); + static PoseRTV Expmap(const Vector9& v); + static Vector9 Logmap(const PoseRTV& p); static PoseRTV identity() { return PoseRTV(); } @@ -129,7 +129,7 @@ public: /// Dynamics predictor for both ground and flying robots, given states at 1 and 2 /// Always move from time 1 to time 2 /// @return imu measurement, as [accel, gyro] - Vector imuPrediction(const PoseRTV& x2, double dt) const; + Vector6 imuPrediction(const PoseRTV& x2, double dt) const; /// predict measurement and where Point3 for x2 should be, as a way /// of enforcing a velocity constraint @@ -183,4 +183,23 @@ private: } }; +// Define GTSAM traits +namespace traits { + +template<> +struct is_manifold : public boost::true_type { +}; + +template<> +struct dimension : public boost::integral_constant { +}; + +template<> +struct zero { + static PoseRTV value() { + return PoseRTV(); + } +}; + +} } // \namespace gtsam diff --git a/gtsam_unstable/dynamics/SimpleHelicopter.h b/gtsam_unstable/dynamics/SimpleHelicopter.h index ebc430277..06d485a88 100644 --- a/gtsam_unstable/dynamics/SimpleHelicopter.h +++ b/gtsam_unstable/dynamics/SimpleHelicopter.h @@ -9,7 +9,6 @@ #include #include -#include #include namespace gtsam { @@ -24,10 +23,10 @@ namespace gtsam { * Note: this factor is necessary if one needs to smooth the entire graph. It's not needed * in sequential update method. */ -class Reconstruction : public NoiseModelFactor3 { +class Reconstruction : public NoiseModelFactor3 { double h_; // time step - typedef NoiseModelFactor3 Base; + typedef NoiseModelFactor3 Base; public: Reconstruction(Key gKey1, Key gKey, Key xiKey, double h, double mu = 1000.0) : Base(noiseModel::Constrained::All(Pose3::Dim(), fabs(mu)), gKey1, gKey, @@ -41,7 +40,7 @@ public: gtsam::NonlinearFactor::shared_ptr(new Reconstruction(*this))); } /** \f$ log((g_k\exp(h\xi_k))^{-1}g_{k+1}) = 0, with optional derivatives */ - Vector evaluateError(const Pose3& gk1, const Pose3& gk, const LieVector& xik, + Vector evaluateError(const Pose3& gk1, const Pose3& gk, const Vector6& xik, boost::optional H1 = boost::none, boost::optional H2 = boost::none, boost::optional H3 = boost::none) const { @@ -74,7 +73,7 @@ public: /** * Implement the Discrete Euler-Poincare' equation: */ -class DiscreteEulerPoincareHelicopter : public NoiseModelFactor3 { +class DiscreteEulerPoincareHelicopter : public NoiseModelFactor3 { double h_; /// time step Matrix Inertia_; /// Inertia tensors Inertia = [ J 0; 0 M ] @@ -87,7 +86,7 @@ class DiscreteEulerPoincareHelicopter : public NoiseModelFactor3 Base; + typedef NoiseModelFactor3 Base; public: DiscreteEulerPoincareHelicopter(Key xiKey1, Key xiKey_1, Key gKey, @@ -108,7 +107,7 @@ public: * where pk = CT_TLN(h*xi_k)*Inertia*xi_k * pk_1 = CT_TLN(-h*xi_k_1)*Inertia*xi_k_1 * */ - Vector evaluateError(const LieVector& xik, const LieVector& xik_1, const Pose3& gk, + Vector evaluateError(const Vector6& xik, const Vector6& xik_1, const Pose3& gk, boost::optional H1 = boost::none, boost::optional H2 = boost::none, boost::optional H3 = boost::none) const { @@ -125,7 +124,7 @@ public: Vector pk_1 = muk_1 - 0.5*Pose3::adjointTranspose(-h_*xik_1, muk_1, D_adjThxik1_muk1); Matrix D_gravityBody_gk; - Point3 gravityBody = gk.rotation().unrotate(Point3(0.0, 0.0, -9.81*m_), D_gravityBody_gk); + Point3 gravityBody = gk.rotation().unrotate(Point3(0.0, 0.0, -9.81*m_), D_gravityBody_gk, boost::none); Vector f_ext = (Vector(6) << 0.0, 0.0, 0.0, gravityBody.x(), gravityBody.y(), gravityBody.z()); Vector hx = pk - pk_1 - h_*Fu_ - h_*f_ext; @@ -149,7 +148,7 @@ public: } #if 0 - Vector computeError(const LieVector& xik, const LieVector& xik_1, const Pose3& gk) const { + Vector computeError(const Vector6& xik, const Vector6& xik_1, const Pose3& gk) const { Vector pk = Pose3::dExpInv_exp(h_*xik).transpose()*Inertia_*xik; Vector pk_1 = Pose3::dExpInv_exp(-h_*xik_1).transpose()*Inertia_*xik_1; @@ -161,13 +160,13 @@ public: return hx; } - Vector evaluateError(const LieVector& xik, const LieVector& xik_1, const Pose3& gk, + Vector evaluateError(const Vector6& xik, const Vector6& xik_1, const Pose3& gk, boost::optional H1 = boost::none, boost::optional H2 = boost::none, boost::optional H3 = boost::none) const { if (H1) { (*H1) = numericalDerivative31( - boost::function( + boost::function( boost::bind(&DiscreteEulerPoincareHelicopter::computeError, *this, _1, _2, _3) ), xik, xik_1, gk, 1e-5 @@ -175,7 +174,7 @@ public: } if (H2) { (*H2) = numericalDerivative32( - boost::function( + boost::function( boost::bind(&DiscreteEulerPoincareHelicopter::computeError, *this, _1, _2, _3) ), xik, xik_1, gk, 1e-5 @@ -183,7 +182,7 @@ public: } if (H3) { (*H3) = numericalDerivative33( - boost::function( + boost::function( boost::bind(&DiscreteEulerPoincareHelicopter::computeError, *this, _1, _2, _3) ), xik, xik_1, gk, 1e-5 diff --git a/gtsam_unstable/dynamics/VelocityConstraint.h b/gtsam_unstable/dynamics/VelocityConstraint.h index 252f9dbfe..c9db449f9 100644 --- a/gtsam_unstable/dynamics/VelocityConstraint.h +++ b/gtsam_unstable/dynamics/VelocityConstraint.h @@ -83,9 +83,9 @@ public: virtual gtsam::Vector evaluateError(const PoseRTV& x1, const PoseRTV& x2, boost::optional H1=boost::none, boost::optional H2=boost::none) const { - if (H1) *H1 = gtsam::numericalDerivative21( + if (H1) *H1 = gtsam::numericalDerivative21( boost::bind(VelocityConstraint::evaluateError_, _1, _2, dt_, integration_mode_), x1, x2, 1e-5); - if (H2) *H2 = gtsam::numericalDerivative22( + if (H2) *H2 = gtsam::numericalDerivative22( boost::bind(VelocityConstraint::evaluateError_, _1, _2, dt_, integration_mode_), x1, x2, 1e-5); return evaluateError_(x1, x2, dt_, integration_mode_); } @@ -103,7 +103,7 @@ public: } private: - static gtsam::LieVector evaluateError_(const PoseRTV& x1, const PoseRTV& x2, + static gtsam::Vector evaluateError_(const PoseRTV& x1, const PoseRTV& x2, double dt, const dynamics::IntegrationMode& mode) { const Velocity3& v1 = x1.v(), v2 = x2.v(), p1 = x1.t(), p2 = x2.t(); diff --git a/gtsam_unstable/dynamics/VelocityConstraint3.h b/gtsam_unstable/dynamics/VelocityConstraint3.h index 83506e2d5..1a432ba1e 100644 --- a/gtsam_unstable/dynamics/VelocityConstraint3.h +++ b/gtsam_unstable/dynamics/VelocityConstraint3.h @@ -1,21 +1,20 @@ /** * @file VelocityConstraint3.h - * @brief A simple 3-way factor constraining LieScalar poses and velocity + * @brief A simple 3-way factor constraining double poses and velocity * @author Duy-Nguyen Ta */ #pragma once -#include #include namespace gtsam { -class VelocityConstraint3 : public NoiseModelFactor3 { +class VelocityConstraint3 : public NoiseModelFactor3 { public: protected: - typedef NoiseModelFactor3 Base; + typedef NoiseModelFactor3 Base; /** default constructor to allow for serialization */ VelocityConstraint3() {} @@ -28,7 +27,7 @@ public: ///TODO: comment VelocityConstraint3(Key key1, Key key2, Key velKey, double dt, double mu = 1000.0) - : Base(noiseModel::Constrained::All(LieScalar::Dim(), fabs(mu)), key1, key2, velKey), dt_(dt) {} + : Base(noiseModel::Constrained::All(1, fabs(mu)), key1, key2, velKey), dt_(dt) {} virtual ~VelocityConstraint3() {} /// @return a deep copy of this factor @@ -37,15 +36,15 @@ public: gtsam::NonlinearFactor::shared_ptr(new VelocityConstraint3(*this))); } /** x1 + v*dt - x2 = 0, with optional derivatives */ - Vector evaluateError(const LieScalar& x1, const LieScalar& x2, const LieScalar& v, + Vector evaluateError(const double& x1, const double& x2, const double& v, boost::optional H1 = boost::none, boost::optional H2 = boost::none, boost::optional H3 = boost::none) const { - const size_t p = LieScalar::Dim(); + const size_t p = 1; if (H1) *H1 = eye(p); if (H2) *H2 = -eye(p); if (H3) *H3 = eye(p)*dt_; - return x2.localCoordinates(x1.compose(LieScalar(v*dt_))); + return (Vector(1) << x1+v*dt_-x2); } private: diff --git a/gtsam_unstable/dynamics/tests/testIMUSystem.cpp b/gtsam_unstable/dynamics/tests/testIMUSystem.cpp index be7078d9a..550b0e33c 100644 --- a/gtsam_unstable/dynamics/tests/testIMUSystem.cpp +++ b/gtsam_unstable/dynamics/tests/testIMUSystem.cpp @@ -62,10 +62,9 @@ TEST( testIMUSystem, optimize_chain ) { // create measurements SharedDiagonal model = noiseModel::Unit::Create(6); - Vector imu12(6), imu23(6), imu34(6); - imu12 = pose1.imuPrediction(pose2, dt); - imu23 = pose2.imuPrediction(pose3, dt); - imu34 = pose3.imuPrediction(pose4, dt); + Vector6 imu12 = pose1.imuPrediction(pose2, dt); + Vector6 imu23 = pose2.imuPrediction(pose3, dt); + Vector6 imu34 = pose3.imuPrediction(pose4, dt); // assemble simple graph with IMU measurements and velocity constraints NonlinearFactorGraph graph; @@ -109,10 +108,9 @@ TEST( testIMUSystem, optimize_chain_fullfactor ) { // create measurements SharedDiagonal model = noiseModel::Isotropic::Sigma(9, 1.0); - Vector imu12(6), imu23(6), imu34(6); - imu12 = pose1.imuPrediction(pose2, dt); - imu23 = pose2.imuPrediction(pose3, dt); - imu34 = pose3.imuPrediction(pose4, dt); + Vector6 imu12 = pose1.imuPrediction(pose2, dt); + Vector6 imu23 = pose2.imuPrediction(pose3, dt); + Vector6 imu34 = pose3.imuPrediction(pose4, dt); // assemble simple graph with IMU measurements and velocity constraints NonlinearFactorGraph graph; diff --git a/gtsam_unstable/dynamics/tests/testPendulumFactors.cpp b/gtsam_unstable/dynamics/tests/testPendulumFactors.cpp index ce176787c..5a4593270 100644 --- a/gtsam_unstable/dynamics/tests/testPendulumFactors.cpp +++ b/gtsam_unstable/dynamics/tests/testPendulumFactors.cpp @@ -18,8 +18,8 @@ namespace { const double g = 9.81, l = 1.0; const double deg2rad = M_PI/180.0; - LieScalar origin, q1(deg2rad*30.0), q2(deg2rad*31.0); - LieScalar v1(deg2rad*1.0/h), v2((v1-h*g/l*sin(q1))); + double q1(deg2rad*30.0), q2(deg2rad*31.0); + double v1(deg2rad*1.0/h), v2((v1-h*g/l*sin(q1))); } @@ -46,7 +46,7 @@ TEST( testPendulumFactorPk, evaluateError) { // hard constraints don't need a noise model PendulumFactorPk constraint(P(1), Q(1), Q(2), h); - LieScalar pk( 1/h * (q2-q1) + h*g*sin(q1) ); + double pk( 1/h * (q2-q1) + h*g*sin(q1) ); // verify error function EXPECT(assert_equal(zero(1), constraint.evaluateError(pk, q1, q2), tol)); @@ -57,7 +57,7 @@ TEST( testPendulumFactorPk1, evaluateError) { // hard constraints don't need a noise model PendulumFactorPk1 constraint(P(2), Q(1), Q(2), h); - LieScalar pk1( 1/h * (q2-q1) ); + double pk1( 1/h * (q2-q1) ); // verify error function EXPECT(assert_equal(zero(1), constraint.evaluateError(pk1, q1, q2), tol)); diff --git a/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp b/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp index 82197302b..475d5285c 100644 --- a/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp +++ b/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp @@ -18,11 +18,11 @@ const double h = 0.01; //const double deg2rad = M_PI/180.0; //Pose3 g1(Rot3::ypr(deg2rad*10.0, deg2rad*20.0, deg2rad*30.0), Point3(100.0, 200.0, 300.0)); Pose3 g1(Rot3(), Point3(100.0, 0.0, 300.0)); -//LieVector v1((Vector(6) << 0.1, 0.05, 0.02, 10.0, 20.0, 30.0)); -LieVector V1_w((Vector(6) << 0.0, 0.0, M_PI/3, 0.0, 0.0, 30.0)); -LieVector V1_g1 = g1.inverse().Adjoint(V1_w); +//Vector6 v1((Vector(6) << 0.1, 0.05, 0.02, 10.0, 20.0, 30.0)); +Vector6 V1_w((Vector(6) << 0.0, 0.0, M_PI/3, 0.0, 0.0, 30.0)); +Vector6 V1_g1 = g1.inverse().Adjoint(V1_w); Pose3 g2(g1.retract(h*V1_g1, Pose3::EXPMAP)); -//LieVector v2 = Pose3::Logmap(g1.between(g2)); +//Vector6 v2 = Pose3::Logmap(g1.between(g2)); double mass = 100.0; Vector gamma2 = (Vector(2) << 0.0, 0.0); // no shape @@ -46,16 +46,16 @@ Vector computeFu(const Vector& gamma, const Vector& control) { } /* ************************************************************************* */ -Vector testExpmapDeriv(const LieVector& v) { +Vector testExpmapDeriv(const Vector6& v) { return Pose3::Logmap(Pose3::Expmap(-h*V1_g1)*Pose3::Expmap(h*V1_g1+v)); } TEST(Reconstruction, ExpmapInvDeriv) { Matrix numericalExpmap = numericalDerivative11( - boost::function( + boost::function( boost::bind(testExpmapDeriv, _1) ), - LieVector(Vector::Zero(6)), 1e-5 + Vector6(Vector::Zero(6)), 1e-5 ); Matrix dExpInv = Pose3::dExpInv_exp(h*V1_g1); EXPECT(assert_equal(numericalExpmap, dExpInv, 1e-2)); @@ -72,21 +72,21 @@ TEST( Reconstruction, evaluateError) { Matrix numericalH1 = numericalDerivative31( - boost::function( + boost::function( boost::bind(&Reconstruction::evaluateError, constraint, _1, _2, _3, boost::none, boost::none, boost::none) ), g2, g1, V1_g1, 1e-5 ); Matrix numericalH2 = numericalDerivative32( - boost::function( + boost::function( boost::bind(&Reconstruction::evaluateError, constraint, _1, _2, _3, boost::none, boost::none, boost::none) ), g2, g1, V1_g1, 1e-5 ); Matrix numericalH3 = numericalDerivative33( - boost::function( + boost::function( boost::bind(&Reconstruction::evaluateError, constraint, _1, _2, _3, boost::none, boost::none, boost::none) ), g2, g1, V1_g1, 1e-5 @@ -119,7 +119,7 @@ TEST( DiscreteEulerPoincareHelicopter, evaluateError) { Pose3 g21 = g2.between(g1); Vector V2_g2 = g21.Adjoint(V2_g1); // convert the new velocity to g2's frame - LieVector expectedv2(V2_g2); + Vector6 expectedv2(V2_g2); // hard constraints don't need a noise model DiscreteEulerPoincareHelicopter constraint(V(2), V(1), G(2), h, @@ -130,21 +130,21 @@ TEST( DiscreteEulerPoincareHelicopter, evaluateError) { EXPECT(assert_equal(zero(6), constraint.evaluateError(expectedv2, V1_g1, g2, H1, H2, H3), 1e0)); Matrix numericalH1 = numericalDerivative31( - boost::function( + boost::function( boost::bind(&DiscreteEulerPoincareHelicopter::evaluateError, constraint, _1, _2, _3, boost::none, boost::none, boost::none) ), expectedv2, V1_g1, g2, 1e-5 ); Matrix numericalH2 = numericalDerivative32( - boost::function( + boost::function( boost::bind(&DiscreteEulerPoincareHelicopter::evaluateError, constraint, _1, _2, _3, boost::none, boost::none, boost::none) ), expectedv2, V1_g1, g2, 1e-5 ); Matrix numericalH3 = numericalDerivative33( - boost::function( + boost::function( boost::bind(&DiscreteEulerPoincareHelicopter::evaluateError, constraint, _1, _2, _3, boost::none, boost::none, boost::none) ), expectedv2, V1_g1, g2, 1e-5 diff --git a/gtsam_unstable/dynamics/tests/testVelocityConstraint3.cpp b/gtsam_unstable/dynamics/tests/testVelocityConstraint3.cpp index db4b7c586..ac27ae563 100644 --- a/gtsam_unstable/dynamics/tests/testVelocityConstraint3.cpp +++ b/gtsam_unstable/dynamics/tests/testVelocityConstraint3.cpp @@ -8,23 +8,16 @@ #include #include -/* ************************************************************************* */ using namespace gtsam; - -namespace { - - const double tol=1e-5; - const double dt = 1.0; - - LieScalar origin, - x1(1.0), x2(2.0), v(1.0); - -} +using namespace gtsam::symbol_shorthand; /* ************************************************************************* */ +// evaluateError TEST( testVelocityConstraint3, evaluateError) { - using namespace gtsam::symbol_shorthand; + const double tol = 1e-5; + const double dt = 1.0; + double x1(1.0), x2(2.0), v(1.0); // hard constraints don't need a noise model VelocityConstraint3 constraint(X(1), X(2), V(1), dt); @@ -33,7 +26,9 @@ TEST( testVelocityConstraint3, evaluateError) { EXPECT(assert_equal(zero(1), constraint.evaluateError(x1, x2, v), tol)); } - /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ diff --git a/gtsam_unstable/examples/Pose2SLAMExampleExpressions.cpp b/gtsam_unstable/examples/Pose2SLAMExampleExpressions.cpp new file mode 100644 index 000000000..936c9957b --- /dev/null +++ b/gtsam_unstable/examples/Pose2SLAMExampleExpressions.cpp @@ -0,0 +1,88 @@ +/* ---------------------------------------------------------------------------- + + * 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 Pose2SLAMExample.cpp + * @brief Expressions version of Pose2SLAMExample.cpp + * @date Oct 2, 2014 + * @author Frank Dellaert + * @author Yong Dian Jian + */ + +// The two new headers that allow using our Automatic Differentiation Expression framework +#include +#include + +// Header order is close to far +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +int main(int argc, char** argv) { + + // 1. Create a factor graph container and add factors to it + NonlinearFactorGraph graph; + + // Create Expressions for unknowns + Pose2_ x1(1), x2(2), x3(3), x4(4), x5(5); + + // 2a. Add a prior on the first pose, setting it to the origin + noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1)); + graph.push_back(ExpressionFactor(priorNoise, Pose2(0, 0, 0), x1)); + + // For simplicity, we will use the same noise model for odometry and loop closures + noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1)); + + // 2b. Add odometry factors + graph.push_back(ExpressionFactor(model, Pose2(2, 0, 0 ), between(x1,x2))); + graph.push_back(ExpressionFactor(model, Pose2(2, 0, M_PI_2), between(x2,x3))); + graph.push_back(ExpressionFactor(model, Pose2(2, 0, M_PI_2), between(x3,x4))); + graph.push_back(ExpressionFactor(model, Pose2(2, 0, M_PI_2), between(x4,x5))); + + // 2c. Add the loop closure constraint + graph.push_back(ExpressionFactor(model, Pose2(2, 0, M_PI_2), between(x5,x2))); + graph.print("\nFactor Graph:\n"); // print + + // 3. Create the data structure to hold the initialEstimate estimate to the solution + // For illustrative purposes, these have been deliberately set to incorrect values + Values initialEstimate; + initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2 )); + initialEstimate.insert(2, Pose2(2.3, 0.1, -0.2 )); + initialEstimate.insert(3, Pose2(4.1, 0.1, M_PI_2)); + initialEstimate.insert(4, Pose2(4.0, 2.0, M_PI )); + initialEstimate.insert(5, Pose2(2.1, 2.1, -M_PI_2)); + initialEstimate.print("\nInitial Estimate:\n"); // print + + // 4. Optimize the initial values using a Gauss-Newton nonlinear optimizer + GaussNewtonParams parameters; + parameters.relativeErrorTol = 1e-5; + parameters.maxIterations = 100; + GaussNewtonOptimizer optimizer(graph, initialEstimate, parameters); + Values result = optimizer.optimize(); + result.print("Final Result:\n"); + + // 5. Calculate and print marginal covariances for all variables + cout.precision(3); + Marginals marginals(graph, result); + cout << "x1 covariance:\n" << marginals.marginalCovariance(1) << endl; + cout << "x2 covariance:\n" << marginals.marginalCovariance(2) << endl; + cout << "x3 covariance:\n" << marginals.marginalCovariance(3) << endl; + cout << "x4 covariance:\n" << marginals.marginalCovariance(4) << endl; + cout << "x5 covariance:\n" << marginals.marginalCovariance(5) << endl; + + return 0; +} diff --git a/gtsam_unstable/examples/SFMExampleExpressions.cpp b/gtsam_unstable/examples/SFMExampleExpressions.cpp new file mode 100644 index 000000000..1537af1d9 --- /dev/null +++ b/gtsam_unstable/examples/SFMExampleExpressions.cpp @@ -0,0 +1,101 @@ +/* ---------------------------------------------------------------------------- + + * 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 SFMExampleExpressions.cpp + * @brief A structure-from-motion example done with Expressions + * @author Frank Dellaert + * @author Duy-Nguyen Ta + * @date October 1, 2014 + */ + +/** + * This is the Expression version of SFMExample + * See detailed description of headers there, this focuses on explaining the AD part + */ + +// The two new headers that allow using our Automatic Differentiation Expression framework +#include +#include + +// Header order is close to far +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +int main(int argc, char* argv[]) { + + Cal3_S2 K(50.0, 50.0, 0.0, 50.0, 50.0); + noiseModel::Isotropic::shared_ptr measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v + + // Create the set of ground-truth landmarks and poses + vector points = createPoints(); + vector poses = createPoses(); + + // Create a factor graph + NonlinearFactorGraph graph; + + // Specify uncertainty on first pose prior + noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1))); + + // Here we don't use a PriorFactor but directly the ExpressionFactor class + // The object x0 is an Expression, and we create a factor wanting it to be equal to poses[0] + Pose3_ x0('x',0); + graph.push_back(ExpressionFactor(poseNoise, poses[0], x0)); + + // We create a constant Expression for the calibration here + Cal3_S2_ cK(K); + + // Simulated measurements from each camera pose, adding them to the factor graph + for (size_t i = 0; i < poses.size(); ++i) { + Pose3_ x('x', i); + SimpleCamera camera(poses[i], K); + for (size_t j = 0; j < points.size(); ++j) { + Point2 measurement = camera.project(points[j]); + // Below an expression for the prediction of the measurement: + Point3_ p('l', j); + Point2_ prediction = uncalibrate(cK, project(transform_to(x, p))); + // Again, here we use a ExpressionFactor + graph.push_back(ExpressionFactor(measurementNoise, measurement, prediction)); + } + } + + // Add prior on first point to constrain scale, again with ExpressionFactor + noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1); + graph.push_back(ExpressionFactor(pointNoise, points[0], Point3_('l', 0))); + + // Create perturbed initial + Values initialEstimate; + for (size_t i = 0; i < poses.size(); ++i) + initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)))); + for (size_t j = 0; j < points.size(); ++j) + initialEstimate.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15))); + cout << "initial error = " << graph.error(initialEstimate) << endl; + + /* Optimize the graph and print results */ + Values result = DoglegOptimizer(graph, initialEstimate).optimize(); + cout << "final error = " << graph.error(result) << endl; + + return 0; +} +/* ************************************************************************* */ + diff --git a/gtsam_unstable/geometry/BearingS2.h b/gtsam_unstable/geometry/BearingS2.h index d48490ccc..4c84bbe56 100644 --- a/gtsam_unstable/geometry/BearingS2.h +++ b/gtsam_unstable/geometry/BearingS2.h @@ -15,7 +15,7 @@ namespace gtsam { -class GTSAM_UNSTABLE_EXPORT BearingS2 : public DerivedValue { +class GTSAM_UNSTABLE_EXPORT BearingS2 { protected: Rot2 azimuth_, elevation_; diff --git a/gtsam_unstable/geometry/InvDepthCamera3.h b/gtsam_unstable/geometry/InvDepthCamera3.h index 4eb7992a2..23d698d86 100644 --- a/gtsam_unstable/geometry/InvDepthCamera3.h +++ b/gtsam_unstable/geometry/InvDepthCamera3.h @@ -15,8 +15,6 @@ #include #include #include -#include -#include #include #include #include @@ -71,10 +69,9 @@ public: * @param inv inverse depth * @return Point3 */ - static gtsam::Point3 invDepthTo3D(const gtsam::LieVector& pw, const gtsam::LieScalar& inv) { - gtsam::Point3 ray_base(pw.vector().segment(0,3)); + static gtsam::Point3 invDepthTo3D(const Vector5& pw, double rho) { + gtsam::Point3 ray_base(pw.segment(0,3)); double theta = pw(3), phi = pw(4); - double rho = inv.value(); // inverse depth gtsam::Point3 m(cos(theta)*cos(phi),sin(theta)*cos(phi),sin(phi)); return ray_base + m/rho; } @@ -84,15 +81,14 @@ public: * @param H1 is the jacobian w.r.t. [pose3 calibration] * @param H2 is the jacobian w.r.t. inv_depth_landmark */ - inline gtsam::Point2 project(const gtsam::LieVector& pw, - const gtsam::LieScalar& inv_depth, + inline gtsam::Point2 project(const Vector5& pw, + double rho, boost::optional H1 = boost::none, boost::optional H2 = boost::none, boost::optional H3 = boost::none) const { - gtsam::Point3 ray_base(pw.vector().segment(0,3)); + gtsam::Point3 ray_base(pw.segment(0,3)); double theta = pw(3), phi = pw(4); - double rho = inv_depth.value(); // inverse depth gtsam::Point3 m(cos(theta)*cos(phi),sin(theta)*cos(phi),sin(phi)); const gtsam::Point3 landmark = ray_base + m/rho; @@ -104,7 +100,7 @@ public: } else { gtsam::Matrix J2; - gtsam::Point2 uv= camera.project(landmark,H1, J2); + gtsam::Point2 uv= camera.project(landmark,H1, J2, boost::none); if (H1) { *H1 = (*H1) * gtsam::eye(6); } @@ -157,7 +153,7 @@ public: * useful for point initialization */ - inline std::pair backproject(const gtsam::Point2& pi, const double depth) const { + inline std::pair backproject(const gtsam::Point2& pi, const double depth) const { const gtsam::Point2 pn = k_->calibrate(pi); gtsam::Point3 pc(pn.x(), pn.y(), 1.0); pc = pc/pc.norm(); @@ -166,8 +162,8 @@ public: gtsam::Point3 ray = pw - pt; double theta = atan2(ray.y(), ray.x()); // longitude double phi = atan2(ray.z(), sqrt(ray.x()*ray.x()+ray.y()*ray.y())); - return std::make_pair(gtsam::LieVector((Vector(5) << pt.x(),pt.y(),pt.z(), theta, phi)), - gtsam::LieScalar(1./depth)); + return std::make_pair(Vector5((Vector(5) << pt.x(),pt.y(),pt.z(), theta, phi)), + double(1./depth)); } private: diff --git a/gtsam_unstable/geometry/Pose3Upright.h b/gtsam_unstable/geometry/Pose3Upright.h index 2a814b915..22aab5d44 100644 --- a/gtsam_unstable/geometry/Pose3Upright.h +++ b/gtsam_unstable/geometry/Pose3Upright.h @@ -22,7 +22,7 @@ namespace gtsam { * @ingroup geometry * \nosubgrouping */ -class GTSAM_UNSTABLE_EXPORT Pose3Upright : public DerivedValue { +class GTSAM_UNSTABLE_EXPORT Pose3Upright { public: static const size_t dimension = 4; @@ -126,11 +126,9 @@ public: /// Log map at identity - return the canonical coordinates of this rotation static Vector Logmap(const Pose3Upright& p); -private: - /// @} - /// @name Advanced Interface - /// @{ + +private: // Serialization function friend class boost::serialization::access; @@ -142,4 +140,18 @@ private: }; // \class Pose3Upright +// Define GTSAM traits +namespace traits { + +template<> +struct is_manifold : public boost::true_type { +}; + +template<> +struct dimension : public boost::integral_constant { +}; + +} + + } // \namespace gtsam diff --git a/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp b/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp index b477d3e44..e72afdc25 100644 --- a/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp +++ b/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp @@ -29,10 +29,10 @@ TEST( InvDepthFactor, Project1) { Point2 expected_uv = level_camera.project(landmark); InvDepthCamera3 inv_camera(level_pose, K); - LieVector inv_landmark((Vector(5) << 1., 0., 1., 0., 0.)); - LieScalar inv_depth(1./4); + Vector5 inv_landmark((Vector(5) << 1., 0., 1., 0., 0.)); + double inv_depth(1./4); Point2 actual_uv = inv_camera.project(inv_landmark, inv_depth); - EXPECT(assert_equal(expected_uv, actual_uv)); + EXPECT(assert_equal(expected_uv, actual_uv,1e-8)); EXPECT(assert_equal(Point2(640,480), actual_uv)); } @@ -45,8 +45,8 @@ TEST( InvDepthFactor, Project2) { Point2 expected = level_camera.project(landmark); InvDepthCamera3 inv_camera(level_pose, K); - LieVector diag_landmark((Vector(5) << 0., 0., 1., M_PI/4., atan(1.0/sqrt(2.0)))); - LieScalar inv_depth(1/sqrt(3.0)); + Vector5 diag_landmark((Vector(5) << 0., 0., 1., M_PI/4., atan(1.0/sqrt(2.0)))); + double inv_depth(1/sqrt(3.0)); Point2 actual = inv_camera.project(diag_landmark, inv_depth); EXPECT(assert_equal(expected, actual)); } @@ -60,8 +60,8 @@ TEST( InvDepthFactor, Project3) { Point2 expected = level_camera.project(landmark); InvDepthCamera3 inv_camera(level_pose, K); - LieVector diag_landmark((Vector(5) << 0., 0., 0., M_PI/4., atan(2./sqrt(2.0)))); - LieScalar inv_depth( 1./sqrt(1.0+1+4)); + Vector5 diag_landmark((Vector(5) << 0., 0., 0., M_PI/4., atan(2./sqrt(2.0)))); + double inv_depth( 1./sqrt(1.0+1+4)); Point2 actual = inv_camera.project(diag_landmark, inv_depth); EXPECT(assert_equal(expected, actual)); } @@ -75,81 +75,81 @@ TEST( InvDepthFactor, Project4) { Point2 expected = level_camera.project(landmark); InvDepthCamera3 inv_camera(level_pose, K); - LieVector diag_landmark((Vector(5) << 0., 0., 0., atan(4.0/1), atan(2./sqrt(1.+16.)))); - LieScalar inv_depth(1./sqrt(1.+16.+4.)); + Vector5 diag_landmark((Vector(5) << 0., 0., 0., atan(4.0/1), atan(2./sqrt(1.+16.)))); + double inv_depth(1./sqrt(1.+16.+4.)); Point2 actual = inv_camera.project(diag_landmark, inv_depth); EXPECT(assert_equal(expected, actual)); } /* ************************************************************************* */ -Point2 project_(const Pose3& pose, const LieVector& landmark, const LieScalar& inv_depth) { +Point2 project_(const Pose3& pose, const Vector5& landmark, const double& inv_depth) { return InvDepthCamera3(pose,K).project(landmark, inv_depth); } TEST( InvDepthFactor, Dproject_pose) { - LieVector landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2)); - LieScalar inv_depth(1./4); - Matrix expected = numericalDerivative31(project_,level_pose, landmark, inv_depth); + Vector5 landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2)); + double inv_depth(1./4); + Matrix expected = numericalDerivative31(project_,level_pose, landmark, inv_depth); InvDepthCamera3 inv_camera(level_pose,K); Matrix actual; - Point2 uv = inv_camera.project(landmark, inv_depth, actual, boost::none, boost::none); + inv_camera.project(landmark, inv_depth, actual, boost::none, boost::none); EXPECT(assert_equal(expected,actual,1e-6)); } /* ************************************************************************* */ TEST( InvDepthFactor, Dproject_landmark) { - LieVector landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2)); - LieScalar inv_depth(1./4); - Matrix expected = numericalDerivative32(project_,level_pose, landmark, inv_depth); + Vector5 landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2)); + double inv_depth(1./4); + Matrix expected = numericalDerivative32(project_,level_pose, landmark, inv_depth); InvDepthCamera3 inv_camera(level_pose,K); Matrix actual; - Point2 uv = inv_camera.project(landmark, inv_depth, boost::none, actual, boost::none); + inv_camera.project(landmark, inv_depth, boost::none, actual, boost::none); EXPECT(assert_equal(expected,actual,1e-7)); } /* ************************************************************************* */ TEST( InvDepthFactor, Dproject_inv_depth) { - LieVector landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2)); - LieScalar inv_depth(1./4); - Matrix expected = numericalDerivative33(project_,level_pose, landmark, inv_depth); + Vector5 landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2)); + double inv_depth(1./4); + Matrix expected = numericalDerivative33(project_,level_pose, landmark, inv_depth); InvDepthCamera3 inv_camera(level_pose,K); Matrix actual; - Point2 uv = inv_camera.project(landmark, inv_depth, boost::none, boost::none, actual); + inv_camera.project(landmark, inv_depth, boost::none, boost::none, actual); EXPECT(assert_equal(expected,actual,1e-7)); } /* ************************************************************************* */ TEST(InvDepthFactor, backproject) { - LieVector expected((Vector(5) << 0.,0.,1., 0.1,0.2)); - LieScalar inv_depth(1./4); + Vector expected((Vector(5) << 0.,0.,1., 0.1,0.2)); + double inv_depth(1./4); InvDepthCamera3 inv_camera(level_pose,K); Point2 z = inv_camera.project(expected, inv_depth); - LieVector actual_vec; - LieScalar actual_inv; + Vector5 actual_vec; + double actual_inv; boost::tie(actual_vec, actual_inv) = inv_camera.backproject(z, 4); EXPECT(assert_equal(expected,actual_vec,1e-7)); - EXPECT(assert_equal(inv_depth,actual_inv,1e-7)); + EXPECT_DOUBLES_EQUAL(inv_depth,actual_inv,1e-7); } /* ************************************************************************* */ TEST(InvDepthFactor, backproject2) { // backwards facing camera - LieVector expected((Vector(5) << -5.,-5.,2., 3., -0.1)); - LieScalar inv_depth(1./10); + Vector expected((Vector(5) << -5.,-5.,2., 3., -0.1)); + double inv_depth(1./10); InvDepthCamera3 inv_camera(Pose3(Rot3::ypr(1.5,0.1, -1.5), Point3(-5, -5, 2)),K); Point2 z = inv_camera.project(expected, inv_depth); - LieVector actual_vec; - LieScalar actual_inv; + Vector5 actual_vec; + double actual_inv; boost::tie(actual_vec, actual_inv) = inv_camera.backproject(z, 10); EXPECT(assert_equal(expected,actual_vec,1e-7)); - EXPECT(assert_equal(inv_depth,actual_inv,1e-7)); + EXPECT_DOUBLES_EQUAL(inv_depth,actual_inv,1e-7); } /* ************************************************************************* */ diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h index f2223c2f4..95e635516 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/gtsam_unstable/gtsam_unstable.h @@ -4,14 +4,15 @@ // specify the classes from gtsam we are using virtual class gtsam::Value; -virtual class gtsam::LieScalar; -virtual class gtsam::LieVector; -virtual class gtsam::Point2; -virtual class gtsam::Rot2; -virtual class gtsam::Pose2; -virtual class gtsam::Point3; -virtual class gtsam::Rot3; -virtual class gtsam::Pose3; +class gtsam::Vector6; +class gtsam::LieScalar; +class gtsam::LieVector; +class gtsam::Point2; +class gtsam::Rot2; +class gtsam::Pose2; +class gtsam::Point3; +class gtsam::Rot3; +class gtsam::Pose3; virtual class gtsam::noiseModel::Base; virtual class gtsam::noiseModel::Gaussian; virtual class gtsam::imuBias::ConstantBias; @@ -23,8 +24,8 @@ virtual class gtsam::NoiseModelFactor4; virtual class gtsam::GaussianFactor; virtual class gtsam::HessianFactor; virtual class gtsam::JacobianFactor; -virtual class gtsam::Cal3_S2; -virtual class gtsam::Cal3DS2; +class gtsam::Cal3_S2; +class gtsam::Cal3DS2; class gtsam::GaussianFactorGraph; class gtsam::NonlinearFactorGraph; class gtsam::Ordering; @@ -48,7 +49,7 @@ class Dummy { }; #include -virtual class PoseRTV : gtsam::Value { +class PoseRTV { PoseRTV(); PoseRTV(Vector rtv); PoseRTV(const gtsam::Point3& pt, const gtsam::Rot3& rot, const gtsam::Point3& vel); @@ -99,7 +100,7 @@ virtual class PoseRTV : gtsam::Value { }; #include -virtual class Pose3Upright : gtsam::Value { +class Pose3Upright { Pose3Upright(); Pose3Upright(const gtsam::Pose3Upright& x); Pose3Upright(const gtsam::Rot2& bearing, const gtsam::Point3& t); @@ -137,7 +138,7 @@ virtual class Pose3Upright : gtsam::Value { }; // \class Pose3Upright #include -virtual class BearingS2 : gtsam::Value { +class BearingS2 { BearingS2(); BearingS2(double azimuth, double elevation); BearingS2(const gtsam::Rot2& azimuth, const gtsam::Rot2& elevation); @@ -515,20 +516,19 @@ virtual class PendulumFactorPk1 : gtsam::NonlinearFactor { Vector evaluateError(const gtsam::LieScalar& pk1, const gtsam::LieScalar& qk, const gtsam::LieScalar& qk1) const; }; -#include #include virtual class Reconstruction : gtsam::NonlinearFactor { Reconstruction(size_t gKey1, size_t gKey, size_t xiKey, double h); - Vector evaluateError(const gtsam::Pose3& gK1, const gtsam::Pose3& gK, const gtsam::LieVector& xiK) const; + Vector evaluateError(const gtsam::Pose3& gK1, const gtsam::Pose3& gK, const gtsam::Vector6& xiK) const; }; virtual class DiscreteEulerPoincareHelicopter : gtsam::NonlinearFactor { DiscreteEulerPoincareHelicopter(size_t xiKey, size_t xiKey_1, size_t gKey, double h, Matrix Inertia, Vector Fu, double m); - Vector evaluateError(const gtsam::LieVector& xiK, const gtsam::LieVector& xiK_1, const gtsam::Pose3& gK) const; + Vector evaluateError(const gtsam::Vector6& xiK, const gtsam::Vector6& xiK_1, const gtsam::Pose3& gK) const; }; //************************************************************************* diff --git a/gtsam_unstable/nonlinear/AdaptAutoDiff.h b/gtsam_unstable/nonlinear/AdaptAutoDiff.h new file mode 100644 index 000000000..96978d9cf --- /dev/null +++ b/gtsam_unstable/nonlinear/AdaptAutoDiff.h @@ -0,0 +1,94 @@ +/* ---------------------------------------------------------------------------- + + * 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 AdaptAutoDiff.h + * @date October 22, 2014 + * @author Frank Dellaert + * @brief Adaptor for Ceres style auto-differentiated functions + */ + +#pragma once + +#include +#include + +namespace gtsam { + +/// Adapt ceres-style autodiff +template +class AdaptAutoDiff { + + static const int N = traits::dimension::value; + static const int M1 = traits::dimension::value; + static const int M2 = traits::dimension::value; + + typedef Eigen::Matrix RowMajor1; + typedef Eigen::Matrix RowMajor2; + + typedef Canonical CanonicalT; + typedef Canonical Canonical1; + typedef Canonical Canonical2; + + typedef typename CanonicalT::vector VectorT; + typedef typename Canonical1::vector Vector1; + typedef typename Canonical2::vector Vector2; + + // Instantiate function and charts + CanonicalT chartT; + Canonical1 chart1; + Canonical2 chart2; + F f; + +public: + + typedef Eigen::Matrix JacobianTA1; + typedef Eigen::Matrix JacobianTA2; + + T operator()(const A1& a1, const A2& a2, boost::optional H1 = + boost::none, boost::optional H2 = boost::none) { + + using ceres::internal::AutoDiff; + + // Make arguments + Vector1 v1 = chart1.local(a1); + Vector2 v2 = chart2.local(a2); + + bool success; + VectorT result; + + if (H1 || H2) { + + // Get derivatives with AutoDiff + double *parameters[] = { v1.data(), v2.data() }; + double rowMajor1[N * M1], rowMajor2[N * M2]; // om the stack + double *jacobians[] = { rowMajor1, rowMajor2 }; + success = AutoDiff::Differentiate(f, parameters, 2, + result.data(), jacobians); + + // Convert from row-major to columnn-major + // TODO: if this is a bottleneck (probably not!) fix Autodiff to be Column-Major + *H1 = Eigen::Map(rowMajor1); + *H2 = Eigen::Map(rowMajor2); + + } else { + // Apply the mapping, to get result + success = f(v1.data(), v2.data(), result.data()); + } + if (!success) + throw std::runtime_error( + "AdaptAutoDiff: function call resulted in failure"); + return chartT.retract(result); + } + +}; + +} diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h new file mode 100644 index 000000000..40fc54751 --- /dev/null +++ b/gtsam_unstable/nonlinear/Expression-inl.h @@ -0,0 +1,811 @@ +/* ---------------------------------------------------------------------------- + + * 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 Expression-inl.h + * @date September 18, 2014 + * @author Frank Dellaert + * @author Paul Furgale + * @brief Internals for Expression.h, not for general consumption + */ + +#pragma once + +#include +#include +#include +#include +#include + +#include +#include +#include + +// template meta-programming headers +#include +#include +#include +#include +#include +#include +#include +#include +#include +namespace MPL = boost::mpl::placeholders; + +#include // for placement new +class ExpressionFactorBinaryTest; +// Forward declare for testing + +namespace gtsam { + +template +class Expression; + +/** + * Expressions are designed to write their derivatives into an already allocated + * Jacobian of the correct size, of type VerticalBlockMatrix. + * The JacobianMap provides a mapping from keys to the underlying blocks. + */ +class JacobianMap { + const FastVector& keys_; + VerticalBlockMatrix& Ab_; +public: + JacobianMap(const FastVector& keys, VerticalBlockMatrix& Ab) : + keys_(keys), Ab_(Ab) { + } + /// Access via key + VerticalBlockMatrix::Block operator()(Key key) { + FastVector::const_iterator it = std::find(keys_.begin(), keys_.end(), + key); + DenseIndex block = it - keys_.begin(); + return Ab_(block); + } +}; + +//----------------------------------------------------------------------------- +/** + * The CallRecord class stores the Jacobians of applying a function + * with respect to each of its arguments. It also stores an executation trace + * (defined below) for each of its arguments. + * + * It is sub-classed in the function-style ExpressionNode sub-classes below. + */ +template +struct CallRecord { + static size_t const N = 0; + virtual void print(const std::string& indent) const { + } + virtual void startReverseAD(JacobianMap& jacobians) const { + } + virtual void reverseAD(const Matrix& dFdT, JacobianMap& jacobians) const { + } + typedef Eigen::Matrix Jacobian2T; + virtual void reverseAD2(const Jacobian2T& dFdT, + JacobianMap& jacobians) const { + } +}; + +//----------------------------------------------------------------------------- +/// Handle Leaf Case: reverseAD ends here, by writing a matrix into Jacobians +template +void handleLeafCase(const Eigen::Matrix& dTdA, + JacobianMap& jacobians, Key key) { + jacobians(key).block(0, 0) += dTdA; // block makes HUGE difference +} +/// Handle Leaf Case for Dynamic Matrix type (slower) +template<> +inline void handleLeafCase( + const Eigen::Matrix& dTdA, + JacobianMap& jacobians, Key key) { + jacobians(key) += dTdA; +} + +//----------------------------------------------------------------------------- +/** + * The ExecutionTrace class records a tree-structured expression's execution. + * + * The class looks a bit complicated but it is so for performance. + * It is a tagged union that obviates the need to create + * a ExecutionTrace subclass for Constants and Leaf Expressions. Instead + * the key for the leaf is stored in the space normally used to store a + * CallRecord*. Nothing is stored for a Constant. + * + * A full execution trace of a Binary(Unary(Binary(Leaf,Constant)),Leaf) would be: + * Trace(Function) -> + * BinaryRecord with two traces in it + * trace1(Function) -> + * UnaryRecord with one trace in it + * trace1(Function) -> + * BinaryRecord with two traces in it + * trace1(Leaf) + * trace2(Constant) + * trace2(Leaf) + * Hence, there are three Record structs, written to memory by traceExecution + */ +template +class ExecutionTrace { + static const int Dim = traits::dimension::value; + enum { + Constant, Leaf, Function + } kind; + union { + Key key; + CallRecord* ptr; + } content; +public: + /// Pointer always starts out as a Constant + ExecutionTrace() : + kind(Constant) { + } + /// Change pointer to a Leaf Record + void setLeaf(Key key) { + kind = Leaf; + content.key = key; + } + /// Take ownership of pointer to a Function Record + void setFunction(CallRecord* record) { + kind = Function; + content.ptr = record; + } + /// Print + void print(const std::string& indent = "") const { + if (kind == Constant) + std::cout << indent << "Constant" << std::endl; + else if (kind == Leaf) + std::cout << indent << "Leaf, key = " << content.key << std::endl; + else if (kind == Function) { + std::cout << indent << "Function" << std::endl; + content.ptr->print(indent + " "); + } + } + /// Return record pointer, quite unsafe, used only for testing + template + boost::optional record() { + if (kind != Function) + return boost::none; + else { + Record* p = dynamic_cast(content.ptr); + return p ? boost::optional(p) : boost::none; + } + } + /** + * *** This is the main entry point for reverseAD, called from Expression *** + * Called only once, either inserts I into Jacobians (Leaf) or starts AD (Function) + */ + typedef Eigen::Matrix JacobianTT; + void startReverseAD(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); + } + // Either add to Jacobians (Leaf) or propagate (Function) + void reverseAD(const Matrix& dTdA, JacobianMap& jacobians) const { + if (kind == Leaf) + handleLeafCase(dTdA, jacobians, content.key); + else if (kind == Function) + content.ptr->reverseAD(dTdA, jacobians); + } + // Either add to Jacobians (Leaf) or propagate (Function) + typedef Eigen::Matrix Jacobian2T; + void reverseAD2(const Jacobian2T& dTdA, JacobianMap& jacobians) const { + if (kind == Leaf) + handleLeafCase(dTdA, jacobians, content.key); + else if (kind == Function) + content.ptr->reverseAD2(dTdA, jacobians); + } + + /// Define type so we can apply it as a meta-function + typedef ExecutionTrace type; +}; + +/// Primary template calls the generic Matrix reverseAD pipeline +template +struct Select { + typedef Eigen::Matrix::value> Jacobian; + static void reverseAD(const ExecutionTrace& trace, const Jacobian& dTdA, + JacobianMap& jacobians) { + trace.reverseAD(dTdA, jacobians); + } +}; + +/// Partially specialized template calls the 2-dimensional output version +template +struct Select<2, A> { + typedef Eigen::Matrix::value> Jacobian; + static void reverseAD(const ExecutionTrace& trace, const Jacobian& dTdA, + JacobianMap& jacobians) { + trace.reverseAD2(dTdA, jacobians); + } +}; + +//----------------------------------------------------------------------------- +/** + * Expression node. The superclass for objects that do the heavy lifting + * An Expression has a pointer to an ExpressionNode underneath + * allowing Expressions to have polymorphic behaviour even though they + * are passed by value. This is the same way boost::function works. + * http://loki-lib.sourceforge.net/html/a00652.html + */ +template +class ExpressionNode { + +protected: + + size_t traceSize_; + + /// Constructor, traceSize is size of the execution trace of expression rooted here + ExpressionNode(size_t traceSize = 0) : + traceSize_(traceSize) { + } + +public: + + /// Destructor + virtual ~ExpressionNode() { + } + + /// Return keys that play in this expression as a set + virtual std::set keys() const { + std::set keys; + return keys; + } + + /// Return dimensions for each argument, as a map + virtual void dims(std::map& map) const { + } + + // Return size needed for memory buffer in traceExecution + size_t traceSize() const { + return traceSize_; + } + + /// Return value + virtual T value(const Values& values) const = 0; + + /// Construct an execution trace for reverse AD + virtual T traceExecution(const Values& values, ExecutionTrace& trace, + char* raw) const = 0; +}; + +//----------------------------------------------------------------------------- +/// Constant Expression +template +class ConstantExpression: public ExpressionNode { + + /// The constant value + T constant_; + + /// Constructor with a value, yielding a constant + ConstantExpression(const T& value) : + constant_(value) { + } + + friend class Expression ; + +public: + + /// Return value + virtual T value(const Values& values) const { + return constant_; + } + + /// Construct an execution trace for reverse AD + virtual T traceExecution(const Values& values, ExecutionTrace& trace, + char* raw) const { + return constant_; + } +}; + +//----------------------------------------------------------------------------- +/// Leaf Expression +template > +class LeafExpression: public ExpressionNode { + typedef ChartValue value_type; // perhaps this can be something else like a std::pair ?? + + /// The key into values + Key key_; + + /// Constructor with a single key + LeafExpression(Key key) : + key_(key) { + } + // todo: do we need a virtual destructor here too? + + friend class Expression ; + +public: + + /// Return keys that play in this expression + virtual std::set keys() const { + std::set keys; + keys.insert(key_); + return keys; + } + + /// Return dimensions for each argument + virtual void dims(std::map& map) const { + // get dimension from the chart; only works for fixed dimension charts + map[key_] = traits::dimension::value; + } + + /// Return value + virtual const value_type& value(const Values& values) const { + return dynamic_cast(values.at(key_)); + } + + /// Construct an execution trace for reverse AD + virtual const value_type& traceExecution(const Values& values, + ExecutionTrace& trace, char* raw) const { + trace.setLeaf(key_); + return dynamic_cast(values.at(key_)); + } + +}; + +//----------------------------------------------------------------------------- +/// Leaf Expression, if no chart is given, assume default chart and value_type is just the plain value +template +class LeafExpression > : public ExpressionNode { + typedef T value_type; + + /// The key into values + Key key_; + + /// Constructor with a single key + LeafExpression(Key key) : + key_(key) { + } + // todo: do we need a virtual destructor here too? + + friend class Expression ; + +public: + + /// Return keys that play in this expression + virtual std::set keys() const { + std::set keys; + keys.insert(key_); + return keys; + } + + /// Return dimensions for each argument + virtual void dims(std::map& map) const { + map[key_] = traits::dimension::value; + } + + /// Return value + virtual T value(const Values& values) const { + return values.at(key_); + } + + /// Construct an execution trace for reverse AD + virtual T traceExecution(const Values& values, ExecutionTrace& trace, + char* raw) const { + trace.setLeaf(key_); + return values.at(key_); + } + +}; + +//----------------------------------------------------------------------------- +// Below we use the "Class Composition" technique described in the book +// C++ Template Metaprogramming: Concepts, Tools, and Techniques from Boost +// and Beyond. Abrahams, David; Gurtovoy, Aleksey. Pearson Education. +// to recursively generate a class, that will be the base for function nodes. +// +// The class generated, for three arguments A1, A2, and A3 will be +// +// struct Base1 : Argument, FunctionalBase { +// ... storage related to A1 ... +// ... methods that work on A1 ... +// }; +// +// struct Base2 : Argument, Base1 { +// ... storage related to A2 ... +// ... methods that work on A2 and (recursively) on A1 ... +// }; +// +// struct Base3 : Argument, Base2 { +// ... storage related to A3 ... +// ... methods that work on A3 and (recursively) on A2 and A1 ... +// }; +// +// struct FunctionalNode : Base3 { +// Provides convenience access to storage in hierarchy by using +// static_cast &>(*this) +// } +// +// All this magic happens when we generate the Base3 base class of FunctionalNode +// by invoking boost::mpl::fold over the meta-function GenerateFunctionalNode +// +// Similarly, the inner Record struct will be +// +// struct Record1 : JacobianTrace, CallRecord::value> { +// ... storage related to A1 ... +// ... methods that work on A1 ... +// }; +// +// struct Record2 : JacobianTrace, Record1 { +// ... storage related to A2 ... +// ... methods that work on A2 and (recursively) on A1 ... +// }; +// +// struct Record3 : JacobianTrace, Record2 { +// ... storage related to A3 ... +// ... methods that work on A3 and (recursively) on A2 and A1 ... +// }; +// +// struct Record : Record3 { +// Provides convenience access to storage in hierarchy by using +// static_cast &>(*this) +// } +// + +//----------------------------------------------------------------------------- + +/// meta-function to generate fixed-size JacobianTA type +template +struct Jacobian { + typedef Eigen::Matrix::value, + traits::dimension::value> type; +}; + +/// meta-function to generate JacobianTA optional reference +template +struct OptionalJacobian { + typedef Eigen::Matrix::value, + traits::dimension::value> Jacobian; + typedef boost::optional type; +}; + +/** + * Base case for recursive FunctionalNode class + */ +template +struct FunctionalBase: ExpressionNode { + static size_t const N = 0; // number of arguments + + typedef CallRecord::value> Record; + + /// Construct an execution trace for reverse AD + void trace(const Values& values, Record* record, char*& raw) const { + // base case: does not do anything + } +}; + +/** + * Building block for recursive FunctionalNode class + * The integer argument N is to guarantee a unique type signature, + * so we are guaranteed to be able to extract their values by static cast. + */ +template +struct Argument { + /// Expression that will generate value/derivatives for argument + boost::shared_ptr > expression; +}; + +/** + * Building block for Recursive Record Class + * Records the evaluation of a single argument in a functional expression + */ +template +struct JacobianTrace { + A value; + ExecutionTrace trace; + typename Jacobian::type dTdA; +}; + +/** + * Recursive Definition of Functional ExpressionNode + */ +template +struct GenerateFunctionalNode: Argument, Base { + + static size_t const N = Base::N + 1; ///< Number of arguments in hierarchy + typedef Argument This; ///< The storage we have direct access to + + /// Return keys that play in this expression + virtual std::set keys() const { + std::set keys = Base::keys(); + std::set myKeys = This::expression->keys(); + keys.insert(myKeys.begin(), myKeys.end()); + return keys; + } + + /// Return dimensions for each argument + virtual void dims(std::map& map) const { + Base::dims(map); + This::expression->dims(map); + } + + /// Recursive Record Class for Functional Expressions + struct Record: JacobianTrace, Base::Record { + + typedef T return_type; + typedef JacobianTrace This; + + /// Print to std::cout + virtual void print(const std::string& indent) const { + Base::Record::print(indent); + static const Eigen::IOFormat matlab(0, 1, " ", "; ", "", "", "[", "]"); + std::cout << This::dTdA.format(matlab) << std::endl; + This::trace.print(indent); + } + + /// Start the reverse AD process + virtual void startReverseAD(JacobianMap& jacobians) const { + Base::Record::startReverseAD(jacobians); + Select::value, A>::reverseAD(This::trace, This::dTdA, + jacobians); + } + + /// Given df/dT, multiply in dT/dA and continue reverse AD process + virtual void reverseAD(const Matrix& dFdT, JacobianMap& jacobians) const { + Base::Record::reverseAD(dFdT, jacobians); + This::trace.reverseAD(dFdT * This::dTdA, jacobians); + } + + /// Version specialized to 2-dimensional output + typedef Eigen::Matrix::value> Jacobian2T; + virtual void reverseAD2(const Jacobian2T& dFdT, + JacobianMap& jacobians) const { + Base::Record::reverseAD2(dFdT, jacobians); + This::trace.reverseAD2(dFdT * This::dTdA, jacobians); + } + }; + + /// Construct an execution trace for reverse AD + void trace(const Values& values, Record* record, char*& raw) const { + Base::trace(values, record, raw); // recurse + // Write an Expression execution trace in record->trace + // Iff Constant or Leaf, this will not write to raw, only to trace. + // Iff the expression is functional, write all Records in raw buffer + // Return value of type T is recorded in record->value + record->Record::This::value = This::expression->traceExecution(values, + record->Record::This::trace, raw); + // raw is never modified by traceExecution, but if traceExecution has + // written in the buffer, the next caller expects we advance the pointer + raw += This::expression->traceSize(); + } +}; + +/** + * Recursive GenerateFunctionalNode class Generator + */ +template +struct FunctionalNode { + + /// The following typedef generates the recursively defined Base class + typedef typename boost::mpl::fold, + GenerateFunctionalNode >::type Base; + + /** + * The type generated by this meta-function derives from Base + * and adds access functions as well as the crucial [trace] function + */ + struct type: public Base { + + // Argument types and derived, note these are base 0 ! + // These are currently not used - useful for Phoenix in future +#ifdef EXPRESSIONS_PHOENIX + typedef TYPES Arguments; + typedef typename boost::mpl::transform >::type Jacobians; + typedef typename boost::mpl::transform >::type Optionals; +#endif + + /// Reset expression shared pointer + template + void reset(const boost::shared_ptr >& ptr) { + static_cast &>(*this).expression = ptr; + } + + /// Access Expression shared pointer + template + boost::shared_ptr > expression() const { + return static_cast const &>(*this).expression; + } + + /// Provide convenience access to Record storage + struct Record: public Base::Record { + + /// Access Value + template + const A& value() const { + return static_cast const &>(*this).value; + } + + /// Access Jacobian + template + typename Jacobian::type& jacobian() { + return static_cast&>(*this).dTdA; + } + + }; + + /// Construct an execution trace for reverse AD + Record* trace(const Values& values, char* raw) const { + + // Create the record and advance the pointer + Record* record = new (raw) Record(); + raw = (char*) (record + 1); + + // Record the traces for all arguments + // After this, the raw pointer is set to after what was written + Base::trace(values, record, raw); + + // Return the record for this function evaluation + return record; + } + }; +}; +//----------------------------------------------------------------------------- + +/// Unary Function Expression +template +class UnaryExpression: public FunctionalNode >::type { + +public: + + typedef boost::function::type)> Function; + typedef typename FunctionalNode >::type Base; + typedef typename Base::Record Record; + +private: + + Function function_; + + /// Constructor with a unary function f, and input argument e + UnaryExpression(Function f, const Expression& e1) : + function_(f) { + this->template reset(e1.root()); + ExpressionNode::traceSize_ = sizeof(Record) + e1.traceSize(); + } + + friend class Expression ; + +public: + + /// Return value + virtual T value(const Values& values) const { + return function_(this->template expression()->value(values), boost::none); + } + + /// Construct an execution trace for reverse AD + virtual T traceExecution(const Values& values, ExecutionTrace& trace, + char* raw) const { + + Record* record = Base::trace(values, raw); + trace.setFunction(record); + + return function_(record->template value(), + record->template jacobian()); + } +}; + +//----------------------------------------------------------------------------- +/// Binary Expression + +template +class BinaryExpression: public FunctionalNode >::type { + +public: + + typedef boost::function< + T(const A1&, const A2&, typename OptionalJacobian::type, + typename OptionalJacobian::type)> Function; + typedef typename FunctionalNode >::type Base; + typedef typename Base::Record Record; + +private: + + Function function_; + + /// Constructor with a ternary function f, and three input arguments + BinaryExpression(Function f, const Expression& e1, + const Expression& e2) : + function_(f) { + this->template reset(e1.root()); + this->template reset(e2.root()); + ExpressionNode::traceSize_ = // + sizeof(Record) + e1.traceSize() + e2.traceSize(); + } + + friend class Expression ; + friend class ::ExpressionFactorBinaryTest; + +public: + + /// Return value + virtual T value(const Values& values) const { + using boost::none; + return function_(this->template expression()->value(values), + this->template expression()->value(values), + none, none); + } + + /// Construct an execution trace for reverse AD + virtual T traceExecution(const Values& values, ExecutionTrace& trace, + char* raw) const { + + Record* record = Base::trace(values, raw); + trace.setFunction(record); + + return function_(record->template value(), + record->template value(), record->template jacobian(), + record->template jacobian()); + } +}; + +//----------------------------------------------------------------------------- +/// Ternary Expression + +template +class TernaryExpression: public FunctionalNode >::type { + +public: + + typedef boost::function< + T(const A1&, const A2&, const A3&, typename OptionalJacobian::type, + typename OptionalJacobian::type, + typename OptionalJacobian::type)> Function; + typedef typename FunctionalNode >::type Base; + typedef typename Base::Record Record; + +private: + + Function function_; + + /// Constructor with a ternary function f, and three input arguments + TernaryExpression(Function f, const Expression& e1, + const Expression& e2, const Expression& e3) : + function_(f) { + this->template reset(e1.root()); + this->template reset(e2.root()); + this->template reset(e3.root()); + ExpressionNode::traceSize_ = // + sizeof(Record) + e1.traceSize() + e2.traceSize() + e3.traceSize(); + } + + friend class Expression ; + +public: + + /// Return value + virtual T value(const Values& values) const { + using boost::none; + return function_(this->template expression()->value(values), + this->template expression()->value(values), + this->template expression()->value(values), + none, none, none); + } + + /// Construct an execution trace for reverse AD + virtual T traceExecution(const Values& values, ExecutionTrace& trace, + char* raw) const { + + Record* record = Base::trace(values, raw); + trace.setFunction(record); + + return function_( + record->template value(), record->template value(), + record->template value(), record->template jacobian(), + record->template jacobian(), record->template jacobian()); + } + +}; +//----------------------------------------------------------------------------- +} diff --git a/gtsam_unstable/nonlinear/Expression.h b/gtsam_unstable/nonlinear/Expression.h new file mode 100644 index 000000000..fc1efeb87 --- /dev/null +++ b/gtsam_unstable/nonlinear/Expression.h @@ -0,0 +1,184 @@ +/* ---------------------------------------------------------------------------- + + * 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 Expression.h + * @date September 18, 2014 + * @author Frank Dellaert + * @author Paul Furgale + * @brief Expressions for Block Automatic Differentiation + */ + +#pragma once + +#include "Expression-inl.h" +#include +#include + +namespace gtsam { + +/** + * Expression class that supports automatic differentiation + */ +template +class Expression { + +private: + + // Paul's trick shared pointer, polymorphic root of entire expression tree + boost::shared_ptr > root_; + +public: + + // Construct a constant expression + Expression(const T& value) : + root_(new ConstantExpression(value)) { + } + + // Construct a leaf expression, with Key + Expression(const Key& key) : + root_(new LeafExpression(key)) { + } + + // Construct a leaf expression, with Symbol + Expression(const Symbol& symbol) : + root_(new LeafExpression(symbol)) { + } + + // Construct a leaf expression, creating Symbol + Expression(unsigned char c, size_t j) : + root_(new LeafExpression(Symbol(c, j))) { + } + + /// Construct a nullary method expression + template + Expression(const Expression& expression, + T (A::*method)(typename OptionalJacobian::type) const) : + root_(new UnaryExpression(boost::bind(method, _1, _2), expression)) { + } + + /// Construct a unary function expression + template + Expression(typename UnaryExpression::Function function, + const Expression& expression) : + root_(new UnaryExpression(function, expression)) { + } + + /// Construct a unary method expression + template + Expression(const Expression& expression1, + T (A1::*method)(const A2&, typename OptionalJacobian::type, + typename OptionalJacobian::type) const, + const Expression& expression2) : + root_( + new BinaryExpression(boost::bind(method, _1, _2, _3, _4), + expression1, expression2)) { + } + + /// Construct a binary function expression + template + Expression(typename BinaryExpression::Function function, + const Expression& expression1, const Expression& expression2) : + root_( + new BinaryExpression(function, expression1, expression2)) { + } + + /// Construct a ternary function expression + template + Expression(typename TernaryExpression::Function function, + const Expression& expression1, const Expression& expression2, + const Expression& expression3) : + root_( + new TernaryExpression(function, expression1, + expression2, expression3)) { + } + + /// Return keys that play in this expression + std::set keys() const { + return root_->keys(); + } + + /// Return dimensions for each argument, as a map + void dims(std::map& map) const { + root_->dims(map); + } + + // Return size needed for memory buffer in traceExecution + size_t traceSize() const { + return root_->traceSize(); + } + + /// trace execution, very unsafe, for testing purposes only + T traceExecution(const Values& values, ExecutionTrace& trace, + char* raw) const { + return root_->traceExecution(values, trace, raw); + } + + /// Return value and derivatives, reverse AD version + T value(const Values& values, JacobianMap& jacobians) const { + // The following piece of code is absolutely crucial for performance. + // We allocate a block of memory on the stack, which can be done at runtime + // with modern C++ compilers. The traceExecution then fills this memory + // with an execution trace, made up entirely of "Record" structs, see + // the FunctionalNode class in expression-inl.h + size_t size = traceSize(); + char raw[size]; + ExecutionTrace trace; + T value(traceExecution(values, trace, raw)); + trace.startReverseAD(jacobians); + return value; + } + + /// Return value + T value(const Values& values) const { + return root_->value(values); + } + + const boost::shared_ptr >& root() const { + return root_; + } + + /// Define type so we can apply it as a meta-function + typedef Expression type; +}; + +// http://stackoverflow.com/questions/16260445/boost-bind-to-operator +template +struct apply_compose { + typedef T result_type; + static const int Dim = traits::dimension::value; + typedef Eigen::Matrix Jacobian; + T operator()(const T& x, const T& y, boost::optional H1, + boost::optional H2) const { + return x.compose(y, H1, H2); + } +}; + +/// Construct a product expression, assumes T::compose(T) -> T +template +Expression operator*(const Expression& expression1, + const Expression& expression2) { + return Expression(boost::bind(apply_compose(), _1, _2, _3, _4), + expression1, expression2); +} + +/// Construct an array of leaves +template +std::vector > createUnknowns(size_t n, char c, size_t start = 0) { + std::vector > unknowns; + unknowns.reserve(n); + for (size_t i = start; i < start + n; i++) + unknowns.push_back(Expression(c, i)); + return unknowns; +} + +} + diff --git a/gtsam_unstable/nonlinear/ExpressionFactor.h b/gtsam_unstable/nonlinear/ExpressionFactor.h new file mode 100644 index 000000000..37a89af6b --- /dev/null +++ b/gtsam_unstable/nonlinear/ExpressionFactor.h @@ -0,0 +1,143 @@ +/* ---------------------------------------------------------------------------- + + * 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 Expression.h + * @date September 18, 2014 + * @author Frank Dellaert + * @author Paul Furgale + * @brief Expressions for Block Automatic Differentiation + */ + +#include +#include +#include +#include +#include +#include + +namespace gtsam { + +/** + * Factor that supports arbitrary expressions via AD + */ +template +class ExpressionFactor: public NoiseModelFactor { + + T measurement_; ///< the measurement to be compared with the expression + Expression expression_; ///< the expression that is AD enabled + std::vector dimensions_; ///< dimensions of the Jacobian matrices + size_t augmentedCols_; ///< total number of columns + 1 (for RHS) + + static const int Dim = traits::dimension::value; + +public: + + /// Constructor + ExpressionFactor(const SharedNoiseModel& noiseModel, // + const T& measurement, const Expression& expression) : + measurement_(measurement), expression_(expression) { + if (!noiseModel) + throw std::invalid_argument("ExpressionFactor: no NoiseModel."); + if (noiseModel->dim() != Dim) + throw std::invalid_argument( + "ExpressionFactor was created with a NoiseModel of incorrect dimension."); + noiseModel_ = noiseModel; + + // Get dimensions of Jacobian matrices + // An Expression is assumed unmutable, so we do this now + std::map map; + expression_.dims(map); + size_t n = map.size(); + + keys_.resize(n); + boost::copy(map | boost::adaptors::map_keys, keys_.begin()); + + dimensions_.resize(n); + boost::copy(map | boost::adaptors::map_values, dimensions_.begin()); + + // Add sizes to know how much memory to allocate on stack in linearize + augmentedCols_ = std::accumulate(dimensions_.begin(), dimensions_.end(), 1); + +#ifdef DEBUG_ExpressionFactor + BOOST_FOREACH(size_t d, dimensions_) + std::cout << d << " "; + std::cout << " -> " << Dim << "x" << augmentedCols_ << std::endl; +#endif + } + + /** + * Error function *without* the NoiseModel, \f$ h(x)-z \f$. + * We override this method to provide + * both the function evaluation and its derivative(s) in H. + */ + virtual Vector unwhitenedError(const Values& x, + boost::optional&> H = boost::none) const { + if (H) { + // H should be pre-allocated + assert(H->size()==size()); + + VerticalBlockMatrix Ab(dimensions_, Dim); + + // Wrap keys and VerticalBlockMatrix into structure passed to expression_ + JacobianMap map(keys_, Ab); + Ab.matrix().setZero(); + + // Evaluate error to get Jacobians and RHS vector b + T value = expression_.value(x, map); // <<< Reverse AD happens here ! + + // Copy blocks into the vector of jacobians passed in + for (DenseIndex i = 0; i < size(); i++) + H->at(i) = Ab(i); + + return measurement_.localCoordinates(value); + } else { + const T& value = expression_.value(x); + return measurement_.localCoordinates(value); + } + } + + virtual boost::shared_ptr linearize(const Values& x) const { + + // Only linearize if the factor is active + if (!active(x)) + return boost::shared_ptr(); + + // Create a writeable JacobianFactor in advance + // In case noise model is constrained, we need to provide a noise model + bool constrained = noiseModel_->is_constrained(); + boost::shared_ptr factor( + constrained ? new JacobianFactor(keys_, dimensions_, Dim, + boost::static_pointer_cast(noiseModel_)->unit()) : + new JacobianFactor(keys_, dimensions_, Dim)); + + // Wrap keys and VerticalBlockMatrix into structure passed to expression_ + VerticalBlockMatrix& Ab = factor->matrixObject(); + JacobianMap map(keys_, Ab); + + // Zero out Jacobian so we can simply add to it + Ab.matrix().setZero(); + + // Evaluate error to get Jacobians and RHS vector b + T value = expression_.value(x, map); // <<< Reverse AD happens here ! + Ab(size()).col(0) = -measurement_.localCoordinates(value); + + // Whiten the corresponding system, Ab already contains RHS + Vector dummy(Dim); + noiseModel_->WhitenSystem(Ab.matrix(),dummy); + + return factor; + } +}; +// ExpressionFactor + +} + diff --git a/gtsam_unstable/nonlinear/ceres_autodiff.h b/gtsam_unstable/nonlinear/ceres_autodiff.h new file mode 100644 index 000000000..2a0ac8987 --- /dev/null +++ b/gtsam_unstable/nonlinear/ceres_autodiff.h @@ -0,0 +1,314 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// +// Computation of the Jacobian matrix for vector-valued functions of multiple +// variables, using automatic differentiation based on the implementation of +// dual numbers in jet.h. Before reading the rest of this file, it is adivsable +// to read jet.h's header comment in detail. +// +// The helper wrapper AutoDiff::Differentiate() computes the jacobian of +// functors with templated operator() taking this form: +// +// struct F { +// template +// bool operator()(const T *x, const T *y, ..., T *z) { +// // Compute z[] based on x[], y[], ... +// // return true if computation succeeded, false otherwise. +// } +// }; +// +// All inputs and outputs may be vector-valued. +// +// To understand how jets are used to compute the jacobian, a +// picture may help. Consider a vector-valued function, F, returning 3 +// dimensions and taking a vector-valued parameter of 4 dimensions: +// +// y x +// [ * ] F [ * ] +// [ * ] <--- [ * ] +// [ * ] [ * ] +// [ * ] +// +// Similar to the 2-parameter example for f described in jet.h, computing the +// jacobian dy/dx is done by substutiting a suitable jet object for x and all +// intermediate steps of the computation of F. Since x is has 4 dimensions, use +// a Jet. +// +// Before substituting a jet object for x, the dual components are set +// appropriately for each dimension of x: +// +// y x +// [ * | * * * * ] f [ * | 1 0 0 0 ] x0 +// [ * | * * * * ] <--- [ * | 0 1 0 0 ] x1 +// [ * | * * * * ] [ * | 0 0 1 0 ] x2 +// ---+--- [ * | 0 0 0 1 ] x3 +// | ^ ^ ^ ^ +// dy/dx | | | +----- infinitesimal for x3 +// | | +------- infinitesimal for x2 +// | +--------- infinitesimal for x1 +// +----------- infinitesimal for x0 +// +// The reason to set the internal 4x4 submatrix to the identity is that we wish +// to take the derivative of y separately with respect to each dimension of x. +// Each column of the 4x4 identity is therefore for a single component of the +// independent variable x. +// +// Then the jacobian of the mapping, dy/dx, is the 3x4 sub-matrix of the +// extended y vector, indicated in the above diagram. +// +// Functors with multiple parameters +// --------------------------------- +// In practice, it is often convenient to use a function f of two or more +// vector-valued parameters, for example, x[3] and z[6]. Unfortunately, the jet +// framework is designed for a single-parameter vector-valued input. The wrapper +// in this file addresses this issue adding support for functions with one or +// more parameter vectors. +// +// To support multiple parameters, all the parameter vectors are concatenated +// into one and treated as a single parameter vector, except that since the +// functor expects different inputs, we need to construct the jets as if they +// were part of a single parameter vector. The extended jets are passed +// separately for each parameter. +// +// For example, consider a functor F taking two vector parameters, p[2] and +// q[3], and producing an output y[4]: +// +// struct F { +// template +// bool operator()(const T *p, const T *q, T *z) { +// // ... +// } +// }; +// +// In this case, the necessary jet type is Jet. Here is a +// visualization of the jet objects in this case: +// +// Dual components for p ----+ +// | +// -+- +// y [ * | 1 0 | 0 0 0 ] --- p[0] +// [ * | 0 1 | 0 0 0 ] --- p[1] +// [ * | . . | + + + ] | +// [ * | . . | + + + ] v +// [ * | . . | + + + ] <--- F(p, q) +// [ * | . . | + + + ] ^ +// ^^^ ^^^^^ | +// dy/dp dy/dq [ * | 0 0 | 1 0 0 ] --- q[0] +// [ * | 0 0 | 0 1 0 ] --- q[1] +// [ * | 0 0 | 0 0 1 ] --- q[2] +// --+-- +// | +// Dual components for q --------------+ +// +// where the 4x2 submatrix (marked with ".") and 4x3 submatrix (marked with "+" +// of y in the above diagram are the derivatives of y with respect to p and q +// respectively. This is how autodiff works for functors taking multiple vector +// valued arguments (up to 6). +// +// Jacobian NULL pointers +// ---------------------- +// In general, the functions below will accept NULL pointers for all or some of +// the Jacobian parameters, meaning that those Jacobians will not be computed. + +#ifndef CERES_PUBLIC_INTERNAL_AUTODIFF_H_ +#define CERES_PUBLIC_INTERNAL_AUTODIFF_H_ + +#include + +#include +#include +#include +#include +#define DCHECK assert +#define DCHECK_GT(a,b) assert((a)>(b)) + +namespace ceres { +namespace internal { + +// Extends src by a 1st order pertubation for every dimension and puts it in +// dst. The size of src is N. Since this is also used for perturbations in +// blocked arrays, offset is used to shift which part of the jet the +// perturbation occurs. This is used to set up the extended x augmented by an +// identity matrix. The JetT type should be a Jet type, and T should be a +// numeric type (e.g. double). For example, +// +// 0 1 2 3 4 5 6 7 8 +// dst[0] [ * | . . | 1 0 0 | . . . ] +// dst[1] [ * | . . | 0 1 0 | . . . ] +// dst[2] [ * | . . | 0 0 1 | . . . ] +// +// is what would get put in dst if N was 3, offset was 3, and the jet type JetT +// was 8-dimensional. +template +inline void Make1stOrderPerturbation(int offset, const T* src, JetT* dst) { + DCHECK(src); + DCHECK(dst); + for (int j = 0; j < N; ++j) { + dst[j].a = src[j]; + dst[j].v.setZero(); + dst[j].v[offset + j] = T(1.0); + } +} + +// Takes the 0th order part of src, assumed to be a Jet type, and puts it in +// dst. This is used to pick out the "vector" part of the extended y. +template +inline void Take0thOrderPart(int M, const JetT *src, T dst) { + DCHECK(src); + for (int i = 0; i < M; ++i) { + dst[i] = src[i].a; + } +} + +// Takes N 1st order parts, starting at index N0, and puts them in the M x N +// matrix 'dst'. This is used to pick out the "matrix" parts of the extended y. +template +inline void Take1stOrderPart(const int M, const JetT *src, T *dst) { + DCHECK(src); + DCHECK(dst); + for (int i = 0; i < M; ++i) { + Eigen::Map >(dst + N * i, N) = + src[i].v.template segment(N0); + } +} + +// This is in a struct because default template parameters on a +// function are not supported in C++03 (though it is available in +// C++0x). N0 through N5 are the dimension of the input arguments to +// the user supplied functor. +template +struct AutoDiff { + static bool Differentiate(const Functor& functor, + T const *const *parameters, + int num_outputs, + T *function_value, + T **jacobians) { + // This block breaks the 80 column rule to keep it somewhat readable. + DCHECK_GT(num_outputs, 0); + DCHECK((!N1 && !N2 && !N3 && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) || + ((N1 > 0) && !N2 && !N3 && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) || + ((N1 > 0) && (N2 > 0) && !N3 && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) || + ((N1 > 0) && (N2 > 0) && (N3 > 0) && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) || + ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && !N5 && !N6 && !N7 && !N8 && !N9) || + ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && !N6 && !N7 && !N8 && !N9) || + ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && !N7 && !N8 && !N9) || + ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && !N8 && !N9) || + ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && (N8 > 0) && !N9) || + ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && (N8 > 0) && (N9 > 0))); + + typedef Jet JetT; + FixedArray x( + N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7 + N8 + N9 + num_outputs); + + // These are the positions of the respective jets in the fixed array x. + const int jet0 = 0; + const int jet1 = N0; + const int jet2 = N0 + N1; + const int jet3 = N0 + N1 + N2; + const int jet4 = N0 + N1 + N2 + N3; + const int jet5 = N0 + N1 + N2 + N3 + N4; + const int jet6 = N0 + N1 + N2 + N3 + N4 + N5; + const int jet7 = N0 + N1 + N2 + N3 + N4 + N5 + N6; + const int jet8 = N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7; + const int jet9 = N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7 + N8; + + const JetT *unpacked_parameters[10] = { + x.get() + jet0, + x.get() + jet1, + x.get() + jet2, + x.get() + jet3, + x.get() + jet4, + x.get() + jet5, + x.get() + jet6, + x.get() + jet7, + x.get() + jet8, + x.get() + jet9, + }; + + JetT* output = x.get() + N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7 + N8 + N9; + +#define CERES_MAKE_1ST_ORDER_PERTURBATION(i) \ + if (N ## i) { \ + internal::Make1stOrderPerturbation( \ + jet ## i, \ + parameters[i], \ + x.get() + jet ## i); \ + } + CERES_MAKE_1ST_ORDER_PERTURBATION(0); + CERES_MAKE_1ST_ORDER_PERTURBATION(1); + CERES_MAKE_1ST_ORDER_PERTURBATION(2); + CERES_MAKE_1ST_ORDER_PERTURBATION(3); + CERES_MAKE_1ST_ORDER_PERTURBATION(4); + CERES_MAKE_1ST_ORDER_PERTURBATION(5); + CERES_MAKE_1ST_ORDER_PERTURBATION(6); + CERES_MAKE_1ST_ORDER_PERTURBATION(7); + CERES_MAKE_1ST_ORDER_PERTURBATION(8); + CERES_MAKE_1ST_ORDER_PERTURBATION(9); +#undef CERES_MAKE_1ST_ORDER_PERTURBATION + + if (!VariadicEvaluate::Call( + functor, unpacked_parameters, output)) { + return false; + } + + internal::Take0thOrderPart(num_outputs, output, function_value); + +#define CERES_TAKE_1ST_ORDER_PERTURBATION(i) \ + if (N ## i) { \ + if (jacobians[i]) { \ + internal::Take1stOrderPart(num_outputs, \ + output, \ + jacobians[i]); \ + } \ + } + CERES_TAKE_1ST_ORDER_PERTURBATION(0); + CERES_TAKE_1ST_ORDER_PERTURBATION(1); + CERES_TAKE_1ST_ORDER_PERTURBATION(2); + CERES_TAKE_1ST_ORDER_PERTURBATION(3); + CERES_TAKE_1ST_ORDER_PERTURBATION(4); + CERES_TAKE_1ST_ORDER_PERTURBATION(5); + CERES_TAKE_1ST_ORDER_PERTURBATION(6); + CERES_TAKE_1ST_ORDER_PERTURBATION(7); + CERES_TAKE_1ST_ORDER_PERTURBATION(8); + CERES_TAKE_1ST_ORDER_PERTURBATION(9); +#undef CERES_TAKE_1ST_ORDER_PERTURBATION + return true; + } +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_PUBLIC_INTERNAL_AUTODIFF_H_ diff --git a/gtsam_unstable/nonlinear/ceres_eigen.h b/gtsam_unstable/nonlinear/ceres_eigen.h new file mode 100644 index 000000000..18a602cf4 --- /dev/null +++ b/gtsam_unstable/nonlinear/ceres_eigen.h @@ -0,0 +1,93 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_EIGEN_H_ +#define CERES_INTERNAL_EIGEN_H_ + +#include + +namespace ceres { + +typedef Eigen::Matrix Vector; +typedef Eigen::Matrix Matrix; +typedef Eigen::Map VectorRef; +typedef Eigen::Map MatrixRef; +typedef Eigen::Map ConstVectorRef; +typedef Eigen::Map ConstMatrixRef; + +// Column major matrices for DenseSparseMatrix/DenseQRSolver +typedef Eigen::Matrix ColMajorMatrix; + +typedef Eigen::Map > ColMajorMatrixRef; + +typedef Eigen::Map > ConstColMajorMatrixRef; + + + +// C++ does not support templated typdefs, thus the need for this +// struct so that we can support statically sized Matrix and Maps. +template +struct EigenTypes { + typedef Eigen::Matrix + Matrix; + + typedef Eigen::Map< + Eigen::Matrix > + MatrixRef; + + typedef Eigen::Matrix + Vector; + + typedef Eigen::Map < + Eigen::Matrix > + VectorRef; + + + typedef Eigen::Map< + const Eigen::Matrix > + ConstMatrixRef; + + typedef Eigen::Map < + const Eigen::Matrix > + ConstVectorRef; +}; + +} // namespace ceres + +#endif // CERES_INTERNAL_EIGEN_H_ diff --git a/gtsam_unstable/nonlinear/ceres_example.h b/gtsam_unstable/nonlinear/ceres_example.h new file mode 100644 index 000000000..45ec3428e --- /dev/null +++ b/gtsam_unstable/nonlinear/ceres_example.h @@ -0,0 +1,78 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// sameeragarwal@google.com (Sameer Agarwal) +// +// Some Ceres Snippets copied for testing + +#pragma once + +#include + +// Templated pinhole camera model for used with Ceres. The camera is +// parameterized using 9 parameters: 3 for rotation, 3 for translation, 1 for +// focal length and 2 for radial distortion. The principal point is not modeled +// (i.e. it is assumed be located at the image center). +struct SnavelyProjection { + + template + bool operator()(const T* const camera, const T* const point, + T* predicted) const { + // camera[0,1,2] are the angle-axis rotation. + T p[3]; + ceres::AngleAxisRotatePoint(camera, point, p); + + // camera[3,4,5] are the translation. + p[0] += camera[3]; + p[1] += camera[4]; + p[2] += camera[5]; + + // Compute the center of distortion. The sign change comes from + // the camera model that Noah Snavely's Bundler assumes, whereby + // the camera coordinate system has a negative z axis. + T xp = -p[0] / p[2]; + T yp = -p[1] / p[2]; + + // Apply second and fourth order radial distortion. + const T& l1 = camera[7]; + const T& l2 = camera[8]; + T r2 = xp * xp + yp * yp; + T distortion = T(1.0) + r2 * (l1 + l2 * r2); + + // Compute final projected point position. + const T& focal = camera[6]; + predicted[0] = focal * distortion * xp; + predicted[1] = focal * distortion * yp; + + return true; + } + +}; + + diff --git a/gtsam_unstable/nonlinear/ceres_fixed_array.h b/gtsam_unstable/nonlinear/ceres_fixed_array.h new file mode 100644 index 000000000..69a426379 --- /dev/null +++ b/gtsam_unstable/nonlinear/ceres_fixed_array.h @@ -0,0 +1,190 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: rennie@google.com (Jeffrey Rennie) +// Author: sanjay@google.com (Sanjay Ghemawat) -- renamed to FixedArray + +#ifndef CERES_PUBLIC_INTERNAL_FIXED_ARRAY_H_ +#define CERES_PUBLIC_INTERNAL_FIXED_ARRAY_H_ + +#include +#include +#include +#include + +namespace ceres { +namespace internal { + +// A FixedArray represents a non-resizable array of T where the +// length of the array does not need to be a compile time constant. +// +// FixedArray allocates small arrays inline, and large arrays on +// the heap. It is a good replacement for non-standard and deprecated +// uses of alloca() and variable length arrays (a GCC extension). +// +// FixedArray keeps performance fast for small arrays, because it +// avoids heap operations. It also helps reduce the chances of +// accidentally overflowing your stack if large input is passed to +// your function. +// +// Also, FixedArray is useful for writing portable code. Not all +// compilers support arrays of dynamic size. + +// Most users should not specify an inline_elements argument and let +// FixedArray<> automatically determine the number of elements +// to store inline based on sizeof(T). +// +// If inline_elements is specified, the FixedArray<> implementation +// will store arrays of length <= inline_elements inline. +// +// Finally note that unlike vector FixedArray will not zero-initialize +// simple types like int, double, bool, etc. +// +// Non-POD types will be default-initialized just like regular vectors or +// arrays. + +#if defined(_WIN64) + typedef __int64 ssize_t; +#elif defined(_WIN32) + typedef __int32 ssize_t; +#endif + +template +class FixedArray { + public: + // For playing nicely with stl: + typedef T value_type; + typedef T* iterator; + typedef T const* const_iterator; + typedef T& reference; + typedef T const& const_reference; + typedef T* pointer; + typedef std::ptrdiff_t difference_type; + typedef size_t size_type; + + // REQUIRES: n >= 0 + // Creates an array object that can store "n" elements. + // + // FixedArray will not zero-initialiaze POD (simple) types like int, + // double, bool, etc. + // Non-POD types will be default-initialized just like regular vectors or + // arrays. + explicit FixedArray(size_type n); + + // Releases any resources. + ~FixedArray(); + + // Returns the length of the array. + inline size_type size() const { return size_; } + + // Returns the memory size of the array in bytes. + inline size_t memsize() const { return size_ * sizeof(T); } + + // Returns a pointer to the underlying element array. + inline const T* get() const { return &array_[0].element; } + inline T* get() { return &array_[0].element; } + + // REQUIRES: 0 <= i < size() + // Returns a reference to the "i"th element. + inline T& operator[](size_type i) { + assert(i < size_); + return array_[i].element; + } + + // REQUIRES: 0 <= i < size() + // Returns a reference to the "i"th element. + inline const T& operator[](size_type i) const { + assert(i < size_); + return array_[i].element; + } + + inline iterator begin() { return &array_[0].element; } + inline iterator end() { return &array_[size_].element; } + + inline const_iterator begin() const { return &array_[0].element; } + inline const_iterator end() const { return &array_[size_].element; } + + private: + // Container to hold elements of type T. This is necessary to handle + // the case where T is a a (C-style) array. The size of InnerContainer + // and T must be the same, otherwise callers' assumptions about use + // of this code will be broken. + struct InnerContainer { + T element; + }; + + // How many elements should we store inline? + // a. If not specified, use a default of 256 bytes (256 bytes + // seems small enough to not cause stack overflow or unnecessary + // stack pollution, while still allowing stack allocation for + // reasonably long character arrays. + // b. Never use 0 length arrays (not ISO C++) + static const size_type S1 = ((inline_elements < 0) + ? (256/sizeof(T)) : inline_elements); + static const size_type S2 = (S1 <= 0) ? 1 : S1; + static const size_type kInlineElements = S2; + + size_type const size_; + InnerContainer* const array_; + + // Allocate some space, not an array of elements of type T, so that we can + // skip calling the T constructors and destructors for space we never use. + ManualConstructor inline_space_[kInlineElements]; +}; + +// Implementation details follow + +template +inline FixedArray::FixedArray(typename FixedArray::size_type n) + : size_(n), + array_((n <= kInlineElements + ? reinterpret_cast(inline_space_) + : new InnerContainer[n])) { + // Construct only the elements actually used. + if (array_ == reinterpret_cast(inline_space_)) { + for (size_t i = 0; i != size_; ++i) { + inline_space_[i].Init(); + } + } +} + +template +inline FixedArray::~FixedArray() { + if (array_ != reinterpret_cast(inline_space_)) { + delete[] array_; + } else { + for (size_t i = 0; i != size_; ++i) { + inline_space_[i].Destroy(); + } + } +} + +} // namespace internal +} // namespace ceres + +#endif // CERES_PUBLIC_INTERNAL_FIXED_ARRAY_H_ diff --git a/gtsam_unstable/nonlinear/ceres_fpclassify.h b/gtsam_unstable/nonlinear/ceres_fpclassify.h new file mode 100644 index 000000000..da8a4d086 --- /dev/null +++ b/gtsam_unstable/nonlinear/ceres_fpclassify.h @@ -0,0 +1,87 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// +// Portable floating point classification. The names are picked such that they +// do not collide with macros. For example, "isnan" in C99 is a macro and hence +// does not respect namespaces. +// +// TODO(keir): Finish porting! + +#ifndef CERES_PUBLIC_FPCLASSIFY_H_ +#define CERES_PUBLIC_FPCLASSIFY_H_ + +#if defined(_MSC_VER) +#include +#endif + +#include + +namespace ceres { + +#if defined(_MSC_VER) + +inline bool IsFinite (double x) { return _finite(x) != 0; } +inline bool IsInfinite(double x) { return _finite(x) == 0 && _isnan(x) == 0; } +inline bool IsNaN (double x) { return _isnan(x) != 0; } +inline bool IsNormal (double x) { + int classification = _fpclass(x); + return classification == _FPCLASS_NN || + classification == _FPCLASS_PN; +} + +#elif defined(ANDROID) && defined(_STLPORT_VERSION) + +// On Android, when using the STLPort, the C++ isnan and isnormal functions +// are defined as macros. +inline bool IsNaN (double x) { return isnan(x); } +inline bool IsNormal (double x) { return isnormal(x); } +// On Android NDK r6, when using STLPort, the isinf and isfinite functions are +// not available, so reimplement them. +inline bool IsInfinite(double x) { + return x == std::numeric_limits::infinity() || + x == -std::numeric_limits::infinity(); +} +inline bool IsFinite(double x) { + return !isnan(x) && !IsInfinite(x); +} + +# else + +// These definitions are for the normal Unix suspects. +inline bool IsFinite (double x) { return std::isfinite(x); } +inline bool IsInfinite(double x) { return std::isinf(x); } +inline bool IsNaN (double x) { return std::isnan(x); } +inline bool IsNormal (double x) { return std::isnormal(x); } + +#endif + +} // namespace ceres + +#endif // CERES_PUBLIC_FPCLASSIFY_H_ diff --git a/gtsam_unstable/nonlinear/ceres_jet.h b/gtsam_unstable/nonlinear/ceres_jet.h new file mode 100644 index 000000000..ed4834caf --- /dev/null +++ b/gtsam_unstable/nonlinear/ceres_jet.h @@ -0,0 +1,670 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// +// A simple implementation of N-dimensional dual numbers, for automatically +// computing exact derivatives of functions. +// +// While a complete treatment of the mechanics of automatic differentation is +// beyond the scope of this header (see +// http://en.wikipedia.org/wiki/Automatic_differentiation for details), the +// basic idea is to extend normal arithmetic with an extra element, "e," often +// denoted with the greek symbol epsilon, such that e != 0 but e^2 = 0. Dual +// numbers are extensions of the real numbers analogous to complex numbers: +// whereas complex numbers augment the reals by introducing an imaginary unit i +// such that i^2 = -1, dual numbers introduce an "infinitesimal" unit e such +// that e^2 = 0. Dual numbers have two components: the "real" component and the +// "infinitesimal" component, generally written as x + y*e. Surprisingly, this +// leads to a convenient method for computing exact derivatives without needing +// to manipulate complicated symbolic expressions. +// +// For example, consider the function +// +// f(x) = x^2 , +// +// evaluated at 10. Using normal arithmetic, f(10) = 100, and df/dx(10) = 20. +// Next, augument 10 with an infinitesimal to get: +// +// f(10 + e) = (10 + e)^2 +// = 100 + 2 * 10 * e + e^2 +// = 100 + 20 * e -+- +// -- | +// | +--- This is zero, since e^2 = 0 +// | +// +----------------- This is df/dx! +// +// Note that the derivative of f with respect to x is simply the infinitesimal +// component of the value of f(x + e). So, in order to take the derivative of +// any function, it is only necessary to replace the numeric "object" used in +// the function with one extended with infinitesimals. The class Jet, defined in +// this header, is one such example of this, where substitution is done with +// templates. +// +// To handle derivatives of functions taking multiple arguments, different +// infinitesimals are used, one for each variable to take the derivative of. For +// example, consider a scalar function of two scalar parameters x and y: +// +// f(x, y) = x^2 + x * y +// +// Following the technique above, to compute the derivatives df/dx and df/dy for +// f(1, 3) involves doing two evaluations of f, the first time replacing x with +// x + e, the second time replacing y with y + e. +// +// For df/dx: +// +// f(1 + e, y) = (1 + e)^2 + (1 + e) * 3 +// = 1 + 2 * e + 3 + 3 * e +// = 4 + 5 * e +// +// --> df/dx = 5 +// +// For df/dy: +// +// f(1, 3 + e) = 1^2 + 1 * (3 + e) +// = 1 + 3 + e +// = 4 + e +// +// --> df/dy = 1 +// +// To take the gradient of f with the implementation of dual numbers ("jets") in +// this file, it is necessary to create a single jet type which has components +// for the derivative in x and y, and passing them to a templated version of f: +// +// template +// T f(const T &x, const T &y) { +// return x * x + x * y; +// } +// +// // The "2" means there should be 2 dual number components. +// Jet x(0); // Pick the 0th dual number for x. +// Jet y(1); // Pick the 1st dual number for y. +// Jet z = f(x, y); +// +// LOG(INFO) << "df/dx = " << z.a[0] +// << "df/dy = " << z.a[1]; +// +// Most users should not use Jet objects directly; a wrapper around Jet objects, +// which makes computing the derivative, gradient, or jacobian of templated +// functors simple, is in autodiff.h. Even autodiff.h should not be used +// directly; instead autodiff_cost_function.h is typically the file of interest. +// +// For the more mathematically inclined, this file implements first-order +// "jets". A 1st order jet is an element of the ring +// +// T[N] = T[t_1, ..., t_N] / (t_1, ..., t_N)^2 +// +// which essentially means that each jet consists of a "scalar" value 'a' from T +// and a 1st order perturbation vector 'v' of length N: +// +// x = a + \sum_i v[i] t_i +// +// A shorthand is to write an element as x = a + u, where u is the pertubation. +// Then, the main point about the arithmetic of jets is that the product of +// perturbations is zero: +// +// (a + u) * (b + v) = ab + av + bu + uv +// = ab + (av + bu) + 0 +// +// which is what operator* implements below. Addition is simpler: +// +// (a + u) + (b + v) = (a + b) + (u + v). +// +// The only remaining question is how to evaluate the function of a jet, for +// which we use the chain rule: +// +// f(a + u) = f(a) + f'(a) u +// +// where f'(a) is the (scalar) derivative of f at a. +// +// By pushing these things through sufficiently and suitably templated +// functions, we can do automatic differentiation. Just be sure to turn on +// function inlining and common-subexpression elimination, or it will be very +// slow! +// +// WARNING: Most Ceres users should not directly include this file or know the +// details of how jets work. Instead the suggested method for automatic +// derivatives is to use autodiff_cost_function.h, which is a wrapper around +// both jets.h and autodiff.h to make taking derivatives of cost functions for +// use in Ceres easier. + +#ifndef CERES_PUBLIC_JET_H_ +#define CERES_PUBLIC_JET_H_ + +#include +#include +#include // NOLINT +#include +#include + +#include +#include + +namespace ceres { + +template +struct Jet { + enum { DIMENSION = N }; + + // Default-construct "a" because otherwise this can lead to false errors about + // uninitialized uses when other classes relying on default constructed T + // (where T is a Jet). This usually only happens in opt mode. Note that + // the C++ standard mandates that e.g. default constructed doubles are + // initialized to 0.0; see sections 8.5 of the C++03 standard. + Jet() : a() { + v.setZero(); + } + + // Constructor from scalar: a + 0. + explicit Jet(const T& value) { + a = value; + v.setZero(); + } + + // Constructor from scalar plus variable: a + t_i. + Jet(const T& value, int k) { + a = value; + v.setZero(); + v[k] = T(1.0); + } + + // Constructor from scalar and vector part + // The use of Eigen::DenseBase allows Eigen expressions + // to be passed in without being fully evaluated until + // they are assigned to v + template + EIGEN_STRONG_INLINE Jet(const T& a, const Eigen::DenseBase &v) + : a(a), v(v) { + } + + // Compound operators + Jet& operator+=(const Jet &y) { + *this = *this + y; + return *this; + } + + Jet& operator-=(const Jet &y) { + *this = *this - y; + return *this; + } + + Jet& operator*=(const Jet &y) { + *this = *this * y; + return *this; + } + + Jet& operator/=(const Jet &y) { + *this = *this / y; + return *this; + } + + // The scalar part. + T a; + + // The infinitesimal part. + // + // Note the Eigen::DontAlign bit is needed here because this object + // gets allocated on the stack and as part of other arrays and + // structs. Forcing the right alignment there is the source of much + // pain and suffering. Even if that works, passing Jets around to + // functions by value has problems because the C++ ABI does not + // guarantee alignment for function arguments. + // + // Setting the DontAlign bit prevents Eigen from using SSE for the + // various operations on Jets. This is a small performance penalty + // since the AutoDiff code will still expose much of the code as + // statically sized loops to the compiler. But given the subtle + // issues that arise due to alignment, especially when dealing with + // multiple platforms, it seems to be a trade off worth making. + Eigen::Matrix v; +}; + +// Unary + +template inline +Jet const& operator+(const Jet& f) { + return f; +} + +// TODO(keir): Try adding __attribute__((always_inline)) to these functions to +// see if it causes a performance increase. + +// Unary - +template inline +Jet operator-(const Jet&f) { + return Jet(-f.a, -f.v); +} + +// Binary + +template inline +Jet operator+(const Jet& f, + const Jet& g) { + return Jet(f.a + g.a, f.v + g.v); +} + +// Binary + with a scalar: x + s +template inline +Jet operator+(const Jet& f, T s) { + return Jet(f.a + s, f.v); +} + +// Binary + with a scalar: s + x +template inline +Jet operator+(T s, const Jet& f) { + return Jet(f.a + s, f.v); +} + +// Binary - +template inline +Jet operator-(const Jet& f, + const Jet& g) { + return Jet(f.a - g.a, f.v - g.v); +} + +// Binary - with a scalar: x - s +template inline +Jet operator-(const Jet& f, T s) { + return Jet(f.a - s, f.v); +} + +// Binary - with a scalar: s - x +template inline +Jet operator-(T s, const Jet& f) { + return Jet(s - f.a, -f.v); +} + +// Binary * +template inline +Jet operator*(const Jet& f, + const Jet& g) { + return Jet(f.a * g.a, f.a * g.v + f.v * g.a); +} + +// Binary * with a scalar: x * s +template inline +Jet operator*(const Jet& f, T s) { + return Jet(f.a * s, f.v * s); +} + +// Binary * with a scalar: s * x +template inline +Jet operator*(T s, const Jet& f) { + return Jet(f.a * s, f.v * s); +} + +// Binary / +template inline +Jet operator/(const Jet& f, + const Jet& g) { + // This uses: + // + // a + u (a + u)(b - v) (a + u)(b - v) + // ----- = -------------- = -------------- + // b + v (b + v)(b - v) b^2 + // + // which holds because v*v = 0. + const T g_a_inverse = T(1.0) / g.a; + const T f_a_by_g_a = f.a * g_a_inverse; + return Jet(f.a * g_a_inverse, (f.v - f_a_by_g_a * g.v) * g_a_inverse); +} + +// Binary / with a scalar: s / x +template inline +Jet operator/(T s, const Jet& g) { + const T minus_s_g_a_inverse2 = -s / (g.a * g.a); + return Jet(s / g.a, g.v * minus_s_g_a_inverse2); +} + +// Binary / with a scalar: x / s +template inline +Jet operator/(const Jet& f, T s) { + const T s_inverse = 1.0 / s; + return Jet(f.a * s_inverse, f.v * s_inverse); +} + +// Binary comparison operators for both scalars and jets. +#define CERES_DEFINE_JET_COMPARISON_OPERATOR(op) \ +template inline \ +bool operator op(const Jet& f, const Jet& g) { \ + return f.a op g.a; \ +} \ +template inline \ +bool operator op(const T& s, const Jet& g) { \ + return s op g.a; \ +} \ +template inline \ +bool operator op(const Jet& f, const T& s) { \ + return f.a op s; \ +} +CERES_DEFINE_JET_COMPARISON_OPERATOR( < ) // NOLINT +CERES_DEFINE_JET_COMPARISON_OPERATOR( <= ) // NOLINT +CERES_DEFINE_JET_COMPARISON_OPERATOR( > ) // NOLINT +CERES_DEFINE_JET_COMPARISON_OPERATOR( >= ) // NOLINT +CERES_DEFINE_JET_COMPARISON_OPERATOR( == ) // NOLINT +CERES_DEFINE_JET_COMPARISON_OPERATOR( != ) // NOLINT +#undef CERES_DEFINE_JET_COMPARISON_OPERATOR + +// Pull some functions from namespace std. +// +// This is necessary because we want to use the same name (e.g. 'sqrt') for +// double-valued and Jet-valued functions, but we are not allowed to put +// Jet-valued functions inside namespace std. +// +// TODO(keir): Switch to "using". +inline double abs (double x) { return std::abs(x); } +inline double log (double x) { return std::log(x); } +inline double exp (double x) { return std::exp(x); } +inline double sqrt (double x) { return std::sqrt(x); } +inline double cos (double x) { return std::cos(x); } +inline double acos (double x) { return std::acos(x); } +inline double sin (double x) { return std::sin(x); } +inline double asin (double x) { return std::asin(x); } +inline double tan (double x) { return std::tan(x); } +inline double atan (double x) { return std::atan(x); } +inline double sinh (double x) { return std::sinh(x); } +inline double cosh (double x) { return std::cosh(x); } +inline double tanh (double x) { return std::tanh(x); } +inline double pow (double x, double y) { return std::pow(x, y); } +inline double atan2(double y, double x) { return std::atan2(y, x); } + +// In general, f(a + h) ~= f(a) + f'(a) h, via the chain rule. + +// abs(x + h) ~= x + h or -(x + h) +template inline +Jet abs(const Jet& f) { + return f.a < T(0.0) ? -f : f; +} + +// log(a + h) ~= log(a) + h / a +template inline +Jet log(const Jet& f) { + const T a_inverse = T(1.0) / f.a; + return Jet(log(f.a), f.v * a_inverse); +} + +// exp(a + h) ~= exp(a) + exp(a) h +template inline +Jet exp(const Jet& f) { + const T tmp = exp(f.a); + return Jet(tmp, tmp * f.v); +} + +// sqrt(a + h) ~= sqrt(a) + h / (2 sqrt(a)) +template inline +Jet sqrt(const Jet& f) { + const T tmp = sqrt(f.a); + const T two_a_inverse = T(1.0) / (T(2.0) * tmp); + return Jet(tmp, f.v * two_a_inverse); +} + +// cos(a + h) ~= cos(a) - sin(a) h +template inline +Jet cos(const Jet& f) { + return Jet(cos(f.a), - sin(f.a) * f.v); +} + +// acos(a + h) ~= acos(a) - 1 / sqrt(1 - a^2) h +template inline +Jet acos(const Jet& f) { + const T tmp = - T(1.0) / sqrt(T(1.0) - f.a * f.a); + return Jet(acos(f.a), tmp * f.v); +} + +// sin(a + h) ~= sin(a) + cos(a) h +template inline +Jet sin(const Jet& f) { + return Jet(sin(f.a), cos(f.a) * f.v); +} + +// asin(a + h) ~= asin(a) + 1 / sqrt(1 - a^2) h +template inline +Jet asin(const Jet& f) { + const T tmp = T(1.0) / sqrt(T(1.0) - f.a * f.a); + return Jet(asin(f.a), tmp * f.v); +} + +// tan(a + h) ~= tan(a) + (1 + tan(a)^2) h +template inline +Jet tan(const Jet& f) { + const T tan_a = tan(f.a); + const T tmp = T(1.0) + tan_a * tan_a; + return Jet(tan_a, tmp * f.v); +} + +// atan(a + h) ~= atan(a) + 1 / (1 + a^2) h +template inline +Jet atan(const Jet& f) { + const T tmp = T(1.0) / (T(1.0) + f.a * f.a); + return Jet(atan(f.a), tmp * f.v); +} + +// sinh(a + h) ~= sinh(a) + cosh(a) h +template inline +Jet sinh(const Jet& f) { + return Jet(sinh(f.a), cosh(f.a) * f.v); +} + +// cosh(a + h) ~= cosh(a) + sinh(a) h +template inline +Jet cosh(const Jet& f) { + return Jet(cosh(f.a), sinh(f.a) * f.v); +} + +// tanh(a + h) ~= tanh(a) + (1 - tanh(a)^2) h +template inline +Jet tanh(const Jet& f) { + const T tanh_a = tanh(f.a); + const T tmp = T(1.0) - tanh_a * tanh_a; + return Jet(tanh_a, tmp * f.v); +} + +// Jet Classification. It is not clear what the appropriate semantics are for +// these classifications. This picks that IsFinite and isnormal are "all" +// operations, i.e. all elements of the jet must be finite for the jet itself +// to be finite (or normal). For IsNaN and IsInfinite, the answer is less +// clear. This takes a "any" approach for IsNaN and IsInfinite such that if any +// part of a jet is nan or inf, then the entire jet is nan or inf. This leads +// to strange situations like a jet can be both IsInfinite and IsNaN, but in +// practice the "any" semantics are the most useful for e.g. checking that +// derivatives are sane. + +// The jet is finite if all parts of the jet are finite. +template inline +bool IsFinite(const Jet& f) { + if (!IsFinite(f.a)) { + return false; + } + for (int i = 0; i < N; ++i) { + if (!IsFinite(f.v[i])) { + return false; + } + } + return true; +} + +// The jet is infinite if any part of the jet is infinite. +template inline +bool IsInfinite(const Jet& f) { + if (IsInfinite(f.a)) { + return true; + } + for (int i = 0; i < N; i++) { + if (IsInfinite(f.v[i])) { + return true; + } + } + return false; +} + +// The jet is NaN if any part of the jet is NaN. +template inline +bool IsNaN(const Jet& f) { + if (IsNaN(f.a)) { + return true; + } + for (int i = 0; i < N; ++i) { + if (IsNaN(f.v[i])) { + return true; + } + } + return false; +} + +// The jet is normal if all parts of the jet are normal. +template inline +bool IsNormal(const Jet& f) { + if (!IsNormal(f.a)) { + return false; + } + for (int i = 0; i < N; ++i) { + if (!IsNormal(f.v[i])) { + return false; + } + } + return true; +} + +// atan2(b + db, a + da) ~= atan2(b, a) + (- b da + a db) / (a^2 + b^2) +// +// In words: the rate of change of theta is 1/r times the rate of +// change of (x, y) in the positive angular direction. +template inline +Jet atan2(const Jet& g, const Jet& f) { + // Note order of arguments: + // + // f = a + da + // g = b + db + + T const tmp = T(1.0) / (f.a * f.a + g.a * g.a); + return Jet(atan2(g.a, f.a), tmp * (- g.a * f.v + f.a * g.v)); +} + + +// pow -- base is a differentiable function, exponent is a constant. +// (a+da)^p ~= a^p + p*a^(p-1) da +template inline +Jet pow(const Jet& f, double g) { + T const tmp = g * pow(f.a, g - T(1.0)); + return Jet(pow(f.a, g), tmp * f.v); +} + +// pow -- base is a constant, exponent is a differentiable function. +// (a)^(p+dp) ~= a^p + a^p log(a) dp +template inline +Jet pow(double f, const Jet& g) { + T const tmp = pow(f, g.a); + return Jet(tmp, log(f) * tmp * g.v); +} + + +// pow -- both base and exponent are differentiable functions. +// (a+da)^(b+db) ~= a^b + b * a^(b-1) da + a^b log(a) * db +template inline +Jet pow(const Jet& f, const Jet& g) { + T const tmp1 = pow(f.a, g.a); + T const tmp2 = g.a * pow(f.a, g.a - T(1.0)); + T const tmp3 = tmp1 * log(f.a); + + return Jet(tmp1, tmp2 * f.v + tmp3 * g.v); +} + +// Define the helper functions Eigen needs to embed Jet types. +// +// NOTE(keir): machine_epsilon() and precision() are missing, because they don't +// work with nested template types (e.g. where the scalar is itself templated). +// Among other things, this means that decompositions of Jet's does not work, +// for example +// +// Matrix ... > A, x, b; +// ... +// A.solve(b, &x) +// +// does not work and will fail with a strange compiler error. +// +// TODO(keir): This is an Eigen 2.0 limitation that is lifted in 3.0. When we +// switch to 3.0, also add the rest of the specialization functionality. +template inline const Jet& ei_conj(const Jet& x) { return x; } // NOLINT +template inline const Jet& ei_real(const Jet& x) { return x; } // NOLINT +template inline Jet ei_imag(const Jet& ) { return Jet(0.0); } // NOLINT +template inline Jet ei_abs (const Jet& x) { return fabs(x); } // NOLINT +template inline Jet ei_abs2(const Jet& x) { return x * x; } // NOLINT +template inline Jet ei_sqrt(const Jet& x) { return sqrt(x); } // NOLINT +template inline Jet ei_exp (const Jet& x) { return exp(x); } // NOLINT +template inline Jet ei_log (const Jet& x) { return log(x); } // NOLINT +template inline Jet ei_sin (const Jet& x) { return sin(x); } // NOLINT +template inline Jet ei_cos (const Jet& x) { return cos(x); } // NOLINT +template inline Jet ei_tan (const Jet& x) { return tan(x); } // NOLINT +template inline Jet ei_atan(const Jet& x) { return atan(x); } // NOLINT +template inline Jet ei_sinh(const Jet& x) { return sinh(x); } // NOLINT +template inline Jet ei_cosh(const Jet& x) { return cosh(x); } // NOLINT +template inline Jet ei_tanh(const Jet& x) { return tanh(x); } // NOLINT +template inline Jet ei_pow (const Jet& x, Jet y) { return pow(x, y); } // NOLINT + +// Note: This has to be in the ceres namespace for argument dependent lookup to +// function correctly. Otherwise statements like CHECK_LE(x, 2.0) fail with +// strange compile errors. +template +inline std::ostream &operator<<(std::ostream &s, const Jet& z) { + return s << "[" << z.a << " ; " << z.v.transpose() << "]"; +} + +} // namespace ceres + +namespace Eigen { + +// Creating a specialization of NumTraits enables placing Jet objects inside +// Eigen arrays, getting all the goodness of Eigen combined with autodiff. +template +struct NumTraits > { + typedef ceres::Jet Real; + typedef ceres::Jet NonInteger; + typedef ceres::Jet Nested; + + static typename ceres::Jet dummy_precision() { + return ceres::Jet(1e-12); + } + + static inline Real epsilon() { + return Real(std::numeric_limits::epsilon()); + } + + enum { + IsComplex = 0, + IsInteger = 0, + IsSigned, + ReadCost = 1, + AddCost = 1, + // For Jet types, multiplication is more expensive than addition. + MulCost = 3, + HasFloatingPoint = 1, + RequireInitialization = 1 + }; +}; + +} // namespace Eigen + +#endif // CERES_PUBLIC_JET_H_ diff --git a/gtsam_unstable/nonlinear/ceres_macros.h b/gtsam_unstable/nonlinear/ceres_macros.h new file mode 100644 index 000000000..1ed55be6e --- /dev/null +++ b/gtsam_unstable/nonlinear/ceres_macros.h @@ -0,0 +1,170 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// +// Various Google-specific macros. +// +// This code is compiled directly on many platforms, including client +// platforms like Windows, Mac, and embedded systems. Before making +// any changes here, make sure that you're not breaking any platforms. + +#ifndef CERES_PUBLIC_INTERNAL_MACROS_H_ +#define CERES_PUBLIC_INTERNAL_MACROS_H_ + +#include // For size_t. + +// A macro to disallow the copy constructor and operator= functions +// This should be used in the private: declarations for a class +// +// For disallowing only assign or copy, write the code directly, but declare +// the intend in a comment, for example: +// +// void operator=(const TypeName&); // _DISALLOW_ASSIGN + +// Note, that most uses of CERES_DISALLOW_ASSIGN and CERES_DISALLOW_COPY +// are broken semantically, one should either use disallow both or +// neither. Try to avoid these in new code. +#define CERES_DISALLOW_COPY_AND_ASSIGN(TypeName) \ + TypeName(const TypeName&); \ + void operator=(const TypeName&) + +// A macro to disallow all the implicit constructors, namely the +// default constructor, copy constructor and operator= functions. +// +// This should be used in the private: declarations for a class +// that wants to prevent anyone from instantiating it. This is +// especially useful for classes containing only static methods. +#define CERES_DISALLOW_IMPLICIT_CONSTRUCTORS(TypeName) \ + TypeName(); \ + CERES_DISALLOW_COPY_AND_ASSIGN(TypeName) + +// The arraysize(arr) macro returns the # of elements in an array arr. +// The expression is a compile-time constant, and therefore can be +// used in defining new arrays, for example. If you use arraysize on +// a pointer by mistake, you will get a compile-time error. +// +// One caveat is that arraysize() doesn't accept any array of an +// anonymous type or a type defined inside a function. In these rare +// cases, you have to use the unsafe ARRAYSIZE() macro below. This is +// due to a limitation in C++'s template system. The limitation might +// eventually be removed, but it hasn't happened yet. + +// This template function declaration is used in defining arraysize. +// Note that the function doesn't need an implementation, as we only +// use its type. +template +char (&ArraySizeHelper(T (&array)[N]))[N]; + +// That gcc wants both of these prototypes seems mysterious. VC, for +// its part, can't decide which to use (another mystery). Matching of +// template overloads: the final frontier. +#ifndef _WIN32 +template +char (&ArraySizeHelper(const T (&array)[N]))[N]; +#endif + +#define arraysize(array) (sizeof(ArraySizeHelper(array))) + +// ARRAYSIZE performs essentially the same calculation as arraysize, +// but can be used on anonymous types or types defined inside +// functions. It's less safe than arraysize as it accepts some +// (although not all) pointers. Therefore, you should use arraysize +// whenever possible. +// +// The expression ARRAYSIZE(a) is a compile-time constant of type +// size_t. +// +// ARRAYSIZE catches a few type errors. If you see a compiler error +// +// "warning: division by zero in ..." +// +// when using ARRAYSIZE, you are (wrongfully) giving it a pointer. +// You should only use ARRAYSIZE on statically allocated arrays. +// +// The following comments are on the implementation details, and can +// be ignored by the users. +// +// ARRAYSIZE(arr) works by inspecting sizeof(arr) (the # of bytes in +// the array) and sizeof(*(arr)) (the # of bytes in one array +// element). If the former is divisible by the latter, perhaps arr is +// indeed an array, in which case the division result is the # of +// elements in the array. Otherwise, arr cannot possibly be an array, +// and we generate a compiler error to prevent the code from +// compiling. +// +// Since the size of bool is implementation-defined, we need to cast +// !(sizeof(a) & sizeof(*(a))) to size_t in order to ensure the final +// result has type size_t. +// +// This macro is not perfect as it wrongfully accepts certain +// pointers, namely where the pointer size is divisible by the pointee +// size. Since all our code has to go through a 32-bit compiler, +// where a pointer is 4 bytes, this means all pointers to a type whose +// size is 3 or greater than 4 will be (righteously) rejected. +// +// Kudos to Jorg Brown for this simple and elegant implementation. +// +// - wan 2005-11-16 +// +// Starting with Visual C++ 2005, WinNT.h includes ARRAYSIZE. However, +// the definition comes from the over-broad windows.h header that +// introduces a macro, ERROR, that conflicts with the logging framework +// that Ceres uses. Instead, rename ARRAYSIZE to CERES_ARRAYSIZE. +#define CERES_ARRAYSIZE(a) \ + ((sizeof(a) / sizeof(*(a))) / \ + static_cast(!(sizeof(a) % sizeof(*(a))))) + +// Tell the compiler to warn about unused return values for functions +// declared with this macro. The macro should be used on function +// declarations following the argument list: +// +// Sprocket* AllocateSprocket() MUST_USE_RESULT; +// +#if (__GNUC__ > 3 || (__GNUC__ == 3 && __GNUC_MINOR__ >= 4)) \ + && !defined(COMPILER_ICC) +#define CERES_MUST_USE_RESULT __attribute__ ((warn_unused_result)) +#else +#define CERES_MUST_USE_RESULT +#endif + +// Platform independent macros to get aligned memory allocations. +// For example +// +// MyFoo my_foo CERES_ALIGN_ATTRIBUTE(16); +// +// Gives us an instance of MyFoo which is aligned at a 16 byte +// boundary. +#if defined(_MSC_VER) +#define CERES_ALIGN_ATTRIBUTE(n) __declspec(align(n)) +#define CERES_ALIGN_OF(T) __alignof(T) +#elif defined(__GNUC__) +#define CERES_ALIGN_ATTRIBUTE(n) __attribute__((aligned(n))) +#define CERES_ALIGN_OF(T) __alignof(T) +#endif + +#endif // CERES_PUBLIC_INTERNAL_MACROS_H_ diff --git a/gtsam_unstable/nonlinear/ceres_manual_constructor.h b/gtsam_unstable/nonlinear/ceres_manual_constructor.h new file mode 100644 index 000000000..7ea723d2a --- /dev/null +++ b/gtsam_unstable/nonlinear/ceres_manual_constructor.h @@ -0,0 +1,208 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: kenton@google.com (Kenton Varda) +// +// ManualConstructor statically-allocates space in which to store some +// object, but does not initialize it. You can then call the constructor +// and destructor for the object yourself as you see fit. This is useful +// for memory management optimizations, where you want to initialize and +// destroy an object multiple times but only allocate it once. +// +// (When I say ManualConstructor statically allocates space, I mean that +// the ManualConstructor object itself is forced to be the right size.) + +#ifndef CERES_PUBLIC_INTERNAL_MANUAL_CONSTRUCTOR_H_ +#define CERES_PUBLIC_INTERNAL_MANUAL_CONSTRUCTOR_H_ + +#include + +namespace ceres { +namespace internal { + +// ------- Define CERES_ALIGNED_CHAR_ARRAY -------------------------------- + +#ifndef CERES_ALIGNED_CHAR_ARRAY + +// Because MSVC and older GCCs require that the argument to their alignment +// construct to be a literal constant integer, we use a template instantiated +// at all the possible powers of two. +template struct AlignType { }; +template struct AlignType<0, size> { typedef char result[size]; }; + +#if !defined(CERES_ALIGN_ATTRIBUTE) +#define CERES_ALIGNED_CHAR_ARRAY you_must_define_CERES_ALIGNED_CHAR_ARRAY_for_your_compiler +#else // !defined(CERES_ALIGN_ATTRIBUTE) + +#define CERES_ALIGN_TYPE_TEMPLATE(X) \ + template struct AlignType { \ + typedef CERES_ALIGN_ATTRIBUTE(X) char result[size]; \ + } + +CERES_ALIGN_TYPE_TEMPLATE(1); +CERES_ALIGN_TYPE_TEMPLATE(2); +CERES_ALIGN_TYPE_TEMPLATE(4); +CERES_ALIGN_TYPE_TEMPLATE(8); +CERES_ALIGN_TYPE_TEMPLATE(16); +CERES_ALIGN_TYPE_TEMPLATE(32); +CERES_ALIGN_TYPE_TEMPLATE(64); +CERES_ALIGN_TYPE_TEMPLATE(128); +CERES_ALIGN_TYPE_TEMPLATE(256); +CERES_ALIGN_TYPE_TEMPLATE(512); +CERES_ALIGN_TYPE_TEMPLATE(1024); +CERES_ALIGN_TYPE_TEMPLATE(2048); +CERES_ALIGN_TYPE_TEMPLATE(4096); +CERES_ALIGN_TYPE_TEMPLATE(8192); +// Any larger and MSVC++ will complain. + +#undef CERES_ALIGN_TYPE_TEMPLATE + +#define CERES_ALIGNED_CHAR_ARRAY(T, Size) \ + typename AlignType::result + +#endif // !defined(CERES_ALIGN_ATTRIBUTE) + +#endif // CERES_ALIGNED_CHAR_ARRAY + +template +class ManualConstructor { + public: + // No constructor or destructor because one of the most useful uses of + // this class is as part of a union, and members of a union cannot have + // constructors or destructors. And, anyway, the whole point of this + // class is to bypass these. + + inline Type* get() { + return reinterpret_cast(space_); + } + inline const Type* get() const { + return reinterpret_cast(space_); + } + + inline Type* operator->() { return get(); } + inline const Type* operator->() const { return get(); } + + inline Type& operator*() { return *get(); } + inline const Type& operator*() const { return *get(); } + + // This is needed to get around the strict aliasing warning GCC generates. + inline void* space() { + return reinterpret_cast(space_); + } + + // You can pass up to four constructor arguments as arguments of Init(). + inline void Init() { + new(space()) Type; + } + + template + inline void Init(const T1& p1) { + new(space()) Type(p1); + } + + template + inline void Init(const T1& p1, const T2& p2) { + new(space()) Type(p1, p2); + } + + template + inline void Init(const T1& p1, const T2& p2, const T3& p3) { + new(space()) Type(p1, p2, p3); + } + + template + inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4) { + new(space()) Type(p1, p2, p3, p4); + } + + template + inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4, + const T5& p5) { + new(space()) Type(p1, p2, p3, p4, p5); + } + + template + inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4, + const T5& p5, const T6& p6) { + new(space()) Type(p1, p2, p3, p4, p5, p6); + } + + template + inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4, + const T5& p5, const T6& p6, const T7& p7) { + new(space()) Type(p1, p2, p3, p4, p5, p6, p7); + } + + template + inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4, + const T5& p5, const T6& p6, const T7& p7, const T8& p8) { + new(space()) Type(p1, p2, p3, p4, p5, p6, p7, p8); + } + + template + inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4, + const T5& p5, const T6& p6, const T7& p7, const T8& p8, + const T9& p9) { + new(space()) Type(p1, p2, p3, p4, p5, p6, p7, p8, p9); + } + + template + inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4, + const T5& p5, const T6& p6, const T7& p7, const T8& p8, + const T9& p9, const T10& p10) { + new(space()) Type(p1, p2, p3, p4, p5, p6, p7, p8, p9, p10); + } + + template + inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4, + const T5& p5, const T6& p6, const T7& p7, const T8& p8, + const T9& p9, const T10& p10, const T11& p11) { + new(space()) Type(p1, p2, p3, p4, p5, p6, p7, p8, p9, p10, p11); + } + + inline void Destroy() { + get()->~Type(); + } + + private: + CERES_ALIGNED_CHAR_ARRAY(Type, 1) space_; +}; + +#undef CERES_ALIGNED_CHAR_ARRAY + +} // namespace internal +} // namespace ceres + +#endif // CERES_PUBLIC_INTERNAL_MANUAL_CONSTRUCTOR_H_ diff --git a/gtsam_unstable/nonlinear/ceres_rotation.h b/gtsam_unstable/nonlinear/ceres_rotation.h new file mode 100644 index 000000000..83627291c --- /dev/null +++ b/gtsam_unstable/nonlinear/ceres_rotation.h @@ -0,0 +1,645 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// sameeragarwal@google.com (Sameer Agarwal) +// +// Templated functions for manipulating rotations. The templated +// functions are useful when implementing functors for automatic +// differentiation. +// +// In the following, the Quaternions are laid out as 4-vectors, thus: +// +// q[0] scalar part. +// q[1] coefficient of i. +// q[2] coefficient of j. +// q[3] coefficient of k. +// +// where: i*i = j*j = k*k = -1 and i*j = k, j*k = i, k*i = j. + +#ifndef CERES_PUBLIC_ROTATION_H_ +#define CERES_PUBLIC_ROTATION_H_ + +#include +#include +#define DCHECK assert + +namespace ceres { + +// Trivial wrapper to index linear arrays as matrices, given a fixed +// column and row stride. When an array "T* array" is wrapped by a +// +// (const) MatrixAdapter M" +// +// the expression M(i, j) is equivalent to +// +// arrary[i * row_stride + j * col_stride] +// +// Conversion functions to and from rotation matrices accept +// MatrixAdapters to permit using row-major and column-major layouts, +// and rotation matrices embedded in larger matrices (such as a 3x4 +// projection matrix). +template +struct MatrixAdapter; + +// Convenience functions to create a MatrixAdapter that treats the +// array pointed to by "pointer" as a 3x3 (contiguous) column-major or +// row-major matrix. +template +MatrixAdapter ColumnMajorAdapter3x3(T* pointer); + +template +MatrixAdapter RowMajorAdapter3x3(T* pointer); + +// Convert a value in combined axis-angle representation to a quaternion. +// The value angle_axis is a triple whose norm is an angle in radians, +// and whose direction is aligned with the axis of rotation, +// and quaternion is a 4-tuple that will contain the resulting quaternion. +// The implementation may be used with auto-differentiation up to the first +// derivative, higher derivatives may have unexpected results near the origin. +template +void AngleAxisToQuaternion(const T* angle_axis, T* quaternion); + +// Convert a quaternion to the equivalent combined axis-angle representation. +// The value quaternion must be a unit quaternion - it is not normalized first, +// and angle_axis will be filled with a value whose norm is the angle of +// rotation in radians, and whose direction is the axis of rotation. +// The implemention may be used with auto-differentiation up to the first +// derivative, higher derivatives may have unexpected results near the origin. +template +void QuaternionToAngleAxis(const T* quaternion, T* angle_axis); + +// Conversions between 3x3 rotation matrix (in column major order) and +// axis-angle rotation representations. Templated for use with +// autodifferentiation. +template +void RotationMatrixToAngleAxis(const T* R, T* angle_axis); + +template +void RotationMatrixToAngleAxis( + const MatrixAdapter& R, + T* angle_axis); + +template +void AngleAxisToRotationMatrix(const T* angle_axis, T* R); + +template +void AngleAxisToRotationMatrix( + const T* angle_axis, + const MatrixAdapter& R); + +// Conversions between 3x3 rotation matrix (in row major order) and +// Euler angle (in degrees) rotation representations. +// +// The {pitch,roll,yaw} Euler angles are rotations around the {x,y,z} +// axes, respectively. They are applied in that same order, so the +// total rotation R is Rz * Ry * Rx. +template +void EulerAnglesToRotationMatrix(const T* euler, int row_stride, T* R); + +template +void EulerAnglesToRotationMatrix( + const T* euler, + const MatrixAdapter& R); + +// Convert a 4-vector to a 3x3 scaled rotation matrix. +// +// The choice of rotation is such that the quaternion [1 0 0 0] goes to an +// identity matrix and for small a, b, c the quaternion [1 a b c] goes to +// the matrix +// +// [ 0 -c b ] +// I + 2 [ c 0 -a ] + higher order terms +// [ -b a 0 ] +// +// which corresponds to a Rodrigues approximation, the last matrix being +// the cross-product matrix of [a b c]. Together with the property that +// R(q1 * q2) = R(q1) * R(q2) this uniquely defines the mapping from q to R. +// +// The rotation matrix is row-major. +// +// No normalization of the quaternion is performed, i.e. +// R = ||q||^2 * Q, where Q is an orthonormal matrix +// such that det(Q) = 1 and Q*Q' = I +template inline +void QuaternionToScaledRotation(const T q[4], T R[3 * 3]); + +template inline +void QuaternionToScaledRotation( + const T q[4], + const MatrixAdapter& R); + +// Same as above except that the rotation matrix is normalized by the +// Frobenius norm, so that R * R' = I (and det(R) = 1). +template inline +void QuaternionToRotation(const T q[4], T R[3 * 3]); + +template inline +void QuaternionToRotation( + const T q[4], + const MatrixAdapter& R); + +// Rotates a point pt by a quaternion q: +// +// result = R(q) * pt +// +// Assumes the quaternion is unit norm. This assumption allows us to +// write the transform as (something)*pt + pt, as is clear from the +// formula below. If you pass in a quaternion with |q|^2 = 2 then you +// WILL NOT get back 2 times the result you get for a unit quaternion. +template inline +void UnitQuaternionRotatePoint(const T q[4], const T pt[3], T result[3]); + +// With this function you do not need to assume that q has unit norm. +// It does assume that the norm is non-zero. +template inline +void QuaternionRotatePoint(const T q[4], const T pt[3], T result[3]); + +// zw = z * w, where * is the Quaternion product between 4 vectors. +template inline +void QuaternionProduct(const T z[4], const T w[4], T zw[4]); + +// xy = x cross y; +template inline +void CrossProduct(const T x[3], const T y[3], T x_cross_y[3]); + +template inline +T DotProduct(const T x[3], const T y[3]); + +// y = R(angle_axis) * x; +template inline +void AngleAxisRotatePoint(const T angle_axis[3], const T pt[3], T result[3]); + +// --- IMPLEMENTATION + +template +struct MatrixAdapter { + T* pointer_; + explicit MatrixAdapter(T* pointer) + : pointer_(pointer) + {} + + T& operator()(int r, int c) const { + return pointer_[r * row_stride + c * col_stride]; + } +}; + +template +MatrixAdapter ColumnMajorAdapter3x3(T* pointer) { + return MatrixAdapter(pointer); +} + +template +MatrixAdapter RowMajorAdapter3x3(T* pointer) { + return MatrixAdapter(pointer); +} + +template +inline void AngleAxisToQuaternion(const T* angle_axis, T* quaternion) { + const T& a0 = angle_axis[0]; + const T& a1 = angle_axis[1]; + const T& a2 = angle_axis[2]; + const T theta_squared = a0 * a0 + a1 * a1 + a2 * a2; + + // For points not at the origin, the full conversion is numerically stable. + if (theta_squared > T(0.0)) { + const T theta = sqrt(theta_squared); + const T half_theta = theta * T(0.5); + const T k = sin(half_theta) / theta; + quaternion[0] = cos(half_theta); + quaternion[1] = a0 * k; + quaternion[2] = a1 * k; + quaternion[3] = a2 * k; + } else { + // At the origin, sqrt() will produce NaN in the derivative since + // the argument is zero. By approximating with a Taylor series, + // and truncating at one term, the value and first derivatives will be + // computed correctly when Jets are used. + const T k(0.5); + quaternion[0] = T(1.0); + quaternion[1] = a0 * k; + quaternion[2] = a1 * k; + quaternion[3] = a2 * k; + } +} + +template +inline void QuaternionToAngleAxis(const T* quaternion, T* angle_axis) { + const T& q1 = quaternion[1]; + const T& q2 = quaternion[2]; + const T& q3 = quaternion[3]; + const T sin_squared_theta = q1 * q1 + q2 * q2 + q3 * q3; + + // For quaternions representing non-zero rotation, the conversion + // is numerically stable. + if (sin_squared_theta > T(0.0)) { + const T sin_theta = sqrt(sin_squared_theta); + const T& cos_theta = quaternion[0]; + + // If cos_theta is negative, theta is greater than pi/2, which + // means that angle for the angle_axis vector which is 2 * theta + // would be greater than pi. + // + // While this will result in the correct rotation, it does not + // result in a normalized angle-axis vector. + // + // In that case we observe that 2 * theta ~ 2 * theta - 2 * pi, + // which is equivalent saying + // + // theta - pi = atan(sin(theta - pi), cos(theta - pi)) + // = atan(-sin(theta), -cos(theta)) + // + const T two_theta = + T(2.0) * ((cos_theta < 0.0) + ? atan2(-sin_theta, -cos_theta) + : atan2(sin_theta, cos_theta)); + const T k = two_theta / sin_theta; + angle_axis[0] = q1 * k; + angle_axis[1] = q2 * k; + angle_axis[2] = q3 * k; + } else { + // For zero rotation, sqrt() will produce NaN in the derivative since + // the argument is zero. By approximating with a Taylor series, + // and truncating at one term, the value and first derivatives will be + // computed correctly when Jets are used. + const T k(2.0); + angle_axis[0] = q1 * k; + angle_axis[1] = q2 * k; + angle_axis[2] = q3 * k; + } +} + +// The conversion of a rotation matrix to the angle-axis form is +// numerically problematic when then rotation angle is close to zero +// or to Pi. The following implementation detects when these two cases +// occurs and deals with them by taking code paths that are guaranteed +// to not perform division by a small number. +template +inline void RotationMatrixToAngleAxis(const T* R, T* angle_axis) { + RotationMatrixToAngleAxis(ColumnMajorAdapter3x3(R), angle_axis); +} + +template +void RotationMatrixToAngleAxis( + const MatrixAdapter& R, + T* angle_axis) { + // x = k * 2 * sin(theta), where k is the axis of rotation. + angle_axis[0] = R(2, 1) - R(1, 2); + angle_axis[1] = R(0, 2) - R(2, 0); + angle_axis[2] = R(1, 0) - R(0, 1); + + static const T kOne = T(1.0); + static const T kTwo = T(2.0); + + // Since the right hand side may give numbers just above 1.0 or + // below -1.0 leading to atan misbehaving, we threshold. + T costheta = std::min(std::max((R(0, 0) + R(1, 1) + R(2, 2) - kOne) / kTwo, + T(-1.0)), + kOne); + + // sqrt is guaranteed to give non-negative results, so we only + // threshold above. + T sintheta = std::min(sqrt(angle_axis[0] * angle_axis[0] + + angle_axis[1] * angle_axis[1] + + angle_axis[2] * angle_axis[2]) / kTwo, + kOne); + + // Use the arctan2 to get the right sign on theta + const T theta = atan2(sintheta, costheta); + + // Case 1: sin(theta) is large enough, so dividing by it is not a + // problem. We do not use abs here, because while jets.h imports + // std::abs into the namespace, here in this file, abs resolves to + // the int version of the function, which returns zero always. + // + // We use a threshold much larger then the machine epsilon, because + // if sin(theta) is small, not only do we risk overflow but even if + // that does not occur, just dividing by a small number will result + // in numerical garbage. So we play it safe. + static const double kThreshold = 1e-12; + if ((sintheta > kThreshold) || (sintheta < -kThreshold)) { + const T r = theta / (kTwo * sintheta); + for (int i = 0; i < 3; ++i) { + angle_axis[i] *= r; + } + return; + } + + // Case 2: theta ~ 0, means sin(theta) ~ theta to a good + // approximation. + if (costheta > 0.0) { + const T kHalf = T(0.5); + for (int i = 0; i < 3; ++i) { + angle_axis[i] *= kHalf; + } + return; + } + + // Case 3: theta ~ pi, this is the hard case. Since theta is large, + // and sin(theta) is small. Dividing by theta by sin(theta) will + // either give an overflow or worse still numerically meaningless + // results. Thus we use an alternate more complicated formula + // here. + + // Since cos(theta) is negative, division by (1-cos(theta)) cannot + // overflow. + const T inv_one_minus_costheta = kOne / (kOne - costheta); + + // We now compute the absolute value of coordinates of the axis + // vector using the diagonal entries of R. To resolve the sign of + // these entries, we compare the sign of angle_axis[i]*sin(theta) + // with the sign of sin(theta). If they are the same, then + // angle_axis[i] should be positive, otherwise negative. + for (int i = 0; i < 3; ++i) { + angle_axis[i] = theta * sqrt((R(i, i) - costheta) * inv_one_minus_costheta); + if (((sintheta < 0.0) && (angle_axis[i] > 0.0)) || + ((sintheta > 0.0) && (angle_axis[i] < 0.0))) { + angle_axis[i] = -angle_axis[i]; + } + } +} + +template +inline void AngleAxisToRotationMatrix(const T* angle_axis, T* R) { + AngleAxisToRotationMatrix(angle_axis, ColumnMajorAdapter3x3(R)); +} + +template +void AngleAxisToRotationMatrix( + const T* angle_axis, + const MatrixAdapter& R) { + static const T kOne = T(1.0); + const T theta2 = DotProduct(angle_axis, angle_axis); + if (theta2 > T(std::numeric_limits::epsilon())) { + // We want to be careful to only evaluate the square root if the + // norm of the angle_axis vector is greater than zero. Otherwise + // we get a division by zero. + const T theta = sqrt(theta2); + const T wx = angle_axis[0] / theta; + const T wy = angle_axis[1] / theta; + const T wz = angle_axis[2] / theta; + + const T costheta = cos(theta); + const T sintheta = sin(theta); + + R(0, 0) = costheta + wx*wx*(kOne - costheta); + R(1, 0) = wz*sintheta + wx*wy*(kOne - costheta); + R(2, 0) = -wy*sintheta + wx*wz*(kOne - costheta); + R(0, 1) = wx*wy*(kOne - costheta) - wz*sintheta; + R(1, 1) = costheta + wy*wy*(kOne - costheta); + R(2, 1) = wx*sintheta + wy*wz*(kOne - costheta); + R(0, 2) = wy*sintheta + wx*wz*(kOne - costheta); + R(1, 2) = -wx*sintheta + wy*wz*(kOne - costheta); + R(2, 2) = costheta + wz*wz*(kOne - costheta); + } else { + // Near zero, we switch to using the first order Taylor expansion. + R(0, 0) = kOne; + R(1, 0) = angle_axis[2]; + R(2, 0) = -angle_axis[1]; + R(0, 1) = -angle_axis[2]; + R(1, 1) = kOne; + R(2, 1) = angle_axis[0]; + R(0, 2) = angle_axis[1]; + R(1, 2) = -angle_axis[0]; + R(2, 2) = kOne; + } +} + +template +inline void EulerAnglesToRotationMatrix(const T* euler, + const int row_stride_parameter, + T* R) { + DCHECK(row_stride_parameter==3); + EulerAnglesToRotationMatrix(euler, RowMajorAdapter3x3(R)); +} + +template +void EulerAnglesToRotationMatrix( + const T* euler, + const MatrixAdapter& R) { + const double kPi = 3.14159265358979323846; + const T degrees_to_radians(kPi / 180.0); + + const T pitch(euler[0] * degrees_to_radians); + const T roll(euler[1] * degrees_to_radians); + const T yaw(euler[2] * degrees_to_radians); + + const T c1 = cos(yaw); + const T s1 = sin(yaw); + const T c2 = cos(roll); + const T s2 = sin(roll); + const T c3 = cos(pitch); + const T s3 = sin(pitch); + + R(0, 0) = c1*c2; + R(0, 1) = -s1*c3 + c1*s2*s3; + R(0, 2) = s1*s3 + c1*s2*c3; + + R(1, 0) = s1*c2; + R(1, 1) = c1*c3 + s1*s2*s3; + R(1, 2) = -c1*s3 + s1*s2*c3; + + R(2, 0) = -s2; + R(2, 1) = c2*s3; + R(2, 2) = c2*c3; +} + +template inline +void QuaternionToScaledRotation(const T q[4], T R[3 * 3]) { + QuaternionToScaledRotation(q, RowMajorAdapter3x3(R)); +} + +template inline +void QuaternionToScaledRotation( + const T q[4], + const MatrixAdapter& R) { + // Make convenient names for elements of q. + T a = q[0]; + T b = q[1]; + T c = q[2]; + T d = q[3]; + // This is not to eliminate common sub-expression, but to + // make the lines shorter so that they fit in 80 columns! + T aa = a * a; + T ab = a * b; + T ac = a * c; + T ad = a * d; + T bb = b * b; + T bc = b * c; + T bd = b * d; + T cc = c * c; + T cd = c * d; + T dd = d * d; + + R(0, 0) = aa + bb - cc - dd; R(0, 1) = T(2) * (bc - ad); R(0, 2) = T(2) * (ac + bd); // NOLINT + R(1, 0) = T(2) * (ad + bc); R(1, 1) = aa - bb + cc - dd; R(1, 2) = T(2) * (cd - ab); // NOLINT + R(2, 0) = T(2) * (bd - ac); R(2, 1) = T(2) * (ab + cd); R(2, 2) = aa - bb - cc + dd; // NOLINT +} + +template inline +void QuaternionToRotation(const T q[4], T R[3 * 3]) { + QuaternionToRotation(q, RowMajorAdapter3x3(R)); +} + +template inline +void QuaternionToRotation(const T q[4], + const MatrixAdapter& R) { + QuaternionToScaledRotation(q, R); + + T normalizer = q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]; + CHECK_NE(normalizer, T(0)); + normalizer = T(1) / normalizer; + + for (int i = 0; i < 3; ++i) { + for (int j = 0; j < 3; ++j) { + R(i, j) *= normalizer; + } + } +} + +template inline +void UnitQuaternionRotatePoint(const T q[4], const T pt[3], T result[3]) { + const T t2 = q[0] * q[1]; + const T t3 = q[0] * q[2]; + const T t4 = q[0] * q[3]; + const T t5 = -q[1] * q[1]; + const T t6 = q[1] * q[2]; + const T t7 = q[1] * q[3]; + const T t8 = -q[2] * q[2]; + const T t9 = q[2] * q[3]; + const T t1 = -q[3] * q[3]; + result[0] = T(2) * ((t8 + t1) * pt[0] + (t6 - t4) * pt[1] + (t3 + t7) * pt[2]) + pt[0]; // NOLINT + result[1] = T(2) * ((t4 + t6) * pt[0] + (t5 + t1) * pt[1] + (t9 - t2) * pt[2]) + pt[1]; // NOLINT + result[2] = T(2) * ((t7 - t3) * pt[0] + (t2 + t9) * pt[1] + (t5 + t8) * pt[2]) + pt[2]; // NOLINT +} + +template inline +void QuaternionRotatePoint(const T q[4], const T pt[3], T result[3]) { + // 'scale' is 1 / norm(q). + const T scale = T(1) / sqrt(q[0] * q[0] + + q[1] * q[1] + + q[2] * q[2] + + q[3] * q[3]); + + // Make unit-norm version of q. + const T unit[4] = { + scale * q[0], + scale * q[1], + scale * q[2], + scale * q[3], + }; + + UnitQuaternionRotatePoint(unit, pt, result); +} + +template inline +void QuaternionProduct(const T z[4], const T w[4], T zw[4]) { + zw[0] = z[0] * w[0] - z[1] * w[1] - z[2] * w[2] - z[3] * w[3]; + zw[1] = z[0] * w[1] + z[1] * w[0] + z[2] * w[3] - z[3] * w[2]; + zw[2] = z[0] * w[2] - z[1] * w[3] + z[2] * w[0] + z[3] * w[1]; + zw[3] = z[0] * w[3] + z[1] * w[2] - z[2] * w[1] + z[3] * w[0]; +} + +// xy = x cross y; +template inline +void CrossProduct(const T x[3], const T y[3], T x_cross_y[3]) { + x_cross_y[0] = x[1] * y[2] - x[2] * y[1]; + x_cross_y[1] = x[2] * y[0] - x[0] * y[2]; + x_cross_y[2] = x[0] * y[1] - x[1] * y[0]; +} + +template inline +T DotProduct(const T x[3], const T y[3]) { + return (x[0] * y[0] + x[1] * y[1] + x[2] * y[2]); +} + +template inline +void AngleAxisRotatePoint(const T angle_axis[3], const T pt[3], T result[3]) { + const T theta2 = DotProduct(angle_axis, angle_axis); + if (theta2 > T(std::numeric_limits::epsilon())) { + // Away from zero, use the rodriguez formula + // + // result = pt costheta + + // (w x pt) * sintheta + + // w (w . pt) (1 - costheta) + // + // We want to be careful to only evaluate the square root if the + // norm of the angle_axis vector is greater than zero. Otherwise + // we get a division by zero. + // + const T theta = sqrt(theta2); + const T costheta = cos(theta); + const T sintheta = sin(theta); + const T theta_inverse = 1.0 / theta; + + const T w[3] = { angle_axis[0] * theta_inverse, + angle_axis[1] * theta_inverse, + angle_axis[2] * theta_inverse }; + + // Explicitly inlined evaluation of the cross product for + // performance reasons. + const T w_cross_pt[3] = { w[1] * pt[2] - w[2] * pt[1], + w[2] * pt[0] - w[0] * pt[2], + w[0] * pt[1] - w[1] * pt[0] }; + const T tmp = + (w[0] * pt[0] + w[1] * pt[1] + w[2] * pt[2]) * (T(1.0) - costheta); + + result[0] = pt[0] * costheta + w_cross_pt[0] * sintheta + w[0] * tmp; + result[1] = pt[1] * costheta + w_cross_pt[1] * sintheta + w[1] * tmp; + result[2] = pt[2] * costheta + w_cross_pt[2] * sintheta + w[2] * tmp; + } else { + // Near zero, the first order Taylor approximation of the rotation + // matrix R corresponding to a vector w and angle w is + // + // R = I + hat(w) * sin(theta) + // + // But sintheta ~ theta and theta * w = angle_axis, which gives us + // + // R = I + hat(w) + // + // and actually performing multiplication with the point pt, gives us + // R * pt = pt + w x pt. + // + // Switching to the Taylor expansion near zero provides meaningful + // derivatives when evaluated using Jets. + // + // Explicitly inlined evaluation of the cross product for + // performance reasons. + const T w_cross_pt[3] = { angle_axis[1] * pt[2] - angle_axis[2] * pt[1], + angle_axis[2] * pt[0] - angle_axis[0] * pt[2], + angle_axis[0] * pt[1] - angle_axis[1] * pt[0] }; + + result[0] = pt[0] + w_cross_pt[0]; + result[1] = pt[1] + w_cross_pt[1]; + result[2] = pt[2] + w_cross_pt[2]; + } +} + +} // namespace ceres + +#endif // CERES_PUBLIC_ROTATION_H_ diff --git a/gtsam_unstable/nonlinear/ceres_variadic_evaluate.h b/gtsam_unstable/nonlinear/ceres_variadic_evaluate.h new file mode 100644 index 000000000..7d22fe22e --- /dev/null +++ b/gtsam_unstable/nonlinear/ceres_variadic_evaluate.h @@ -0,0 +1,181 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2013 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// mierle@gmail.com (Keir Mierle) + +#ifndef CERES_PUBLIC_INTERNAL_VARIADIC_EVALUATE_H_ +#define CERES_PUBLIC_INTERNAL_VARIADIC_EVALUATE_H_ + +#include + +#include +#include +#include + +namespace ceres { +namespace internal { + +// This block of quasi-repeated code calls the user-supplied functor, which may +// take a variable number of arguments. This is accomplished by specializing the +// struct based on the size of the trailing parameters; parameters with 0 size +// are assumed missing. +template +struct VariadicEvaluate { + static bool Call(const Functor& functor, T const *const *input, T* output) { + return functor(input[0], + input[1], + input[2], + input[3], + input[4], + input[5], + input[6], + input[7], + input[8], + input[9], + output); + } +}; + +template +struct VariadicEvaluate { + static bool Call(const Functor& functor, T const *const *input, T* output) { + return functor(input[0], + input[1], + input[2], + input[3], + input[4], + input[5], + input[6], + input[7], + input[8], + output); + } +}; + +template +struct VariadicEvaluate { + static bool Call(const Functor& functor, T const *const *input, T* output) { + return functor(input[0], + input[1], + input[2], + input[3], + input[4], + input[5], + input[6], + input[7], + output); + } +}; + +template +struct VariadicEvaluate { + static bool Call(const Functor& functor, T const *const *input, T* output) { + return functor(input[0], + input[1], + input[2], + input[3], + input[4], + input[5], + input[6], + output); + } +}; + +template +struct VariadicEvaluate { + static bool Call(const Functor& functor, T const *const *input, T* output) { + return functor(input[0], + input[1], + input[2], + input[3], + input[4], + input[5], + output); + } +}; + +template +struct VariadicEvaluate { + static bool Call(const Functor& functor, T const *const *input, T* output) { + return functor(input[0], + input[1], + input[2], + input[3], + input[4], + output); + } +}; + +template +struct VariadicEvaluate { + static bool Call(const Functor& functor, T const *const *input, T* output) { + return functor(input[0], + input[1], + input[2], + input[3], + output); + } +}; + +template +struct VariadicEvaluate { + static bool Call(const Functor& functor, T const *const *input, T* output) { + return functor(input[0], + input[1], + input[2], + output); + } +}; + +template +struct VariadicEvaluate { + static bool Call(const Functor& functor, T const *const *input, T* output) { + return functor(input[0], + input[1], + output); + } +}; + +template +struct VariadicEvaluate { + static bool Call(const Functor& functor, T const *const *input, T* output) { + return functor(input[0], + output); + } +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_PUBLIC_INTERNAL_VARIADIC_EVALUATE_H_ diff --git a/gtsam_unstable/nonlinear/tests/testAdaptAutoDiff.cpp b/gtsam_unstable/nonlinear/tests/testAdaptAutoDiff.cpp new file mode 100644 index 000000000..edb26c4f4 --- /dev/null +++ b/gtsam_unstable/nonlinear/tests/testAdaptAutoDiff.cpp @@ -0,0 +1,217 @@ +/* ---------------------------------------------------------------------------- + + * 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 testExpression.cpp + * @date September 18, 2014 + * @author Frank Dellaert + * @author Paul Furgale + * @brief unit tests for Block Automatic Differentiation + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#undef CHECK +#include + +#include +using boost::assign::list_of; +using boost::assign::map_list_of; + +using namespace std; +using namespace gtsam; + +// The DefaultChart of Camera below is laid out like Snavely's 9-dim vector +typedef PinholeCamera Camera; + +/* ************************************************************************* */ +// Some Ceres Snippets copied for testing +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +template inline T &RowMajorAccess(T *base, int rows, int cols, + int i, int j) { + return base[cols * i + j]; +} + +inline double RandDouble() { + double r = static_cast(rand()); + return r / RAND_MAX; +} + +// A structure for projecting a 3x4 camera matrix and a +// homogeneous 3D point, to a 2D inhomogeneous point. +struct Projective { + // Function that takes P and X as separate vectors: + // P, X -> x + template + bool operator()(A const P[12], A const X[4], A x[2]) const { + A PX[3]; + for (int i = 0; i < 3; ++i) { + PX[i] = RowMajorAccess(P, 3, 4, i, 0) * X[0] + + RowMajorAccess(P, 3, 4, i, 1) * X[1] + + RowMajorAccess(P, 3, 4, i, 2) * X[2] + + RowMajorAccess(P, 3, 4, i, 3) * X[3]; + } + if (PX[2] != 0.0) { + x[0] = PX[0] / PX[2]; + x[1] = PX[1] / PX[2]; + return true; + } + return false; + } + + // Adapt to eigen types + Vector2 operator()(const MatrixRowMajor& P, const Vector4& X) const { + Vector2 x; + if (operator()(P.data(), X.data(), x.data())) + return x; + else + throw std::runtime_error("Projective fail"); + } +}; + +/* ************************************************************************* */ +// Test Ceres AutoDiff +TEST(Expression, AutoDiff) { + using ceres::internal::AutoDiff; + + // Instantiate function + Projective projective; + + // Make arguments + typedef Eigen::Matrix M; + M P; + P << 1, 0, 0, 0, 0, 1, 0, 5, 0, 0, 1, 0; + Vector4 X(10, 0, 5, 1); + + // Apply the mapping, to get image point b_x. + Vector expected = Vector2(2, 1); + Vector2 actual = projective(P, X); + EXPECT(assert_equal(expected,actual,1e-9)); + + // Get expected derivatives + Matrix E1 = numericalDerivative21(Projective(), P, X); + Matrix E2 = numericalDerivative22(Projective(), P, X); + + // Get derivatives with AutoDiff + Vector2 actual2; + MatrixRowMajor H1(2, 12), H2(2, 4); + double *parameters[] = { P.data(), X.data() }; + double *jacobians[] = { H1.data(), H2.data() }; + CHECK( + (AutoDiff::Differentiate( projective, parameters, 2, actual2.data(), jacobians))); + EXPECT(assert_equal(E1,H1,1e-8)); + EXPECT(assert_equal(E2,H2,1e-8)); +} + +/* ************************************************************************* */ +// Test Ceres AutoDiff on Snavely, defined in ceres_example.h +// Adapt to GTSAM types +Vector2 adapted(const Vector9& P, const Vector3& X) { + SnavelyProjection snavely; + Vector2 x; + if (snavely(P.data(), X.data(), x.data())) + return x; + else + throw std::runtime_error("Snavely fail"); +} + +TEST(Expression, AutoDiff2) { + using ceres::internal::AutoDiff; + + // Instantiate function + SnavelyProjection snavely; + + // Make arguments + Vector9 P; // zero rotation, (0,5,0) translation, focal length 1 + P << 0, 0, 0, 0, 5, 0, 1, 0, 0; + Vector3 X(10, 0, -5); // negative Z-axis convention of Snavely! + + // Apply the mapping, to get image point b_x. + Vector expected = Vector2(2, 1); + Vector2 actual = adapted(P, X); + EXPECT(assert_equal(expected,actual,1e-9)); + + // Get expected derivatives + Matrix E1 = numericalDerivative21(adapted, P, X); + Matrix E2 = numericalDerivative22(adapted, P, X); + + // Get derivatives with AutoDiff + Vector2 actual2; + MatrixRowMajor H1(2, 9), H2(2, 3); + double *parameters[] = { P.data(), X.data() }; + double *jacobians[] = { H1.data(), H2.data() }; + CHECK( + (AutoDiff::Differentiate( snavely, parameters, 2, actual2.data(), jacobians))); + EXPECT(assert_equal(E1,H1,1e-8)); + EXPECT(assert_equal(E2,H2,1e-8)); +} + +/* ************************************************************************* */ +// Test AutoDiff wrapper Snavely +TEST(Expression, AutoDiff3) { + + // Make arguments + Camera P(Pose3(Rot3(), Point3(0, 5, 0)), Cal3Bundler(1, 0, 0)); + Point3 X(10, 0, -5); // negative Z-axis convention of Snavely! + + typedef AdaptAutoDiff Adaptor; + Adaptor snavely; + + // Apply the mapping, to get image point b_x. + Point2 expected(2, 1); + Point2 actual = snavely(P, X); + EXPECT(assert_equal(expected,actual,1e-9)); + +// // Get expected derivatives + Matrix E1 = numericalDerivative21(Adaptor(), P, X); + Matrix E2 = numericalDerivative22(Adaptor(), P, X); + + // Get derivatives with AutoDiff, not gives RowMajor results! + Matrix29 H1; + Matrix23 H2; + Point2 actual2 = snavely(P, X, H1, H2); + EXPECT(assert_equal(expected,actual2,1e-9)); + EXPECT(assert_equal(E1,H1,1e-8)); +} + +/* ************************************************************************* */ +// Test AutoDiff wrapper in an expression +TEST(Expression, Snavely) { + Expression P(1); + Expression X(2); + typedef AdaptAutoDiff Adaptor; + Expression expression(Adaptor(), P, X); +#ifdef GTSAM_USE_QUATERNIONS + EXPECT_LONGS_EQUAL(400,expression.traceSize()); // Todo, should be zero +#else + EXPECT_LONGS_EQUAL(432,expression.traceSize()); // Todo, should be zero +#endif + set expected = list_of(1)(2); + EXPECT(expected == expression.keys()); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + diff --git a/gtsam_unstable/nonlinear/tests/testExpression.cpp b/gtsam_unstable/nonlinear/tests/testExpression.cpp new file mode 100644 index 000000000..d660d28b5 --- /dev/null +++ b/gtsam_unstable/nonlinear/tests/testExpression.cpp @@ -0,0 +1,222 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------1------------------------------------------- */ + +/** + * @file testExpression.cpp + * @date September 18, 2014 + * @author Frank Dellaert + * @author Paul Furgale + * @brief unit tests for Block Automatic Differentiation + */ + +#include +#include +#include +#include +#include + +#include + +#include +using boost::assign::list_of; +using boost::assign::map_list_of; + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +template +Point2 uncalibrate(const CAL& K, const Point2& p, + boost::optional Dcal, boost::optional Dp) { + return K.uncalibrate(p, Dcal, Dp); +} + +static const Rot3 someR = Rot3::RzRyRx(1, 2, 3); + +/* ************************************************************************* */ +// Constant +TEST(Expression, constant) { + Expression R(someR); + Values values; + Rot3 actual = R.value(values); + EXPECT(assert_equal(someR, actual)); + EXPECT_LONGS_EQUAL(0, R.traceSize()) +} + +/* ************************************************************************* */ +// Leaf +TEST(Expression, Leaf) { + Expression R(100); + Values values; + values.insert(100, someR); + + Rot3 actual2 = R.value(values); + EXPECT(assert_equal(someR, actual2)); +} + +/* ************************************************************************* */ +// Many Leaves +TEST(Expression, Leaves) { + Values values; + Point3 somePoint(1, 2, 3); + values.insert(Symbol('p', 10), somePoint); + std::vector > points = createUnknowns(10, 'p', 1); + EXPECT(assert_equal(somePoint,points.back().value(values))); +} + +/* ************************************************************************* */ + +//TEST(Expression, NullaryMethod) { +// Expression p(67); +// Expression norm(p, &Point3::norm); +// Values values; +// values.insert(67,Point3(3,4,5)); +// Augmented a = norm.augmented(values); +// EXPECT(a.value() == sqrt(50)); +// JacobianMap expected; +// expected[67] = (Matrix(1,3) << 3/sqrt(50),4/sqrt(50),5/sqrt(50)); +// EXPECT(assert_equal(expected.at(67),a.jacobians().at(67))); +//} +/* ************************************************************************* */ +// Binary(Leaf,Leaf) +namespace binary { +// Create leaves +Expression x(1); +Expression p(2); +Expression p_cam(x, &Pose3::transform_to, p); +} +/* ************************************************************************* */ +// keys +TEST(Expression, BinaryKeys) { + set expected = list_of(1)(2); + EXPECT(expected == binary::p_cam.keys()); +} +/* ************************************************************************* */ +// dimensions +TEST(Expression, BinaryDimensions) { + map actual, expected = map_list_of(1, 6)(2, 3); + binary::p_cam.dims(actual); + EXPECT(actual==expected); +} +/* ************************************************************************* */ +// dimensions +TEST(Expression, BinaryTraceSize) { + typedef BinaryExpression Binary; + size_t expectedTraceSize = sizeof(Binary::Record); + EXPECT_LONGS_EQUAL(expectedTraceSize, binary::p_cam.traceSize()); +} +/* ************************************************************************* */ +// Binary(Leaf,Unary(Binary(Leaf,Leaf))) +namespace tree { +using namespace binary; +// Create leaves +Expression K(3); + +// Create expression tree +Expression projection(PinholeCamera::project_to_camera, p_cam); +Expression uv_hat(uncalibrate, K, projection); +} +/* ************************************************************************* */ +// keys +TEST(Expression, TreeKeys) { + set expected = list_of(1)(2)(3); + EXPECT(expected == tree::uv_hat.keys()); +} +/* ************************************************************************* */ +// dimensions +TEST(Expression, TreeDimensions) { + map actual, expected = map_list_of(1, 6)(2, 3)(3, + 5); + tree::uv_hat.dims(actual); + EXPECT(actual==expected); +} +/* ************************************************************************* */ +// TraceSize +TEST(Expression, TreeTraceSize) { + typedef UnaryExpression Unary; + typedef BinaryExpression Binary1; + typedef BinaryExpression Binary2; + size_t expectedTraceSize = sizeof(Unary::Record) + sizeof(Binary1::Record) + + sizeof(Binary2::Record); + EXPECT_LONGS_EQUAL(expectedTraceSize, tree::uv_hat.traceSize()); +} +/* ************************************************************************* */ + +TEST(Expression, compose1) { + + // Create expression + Expression R1(1), R2(2); + Expression R3 = R1 * R2; + + // Check keys + set expected = list_of(1)(2); + EXPECT(expected == R3.keys()); +} + +/* ************************************************************************* */ +// Test compose with arguments referring to the same rotation +TEST(Expression, compose2) { + + // Create expression + Expression R1(1), R2(1); + Expression R3 = R1 * R2; + + // Check keys + set expected = list_of(1); + EXPECT(expected == R3.keys()); +} + +/* ************************************************************************* */ +// Test compose with one arguments referring to constant rotation +TEST(Expression, compose3) { + + // Create expression + Expression R1(Rot3::identity()), R2(3); + Expression R3 = R1 * R2; + + // Check keys + set expected = list_of(3); + EXPECT(expected == R3.keys()); +} + +/* ************************************************************************* */ +// Test with ternary function +Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3, + boost::optional H1, boost::optional H2, + boost::optional H3) { + // return dummy derivatives (not correct, but that's ok for testing here) + if (H1) + *H1 = eye(3); + if (H2) + *H2 = eye(3); + if (H3) + *H3 = eye(3); + return R1 * (R2 * R3); +} + +TEST(Expression, ternary) { + + // Create expression + Expression A(1), B(2), C(3); + Expression ABC(composeThree, A, B, C); + + // Check keys + set expected = list_of(1)(2)(3); + EXPECT(expected == ABC.keys()); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + diff --git a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp new file mode 100644 index 000000000..c63bfeb6f --- /dev/null +++ b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp @@ -0,0 +1,434 @@ +/* ---------------------------------------------------------------------------- + + * 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 testExpressionFactor.cpp + * @date September 18, 2014 + * @author Frank Dellaert + * @author Paul Furgale + * @brief unit tests for Block Automatic Differentiation + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +using boost::assign::list_of; + +using namespace std; +using namespace gtsam; + +Point2 measured(-17, 30); +SharedNoiseModel model = noiseModel::Unit::Create(2); + +namespace leaf { +// Create some values +struct MyValues: public Values { + MyValues() { + insert(2, Point2(3, 5)); + } +} values; + +// Create leaf +Point2_ p(2); +} + +/* ************************************************************************* */ +// Leaf +TEST(ExpressionFactor, Leaf) { + using namespace leaf; + + // Create old-style factor to create expected value and derivatives + PriorFactor old(2, Point2(0, 0), model); + + // Concise version + ExpressionFactor f(model, Point2(0, 0), p); + EXPECT_DOUBLES_EQUAL(old.error(values), f.error(values), 1e-9); + EXPECT_LONGS_EQUAL(2, f.dim()); + boost::shared_ptr gf2 = f.linearize(values); + EXPECT( assert_equal(*old.linearize(values), *gf2, 1e-9)); +} + +/* ************************************************************************* */ +// non-zero noise model +TEST(ExpressionFactor, Model) { + using namespace leaf; + + SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.01)); + + // Create old-style factor to create expected value and derivatives + PriorFactor old(2, Point2(0, 0), model); + + // Concise version + ExpressionFactor f(model, Point2(0, 0), p); + EXPECT_DOUBLES_EQUAL(old.error(values), f.error(values), 1e-9); + EXPECT_LONGS_EQUAL(2, f.dim()); + boost::shared_ptr gf2 = f.linearize(values); + EXPECT( assert_equal(*old.linearize(values), *gf2, 1e-9)); +} + +/* ************************************************************************* */ +// Constrained noise model +TEST(ExpressionFactor, Constrained) { + using namespace leaf; + + SharedDiagonal model = noiseModel::Constrained::MixedSigmas(Vector2(0.2, 0)); + + // Create old-style factor to create expected value and derivatives + PriorFactor old(2, Point2(0, 0), model); + + // Concise version + ExpressionFactor f(model, Point2(0, 0), p); + EXPECT_DOUBLES_EQUAL(old.error(values), f.error(values), 1e-9); + EXPECT_LONGS_EQUAL(2, f.dim()); + boost::shared_ptr gf2 = f.linearize(values); + EXPECT( assert_equal(*old.linearize(values), *gf2, 1e-9)); +} + +/* ************************************************************************* */ +// Unary(Leaf)) +TEST(ExpressionFactor, Unary) { + + // Create some values + Values values; + values.insert(2, Point3(0, 0, 1)); + + JacobianFactor expected( // + 2, (Matrix(2, 3) << 1, 0, 0, 0, 1, 0), // + (Vector(2) << -17, 30)); + + // Create leaves + Point3_ p(2); + + // Concise version + ExpressionFactor f(model, measured, project(p)); + EXPECT_LONGS_EQUAL(2, f.dim()); + boost::shared_ptr gf = f.linearize(values); + boost::shared_ptr jf = // + boost::dynamic_pointer_cast(gf); + EXPECT( assert_equal(expected, *jf, 1e-9)); +} + +/* ************************************************************************* */ +static Point2 myUncal(const Cal3_S2& K, const Point2& p, + boost::optional Dcal, boost::optional Dp) { + return K.uncalibrate(p, Dcal, Dp); +} + +// Binary(Leaf,Leaf) +TEST(ExpressionFactor, Binary) { + + typedef BinaryExpression Binary; + + Cal3_S2_ K_(1); + Point2_ p_(2); + Binary binary(myUncal, K_, p_); + + // Create some values + Values values; + values.insert(1, Cal3_S2()); + values.insert(2, Point2(0, 0)); + + // traceRaw will fill raw with [Trace | Binary::Record] + EXPECT_LONGS_EQUAL(8, sizeof(double)); + EXPECT_LONGS_EQUAL(16, sizeof(Point2)); + EXPECT_LONGS_EQUAL(40, sizeof(Cal3_S2)); + EXPECT_LONGS_EQUAL(16, sizeof(ExecutionTrace)); + EXPECT_LONGS_EQUAL(16, sizeof(ExecutionTrace)); + EXPECT_LONGS_EQUAL(2*5*8, sizeof(Jacobian::type)); + EXPECT_LONGS_EQUAL(2*2*8, sizeof(Jacobian::type)); + size_t expectedRecordSize = 16 + 16 + 40 + 2 * 16 + 80 + 32; + EXPECT_LONGS_EQUAL(expectedRecordSize + 8, sizeof(Binary::Record)); + + // Check size + size_t size = binary.traceSize(); + CHECK(size); + EXPECT_LONGS_EQUAL(expectedRecordSize + 8, size); + // Use Variable Length Array, allocated on stack by gcc + // Note unclear for Clang: http://clang.llvm.org/compatibility.html#vla + char raw[size]; + ExecutionTrace trace; + Point2 value = binary.traceExecution(values, trace, raw); + EXPECT(assert_equal(Point2(),value, 1e-9)); + // trace.print(); + + // Expected Jacobians + Matrix25 expected25; + expected25 << 0, 0, 0, 1, 0, 0, 0, 0, 0, 1; + Matrix2 expected22; + expected22 << 1, 0, 0, 1; + + // Check matrices + boost::optional r = trace.record(); + CHECK(r); + EXPECT( + assert_equal(expected25, (Matrix) (*r)-> jacobian(), 1e-9)); + EXPECT( assert_equal(expected22, (Matrix) (*r)->jacobian(), 1e-9)); +} +/* ************************************************************************* */ +// Unary(Binary(Leaf,Leaf)) +TEST(ExpressionFactor, Shallow) { + + // Create some values + Values values; + values.insert(1, Pose3()); + values.insert(2, Point3(0, 0, 1)); + + // Create old-style factor to create expected value and derivatives + GenericProjectionFactor old(measured, model, 1, 2, + boost::make_shared()); + double expected_error = old.error(values); + GaussianFactor::shared_ptr expected = old.linearize(values); + + // Create leaves + Pose3_ x_(1); + Point3_ p_(2); + + // Construct expression, concise evrsion + Point2_ expression = project(transform_to(x_, p_)); + + // traceExecution of shallow tree + typedef UnaryExpression Unary; + typedef BinaryExpression Binary; + size_t expectedTraceSize = sizeof(Unary::Record) + sizeof(Binary::Record); + EXPECT_LONGS_EQUAL(112, sizeof(Unary::Record)); +#ifdef GTSAM_USE_QUATERNIONS + EXPECT_LONGS_EQUAL(352, sizeof(Binary::Record)); + LONGS_EQUAL(112+352, expectedTraceSize); +#else + EXPECT_LONGS_EQUAL(400, sizeof(Binary::Record)); + LONGS_EQUAL(112+400, expectedTraceSize); +#endif + size_t size = expression.traceSize(); + CHECK(size); + EXPECT_LONGS_EQUAL(expectedTraceSize, size); + char raw[size]; + ExecutionTrace trace; + Point2 value = expression.traceExecution(values, trace, raw); + EXPECT(assert_equal(Point2(),value, 1e-9)); + // trace.print(); + + // Expected Jacobians + Matrix23 expected23; + expected23 << 1, 0, 0, 0, 1, 0; + + // Check matrices + boost::optional r = trace.record(); + CHECK(r); + EXPECT(assert_equal(expected23, (Matrix)(*r)->jacobian(), 1e-9)); + + // Linearization + ExpressionFactor f2(model, measured, expression); + EXPECT_DOUBLES_EQUAL(expected_error, f2.error(values), 1e-9); + EXPECT_LONGS_EQUAL(2, f2.dim()); + boost::shared_ptr gf2 = f2.linearize(values); + EXPECT( assert_equal(*expected, *gf2, 1e-9)); +} + +/* ************************************************************************* */ +// Binary(Leaf,Unary(Binary(Leaf,Leaf))) +TEST(ExpressionFactor, tree) { + + // Create some values + Values values; + values.insert(1, Pose3()); + values.insert(2, Point3(0, 0, 1)); + values.insert(3, Cal3_S2()); + + // Create old-style factor to create expected value and derivatives + GeneralSFMFactor2 old(measured, model, 1, 2, 3); + double expected_error = old.error(values); + GaussianFactor::shared_ptr expected = old.linearize(values); + + // Create leaves + Pose3_ x(1); + Point3_ p(2); + Cal3_S2_ K(3); + + // Create expression tree + Point3_ p_cam(x, &Pose3::transform_to, p); + Point2_ xy_hat(PinholeCamera::project_to_camera, p_cam); + Point2_ uv_hat(K, &Cal3_S2::uncalibrate, xy_hat); + + // Create factor and check value, dimension, linearization + ExpressionFactor f(model, measured, uv_hat); + EXPECT_DOUBLES_EQUAL(expected_error, f.error(values), 1e-9); + EXPECT_LONGS_EQUAL(2, f.dim()); + boost::shared_ptr gf = f.linearize(values); + EXPECT( assert_equal(*expected, *gf, 1e-9)); + + // Concise version + ExpressionFactor f2(model, measured, + uncalibrate(K, project(transform_to(x, p)))); + EXPECT_DOUBLES_EQUAL(expected_error, f2.error(values), 1e-9); + EXPECT_LONGS_EQUAL(2, f2.dim()); + boost::shared_ptr gf2 = f2.linearize(values); + EXPECT( assert_equal(*expected, *gf2, 1e-9)); + + TernaryExpression::Function fff = project6; + + // Try ternary version + ExpressionFactor f3(model, measured, project3(x, p, K)); + EXPECT_DOUBLES_EQUAL(expected_error, f3.error(values), 1e-9); + EXPECT_LONGS_EQUAL(2, f3.dim()); + boost::shared_ptr gf3 = f3.linearize(values); + EXPECT( assert_equal(*expected, *gf3, 1e-9)); +} + +/* ************************************************************************* */ + +TEST(ExpressionFactor, Compose1) { + + // Create expression + Rot3_ R1(1), R2(2); + Rot3_ R3 = R1 * R2; + + // Create factor + ExpressionFactor f(noiseModel::Unit::Create(3), Rot3(), R3); + + // Create some values + Values values; + values.insert(1, Rot3()); + values.insert(2, Rot3()); + + // Check unwhitenedError + std::vector H(2); + Vector actual = f.unwhitenedError(values, H); + EXPECT( assert_equal(eye(3), H[0],1e-9)); + EXPECT( assert_equal(eye(3), H[1],1e-9)); + + // Check linearization + JacobianFactor expected(1, eye(3), 2, eye(3), zero(3)); + boost::shared_ptr gf = f.linearize(values); + boost::shared_ptr jf = // + boost::dynamic_pointer_cast(gf); + EXPECT( assert_equal(expected, *jf,1e-9)); +} + +/* ************************************************************************* */ +// Test compose with arguments referring to the same rotation +TEST(ExpressionFactor, compose2) { + + // Create expression + Rot3_ R1(1), R2(1); + Rot3_ R3 = R1 * R2; + + // Create factor + ExpressionFactor f(noiseModel::Unit::Create(3), Rot3(), R3); + + // Create some values + Values values; + values.insert(1, Rot3()); + + // Check unwhitenedError + std::vector H(1); + Vector actual = f.unwhitenedError(values, H); + EXPECT_LONGS_EQUAL(1, H.size()); + EXPECT( assert_equal(2*eye(3), H[0],1e-9)); + + // Check linearization + JacobianFactor expected(1, 2 * eye(3), zero(3)); + boost::shared_ptr gf = f.linearize(values); + boost::shared_ptr jf = // + boost::dynamic_pointer_cast(gf); + EXPECT( assert_equal(expected, *jf,1e-9)); +} + +/* ************************************************************************* */ +// Test compose with one arguments referring to a constant same rotation +TEST(ExpressionFactor, compose3) { + + // Create expression + Rot3_ R1(Rot3::identity()), R2(3); + Rot3_ R3 = R1 * R2; + + // Create factor + ExpressionFactor f(noiseModel::Unit::Create(3), Rot3(), R3); + + // Create some values + Values values; + values.insert(3, Rot3()); + + // Check unwhitenedError + std::vector H(1); + Vector actual = f.unwhitenedError(values, H); + EXPECT_LONGS_EQUAL(1, H.size()); + EXPECT( assert_equal(eye(3), H[0],1e-9)); + + // Check linearization + JacobianFactor expected(3, eye(3), zero(3)); + boost::shared_ptr gf = f.linearize(values); + boost::shared_ptr jf = // + boost::dynamic_pointer_cast(gf); + EXPECT( assert_equal(expected, *jf,1e-9)); +} + +/* ************************************************************************* */ +// Test compose with three arguments +Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3, + boost::optional H1, boost::optional H2, + boost::optional H3) { + // return dummy derivatives (not correct, but that's ok for testing here) + if (H1) + *H1 = eye(3); + if (H2) + *H2 = eye(3); + if (H3) + *H3 = eye(3); + return R1 * (R2 * R3); +} + +TEST(ExpressionFactor, composeTernary) { + + // Create expression + Rot3_ A(1), B(2), C(3); + Rot3_ ABC(composeThree, A, B, C); + + // Create factor + ExpressionFactor f(noiseModel::Unit::Create(3), Rot3(), ABC); + + // Create some values + Values values; + values.insert(1, Rot3()); + values.insert(2, Rot3()); + values.insert(3, Rot3()); + + // Check unwhitenedError + std::vector H(3); + Vector actual = f.unwhitenedError(values, H); + EXPECT_LONGS_EQUAL(3, H.size()); + EXPECT( assert_equal(eye(3), H[0],1e-9)); + EXPECT( assert_equal(eye(3), H[1],1e-9)); + EXPECT( assert_equal(eye(3), H[2],1e-9)); + + // Check linearization + JacobianFactor expected(1, eye(3), 2, eye(3), 3, eye(3), zero(3)); + boost::shared_ptr gf = f.linearize(values); + boost::shared_ptr jf = // + boost::dynamic_pointer_cast(gf); + EXPECT( assert_equal(expected, *jf,1e-9)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + diff --git a/gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp b/gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp new file mode 100644 index 000000000..b2cdcdf34 --- /dev/null +++ b/gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp @@ -0,0 +1,248 @@ +/* ---------------------------------------------------------------------------- + + * 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 testExpressionMeta.cpp + * @date October 14, 2014 + * @author Frank Dellaert + * @brief Test meta-programming constructs for Expressions + */ + +#include +#include +#include +#include + +#include +#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +namespace mpl = boost::mpl; + +#include +#include +template struct Incomplete; + +// Check generation of FunctionalNode +typedef mpl::vector MyTypes; +typedef FunctionalNode::type Generated; +//Incomplete incomplete; +BOOST_MPL_ASSERT((boost::is_same< Matrix2, Generated::Record::Jacobian2T >)); + +// Try generating vectors of ExecutionTrace +typedef mpl::vector, ExecutionTrace > ExpectedTraces; + +typedef mpl::transform >::type MyTraces; +BOOST_MPL_ASSERT((mpl::equal< ExpectedTraces, MyTraces >)); + +template +struct MakeTrace { + typedef ExecutionTrace type; +}; +typedef mpl::transform >::type MyTraces1; +BOOST_MPL_ASSERT((mpl::equal< ExpectedTraces, MyTraces1 >)); + +// Try generating vectors of Expression types +typedef mpl::vector, Expression > ExpectedExpressions; +typedef mpl::transform >::type Expressions; +BOOST_MPL_ASSERT((mpl::equal< ExpectedExpressions, Expressions >)); + +// Try generating vectors of Jacobian types +typedef mpl::vector ExpectedJacobians; +typedef mpl::transform >::type Jacobians; +BOOST_MPL_ASSERT((mpl::equal< ExpectedJacobians, Jacobians >)); + +// Try accessing a Jacobian +typedef mpl::at_c::type Jacobian23; // base zero ! +BOOST_MPL_ASSERT((boost::is_same< Matrix23, Jacobian23>)); + +/* ************************************************************************* */ +// Boost Fusion includes allow us to create/access values from MPL vectors +#include +#include + +// Create a value and access it +TEST(ExpressionFactor, JacobiansValue) { + using boost::fusion::at_c; + Matrix23 expected; + Jacobians jacobians; + + at_c<1>(jacobians) << 1, 2, 3, 4, 5, 6; + + Matrix23 actual = at_c<1>(jacobians); + CHECK(actual.cols() == expected.cols()); + CHECK(actual.rows() == expected.rows()); +} + +/* ************************************************************************* */ +// Test out polymorphic transform +#include +#include +#include + +struct triple { + template struct result; // says we will provide result + + template + struct result { + typedef double type; // result for int argument + }; + + template + struct result { + typedef double type; // result for int argument + }; + + template + struct result { + typedef double type; // result for double argument + }; + + template + struct result { + typedef double type; // result for double argument + }; + + // actual function + template + typename result::type operator()(const T& x) const { + return (double) x; + } +}; + +// Test out polymorphic transform +TEST(ExpressionFactor, Triple) { + typedef boost::fusion::vector IntDouble; + IntDouble H = boost::fusion::make_vector(1, 2.0); + + // Only works if I use Double2 + typedef boost::fusion::result_of::transform::type Result; + typedef boost::fusion::vector Double2; + Double2 D = boost::fusion::transform(H, triple()); + + using boost::fusion::at_c; + DOUBLES_EQUAL(1.0, at_c<0>(D), 1e-9); + DOUBLES_EQUAL(2.0, at_c<1>(D), 1e-9); +} + +/* ************************************************************************* */ +#include +#include +#include +#include + +// Test out invoke +TEST(ExpressionFactor, Invoke) { + EXPECT_LONGS_EQUAL(2, invoke(plus(),boost::fusion::make_vector(1,1))); + + // Creating a Pose3 (is there another way?) + boost::fusion::vector pair; + Pose3 pose = boost::fusion::invoke(boost::value_factory(), pair); +} + +/* ************************************************************************* */ +// debug const issue (how to make read/write arguments for invoke) +struct test { + typedef void result_type; + void operator()(int& a, int& b) const { + a = 6; + b = 7; + } +}; + +TEST(ExpressionFactor, ConstIssue) { + int a, b; + boost::fusion::invoke(test(), + boost::fusion::make_vector(boost::ref(a), boost::ref(b))); + LONGS_EQUAL(6, a); + LONGS_EQUAL(7, b); +} + +/* ************************************************************************* */ +// Test out invoke on a given GTSAM function +// then construct prototype for it's derivatives +TEST(ExpressionFactor, InvokeDerivatives) { + // This is the method in Pose3: + // Point3 transform_to(const Point3& p) const; + // Point3 transform_to(const Point3& p, + // boost::optional Dpose, boost::optional Dpoint) const; + + // Let's assign it it to a boost function object + // cast is needed because Pose3::transform_to is overloaded + typedef boost::function F; + F f = static_cast(&Pose3::transform_to); + + // Create arguments +Pose3 pose; + Point3 point; + typedef boost::fusion::vector Arguments; + Arguments args = boost::fusion::make_vector(pose, point); + + // Create fused function (takes fusion vector) and call it + boost::fusion::fused g(f); + Point3 actual = g(args); + CHECK(assert_equal(point,actual)); + + // We can *immediately* do this using invoke + Point3 actual2 = boost::fusion::invoke(f, args); + CHECK(assert_equal(point,actual2)); + + // Now, let's create the optional Jacobian arguments + typedef Point3 T; + typedef boost::mpl::vector TYPES; + typedef boost::mpl::transform >::type Optionals; + + // Unfortunately this is moot: we need a pointer to a function with the + // optional derivatives; I don't see a way of calling a function that we + // did not get access to by the caller passing us a pointer. + // Let's test below whether we can have a proxy object +} + +struct proxy { + typedef Point3 result_type; + Point3 operator()(const Pose3& pose, const Point3& point) const { + return pose.transform_to(point); + } + Point3 operator()(const Pose3& pose, const Point3& point, + boost::optional Dpose, + boost::optional Dpoint) const { + return pose.transform_to(point, Dpose, Dpoint); + } +}; + +TEST(ExpressionFactor, InvokeDerivatives2) { + // without derivatives + Pose3 pose; + Point3 point; + Point3 actual = boost::fusion::invoke(proxy(), + boost::fusion::make_vector(pose, point)); + CHECK(assert_equal(point,actual)); + + // with derivatives, does not work, const issue again + Matrix36 Dpose; + Matrix3 Dpoint; + Point3 actual2 = boost::fusion::invoke(proxy(), + boost::fusion::make_vector(pose, point, boost::ref(Dpose), + boost::ref(Dpoint))); + CHECK(assert_equal(point,actual2)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + diff --git a/gtsam_unstable/slam/AHRS.cpp b/gtsam_unstable/slam/AHRS.cpp index af562c1b2..1719abc86 100644 --- a/gtsam_unstable/slam/AHRS.cpp +++ b/gtsam_unstable/slam/AHRS.cpp @@ -48,12 +48,12 @@ AHRS::AHRS(const Matrix& stationaryU, const Matrix& stationaryF, double g_e, F_g_ = -I3 / tau_g; F_a_ = -I3 / tau_a; - Vector var_omega_w = 0 * ones(3); // TODO - Vector var_omega_g = (0.0034 * 0.0034) * ones(3); - Vector var_omega_a = (0.034 * 0.034) * ones(3); - Vector sigmas_v_g_sq = emul(sigmas_v_g, sigmas_v_g); - Vector vars = concatVectors(4, &var_omega_w, &var_omega_g, &sigmas_v_g_sq, - &var_omega_a); + Vector3 var_omega_w = 0 * ones(3); // TODO + Vector3 var_omega_g = (0.0034 * 0.0034) * ones(3); + Vector3 var_omega_a = (0.034 * 0.034) * ones(3); + Vector3 sigmas_v_g_sq = emul(sigmas_v_g, sigmas_v_g); + Vector vars(12); + vars << var_omega_w, var_omega_g, sigmas_v_g_sq, var_omega_a; var_w_ = diag(vars); // Quantities needed for aiding diff --git a/gtsam_unstable/slam/BetweenFactorEM.h b/gtsam_unstable/slam/BetweenFactorEM.h index c147552b3..9082c0101 100644 --- a/gtsam_unstable/slam/BetweenFactorEM.h +++ b/gtsam_unstable/slam/BetweenFactorEM.h @@ -25,384 +25,405 @@ namespace gtsam { +/** + * A class for a measurement predicted by "between(config[key1],config[key2])" + * @tparam VALUE the Value type + * @addtogroup SLAM + */ +template +class BetweenFactorEM: public NonlinearFactor { + +public: + + typedef VALUE T; + +private: + + typedef BetweenFactorEM This; + typedef gtsam::NonlinearFactor Base; + + gtsam::Key key1_; + gtsam::Key key2_; + + VALUE measured_; /** The measurement */ + + SharedGaussian model_inlier_; + SharedGaussian model_outlier_; + + double prior_inlier_; + double prior_outlier_; + + bool flag_bump_up_near_zero_probs_; + + /** concept check by type */ + GTSAM_CONCEPT_LIE_TYPE(T)GTSAM_CONCEPT_TESTABLE_TYPE(T) + +public: + + // shorthand for a smart pointer to a factor + typedef typename boost::shared_ptr shared_ptr; + + /** default constructor - only use for serialization */ + BetweenFactorEM() { + } + + /** Constructor */ + BetweenFactorEM(Key key1, Key key2, const VALUE& measured, + const SharedGaussian& model_inlier, const SharedGaussian& model_outlier, + const double prior_inlier, const double prior_outlier, + const bool flag_bump_up_near_zero_probs = false) : + Base(cref_list_of<2>(key1)(key2)), key1_(key1), key2_(key2), measured_( + measured), model_inlier_(model_inlier), model_outlier_(model_outlier), prior_inlier_( + prior_inlier), prior_outlier_(prior_outlier), flag_bump_up_near_zero_probs_( + flag_bump_up_near_zero_probs) { + } + + virtual ~BetweenFactorEM() { + } + + /** implement functions needed for Testable */ + + /** print */ + virtual void print(const std::string& s, const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const { + std::cout << s << "BetweenFactorEM(" << keyFormatter(key1_) << "," + << keyFormatter(key2_) << ")\n"; + measured_.print(" measured: "); + model_inlier_->print(" noise model inlier: "); + model_outlier_->print(" noise model outlier: "); + std::cout << "(prior_inlier, prior_outlier_) = (" << prior_inlier_ << "," + << prior_outlier_ << ")\n"; + // Base::print(s, keyFormatter); + } + + /** equals */ + virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const { + const This *t = dynamic_cast(&f); + + if (t && Base::equals(f)) + return key1_ == t->key1_ && key2_ == t->key2_ + && + // model_inlier_->equals(t->model_inlier_ ) && // TODO: fix here + // model_outlier_->equals(t->model_outlier_ ) && + prior_outlier_ == t->prior_outlier_ + && prior_inlier_ == t->prior_inlier_ && measured_.equals(t->measured_); + else + return false; + } + + /** implement functions needed to derive from Factor */ + + /* ************************************************************************* */ + virtual double error(const gtsam::Values& x) const { + return whitenedError(x).squaredNorm(); + } + + /* ************************************************************************* */ /** - * A class for a measurement predicted by "between(config[key1],config[key2])" - * @tparam VALUE the Value type - * @addtogroup SLAM + * Linearize a non-linearFactorN to get a gtsam::GaussianFactor, + * \f$ Ax-b \approx h(x+\delta x)-z = h(x) + A \delta x - z \f$ + * Hence \f$ b = z - h(x) = - \mathtt{error\_vector}(x) \f$ */ - template - class BetweenFactorEM: public NonlinearFactor { + /* This version of linearize recalculates the noise model each time */ + virtual boost::shared_ptr linearize( + const gtsam::Values& x) const { + // Only linearize if the factor is active + if (!this->active(x)) + return boost::shared_ptr(); - public: + //std::cout<<"About to linearize"< A(this->size()); + gtsam::Vector b = -whitenedError(x, A); + A1 = A[0]; + A2 = A[1]; - typedef VALUE T; + return gtsam::GaussianFactor::shared_ptr( + new gtsam::JacobianFactor(key1_, A1, key2_, A2, b, + gtsam::noiseModel::Unit::Create(b.size()))); + } - private: + /* ************************************************************************* */ + gtsam::Vector whitenedError(const gtsam::Values& x, + boost::optional&> H = boost::none) const { - typedef BetweenFactorEM This; - typedef gtsam::NonlinearFactor Base; + bool debug = true; - gtsam::Key key1_; - gtsam::Key key2_; + const T& p1 = x.at(key1_); + const T& p2 = x.at(key2_); - VALUE measured_; /** The measurement */ + Matrix H1, H2; - SharedGaussian model_inlier_; - SharedGaussian model_outlier_; + T hx = p1.between(p2, H1, H2); // h(x) + // manifold equivalent of h(x)-z -> log(z,h(x)) - double prior_inlier_; - double prior_outlier_; + Vector err = measured_.localCoordinates(hx); - bool flag_bump_up_near_zero_probs_; + // Calculate indicator probabilities (inlier and outlier) + Vector p_inlier_outlier = calcIndicatorProb(x); + double p_inlier = p_inlier_outlier[0]; + double p_outlier = p_inlier_outlier[1]; - /** concept check by type */ - GTSAM_CONCEPT_LIE_TYPE(T) - GTSAM_CONCEPT_TESTABLE_TYPE(T) + Vector err_wh_inlier = model_inlier_->whiten(err); + Vector err_wh_outlier = model_outlier_->whiten(err); - public: + Matrix invCov_inlier = model_inlier_->R().transpose() * model_inlier_->R(); + Matrix invCov_outlier = model_outlier_->R().transpose() + * model_outlier_->R(); - // shorthand for a smart pointer to a factor - typedef typename boost::shared_ptr shared_ptr; + Vector err_wh_eq; + err_wh_eq.resize(err_wh_inlier.rows() * 2); + err_wh_eq << sqrt(p_inlier) * err_wh_inlier.array(), sqrt(p_outlier) + * err_wh_outlier.array(); - /** default constructor - only use for serialization */ - BetweenFactorEM() {} + if (H) { + // stack Jacobians for the two indicators for each of the key - /** Constructor */ - BetweenFactorEM(Key key1, Key key2, const VALUE& measured, - const SharedGaussian& model_inlier, const SharedGaussian& model_outlier, - const double prior_inlier, const double prior_outlier, const bool flag_bump_up_near_zero_probs = false) : - Base(cref_list_of<2>(key1)(key2)), key1_(key1), key2_(key2), measured_(measured), - model_inlier_(model_inlier), model_outlier_(model_outlier), - prior_inlier_(prior_inlier), prior_outlier_(prior_outlier), flag_bump_up_near_zero_probs_(flag_bump_up_near_zero_probs){ + Matrix H1_inlier = sqrt(p_inlier) * model_inlier_->Whiten(H1); + Matrix H1_outlier = sqrt(p_outlier) * model_outlier_->Whiten(H1); + Matrix H1_aug = gtsam::stack(2, &H1_inlier, &H1_outlier); + + Matrix H2_inlier = sqrt(p_inlier) * model_inlier_->Whiten(H2); + Matrix H2_outlier = sqrt(p_outlier) * model_outlier_->Whiten(H2); + Matrix H2_aug = gtsam::stack(2, &H2_inlier, &H2_outlier); + + (*H)[0].resize(H1_aug.rows(), H1_aug.cols()); + (*H)[1].resize(H2_aug.rows(), H2_aug.cols()); + + (*H)[0] = H1_aug; + (*H)[1] = H2_aug; } - virtual ~BetweenFactorEM() {} + if (debug) { + // std::cout<<"unwhitened error: "<print(" noise model inlier: "); - model_outlier_->print(" noise model outlier: "); - std::cout << "(prior_inlier, prior_outlier_) = (" - << prior_inlier_ << "," - << prior_outlier_ << ")\n"; - // Base::print(s, keyFormatter); + // Matrix Cov_inlier = invCov_inlier.inverse(); + // Matrix Cov_outlier = invCov_outlier.inverse(); + // std::cout<<"Cov_inlier: "< (&f); + return err_wh_eq; + } - if(t && Base::equals(f)) - return key1_ == t->key1_ && key2_ == t->key2_ && - // model_inlier_->equals(t->model_inlier_ ) && // TODO: fix here - // model_outlier_->equals(t->model_outlier_ ) && - prior_outlier_ == t->prior_outlier_ && prior_inlier_ == t->prior_inlier_ && measured_.equals(t->measured_); - else - return false; + /* ************************************************************************* */ + gtsam::Vector calcIndicatorProb(const gtsam::Values& x) const { + + bool debug = false; + + Vector err = unwhitenedError(x); + + // Calculate indicator probabilities (inlier and outlier) + Vector err_wh_inlier = model_inlier_->whiten(err); + Vector err_wh_outlier = model_outlier_->whiten(err); + + Matrix invCov_inlier = model_inlier_->R().transpose() * model_inlier_->R(); + Matrix invCov_outlier = model_outlier_->R().transpose() + * model_outlier_->R(); + + double p_inlier = prior_inlier_ * std::sqrt(invCov_inlier.determinant()) + * exp(-0.5 * err_wh_inlier.dot(err_wh_inlier)); + double p_outlier = prior_outlier_ * std::sqrt(invCov_outlier.determinant()) + * exp(-0.5 * err_wh_outlier.dot(err_wh_outlier)); + + if (debug) { + std::cout << "in calcIndicatorProb. err_unwh: " << err[0] << ", " + << err[1] << ", " << err[2] << std::endl; + std::cout << "in calcIndicatorProb. err_wh_inlier: " << err_wh_inlier[0] + << ", " << err_wh_inlier[1] << ", " << err_wh_inlier[2] << std::endl; + std::cout << "in calcIndicatorProb. err_wh_inlier.dot(err_wh_inlier): " + << err_wh_inlier.dot(err_wh_inlier) << std::endl; + std::cout << "in calcIndicatorProb. err_wh_outlier.dot(err_wh_outlier): " + << err_wh_outlier.dot(err_wh_outlier) << std::endl; + + std::cout + << "in calcIndicatorProb. p_inlier, p_outlier before normalization: " + << p_inlier << ", " << p_outlier << std::endl; } - /** implement functions needed to derive from Factor */ + double sumP = p_inlier + p_outlier; + p_inlier /= sumP; + p_outlier /= sumP; - /* ************************************************************************* */ - virtual double error(const gtsam::Values& x) const { - return whitenedError(x).squaredNorm(); + if (flag_bump_up_near_zero_probs_) { + // Bump up near-zero probabilities (as in linerFlow.h) + double minP = 0.05; // == 0.1 / 2 indicator variables + if (p_inlier < minP || p_outlier < minP) { + if (p_inlier < minP) + p_inlier = minP; + if (p_outlier < minP) + p_outlier = minP; + sumP = p_inlier + p_outlier; + p_inlier /= sumP; + p_outlier /= sumP; + } } - /* ************************************************************************* */ - /** - * Linearize a non-linearFactorN to get a gtsam::GaussianFactor, - * \f$ Ax-b \approx h(x+\delta x)-z = h(x) + A \delta x - z \f$ - * Hence \f$ b = z - h(x) = - \mathtt{error\_vector}(x) \f$ + return (Vector(2) << p_inlier, p_outlier); + } + + /* ************************************************************************* */ + gtsam::Vector unwhitenedError(const gtsam::Values& x) const { + + const T& p1 = x.at(key1_); + const T& p2 = x.at(key2_); + + Matrix H1, H2; + + T hx = p1.between(p2, H1, H2); // h(x) + + return measured_.localCoordinates(hx); + } + + /* ************************************************************************* */ + void set_flag_bump_up_near_zero_probs(bool flag) { + flag_bump_up_near_zero_probs_ = flag; + } + + /* ************************************************************************* */ + bool get_flag_bump_up_near_zero_probs() const { + return flag_bump_up_near_zero_probs_; + } + + /* ************************************************************************* */ + SharedGaussian get_model_inlier() const { + return model_inlier_; + } + + /* ************************************************************************* */ + SharedGaussian get_model_outlier() const { + return model_outlier_; + } + + /* ************************************************************************* */ + Matrix get_model_inlier_cov() const { + return (model_inlier_->R().transpose() * model_inlier_->R()).inverse(); + } + + /* ************************************************************************* */ + Matrix get_model_outlier_cov() const { + return (model_outlier_->R().transpose() * model_outlier_->R()).inverse(); + } + + /* ************************************************************************* */ + void updateNoiseModels(const gtsam::Values& values, + const gtsam::NonlinearFactorGraph& graph) { + /* Update model_inlier_ and model_outlier_ to account for uncertainty in robot trajectories + * (note these are given in the E step, where indicator probabilities are calculated). + * + * Principle: R += [H1 H2] * joint_cov12 * [H1 H2]', where H1, H2 are Jacobians of the + * unwhitened error w.r.t. states, and R is the measurement covariance (inlier or outlier modes). + * + * TODO: improve efficiency (info form) */ - /* This version of linearize recalculates the noise model each time */ - virtual boost::shared_ptr linearize(const gtsam::Values& x) const { - // Only linearize if the factor is active - if (!this->active(x)) - return boost::shared_ptr(); - //std::cout<<"About to linearize"< A(this->size()); - gtsam::Vector b = -whitenedError(x, A); - A1 = A[0]; - A2 = A[1]; + // get joint covariance of the involved states + std::vector Keys; + Keys.push_back(key1_); + Keys.push_back(key2_); + Marginals marginals(graph, values, Marginals::QR); + JointMarginal joint_marginal12 = marginals.jointMarginalCovariance(Keys); + Matrix cov1 = joint_marginal12(key1_, key1_); + Matrix cov2 = joint_marginal12(key2_, key2_); + Matrix cov12 = joint_marginal12(key1_, key2_); - return gtsam::GaussianFactor::shared_ptr( - new gtsam::JacobianFactor(key1_, A1, key2_, A2, b, gtsam::noiseModel::Unit::Create(b.size()))); - } + updateNoiseModels_givenCovs(values, cov1, cov2, cov12); + } + /* ************************************************************************* */ + void updateNoiseModels_givenCovs(const gtsam::Values& values, + const Matrix& cov1, const Matrix& cov2, const Matrix& cov12) { + /* Update model_inlier_ and model_outlier_ to account for uncertainty in robot trajectories + * (note these are given in the E step, where indicator probabilities are calculated). + * + * Principle: R += [H1 H2] * joint_cov12 * [H1 H2]', where H1, H2 are Jacobians of the + * unwhitened error w.r.t. states, and R is the measurement covariance (inlier or outlier modes). + * + * TODO: improve efficiency (info form) + */ - /* ************************************************************************* */ - gtsam::Vector whitenedError(const gtsam::Values& x, - boost::optional&> H = boost::none) const { + const T& p1 = values.at(key1_); + const T& p2 = values.at(key2_); - bool debug = true; + Matrix H1, H2; + p1.between(p2, H1, H2); // h(x) - const T& p1 = x.at(key1_); - const T& p2 = x.at(key2_); + Matrix H; + H.resize(H1.rows(), H1.rows() + H2.rows()); + H << H1, H2; // H = [H1 H2] - Matrix H1, H2; + Matrix joint_cov; + joint_cov.resize(cov1.rows() + cov2.rows(), cov1.cols() + cov2.cols()); + joint_cov << cov1, cov12, cov12.transpose(), cov2; - T hx = p1.between(p2, H1, H2); // h(x) - // manifold equivalent of h(x)-z -> log(z,h(x)) + Matrix cov_state = H * joint_cov * H.transpose(); - Vector err = measured_.localCoordinates(hx); + // model_inlier_->print("before:"); - // Calculate indicator probabilities (inlier and outlier) - Vector p_inlier_outlier = calcIndicatorProb(x); - double p_inlier = p_inlier_outlier[0]; - double p_outlier = p_inlier_outlier[1]; + // update inlier and outlier noise models + Matrix covRinlier = + (model_inlier_->R().transpose() * model_inlier_->R()).inverse(); + model_inlier_ = gtsam::noiseModel::Gaussian::Covariance( + covRinlier + cov_state); - Vector err_wh_inlier = model_inlier_->whiten(err); - Vector err_wh_outlier = model_outlier_->whiten(err); + Matrix covRoutlier = + (model_outlier_->R().transpose() * model_outlier_->R()).inverse(); + model_outlier_ = gtsam::noiseModel::Gaussian::Covariance( + covRoutlier + cov_state); - Matrix invCov_inlier = model_inlier_->R().transpose() * model_inlier_->R(); - Matrix invCov_outlier = model_outlier_->R().transpose() * model_outlier_->R(); + // model_inlier_->print("after:"); + // std::cout<<"covRinlier + cov_state: "<Whiten(H1); - Matrix H1_outlier = sqrt(p_outlier)*model_outlier_->Whiten(H1); - Matrix H1_aug = gtsam::stack(2, &H1_inlier, &H1_outlier); + virtual size_t dim() const { + return model_inlier_->R().rows() + model_inlier_->R().cols(); + } - Matrix H2_inlier = sqrt(p_inlier)*model_inlier_->Whiten(H2); - Matrix H2_outlier = sqrt(p_outlier)*model_outlier_->Whiten(H2); - Matrix H2_aug = gtsam::stack(2, &H2_inlier, &H2_outlier); +private: - (*H)[0].resize(H1_aug.rows(),H1_aug.cols()); - (*H)[1].resize(H2_aug.rows(),H2_aug.cols()); + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar + & boost::serialization::make_nvp("NonlinearFactor", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(measured_); + } +}; +// \class BetweenFactorEM - (*H)[0] = H1_aug; - (*H)[1] = H2_aug; - } - - if (debug){ - // std::cout<<"unwhitened error: "<whiten(err); - Vector err_wh_outlier = model_outlier_->whiten(err); - - Matrix invCov_inlier = model_inlier_->R().transpose() * model_inlier_->R(); - Matrix invCov_outlier = model_outlier_->R().transpose() * model_outlier_->R(); - - double p_inlier = prior_inlier_ * std::sqrt(invCov_inlier.determinant()) * exp( -0.5 * err_wh_inlier.dot(err_wh_inlier) ); - double p_outlier = prior_outlier_ * std::sqrt(invCov_outlier.determinant()) * exp( -0.5 * err_wh_outlier.dot(err_wh_outlier) ); - - if (debug){ - std::cout<<"in calcIndicatorProb. err_unwh: "<(key1_); - const T& p2 = x.at(key2_); - - Matrix H1, H2; - - T hx = p1.between(p2, H1, H2); // h(x) - - return measured_.localCoordinates(hx); - } - - /* ************************************************************************* */ - void set_flag_bump_up_near_zero_probs(bool flag) { - flag_bump_up_near_zero_probs_ = flag; - } - - /* ************************************************************************* */ - bool get_flag_bump_up_near_zero_probs() const { - return flag_bump_up_near_zero_probs_; - } - - /* ************************************************************************* */ - SharedGaussian get_model_inlier() const { - return model_inlier_; - } - - /* ************************************************************************* */ - SharedGaussian get_model_outlier() const { - return model_outlier_; - } - - /* ************************************************************************* */ - Matrix get_model_inlier_cov() const { - return (model_inlier_->R().transpose()*model_inlier_->R()).inverse(); - } - - /* ************************************************************************* */ - Matrix get_model_outlier_cov() const { - return (model_outlier_->R().transpose()*model_outlier_->R()).inverse(); - } - - /* ************************************************************************* */ - void updateNoiseModels(const gtsam::Values& values, const gtsam::NonlinearFactorGraph& graph){ - /* Update model_inlier_ and model_outlier_ to account for uncertainty in robot trajectories - * (note these are given in the E step, where indicator probabilities are calculated). - * - * Principle: R += [H1 H2] * joint_cov12 * [H1 H2]', where H1, H2 are Jacobians of the - * unwhitened error w.r.t. states, and R is the measurement covariance (inlier or outlier modes). - * - * TODO: improve efficiency (info form) - */ - - // get joint covariance of the involved states - std::vector Keys; - Keys.push_back(key1_); - Keys.push_back(key2_); - Marginals marginals( graph, values, Marginals::QR ); - JointMarginal joint_marginal12 = marginals.jointMarginalCovariance(Keys); - Matrix cov1 = joint_marginal12(key1_, key1_); - Matrix cov2 = joint_marginal12(key2_, key2_); - Matrix cov12 = joint_marginal12(key1_, key2_); - - updateNoiseModels_givenCovs(values, cov1, cov2, cov12); - } - - /* ************************************************************************* */ - void updateNoiseModels_givenCovs(const gtsam::Values& values, const Matrix& cov1, const Matrix& cov2, const Matrix& cov12){ - /* Update model_inlier_ and model_outlier_ to account for uncertainty in robot trajectories - * (note these are given in the E step, where indicator probabilities are calculated). - * - * Principle: R += [H1 H2] * joint_cov12 * [H1 H2]', where H1, H2 are Jacobians of the - * unwhitened error w.r.t. states, and R is the measurement covariance (inlier or outlier modes). - * - * TODO: improve efficiency (info form) - */ - - const T& p1 = values.at(key1_); - const T& p2 = values.at(key2_); - - Matrix H1, H2; - T hx = p1.between(p2, H1, H2); // h(x) - - Matrix H; - H.resize(H1.rows(), H1.rows()+H2.rows()); - H << H1, H2; // H = [H1 H2] - - Matrix joint_cov; - joint_cov.resize(cov1.rows()+cov2.rows(), cov1.cols()+cov2.cols()); - joint_cov << cov1, cov12, - cov12.transpose(), cov2; - - Matrix cov_state = H*joint_cov*H.transpose(); - - // model_inlier_->print("before:"); - - // update inlier and outlier noise models - Matrix covRinlier = (model_inlier_->R().transpose()*model_inlier_->R()).inverse(); - model_inlier_ = gtsam::noiseModel::Gaussian::Covariance(covRinlier + cov_state); - - Matrix covRoutlier = (model_outlier_->R().transpose()*model_outlier_->R()).inverse(); - model_outlier_ = gtsam::noiseModel::Gaussian::Covariance(covRoutlier + cov_state); - - // model_inlier_->print("after:"); - // std::cout<<"covRinlier + cov_state: "<R().rows() + model_inlier_->R().cols(); - } - - private: - - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("NonlinearFactor", - boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(measured_); - } - }; // \class BetweenFactorEM - -} /// namespace gtsam +}/// namespace gtsam diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h index b2174f8a9..f7130d611 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h @@ -22,7 +22,6 @@ #include #include #include -#include #include // Using numerical derivative to calculate d(Pose3::Expmap)/dw @@ -269,7 +268,7 @@ public: VelDelta -= 2*skewSymmetric(world_rho + world_omega_earth)*world_V1_body * dt12; // Predict - return Vel1.compose( VelDelta ); + return Vel1 + VelDelta; } @@ -295,7 +294,7 @@ public: VELOCITY Vel2Pred = predictVelocity(Pose1, Vel1, Bias1); // Calculate error - return Vel2.between(Vel2Pred); + return Vel2Pred-Vel2; } Vector evaluateError(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1, const POSE& Pose2, const VELOCITY& Vel2, @@ -315,8 +314,9 @@ public: // Jacobian w.r.t. Vel1 if (H2){ - Matrix H2_Pose = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1); - Matrix H2_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1); + if (Vel1.size()!=3) throw std::runtime_error("Frank's hack to make this compile will not work if size != 3"); + Matrix H2_Pose = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1); + Matrix H2_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1); *H2 = stack(2, &H2_Pose, &H2_Vel); } @@ -336,13 +336,14 @@ public: // Jacobian w.r.t. Vel2 if (H5){ - Matrix H5_Pose = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2); - Matrix H5_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2); + if (Vel2.size()!=3) throw std::runtime_error("Frank's hack to make this compile will not work if size != 3"); + Matrix H5_Pose = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2); + Matrix H5_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2); *H5 = stack(2, &H5_Pose, &H5_Vel); } Vector ErrPoseVector(POSE::Logmap(evaluatePoseError(Pose1, Vel1, Bias1, Pose2, Vel2))); - Vector ErrVelVector(VELOCITY::Logmap(evaluateVelocityError(Pose1, Vel1, Bias1, Pose2, Vel2))); + Vector ErrVelVector(evaluateVelocityError(Pose1, Vel1, Bias1, Pose2, Vel2)); return concatVectors(2, &ErrPoseVector, &ErrVelVector); } @@ -436,18 +437,18 @@ public: Matrix Z_3x3 = zeros(3,3); Matrix I_3x3 = eye(3,3); - Matrix H_pos_pos = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, _1, delta_vel_in_t0), delta_pos_in_t0); - Matrix H_pos_vel = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, delta_pos_in_t0, _1), delta_vel_in_t0); + Matrix H_pos_pos = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, _1, delta_vel_in_t0), delta_pos_in_t0); + Matrix H_pos_vel = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, delta_pos_in_t0, _1), delta_vel_in_t0); Matrix H_pos_angles = Z_3x3; Matrix H_pos_bias = collect(2, &Z_3x3, &Z_3x3); - Matrix H_vel_vel = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, delta_angles, _1, flag_use_body_P_sensor, body_P_sensor, Bias_t0), delta_vel_in_t0); - Matrix H_vel_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, _1, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor, Bias_t0), delta_angles); - Matrix H_vel_bias = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, delta_angles, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor, _1), Bias_t0); + Matrix H_vel_vel = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, delta_angles, _1, flag_use_body_P_sensor, body_P_sensor, Bias_t0), delta_vel_in_t0); + Matrix H_vel_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, _1, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor, Bias_t0), delta_angles); + Matrix H_vel_bias = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, delta_angles, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor, _1), Bias_t0); Matrix H_vel_pos = Z_3x3; - Matrix H_angles_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, msr_dt, _1, flag_use_body_P_sensor, body_P_sensor, Bias_t0), delta_angles); - Matrix H_angles_bias = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, msr_dt, delta_angles, flag_use_body_P_sensor, body_P_sensor, _1), Bias_t0); + Matrix H_angles_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, msr_dt, _1, flag_use_body_P_sensor, body_P_sensor, Bias_t0), delta_angles); + Matrix H_angles_bias = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, msr_dt, delta_angles, flag_use_body_P_sensor, body_P_sensor, _1), Bias_t0); Matrix H_angles_pos = Z_3x3; Matrix H_angles_vel = Z_3x3; diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h index dec1b2378..a38525bd9 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h @@ -22,7 +22,6 @@ #include #include #include -#include #include // Using numerical derivative to calculate d(Pose3::Expmap)/dw diff --git a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h index 5ca736c01..908239d93 100644 --- a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h +++ b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h @@ -21,7 +21,6 @@ #include #include #include -#include #include // Using numerical derivative to calculate d(Pose3::Expmap)/dw @@ -200,7 +199,7 @@ public: VELOCITY VelDelta(world_a_body*dt_); // Predict - return Vel1.compose(VelDelta); + return Vel1 + VelDelta; } void predict(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1, POSE& Pose2, VELOCITY& Vel2) const { @@ -221,7 +220,7 @@ public: VELOCITY Vel2Pred = predictVelocity(Pose1, Vel1, Bias1); // Calculate error - return Vel2.between(Vel2Pred); + return Vel2Pred - Vel2; } /** implement functions needed to derive from Factor */ @@ -242,8 +241,9 @@ public: // Jacobian w.r.t. Vel1 if (H2){ - Matrix H2_Pose = gtsam::numericalDerivative11(boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1); - Matrix H2_Vel = gtsam::numericalDerivative11(boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1); + if (Vel1.size()!=3) throw std::runtime_error("Frank's hack to make this compile will not work if size != 3"); + Matrix H2_Pose = gtsam::numericalDerivative11(boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1); + Matrix H2_Vel = gtsam::numericalDerivative11(boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1); *H2 = stack(2, &H2_Pose, &H2_Vel); } @@ -263,13 +263,14 @@ public: // Jacobian w.r.t. Vel2 if (H5){ - Matrix H5_Pose = gtsam::numericalDerivative11(boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2); - Matrix H5_Vel = gtsam::numericalDerivative11(boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2); + if (Vel2.size()!=3) throw std::runtime_error("Frank's hack to make this compile will not work if size != 3"); + Matrix H5_Pose = gtsam::numericalDerivative11(boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2); + Matrix H5_Vel = gtsam::numericalDerivative11(boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2); *H5 = stack(2, &H5_Pose, &H5_Vel); } Vector ErrPoseVector(POSE::Logmap(evaluatePoseError(Pose1, Vel1, Bias1, Pose2, Vel2))); - Vector ErrVelVector(VELOCITY::Logmap(evaluateVelocityError(Pose1, Vel1, Bias1, Pose2, Vel2))); + Vector ErrVelVector(evaluateVelocityError(Pose1, Vel1, Bias1, Pose2, Vel2)); return concatVectors(2, &ErrPoseVector, &ErrVelVector); } diff --git a/gtsam_unstable/slam/InvDepthFactor3.h b/gtsam_unstable/slam/InvDepthFactor3.h index 29f9d4972..ae30a4a49 100644 --- a/gtsam_unstable/slam/InvDepthFactor3.h +++ b/gtsam_unstable/slam/InvDepthFactor3.h @@ -80,7 +80,7 @@ public: } /// Evaluate error h(x)-z and optionally derivatives - gtsam::Vector evaluateError(const POSE& pose, const gtsam::LieVector& point, const INVDEPTH& invDepth, + gtsam::Vector evaluateError(const POSE& pose, const Vector5& point, const INVDEPTH& invDepth, boost::optional H1=boost::none, boost::optional H2=boost::none, boost::optional H3=boost::none) const { diff --git a/gtsam_unstable/slam/InvDepthFactorVariant1.h b/gtsam_unstable/slam/InvDepthFactorVariant1.h index d61787358..6bf0722a5 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant1.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant1.h @@ -15,7 +15,6 @@ #include #include #include -#include #include namespace gtsam { @@ -23,7 +22,7 @@ namespace gtsam { /** * Binary factor representing a visual measurement using an inverse-depth parameterization */ -class InvDepthFactorVariant1: public NoiseModelFactor2 { +class InvDepthFactorVariant1: public NoiseModelFactor2 { protected: // Keep a copy of measurement and calibration for I/O @@ -33,7 +32,7 @@ protected: public: /// shorthand for base class type - typedef NoiseModelFactor2 Base; + typedef NoiseModelFactor2 Base; /// shorthand for this class typedef InvDepthFactorVariant1 This; @@ -79,7 +78,7 @@ public: && this->K_->equals(*e->K_, tol); } - Vector inverseDepthError(const Pose3& pose, const LieVector& landmark) const { + Vector inverseDepthError(const Pose3& pose, const Vector6& landmark) const { try { // Calculate the 3D coordinates of the landmark in the world frame double x = landmark(0), y = landmark(1), z = landmark(2); @@ -100,15 +99,19 @@ public: } /// Evaluate error h(x)-z and optionally derivatives - Vector evaluateError(const Pose3& pose, const LieVector& landmark, + Vector evaluateError(const Pose3& pose, const Vector6& landmark, boost::optional H1=boost::none, boost::optional H2=boost::none) const { - if(H1) { - (*H1) = numericalDerivative11(boost::bind(&InvDepthFactorVariant1::inverseDepthError, this, _1, landmark), pose); + if (H1) { + (*H1) = numericalDerivative11( + boost::bind(&InvDepthFactorVariant1::inverseDepthError, this, _1, + landmark), pose); } - if(H2) { - (*H2) = numericalDerivative11(boost::bind(&InvDepthFactorVariant1::inverseDepthError, this, pose, _1), landmark); + if (H2) { + (*H2) = numericalDerivative11( + boost::bind(&InvDepthFactorVariant1::inverseDepthError, this, pose, + _1), landmark); } return inverseDepthError(pose, landmark); diff --git a/gtsam_unstable/slam/InvDepthFactorVariant2.h b/gtsam_unstable/slam/InvDepthFactorVariant2.h index 9d4113431..ae26b4240 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant2.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant2.h @@ -16,7 +16,6 @@ #include #include #include -#include #include namespace gtsam { @@ -24,7 +23,7 @@ namespace gtsam { /** * Binary factor representing a visual measurement using an inverse-depth parameterization */ -class InvDepthFactorVariant2: public NoiseModelFactor2 { +class InvDepthFactorVariant2: public NoiseModelFactor2 { protected: // Keep a copy of measurement and calibration for I/O @@ -35,7 +34,7 @@ protected: public: /// shorthand for base class type - typedef NoiseModelFactor2 Base; + typedef NoiseModelFactor2 Base; /// shorthand for this class typedef InvDepthFactorVariant2 This; @@ -83,7 +82,7 @@ public: && this->referencePoint_.equals(e->referencePoint_, tol); } - Vector inverseDepthError(const Pose3& pose, const LieVector& landmark) const { + Vector inverseDepthError(const Pose3& pose, const Vector3& landmark) const { try { // Calculate the 3D coordinates of the landmark in the world frame double theta = landmark(0), phi = landmark(1), rho = landmark(2); @@ -103,15 +102,19 @@ public: } /// Evaluate error h(x)-z and optionally derivatives - Vector evaluateError(const Pose3& pose, const LieVector& landmark, + Vector evaluateError(const Pose3& pose, const Vector3& landmark, boost::optional H1=boost::none, boost::optional H2=boost::none) const { - if(H1) { - (*H1) = numericalDerivative11(boost::bind(&InvDepthFactorVariant2::inverseDepthError, this, _1, landmark), pose); + if (H1) { + (*H1) = numericalDerivative11( + boost::bind(&InvDepthFactorVariant2::inverseDepthError, this, _1, + landmark), pose); } - if(H2) { - (*H2) = numericalDerivative11(boost::bind(&InvDepthFactorVariant2::inverseDepthError, this, pose, _1), landmark); + if (H2) { + (*H2) = numericalDerivative11( + boost::bind(&InvDepthFactorVariant2::inverseDepthError, this, pose, + _1), landmark); } return inverseDepthError(pose, landmark); diff --git a/gtsam_unstable/slam/InvDepthFactorVariant3.h b/gtsam_unstable/slam/InvDepthFactorVariant3.h index aef15638f..f805e26a4 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant3.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant3.h @@ -15,7 +15,6 @@ #include #include #include -#include #include namespace gtsam { @@ -23,7 +22,7 @@ namespace gtsam { /** * Binary factor representing the first visual measurement using an inverse-depth parameterization */ -class InvDepthFactorVariant3a: public NoiseModelFactor2 { +class InvDepthFactorVariant3a: public NoiseModelFactor2 { protected: // Keep a copy of measurement and calibration for I/O @@ -33,7 +32,7 @@ protected: public: /// shorthand for base class type - typedef NoiseModelFactor2 Base; + typedef NoiseModelFactor2 Base; /// shorthand for this class typedef InvDepthFactorVariant3a This; @@ -81,7 +80,7 @@ public: && this->K_->equals(*e->K_, tol); } - Vector inverseDepthError(const Pose3& pose, const LieVector& landmark) const { + Vector inverseDepthError(const Pose3& pose, const Vector3& landmark) const { try { // Calculate the 3D coordinates of the landmark in the Pose frame double theta = landmark(0), phi = landmark(1), rho = landmark(2); @@ -103,15 +102,15 @@ public: } /// Evaluate error h(x)-z and optionally derivatives - Vector evaluateError(const Pose3& pose, const LieVector& landmark, + Vector evaluateError(const Pose3& pose, const Vector3& landmark, boost::optional H1=boost::none, boost::optional H2=boost::none) const { if(H1) { - (*H1) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3a::inverseDepthError, this, _1, landmark), pose); + (*H1) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3a::inverseDepthError, this, _1, landmark), pose); } if(H2) { - (*H2) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3a::inverseDepthError, this, pose, _1), landmark); + (*H2) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3a::inverseDepthError, this, pose, _1), landmark); } return inverseDepthError(pose, landmark); @@ -142,7 +141,7 @@ private: /** * Ternary factor representing a visual measurement using an inverse-depth parameterization */ -class InvDepthFactorVariant3b: public NoiseModelFactor3 { +class InvDepthFactorVariant3b: public NoiseModelFactor3 { protected: // Keep a copy of measurement and calibration for I/O @@ -152,7 +151,7 @@ protected: public: /// shorthand for base class type - typedef NoiseModelFactor3 Base; + typedef NoiseModelFactor3 Base; /// shorthand for this class typedef InvDepthFactorVariant3b This; @@ -200,7 +199,7 @@ public: && this->K_->equals(*e->K_, tol); } - Vector inverseDepthError(const Pose3& pose1, const Pose3& pose2, const LieVector& landmark) const { + Vector inverseDepthError(const Pose3& pose1, const Pose3& pose2, const Vector3& landmark) const { try { // Calculate the 3D coordinates of the landmark in the Pose1 frame double theta = landmark(0), phi = landmark(1), rho = landmark(2); @@ -222,20 +221,19 @@ public: } /// Evaluate error h(x)-z and optionally derivatives - Vector evaluateError(const Pose3& pose1, const Pose3& pose2, const LieVector& landmark, + Vector evaluateError(const Pose3& pose1, const Pose3& pose2, const Vector3& landmark, boost::optional H1=boost::none, boost::optional H2=boost::none, boost::optional H3=boost::none) const { - if(H1) { - (*H1) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, _1, pose2, landmark), pose1); - } - if(H2) { - (*H2) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1, _1, landmark), pose2); - } - if(H3) { - (*H3) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1, pose2, _1), landmark); - } + if(H1) + (*H1) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, _1, pose2, landmark), pose1); + + if(H2) + (*H2) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1, _1, landmark), pose2); + + if(H3) + (*H3) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1, pose2, _1), landmark); return inverseDepthError(pose1, pose2, landmark); } diff --git a/gtsam_unstable/slam/ProjectionFactorPPP.h b/gtsam_unstable/slam/ProjectionFactorPPP.h index e7060dcd4..b69f852b4 100644 --- a/gtsam_unstable/slam/ProjectionFactorPPP.h +++ b/gtsam_unstable/slam/ProjectionFactorPPP.h @@ -127,13 +127,13 @@ namespace gtsam { if(H1 || H2 || H3) { gtsam::Matrix H0, H02; PinholeCamera camera(pose.compose(transform, H0, H02), *K_); - Point2 reprojectionError(camera.project(point, H1, H3) - measured_); + Point2 reprojectionError(camera.project(point, H1, H3, boost::none) - measured_); *H2 = *H1 * H02; *H1 = *H1 * H0; return reprojectionError.vector(); } else { PinholeCamera camera(pose.compose(transform), *K_); - Point2 reprojectionError(camera.project(point, H1, H3) - measured_); + Point2 reprojectionError(camera.project(point, H1, H3, boost::none) - measured_); return reprojectionError.vector(); } } catch( CheiralityException& e) { diff --git a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h index 35010cdc6..2f0b2c7a8 100644 --- a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h +++ b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h @@ -372,7 +372,7 @@ namespace gtsam { const T& p2 = values.at(keyB_); Matrix H1, H2; - T hx = p1.between(p2, H1, H2); // h(x) + p1.between(p2, H1, H2); // h(x) Matrix H; H.resize(H1.rows(), H1.rows()+H2.rows()); diff --git a/gtsam_unstable/slam/expressions.h b/gtsam_unstable/slam/expressions.h new file mode 100644 index 000000000..d9cbd5716 --- /dev/null +++ b/gtsam_unstable/slam/expressions.h @@ -0,0 +1,68 @@ +/** + * @file expressions.h + * @brief Common expressions for solving geometry/slam/sfm problems + * @date Oct 1, 2014 + * @author Frank Dellaert + */ + +#pragma once + +#include +#include +#include +#include + +namespace gtsam { + +// Generics + +template +Expression between(const Expression& t1, const Expression& t2) { + return Expression(t1, &T::between, t2); +} + +// 2D Geometry + +typedef Expression Point2_; +typedef Expression Rot2_; +typedef Expression Pose2_; + +Point2_ transform_to(const Pose2_& x, const Point2_& p) { + return Point2_(x, &Pose2::transform_to, p); +} + +// 3D Geometry + +typedef Expression Point3_; +typedef Expression Rot3_; +typedef Expression Pose3_; + +Point3_ transform_to(const Pose3_& x, const Point3_& p) { + return Point3_(x, &Pose3::transform_to, p); +} + +// Projection + +typedef Expression Cal3_S2_; + +Point2_ project(const Point3_& p_cam) { + return Point2_(PinholeCamera::project_to_camera, p_cam); +} + +Point2 project6(const Pose3& x, const Point3& p, const Cal3_S2& K, + boost::optional Dpose, boost::optional Dpoint, + boost::optional Dcal) { + return PinholeCamera(x, K).project(p, Dpose, Dpoint, Dcal); +} + +Point2_ project3(const Pose3_& x, const Point3_& p, const Cal3_S2_& K) { + return Point2_(project6, x, p, K); +} + +template +Point2_ uncalibrate(const Expression& K, const Point2_& xy_hat) { + return Point2_(K, &CAL::uncalibrate, xy_hat); +} + +} // \namespace gtsam + diff --git a/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp b/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp index b3df86886..bd15b9203 100644 --- a/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp +++ b/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp @@ -10,7 +10,6 @@ #include #include #include -#include #include #include diff --git a/gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp b/gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp index 2a48aa73c..c0da655c6 100644 --- a/gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp +++ b/gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp @@ -21,7 +21,6 @@ #include #include #include -#include #include #include @@ -55,7 +54,7 @@ TEST( EquivInertialNavFactor_GlobalVel, Constructor) SharedGaussian imu_model = noiseModel::Gaussian::Covariance(EquivCov_Overall.block(0,0,9,9)); // Constructor - EquivInertialNavFactor_GlobalVel factor( + EquivInertialNavFactor_GlobalVel factor( poseKey1, velKey1, biasKey1, poseKey2, velKey2, delta_pos_in_t0, delta_vel_in_t0, delta_angles, delta_t, g, rho, omega_earth, imu_model, Jacobian_wrt_t0_Overall, bias1); diff --git a/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp b/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp index c708c1949..6cac34573 100644 --- a/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp +++ b/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp @@ -16,20 +16,22 @@ * @date Jan 17, 2012 */ -#include #include #include #include #include +#include + +#include using namespace std; using namespace gtsam; //! Factors -typedef GaussMarkov1stOrderFactor GaussMarkovFactor; +typedef GaussMarkov1stOrderFactor GaussMarkovFactor; /* ************************************************************************* */ -gtsam::LieVector predictionError(const gtsam::LieVector& v1, const gtsam::LieVector& v2, const GaussMarkovFactor factor) { +LieVector predictionError(const LieVector& v1, const LieVector& v2, const GaussMarkovFactor factor) { return factor.evaluateError(v1, v2); } @@ -37,37 +39,37 @@ gtsam::LieVector predictionError(const gtsam::LieVector& v1, const gtsam::LieVec TEST( GaussMarkovFactor, equals ) { // Create two identical factors and make sure they're equal - gtsam::Key x1(1); - gtsam::Key x2(2); + Key x1(1); + Key x2(2); double delta_t = 0.10; Vector tau = (Vector(3) << 100.0, 150.0, 10.0); - gtsam::SharedGaussian model = gtsam::noiseModel::Isotropic::Sigma(3, 1.0); + SharedGaussian model = noiseModel::Isotropic::Sigma(3, 1.0); GaussMarkovFactor factor1(x1, x2, delta_t, tau, model); GaussMarkovFactor factor2(x1, x2, delta_t, tau, model); - CHECK(gtsam::assert_equal(factor1, factor2)); + CHECK(assert_equal(factor1, factor2)); } /* ************************************************************************* */ TEST( GaussMarkovFactor, error ) { - gtsam::Values linPoint; - gtsam::Key x1(1); - gtsam::Key x2(2); + Values linPoint; + Key x1(1); + Key x2(2); double delta_t = 0.10; Vector tau = (Vector(3) << 100.0, 150.0, 10.0); - gtsam::SharedGaussian model = gtsam::noiseModel::Isotropic::Sigma(3, 1.0); + SharedGaussian model = noiseModel::Isotropic::Sigma(3, 1.0); - gtsam::LieVector v1 = gtsam::LieVector((gtsam::Vector(3) << 10.0, 12.0, 13.0)); - gtsam::LieVector v2 = gtsam::LieVector((gtsam::Vector(3) << 10.0, 15.0, 14.0)); + LieVector v1 = LieVector((Vector(3) << 10.0, 12.0, 13.0)); + LieVector v2 = LieVector((Vector(3) << 10.0, 15.0, 14.0)); // Create two nodes linPoint.insert(x1, v1); linPoint.insert(x2, v2); GaussMarkovFactor factor(x1, x2, delta_t, tau, model); - gtsam::Vector Err1( factor.evaluateError(v1, v2) ); + Vector Err1( factor.evaluateError(v1, v2) ); // Manually calculate the error Vector alpha(tau.size()); @@ -76,41 +78,41 @@ TEST( GaussMarkovFactor, error ) alpha(i) = exp(- 1/tau(i)*delta_t ); alpha_v1(i) = alpha(i) * v1(i); } - gtsam::Vector Err2( v2 - alpha_v1 ); + Vector Err2( v2 - alpha_v1 ); - CHECK(gtsam::assert_equal(Err1, Err2, 1e-9)); + CHECK(assert_equal(Err1, Err2, 1e-9)); } /* ************************************************************************* */ TEST (GaussMarkovFactor, jacobian ) { - gtsam::Values linPoint; - gtsam::Key x1(1); - gtsam::Key x2(2); + Values linPoint; + Key x1(1); + Key x2(2); double delta_t = 0.10; Vector tau = (Vector(3) << 100.0, 150.0, 10.0); - gtsam::SharedGaussian model = gtsam::noiseModel::Isotropic::Sigma(3, 1.0); + SharedGaussian model = noiseModel::Isotropic::Sigma(3, 1.0); GaussMarkovFactor factor(x1, x2, delta_t, tau, model); // Update the linearization point - gtsam::LieVector v1_upd = gtsam::LieVector((gtsam::Vector(3) << 0.5, -0.7, 0.3)); - gtsam::LieVector v2_upd = gtsam::LieVector((gtsam::Vector(3) << -0.7, 0.4, 0.9)); + LieVector v1_upd = LieVector((Vector(3) << 0.5, -0.7, 0.3)); + LieVector v2_upd = LieVector((Vector(3) << -0.7, 0.4, 0.9)); // Calculate the Jacobian matrix using the factor Matrix computed_H1, computed_H2; factor.evaluateError(v1_upd, v2_upd, computed_H1, computed_H2); // Calculate the Jacobian matrices H1 and H2 using the numerical derivative function - gtsam::Matrix numerical_H1, numerical_H2; - numerical_H1 = gtsam::numericalDerivative21(boost::bind(&predictionError, _1, _2, factor), v1_upd, v2_upd); - numerical_H2 = gtsam::numericalDerivative22(boost::bind(&predictionError, _1, _2, factor), v1_upd, v2_upd); + Matrix numerical_H1, numerical_H2; + numerical_H1 = numericalDerivative21( + boost::bind(&predictionError, _1, _2, factor), v1_upd, v2_upd); + numerical_H2 = numericalDerivative22( + boost::bind(&predictionError, _1, _2, factor), v1_upd, v2_upd); // Verify they are equal for this choice of state - CHECK( gtsam::assert_equal(numerical_H1, computed_H1, 1e-9)); - CHECK( gtsam::assert_equal(numerical_H2, computed_H2, 1e-9)); + CHECK( assert_equal(numerical_H1, computed_H1, 1e-9)); + CHECK( assert_equal(numerical_H2, computed_H2, 1e-9)); } /* ************************************************************************* */ diff --git a/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp b/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp index 7756e79d3..ad4efe26c 100644 --- a/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp +++ b/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp @@ -23,142 +23,132 @@ #include #include #include -#include #include using namespace std; using namespace gtsam; -gtsam::Rot3 world_R_ECEF( - 0.31686, 0.51505, 0.79645, - 0.85173, -0.52399, 0, - 0.41733, 0.67835, -0.60471); +Rot3 world_R_ECEF(0.31686, 0.51505, 0.79645, 0.85173, -0.52399, 0, 0.41733, + 0.67835, -0.60471); -gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); -gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); +static const Vector3 world_g(0.0, 0.0, 9.81); +static const Vector3 world_rho(0.0, -1.5724e-05, 0.0); // NED system +static const Vector3 ECEF_omega_earth(0.0, 0.0, 7.292115e-5); +static const Vector3 world_omega_earth = world_R_ECEF.matrix() + * ECEF_omega_earth; /* ************************************************************************* */ -gtsam::Pose3 predictionErrorPose(const Pose3& p1, const LieVector& v1, const imuBias::ConstantBias& b1, const Pose3& p2, const LieVector& v2, const InertialNavFactor_GlobalVelocity& factor) { +Pose3 predictionErrorPose(const Pose3& p1, const Vector3& v1, + const imuBias::ConstantBias& b1, const Pose3& p2, const Vector3& v2, + const InertialNavFactor_GlobalVelocity& factor) { return Pose3::Expmap(factor.evaluateError(p1, v1, b1, p2, v2).head(6)); } -gtsam::LieVector predictionErrorVel(const Pose3& p1, const LieVector& v1, const imuBias::ConstantBias& b1, const Pose3& p2, const LieVector& v2, const InertialNavFactor_GlobalVelocity& factor) { - return LieVector::Expmap(factor.evaluateError(p1, v1, b1, p2, v2).tail(3)); +Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, + const imuBias::ConstantBias& b1, const Pose3& p2, const Vector3& v2, + const InertialNavFactor_GlobalVelocity& factor) { + return factor.evaluateError(p1, v1, b1, p2, v2).tail(3); } -/* ************************************************************************* */ -TEST( InertialNavFactor_GlobalVelocity, Constructor) -{ - gtsam::Key Pose1(11); - gtsam::Key Pose2(12); - gtsam::Key Vel1(21); - gtsam::Key Vel2(22); - gtsam::Key Bias1(31); +/* ************************************************************************* */TEST( InertialNavFactor_GlobalVelocity, Constructor) { + Key Pose1(11); + Key Pose2(12); + Key Vel1(21); + Key Vel2(22); + Key Bias1(31); - Vector measurement_acc((Vector(3) << 0.1,0.2,0.4)); - Vector measurement_gyro((Vector(3) << -0.2, 0.5, 0.03)); + Vector3 measurement_acc(0.1, 0.2, 0.4); + Vector3 measurement_gyro(-0.2, 0.5, 0.03); double measurement_dt(0.1); - Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); - Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); - gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); - InertialNavFactor_GlobalVelocity f(Pose1, Vel1, Bias1, Pose2, Vel2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); + InertialNavFactor_GlobalVelocity f( + Pose1, Vel1, Bias1, Pose2, Vel2, measurement_acc, measurement_gyro, + measurement_dt, world_g, world_rho, world_omega_earth, model); } -/* ************************************************************************* */ -TEST( InertialNavFactor_GlobalVelocity, Equals) -{ - gtsam::Key Pose1(11); - gtsam::Key Pose2(12); - gtsam::Key Vel1(21); - gtsam::Key Vel2(22); - gtsam::Key Bias1(31); +/* ************************************************************************* */TEST( InertialNavFactor_GlobalVelocity, Equals) { + Key Pose1(11); + Key Pose2(12); + Key Vel1(21); + Key Vel2(22); + Key Bias1(31); - Vector measurement_acc((Vector(3) << 0.1,0.2,0.4)); - Vector measurement_gyro((Vector(3) << -0.2, 0.5, 0.03)); + Vector3 measurement_acc(0.1, 0.2, 0.4); + Vector3 measurement_gyro(-0.2, 0.5, 0.03); double measurement_dt(0.1); - Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); - Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); - gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); - InertialNavFactor_GlobalVelocity f(Pose1, Vel1, Bias1, Pose2, Vel2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); - InertialNavFactor_GlobalVelocity g(Pose1, Vel1, Bias1, Pose2, Vel2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); + InertialNavFactor_GlobalVelocity f( + Pose1, Vel1, Bias1, Pose2, Vel2, measurement_acc, measurement_gyro, + measurement_dt, world_g, world_rho, world_omega_earth, model); + InertialNavFactor_GlobalVelocity g( + Pose1, Vel1, Bias1, Pose2, Vel2, measurement_acc, measurement_gyro, + measurement_dt, world_g, world_rho, world_omega_earth, model); CHECK(assert_equal(f, g, 1e-5)); } -/* ************************************************************************* */ -TEST( InertialNavFactor_GlobalVelocity, Predict) -{ - gtsam::Key PoseKey1(11); - gtsam::Key PoseKey2(12); - gtsam::Key VelKey1(21); - gtsam::Key VelKey2(22); - gtsam::Key BiasKey1(31); +/* ************************************************************************* */TEST( InertialNavFactor_GlobalVelocity, Predict) { + Key PoseKey1(11); + Key PoseKey2(12); + Key VelKey1(21); + Key VelKey2(22); + Key BiasKey1(31); double measurement_dt(0.1); - Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); - Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); - gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); - // First test: zero angular motion, some acceleration - Vector measurement_acc((Vector(3) <<0.1,0.2,0.3-9.81)); + Vector measurement_acc((Vector(3) << 0.1, 0.2, 0.3 - 9.81)); Vector measurement_gyro((Vector(3) << 0.0, 0.0, 0.0)); - InertialNavFactor_GlobalVelocity f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); + InertialNavFactor_GlobalVelocity f( + PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, + measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, + model); Pose3 Pose1(Rot3(), Point3(2.00, 1.00, 3.00)); - LieVector Vel1((Vector(3) << 0.50, -0.50, 0.40)); + Vector3 Vel1((Vector(3) << 0.50, -0.50, 0.40)); imuBias::ConstantBias Bias1; Pose3 expectedPose2(Rot3(), Point3(2.05, 0.95, 3.04)); - LieVector expectedVel2((Vector(3) << 0.51, -0.48, 0.43)); + Vector3 expectedVel2((Vector(3) << 0.51, -0.48, 0.43)); Pose3 actualPose2; - LieVector actualVel2; + Vector3 actualVel2; f.predict(Pose1, Vel1, Bias1, actualPose2, actualVel2); CHECK(assert_equal(expectedPose2, actualPose2, 1e-5)); - CHECK(assert_equal(expectedVel2, actualVel2, 1e-5)); + CHECK(assert_equal((Vector)expectedVel2, actualVel2, 1e-5)); } -/* ************************************************************************* */ -TEST( InertialNavFactor_GlobalVelocity, ErrorPosVel) -{ - gtsam::Key PoseKey1(11); - gtsam::Key PoseKey2(12); - gtsam::Key VelKey1(21); - gtsam::Key VelKey2(22); - gtsam::Key BiasKey1(31); +/* ************************************************************************* */TEST( InertialNavFactor_GlobalVelocity, ErrorPosVel) { + Key PoseKey1(11); + Key PoseKey2(12); + Key VelKey1(21); + Key VelKey2(22); + Key BiasKey1(31); double measurement_dt(0.1); - Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); - Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); - gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); - // First test: zero angular motion, some acceleration - Vector measurement_acc((Vector(3) <<0.1,0.2,0.3-9.81)); + Vector measurement_acc((Vector(3) << 0.1, 0.2, 0.3 - 9.81)); Vector measurement_gyro((Vector(3) << 0.0, 0.0, 0.0)); - InertialNavFactor_GlobalVelocity f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); + InertialNavFactor_GlobalVelocity f( + PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, + measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, + model); Pose3 Pose1(Rot3(), Point3(2.00, 1.00, 3.00)); Pose3 Pose2(Rot3(), Point3(2.05, 0.95, 3.04)); - LieVector Vel1((Vector(3) << 0.50, -0.50, 0.40)); - LieVector Vel2((Vector(3) << 0.51, -0.48, 0.43)); + Vector3 Vel1((Vector(3) << 0.50, -0.50, 0.40)); + Vector3 Vel2((Vector(3) << 0.51, -0.48, 0.43)); imuBias::ConstantBias Bias1; Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); @@ -167,33 +157,31 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorPosVel) CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5)); } -/* ************************************************************************* */ -TEST( InertialNavFactor_GlobalVelocity, ErrorRot) -{ - gtsam::Key PoseKey1(11); - gtsam::Key PoseKey2(12); - gtsam::Key VelKey1(21); - gtsam::Key VelKey2(22); - gtsam::Key BiasKey1(31); +/* ************************************************************************* */TEST( InertialNavFactor_GlobalVelocity, ErrorRot) { + Key PoseKey1(11); + Key PoseKey2(12); + Key VelKey1(21); + Key VelKey2(22); + Key BiasKey1(31); double measurement_dt(0.1); - Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); - Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); - gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); // Second test: zero angular motion, some acceleration - Vector measurement_acc((Vector(3) <<0.0,0.0,0.0-9.81)); + Vector measurement_acc((Vector(3) << 0.0, 0.0, 0.0 - 9.81)); Vector measurement_gyro((Vector(3) << 0.1, 0.2, 0.3)); - InertialNavFactor_GlobalVelocity f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); + InertialNavFactor_GlobalVelocity f( + PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, + measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, + model); - Pose3 Pose1(Rot3(), Point3(2.0,1.0,3.0)); - Pose3 Pose2(Rot3::Expmap(measurement_gyro*measurement_dt), Point3(2.0,1.0,3.0)); - LieVector Vel1((Vector(3) << 0.0, 0.0, 0.0)); - LieVector Vel2((Vector(3) << 0.0, 0.0, 0.0)); + Pose3 Pose1(Rot3(), Point3(2.0, 1.0, 3.0)); + Pose3 Pose2(Rot3::Expmap(measurement_gyro * measurement_dt), + Point3(2.0, 1.0, 3.0)); + Vector3 Vel1((Vector(3) << 0.0, 0.0, 0.0)); + Vector3 Vel2((Vector(3) << 0.0, 0.0, 0.0)); imuBias::ConstantBias Bias1; Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); @@ -202,42 +190,38 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRot) CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5)); } -/* ************************************************************************* */ -TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVel) -{ - gtsam::Key PoseKey1(11); - gtsam::Key PoseKey2(12); - gtsam::Key VelKey1(21); - gtsam::Key VelKey2(22); - gtsam::Key BiasKey1(31); +/* ************************************************************************* */TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVel) { + Key PoseKey1(11); + Key PoseKey2(12); + Key VelKey1(21); + Key VelKey2(22); + Key BiasKey1(31); double measurement_dt(0.1); - Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); - Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); - gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); // Second test: zero angular motion, some acceleration - generated in matlab - Vector measurement_acc((Vector(3) << 6.501390843381716, -6.763926150509185, -2.300389940090343)); + Vector measurement_acc( + (Vector(3) << 6.501390843381716, -6.763926150509185, -2.300389940090343)); Vector measurement_gyro((Vector(3) << 0.1, 0.2, 0.3)); - InertialNavFactor_GlobalVelocity f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); + InertialNavFactor_GlobalVelocity f( + PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, + measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, + model); - Rot3 R1(0.487316618, 0.125253866, 0.86419557, - 0.580273724, 0.693095498, -0.427669306, - -0.652537293, 0.709880342, 0.265075427); - Point3 t1(2.0,1.0,3.0); + Rot3 R1(0.487316618, 0.125253866, 0.86419557, 0.580273724, 0.693095498, + -0.427669306, -0.652537293, 0.709880342, 0.265075427); + Point3 t1(2.0, 1.0, 3.0); Pose3 Pose1(R1, t1); - LieVector Vel1((Vector(3) << 0.5, -0.5, 0.4)); - Rot3 R2(0.473618898, 0.119523052, 0.872582019, - 0.609241153, 0.67099888, -0.422594037, - -0.636011287, 0.731761397, 0.244979388); - Point3 t2 = t1.compose( Point3(Vel1*measurement_dt) ); + Vector3 Vel1((Vector(3) << 0.5, -0.5, 0.4)); + Rot3 R2(0.473618898, 0.119523052, 0.872582019, 0.609241153, 0.67099888, + -0.422594037, -0.636011287, 0.731761397, 0.244979388); + Point3 t2 = t1.compose(Point3(Vel1 * measurement_dt)); Pose3 Pose2(R2, t2); Vector dv = measurement_dt * (R1.matrix() * measurement_acc + world_g); - LieVector Vel2 = Vel1.compose( dv ); + Vector3 Vel2 = Vel1 + dv; imuBias::ConstantBias Bias1; Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); @@ -247,249 +231,260 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVel) CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5)); } - ///* VADIM - START ************************************************************************* */ -//LieVector predictionRq(const LieVector angles, const LieVector q) { +//Vector3 predictionRq(const Vector3 angles, const Vector3 q) { // return (Rot3().RzRyRx(angles) * q).vector(); //} // //TEST (InertialNavFactor_GlobalVelocity, Rotation_Deriv ) { -// LieVector angles((Vector(3) << 3.001, -1.0004, 2.0005)); +// Vector3 angles((Vector(3) << 3.001, -1.0004, 2.0005)); // Rot3 R1(Rot3().RzRyRx(angles)); -// LieVector q((Vector(3) << 5.8, -2.2, 4.105)); +// Vector3 q((Vector(3) << 5.8, -2.2, 4.105)); // Rot3 qx(0.0, -q[2], q[1], // q[2], 0.0, -q[0], // -q[1], q[0],0.0); // Matrix J_hyp( -(R1*qx).matrix() ); // -// gtsam::Matrix J_expected; +// Matrix J_expected; // -// LieVector v(predictionRq(angles, q)); +// Vector3 v(predictionRq(angles, q)); // -// J_expected = gtsam::numericalDerivative11(boost::bind(&predictionRq, _1, q), angles); +// J_expected = numericalDerivative11(boost::bind(&predictionRq, _1, q), angles); // // cout<<"J_hyp"< factor(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); + InertialNavFactor_GlobalVelocity factor( + PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, + measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, + model); - Rot3 R1(0.487316618, 0.125253866, 0.86419557, - 0.580273724, 0.693095498, -0.427669306, - -0.652537293, 0.709880342, 0.265075427); - Point3 t1(2.0,1.0,3.0); + Rot3 R1(0.487316618, 0.125253866, 0.86419557, 0.580273724, 0.693095498, + -0.427669306, -0.652537293, 0.709880342, 0.265075427); + Point3 t1(2.0, 1.0, 3.0); Pose3 Pose1(R1, t1); - LieVector Vel1((Vector(3) << 0.5, -0.5, 0.4)); - Rot3 R2(0.473618898, 0.119523052, 0.872582019, - 0.609241153, 0.67099888, -0.422594037, - -0.636011287, 0.731761397, 0.244979388); - Point3 t2(2.052670960415706, 0.977252139079380, 2.942482135362800); + Vector3 Vel1((Vector(3) << 0.5, -0.5, 0.4)); + Rot3 R2(0.473618898, 0.119523052, 0.872582019, 0.609241153, 0.67099888, + -0.422594037, -0.636011287, 0.731761397, 0.244979388); + Point3 t2(2.052670960415706, 0.977252139079380, 2.942482135362800); Pose3 Pose2(R2, t2); - LieVector Vel2((Vector(3) << 0.510000000000000, -0.480000000000000, 0.430000000000000)); + Vector3 Vel2( + (Vector(3) << 0.510000000000000, -0.480000000000000, 0.430000000000000)); imuBias::ConstantBias Bias1; Matrix H1_actual, H2_actual, H3_actual, H4_actual, H5_actual; - Vector ActualErr(factor.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2, H1_actual, H2_actual, H3_actual, H4_actual, H5_actual)); + Vector ActualErr( + factor.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2, H1_actual, + H2_actual, H3_actual, H4_actual, H5_actual)); // Checking for Pose part in the jacobians // ****** - Matrix H1_actualPose(H1_actual.block(0,0,6,H1_actual.cols())); - Matrix H2_actualPose(H2_actual.block(0,0,6,H2_actual.cols())); - Matrix H3_actualPose(H3_actual.block(0,0,6,H3_actual.cols())); - Matrix H4_actualPose(H4_actual.block(0,0,6,H4_actual.cols())); - Matrix H5_actualPose(H5_actual.block(0,0,6,H5_actual.cols())); + Matrix H1_actualPose(H1_actual.block(0, 0, 6, H1_actual.cols())); + Matrix H2_actualPose(H2_actual.block(0, 0, 6, H2_actual.cols())); + Matrix H3_actualPose(H3_actual.block(0, 0, 6, H3_actual.cols())); + Matrix H4_actualPose(H4_actual.block(0, 0, 6, H4_actual.cols())); + Matrix H5_actualPose(H5_actual.block(0, 0, 6, H5_actual.cols())); // Calculate the Jacobian matrices H1 until H5 using the numerical derivative function - gtsam::Matrix H1_expectedPose, H2_expectedPose, H3_expectedPose, H4_expectedPose, H5_expectedPose; - H1_expectedPose = gtsam::numericalDerivative11(boost::bind(&predictionErrorPose, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1); - H2_expectedPose = gtsam::numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1); - H3_expectedPose = gtsam::numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1); - H4_expectedPose = gtsam::numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2); - H5_expectedPose = gtsam::numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, Pose2, _1, factor), Vel2); + Matrix H1_expectedPose, H2_expectedPose, H3_expectedPose, H4_expectedPose, + H5_expectedPose; + H1_expectedPose = numericalDerivative11( + boost::bind(&predictionErrorPose, _1, Vel1, Bias1, Pose2, Vel2, factor), + Pose1); + H2_expectedPose = numericalDerivative11( + boost::bind(&predictionErrorPose, Pose1, _1, Bias1, Pose2, Vel2, factor), + Vel1); + H3_expectedPose = numericalDerivative11( + boost::bind(&predictionErrorPose, Pose1, Vel1, _1, Pose2, Vel2, factor), + Bias1); + H4_expectedPose = numericalDerivative11( + boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, _1, Vel2, factor), + Pose2); + H5_expectedPose = numericalDerivative11( + boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, Pose2, _1, factor), + Vel2); // Verify they are equal for this choice of state - CHECK( gtsam::assert_equal(H1_expectedPose, H1_actualPose, 1e-5)); - CHECK( gtsam::assert_equal(H2_expectedPose, H2_actualPose, 1e-5)); - CHECK( gtsam::assert_equal(H3_expectedPose, H3_actualPose, 2e-3)); - CHECK( gtsam::assert_equal(H4_expectedPose, H4_actualPose, 1e-5)); - CHECK( gtsam::assert_equal(H5_expectedPose, H5_actualPose, 1e-5)); + CHECK( assert_equal(H1_expectedPose, H1_actualPose, 1e-5)); + CHECK( assert_equal(H2_expectedPose, H2_actualPose, 1e-5)); + CHECK( assert_equal(H3_expectedPose, H3_actualPose, 2e-3)); + CHECK( assert_equal(H4_expectedPose, H4_actualPose, 1e-5)); + CHECK( assert_equal(H5_expectedPose, H5_actualPose, 1e-5)); // Checking for Vel part in the jacobians // ****** - Matrix H1_actualVel(H1_actual.block(6,0,3,H1_actual.cols())); - Matrix H2_actualVel(H2_actual.block(6,0,3,H2_actual.cols())); - Matrix H3_actualVel(H3_actual.block(6,0,3,H3_actual.cols())); - Matrix H4_actualVel(H4_actual.block(6,0,3,H4_actual.cols())); - Matrix H5_actualVel(H5_actual.block(6,0,3,H5_actual.cols())); + Matrix H1_actualVel(H1_actual.block(6, 0, 3, H1_actual.cols())); + Matrix H2_actualVel(H2_actual.block(6, 0, 3, H2_actual.cols())); + Matrix H3_actualVel(H3_actual.block(6, 0, 3, H3_actual.cols())); + Matrix H4_actualVel(H4_actual.block(6, 0, 3, H4_actual.cols())); + Matrix H5_actualVel(H5_actual.block(6, 0, 3, H5_actual.cols())); // Calculate the Jacobian matrices H1 until H5 using the numerical derivative function - gtsam::Matrix H1_expectedVel, H2_expectedVel, H3_expectedVel, H4_expectedVel, H5_expectedVel; - H1_expectedVel = gtsam::numericalDerivative11(boost::bind(&predictionErrorVel, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1); - H2_expectedVel = gtsam::numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1); - H3_expectedVel = gtsam::numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1); - H4_expectedVel = gtsam::numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2); - H5_expectedVel = gtsam::numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, Pose2, _1, factor), Vel2); + Matrix H1_expectedVel, H2_expectedVel, H3_expectedVel, H4_expectedVel, + H5_expectedVel; + H1_expectedVel = numericalDerivative11( + boost::bind(&predictionErrorVel, _1, Vel1, Bias1, Pose2, Vel2, factor), + Pose1); + H2_expectedVel = numericalDerivative11( + boost::bind(&predictionErrorVel, Pose1, _1, Bias1, Pose2, Vel2, factor), + Vel1); + H3_expectedVel = numericalDerivative11( + boost::bind(&predictionErrorVel, Pose1, Vel1, _1, Pose2, Vel2, factor), + Bias1); + H4_expectedVel = numericalDerivative11( + boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, _1, Vel2, factor), + Pose2); + H5_expectedVel = numericalDerivative11( + boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, Pose2, _1, factor), + Vel2); // Verify they are equal for this choice of state - CHECK( gtsam::assert_equal(H1_expectedVel, H1_actualVel, 1e-5)); - CHECK( gtsam::assert_equal(H2_expectedVel, H2_actualVel, 1e-5)); - CHECK( gtsam::assert_equal(H3_expectedVel, H3_actualVel, 1e-5)); - CHECK( gtsam::assert_equal(H4_expectedVel, H4_actualVel, 1e-5)); - CHECK( gtsam::assert_equal(H5_expectedVel, H5_actualVel, 1e-5)); + CHECK( assert_equal(H1_expectedVel, H1_actualVel, 1e-5)); + CHECK( assert_equal(H2_expectedVel, H2_actualVel, 1e-5)); + CHECK( assert_equal(H3_expectedVel, H3_actualVel, 1e-5)); + CHECK( assert_equal(H4_expectedVel, H4_actualVel, 1e-5)); + CHECK( assert_equal(H5_expectedVel, H5_actualVel, 1e-5)); } - - - -/* ************************************************************************* */ -TEST( InertialNavFactor_GlobalVelocity, ConstructorWithTransform) -{ - gtsam::Key Pose1(11); - gtsam::Key Pose2(12); - gtsam::Key Vel1(21); - gtsam::Key Vel2(22); - gtsam::Key Bias1(31); +/* ************************************************************************* */TEST( InertialNavFactor_GlobalVelocity, ConstructorWithTransform) { + Key Pose1(11); + Key Pose2(12); + Key Vel1(21); + Key Vel2(22); + Key Bias1(31); Vector measurement_acc((Vector(3) << 0.1, 0.2, 0.4)); Vector measurement_gyro((Vector(3) << -0.2, 0.5, 0.03)); double measurement_dt(0.1); - Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); - Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); - gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); - Pose3 body_P_sensor(Rot3(0, 1, 0, 1, 0, 0, 0, 0, -1), Point3(0.0, 0.0, 0.0)); // IMU is in ENU orientation + Pose3 body_P_sensor(Rot3(0, 1, 0, 1, 0, 0, 0, 0, -1), Point3(0.0, 0.0, 0.0)); // IMU is in ENU orientation - - InertialNavFactor_GlobalVelocity f(Pose1, Vel1, Bias1, Pose2, Vel2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model, body_P_sensor); + InertialNavFactor_GlobalVelocity f( + Pose1, Vel1, Bias1, Pose2, Vel2, measurement_acc, measurement_gyro, + measurement_dt, world_g, world_rho, world_omega_earth, model, + body_P_sensor); } -/* ************************************************************************* */ -TEST( InertialNavFactor_GlobalVelocity, EqualsWithTransform) -{ - gtsam::Key Pose1(11); - gtsam::Key Pose2(12); - gtsam::Key Vel1(21); - gtsam::Key Vel2(22); - gtsam::Key Bias1(31); +/* ************************************************************************* */TEST( InertialNavFactor_GlobalVelocity, EqualsWithTransform) { + Key Pose1(11); + Key Pose2(12); + Key Vel1(21); + Key Vel2(22); + Key Bias1(31); Vector measurement_acc((Vector(3) << 0.1, 0.2, 0.4)); Vector measurement_gyro((Vector(3) << -0.2, 0.5, 0.03)); double measurement_dt(0.1); - Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); - Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); - gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); - Pose3 body_P_sensor(Rot3(0, 1, 0, 1, 0, 0, 0, 0, -1), Point3(0.0, 0.0, 0.0)); // IMU is in ENU orientation + Pose3 body_P_sensor(Rot3(0, 1, 0, 1, 0, 0, 0, 0, -1), Point3(0.0, 0.0, 0.0)); // IMU is in ENU orientation - - InertialNavFactor_GlobalVelocity f(Pose1, Vel1, Bias1, Pose2, Vel2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model, body_P_sensor); - InertialNavFactor_GlobalVelocity g(Pose1, Vel1, Bias1, Pose2, Vel2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model, body_P_sensor); + InertialNavFactor_GlobalVelocity f( + Pose1, Vel1, Bias1, Pose2, Vel2, measurement_acc, measurement_gyro, + measurement_dt, world_g, world_rho, world_omega_earth, model, + body_P_sensor); + InertialNavFactor_GlobalVelocity g( + Pose1, Vel1, Bias1, Pose2, Vel2, measurement_acc, measurement_gyro, + measurement_dt, world_g, world_rho, world_omega_earth, model, + body_P_sensor); CHECK(assert_equal(f, g, 1e-5)); } -/* ************************************************************************* */ -TEST( InertialNavFactor_GlobalVelocity, PredictWithTransform) -{ - gtsam::Key PoseKey1(11); - gtsam::Key PoseKey2(12); - gtsam::Key VelKey1(21); - gtsam::Key VelKey2(22); - gtsam::Key BiasKey1(31); +/* ************************************************************************* */TEST( InertialNavFactor_GlobalVelocity, PredictWithTransform) { + Key PoseKey1(11); + Key PoseKey2(12); + Key VelKey1(21); + Key VelKey2(22); + Key BiasKey1(31); double measurement_dt(0.1); - Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); - Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); - gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); - Pose3 body_P_sensor(Rot3(0, 1, 0, 1, 0, 0, 0, 0, -1), Point3(1.0, -2.0, 3.0)); // IMU is in ENU orientation - + Pose3 body_P_sensor(Rot3(0, 1, 0, 1, 0, 0, 0, 0, -1), Point3(1.0, -2.0, 3.0)); // IMU is in ENU orientation // First test: zero angular motion, some acceleration - Vector measurement_gyro((Vector(3) << 0.0, 0.0, 0.0)); // Measured in ENU orientation + Vector measurement_gyro((Vector(3) << 0.0, 0.0, 0.0)); // Measured in ENU orientation Matrix omega__cross = skewSymmetric(measurement_gyro); - Vector measurement_acc = (Vector(3) << 0.2, 0.1, -0.3+9.81) + omega__cross*omega__cross*body_P_sensor.rotation().inverse().matrix()*body_P_sensor.translation().vector(); // Measured in ENU orientation + Vector measurement_acc = (Vector(3) << 0.2, 0.1, -0.3 + 9.81) + + omega__cross * omega__cross + * body_P_sensor.rotation().inverse().matrix() + * body_P_sensor.translation().vector(); // Measured in ENU orientation - InertialNavFactor_GlobalVelocity f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model, body_P_sensor); + InertialNavFactor_GlobalVelocity f( + PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, + measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, + model, body_P_sensor); Pose3 Pose1(Rot3(), Point3(2.00, 1.00, 3.00)); - LieVector Vel1((Vector(3) << 0.50, -0.50, 0.40)); + Vector3 Vel1((Vector(3) << 0.50, -0.50, 0.40)); imuBias::ConstantBias Bias1; Pose3 expectedPose2(Rot3(), Point3(2.05, 0.95, 3.04)); - LieVector expectedVel2((Vector(3) << 0.51, -0.48, 0.43)); + Vector3 expectedVel2((Vector(3) << 0.51, -0.48, 0.43)); Pose3 actualPose2; - LieVector actualVel2; + Vector3 actualVel2; f.predict(Pose1, Vel1, Bias1, actualPose2, actualVel2); CHECK(assert_equal(expectedPose2, actualPose2, 1e-5)); - CHECK(assert_equal(expectedVel2, actualVel2, 1e-5)); + CHECK(assert_equal((Vector)expectedVel2, actualVel2, 1e-5)); } -/* ************************************************************************* */ -TEST( InertialNavFactor_GlobalVelocity, ErrorPosVelWithTransform) -{ - gtsam::Key PoseKey1(11); - gtsam::Key PoseKey2(12); - gtsam::Key VelKey1(21); - gtsam::Key VelKey2(22); - gtsam::Key BiasKey1(31); +/* ************************************************************************* */TEST( InertialNavFactor_GlobalVelocity, ErrorPosVelWithTransform) { + Key PoseKey1(11); + Key PoseKey2(12); + Key VelKey1(21); + Key VelKey2(22); + Key BiasKey1(31); double measurement_dt(0.1); - Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); - Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); - gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); - Pose3 body_P_sensor(Rot3(0, 1, 0, 1, 0, 0, 0, 0, -1), Point3(1.0, -2.0, 3.0)); // IMU is in ENU orientation - + Pose3 body_P_sensor(Rot3(0, 1, 0, 1, 0, 0, 0, 0, -1), Point3(1.0, -2.0, 3.0)); // IMU is in ENU orientation // First test: zero angular motion, some acceleration - Vector measurement_gyro((Vector(3) << 0.0, 0.0, 0.0)); // Measured in ENU orientation + Vector measurement_gyro((Vector(3) << 0.0, 0.0, 0.0)); // Measured in ENU orientation Matrix omega__cross = skewSymmetric(measurement_gyro); - Vector measurement_acc = (Vector(3) << 0.2, 0.1, -0.3+9.81) + omega__cross*omega__cross*body_P_sensor.rotation().inverse().matrix()*body_P_sensor.translation().vector(); // Measured in ENU orientation + Vector measurement_acc = (Vector(3) << 0.2, 0.1, -0.3 + 9.81) + + omega__cross * omega__cross + * body_P_sensor.rotation().inverse().matrix() + * body_P_sensor.translation().vector(); // Measured in ENU orientation - InertialNavFactor_GlobalVelocity f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model, body_P_sensor); + InertialNavFactor_GlobalVelocity f( + PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, + measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, + model, body_P_sensor); Pose3 Pose1(Rot3(), Point3(2.00, 1.00, 3.00)); Pose3 Pose2(Rot3(), Point3(2.05, 0.95, 3.04)); - LieVector Vel1((Vector(3) << 0.50, -0.50, 0.40)); - LieVector Vel2((Vector(3) << 0.51, -0.48, 0.43)); + Vector3 Vel1((Vector(3) << 0.50, -0.50, 0.40)); + Vector3 Vel2((Vector(3) << 0.51, -0.48, 0.43)); imuBias::ConstantBias Bias1; Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); @@ -498,37 +493,39 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorPosVelWithTransform) CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5)); } -/* ************************************************************************* */ -TEST( InertialNavFactor_GlobalVelocity, ErrorRotWithTransform) -{ - gtsam::Key PoseKey1(11); - gtsam::Key PoseKey2(12); - gtsam::Key VelKey1(21); - gtsam::Key VelKey2(22); - gtsam::Key BiasKey1(31); +/* ************************************************************************* */TEST( InertialNavFactor_GlobalVelocity, ErrorRotWithTransform) { + Key PoseKey1(11); + Key PoseKey2(12); + Key VelKey1(21); + Key VelKey2(22); + Key BiasKey1(31); double measurement_dt(0.1); - Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); - Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); - gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); - Pose3 body_P_sensor(Rot3(0, 1, 0, 1, 0, 0, 0, 0, -1), Point3(1.0, -2.0, 3.0)); // IMU is in ENU orientation - + Pose3 body_P_sensor(Rot3(0, 1, 0, 1, 0, 0, 0, 0, -1), Point3(1.0, -2.0, 3.0)); // IMU is in ENU orientation // Second test: zero angular motion, some acceleration - Vector measurement_gyro((Vector(3) << 0.2, 0.1, -0.3)); // Measured in ENU orientation + Vector measurement_gyro((Vector(3) << 0.2, 0.1, -0.3)); // Measured in ENU orientation Matrix omega__cross = skewSymmetric(measurement_gyro); - Vector measurement_acc = (Vector(3) << 0.0, 0.0, 0.0+9.81) + omega__cross*omega__cross*body_P_sensor.rotation().inverse().matrix()*body_P_sensor.translation().vector(); // Measured in ENU orientation + Vector measurement_acc = (Vector(3) << 0.0, 0.0, 0.0 + 9.81) + + omega__cross * omega__cross + * body_P_sensor.rotation().inverse().matrix() + * body_P_sensor.translation().vector(); // Measured in ENU orientation - InertialNavFactor_GlobalVelocity f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model, body_P_sensor); + InertialNavFactor_GlobalVelocity f( + PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, + measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, + model, body_P_sensor); - Pose3 Pose1(Rot3(), Point3(2.0,1.0,3.0)); - Pose3 Pose2(Rot3::Expmap(body_P_sensor.rotation().matrix()*measurement_gyro*measurement_dt), Point3(2.0, 1.0, 3.0)); - LieVector Vel1((Vector(3) << 0.0,0.0,0.0)); - LieVector Vel2((Vector(3) << 0.0,0.0,0.0)); + Pose3 Pose1(Rot3(), Point3(2.0, 1.0, 3.0)); + Pose3 Pose2( + Rot3::Expmap( + body_P_sensor.rotation().matrix() * measurement_gyro + * measurement_dt), Point3(2.0, 1.0, 3.0)); + Vector3 Vel1((Vector(3) << 0.0, 0.0, 0.0)); + Vector3 Vel2((Vector(3) << 0.0, 0.0, 0.0)); imuBias::ConstantBias Bias1; Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); @@ -537,46 +534,48 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRotWithTransform) CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5)); } -/* ************************************************************************* */ -TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVelWithTransform) -{ - gtsam::Key PoseKey1(11); - gtsam::Key PoseKey2(12); - gtsam::Key VelKey1(21); - gtsam::Key VelKey2(22); - gtsam::Key BiasKey1(31); +/* ************************************************************************* */TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVelWithTransform) { + Key PoseKey1(11); + Key PoseKey2(12); + Key VelKey1(21); + Key VelKey2(22); + Key BiasKey1(31); double measurement_dt(0.1); - Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); - Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); - gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); - Pose3 body_P_sensor(Rot3(0, 1, 0, 1, 0, 0, 0, 0, -1), Point3(1.0, -2.0, 3.0)); // IMU is in ENU orientation - + Pose3 body_P_sensor(Rot3(0, 1, 0, 1, 0, 0, 0, 0, -1), Point3(1.0, -2.0, 3.0)); // IMU is in ENU orientation // Second test: zero angular motion, some acceleration - generated in matlab Vector measurement_gyro((Vector(3) << 0.2, 0.1, -0.3)); // Measured in ENU orientation Matrix omega__cross = skewSymmetric(measurement_gyro); - Vector measurement_acc = (Vector(3) << -6.763926150509185, 6.501390843381716, +2.300389940090343) + omega__cross*omega__cross*body_P_sensor.rotation().inverse().matrix()*body_P_sensor.translation().vector(); // Measured in ENU orientation + Vector measurement_acc = + (Vector(3) << -6.763926150509185, 6.501390843381716, +2.300389940090343) + + omega__cross * omega__cross + * body_P_sensor.rotation().inverse().matrix() + * body_P_sensor.translation().vector(); // Measured in ENU orientation - InertialNavFactor_GlobalVelocity f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model, body_P_sensor); + InertialNavFactor_GlobalVelocity f( + PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, + measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, + model, body_P_sensor); - Rot3 R1(0.487316618, 0.125253866, 0.86419557, - 0.580273724, 0.693095498, -0.427669306, - -0.652537293, 0.709880342, 0.265075427); - Point3 t1(2.0,1.0,3.0); + Rot3 R1(0.487316618, 0.125253866, 0.86419557, 0.580273724, 0.693095498, + -0.427669306, -0.652537293, 0.709880342, 0.265075427); + Point3 t1(2.0, 1.0, 3.0); Pose3 Pose1(R1, t1); - LieVector Vel1((Vector(3) << 0.5,-0.5,0.4)); - Rot3 R2(0.473618898, 0.119523052, 0.872582019, - 0.609241153, 0.67099888, -0.422594037, - -0.636011287, 0.731761397, 0.244979388); - Point3 t2 = t1.compose( Point3(Vel1*measurement_dt) ); + Vector3 Vel1((Vector(3) << 0.5, -0.5, 0.4)); + Rot3 R2(0.473618898, 0.119523052, 0.872582019, 0.609241153, 0.67099888, + -0.422594037, -0.636011287, 0.731761397, 0.244979388); + Point3 t2 = t1.compose(Point3(Vel1 * measurement_dt)); Pose3 Pose2(R2, t2); - Vector dv = measurement_dt * (R1.matrix() * body_P_sensor.rotation().matrix() * (Vector(3) << -6.763926150509185, 6.501390843381716, +2.300389940090343) + world_g); - LieVector Vel2 = Vel1.compose( dv ); + Vector dv = + measurement_dt + * (R1.matrix() * body_P_sensor.rotation().matrix() + * (Vector(3) << -6.763926150509185, 6.501390843381716, +2.300389940090343) + + world_g); + Vector3 Vel2 = Vel1 + dv; imuBias::ConstantBias Bias1; Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); @@ -586,98 +585,123 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVelWithTransform) CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5)); } -/* ************************************************************************* */ -TEST (InertialNavFactor_GlobalVelocity, JacobianWithTransform ) { +/* ************************************************************************* */TEST (InertialNavFactor_GlobalVelocity, JacobianWithTransform ) { - gtsam::Key PoseKey1(11); - gtsam::Key PoseKey2(12); - gtsam::Key VelKey1(21); - gtsam::Key VelKey2(22); - gtsam::Key BiasKey1(31); + Key PoseKey1(11); + Key PoseKey2(12); + Key VelKey1(21); + Key VelKey2(22); + Key BiasKey1(31); double measurement_dt(0.01); - Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); - Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); - gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); - Pose3 body_P_sensor(Rot3(0, 1, 0, 1, 0, 0, 0, 0, -1), Point3(1.0, -2.0, 3.0)); // IMU is in ENU orientation + Pose3 body_P_sensor(Rot3(0, 1, 0, 1, 0, 0, 0, 0, -1), Point3(1.0, -2.0, 3.0)); // IMU is in ENU orientation - - Vector measurement_gyro((Vector(3) << 3.14/2, 3.14, +3.14)); // Measured in ENU orientation + Vector measurement_gyro((Vector(3) << 3.14 / 2, 3.14, +3.14)); // Measured in ENU orientation Matrix omega__cross = skewSymmetric(measurement_gyro); - Vector measurement_acc = (Vector(3) << -6.763926150509185, 6.501390843381716, +2.300389940090343) + omega__cross*omega__cross*body_P_sensor.rotation().inverse().matrix()*body_P_sensor.translation().vector(); // Measured in ENU orientation + Vector measurement_acc = + (Vector(3) << -6.763926150509185, 6.501390843381716, +2.300389940090343) + + omega__cross * omega__cross + * body_P_sensor.rotation().inverse().matrix() + * body_P_sensor.translation().vector(); // Measured in ENU orientation + InertialNavFactor_GlobalVelocity factor( + PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, + measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, + model, body_P_sensor); - InertialNavFactor_GlobalVelocity factor(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model, body_P_sensor); - - Rot3 R1(0.487316618, 0.125253866, 0.86419557, - 0.580273724, 0.693095498, -0.427669306, - -0.652537293, 0.709880342, 0.265075427); - Point3 t1(2.0,1.0,3.0); + Rot3 R1(0.487316618, 0.125253866, 0.86419557, 0.580273724, 0.693095498, + -0.427669306, -0.652537293, 0.709880342, 0.265075427); + Point3 t1(2.0, 1.0, 3.0); Pose3 Pose1(R1, t1); - LieVector Vel1((Vector(3) << 0.5,-0.5,0.4)); - Rot3 R2(0.473618898, 0.119523052, 0.872582019, - 0.609241153, 0.67099888, -0.422594037, - -0.636011287, 0.731761397, 0.244979388); - Point3 t2(2.052670960415706, 0.977252139079380, 2.942482135362800); + Vector3 Vel1(0.5, -0.5, 0.4); + Rot3 R2(0.473618898, 0.119523052, 0.872582019, 0.609241153, 0.67099888, + -0.422594037, -0.636011287, 0.731761397, 0.244979388); + Point3 t2(2.052670960415706, 0.977252139079380, 2.942482135362800); Pose3 Pose2(R2, t2); - LieVector Vel2((Vector(3) << 0.510000000000000, -0.480000000000000, 0.430000000000000)); + Vector3 Vel2(0.510000000000000, -0.480000000000000, 0.430000000000000); imuBias::ConstantBias Bias1; Matrix H1_actual, H2_actual, H3_actual, H4_actual, H5_actual; - Vector ActualErr(factor.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2, H1_actual, H2_actual, H3_actual, H4_actual, H5_actual)); + Vector ActualErr( + factor.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2, H1_actual, + H2_actual, H3_actual, H4_actual, H5_actual)); // Checking for Pose part in the jacobians // ****** - Matrix H1_actualPose(H1_actual.block(0,0,6,H1_actual.cols())); - Matrix H2_actualPose(H2_actual.block(0,0,6,H2_actual.cols())); - Matrix H3_actualPose(H3_actual.block(0,0,6,H3_actual.cols())); - Matrix H4_actualPose(H4_actual.block(0,0,6,H4_actual.cols())); - Matrix H5_actualPose(H5_actual.block(0,0,6,H5_actual.cols())); + Matrix H1_actualPose(H1_actual.block(0, 0, 6, H1_actual.cols())); + Matrix H2_actualPose(H2_actual.block(0, 0, 6, H2_actual.cols())); + Matrix H3_actualPose(H3_actual.block(0, 0, 6, H3_actual.cols())); + Matrix H4_actualPose(H4_actual.block(0, 0, 6, H4_actual.cols())); + Matrix H5_actualPose(H5_actual.block(0, 0, 6, H5_actual.cols())); // Calculate the Jacobian matrices H1 until H5 using the numerical derivative function - gtsam::Matrix H1_expectedPose, H2_expectedPose, H3_expectedPose, H4_expectedPose, H5_expectedPose; - H1_expectedPose = gtsam::numericalDerivative11(boost::bind(&predictionErrorPose, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1); - H2_expectedPose = gtsam::numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1); - H3_expectedPose = gtsam::numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1); - H4_expectedPose = gtsam::numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2); - H5_expectedPose = gtsam::numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, Pose2, _1, factor), Vel2); + Matrix H1_expectedPose, H2_expectedPose, H3_expectedPose, H4_expectedPose, + H5_expectedPose; + H1_expectedPose = numericalDerivative11( + boost::bind(&predictionErrorPose, _1, Vel1, Bias1, Pose2, Vel2, factor), + Pose1); + H2_expectedPose = numericalDerivative11( + boost::bind(&predictionErrorPose, Pose1, _1, Bias1, Pose2, Vel2, factor), + Vel1); + H3_expectedPose = numericalDerivative11( + boost::bind(&predictionErrorPose, Pose1, Vel1, _1, Pose2, Vel2, factor), + Bias1); + H4_expectedPose = numericalDerivative11( + boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, _1, Vel2, factor), + Pose2); + H5_expectedPose = numericalDerivative11( + boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, Pose2, _1, factor), + Vel2); // Verify they are equal for this choice of state - CHECK( gtsam::assert_equal(H1_expectedPose, H1_actualPose, 1e-5)); - CHECK( gtsam::assert_equal(H2_expectedPose, H2_actualPose, 1e-5)); - CHECK( gtsam::assert_equal(H3_expectedPose, H3_actualPose, 2e-3)); - CHECK( gtsam::assert_equal(H4_expectedPose, H4_actualPose, 1e-5)); - CHECK( gtsam::assert_equal(H5_expectedPose, H5_actualPose, 1e-5)); + CHECK( assert_equal(H1_expectedPose, H1_actualPose, 1e-5)); + CHECK( assert_equal(H2_expectedPose, H2_actualPose, 1e-5)); + CHECK( assert_equal(H3_expectedPose, H3_actualPose, 2e-3)); + CHECK( assert_equal(H4_expectedPose, H4_actualPose, 1e-5)); + CHECK( assert_equal(H5_expectedPose, H5_actualPose, 1e-5)); // Checking for Vel part in the jacobians // ****** - Matrix H1_actualVel(H1_actual.block(6,0,3,H1_actual.cols())); - Matrix H2_actualVel(H2_actual.block(6,0,3,H2_actual.cols())); - Matrix H3_actualVel(H3_actual.block(6,0,3,H3_actual.cols())); - Matrix H4_actualVel(H4_actual.block(6,0,3,H4_actual.cols())); - Matrix H5_actualVel(H5_actual.block(6,0,3,H5_actual.cols())); + Matrix H1_actualVel(H1_actual.block(6, 0, 3, H1_actual.cols())); + Matrix H2_actualVel(H2_actual.block(6, 0, 3, H2_actual.cols())); + Matrix H3_actualVel(H3_actual.block(6, 0, 3, H3_actual.cols())); + Matrix H4_actualVel(H4_actual.block(6, 0, 3, H4_actual.cols())); + Matrix H5_actualVel(H5_actual.block(6, 0, 3, H5_actual.cols())); // Calculate the Jacobian matrices H1 until H5 using the numerical derivative function - gtsam::Matrix H1_expectedVel, H2_expectedVel, H3_expectedVel, H4_expectedVel, H5_expectedVel; - H1_expectedVel = gtsam::numericalDerivative11(boost::bind(&predictionErrorVel, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1); - H2_expectedVel = gtsam::numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1); - H3_expectedVel = gtsam::numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1); - H4_expectedVel = gtsam::numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2); - H5_expectedVel = gtsam::numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, Pose2, _1, factor), Vel2); + Matrix H1_expectedVel, H2_expectedVel, H3_expectedVel, H4_expectedVel, + H5_expectedVel; + H1_expectedVel = numericalDerivative11( + boost::bind(&predictionErrorVel, _1, Vel1, Bias1, Pose2, Vel2, factor), + Pose1); + H2_expectedVel = numericalDerivative11( + boost::bind(&predictionErrorVel, Pose1, _1, Bias1, Pose2, Vel2, factor), + Vel1); + H3_expectedVel = numericalDerivative11( + boost::bind(&predictionErrorVel, Pose1, Vel1, _1, Pose2, Vel2, factor), + Bias1); + H4_expectedVel = numericalDerivative11( + boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, _1, Vel2, factor), + Pose2); + H5_expectedVel = numericalDerivative11( + boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, Pose2, _1, factor), + Vel2); // Verify they are equal for this choice of state - CHECK( gtsam::assert_equal(H1_expectedVel, H1_actualVel, 1e-5)); - CHECK( gtsam::assert_equal(H2_expectedVel, H2_actualVel, 1e-5)); - CHECK( gtsam::assert_equal(H3_expectedVel, H3_actualVel, 1e-5)); - CHECK( gtsam::assert_equal(H4_expectedVel, H4_actualVel, 1e-5)); - CHECK( gtsam::assert_equal(H5_expectedVel, H5_actualVel, 1e-5)); + CHECK( assert_equal(H1_expectedVel, H1_actualVel, 1e-5)); + CHECK( assert_equal(H2_expectedVel, H2_actualVel, 1e-5)); + CHECK( assert_equal(H3_expectedVel, H3_actualVel, 1e-5)); + CHECK( assert_equal(H4_expectedVel, H4_actualVel, 1e-5)); + CHECK( assert_equal(H5_expectedVel, H5_actualVel, 1e-5)); } /* ************************************************************************* */ - int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ diff --git a/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp b/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp index 5ea9fe29d..bf15c7322 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp +++ b/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp @@ -25,7 +25,7 @@ static SharedNoiseModel sigma(noiseModel::Unit::Create(2)); Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); SimpleCamera level_camera(level_pose, *K); -typedef InvDepthFactor3 InverseDepthFactor; +typedef InvDepthFactor3 InverseDepthFactor; typedef NonlinearEquality PoseConstraint; /* ************************************************************************* */ @@ -38,10 +38,10 @@ TEST( InvDepthFactor, optimize) { Point2 expected_uv = level_camera.project(landmark); InvDepthCamera3 inv_camera(level_pose, K); - LieVector inv_landmark((Vector(5) << 0., 0., 1., 0., 0.)); + Vector5 inv_landmark((Vector(5) << 0., 0., 1., 0., 0.)); // initialize inverse depth with "incorrect" depth of 1/4 // in reality this is 1/5, but initial depth is guessed - LieScalar inv_depth(1./4); + double inv_depth(1./4); gtsam::NonlinearFactorGraph graph; Values initial; @@ -82,8 +82,8 @@ TEST( InvDepthFactor, optimize) { Values result2 = LevenbergMarquardtOptimizer(graph, initial, lmParams).optimize(); Point3 result2_lmk = InvDepthCamera3::invDepthTo3D( - result2.at(Symbol('l',1)), - result2.at(Symbol('d',1))); + result2.at(Symbol('l',1)), + result2.at(Symbol('d',1))); EXPECT(assert_equal(landmark, result2_lmk, 1e-9)); // TODO: need to add priors to make this work with diff --git a/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp b/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp index 24535854d..bb3b81481 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp +++ b/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp @@ -24,8 +24,8 @@ using namespace gtsam; TEST( InvDepthFactorVariant1, optimize) { // Create two poses looking in the x-direction - Pose3 pose1(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1.0)); - Pose3 pose2(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1.5)); + Pose3 pose1(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1.0)); + Pose3 pose2(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1.5)); // Create a landmark 5 meters in front of pose1 (camera center at (0,0,1)) Point3 landmark(5, 0, 1); @@ -45,7 +45,7 @@ TEST( InvDepthFactorVariant1, optimize) { double theta = atan2(ray.y(), ray.x()); double phi = atan2(ray.z(), sqrt(ray.x()*ray.x()+ray.y()*ray.y())); double rho = 1./ray.norm(); - LieVector expected((Vector(6) << x, y, z, theta, phi, rho)); + Vector6 expected((Vector(6) << x, y, z, theta, phi, rho)); @@ -68,47 +68,47 @@ TEST( InvDepthFactorVariant1, optimize) { Values values; values.insert(poseKey1, pose1.retract((Vector(6) << +0.01, -0.02, +0.03, -0.10, +0.20, -0.30))); values.insert(poseKey2, pose2.retract((Vector(6) << +0.01, +0.02, -0.03, -0.10, +0.20, +0.30))); - values.insert(landmarkKey, expected.retract((Vector(6) << -0.20, +0.20, -0.35, +0.02, -0.04, +0.05))); + values.insert(landmarkKey, Vector6(expected + (Vector(6) << -0.20, +0.20, -0.35, +0.02, -0.04, +0.05))); // Optimize the graph to recover the actual landmark position LevenbergMarquardtParams params; Values result = LevenbergMarquardtOptimizer(graph, values, params).optimize(); - LieVector actual = result.at(landmarkKey); + Vector6 actual = result.at(landmarkKey); - values.at(poseKey1).print("Pose1 Before:\n"); - result.at(poseKey1).print("Pose1 After:\n"); - pose1.print("Pose1 Truth:\n"); - std::cout << std::endl << std::endl; - values.at(poseKey2).print("Pose2 Before:\n"); - result.at(poseKey2).print("Pose2 After:\n"); - pose2.print("Pose2 Truth:\n"); - std::cout << std::endl << std::endl; - values.at(landmarkKey).print("Landmark Before:\n"); - result.at(landmarkKey).print("Landmark After:\n"); - expected.print("Landmark Truth:\n"); - std::cout << std::endl << std::endl; +// values.at(poseKey1).print("Pose1 Before:\n"); +// result.at(poseKey1).print("Pose1 After:\n"); +// pose1.print("Pose1 Truth:\n"); +// cout << endl << endl; +// values.at(poseKey2).print("Pose2 Before:\n"); +// result.at(poseKey2).print("Pose2 After:\n"); +// pose2.print("Pose2 Truth:\n"); +// cout << endl << endl; +// values.at(landmarkKey).print("Landmark Before:\n"); +// result.at(landmarkKey).print("Landmark After:\n"); +// expected.print("Landmark Truth:\n"); +// cout << endl << endl; // Calculate world coordinates of landmark versions Point3 world_landmarkBefore; { - LieVector landmarkBefore = values.at(landmarkKey); + Vector6 landmarkBefore = values.at(landmarkKey); double x = landmarkBefore(0), y = landmarkBefore(1), z = landmarkBefore(2); double theta = landmarkBefore(3), phi = landmarkBefore(4), rho = landmarkBefore(5); world_landmarkBefore = Point3(x, y, z) + Point3(cos(theta)*cos(phi)/rho, sin(theta)*cos(phi)/rho, sin(phi)/rho); } Point3 world_landmarkAfter; { - LieVector landmarkAfter = result.at(landmarkKey); + Vector6 landmarkAfter = result.at(landmarkKey); double x = landmarkAfter(0), y = landmarkAfter(1), z = landmarkAfter(2); double theta = landmarkAfter(3), phi = landmarkAfter(4), rho = landmarkAfter(5); world_landmarkAfter = Point3(x, y, z) + Point3(cos(theta)*cos(phi)/rho, sin(theta)*cos(phi)/rho, sin(phi)/rho); } - world_landmarkBefore.print("World Landmark Before:\n"); - world_landmarkAfter.print("World Landmark After:\n"); - landmark.print("World Landmark Truth:\n"); - std::cout << std::endl << std::endl; +// world_landmarkBefore.print("World Landmark Before:\n"); +// world_landmarkAfter.print("World Landmark After:\n"); +// landmark.print("World Landmark Truth:\n"); +// cout << endl << endl; diff --git a/gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp b/gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp index e99c9bcdf..23f642a13 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp +++ b/gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp @@ -43,7 +43,7 @@ TEST( InvDepthFactorVariant2, optimize) { double theta = atan2(ray.y(), ray.x()); double phi = atan2(ray.z(), sqrt(ray.x()*ray.x()+ray.y()*ray.y())); double rho = 1./ray.norm(); - LieVector expected((Vector(3) << theta, phi, rho)); + Vector3 expected((Vector(3) << theta, phi, rho)); @@ -66,50 +66,47 @@ TEST( InvDepthFactorVariant2, optimize) { Values values; values.insert(poseKey1, pose1.retract((Vector(6) << +0.01, -0.02, +0.03, -0.10, +0.20, -0.30))); values.insert(poseKey2, pose2.retract((Vector(6) << +0.01, +0.02, -0.03, -0.10, +0.20, +0.30))); - values.insert(landmarkKey, expected.retract((Vector(3) << +0.02, -0.04, +0.05))); + values.insert(landmarkKey, Vector3(expected + (Vector(3) << +0.02, -0.04, +0.05))); // Optimize the graph to recover the actual landmark position LevenbergMarquardtParams params; Values result = LevenbergMarquardtOptimizer(graph, values, params).optimize(); - LieVector actual = result.at(landmarkKey); + Vector3 actual = result.at(landmarkKey); - - values.at(poseKey1).print("Pose1 Before:\n"); - result.at(poseKey1).print("Pose1 After:\n"); - pose1.print("Pose1 Truth:\n"); - std::cout << std::endl << std::endl; - values.at(poseKey2).print("Pose2 Before:\n"); - result.at(poseKey2).print("Pose2 After:\n"); - pose2.print("Pose2 Truth:\n"); - std::cout << std::endl << std::endl; - values.at(landmarkKey).print("Landmark Before:\n"); - result.at(landmarkKey).print("Landmark After:\n"); - expected.print("Landmark Truth:\n"); - std::cout << std::endl << std::endl; +// values.at(poseKey1).print("Pose1 Before:\n"); +// result.at(poseKey1).print("Pose1 After:\n"); +// pose1.print("Pose1 Truth:\n"); +// std::cout << std::endl << std::endl; +// values.at(poseKey2).print("Pose2 Before:\n"); +// result.at(poseKey2).print("Pose2 After:\n"); +// pose2.print("Pose2 Truth:\n"); +// std::cout << std::endl << std::endl; +// values.at(landmarkKey).print("Landmark Before:\n"); +// result.at(landmarkKey).print("Landmark After:\n"); +// expected.print("Landmark Truth:\n"); +// std::cout << std::endl << std::endl; // Calculate world coordinates of landmark versions Point3 world_landmarkBefore; { - LieVector landmarkBefore = values.at(landmarkKey); + Vector3 landmarkBefore = values.at(landmarkKey); double theta = landmarkBefore(0), phi = landmarkBefore(1), rho = landmarkBefore(2); world_landmarkBefore = referencePoint + Point3(cos(theta)*cos(phi)/rho, sin(theta)*cos(phi)/rho, sin(phi)/rho); } Point3 world_landmarkAfter; { - LieVector landmarkAfter = result.at(landmarkKey); + Vector3 landmarkAfter = result.at(landmarkKey); double theta = landmarkAfter(0), phi = landmarkAfter(1), rho = landmarkAfter(2); world_landmarkAfter = referencePoint + Point3(cos(theta)*cos(phi)/rho, sin(theta)*cos(phi)/rho, sin(phi)/rho); } - world_landmarkBefore.print("World Landmark Before:\n"); - world_landmarkAfter.print("World Landmark After:\n"); - landmark.print("World Landmark Truth:\n"); - std::cout << std::endl << std::endl; - - +// world_landmarkBefore.print("World Landmark Before:\n"); +// world_landmarkAfter.print("World Landmark After:\n"); +// landmark.print("World Landmark Truth:\n"); +// std::cout << std::endl << std::endl; // Test that the correct landmark parameters have been recovered - EXPECT(assert_equal(expected, actual, 1e-9)); + EXPECT(assert_equal((Vector)expected, actual, 1e-9)); } diff --git a/gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp b/gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp index e65b7cacb..e0be9c79c 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp +++ b/gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp @@ -39,11 +39,11 @@ TEST( InvDepthFactorVariant3, optimize) { // Create expected landmark Point3 landmark_p1 = pose1.transform_to(landmark); - landmark_p1.print("Landmark in Pose1 Frame:\n"); + // landmark_p1.print("Landmark in Pose1 Frame:\n"); double theta = atan2(landmark_p1.x(), landmark_p1.z()); double phi = atan2(landmark_p1.y(), sqrt(landmark_p1.x()*landmark_p1.x()+landmark_p1.z()*landmark_p1.z())); double rho = 1./landmark_p1.norm(); - LieVector expected((Vector(3) << theta, phi, rho)); + Vector3 expected((Vector(3) << theta, phi, rho)); @@ -66,17 +66,17 @@ TEST( InvDepthFactorVariant3, optimize) { Values values; values.insert(poseKey1, pose1.retract((Vector(6) << +0.01, -0.02, +0.03, -0.10, +0.20, -0.30))); values.insert(poseKey2, pose2.retract((Vector(6) << +0.01, +0.02, -0.03, -0.10, +0.20, +0.30))); - values.insert(landmarkKey, expected.retract((Vector(3) << +0.02, -0.04, +0.05))); + values.insert(landmarkKey, Vector3(expected + (Vector(3) << +0.02, -0.04, +0.05))); // Optimize the graph to recover the actual landmark position LevenbergMarquardtParams params; Values result = LevenbergMarquardtOptimizer(graph, values, params).optimize(); - LieVector actual = result.at(landmarkKey); + Vector3 actual = result.at(landmarkKey); // Test that the correct landmark parameters have been recovered - EXPECT(assert_equal(expected, actual, 1e-9)); + EXPECT(assert_equal((Vector)expected, actual, 1e-9)); } diff --git a/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp b/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp index 900356213..238bece62 100644 --- a/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp +++ b/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp @@ -189,8 +189,8 @@ TEST( PoseBetweenFactor, Jacobian ) { Point3(-3.37493895, 6.14660244, -8.93650986)); // Calculate numerical derivatives - Matrix expectedH1 = numericalDerivative11(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, _1, pose2, boost::none, boost::none), pose1); - Matrix expectedH2 = numericalDerivative11(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, pose1, _1, boost::none, boost::none), pose2); + Matrix expectedH1 = numericalDerivative11(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, _1, pose2, boost::none, boost::none), pose1); + Matrix expectedH2 = numericalDerivative11(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, pose1, _1, boost::none, boost::none), pose2); // Use the factor to calculate the derivative Matrix actualH1; @@ -218,8 +218,8 @@ TEST( PoseBetweenFactor, JacobianWithTransform ) { Point3(-3.5257579, 6.02637531, -8.98382384)); // Calculate numerical derivatives - Matrix expectedH1 = numericalDerivative11(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, _1, pose2, boost::none, boost::none), pose1); - Matrix expectedH2 = numericalDerivative11(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, pose1, _1, boost::none, boost::none), pose2); + Matrix expectedH1 = numericalDerivative11(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, _1, pose2, boost::none, boost::none), pose1); + Matrix expectedH2 = numericalDerivative11(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, pose1, _1, boost::none, boost::none), pose2); // Use the factor to calculate the derivative Matrix actualH1; diff --git a/gtsam_unstable/slam/tests/testPosePriorFactor.cpp b/gtsam_unstable/slam/tests/testPosePriorFactor.cpp index 16b2bccc1..9aa87fb83 100644 --- a/gtsam_unstable/slam/tests/testPosePriorFactor.cpp +++ b/gtsam_unstable/slam/tests/testPosePriorFactor.cpp @@ -178,7 +178,7 @@ TEST( PosePriorFactor, Jacobian ) { Pose3 pose(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0)); // Calculate numerical derivatives - Matrix expectedH1 = numericalDerivative11(boost::bind(&TestPosePriorFactor::evaluateError, &factor, _1, boost::none), pose); + Matrix expectedH1 = numericalDerivative11(boost::bind(&TestPosePriorFactor::evaluateError, &factor, _1, boost::none), pose); // Use the factor to calculate the derivative Matrix actualH1; @@ -202,7 +202,7 @@ TEST( PosePriorFactor, JacobianWithTransform ) { Point3(-4.74767676, 7.67044942, -11.00985)); // Calculate numerical derivatives - Matrix expectedH1 = numericalDerivative11(boost::bind(&TestPosePriorFactor::evaluateError, &factor, _1, boost::none), pose); + Matrix expectedH1 = numericalDerivative11(boost::bind(&TestPosePriorFactor::evaluateError, &factor, _1, boost::none), pose); // Use the factor to calculate the derivative Matrix actualH1; diff --git a/gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp b/gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp index 7c370b534..b732b8088 100644 --- a/gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp +++ b/gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp @@ -170,7 +170,7 @@ TEST( ProjectionFactorPPP, Jacobian ) { CHECK(assert_equal(H3Expected, H3Actual, 1e-3)); // Verify H2 with numerical derivative - Matrix H2Expected = numericalDerivative32( + Matrix H2Expected = numericalDerivative32( boost::function( boost::bind(&TestProjectionFactor::evaluateError, &factor, _1, _2, _3, boost::none, boost::none, boost::none)), pose, Pose3(), point); @@ -205,7 +205,7 @@ TEST( ProjectionFactorPPP, JacobianWithTransform ) { CHECK(assert_equal(H3Expected, H3Actual, 1e-3)); // Verify H2 with numerical derivative - Matrix H2Expected = numericalDerivative32( + Matrix H2Expected = numericalDerivative32( boost::function( boost::bind(&TestProjectionFactor::evaluateError, &factor, _1, _2, _3, boost::none, boost::none, boost::none)), pose, body_P_sensor, point); diff --git a/gtsam_unstable/slam/tests/testProjectionFactorPPPC.cpp b/gtsam_unstable/slam/tests/testProjectionFactorPPPC.cpp index 36f7fdf52..cda588d90 100644 --- a/gtsam_unstable/slam/tests/testProjectionFactorPPPC.cpp +++ b/gtsam_unstable/slam/tests/testProjectionFactorPPPC.cpp @@ -136,11 +136,11 @@ TEST( ProjectionFactorPPPC, Jacobian ) { CHECK(assert_equal(H3Expected, H3Actual, 1e-3)); // Verify H2 and H4 with numerical derivatives - Matrix H2Expected = numericalDerivative11( + Matrix H2Expected = numericalDerivative11( boost::bind(&TestProjectionFactor::evaluateError, &factor, pose, _1, point, *K1, boost::none, boost::none, boost::none, boost::none), Pose3()); - Matrix H4Expected = numericalDerivative11( + Matrix H4Expected = numericalDerivative11( boost::bind(&TestProjectionFactor::evaluateError, &factor, pose, Pose3(), point, _1, boost::none, boost::none, boost::none, boost::none), *K1); @@ -172,11 +172,11 @@ TEST( ProjectionFactorPPPC, JacobianWithTransform ) { CHECK(assert_equal(H3Expected, H3Actual, 1e-3)); // Verify H2 and H4 with numerical derivatives - Matrix H2Expected = numericalDerivative11( + Matrix H2Expected = numericalDerivative11( boost::bind(&TestProjectionFactor::evaluateError, &factor, pose, _1, point, *K1, boost::none, boost::none, boost::none, boost::none), body_P_sensor); - Matrix H4Expected = numericalDerivative11( + Matrix H4Expected = numericalDerivative11( boost::bind(&TestProjectionFactor::evaluateError, &factor, pose, body_P_sensor, point, _1, boost::none, boost::none, boost::none, boost::none), *K1); diff --git a/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp b/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp index ffc7344cf..0d5cb2e36 100644 --- a/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp +++ b/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp @@ -24,8 +24,8 @@ const Point3 point1(3.0, 4.0, 2.0); const gtsam::Key poseKey = 1, pointKey = 2; /* ************************************************************************* */ -LieVector evalFactorError(const RelativeElevationFactor& factor, const Pose3& x, const Point3& p) { - return LieVector(factor.evaluateError(x, p)); +Vector evalFactorError(const RelativeElevationFactor& factor, const Pose3& x, const Point3& p) { + return factor.evaluateError(x, p); } /* ************************************************************************* */ @@ -35,9 +35,9 @@ TEST( testRelativeElevationFactor, level_zero_error ) { RelativeElevationFactor factor(poseKey, pointKey, measured, model1); Matrix actH1, actH2; EXPECT(assert_equal(zero(1), factor.evaluateError(pose1, point1, actH1, actH2))); - Matrix expH1 = numericalDerivative21( + Matrix expH1 = numericalDerivative21( boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5); - Matrix expH2 = numericalDerivative22( + Matrix expH2 = numericalDerivative22( boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH2, actH2, tol)); @@ -50,9 +50,9 @@ TEST( testRelativeElevationFactor, level_positive ) { RelativeElevationFactor factor(poseKey, pointKey, measured, model1); Matrix actH1, actH2; EXPECT(assert_equal((Vector(1) << 2.0), factor.evaluateError(pose1, point1, actH1, actH2))); - Matrix expH1 = numericalDerivative21( + Matrix expH1 = numericalDerivative21( boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5); - Matrix expH2 = numericalDerivative22( + Matrix expH2 = numericalDerivative22( boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH2, actH2, tol)); @@ -65,9 +65,9 @@ TEST( testRelativeElevationFactor, level_negative ) { RelativeElevationFactor factor(poseKey, pointKey, measured, model1); Matrix actH1, actH2; EXPECT(assert_equal((Vector(1) << 3.0), factor.evaluateError(pose1, point1, actH1, actH2))); - Matrix expH1 = numericalDerivative21( + Matrix expH1 = numericalDerivative21( boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5); - Matrix expH2 = numericalDerivative22( + Matrix expH2 = numericalDerivative22( boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH2, actH2, tol)); @@ -80,9 +80,9 @@ TEST( testRelativeElevationFactor, rotated_zero_error ) { RelativeElevationFactor factor(poseKey, pointKey, measured, model1); Matrix actH1, actH2; EXPECT(assert_equal(zero(1), factor.evaluateError(pose2, point1, actH1, actH2))); - Matrix expH1 = numericalDerivative21( + Matrix expH1 = numericalDerivative21( boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5); - Matrix expH2 = numericalDerivative22( + Matrix expH2 = numericalDerivative22( boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH2, actH2, tol)); @@ -95,9 +95,9 @@ TEST( testRelativeElevationFactor, rotated_positive ) { RelativeElevationFactor factor(poseKey, pointKey, measured, model1); Matrix actH1, actH2; EXPECT(assert_equal((Vector(1) << 2.0), factor.evaluateError(pose2, point1, actH1, actH2))); - Matrix expH1 = numericalDerivative21( + Matrix expH1 = numericalDerivative21( boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5); - Matrix expH2 = numericalDerivative22( + Matrix expH2 = numericalDerivative22( boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH2, actH2, tol)); @@ -110,9 +110,9 @@ TEST( testRelativeElevationFactor, rotated_negative1 ) { RelativeElevationFactor factor(poseKey, pointKey, measured, model1); Matrix actH1, actH2; EXPECT(assert_equal((Vector(1) << 3.0), factor.evaluateError(pose2, point1, actH1, actH2))); - Matrix expH1 = numericalDerivative21( + Matrix expH1 = numericalDerivative21( boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5); - Matrix expH2 = numericalDerivative22( + Matrix expH2 = numericalDerivative22( boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH2, actH2, tol)); @@ -125,9 +125,9 @@ TEST( testRelativeElevationFactor, rotated_negative2 ) { RelativeElevationFactor factor(poseKey, pointKey, measured, model1); Matrix actH1, actH2; EXPECT(assert_equal((Vector(1) << 3.0), factor.evaluateError(pose3, point1, actH1, actH2))); - Matrix expH1 = numericalDerivative21( + Matrix expH1 = numericalDerivative21( boost::bind(evalFactorError, factor, _1, _2), pose3, point1, 1e-5); - Matrix expH2 = numericalDerivative22( + Matrix expH2 = numericalDerivative22( boost::bind(evalFactorError, factor, _1, _2), pose3, point1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH2, actH2, tol)); diff --git a/gtsam_unstable/slam/tests/testTSAMFactors.cpp b/gtsam_unstable/slam/tests/testTSAMFactors.cpp index 44dca9b8e..a8a3ae5e9 100644 --- a/gtsam_unstable/slam/tests/testTSAMFactors.cpp +++ b/gtsam_unstable/slam/tests/testTSAMFactors.cpp @@ -44,10 +44,10 @@ TEST( DeltaFactor, all ) { // Use numerical derivatives to calculate the Jacobians Matrix H1Expected, H2Expected; - H1Expected = numericalDerivative11( + H1Expected = numericalDerivative11( boost::bind(&DeltaFactor::evaluateError, &factor, _1, point, boost::none, boost::none), pose); - H2Expected = numericalDerivative11( + H2Expected = numericalDerivative11( boost::bind(&DeltaFactor::evaluateError, &factor, pose, _1, boost::none, boost::none), point); @@ -78,16 +78,16 @@ TEST( DeltaFactorBase, all ) { // Use numerical derivatives to calculate the Jacobians Matrix H1Expected, H2Expected, H3Expected, H4Expected; - H1Expected = numericalDerivative11( + H1Expected = numericalDerivative11( boost::bind(&DeltaFactorBase::evaluateError, &factor, _1, pose, base2, point, boost::none, boost::none, boost::none, boost::none), base1); - H2Expected = numericalDerivative11( + H2Expected = numericalDerivative11( boost::bind(&DeltaFactorBase::evaluateError, &factor, base1, _1, base2, point, boost::none, boost::none, boost::none, boost::none), pose); - H3Expected = numericalDerivative11( + H3Expected = numericalDerivative11( boost::bind(&DeltaFactorBase::evaluateError, &factor, base1, pose, _1, point, boost::none, boost::none, boost::none, boost::none), base2); - H4Expected = numericalDerivative11( + H4Expected = numericalDerivative11( boost::bind(&DeltaFactorBase::evaluateError, &factor, base1, pose, base2, _1, boost::none, boost::none, boost::none, boost::none), point); @@ -119,16 +119,16 @@ TEST( OdometryFactorBase, all ) { // Use numerical derivatives to calculate the Jacobians Matrix H1Expected, H2Expected, H3Expected, H4Expected; - H1Expected = numericalDerivative11( + H1Expected = numericalDerivative11( boost::bind(&OdometryFactorBase::evaluateError, &factor, _1, pose1, base2, pose2, boost::none, boost::none, boost::none, boost::none), base1); - H2Expected = numericalDerivative11( + H2Expected = numericalDerivative11( boost::bind(&OdometryFactorBase::evaluateError, &factor, base1, _1, base2, pose2, boost::none, boost::none, boost::none, boost::none), pose1); - H3Expected = numericalDerivative11( + H3Expected = numericalDerivative11( boost::bind(&OdometryFactorBase::evaluateError, &factor, base1, pose1, _1, pose2, boost::none, boost::none, boost::none, boost::none), base2); - H4Expected = numericalDerivative11( + H4Expected = numericalDerivative11( boost::bind(&OdometryFactorBase::evaluateError, &factor, base1, pose1, base2, _1, boost::none, boost::none, boost::none, boost::none), pose2); diff --git a/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp b/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp index a063244a3..7e2aa507e 100644 --- a/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp +++ b/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp @@ -10,7 +10,6 @@ #include #include #include -#include #include #include @@ -23,26 +22,21 @@ using namespace std; using namespace gtsam; -// Disabled this test because it is currently failing - remove the lines "#if 0" and "#endif" below -// to reenable the test. -//#if 0 /* ************************************************************************* */ -LieVector predictionError(const Pose2& org1_T_org2, const gtsam::Key& key, const TransformBtwRobotsUnaryFactor& factor){ +Vector predictionError(const Pose2& org1_T_org2, const gtsam::Key& key, const TransformBtwRobotsUnaryFactor& factor){ gtsam::Values values; values.insert(key, org1_T_org2); - // LieVector err = factor.whitenedError(values); - // return err; - return LieVector::Expmap(factor.whitenedError(values)); + return factor.whitenedError(values); } /* ************************************************************************* */ -//LieVector predictionError_standard(const Pose2& p1, const Pose2& p2, const gtsam::Key& keyA, const gtsam::Key& keyB, const BetweenFactor& factor){ +//Vector predictionError_standard(const Pose2& p1, const Pose2& p2, const gtsam::Key& keyA, const gtsam::Key& keyB, const BetweenFactor& factor){ // gtsam::Values values; // values.insert(keyA, p1); // values.insert(keyB, p2); -// // LieVector err = factor.whitenedError(values); +// // Vector err = factor.whitenedError(values); // // return err; -// return LieVector::Expmap(factor.whitenedError(values)); +// return Vector::Expmap(factor.whitenedError(values)); //} /* ************************************************************************* */ @@ -237,7 +231,7 @@ TEST( TransformBtwRobotsUnaryFactor, Jacobian) Matrix H1_actual = H_actual[0]; double stepsize = 1.0e-9; - Matrix H1_expected = gtsam::numericalDerivative11(boost::bind(&predictionError, _1, key, g), orgA_T_orgB, stepsize); + Matrix H1_expected = gtsam::numericalDerivative11(boost::bind(&predictionError, _1, key, g), orgA_T_orgB, stepsize); // CHECK( assert_equal(H1_expected, H1_actual, 1e-5)); } @@ -291,12 +285,12 @@ TEST( TransformBtwRobotsUnaryFactor, Jacobian) //// CHECK( assert_equal(H2_actual_stnd, H2_actual, 1e-8)); // // double stepsize = 1.0e-9; -// Matrix H1_expected = gtsam::numericalDerivative11(boost::bind(&predictionError, _1, p2, keyA, keyB, f), p1, stepsize); -// Matrix H2_expected = gtsam::numericalDerivative11(boost::bind(&predictionError, p1, _1, keyA, keyB, f), p2, stepsize); +// Matrix H1_expected = gtsam::numericalDerivative11(boost::bind(&predictionError, _1, p2, keyA, keyB, f), p1, stepsize); +// Matrix H2_expected = gtsam::numericalDerivative11(boost::bind(&predictionError, p1, _1, keyA, keyB, f), p2, stepsize); // // // // try to check numerical derivatives of a standard between factor -// Matrix H1_expected_stnd = gtsam::numericalDerivative11(boost::bind(&predictionError_standard, _1, p2, keyA, keyB, h), p1, stepsize); +// Matrix H1_expected_stnd = gtsam::numericalDerivative11(boost::bind(&predictionError_standard, _1, p2, keyA, keyB, h), p1, stepsize); // CHECK( assert_equal(H1_expected_stnd, H1_actual_stnd, 1e-5)); // // @@ -305,8 +299,6 @@ TEST( TransformBtwRobotsUnaryFactor, Jacobian) // //} -//#endif - /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */ diff --git a/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp b/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp index 1ffb2bebe..146cdc06f 100644 --- a/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp +++ b/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp @@ -10,7 +10,6 @@ #include #include #include -#include #include #include @@ -23,26 +22,21 @@ using namespace std; using namespace gtsam; -// Disabled this test because it is currently failing - remove the lines "#if 0" and "#endif" below -// to reenable the test. -//#if 0 /* ************************************************************************* */ -LieVector predictionError(const Pose2& org1_T_org2, const gtsam::Key& key, const TransformBtwRobotsUnaryFactorEM& factor){ +Vector predictionError(const Pose2& org1_T_org2, const gtsam::Key& key, const TransformBtwRobotsUnaryFactorEM& factor){ gtsam::Values values; values.insert(key, org1_T_org2); - // LieVector err = factor.whitenedError(values); - // return err; - return LieVector::Expmap(factor.whitenedError(values)); + return factor.whitenedError(values); } /* ************************************************************************* */ -//LieVector predictionError_standard(const Pose2& p1, const Pose2& p2, const gtsam::Key& keyA, const gtsam::Key& keyB, const BetweenFactor& factor){ +//Vector predictionError_standard(const Pose2& p1, const Pose2& p2, const gtsam::Key& keyA, const gtsam::Key& keyB, const BetweenFactor& factor){ // gtsam::Values values; // values.insert(keyA, p1); // values.insert(keyB, p2); -// // LieVector err = factor.whitenedError(values); +// // Vector err = factor.whitenedError(values); // // return err; -// return LieVector::Expmap(factor.whitenedError(values)); +// return Vector::Expmap(factor.whitenedError(values)); //} /* ************************************************************************* */ @@ -266,7 +260,7 @@ TEST( TransformBtwRobotsUnaryFactorEM, Jacobian) Matrix H1_actual = H_actual[0]; double stepsize = 1.0e-9; - Matrix H1_expected = gtsam::numericalDerivative11(boost::bind(&predictionError, _1, key, g), orgA_T_orgB, stepsize); + Matrix H1_expected = gtsam::numericalDerivative11(boost::bind(&predictionError, _1, key, g), orgA_T_orgB, stepsize); // CHECK( assert_equal(H1_expected, H1_actual, 1e-5)); } /////* ************************************************************************** */ @@ -316,12 +310,12 @@ TEST( TransformBtwRobotsUnaryFactorEM, Jacobian) //// CHECK( assert_equal(H2_actual_stnd, H2_actual, 1e-8)); // // double stepsize = 1.0e-9; -// Matrix H1_expected = gtsam::numericalDerivative11(boost::bind(&predictionError, _1, p2, keyA, keyB, f), p1, stepsize); -// Matrix H2_expected = gtsam::numericalDerivative11(boost::bind(&predictionError, p1, _1, keyA, keyB, f), p2, stepsize); +// Matrix H1_expected = gtsam::numericalDerivative11(boost::bind(&predictionError, _1, p2, keyA, keyB, f), p1, stepsize); +// Matrix H2_expected = gtsam::numericalDerivative11(boost::bind(&predictionError, p1, _1, keyA, keyB, f), p2, stepsize); // // // // try to check numerical derivatives of a standard between factor -// Matrix H1_expected_stnd = gtsam::numericalDerivative11(boost::bind(&predictionError_standard, _1, p2, keyA, keyB, h), p1, stepsize); +// Matrix H1_expected_stnd = gtsam::numericalDerivative11(boost::bind(&predictionError_standard, _1, p2, keyA, keyB, h), p1, stepsize); // CHECK( assert_equal(H1_expected_stnd, H1_actual_stnd, 1e-5)); // // @@ -330,8 +324,6 @@ TEST( TransformBtwRobotsUnaryFactorEM, Jacobian) // //} -//#endif - /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */ diff --git a/gtsam_unstable/timing/timeAdaptAutoDiff.cpp b/gtsam_unstable/timing/timeAdaptAutoDiff.cpp new file mode 100644 index 000000000..32bce9ba5 --- /dev/null +++ b/gtsam_unstable/timing/timeAdaptAutoDiff.cpp @@ -0,0 +1,72 @@ +/* ---------------------------------------------------------------------------- + + * 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 timeCameraExpression.cpp + * @brief time CalibratedCamera derivatives + * @author Frank Dellaert + * @date October 3, 2014 + */ + +#include "timeLinearize.h" +#include +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +#define time timeMultiThreaded + +int main() { + + // The DefaultChart of Camera below is laid out like Snavely's 9-dim vector + typedef PinholeCamera Camera; + typedef Expression Point2_; + typedef Expression Camera_; + typedef Expression Point3_; + + // Create leaves + Camera_ camera(1); + Point3_ point(2); + + // Some parameters needed + Point2 z(-17, 30); + SharedNoiseModel model = noiseModel::Unit::Create(2); + + // Create values + Values values; + values.insert(1, Camera()); + values.insert(2, Point3(0, 0, 1)); + + NonlinearFactor::shared_ptr f1, f2, f3; + + // Dedicated factor + f1 = boost::make_shared >(z, model, 1, 2); + time("GeneralSFMFactor : ", f1, values); + + // AdaptAutoDiff + typedef AdaptAutoDiff AdaptedSnavely; + Point2_ expression(AdaptedSnavely(), camera, point); + f2 = boost::make_shared >(model, z, expression); + time("Point2_(AdaptedSnavely(), camera, point): ", f2, values); + + // ExpressionFactor + Point2_ expression2(camera, &Camera::project2, point); + f3 = boost::make_shared >(model, z, expression2); + time("Point2_(camera, &Camera::project, point): ", f3, values); + + return 0; +} diff --git a/gtsam_unstable/timing/timeCameraExpression.cpp b/gtsam_unstable/timing/timeCameraExpression.cpp new file mode 100644 index 000000000..92522c440 --- /dev/null +++ b/gtsam_unstable/timing/timeCameraExpression.cpp @@ -0,0 +1,109 @@ +/* ---------------------------------------------------------------------------- + + * 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 timeCameraExpression.cpp + * @brief time CalibratedCamera derivatives + * @author Frank Dellaert + * @date October 3, 2014 + */ + +#include +#include +#include +#include +#include +#include +#include "timeLinearize.h" + +using namespace std; +using namespace gtsam; + +#define time timeSingleThreaded + +boost::shared_ptr fixedK(new Cal3_S2()); + +Point2 myProject(const Pose3& pose, const Point3& point, + boost::optional H1, boost::optional H2) { + PinholeCamera camera(pose, *fixedK); + return camera.project(point, H1, H2, boost::none); +} + +int main() { + + // Create leaves + Pose3_ x(1); + Point3_ p(2); + Cal3_S2_ K(3); + + // Some parameters needed + Point2 z(-17, 30); + SharedNoiseModel model = noiseModel::Unit::Create(2); + + // Create values + Values values; + values.insert(1, Pose3()); + values.insert(2, Point3(0, 0, 1)); + values.insert(3, Cal3_S2()); + + // UNCALIBRATED + + // Dedicated factor + // Oct 3, 2014, Macbook Air + // 4.2 musecs/call + NonlinearFactor::shared_ptr f1 = + boost::make_shared >(z, model, 1, 2, 3); + time("GeneralSFMFactor2 : ", f1, values); + + // ExpressionFactor + // Oct 3, 2014, Macbook Air + // 20.3 musecs/call + NonlinearFactor::shared_ptr f2 = + boost::make_shared >(model, z, + uncalibrate(K, project(transform_to(x, p)))); + time("Bin(Leaf,Un(Bin(Leaf,Leaf))): ", f2, values); + + // ExpressionFactor ternary + // Oct 3, 2014, Macbook Air + // 20.3 musecs/call + NonlinearFactor::shared_ptr f3 = + boost::make_shared >(model, z, + project3(x, p, K)); + time("Ternary(Leaf,Leaf,Leaf) : ", f3, values); + + // CALIBRATED + + // Dedicated factor + // Oct 3, 2014, Macbook Air + // 3.4 musecs/call + NonlinearFactor::shared_ptr g1 = boost::make_shared< + GenericProjectionFactor >(z, model, 1, 2, fixedK); + time("GenericProjectionFactor: ", g1, values); + + // ExpressionFactor + // Oct 3, 2014, Macbook Air + // 16.0 musecs/call + NonlinearFactor::shared_ptr g2 = + boost::make_shared >(model, z, + uncalibrate(Cal3_S2_(*fixedK), project(transform_to(x, p)))); + time("Bin(Cnst,Un(Bin(Leaf,Leaf))): ", g2, values); + + // ExpressionFactor, optimized + // Oct 3, 2014, Macbook Air + // 9.0 musecs/call + typedef PinholeCamera Camera; + typedef Expression Camera_; + NonlinearFactor::shared_ptr g3 = + boost::make_shared >(model, z, + Point2_(myProject, x, p)); + time("Binary(Leaf,Leaf) : ", g3, values); + return 0; +} diff --git a/gtsam_unstable/timing/timeDSFvariants.cpp b/gtsam_unstable/timing/timeDSFvariants.cpp index 134c318cb..f0bbb9072 100644 --- a/gtsam_unstable/timing/timeDSFvariants.cpp +++ b/gtsam_unstable/timing/timeDSFvariants.cpp @@ -57,7 +57,8 @@ int main(int argc, char* argv[]) { volatile double fpm = 0.5; // fraction of points matched volatile size_t nm = fpm * n * np; // number of matches - cout << format("\nTesting with %1% images, %2% points, %3% matches\n") % m % N % nm; + cout << format("\nTesting with %1% images, %2% points, %3% matches\n") + % (int)m % (int)N % (int)nm; cout << "Generating " << nm << " matches" << endl; boost::variate_generator > rn( boost::mt19937(), boost::uniform_int(0, N - 1)); @@ -67,7 +68,7 @@ int main(int argc, char* argv[]) { for (size_t k = 0; k < nm; k++) matches.push_back(Match(rn(), rn())); - os << format("%1%,%2%,%3%,") % m % N % nm; + os << format("%1%,%2%,%3%,") % (int)m % (int)N % (int)nm; { // DSFBase version diff --git a/gtsam_unstable/timing/timeInertialNavFactor_GlobalVelocity.cpp b/gtsam_unstable/timing/timeInertialNavFactor_GlobalVelocity.cpp index 0d514abcd..dabf283c6 100644 --- a/gtsam_unstable/timing/timeInertialNavFactor_GlobalVelocity.cpp +++ b/gtsam_unstable/timing/timeInertialNavFactor_GlobalVelocity.cpp @@ -21,7 +21,6 @@ #include #include #include -#include #include using namespace std; @@ -36,12 +35,12 @@ gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); /* ************************************************************************* */ -gtsam::Pose3 predictionErrorPose(const Pose3& p1, const LieVector& v1, const imuBias::ConstantBias& b1, const Pose3& p2, const LieVector& v2, const InertialNavFactor_GlobalVelocity& factor) { +gtsam::Pose3 predictionErrorPose(const Pose3& p1, const Vector3& v1, const imuBias::ConstantBias& b1, const Pose3& p2, const Vector3& v2, const InertialNavFactor_GlobalVelocity& factor) { return Pose3::Expmap(factor.evaluateError(p1, v1, b1, p2, v2).head(6)); } -gtsam::LieVector predictionErrorVel(const Pose3& p1, const LieVector& v1, const imuBias::ConstantBias& b1, const Pose3& p2, const LieVector& v2, const InertialNavFactor_GlobalVelocity& factor) { - return LieVector::Expmap(factor.evaluateError(p1, v1, b1, p2, v2).tail(3)); +gtsam::Vector3 predictionErrorVel(const Pose3& p1, const Vector3& v1, const imuBias::ConstantBias& b1, const Pose3& p2, const Vector3& v2, const InertialNavFactor_GlobalVelocity& factor) { + return factor.evaluateError(p1, v1, b1, p2, v2).tail(3); } #include @@ -65,21 +64,21 @@ int main() { Vector measurement_acc((Vector(3) << 6.501390843381716, -6.763926150509185, -2.300389940090343)); Vector measurement_gyro((Vector(3) << 0.1, 0.2, 0.3)); - InertialNavFactor_GlobalVelocity f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); + InertialNavFactor_GlobalVelocity f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); Rot3 R1(0.487316618, 0.125253866, 0.86419557, 0.580273724, 0.693095498, -0.427669306, -0.652537293, 0.709880342, 0.265075427); Point3 t1(2.0,1.0,3.0); Pose3 Pose1(R1, t1); - LieVector Vel1 = Vector((Vector(3) << 0.5,-0.5,0.4)); + Vector3 Vel1 = Vector((Vector(3) << 0.5,-0.5,0.4)); Rot3 R2(0.473618898, 0.119523052, 0.872582019, 0.609241153, 0.67099888, -0.422594037, -0.636011287, 0.731761397, 0.244979388); Point3 t2 = t1.compose( Point3(Vel1*measurement_dt) ); Pose3 Pose2(R2, t2); Vector dv = measurement_dt * (R1.matrix() * measurement_acc + world_g); - LieVector Vel2 = Vel1.compose( dv ); + Vector3 Vel2 = Vel1 + dv; imuBias::ConstantBias Bias1; Values values; diff --git a/gtsam_unstable/timing/timeLinearize.h b/gtsam_unstable/timing/timeLinearize.h new file mode 100644 index 000000000..dfb63ffa3 --- /dev/null +++ b/gtsam_unstable/timing/timeLinearize.h @@ -0,0 +1,56 @@ +/* ---------------------------------------------------------------------------- + + * 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 timeLinearize.h + * @brief time linearize + * @author Frank Dellaert + * @date October 10, 2014 + */ + +#pragma once + +#include +#include + +#include +#include +#include + +static const int n = 1000000; + +void timeSingleThreaded(const std::string& str, + const gtsam::NonlinearFactor::shared_ptr& f, const gtsam::Values& values) { + long timeLog = clock(); + gtsam::GaussianFactor::shared_ptr gf; + for (int i = 0; i < n; i++) + gf = f->linearize(values); + long timeLog2 = clock(); + double seconds = (double) (timeLog2 - timeLog) / CLOCKS_PER_SEC; + std::cout << std::setprecision(3); + std::cout << str << ((double) seconds * 1000000 / n) << " musecs/call" + << std::endl; +} + +void timeMultiThreaded(const std::string& str, + const gtsam::NonlinearFactor::shared_ptr& f, const gtsam::Values& values) { + gtsam::NonlinearFactorGraph graph; + for (int i = 0; i < n; i++) + graph.push_back(f); + long timeLog = clock(); + gtsam::GaussianFactorGraph::shared_ptr gfg = graph.linearize(values); + long timeLog2 = clock(); + double seconds = (double) (timeLog2 - timeLog) / CLOCKS_PER_SEC; + std::cout << std::setprecision(3); + std::cout << str << ((double) seconds * 1000000 / n) << " musecs/call" + << std::endl; +} + diff --git a/gtsam_unstable/timing/timeOneCameraExpression.cpp b/gtsam_unstable/timing/timeOneCameraExpression.cpp new file mode 100644 index 000000000..4cb655825 --- /dev/null +++ b/gtsam_unstable/timing/timeOneCameraExpression.cpp @@ -0,0 +1,58 @@ +/* ---------------------------------------------------------------------------- + + * 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 timeOneCameraExpression.cpp + * @brief time CalibratedCamera derivatives + * @author Frank Dellaert + * @date October 3, 2014 + */ + +#include +#include +#include "timeLinearize.h" + +using namespace std; +using namespace gtsam; + +#define time timeSingleThreaded + +int main() { + + // Create leaves + Pose3_ x(1); + Point3_ p(2); + Cal3_S2_ K(3); + + // Some parameters needed + Point2 z(-17, 30); + SharedNoiseModel model = noiseModel::Unit::Create(2); + + // Create values + Values values; + values.insert(1, Pose3()); + values.insert(2, Point3(0, 0, 1)); + values.insert(3, Cal3_S2()); + + // ExpressionFactor + // Oct 3, 2014, Macbook Air + // 20.3 musecs/call +//#define TERNARY + NonlinearFactor::shared_ptr f = boost::make_shared > +#ifdef TERNARY + (model, z, project3(x, p, K)); +#else + (model, z, uncalibrate(K, project(transform_to(x, p)))); +#endif + time("timing:", f, values); + + return 0; +} diff --git a/gtsam_unstable/timing/timeSFMExpressions.cpp b/gtsam_unstable/timing/timeSFMExpressions.cpp new file mode 100644 index 000000000..678e4db60 --- /dev/null +++ b/gtsam_unstable/timing/timeSFMExpressions.cpp @@ -0,0 +1,82 @@ +/* ---------------------------------------------------------------------------- + + * 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 timeSFMExpressions.cpp + * @brief time CalibratedCamera derivatives, realistic scenario + * @author Frank Dellaert + * @date October 3, 2014 + */ + +#include +#include +#include +#include + +#include +#include +#include // std::setprecision + +using namespace std; +using namespace gtsam; + +//#define TERNARY + +int main() { + + // number of cameras, and points + static const size_t M=100, N = 10000, n = M*N; + + // Create leaves + Cal3_S2_ K('K', 0); + std::vector > x = createUnknowns(M, 'x'); + std::vector > p = createUnknowns(N, 'p'); + + // Some parameters needed + Point2 z(-17, 30); + SharedNoiseModel model = noiseModel::Unit::Create(2); + + // Create values + Values values; + values.insert(Symbol('K', 0), Cal3_S2()); + for (size_t i = 0; i < M; i++) + values.insert(Symbol('x', i), Pose3()); + for (size_t j = 0; j < N; j++) + values.insert(Symbol('p', j), Point3(0, 0, 1)); + + long timeLog = clock(); + NonlinearFactorGraph graph; + for (size_t i = 0; i < M; i++) { + for (size_t j = 0; j < N; j++) { + NonlinearFactor::shared_ptr f = boost::make_shared< + ExpressionFactor > +#ifdef TERNARY + (model, z, project3(x[i], p[j], K)); +#else + (model, z, uncalibrate(K, project(transform_to(x[i], p[j])))); +#endif + graph.push_back(f); + } + } + long timeLog2 = clock(); + cout << setprecision(3); + double seconds = (double) (timeLog2 - timeLog) / CLOCKS_PER_SEC; + cout << seconds << " seconds to build" << endl; + + timeLog = clock(); + GaussianFactorGraph::shared_ptr gfg = graph.linearize(values); + timeLog2 = clock(); + seconds = (double) (timeLog2 - timeLog) / CLOCKS_PER_SEC; + cout << seconds << " seconds to linearize" << endl; + cout << ((double) seconds * 1000000 / n) << " musecs/call" << endl; + + return 0; +} diff --git a/matlab/+gtsam/VisualISAMPlot.m b/matlab/+gtsam/VisualISAMPlot.m index 874dbf523..9b52f016f 100644 --- a/matlab/+gtsam/VisualISAMPlot.m +++ b/matlab/+gtsam/VisualISAMPlot.m @@ -16,7 +16,7 @@ gtsam.plot3DPoints(result, [], marginals); M = 1; while result.exists(symbol('x',M)) ii = symbol('x',M); - pose_i = result.at(ii); + pose_i = result.atPose3(ii); if options.hardConstraint && (M==1) gtsam.plotPose3(pose_i,[],10); else diff --git a/matlab/+gtsam/VisualISAMStep.m b/matlab/+gtsam/VisualISAMStep.m index 82b3754ef..6fa81e072 100644 --- a/matlab/+gtsam/VisualISAMStep.m +++ b/matlab/+gtsam/VisualISAMStep.m @@ -27,7 +27,7 @@ for k=1:length(data.Z{nextPoseIndex}) end %% Initial estimates for the new pose. -prevPose = result.at(symbol('x',nextPoseIndex-1)); +prevPose = result.atPose3(symbol('x',nextPoseIndex-1)); initialEstimates.insert(symbol('x',nextPoseIndex), prevPose.compose(odometry)); %% Update ISAM diff --git a/matlab/+gtsam/load3D.m b/matlab/+gtsam/load3D.m index 94202e0c8..d536162e1 100644 --- a/matlab/+gtsam/load3D.m +++ b/matlab/+gtsam/load3D.m @@ -46,9 +46,9 @@ for i=1:n graph.add(BetweenFactorPose3(i1, i2, dpose, model)); if successive if i2>i1 - initial.insert(i2,initial.at(i1).compose(dpose)); + initial.insert(i2,initial.atPose3(i1).compose(dpose)); else - initial.insert(i1,initial.at(i2).compose(dpose.inverse)); + initial.insert(i1,initial.atPose3(i2).compose(dpose.inverse)); end end end diff --git a/matlab/+gtsam/plot2DPoints.m b/matlab/+gtsam/plot2DPoints.m index d4703a5d7..8e91fa19d 100644 --- a/matlab/+gtsam/plot2DPoints.m +++ b/matlab/+gtsam/plot2DPoints.m @@ -18,15 +18,18 @@ hold on % Plot points and covariance matrices for i = 0:keys.size-1 key = keys.at(i); - p = values.at(key); - if isa(p, 'gtsam.Point2') + try + p = values.atPoint2(key); if haveMarginals P = marginals.marginalCovariance(key); gtsam.plotPoint2(p, linespec, P); else gtsam.plotPoint2(p, linespec); end + catch err + % I guess it's not a Point2 end + end if ~holdstate diff --git a/matlab/+gtsam/plot2DTrajectory.m b/matlab/+gtsam/plot2DTrajectory.m index 45e7fe467..1c29213cd 100644 --- a/matlab/+gtsam/plot2DTrajectory.m +++ b/matlab/+gtsam/plot2DTrajectory.m @@ -8,7 +8,7 @@ function plot2DTrajectory(values, linespec, marginals) import gtsam.* if ~exist('linespec', 'var') || isempty(linespec) - linespec = 'k*-'; + linespec = 'k*-'; end haveMarginals = exist('marginals', 'var'); @@ -25,24 +25,26 @@ plot(X,Y,linespec); % Quiver can also be vectorized if no marginals asked if ~haveMarginals - C = cos(theta); - S = sin(theta); - quiver(X,Y,C,S,0.1,linespec); + C = cos(theta); + S = sin(theta); + quiver(X,Y,C,S,0.1,linespec); else - % plotPose2 does both quiver and covariance matrix - keys = KeyVector(values.keys); - for i = 0:keys.size-1 - key = keys.at(i); - x = values.at(key); - if isa(x, 'gtsam.Pose2') - P = marginals.marginalCovariance(key); - gtsam.plotPose2(x,linespec(1), P); + % plotPose2 does both quiver and covariance matrix + keys = KeyVector(values.keys); + for i = 0:keys.size-1 + key = keys.at(i); + try + x = values.atPose2(key); + P = marginals.marginalCovariance(key); + gtsam.plotPose2(x,linespec(1), P); + catch err + % I guess it's not a Pose2 + end end - end end if ~holdstate - hold off + hold off end end diff --git a/matlab/+gtsam/plot3DPoints.m b/matlab/+gtsam/plot3DPoints.m index 8feab1744..46e161807 100644 --- a/matlab/+gtsam/plot3DPoints.m +++ b/matlab/+gtsam/plot3DPoints.m @@ -18,14 +18,16 @@ hold on % Plot points and covariance matrices for i = 0:keys.size-1 key = keys.at(i); - p = values.at(key); - if isa(p, 'gtsam.Point3') + try + p = values.atPoint3(key); if haveMarginals P = marginals.marginalCovariance(key); gtsam.plotPoint3(p, linespec, P); else gtsam.plotPoint3(p, linespec); end + catch + % I guess it's not a Point3 end end diff --git a/matlab/+gtsam/plot3DTrajectory.m b/matlab/+gtsam/plot3DTrajectory.m index d24e21297..05483e589 100644 --- a/matlab/+gtsam/plot3DTrajectory.m +++ b/matlab/+gtsam/plot3DTrajectory.m @@ -17,39 +17,48 @@ hold on lastIndex = []; for i = 0:keys.size-1 key = keys.at(i); - x = values.at(key); - if isa(x, 'gtsam.Pose3') + try + x = values.atPose3(key); if ~isempty(lastIndex) % Draw line from last pose then covariance ellipse on top of % last pose. lastKey = keys.at(lastIndex); - lastPose = values.at(lastKey); - plot3([ x.x; lastPose.x ], [ x.y; lastPose.y ], [ x.z; lastPose.z ], linespec); + try + lastPose = values.atPose3(lastKey); + plot3([ x.x; lastPose.x ], [ x.y; lastPose.y ], [ x.z; lastPose.z ], linespec); + if haveMarginals + P = marginals.marginalCovariance(lastKey); + else + P = []; + end + gtsam.plotPose3(lastPose, P, scale); + catch err + warning(['no Pose3 at ' lastKey]); + end + lastIndex = i; + end + catch + warning(['no Pose3 at ' key]); + end + + % Draw final pose + if ~isempty(lastIndex) + lastKey = keys.at(lastIndex); + try + lastPose = values.atPose3(lastKey); if haveMarginals P = marginals.marginalCovariance(lastKey); else P = []; end gtsam.plotPose3(lastPose, P, scale); + catch + warning(['no Pose3 at ' lastIndex]); end - lastIndex = i; end -end - -% Draw final pose -if ~isempty(lastIndex) - lastKey = keys.at(lastIndex); - lastPose = values.at(lastKey); - if haveMarginals - P = marginals.marginalCovariance(lastKey); - else - P = []; + + if ~holdstate + hold off end - gtsam.plotPose3(lastPose, P, scale); -end - -if ~holdstate - hold off -end - + end \ No newline at end of file diff --git a/matlab/gtsam_examples/PlanarSLAMExample.m b/matlab/gtsam_examples/PlanarSLAMExample.m index ef1516017..aec933d74 100644 --- a/matlab/gtsam_examples/PlanarSLAMExample.m +++ b/matlab/gtsam_examples/PlanarSLAMExample.m @@ -71,9 +71,9 @@ marginals = Marginals(graph, result); plot2DTrajectory(result, [], marginals); plot2DPoints(result, 'b', marginals); -plot([result.at(i1).x; result.at(j1).x],[result.at(i1).y; result.at(j1).y], 'c-'); -plot([result.at(i2).x; result.at(j1).x],[result.at(i2).y; result.at(j1).y], 'c-'); -plot([result.at(i3).x; result.at(j2).x],[result.at(i3).y; result.at(j2).y], 'c-'); +plot([result.atPose2(i1).x; result.atPoint2(j1).x],[result.atPose2(i1).y; result.atPoint2(j1).y], 'c-'); +plot([result.atPose2(i2).x; result.atPoint2(j1).x],[result.atPose2(i2).y; result.atPoint2(j1).y], 'c-'); +plot([result.atPose2(i3).x; result.atPoint2(j2).x],[result.atPose2(i3).y; result.atPoint2(j2).y], 'c-'); axis([-0.6 4.8 -1 1]) axis equal view(2) diff --git a/matlab/gtsam_examples/PlanarSLAMExample_graph.m b/matlab/gtsam_examples/PlanarSLAMExample_graph.m index 9ca88e49a..db6484c5c 100644 --- a/matlab/gtsam_examples/PlanarSLAMExample_graph.m +++ b/matlab/gtsam_examples/PlanarSLAMExample_graph.m @@ -22,7 +22,7 @@ model = noiseModel.Diagonal.Sigmas([0.05; 0.05; 2*pi/180]); [graph,initial] = load2D(datafile, model); %% Add a Gaussian prior on a pose in the middle -priorMean = initial.at(40); +priorMean = initial.atPose2(40); priorNoise = noiseModel.Diagonal.Sigmas([0.1; 0.1; 2*pi/180]); graph.add(PriorFactorPose2(40, priorMean, priorNoise)); % add directly to graph diff --git a/matlab/gtsam_examples/PlanarSLAMExample_sampling.m b/matlab/gtsam_examples/PlanarSLAMExample_sampling.m index 327c64d4d..375ed645c 100644 --- a/matlab/gtsam_examples/PlanarSLAMExample_sampling.m +++ b/matlab/gtsam_examples/PlanarSLAMExample_sampling.m @@ -55,14 +55,14 @@ plot2DPoints(sample, [], marginals); for j=1:2 key = symbol('l',j); - point{j} = sample.at(key); + point{j} = sample.atPoint2(key); Q{j}=marginals.marginalCovariance(key); S{j}=chol(Q{j}); % for sampling end -plot([sample.at(i1).x; sample.at(j1).x],[sample.at(i1).y; sample.at(j1).y], 'c-'); -plot([sample.at(i2).x; sample.at(j1).x],[sample.at(i2).y; sample.at(j1).y], 'c-'); -plot([sample.at(i3).x; sample.at(j2).x],[sample.at(i3).y; sample.at(j2).y], 'c-'); +plot([sample.atPose2(i1).x; sample.atPoint2(j1).x],[sample.atPose2(i1).y; sample.atPoint2(j1).y], 'c-'); +plot([sample.atPose2(i2).x; sample.atPoint2(j1).x],[sample.atPose2(i2).y; sample.atPoint2(j1).y], 'c-'); +plot([sample.atPose2(i3).x; sample.atPoint2(j2).x],[sample.atPose2(i3).y; sample.atPoint2(j2).y], 'c-'); view(2); axis auto; axis equal %% Do Sampling on point 2 diff --git a/matlab/gtsam_examples/Pose2SLAMExample.m b/matlab/gtsam_examples/Pose2SLAMExample.m index 65ce28dbb..789d1c483 100644 --- a/matlab/gtsam_examples/Pose2SLAMExample.m +++ b/matlab/gtsam_examples/Pose2SLAMExample.m @@ -57,7 +57,7 @@ result.print(sprintf('\nFinal result:\n')); %% Plot Covariance Ellipses cla; hold on -plot([result.at(5).x;result.at(2).x],[result.at(5).y;result.at(2).y],'r-'); +plot([result.atPose2(5).x;result.atPose2(2).x],[result.atPose2(5).y;result.atPose2(2).y],'r-'); marginals = Marginals(graph, result); plot2DTrajectory(result, [], marginals); diff --git a/matlab/gtsam_examples/Pose2SLAMExample_circle.m b/matlab/gtsam_examples/Pose2SLAMExample_circle.m index 3d2265d76..d2676845c 100644 --- a/matlab/gtsam_examples/Pose2SLAMExample_circle.m +++ b/matlab/gtsam_examples/Pose2SLAMExample_circle.m @@ -14,8 +14,8 @@ import gtsam.* %% Create a hexagon of poses hexagon = circlePose2(6,1.0); -p0 = hexagon.at(0); -p1 = hexagon.at(1); +p0 = hexagon.atPose2(0); +p1 = hexagon.atPose2(1); %% create a Pose graph with one equality constraint and one measurement fg = NonlinearFactorGraph; @@ -32,11 +32,11 @@ fg.add(BetweenFactorPose2(5,0, delta, covariance)); %% Create initial config initial = Values; initial.insert(0, p0); -initial.insert(1, hexagon.at(1).retract([-0.1, 0.1,-0.1]')); -initial.insert(2, hexagon.at(2).retract([ 0.1,-0.1, 0.1]')); -initial.insert(3, hexagon.at(3).retract([-0.1, 0.1,-0.1]')); -initial.insert(4, hexagon.at(4).retract([ 0.1,-0.1, 0.1]')); -initial.insert(5, hexagon.at(5).retract([-0.1, 0.1,-0.1]')); +initial.insert(1, hexagon.atPose2(1).retract([-0.1, 0.1,-0.1]')); +initial.insert(2, hexagon.atPose2(2).retract([ 0.1,-0.1, 0.1]')); +initial.insert(3, hexagon.atPose2(3).retract([-0.1, 0.1,-0.1]')); +initial.insert(4, hexagon.atPose2(4).retract([ 0.1,-0.1, 0.1]')); +initial.insert(5, hexagon.atPose2(5).retract([-0.1, 0.1,-0.1]')); %% Plot Initial Estimate cla diff --git a/matlab/gtsam_examples/Pose2SLAMExample_graph.m b/matlab/gtsam_examples/Pose2SLAMExample_graph.m index 83ec949cc..c479278c1 100644 --- a/matlab/gtsam_examples/Pose2SLAMExample_graph.m +++ b/matlab/gtsam_examples/Pose2SLAMExample_graph.m @@ -41,7 +41,7 @@ marginals = Marginals(graph, result); toc P={}; for i=1:result.size()-1 - pose_i = result.at(i); + pose_i = result.atPose2(i); P{i}=marginals.marginalCovariance(i); plotPose2(pose_i,'b',P{i}) end diff --git a/matlab/gtsam_examples/Pose3SLAMExample.m b/matlab/gtsam_examples/Pose3SLAMExample.m index 1d9c3b579..5a9c8bf03 100644 --- a/matlab/gtsam_examples/Pose3SLAMExample.m +++ b/matlab/gtsam_examples/Pose3SLAMExample.m @@ -14,8 +14,8 @@ import gtsam.* %% Create a hexagon of poses hexagon = circlePose3(6,1.0); -p0 = hexagon.at(0); -p1 = hexagon.at(1); +p0 = hexagon.atPose3(0); +p1 = hexagon.atPose3(1); %% create a Pose graph with one equality constraint and one measurement fg = NonlinearFactorGraph; @@ -33,11 +33,11 @@ fg.add(BetweenFactorPose3(5,0, delta, covariance)); initial = Values; s = 0.10; initial.insert(0, p0); -initial.insert(1, hexagon.at(1).retract(s*randn(6,1))); -initial.insert(2, hexagon.at(2).retract(s*randn(6,1))); -initial.insert(3, hexagon.at(3).retract(s*randn(6,1))); -initial.insert(4, hexagon.at(4).retract(s*randn(6,1))); -initial.insert(5, hexagon.at(5).retract(s*randn(6,1))); +initial.insert(1, hexagon.atPose3(1).retract(s*randn(6,1))); +initial.insert(2, hexagon.atPose3(2).retract(s*randn(6,1))); +initial.insert(3, hexagon.atPose3(3).retract(s*randn(6,1))); +initial.insert(4, hexagon.atPose3(4).retract(s*randn(6,1))); +initial.insert(5, hexagon.atPose3(5).retract(s*randn(6,1))); %% Plot Initial Estimate cla diff --git a/matlab/gtsam_examples/Pose3SLAMExample_graph.m b/matlab/gtsam_examples/Pose3SLAMExample_graph.m index 39e48c204..de6f9e86d 100644 --- a/matlab/gtsam_examples/Pose3SLAMExample_graph.m +++ b/matlab/gtsam_examples/Pose3SLAMExample_graph.m @@ -28,7 +28,7 @@ model = noiseModel.Diagonal.Sigmas([5*pi/180; 5*pi/180; 5*pi/180; 0.05; 0.05; 0. %% Plot Initial Estimate cla -first = initial.at(0); +first = initial.atPose3(0); plot3(first.x(),first.y(),first.z(),'r*'); hold on plot3DTrajectory(initial,'g-',false); drawnow; diff --git a/matlab/gtsam_examples/StereoVOExample_large.m b/matlab/gtsam_examples/StereoVOExample_large.m index b77733abd..464448335 100644 --- a/matlab/gtsam_examples/StereoVOExample_large.m +++ b/matlab/gtsam_examples/StereoVOExample_large.m @@ -45,7 +45,7 @@ for i=1:size(measurements,1) if ~initial.exists(symbol('l',sf(2))) % 3D landmarks are stored in camera coordinates: transform % to world coordinates using the respective initial pose - pose = initial.at(symbol('x', sf(1))); + pose = initial.atPose3(symbol('x', sf(1))); world_point = pose.transform_from(Point3(sf(6),sf(7),sf(8))); initial.insert(symbol('l',sf(2)), world_point); end @@ -54,7 +54,7 @@ toc %% add a constraint on the starting pose key = symbol('x',1); -first_pose = initial.at(key); +first_pose = initial.atPose3(key); graph.add(NonlinearEqualityPose3(key, first_pose)); %% optimize diff --git a/matlab/gtsam_tests/testLocalizationExample.m b/matlab/gtsam_tests/testLocalizationExample.m index 5f1d89c99..ed091ea71 100644 --- a/matlab/gtsam_tests/testLocalizationExample.m +++ b/matlab/gtsam_tests/testLocalizationExample.m @@ -29,7 +29,7 @@ groundTruth.insert(2, Pose2(2.0, 0.0, 0.0)); groundTruth.insert(3, Pose2(4.0, 0.0, 0.0)); model = noiseModel.Diagonal.Sigmas([0.1; 0.1; 10]); for i=1:3 - graph.add(PriorFactorPose2(i, groundTruth.at(i), model)); + graph.add(PriorFactorPose2(i, groundTruth.atPose2(i), model)); end %% Initialize to noisy points @@ -46,7 +46,7 @@ result = optimizer.optimizeSafely(); marginals = Marginals(graph, result); P={}; for i=1:result.size() - pose_i = result.at(i); - CHECK('pose_i.equals(groundTruth.pose(i)',pose_i.equals(groundTruth.at(i),1e-4)); + pose_i = result.atPose2(i); + CHECK('pose_i.equals(groundTruth.pose(i)',pose_i.equals(groundTruth.atPose2(i),1e-4)); P{i}=marginals.marginalCovariance(i); end diff --git a/matlab/gtsam_tests/testOdometryExample.m b/matlab/gtsam_tests/testOdometryExample.m index 442b36801..9bd4762a7 100644 --- a/matlab/gtsam_tests/testOdometryExample.m +++ b/matlab/gtsam_tests/testOdometryExample.m @@ -39,5 +39,5 @@ marginals = Marginals(graph, result); marginals.marginalCovariance(1); %% Check first pose equality -pose_1 = result.at(1); +pose_1 = result.atPose2(1); CHECK('pose_1.equals(Pose2,1e-4)',pose_1.equals(Pose2,1e-4)); diff --git a/matlab/gtsam_tests/testPlanarSLAMExample.m b/matlab/gtsam_tests/testPlanarSLAMExample.m index 7694a1a0b..d3cab5d1f 100644 --- a/matlab/gtsam_tests/testPlanarSLAMExample.m +++ b/matlab/gtsam_tests/testPlanarSLAMExample.m @@ -60,10 +60,10 @@ result = optimizer.optimizeSafely(); marginals = Marginals(graph, result); %% Check first pose and point equality -pose_1 = result.at(symbol('x',1)); +pose_1 = result.atPose2(symbol('x',1)); marginals.marginalCovariance(symbol('x',1)); CHECK('pose_1.equals(Pose2,1e-4)',pose_1.equals(Pose2,1e-4)); -point_1 = result.at(symbol('l',1)); +point_1 = result.atPoint2(symbol('l',1)); marginals.marginalCovariance(symbol('l',1)); CHECK('point_1.equals(Point2(2,2),1e-4)',point_1.equals(Point2(2,2),1e-4)); diff --git a/matlab/gtsam_tests/testPose2SLAMExample.m b/matlab/gtsam_tests/testPose2SLAMExample.m index 8c70c27e7..72e5331f2 100644 --- a/matlab/gtsam_tests/testPose2SLAMExample.m +++ b/matlab/gtsam_tests/testPose2SLAMExample.m @@ -57,6 +57,6 @@ result = optimizer.optimizeSafely(); marginals = Marginals(graph, result); P = marginals.marginalCovariance(1); -pose_1 = result.at(1); +pose_1 = result.atPose2(1); CHECK('pose_1.equals(Pose2,1e-4)',pose_1.equals(Pose2,1e-4)); diff --git a/matlab/gtsam_tests/testPose3SLAMExample.m b/matlab/gtsam_tests/testPose3SLAMExample.m index dafad4e47..51ba69240 100644 --- a/matlab/gtsam_tests/testPose3SLAMExample.m +++ b/matlab/gtsam_tests/testPose3SLAMExample.m @@ -14,8 +14,8 @@ import gtsam.* %% Create a hexagon of poses hexagon = circlePose3(6,1.0); -p0 = hexagon.at(0); -p1 = hexagon.at(1); +p0 = hexagon.atPose3(0); +p1 = hexagon.atPose3(1); %% create a Pose graph with one equality constraint and one measurement fg = NonlinearFactorGraph; @@ -33,17 +33,17 @@ fg.add(BetweenFactorPose3(5,0, delta, covariance)); initial = Values; s = 0.10; initial.insert(0, p0); -initial.insert(1, hexagon.at(1).retract(s*randn(6,1))); -initial.insert(2, hexagon.at(2).retract(s*randn(6,1))); -initial.insert(3, hexagon.at(3).retract(s*randn(6,1))); -initial.insert(4, hexagon.at(4).retract(s*randn(6,1))); -initial.insert(5, hexagon.at(5).retract(s*randn(6,1))); +initial.insert(1, hexagon.atPose3(1).retract(s*randn(6,1))); +initial.insert(2, hexagon.atPose3(2).retract(s*randn(6,1))); +initial.insert(3, hexagon.atPose3(3).retract(s*randn(6,1))); +initial.insert(4, hexagon.atPose3(4).retract(s*randn(6,1))); +initial.insert(5, hexagon.atPose3(5).retract(s*randn(6,1))); %% optimize optimizer = LevenbergMarquardtOptimizer(fg, initial); result = optimizer.optimizeSafely; -pose_1 = result.at(1); +pose_1 = result.atPose3(1); CHECK('pose_1.equals(Pose3,1e-4)',pose_1.equals(p1,1e-4)); diff --git a/matlab/gtsam_tests/testSFMExample.m b/matlab/gtsam_tests/testSFMExample.m index 2b04be8f1..985cbdb2c 100644 --- a/matlab/gtsam_tests/testSFMExample.m +++ b/matlab/gtsam_tests/testSFMExample.m @@ -63,11 +63,11 @@ marginals.marginalCovariance(symbol('x',1)); %% Check optimized results, should be equal to ground truth for i=1:size(truth.cameras,2) - pose_i = result.at(symbol('x',i)); + pose_i = result.atPose3(symbol('x',i)); CHECK('pose_i.equals(truth.cameras{i}.pose,1e-5)',pose_i.equals(truth.cameras{i}.pose,1e-5)) end for j=1:size(truth.points,2) - point_j = result.at(symbol('p',j)); + point_j = result.atPoint3(symbol('p',j)); CHECK('point_j.equals(truth.points{j},1e-5)',point_j.equals(truth.points{j},1e-5)) end diff --git a/matlab/gtsam_tests/testStereoVOExample.m b/matlab/gtsam_tests/testStereoVOExample.m index e2d6f2479..218d0ace1 100644 --- a/matlab/gtsam_tests/testStereoVOExample.m +++ b/matlab/gtsam_tests/testStereoVOExample.m @@ -61,8 +61,8 @@ optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate); result = optimizer.optimize(); %% check equality for the first pose and point -pose_x1 = result.at(x1); +pose_x1 = result.atPose3(x1); CHECK('pose_x1.equals(first_pose,1e-4)',pose_x1.equals(first_pose,1e-4)); -point_l1 = result.at(l1); +point_l1 = result.atPoint3(l1); CHECK('point_1.equals(expected_l1,1e-4)',point_l1.equals(expected_l1,1e-4)); \ No newline at end of file diff --git a/matlab/gtsam_tests/testVisualISAMExample.m b/matlab/gtsam_tests/testVisualISAMExample.m index 40aca458e..223e823a6 100644 --- a/matlab/gtsam_tests/testVisualISAMExample.m +++ b/matlab/gtsam_tests/testVisualISAMExample.m @@ -45,11 +45,11 @@ for frame_i=3:options.nrCameras end for i=1:size(truth.cameras,2) - pose_i = result.at(symbol('x',i)); + pose_i = result.atPose3(symbol('x',i)); CHECK('pose_i.equals(truth.cameras{i}.pose,1e-5)',pose_i.equals(truth.cameras{i}.pose,1e-5)) end for j=1:size(truth.points,2) - point_j = result.at(symbol('l',j)); + point_j = result.atPoint3(symbol('l',j)); CHECK('point_j.equals(truth.points{j},1e-5)',point_j.equals(truth.points{j},1e-5)) end diff --git a/matlab/gtsam_tests/test_gtsam.m b/matlab/gtsam_tests/test_gtsam.m index c379179c5..e3705948d 100644 --- a/matlab/gtsam_tests/test_gtsam.m +++ b/matlab/gtsam_tests/test_gtsam.m @@ -30,11 +30,11 @@ testStereoVOExample display 'Starting: testVisualISAMExample' testVisualISAMExample -display 'Starting: testSerialization' -testSerialization - display 'Starting: testUtilities' testUtilities +display 'Starting: testSerialization' +testSerialization + % end of tests display 'Tests complete!' diff --git a/package_scripts/toolbox_package_unix.sh b/package_scripts/toolbox_package_unix.sh index 844987324..182533672 100755 --- a/package_scripts/toolbox_package_unix.sh +++ b/package_scripts/toolbox_package_unix.sh @@ -61,4 +61,4 @@ if [ $? -ne 0 ]; then fi # Create package -tar czf gtsam-toolbox-3.0.0-$platform.tgz -C stage/gtsam_toolbox toolbox +tar czf gtsam-toolbox-3.2.0-$platform.tgz -C stage/gtsam_toolbox toolbox diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index 55329d8e9..553a7fd5f 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -6,24 +6,24 @@ #include -#include -#include -#include -#include +#include +#include +#include +#include #include #include -#include -#include -#include -#include #include #include #include #include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include #include #include @@ -35,6 +35,9 @@ using namespace std; using namespace gtsam; using boost::shared_ptr; +static const SharedNoiseModel model; +static const LieScalar Zero(0); + // SETDEBUG("ISAM2 update", true); // SETDEBUG("ISAM2 update verbose", true); // SETDEBUG("ISAM2 recalculate", true); @@ -203,11 +206,11 @@ struct ConsistencyVisitor consistent(true), isam(isam) {} int operator()(const ISAM2::sharedClique& node, int& parentData) { - if(std::find(isam.roots().begin(), isam.roots().end(), node) == isam.roots().end()) + if(find(isam.roots().begin(), isam.roots().end(), node) == isam.roots().end()) { if(node->parent_.expired()) consistent = false; - if(std::find(node->parent()->children.begin(), node->parent()->children.end(), node) == node->parent()->children.end()) + if(find(node->parent()->children.begin(), node->parent()->children.end(), node) == node->parent()->children.end()) consistent = false; } BOOST_FOREACH(Key j, node->conditional()->frontals()) @@ -223,7 +226,7 @@ struct ConsistencyVisitor bool isam_check(const NonlinearFactorGraph& fullgraph, const Values& fullinit, const ISAM2& isam, Test& test, TestResult& result) { TestResult& result_ = result; - const std::string name_ = test.getName(); + const string name_ = test.getName(); Values actual = isam.calculateEstimate(); Values expected = fullinit.retract(fullgraph.linearize(fullinit)->optimize()); @@ -285,19 +288,19 @@ bool isam_check(const NonlinearFactorGraph& fullgraph, const Values& fullinit, c TEST(ISAM2, AddFactorsStep1) { NonlinearFactorGraph nonlinearFactors; - nonlinearFactors += PriorFactor(10, LieVector(), gtsam::SharedNoiseModel()); + nonlinearFactors += PriorFactor(10, Zero, model); nonlinearFactors += NonlinearFactor::shared_ptr(); - nonlinearFactors += PriorFactor(11, LieVector(), gtsam::SharedNoiseModel()); + nonlinearFactors += PriorFactor(11, Zero, model); NonlinearFactorGraph newFactors; - newFactors += PriorFactor(1, LieVector(), gtsam::SharedNoiseModel()); - newFactors += PriorFactor(2, LieVector(), gtsam::SharedNoiseModel()); + newFactors += PriorFactor(1, Zero, model); + newFactors += PriorFactor(2, Zero, model); NonlinearFactorGraph expectedNonlinearFactors; - expectedNonlinearFactors += PriorFactor(10, LieVector(), gtsam::SharedNoiseModel()); - expectedNonlinearFactors += PriorFactor(1, LieVector(), gtsam::SharedNoiseModel()); - expectedNonlinearFactors += PriorFactor(11, LieVector(), gtsam::SharedNoiseModel()); - expectedNonlinearFactors += PriorFactor(2, LieVector(), gtsam::SharedNoiseModel()); + expectedNonlinearFactors += PriorFactor(10, Zero, model); + expectedNonlinearFactors += PriorFactor(1, Zero, model); + expectedNonlinearFactors += PriorFactor(11, Zero, model); + expectedNonlinearFactors += PriorFactor(2, Zero, model); const FastVector expectedNewFactorIndices = list_of(1)(3); @@ -694,18 +697,17 @@ namespace { TEST(ISAM2, marginalizeLeaves1) { ISAM2 isam; - NonlinearFactorGraph factors; - factors += PriorFactor(0, LieVector(0.0), noiseModel::Unit::Create(1)); + factors += PriorFactor(0, Zero, model); - factors += BetweenFactor(0, 1, LieVector(0.0), noiseModel::Unit::Create(1)); - factors += BetweenFactor(1, 2, LieVector(0.0), noiseModel::Unit::Create(1)); - factors += BetweenFactor(0, 2, LieVector(0.0), noiseModel::Unit::Create(1)); + factors += BetweenFactor(0, 1, Zero, model); + factors += BetweenFactor(1, 2, Zero, model); + factors += BetweenFactor(0, 2, Zero, model); Values values; - values.insert(0, LieVector(0.0)); - values.insert(1, LieVector(0.0)); - values.insert(2, LieVector(0.0)); + values.insert(0, Zero); + values.insert(1, Zero); + values.insert(2, Zero); FastMap constrainedKeys; constrainedKeys.insert(make_pair(0,0)); @@ -724,18 +726,18 @@ TEST(ISAM2, marginalizeLeaves2) ISAM2 isam; NonlinearFactorGraph factors; - factors += PriorFactor(0, LieVector(0.0), noiseModel::Unit::Create(1)); + factors += PriorFactor(0, Zero, model); - factors += BetweenFactor(0, 1, LieVector(0.0), noiseModel::Unit::Create(1)); - factors += BetweenFactor(1, 2, LieVector(0.0), noiseModel::Unit::Create(1)); - factors += BetweenFactor(0, 2, LieVector(0.0), noiseModel::Unit::Create(1)); - factors += BetweenFactor(2, 3, LieVector(0.0), noiseModel::Unit::Create(1)); + factors += BetweenFactor(0, 1, Zero, model); + factors += BetweenFactor(1, 2, Zero, model); + factors += BetweenFactor(0, 2, Zero, model); + factors += BetweenFactor(2, 3, Zero, model); Values values; - values.insert(0, LieVector(0.0)); - values.insert(1, LieVector(0.0)); - values.insert(2, LieVector(0.0)); - values.insert(3, LieVector(0.0)); + values.insert(0, Zero); + values.insert(1, Zero); + values.insert(2, Zero); + values.insert(3, Zero); FastMap constrainedKeys; constrainedKeys.insert(make_pair(0,0)); @@ -755,25 +757,25 @@ TEST(ISAM2, marginalizeLeaves3) ISAM2 isam; NonlinearFactorGraph factors; - factors += PriorFactor(0, LieVector(0.0), noiseModel::Unit::Create(1)); + factors += PriorFactor(0, Zero, model); - factors += BetweenFactor(0, 1, LieVector(0.0), noiseModel::Unit::Create(1)); - factors += BetweenFactor(1, 2, LieVector(0.0), noiseModel::Unit::Create(1)); - factors += BetweenFactor(0, 2, LieVector(0.0), noiseModel::Unit::Create(1)); + factors += BetweenFactor(0, 1, Zero, model); + factors += BetweenFactor(1, 2, Zero, model); + factors += BetweenFactor(0, 2, Zero, model); - factors += BetweenFactor(2, 3, LieVector(0.0), noiseModel::Unit::Create(1)); + factors += BetweenFactor(2, 3, Zero, model); - factors += BetweenFactor(3, 4, LieVector(0.0), noiseModel::Unit::Create(1)); - factors += BetweenFactor(4, 5, LieVector(0.0), noiseModel::Unit::Create(1)); - factors += BetweenFactor(3, 5, LieVector(0.0), noiseModel::Unit::Create(1)); + factors += BetweenFactor(3, 4, Zero, model); + factors += BetweenFactor(4, 5, Zero, model); + factors += BetweenFactor(3, 5, Zero, model); Values values; - values.insert(0, LieVector(0.0)); - values.insert(1, LieVector(0.0)); - values.insert(2, LieVector(0.0)); - values.insert(3, LieVector(0.0)); - values.insert(4, LieVector(0.0)); - values.insert(5, LieVector(0.0)); + values.insert(0, Zero); + values.insert(1, Zero); + values.insert(2, Zero); + values.insert(3, Zero); + values.insert(4, Zero); + values.insert(5, Zero); FastMap constrainedKeys; constrainedKeys.insert(make_pair(0,0)); @@ -795,14 +797,14 @@ TEST(ISAM2, marginalizeLeaves4) ISAM2 isam; NonlinearFactorGraph factors; - factors += PriorFactor(0, LieVector(0.0), noiseModel::Unit::Create(1)); - factors += BetweenFactor(0, 2, LieVector(0.0), noiseModel::Unit::Create(1)); - factors += BetweenFactor(1, 2, LieVector(0.0), noiseModel::Unit::Create(1)); + factors += PriorFactor(0, Zero, model); + factors += BetweenFactor(0, 2, Zero, model); + factors += BetweenFactor(1, 2, Zero, model); Values values; - values.insert(0, LieVector(0.0)); - values.insert(1, LieVector(0.0)); - values.insert(2, LieVector(0.0)); + values.insert(0, Zero); + values.insert(1, Zero); + values.insert(2, Zero); FastMap constrainedKeys; constrainedKeys.insert(make_pair(0,0)); diff --git a/tests/testManifold.cpp b/tests/testManifold.cpp new file mode 100644 index 000000000..32f04225f --- /dev/null +++ b/tests/testManifold.cpp @@ -0,0 +1,211 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------1------------------------------------------- */ + +/** + * @file testExpression.cpp + * @date September 18, 2014 + * @author Frank Dellaert + * @author Paul Furgale + * @brief unit tests for Block Automatic Differentiation + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#undef CHECK +#include + +#include +using boost::assign::list_of; +using boost::assign::map_list_of; + +using namespace std; +using namespace gtsam; + +// The DefaultChart of Camera below is laid out like Snavely's 9-dim vector +typedef PinholeCamera Camera; + +/* ************************************************************************* */ +// is_manifold +TEST(Manifold, _is_manifold) { + using namespace traits; + EXPECT(!is_manifold::value); + EXPECT(is_manifold::value); + EXPECT(is_manifold::value); + EXPECT(is_manifold::value); + EXPECT(is_manifold::value); + EXPECT(is_manifold::value); +} + +/* ************************************************************************* */ +// dimension +TEST(Manifold, _dimension) { + using namespace traits; + EXPECT_LONGS_EQUAL(2, dimension::value); + EXPECT_LONGS_EQUAL(8, dimension::value); + EXPECT_LONGS_EQUAL(1, dimension::value); + EXPECT_LONGS_EQUAL(Eigen::Dynamic, dimension::value); + EXPECT_LONGS_EQUAL(Eigen::Dynamic, dimension::value); +} + +/* ************************************************************************* */ +// charts +TEST(Manifold, DefaultChart) { + + DefaultChart chart1; + EXPECT(chart1.local(Point2(0, 0), Point2(1, 0)) == Vector2(1, 0)); + EXPECT(chart1.retract(Point2(0, 0), Vector2(1, 0)) == Point2(1, 0)); + + Vector v2(2); + v2 << 1, 0; + DefaultChart chart2; + EXPECT(assert_equal(v2, chart2.local(Vector2(0, 0), Vector2(1, 0)))); + EXPECT(chart2.retract(Vector2(0, 0), v2) == Vector2(1, 0)); + + { + typedef Matrix2 ManifoldPoint; + ManifoldPoint m; + DefaultChart chart; + m << 1, 3, + 2, 4; + // m as per default is in column-major storage mode. So this yields a linear representation of (1, 2, 3, 4)! + EXPECT(assert_equal(Vector(Vector4(1, 2, 3, 4)), Vector(chart.local(ManifoldPoint::Zero(), m)))); + EXPECT(chart.retract(m, Vector4(1, 2, 3, 4)) == 2 * m); + } + + { + typedef Eigen::Matrix ManifoldPoint; + ManifoldPoint m; + DefaultChart chart; + m << 1, 2; + EXPECT(assert_equal(Vector(Vector2(1, 2)), Vector(chart.local(ManifoldPoint::Zero(), m)))); + EXPECT(chart.retract(m, Vector2(1, 2)) == 2 * m); + } + + { + typedef Eigen::Matrix ManifoldPoint; + ManifoldPoint m; + DefaultChart chart; + m << 1; + EXPECT(assert_equal(Vector(ManifoldPoint::Ones()), Vector(chart.local(ManifoldPoint::Zero(), m)))); + EXPECT(chart.retract(m, ManifoldPoint::Ones()) == 2 * m); + } + + DefaultChart chart3; + Vector v1(1); + v1 << 1; + EXPECT(assert_equal(v1, chart3.local(0, 1))); + EXPECT_DOUBLES_EQUAL(chart3.retract(0, v1), 1, 1e-9); + + // Dynamic does not work yet ! + Vector z = zero(2), v(2); + v << 1, 0; + DefaultChart chart4; + EXPECT(assert_equal(chart4.local(z, v), v)); + EXPECT(assert_equal(chart4.retract(z, v), v)); + + Vector v3(3); + v3 << 1, 1, 1; + Rot3 I = Rot3::identity(); + Rot3 R = I.retract(v3); + DefaultChart chart5; + EXPECT(assert_equal(v3, chart5.local(I, R))); + EXPECT(assert_equal(chart5.retract(I, v3), R)); + // Check zero vector + DefaultChart chart6; + EXPECT(assert_equal(zero(3), chart6.local(R, R))); +} + +/* ************************************************************************* */ +// zero +TEST(Manifold, _zero) { + EXPECT(assert_equal(Pose3(), traits::zero::value())); + Cal3Bundler cal(0, 0, 0); + EXPECT(assert_equal(cal, traits::zero::value())); + EXPECT(assert_equal(Camera(Pose3(), cal), traits::zero::value())); + EXPECT(assert_equal(Point2(), traits::zero::value())); + EXPECT(assert_equal(Matrix(Matrix24::Zero()), Matrix(traits::zero::value()))); + EXPECT_DOUBLES_EQUAL(0.0, traits::zero::value(), 0.0); +} + +/* ************************************************************************* */ +// identity +TEST(Manifold, _identity) { + EXPECT(assert_equal(Pose3(), traits::identity::value())); + EXPECT(assert_equal(Cal3Bundler(), traits::identity::value())); + EXPECT(assert_equal(Camera(), traits::identity::value())); + EXPECT(assert_equal(Point2(), traits::identity::value())); + EXPECT(assert_equal(Matrix(Matrix24::Zero()), Matrix(traits::identity::value()))); + EXPECT_DOUBLES_EQUAL(0.0, traits::identity::value(), 0.0); +} + +/* ************************************************************************* */ +// charts +TEST(Manifold, Canonical) { + + Canonical chart1; + EXPECT(chart1.local(Point2(1, 0))==Vector2(1, 0)); + EXPECT(chart1.retract(Vector2(1, 0))==Point2(1, 0)); + + Vector v2(2); + v2 << 1, 0; + Canonical chart2; + EXPECT(assert_equal(v2, chart2.local(Vector2(1, 0)))); + EXPECT(chart2.retract(v2)==Vector2(1, 0)); + + Canonical chart3; + Eigen::Matrix v1; + v1 << 1; + EXPECT(chart3.local(1)==v1); + EXPECT_DOUBLES_EQUAL(chart3.retract(v1), 1, 1e-9); + + Canonical chart4; + Point3 point(1, 2, 3); + Vector v3(3); + v3 << 1, 2, 3; + EXPECT(assert_equal(v3, chart4.local(point))); + EXPECT(assert_equal(chart4.retract(v3), point)); + + Canonical chart5; + Pose3 pose(Rot3::identity(), point); + Vector v6(6); + v6 << 0, 0, 0, 1, 2, 3; + EXPECT(assert_equal(v6, chart5.local(pose))); + EXPECT(assert_equal(chart5.retract(v6), pose)); + + Canonical chart6; + Cal3Bundler cal0(0, 0, 0); + Camera camera(Pose3(), cal0); + Vector z9 = Vector9::Zero(); + EXPECT(assert_equal(z9, chart6.local(camera))); + EXPECT(assert_equal(chart6.retract(z9), camera)); + + Cal3Bundler cal; // Note !! Cal3Bundler() != zero::value() + Camera camera2(pose, cal); + Vector v9(9); + v9 << 0, 0, 0, 1, 2, 3, 1, 0, 0; + EXPECT(assert_equal(v9, chart6.local(camera2))); + EXPECT(assert_equal(chart6.retract(v9), camera2)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp index 390257f02..025a3c12e 100644 --- a/tests/testNonlinearFactor.cpp +++ b/tests/testNonlinearFactor.cpp @@ -27,7 +27,6 @@ #include #include -#include #include #include #include @@ -197,8 +196,7 @@ TEST( NonlinearFactor, size ) /* ************************************************************************* */ TEST( NonlinearFactor, linearize_constraint1 ) { - Vector sigmas = (Vector(2) << 0.2, 0.0); - SharedDiagonal constraint = noiseModel::Constrained::MixedSigmas(sigmas); + SharedDiagonal constraint = noiseModel::Constrained::MixedSigmas(Vector2(0.2,0)); Point2 mu(1., -1.); NonlinearFactorGraph::sharedFactor f0(new simulated2D::Prior(mu, constraint, X(1))); @@ -208,17 +206,16 @@ TEST( NonlinearFactor, linearize_constraint1 ) GaussianFactor::shared_ptr actual = f0->linearize(config); // create expected - Vector b = (Vector(2) << 0., -3.); + Vector2 b(0., -3.); JacobianFactor expected(X(1), (Matrix(2, 2) << 5.0, 0.0, 0.0, 1.0), b, - noiseModel::Constrained::MixedSigmas((Vector(2) << 1.0, 0.0))); + noiseModel::Constrained::MixedSigmas(Vector2(1,0))); CHECK(assert_equal((const GaussianFactor&)expected, *actual)); } /* ************************************************************************* */ TEST( NonlinearFactor, linearize_constraint2 ) { - Vector sigmas = (Vector(2) << 0.2, 0.0); - SharedDiagonal constraint = noiseModel::Constrained::MixedSigmas(sigmas); + SharedDiagonal constraint = noiseModel::Constrained::MixedSigmas(Vector2(0.2,0)); Point2 z3(1.,-1.); simulated2D::Measurement f0(z3, constraint, X(1),L(1)); @@ -229,21 +226,21 @@ TEST( NonlinearFactor, linearize_constraint2 ) GaussianFactor::shared_ptr actual = f0.linearize(config); // create expected - Matrix A = (Matrix(2, 2) << 5.0, 0.0, 0.0, 1.0); - Vector b = (Vector(2) << -15., -3.); + Matrix2 A; A << 5.0, 0.0, 0.0, 1.0; + Vector2 b(-15., -3.); JacobianFactor expected(X(1), -1*A, L(1), A, b, - noiseModel::Constrained::MixedSigmas((Vector(2) << 1.0, 0.0))); + noiseModel::Constrained::MixedSigmas(Vector2(1,0))); CHECK(assert_equal((const GaussianFactor&)expected, *actual)); } /* ************************************************************************* */ -class TestFactor4 : public NoiseModelFactor4 { +class TestFactor4 : public NoiseModelFactor4 { public: - typedef NoiseModelFactor4 Base; + typedef NoiseModelFactor4 Base; TestFactor4() : Base(noiseModel::Diagonal::Sigmas((Vector(1) << 2.0)), X(1), X(2), X(3), X(4)) {} virtual Vector - evaluateError(const LieVector& x1, const LieVector& x2, const LieVector& x3, const LieVector& x4, + evaluateError(const double& x1, const double& x2, const double& x3, const double& x4, boost::optional H1 = boost::none, boost::optional H2 = boost::none, boost::optional H3 = boost::none, @@ -266,10 +263,10 @@ public: TEST(NonlinearFactor, NoiseModelFactor4) { TestFactor4 tf; Values tv; - tv.insert(X(1), LieVector((Vector(1) << 1.0))); - tv.insert(X(2), LieVector((Vector(1) << 2.0))); - tv.insert(X(3), LieVector((Vector(1) << 3.0))); - tv.insert(X(4), LieVector((Vector(1) << 4.0))); + tv.insert(X(1), double((1.0))); + tv.insert(X(2), double((2.0))); + tv.insert(X(3), double((3.0))); + tv.insert(X(4), double((4.0))); EXPECT(assert_equal((Vector(1) << 10.0), tf.unwhitenedError(tv))); DOUBLES_EQUAL(25.0/2.0, tf.error(tv), 1e-9); JacobianFactor jf(*boost::dynamic_pointer_cast(tf.linearize(tv))); @@ -285,9 +282,9 @@ TEST(NonlinearFactor, NoiseModelFactor4) { } /* ************************************************************************* */ -class TestFactor5 : public NoiseModelFactor5 { +class TestFactor5 : public NoiseModelFactor5 { public: - typedef NoiseModelFactor5 Base; + typedef NoiseModelFactor5 Base; TestFactor5() : Base(noiseModel::Diagonal::Sigmas((Vector(1) << 2.0)), X(1), X(2), X(3), X(4), X(5)) {} virtual Vector @@ -312,11 +309,11 @@ public: TEST(NonlinearFactor, NoiseModelFactor5) { TestFactor5 tf; Values tv; - tv.insert(X(1), LieVector((Vector(1) << 1.0))); - tv.insert(X(2), LieVector((Vector(1) << 2.0))); - tv.insert(X(3), LieVector((Vector(1) << 3.0))); - tv.insert(X(4), LieVector((Vector(1) << 4.0))); - tv.insert(X(5), LieVector((Vector(1) << 5.0))); + tv.insert(X(1), double((1.0))); + tv.insert(X(2), double((2.0))); + tv.insert(X(3), double((3.0))); + tv.insert(X(4), double((4.0))); + tv.insert(X(5), double((5.0))); EXPECT(assert_equal((Vector(1) << 15.0), tf.unwhitenedError(tv))); DOUBLES_EQUAL(56.25/2.0, tf.error(tv), 1e-9); JacobianFactor jf(*boost::dynamic_pointer_cast(tf.linearize(tv))); @@ -334,9 +331,9 @@ TEST(NonlinearFactor, NoiseModelFactor5) { } /* ************************************************************************* */ -class TestFactor6 : public NoiseModelFactor6 { +class TestFactor6 : public NoiseModelFactor6 { public: - typedef NoiseModelFactor6 Base; + typedef NoiseModelFactor6 Base; TestFactor6() : Base(noiseModel::Diagonal::Sigmas((Vector(1) << 2.0)), X(1), X(2), X(3), X(4), X(5), X(6)) {} virtual Vector @@ -364,12 +361,12 @@ public: TEST(NonlinearFactor, NoiseModelFactor6) { TestFactor6 tf; Values tv; - tv.insert(X(1), LieVector((Vector(1) << 1.0))); - tv.insert(X(2), LieVector((Vector(1) << 2.0))); - tv.insert(X(3), LieVector((Vector(1) << 3.0))); - tv.insert(X(4), LieVector((Vector(1) << 4.0))); - tv.insert(X(5), LieVector((Vector(1) << 5.0))); - tv.insert(X(6), LieVector((Vector(1) << 6.0))); + tv.insert(X(1), double((1.0))); + tv.insert(X(2), double((2.0))); + tv.insert(X(3), double((3.0))); + tv.insert(X(4), double((4.0))); + tv.insert(X(5), double((5.0))); + tv.insert(X(6), double((6.0))); EXPECT(assert_equal((Vector(1) << 21.0), tf.unwhitenedError(tv))); DOUBLES_EQUAL(110.25/2.0, tf.error(tv), 1e-9); JacobianFactor jf(*boost::dynamic_pointer_cast(tf.linearize(tv))); diff --git a/timing/timeMatrixOps.cpp b/timing/timeMatrixOps.cpp index e924c8402..a2d11a756 100644 --- a/timing/timeMatrixOps.cpp +++ b/timing/timeMatrixOps.cpp @@ -62,9 +62,9 @@ int main(int argc, char* argv[]) { gtsam::SubMatrix top = mat.block(0, 0, n, n); gtsam::SubMatrix block = mat.block(m/4, n/4, m-m/2, n-n/2); - cout << format(" Basic: %1%x%2%\n") % m % n; - cout << format(" Full: mat(%1%:%2%, %3%:%4%)\n") % 0 % m % 0 % n; - cout << format(" Top: mat(%1%:%2%, %3%:%4%)\n") % 0 % n % 0 % n; + cout << format(" Basic: %1%x%2%\n") % (int)m % (int)n; + cout << format(" Full: mat(%1%:%2%, %3%:%4%)\n") % 0 % (int)m % 0 % (int)n; + cout << format(" Top: mat(%1%:%2%, %3%:%4%)\n") % 0 % (int)n % 0 % (int)n; cout << format(" Block: mat(%1%:%2%, %3%:%4%)\n") % size_t(m/4) % size_t(m-m/4) % size_t(n/4) % size_t(n-n/4); cout << endl; diff --git a/timing/timePinholeCamera.cpp b/timing/timePinholeCamera.cpp index 2113ad56d..bd39589f4 100644 --- a/timing/timePinholeCamera.cpp +++ b/timing/timePinholeCamera.cpp @@ -89,7 +89,7 @@ int main() Matrix Dpose, Dpoint; long timeLog = clock(); for(int i = 0; i < n; i++) - camera.project(point1, Dpose, Dpoint); + camera.project(point1, Dpose, Dpoint, boost::none); long timeLog2 = clock(); double seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC; cout << ((double)seconds*1e9/n) << " nanosecs/call" << endl; diff --git a/wrap/Argument.cpp b/wrap/Argument.cpp index d76556e4a..4989afb0d 100644 --- a/wrap/Argument.cpp +++ b/wrap/Argument.cpp @@ -28,34 +28,53 @@ using namespace std; using namespace wrap; +/* ************************************************************************* */ +Argument Argument::expandTemplate(const TemplateSubstitution& ts) const { + Argument instArg = *this; + instArg.type = ts(type); + return instArg; +} + +/* ************************************************************************* */ +ArgumentList ArgumentList::expandTemplate(const TemplateSubstitution& ts) const { + ArgumentList instArgList; + BOOST_FOREACH(const Argument& arg, *this) { + Argument instArg = arg.expandTemplate(ts); + instArgList.push_back(instArg); + } + return instArgList; +} + /* ************************************************************************* */ string Argument::matlabClass(const string& delim) const { string result; - BOOST_FOREACH(const string& ns, namespaces) + BOOST_FOREACH(const string& ns, type.namespaces) result += ns + delim; - if (type == "string" || type == "unsigned char" || type == "char") + if (type.name == "string" || type.name == "unsigned char" + || type.name == "char") return result + "char"; - if (type == "Vector" || type == "Matrix") + if (type.name == "Vector" || type.name == "Matrix") return result + "double"; - if (type == "int" || type == "size_t") + if (type.name == "int" || type.name == "size_t") return result + "numeric"; - if (type == "bool") + if (type.name == "bool") return result + "logical"; - return result + type; + return result + type.name; } /* ************************************************************************* */ bool Argument::isScalar() const { - return (type == "bool" || type == "char" || type == "unsigned char" - || type == "int" || type == "size_t" || type == "double"); + return (type.name == "bool" || type.name == "char" + || type.name == "unsigned char" || type.name == "int" + || type.name == "size_t" || type.name == "double"); } /* ************************************************************************* */ void Argument::matlab_unwrap(FileWriter& file, const string& matlabName) const { file.oss << " "; - string cppType = qualifiedType("::"); - string matlabUniqueType = qualifiedType(); + string cppType = type.qualifiedName("::"); + string matlabUniqueType = type.qualifiedName(); if (is_ptr) // A pointer: emit an "unwrap_shared_ptr" call which returns a pointer @@ -78,14 +97,6 @@ void Argument::matlab_unwrap(FileWriter& file, const string& matlabName) const { file.oss << ");" << endl; } -/* ************************************************************************* */ -string Argument::qualifiedType(const string& delim) const { - string result; - BOOST_FOREACH(const string& ns, namespaces) - result += ns + delim; - return result + type; -} - /* ************************************************************************* */ string ArgumentList::types() const { string str; @@ -93,7 +104,7 @@ string ArgumentList::types() const { BOOST_FOREACH(Argument arg, *this) { if (!first) str += ","; - str += arg.type; + str += arg.type.name; first = false; } return str; @@ -105,14 +116,14 @@ string ArgumentList::signature() const { bool cap = false; BOOST_FOREACH(Argument arg, *this) { - BOOST_FOREACH(char ch, arg.type) + 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[0]; + sig += arg.type.name[0]; //Reset to default cap = false; } @@ -136,7 +147,8 @@ string ArgumentList::names() const { /* ************************************************************************* */ bool ArgumentList::allScalar() const { BOOST_FOREACH(Argument arg, *this) - if (!arg.isScalar()) return false; + if (!arg.isScalar()) + return false; return true; } @@ -158,42 +170,43 @@ void ArgumentList::emit_prototype(FileWriter& file, const string& name) const { BOOST_FOREACH(Argument arg, *this) { if (!first) file.oss << ", "; - file.oss << arg.type << " " << arg.name; + file.oss << arg.type.name << " " << arg.name; first = false; } file.oss << ")"; } /* ************************************************************************* */ -void ArgumentList::emit_call(FileWriter& file, const ReturnValue& returnVal, - const string& wrapperName, int id, bool staticMethod) const { - returnVal.emit_matlab(file); - file.oss << wrapperName << "(" << id; +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) - file.oss << ", this"; - file.oss << ", varargin{:});\n"; + proxyFile.oss << ", this"; + proxyFile.oss << ", varargin{:});\n"; } /* ************************************************************************* */ -void ArgumentList::emit_conditional_call(FileWriter& file, +void ArgumentList::emit_conditional_call(FileWriter& proxyFile, const ReturnValue& returnVal, const string& wrapperName, int id, bool staticMethod) const { // Check nr of arguments - file.oss << "if length(varargin) == " << size(); + proxyFile.oss << "if length(varargin) == " << size(); if (size() > 0) - file.oss << " && "; - // ...and their types + proxyFile.oss << " && "; + // ...and their type.names bool first = true; for (size_t i = 0; i < size(); i++) { if (!first) - file.oss << " && "; - file.oss << "isa(varargin{" << i + 1 << "},'" << (*this)[i].matlabClass(".") - << "')"; + proxyFile.oss << " && "; + proxyFile.oss << "isa(varargin{" << i + 1 << "},'" + << (*this)[i].matlabClass(".") << "')"; first = false; } - file.oss << "\n"; + proxyFile.oss << "\n"; // output call to C++ wrapper - file.oss << " "; - emit_call(file, returnVal, wrapperName, id, staticMethod); + proxyFile.oss << " "; + emit_call(proxyFile, returnVal, wrapperName, id, staticMethod); } /* ************************************************************************* */ diff --git a/wrap/Argument.h b/wrap/Argument.h index 6f791978a..02f104418 100644 --- a/wrap/Argument.h +++ b/wrap/Argument.h @@ -19,36 +19,39 @@ #pragma once +#include "TemplateSubstitution.h" #include "FileWriter.h" #include "ReturnValue.h" -#include -#include - namespace wrap { /// Argument class struct Argument { + Qualified type; bool is_const, is_ref, is_ptr; - std::string type; std::string name; - std::vector namespaces; Argument() : is_const(false), is_ref(false), is_ptr(false) { } + Argument expandTemplate(const TemplateSubstitution& ts) const; + /// return MATLAB class for use in isa(x,class) std::string matlabClass(const std::string& delim = "") const; /// Check if will be unwrapped using scalar login in wrap/matlab.h bool isScalar() const; - /// adds namespaces to type - std::string qualifiedType(const std::string& delim = "") const; - /// MATLAB code generation, MATLAB to C++ void matlab_unwrap(FileWriter& file, const std::string& matlabName) const; + + friend std::ostream& operator<<(std::ostream& os, const Argument& arg) { + os << (arg.is_const ? "const " : "") << arg.type << (arg.is_ptr ? "*" : "") + << (arg.is_ref ? "&" : ""); + return os; + } + }; /// Argument list is just a container with Arguments @@ -66,6 +69,8 @@ struct ArgumentList: public std::vector { /// Check if all arguments scalar bool allScalar() const; + ArgumentList expandTemplate(const TemplateSubstitution& ts) const; + // MATLAB code generation: /** @@ -83,25 +88,47 @@ struct ArgumentList: public std::vector { void emit_prototype(FileWriter& file, const std::string& name) const; /** - * emit emit MATLAB call to wrapper - * @param file output stream + * emit emit MATLAB call to 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& file, const ReturnValue& returnVal, + void emit_call(FileWriter& proxyFile, const ReturnValue& returnVal, const std::string& wrapperName, int id, bool staticMethod = false) const; /** - * emit conditional MATLAB call to wrapper (checking arguments first) - * @param file output stream + * 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& file, const ReturnValue& returnVal, - const std::string& wrapperName, int id, bool staticMethod = false) const; + void emit_conditional_call(FileWriter& proxyFile, + const ReturnValue& returnVal, const std::string& wrapperName, int id, + bool staticMethod = false) const; + + friend std::ostream& operator<<(std::ostream& os, + const ArgumentList& argList) { + os << "("; + if (argList.size() > 0) + os << argList.front(); + if (argList.size() > 1) + for (size_t i = 1; i < argList.size(); i++) + os << ", " << argList[i]; + os << ")"; + return os; + } + }; +template +inline void verifyArguments(const std::vector& validArgs, + const std::map& vt) { + typedef typename std::map::value_type NamedMethod; + BOOST_FOREACH(const NamedMethod& namedMethod, vt) + namedMethod.second.verifyArguments(validArgs); +} + } // \namespace wrap diff --git a/wrap/Class.cpp b/wrap/Class.cpp index 075c98811..3a3432eb3 100644 --- a/wrap/Class.cpp +++ b/wrap/Class.cpp @@ -33,7 +33,7 @@ using namespace std; using namespace wrap; /* ************************************************************************* */ -void Class::matlab_proxy(const string& toolboxPath, const string& wrapperName, +void Class::matlab_proxy(Str toolboxPath, Str wrapperName, const TypeAttributesTable& typeAttributes, FileWriter& wrapperFile, vector& functionNames) const { @@ -41,17 +41,15 @@ void Class::matlab_proxy(const string& toolboxPath, const string& wrapperName, createNamespaceStructure(namespaces, toolboxPath); // open destination classFile - string classFile = toolboxPath; - if (!namespaces.empty()) - classFile += "/+" + wrap::qualifiedName("/+", namespaces); - classFile += "/" + name + ".m"; + string classFile = matlabName(toolboxPath); FileWriter proxyFile(classFile, verbose_, "%"); // get the name of actual matlab object - const string matlabQualName = qualifiedName("."), matlabUniqueName = - qualifiedName(), cppName = qualifiedName("::"); - const string matlabBaseName = wrap::qualifiedName(".", qualifiedParent); - const string cppBaseName = wrap::qualifiedName("::", qualifiedParent); + 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 @@ -75,13 +73,15 @@ void Class::matlab_proxy(const string& toolboxPath, const string& wrapperName, pointer_constructor_fragments(proxyFile, wrapperFile, wrapperName, functionNames); wrapperFile.oss << "\n"; + // Regular constructors - BOOST_FOREACH(ArgumentList a, constructor.args_list) { + 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, a); + id, args); const string wrapFunctionName = constructor.wrapper_fragment(wrapperFile, - cppName, matlabUniqueName, cppBaseName, id, a); + cppName, matlabUniqueName, cppBaseName, id, args); wrapperFile.oss << "\n"; functionNames.push_back(wrapFunctionName); } @@ -144,19 +144,14 @@ void Class::matlab_proxy(const string& toolboxPath, const string& wrapperName, proxyFile.emit(true); } -/* ************************************************************************* */ -string Class::qualifiedName(const string& delim) const { - return ::wrap::qualifiedName(delim, namespaces, name); -} - /* ************************************************************************* */ void Class::pointer_constructor_fragments(FileWriter& proxyFile, - FileWriter& wrapperFile, const string& wrapperName, + FileWriter& wrapperFile, Str wrapperName, vector& functionNames) const { - const string matlabUniqueName = qualifiedName(), cppName = qualifiedName( - "::"); - const string baseCppName = wrap::qualifiedName("::", qualifiedParent); + const string matlabUniqueName = qualifiedName(); + const string cppName = qualifiedName("::"); + const string baseCppName = qualifiedParent.qualifiedName("::"); const int collectorInsertId = (int) functionNames.size(); const string collectorInsertFunctionName = matlabUniqueName @@ -247,128 +242,126 @@ void Class::pointer_constructor_fragments(FileWriter& proxyFile, } /* ************************************************************************* */ -vector expandArgumentListsTemplate( - const vector& argLists, const string& templateArg, - const vector& instName, - const std::vector& expandedClassNamespace, - const string& expandedClassName) { - vector result; - BOOST_FOREACH(const ArgumentList& argList, argLists) { - ArgumentList instArgList; - BOOST_FOREACH(const Argument& arg, argList) { - Argument instArg = arg; - if (arg.type == templateArg) { - instArg.namespaces.assign(instName.begin(), instName.end() - 1); - instArg.type = instName.back(); - } else if (arg.type == "This") { - instArg.namespaces.assign(expandedClassNamespace.begin(), - expandedClassNamespace.end()); - instArg.type = expandedClassName; - } - instArgList.push_back(instArg); - } - result.push_back(instArgList); - } - return result; -} - -/* ************************************************************************* */ -template -map expandMethodTemplate(const map& methods, - const string& templateArg, const vector& instName, - const std::vector& expandedClassNamespace, - const string& expandedClassName) { - map result; - typedef pair Name_Method; - BOOST_FOREACH(const Name_Method& name_method, methods) { - const METHOD& method = name_method.second; - METHOD instMethod = method; - instMethod.argLists = expandArgumentListsTemplate(method.argLists, - templateArg, instName, expandedClassNamespace, expandedClassName); - instMethod.returnVals.clear(); - BOOST_FOREACH(const ReturnValue& retVal, method.returnVals) { - ReturnValue instRetVal = retVal; - if (retVal.type1 == templateArg) { - instRetVal.namespaces1.assign(instName.begin(), instName.end() - 1); - instRetVal.type1 = instName.back(); - } else if (retVal.type1 == "This") { - instRetVal.namespaces1.assign(expandedClassNamespace.begin(), - expandedClassNamespace.end()); - instRetVal.type1 = expandedClassName; - } - if (retVal.type2 == templateArg) { - instRetVal.namespaces2.assign(instName.begin(), instName.end() - 1); - instRetVal.type2 = instName.back(); - } else if (retVal.type1 == "This") { - instRetVal.namespaces2.assign(expandedClassNamespace.begin(), - expandedClassNamespace.end()); - instRetVal.type2 = expandedClassName; - } - instMethod.returnVals.push_back(instRetVal); - } - result.insert(make_pair(name_method.first, instMethod)); - } - return result; -} - -/* ************************************************************************* */ -Class expandClassTemplate(const Class& cls, const string& templateArg, - const vector& instName, - const std::vector& expandedClassNamespace, - const string& expandedClassName) { - Class inst; - inst.name = cls.name; - inst.templateArgs = cls.templateArgs; - inst.typedefName = cls.typedefName; - inst.isVirtual = cls.isVirtual; - inst.isSerializable = cls.isSerializable; - inst.qualifiedParent = cls.qualifiedParent; - inst.methods = expandMethodTemplate(cls.methods, templateArg, instName, - expandedClassNamespace, expandedClassName); - inst.static_methods = expandMethodTemplate(cls.static_methods, templateArg, - instName, expandedClassNamespace, expandedClassName); - inst.namespaces = cls.namespaces; - inst.constructor = cls.constructor; - inst.constructor.args_list = expandArgumentListsTemplate( - cls.constructor.args_list, templateArg, instName, expandedClassNamespace, - expandedClassName); - inst.constructor.name = inst.name; - inst.deconstructor = cls.deconstructor; +Class Class::expandTemplate(const TemplateSubstitution& ts) const { + Class inst = *this; + inst.methods = expandMethodTemplate(methods, ts); + inst.static_methods = expandMethodTemplate(static_methods, ts); + inst.constructor = constructor.expandTemplate(ts); inst.deconstructor.name = inst.name; - inst.verbose_ = cls.verbose_; return inst; } /* ************************************************************************* */ -vector Class::expandTemplate(const string& templateArg, - const vector >& instantiations) const { +vector Class::expandTemplate(Str templateArg, + const vector& instantiations) const { vector result; - BOOST_FOREACH(const vector& instName, instantiations) { - const string expandedName = name + instName.back(); - Class inst = expandClassTemplate(*this, templateArg, instName, - this->namespaces, expandedName); - inst.name = expandedName; + BOOST_FOREACH(const Qualified& instName, instantiations) { + Qualified expandedClass = (Qualified) (*this); + expandedClass.name += instName.name; + const TemplateSubstitution ts(templateArg, instName, expandedClass); + Class inst = expandTemplate(ts); + inst.name = expandedClass.name; inst.templateArgs.clear(); - inst.typedefName = qualifiedName("::") + "<" - + wrap::qualifiedName("::", instName) + ">"; + inst.typedefName = qualifiedName("::") + "<" + instName.qualifiedName("::") + + ">"; result.push_back(inst); } return result; } /* ************************************************************************* */ -Class Class::expandTemplate(const string& templateArg, - const vector& instantiation, - const std::vector& expandedClassNamespace, - const string& expandedClassName) const { - return expandClassTemplate(*this, templateArg, instantiation, - expandedClassNamespace, expandedClassName); +void Class::addMethod(bool verbose, bool is_const, Str methodName, + const ArgumentList& argumentList, const ReturnValue& returnValue, + Str templateArgName, const vector& templateArgValues) { + // Check if templated + if (!templateArgName.empty() && templateArgValues.size() > 0) { + // 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); + // 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(verbose, is_const, methodName, + expandedArgs, expandedRetVal, instName); + } + } else + // just add overload + methods[methodName].addOverload(verbose, is_const, methodName, argumentList, + returnValue); } /* ************************************************************************* */ -std::string Class::getTypedef() const { +void Class::erase_serialization() { + 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); + } + + 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); + } +} + +/* ************************************************************************* */ +void Class::verifyAll(vector& validTypes, bool& hasSerialiable) const { + + hasSerialiable |= isSerializable; + + // verify all of the function arguments + //TODO:verifyArguments(validTypes, constructor.args_list); + verifyArguments(validTypes, static_methods); + verifyArguments(validTypes, methods); + + // verify function return types + verifyReturnTypes(validTypes, static_methods); + verifyReturnTypes(validTypes, methods); + + // verify parents + if (!qualifiedParent.empty() + && find(validTypes.begin(), validTypes.end(), + qualifiedParent.qualifiedName("::")) == validTypes.end()) + throw DependencyMissing(qualifiedParent.qualifiedName("::"), + qualifiedName("::")); +} + +/* ************************************************************************* */ +void Class::appendInheritedMethods(const Class& cls, + const vector& classes) { + + if (!cls.qualifiedParent.empty()) { + + // 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()); + appendInheritedMethods(parent, classes); + } + } + } +} + +/* ************************************************************************* */ +string Class::getTypedef() const { string result; - BOOST_FOREACH(const string& namesp, namespaces) { + BOOST_FOREACH(Str namesp, namespaces) { result += ("namespace " + namesp + " { "); } result += ("typedef " + typedefName + " " + name + ";"); @@ -379,43 +372,22 @@ std::string Class::getTypedef() const { } /* ************************************************************************* */ - void Class::comment_fragment(FileWriter& proxyFile) const { 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"; - if (!constructor.args_list.empty()) - proxyFile.oss << "%\n%-------Constructors-------\n"; - BOOST_FOREACH(ArgumentList argList, constructor.args_list) { - proxyFile.oss << "%"; - argList.emit_prototype(proxyFile, name); - proxyFile.oss << "\n"; - } + constructor.comment_fragment(proxyFile); if (!methods.empty()) proxyFile.oss << "%\n%-------Methods-------\n"; - BOOST_FOREACH(const Methods::value_type& name_m, methods) { - const Method& m = name_m.second; - BOOST_FOREACH(ArgumentList argList, m.argLists) { - proxyFile.oss << "%"; - argList.emit_prototype(proxyFile, m.name); - proxyFile.oss << " : returns " - << m.returnVals[0].return_type(false, m.returnVals[0].pair) << endl; - } - } + BOOST_FOREACH(const Methods::value_type& name_m, methods) + name_m.second.comment_fragment(proxyFile); if (!static_methods.empty()) proxyFile.oss << "%\n%-------Static Methods-------\n"; - BOOST_FOREACH(const StaticMethods::value_type& name_m, static_methods) { - const StaticMethod& m = name_m.second; - BOOST_FOREACH(ArgumentList argList, m.argLists) { - proxyFile.oss << "%"; - argList.emit_prototype(proxyFile, m.name); - proxyFile.oss << " : returns " - << m.returnVals[0].return_type(false, m.returnVals[0].pair) << endl; - } - } + BOOST_FOREACH(const StaticMethods::value_type& name_m, static_methods) + name_m.second.comment_fragment(proxyFile); if (hasSerialization) { proxyFile.oss << "%\n%-------Serialization Interface-------\n"; @@ -429,23 +401,24 @@ void Class::comment_fragment(FileWriter& proxyFile) const { /* ************************************************************************* */ void Class::serialization_fragments(FileWriter& proxyFile, - FileWriter& wrapperFile, const std::string& wrapperName, - std::vector& functionNames) const { + FileWriter& wrapperFile, Str wrapperName, + vector& functionNames) const { //void Point3_string_serialize_17(int nargout, mxArray *out[], int nargin, const mxArray *in[]) //{ // typedef boost::shared_ptr Shared; // checkArguments("string_serialize",nargout,nargin-1,0); // Shared obj = unwrap_shared_ptr(in[0], "ptr_Point3"); -// std::ostringstream out_archive_stream; +// ostringstream out_archive_stream; // boost::archive::text_oarchive out_archive(out_archive_stream); // out_archive << *obj; // out[0] = wrap< string >(out_archive_stream.str()); //} int serialize_id = functionNames.size(); - const string matlabQualName = qualifiedName("."), matlabUniqueName = - qualifiedName(), cppClassName = qualifiedName("::"); + const string matlabQualName = qualifiedName("."); + const string matlabUniqueName = qualifiedName(); + const string cppClassName = qualifiedName("::"); const string wrapFunctionNameSerialize = matlabUniqueName + "_string_serialize_" + boost::lexical_cast(serialize_id); functionNames.push_back(wrapFunctionNameSerialize); @@ -469,7 +442,7 @@ void Class::serialization_fragments(FileWriter& proxyFile, << ">(in[0], \"ptr_" << matlabUniqueName << "\");" << endl; // Serialization boilerplate - wrapperFile.oss << " std::ostringstream out_archive_stream;\n"; + wrapperFile.oss << " ostringstream out_archive_stream;\n"; wrapperFile.oss << " boost::archive::text_oarchive out_archive(out_archive_stream);\n"; wrapperFile.oss << " out_archive << *obj;\n"; @@ -520,22 +493,23 @@ void Class::serialization_fragments(FileWriter& proxyFile, /* ************************************************************************* */ void Class::deserialization_fragments(FileWriter& proxyFile, - FileWriter& wrapperFile, const std::string& wrapperName, - std::vector& functionNames) const { + FileWriter& wrapperFile, Str wrapperName, + vector& functionNames) const { //void Point3_string_deserialize_18(int nargout, mxArray *out[], int nargin, const mxArray *in[]) //{ // typedef boost::shared_ptr Shared; // checkArguments("Point3.string_deserialize",nargout,nargin,1); // string serialized = unwrap< string >(in[0]); - // std::istringstream in_archive_stream(serialized); + // istringstream in_archive_stream(serialized); // boost::archive::text_iarchive in_archive(in_archive_stream); // Shared output(new Point3()); // in_archive >> *output; // out[0] = wrap_shared_ptr(output,"Point3", false); //} int deserialize_id = functionNames.size(); - const string matlabQualName = qualifiedName("."), matlabUniqueName = - qualifiedName(), cppClassName = qualifiedName("::"); + const string matlabQualName = qualifiedName("."); + const string matlabUniqueName = qualifiedName(); + const string cppClassName = qualifiedName("::"); const string wrapFunctionNameDeserialize = matlabUniqueName + "_string_deserialize_" + boost::lexical_cast(deserialize_id); functionNames.push_back(wrapFunctionNameDeserialize); @@ -553,7 +527,7 @@ void Class::deserialization_fragments(FileWriter& proxyFile, // string argument with deserialization boilerplate wrapperFile.oss << " string serialized = unwrap< string >(in[0]);\n"; - wrapperFile.oss << " std::istringstream in_archive_stream(serialized);\n"; + wrapperFile.oss << " istringstream in_archive_stream(serialized);\n"; wrapperFile.oss << " boost::archive::text_iarchive in_archive(in_archive_stream);\n"; wrapperFile.oss << " Shared output(new " << cppClassName << "());\n"; @@ -604,7 +578,7 @@ void Class::deserialization_fragments(FileWriter& proxyFile, } /* ************************************************************************* */ -std::string Class::getSerializationExport() const { +string Class::getSerializationExport() const { //BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsamSharedDiagonal"); return "BOOST_CLASS_EXPORT_GUID(" + qualifiedName("::") + ", \"" + qualifiedName() + "\");"; diff --git a/wrap/Class.h b/wrap/Class.h index 2ca976f66..6b9119316 100644 --- a/wrap/Class.h +++ b/wrap/Class.h @@ -19,67 +19,116 @@ #pragma once -#include -#include - #include "Constructor.h" #include "Deconstructor.h" #include "Method.h" #include "StaticMethod.h" #include "TypeAttributesTable.h" +#include +#include + +#include +#include + namespace wrap { /// Class has name, constructors, methods -struct Class { +class Class: public Qualified { + typedef std::map Methods; + Methods methods; ///< Class methods + +public: + + typedef const std::string& Str; typedef std::map StaticMethods; - /// Constructor creates an empty class - Class(bool verbose=true) : isVirtual(false), isSerializable(false), hasSerialization(false), verbose_(verbose) {} - // Then the instance variables are set directly by the Module constructor - std::string name; ///< Class name - std::vector templateArgs; ///< Template arguments - std::string typedefName; ///< The name to typedef *from*, if this class is actually a typedef, i.e. typedef [typedefName] [name] - 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 - std::vector qualifiedParent; ///< The *single* parent - the last string is the parent class name, preceededing elements are a namespace stack - Methods methods; ///< Class methods - StaticMethods static_methods; ///< Static methods - std::vector namespaces; ///< Stack of namespaces - Constructor constructor; ///< Class constructors - Deconstructor deconstructor; ///< Deconstructor to deallocate C++ object - bool verbose_; ///< verbose flag + std::vector templateArgs; ///< Template arguments + std::string typedefName; ///< The name to typedef *from*, if this class is actually a typedef, i.e. typedef [typedefName] [name] + 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) { + } + + size_t nrMethods() const { + return methods.size(); + } + Method& method(Str name) { + return methods.at(name); + } + bool exists(Str name) const { + return methods.find(name) != methods.end(); + } // And finally MATLAB code is emitted, methods below called by Module::matlab_code - void matlab_proxy(const std::string& toolboxPath, const std::string& wrapperName, const TypeAttributesTable& typeAttributes, - FileWriter& wrapperFile, std::vector& functionNames) const; ///< emit proxy class + void matlab_proxy(Str toolboxPath, Str wrapperName, + const TypeAttributesTable& typeAttributes, FileWriter& wrapperFile, + std::vector& functionNames) const; ///< emit proxy class - std::string qualifiedName(const std::string& delim = "") const; ///< creates a namespace-qualified name, optional delimiter + Class expandTemplate(const TemplateSubstitution& ts) const; - std::vector expandTemplate(const std::string& templateArg, const std::vector >& instantiations) const; + std::vector expandTemplate(Str templateArg, + const std::vector& instantiations) const; - Class expandTemplate(const std::string& templateArg, const std::vector& instantiation, const std::vector& expandedClassNamespace, const std::string& expandedClassName) const; + /// 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& templateArgValues); - // The typedef line for this class, if this class is a typedef, otherwise returns an empty string. + /// Post-process classes for serialization markers + void erase_serialization(); // non-const ! + + /// verify all of the function arguments + void verifyAll(std::vector& functionNames, + bool& hasSerialiable) const; + + void appendInheritedMethods(const Class& cls, + const std::vector& classes); + + /// The typedef line for this class, if this class is a typedef, otherwise returns an empty string. std::string getTypedef() const; - // Returns the string for an export flag + /// Returns the string for an export flag std::string getSerializationExport() const; - // Creates a member function that performs serialization + /// Creates a member function that performs serialization void serialization_fragments(FileWriter& proxyFile, FileWriter& wrapperFile, - const std::string& wrapperName, std::vector& functionNames) const; + Str wrapperName, std::vector& functionNames) const; - // Creates a static member function that performs deserialization + /// Creates a static member function that performs deserialization void deserialization_fragments(FileWriter& proxyFile, FileWriter& wrapperFile, - const std::string& wrapperName, std::vector& functionNames) const; + Str wrapperName, std::vector& functionNames) const; + + friend std::ostream& operator<<(std::ostream& os, const Class& cls) { + 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) + os << m << ";\n"; + os << "};" << std::endl; + return os; + } private: - void pointer_constructor_fragments(FileWriter& proxyFile, FileWriter& wrapperFile, const std::string& wrapperName, std::vector& functionNames) const; - void comment_fragment(FileWriter& proxyFile) const; + + void pointer_constructor_fragments(FileWriter& proxyFile, + FileWriter& wrapperFile, Str wrapperName, + std::vector& functionNames) const; + + void comment_fragment(FileWriter& proxyFile) const; }; } // \namespace wrap diff --git a/wrap/Constructor.h b/wrap/Constructor.h index 5438c515c..adfcb8472 100644 --- a/wrap/Constructor.h +++ b/wrap/Constructor.h @@ -18,7 +18,7 @@ #pragma once -#include "Argument.h" +#include "Function.h" #include #include @@ -26,7 +26,7 @@ namespace wrap { // Constructor class -struct Constructor { +struct Constructor: public ArgumentOverloads { /// Constructor creates an empty class Constructor(bool verbose = false) : @@ -34,10 +34,16 @@ struct Constructor { } // Then the instance variables are set directly by the Module constructor - std::vector args_list; std::string name; bool verbose_; + Constructor expandTemplate(const TemplateSubstitution& ts) const { + Constructor inst = *this; + inst.argLists_ = expandArgumentListsTemplate(ts); + inst.name = ts.expandedClassName(); + return inst; + } + // MATLAB code generation // toolboxPath is main toolbox directory, e.g., ../matlab // classFile is class proxy file, e.g., ../matlab/@Point2/Point2.m @@ -45,6 +51,16 @@ struct Constructor { /// wrapper name std::string matlab_wrapper_name(const std::string& className) const; + void comment_fragment(FileWriter& proxyFile) const { + if (nrOverloads() > 0) + proxyFile.oss << "%\n%-------Constructors-------\n"; + for (size_t i = 0; i < nrOverloads(); i++) { + proxyFile.oss << "%"; + argumentList(i).emit_prototype(proxyFile, name); + proxyFile.oss << "\n"; + } + } + /** * Create fragment to select constructor in proxy class, e.g., * if nargin == 2, obj.self = new_Pose3_RP(varargin{1},varargin{2}); end @@ -62,6 +78,12 @@ struct Constructor { void generate_construct(FileWriter& file, const std::string& cppClassName, std::vector& args_list) const; + friend std::ostream& operator<<(std::ostream& os, const Constructor& m) { + for (size_t i = 0; i < m.nrOverloads(); i++) + os << m.name << m.argLists_[i]; + return os; + } + }; } // \namespace wrap diff --git a/wrap/FileWriter.cpp b/wrap/FileWriter.cpp index 3f5461078..783661819 100644 --- a/wrap/FileWriter.cpp +++ b/wrap/FileWriter.cpp @@ -15,14 +15,15 @@ using namespace std; using namespace wrap; /* ************************************************************************* */ -FileWriter::FileWriter(const string& filename, bool verbose, const string& comment_str) -: verbose_(verbose),filename_(filename), comment_str_(comment_str) -{ +FileWriter::FileWriter(const string& filename, bool verbose, + const string& comment_str) : + verbose_(verbose), filename_(filename), comment_str_(comment_str) { } /* ************************************************************************* */ void FileWriter::emit(bool add_header, bool force_overwrite) const { - if (verbose_) cerr << "generating " << filename_ << " "; + if (verbose_) + cerr << "generating " << filename_ << " "; // read in file if it exists string existing_contents; bool file_exists = true; @@ -35,23 +36,17 @@ void FileWriter::emit(bool add_header, bool force_overwrite) const { // Only write a file if it is new, an update, or overwrite is forced string new_contents = oss.str(); if (force_overwrite || !file_exists || existing_contents != new_contents) { - ofstream ofs(filename_.c_str(), ios::binary); // Binary to use LF line endings instead of CRLF - if (!ofs) throw CantOpenFile(filename_); + // Binary to use LF line endings instead of CRLF + ofstream ofs(filename_.c_str(), ios::binary); + if (!ofs) + throw CantOpenFile(filename_); // dump in stringstream ofs << new_contents; ofs.close(); - if (verbose_) cerr << " ...complete" << endl; - - // Add small message whenever writing a new file and not running in full verbose mode - if (!verbose_) - cout << "wrap: generating " << filename_ << endl; - } else { - if (verbose_) cerr << " ...no update" << endl; } + if (verbose_) + cerr << " ...no update" << endl; } /* ************************************************************************* */ - - - diff --git a/wrap/Function.cpp b/wrap/Function.cpp new file mode 100644 index 000000000..ab3958c62 --- /dev/null +++ b/wrap/Function.cpp @@ -0,0 +1,66 @@ +/* ---------------------------------------------------------------------------- + + * 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 Function.ccp + * @author Frank Dellaert + * @date Nov 13, 2014 + **/ + +#include "Function.h" +#include "utilities.h" + +#include +#include +#include + +#include +#include + +using namespace std; +using namespace wrap; + +/* ************************************************************************* */ +void Function::addOverload(bool verbose, const std::string& name, + const Qualified& instName) { + + // Check if this overload is give to the correct method + if (name_.empty()) + name_ = name; + else if (name_ != name) + throw std::runtime_error( + "Function::addOverload: tried to add overload with name " + name + + " instead of expected " + name_); + + // Check if this overload is give to the correct method + if (templateArgValue_.empty()) + templateArgValue_ = instName; + else if (templateArgValue_ != instName) + throw std::runtime_error( + "Function::addOverload: tried to add overload with template argument " + + instName.qualifiedName(":") + " instead of expected " + + templateArgValue_.qualifiedName(":")); + + verbose_ = verbose; +} + +/* ************************************************************************* */ +vector ArgumentOverloads::expandArgumentListsTemplate( + const TemplateSubstitution& ts) const { + vector result; + BOOST_FOREACH(const ArgumentList& argList, argLists_) { + ArgumentList instArgList = argList.expandTemplate(ts); + result.push_back(instArgList); + } + return result; +} + +/* ************************************************************************* */ diff --git a/wrap/Function.h b/wrap/Function.h new file mode 100644 index 000000000..1d40fcbea --- /dev/null +++ b/wrap/Function.h @@ -0,0 +1,228 @@ +/* ---------------------------------------------------------------------------- + + * 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 Function.h + * @brief Base class for global functions and methods + * @author Frank Dellaert + * @date Nov 13, 2014 + **/ + +#pragma once + +#include "Argument.h" +#include "ReturnValue.h" +#include "TypeAttributesTable.h" + +#include +#include + +namespace wrap { + +/// Function class +class Function { + +protected: + + bool verbose_; + std::string name_; ///< name of method + Qualified templateArgValue_; ///< value of template argument if applicable + +public: + + /// Constructor creates empty object + Function(bool verbose = true) : + verbose_(verbose) { + } + + Function(const std::string& name, bool verbose = true) : + verbose_(verbose), name_(name) { + } + + std::string name() const { + return name_; + } + + std::string matlabName() const { + if (templateArgValue_.empty()) + return name_; + else + return name_ + templateArgValue_.name; + } + + // The first time this function is called, it initializes the class members + // with those in rhs, but in subsequent calls it adds additional argument + // lists as function overloads. + void addOverload(bool verbose, const std::string& name, + const Qualified& instName = Qualified()); +}; + +/** + * ArgumentList Overloads + */ +class ArgumentOverloads { + +protected: + + std::vector argLists_; + +public: + + size_t nrOverloads() const { + return argLists_.size(); + } + + const ArgumentList& argumentList(size_t i) const { + return argLists_.at(i); + } + + void addOverload(const ArgumentList& args) { + argLists_.push_back(args); + } + + std::vector expandArgumentListsTemplate( + const TemplateSubstitution& ts) const; + + /// Expand templates, imperative ! + virtual void ExpandTemplate(const TemplateSubstitution& ts) { + argLists_ = expandArgumentListsTemplate(ts); + } + + void verifyArguments(const std::vector& validArgs, + const std::string s) const { + BOOST_FOREACH(const ArgumentList& argList, argLists_) { + BOOST_FOREACH(Argument arg, argList) { + std::string fullType = arg.type.qualifiedName("::"); + if (find(validArgs.begin(), validArgs.end(), fullType) + == validArgs.end()) + throw DependencyMissing(fullType, "checking argument of " + s); + } + } + } + + friend std::ostream& operator<<(std::ostream& os, + const ArgumentOverloads& overloads) { + BOOST_FOREACH(const ArgumentList& argList, overloads.argLists_) + os << argList << std::endl; + return os; + } + +}; + +/** + * Signature Overload (including return value) + */ +class SignatureOverloads: public ArgumentOverloads { + +protected: + + std::vector returnVals_; + +public: + + const ReturnValue& returnValue(size_t i) const { + return returnVals_.at(i); + } + + void addOverload(const ArgumentList& args, const ReturnValue& retVal) { + argLists_.push_back(args); + returnVals_.push_back(retVal); + } + + void verifyReturnTypes(const std::vector& validtypes, + const std::string& s) const { + BOOST_FOREACH(const ReturnValue& retval, returnVals_) { + retval.type1.verify(validtypes, s); + if (retval.isPair) + retval.type2.verify(validtypes, s); + } + } + + // TODO use transform ? + std::vector expandReturnValuesTemplate( + const TemplateSubstitution& ts) const { + std::vector result; + BOOST_FOREACH(const ReturnValue& retVal, returnVals_) { + ReturnValue instRetVal = retVal.expandTemplate(ts); + result.push_back(instRetVal); + } + return result; + } + + /// Expand templates, imperative ! + void expandTemplate(const TemplateSubstitution& ts) { + // substitute template in arguments + argLists_ = expandArgumentListsTemplate(ts); + // do the same for return types + returnVals_ = expandReturnValuesTemplate(ts); + } + + // emit a list of comments, one for each overload + void usage_fragment(FileWriter& proxyFile, const std::string& name) const { + unsigned int argLCount = 0; + BOOST_FOREACH(ArgumentList argList, argLists_) { + argList.emit_prototype(proxyFile, name); + if (argLCount != nrOverloads() - 1) + proxyFile.oss << ", "; + else + proxyFile.oss << " : returns " << returnValue(0).return_type(false) + << std::endl; + argLCount++; + } + } + + // emit a list of comments, one for each overload + void comment_fragment(FileWriter& proxyFile, const std::string& name) const { + size_t i = 0; + BOOST_FOREACH(ArgumentList argList, argLists_) { + proxyFile.oss << "%"; + argList.emit_prototype(proxyFile, name); + proxyFile.oss << " : returns " << returnVals_[i++].return_type(false) + << std::endl; + } + } + + friend std::ostream& operator<<(std::ostream& os, + const SignatureOverloads& overloads) { + for (size_t i = 0; i < overloads.nrOverloads(); i++) + os << overloads.returnVals_[i] << overloads.argLists_[i] << std::endl; + return os; + } + +}; + +// Templated checking functions +// TODO: do this via polymorphism ? + +// TODO use transform +template +static std::map expandMethodTemplate( + const std::map& methods, const TemplateSubstitution& ts) { + std::map result; + typedef std::pair NamedMethod; + BOOST_FOREACH(NamedMethod namedMethod, methods) { + F instMethod = namedMethod.second; + instMethod.expandTemplate(ts); + namedMethod.second = instMethod; + result.insert(namedMethod); + } + return result; +} +template +inline void verifyReturnTypes(const std::vector& validTypes, + const std::map& vt) { + typedef typename std::map::value_type NamedMethod; + BOOST_FOREACH(const NamedMethod& namedMethod, vt) + namedMethod.second.verifyReturnTypes(validTypes); +} + +} // \namespace wrap + diff --git a/wrap/GlobalFunction.cpp b/wrap/GlobalFunction.cpp index 25e1dcedb..1f9d6518e 100644 --- a/wrap/GlobalFunction.cpp +++ b/wrap/GlobalFunction.cpp @@ -16,14 +16,12 @@ namespace wrap { using namespace std; /* ************************************************************************* */ -void GlobalFunction::addOverload(bool verbose, const std::string& name, +void GlobalFunction::addOverload(bool verbose, const Qualified& overload, const ArgumentList& args, const ReturnValue& retVal, - const StrVec& ns_stack) { - this->verbose_ = verbose; - this->name = name; - this->argLists.push_back(args); - this->returnVals.push_back(retVal); - this->namespaces.push_back(ns_stack); + const Qualified& instName) { + Function::addOverload(verbose, overload.name, instName); + SignatureOverloads::addOverload(args, retVal); + overloads.push_back(overload); } /* ************************************************************************* */ @@ -36,18 +34,14 @@ void GlobalFunction::matlab_proxy(const std::string& toolboxPath, // map of namespace to global function typedef map GlobalFunctionMap; GlobalFunctionMap grouped_functions; - for (size_t i = 0; i < namespaces.size(); ++i) { - StrVec ns = namespaces.at(i); - string str_ns = qualifiedName("", ns, ""); - ReturnValue ret = returnVals.at(i); - ArgumentList args = argLists.at(i); - - if (!grouped_functions.count(str_ns)) - grouped_functions[str_ns] = GlobalFunction(name, verbose_); - - grouped_functions[str_ns].argLists.push_back(args); - grouped_functions[str_ns].returnVals.push_back(ret); - grouped_functions[str_ns].namespaces.push_back(ns); + 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); + const ReturnValue& ret = returnValue(i); + const ArgumentList& args = argumentList(i); + grouped_functions[str_ns].addOverload(verbose_, overload, args, ret, + templateArgValue_); } size_t lastcheck = grouped_functions.size(); @@ -65,32 +59,29 @@ void GlobalFunction::generateSingleFunction(const std::string& toolboxPath, FileWriter& file, std::vector& functionNames) const { // create the folder for the namespace - const StrVec& ns = namespaces.front(); - createNamespaceStructure(ns, toolboxPath); + const Qualified& overload1 = overloads.front(); + createNamespaceStructure(overload1.namespaces, toolboxPath); // open destination mfunctionFileName - string mfunctionFileName = toolboxPath; - if (!ns.empty()) - mfunctionFileName += "/+" + wrap::qualifiedName("/+", ns); - mfunctionFileName += "/" + name + ".m"; + string mfunctionFileName = overload1.matlabName(toolboxPath); FileWriter mfunctionFile(mfunctionFileName, verbose_, "%"); // get the name of actual matlab object - const string matlabQualName = qualifiedName(".", ns, name), matlabUniqueName = - qualifiedName("", ns, name), cppName = qualifiedName("::", ns, name); + const string matlabQualName = overload1.qualifiedName("."); + const string matlabUniqueName = overload1.qualifiedName(""); + const string cppName = overload1.qualifiedName("::"); - mfunctionFile.oss << "function varargout = " << name << "(varargin)\n"; + mfunctionFile.oss << "function varargout = " << name_ << "(varargin)\n"; - for (size_t overload = 0; overload < argLists.size(); ++overload) { - const ArgumentList& args = argLists[overload]; - const ReturnValue& returnVal = returnVals[overload]; + for (size_t i = 0; i < nrOverloads(); ++i) { + const ArgumentList& args = argumentList(i); + const ReturnValue& returnVal = returnValue(i); const int id = functionNames.size(); // Output proxy matlab code - mfunctionFile.oss << " " << (overload == 0 ? "" : "else"); - argLists[overload].emit_conditional_call(mfunctionFile, - returnVals[overload], wrapperName, id, true); // true omits "this" + mfunctionFile.oss << " " << (i == 0 ? "" : "else"); + args.emit_conditional_call(mfunctionFile, returnVal, wrapperName, id, true); // true omits "this" // Output C++ wrapper code @@ -114,7 +105,7 @@ void GlobalFunction::generateSingleFunction(const std::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 != "void") + if (returnVal.type1.name != "void") returnVal.wrap_result(cppName + "(" + args.names() + ")", file, typeAttributes); else diff --git a/wrap/GlobalFunction.h b/wrap/GlobalFunction.h index 6c8ad0c86..18bb91995 100644 --- a/wrap/GlobalFunction.h +++ b/wrap/GlobalFunction.h @@ -9,37 +9,36 @@ #pragma once -#include "Argument.h" -#include "ReturnValue.h" +#include "Function.h" namespace wrap { -struct GlobalFunction { +struct GlobalFunction: public Function, public SignatureOverloads { - typedef std::vector StrVec; - - bool verbose_; - std::string name; - - // each overload, regardless of namespace - std::vector argLists; ///< arugments for each overload - std::vector returnVals; ///< returnVals for each overload - std::vector namespaces; ///< Stack of namespaces + std::vector overloads; ///< Stack of qualified names // Constructor only used in Module GlobalFunction(bool verbose = true) : - verbose_(verbose) { + Function(verbose) { } // Used to reconstruct - GlobalFunction(const std::string& name_, bool verbose = true) : - verbose_(verbose), name(name_) { + GlobalFunction(const std::string& name, bool verbose = true) : + Function(name,verbose) { } - // adds an overloaded version of this function - void addOverload(bool verbose, const std::string& name, + void verifyArguments(const std::vector& validArgs) const { + SignatureOverloads::verifyArguments(validArgs, name_); + } + + void verifyReturnTypes(const std::vector& validtypes) const { + SignatureOverloads::verifyReturnTypes(validtypes, name_); + } + + // adds an overloaded version of this function, + void addOverload(bool verbose, const Qualified& overload, const ArgumentList& args, const ReturnValue& retVal, - const StrVec& ns_stack); + const Qualified& instName = Qualified()); // codegen function called from Module to build the cpp and matlab versions of the function void matlab_proxy(const std::string& toolboxPath, diff --git a/wrap/Method.cpp b/wrap/Method.cpp index 7b88b9cdc..a7072c9e7 100644 --- a/wrap/Method.cpp +++ b/wrap/Method.cpp @@ -29,155 +29,45 @@ using namespace std; using namespace wrap; /* ************************************************************************* */ -void Method::addOverload(bool verbose, bool is_const, const std::string& name, - const ArgumentList& args, const ReturnValue& retVal) { - this->verbose_ = verbose; - this->is_const_ = is_const; - this->name = name; - this->argLists.push_back(args); - this->returnVals.push_back(retVal); +void Method::addOverload(bool verbose, bool is_const, Str name, + const ArgumentList& args, const ReturnValue& retVal, + const Qualified& instName) { + + StaticMethod::addOverload(verbose, name, args, retVal, instName); + is_const_ = is_const; } /* ************************************************************************* */ -void Method::proxy_wrapper_fragments(FileWriter& file, FileWriter& wrapperFile, - const string& cppClassName, const std::string& matlabQualName, - const std::string& matlabUniqueName, const string& wrapperName, - const TypeAttributesTable& typeAttributes, - vector& functionNames) const { - - // Create function header - file.oss << " function varargout = " << name << "(this, varargin)\n"; - - // Emit comments for documentation - string up_name = boost::to_upper_copy(name); - file.oss << " % " << up_name << " usage: "; - unsigned int argLCount = 0; - BOOST_FOREACH(ArgumentList argList, argLists) { - argList.emit_prototype(file, name); - if (argLCount != argLists.size() - 1) - file.oss << ", "; - else - file.oss << " : returns " - << returnVals[0].return_type(false, returnVals[0].pair) << endl; - argLCount++; - } - - // Emit URL to Doxygen page - file.oss << " % " - << "Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html" - << endl; - - // Document all overloads, if any - if (argLists.size() > 1) { - file.oss << " % " << "" << endl; - file.oss << " % " << "Method Overloads" << endl; - BOOST_FOREACH(ArgumentList argList, argLists) { - file.oss << " % "; - argList.emit_prototype(file, name); - file.oss << endl; - } - } - - // Handle special case of single overload with all numeric arguments - if (argLists.size() == 1 && argLists[0].allScalar()) { - // Output proxy matlab code - file.oss << " "; - const int id = (int) functionNames.size(); - argLists[0].emit_call(file, returnVals[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 overload = 0; overload < argLists.size(); ++overload) { - - // Output proxy matlab code - file.oss << " " << (overload == 0 ? "" : "else"); - const int id = (int) functionNames.size(); - argLists[overload].emit_conditional_call(file, returnVals[overload], - wrapperName, id); - - // Output C++ wrapper code - const string wrapFunctionName = wrapper_fragment(wrapperFile, - cppClassName, matlabUniqueName, overload, id, typeAttributes); - - // Add to function list - functionNames.push_back(wrapFunctionName); - } - file.oss << " else\n"; - file.oss - << " error('Arguments do not match any overload of function " - << matlabQualName << "." << name << "');" << endl; - file.oss << " end\n"; - } - - file.oss << " end\n"; +void Method::proxy_header(FileWriter& proxyFile) const { + proxyFile.oss << " function varargout = " << matlabName() << "(this, varargin)\n"; } /* ************************************************************************* */ -string Method::wrapper_fragment(FileWriter& file, const string& cppClassName, - const string& matlabUniqueName, int overload, int id, - const TypeAttributesTable& typeAttributes) const { - - // generate code - - const string wrapFunctionName = matlabUniqueName + "_" + name + "_" - + boost::lexical_cast(id); - - const ArgumentList& args = argLists[overload]; - const ReturnValue& returnVal = returnVals[overload]; - - // call - file.oss << "void " << wrapFunctionName - << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; - // start - file.oss << "{\n"; - - if (returnVal.isPair) { - if (returnVal.category1 == ReturnValue::CLASS) - file.oss << " typedef boost::shared_ptr<" - << returnVal.qualifiedType1("::") << "> Shared" << returnVal.type1 - << ";" << endl; - if (returnVal.category2 == ReturnValue::CLASS) - file.oss << " typedef boost::shared_ptr<" - << returnVal.qualifiedType2("::") << "> Shared" << returnVal.type2 - << ";" << endl; - } else if (returnVal.category1 == ReturnValue::CLASS) - file.oss << " typedef boost::shared_ptr<" << returnVal.qualifiedType1("::") - << "> Shared" << returnVal.type1 << ";" << endl; - - file.oss << " typedef boost::shared_ptr<" << cppClassName << "> Shared;" - << endl; - +string Method::wrapper_call(FileWriter& wrapperFile, Str cppClassName, + Str matlabUniqueName, const ArgumentList& args, + const ReturnValue& returnVal, const TypeAttributesTable& typeAttributes, + const Qualified& instName) const { // check arguments // extra argument obj -> nargin-1 is passed ! // example: checkArguments("equals",nargout,nargin-1,2); - file.oss << " checkArguments(\"" << name << "\",nargout,nargin-1," + wrapperFile.oss << " checkArguments(\"" << name_ << "\",nargout,nargin-1," << args.size() << ");\n"; // get class pointer // example: shared_ptr = unwrap_shared_ptr< Test >(in[0], "Test"); - file.oss << " Shared obj = unwrap_shared_ptr<" << cppClassName + wrapperFile.oss << " Shared obj = unwrap_shared_ptr<" << cppClassName << ">(in[0], \"ptr_" << matlabUniqueName << "\");" << endl; - // unwrap arguments, see Argument.cpp - args.matlab_unwrap(file, 1); + + // unwrap arguments, see Argument.cpp, we start at 1 as first is obj + args.matlab_unwrap(wrapperFile, 1); // call method and wrap result - // example: out[0]=wrap(self->return_field(t)); - if (returnVal.type1 != "void") - returnVal.wrap_result("obj->" + name + "(" + args.names() + ")", file, - typeAttributes); - else - file.oss << " obj->" + name + "(" + args.names() + ");\n"; + // example: out[0]=wrap(obj->return_field(t)); + string expanded = "obj->" + name_; + if (!instName.empty()) + expanded += ("<" + instName.qualifiedName("::") + ">"); - // finish - file.oss << "}\n"; - - return wrapFunctionName; + return expanded; } /* ************************************************************************* */ diff --git a/wrap/Method.h b/wrap/Method.h index 9926a5179..13847700d 100644 --- a/wrap/Method.h +++ b/wrap/Method.h @@ -18,49 +18,58 @@ #pragma once -#include "Argument.h" -#include "ReturnValue.h" -#include "TypeAttributesTable.h" - -#include -#include +#include "StaticMethod.h" namespace wrap { /// Method class -struct Method { +class Method: public StaticMethod { + + bool is_const_; + +public: + + typedef const std::string& Str; /// Constructor creates empty object Method(bool verbose = true) : - verbose_(verbose), is_const_(false) {} + StaticMethod(verbose), is_const_(false) { + } - // Then the instance variables are set directly by the Module constructor - bool verbose_; - bool is_const_; - std::string name; - std::vector argLists; - std::vector returnVals; + Method(const std::string& name, bool verbose = true) : + StaticMethod(name,verbose), is_const_(false) { + } + + virtual bool isStatic() const { + return false; + } + + virtual bool isConst() const { + return is_const_; + } // The first time this function is called, it initializes the class members // with those in rhs, but in subsequent calls it adds additional argument // lists as function overloads. - void addOverload(bool verbose, bool is_const, const std::string& name, - const ArgumentList& args, const ReturnValue& retVal); + void addOverload(bool verbose, bool is_const, Str name, + const ArgumentList& args, const ReturnValue& retVal, + const Qualified& instName = Qualified()); - // MATLAB code generation - // classPath is class directory, e.g., ../matlab/@Point2 - void proxy_wrapper_fragments(FileWriter& proxyFile, FileWriter& wrapperFile, - const std::string& cppClassName, const std::string& matlabQualName, const std::string& matlabUniqueName, - const std::string& wrapperName, const TypeAttributesTable& typeAttributes, - std::vector& functionNames) const; + friend std::ostream& operator<<(std::ostream& os, const Method& m) { + for (size_t i = 0; i < m.nrOverloads(); i++) + os << m.returnVals_[i] << " " << m.name_ << m.argLists_[i]; + return os; + } private: - std::string wrapper_fragment(FileWriter& file, - const std::string& cppClassName, - const std::string& matlabUniqueName, - int overload, - int id, - const TypeAttributesTable& typeAttributes) const; ///< cpp wrapper + + // Emit method header + 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; }; } // \namespace wrap diff --git a/wrap/Module.cpp b/wrap/Module.cpp index 2cb5ea7ed..2fc8f92bc 100644 --- a/wrap/Module.cpp +++ b/wrap/Module.cpp @@ -62,17 +62,23 @@ typedef rule Rule; /* ************************************************************************* */ /* ************************************************************************* */ -void handle_possible_template(vector& classes, const Class& cls, const string& templateArgument, const vector >& instantiations) { - if(instantiations.empty()) { - classes.push_back(cls); - } else { - vector classInstantiations = cls.expandTemplate(templateArgument, instantiations); - BOOST_FOREACH(const Class& c, classInstantiations) { - classes.push_back(c); - } - } -} - +// If a number of template arguments were given, generate a number of expanded +// class names, e.g., PriorFactor -> PriorFactorPose2, and add those classes +static void handle_possible_template(vector& classes, const Class& cls, + const vector& instantiations) { + if (cls.templateArgs.empty() || instantiations.empty()) { + classes.push_back(cls); + } else { + if (cls.templateArgs.size() != 1) + throw std::runtime_error( + "In-line template instantiations only handle a single template argument"); + vector classInstantiations = // + cls.expandTemplate(cls.templateArgs.front(), instantiations); + BOOST_FOREACH(const Class& c, classInstantiations) + classes.push_back(c); + } +} + /* ************************************************************************* */ Module::Module(const std::string& moduleName, bool enable_verbose) : name(moduleName), verbose(enable_verbose) @@ -94,28 +100,9 @@ Module::Module(const string& interfacePath, /* ************************************************************************* */ void Module::parseMarkup(const std::string& data) { - // these variables will be imperatively updated to gradually build [cls] + // The parse imperatively :-( updates variables gradually during parse // The one with postfix 0 are used to reset the variables after parse. - string methodName, methodName0; - bool isConst, isConst0 = false; - ReturnValue retVal0, retVal; - Argument arg0, arg; - ArgumentList args0, args; - vector arg_dup; ///keep track of duplicates - Constructor constructor0(verbose), constructor(verbose); - Deconstructor deconstructor0(verbose), deconstructor(verbose); - StaticMethod static_method0(verbose), static_method(verbose); - Class cls0(verbose),cls(verbose); - GlobalFunction globalFunc0(verbose), globalFunc(verbose); - ForwardDeclaration fwDec0, fwDec; - vector namespaces, /// current namespace tag - namespaces_return; /// namespace for current return type - string templateArgument; - vector templateInstantiationNamespace; - vector > templateInstantiations; - TemplateInstantiationTypedef singleInstantiation, singleInstantiation0; - string include_path = ""; - const string null_str = ""; + vector namespaces; // current namespace tag //---------------------------------------------------------------------------- // Grammar with actions that build the Class object. Actions are @@ -144,51 +131,63 @@ void Module::parseMarkup(const std::string& data) { Rule namespace_name_p = lexeme_d[lower_p >> *(alnum_p | '_')] - keywords_p; - Rule namespace_arg_p = namespace_name_p[push_back_a(arg.namespaces)] >> str_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)]; + 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)] >> + 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)] >> + 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 + Class cls0(verbose),cls(verbose); Rule classParent_p = - *(namespace_name_p[push_back_a(cls.qualifiedParent)] >> str_p("::")) >> - className_p[push_back_a(cls.qualifiedParent)]; + *(namespace_name_p[push_back_a(cls.qualifiedParent.namespaces)] >> str_p("::")) >> + className_p[assign_a(cls.qualifiedParent.name)]; - Rule templateInstantiation_p = - (*(namespace_name_p[push_back_a(templateInstantiationNamespace)] >> str_p("::")) >> - className_p[push_back_a(templateInstantiationNamespace)]) - [push_back_a(templateInstantiations, templateInstantiationNamespace)] - [clear_a(templateInstantiationNamespace)]; + // parse "gtsam::Pose2" and add to templateArgValues + Qualified templateArgValue; + vector 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)]; - Rule templateInstantiations_p = + // template + string templateArgName; + Rule templateArgValues_p = (str_p("template") >> - '<' >> name_p[assign_a(templateArgument)] >> '=' >> '{' >> - !(templateInstantiation_p >> *(',' >> templateInstantiation_p)) >> - '}' >> '>') - [push_back_a(cls.templateArgs, templateArgument)]; + '<' >> name_p[assign_a(templateArgName)] >> '=' >> + '{' >> !(templateArgValue_p >> *(',' >> templateArgValue_p)) >> '}' >> + '>'); + // parse "gtsam::Pose2" and add to singleInstantiation.typeList + TemplateInstantiationTypedef singleInstantiation; Rule templateSingleInstantiationArg_p = - (*(namespace_name_p[push_back_a(templateInstantiationNamespace)] >> str_p("::")) >> - className_p[push_back_a(templateInstantiationNamespace)]) - [push_back_a(singleInstantiation.typeList, templateInstantiationNamespace)] - [clear_a(templateInstantiationNamespace)]; + (*(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)]; + // typedef gtsam::RangeFactor RangeFactorPosePoint2; + TemplateInstantiationTypedef singleInstantiation0; Rule templateSingleInstantiation_p = (str_p("typedef") >> - *(namespace_name_p[push_back_a(singleInstantiation.classNamespaces)] >> str_p("::")) >> - className_p[assign_a(singleInstantiation.className)] >> + *(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)] >> @@ -197,98 +196,99 @@ void Module::parseMarkup(const std::string& data) { [push_back_a(templateInstantiationTypedefs, singleInstantiation)] [assign_a(singleInstantiation, singleInstantiation0)]; + // template 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)] | argEigenType_p | eigenRef_p | classArg_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) - [push_back_a(constructor.args_list, args)] - [assign_a(args,args0)]; - //[assign_a(constructor.args,args)] - //[assign_a(constructor.name,cls.name)] - //[push_back_a(cls.constructors, constructor)] - //[assign_a(constructor,constructor0)]; + [bl::bind(&Constructor::addOverload, bl::var(constructor), bl::var(args))] + [clear_a(args)]; + vector 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 ReturnValue::return_category RETURN_EIGEN = ReturnValue::EIGEN; - static const ReturnValue::return_category RETURN_BASIS = ReturnValue::BASIS; - static const ReturnValue::return_category RETURN_CLASS = ReturnValue::CLASS; - static const ReturnValue::return_category RETURN_VOID = ReturnValue::VOID; + 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; - Rule returnType1_p = - (basisType_p[assign_a(retVal.type1)][assign_a(retVal.category1, RETURN_BASIS)]) | - ((*namespace_ret_p)[assign_a(retVal.namespaces1, namespaces_return)][clear_a(namespaces_return)] - >> (className_p[assign_a(retVal.type1)][assign_a(retVal.category1, RETURN_CLASS)]) >> - !ch_p('*')[assign_a(retVal.isPtr1,true)]) | - (eigenType_p[assign_a(retVal.type1)][assign_a(retVal.category1, RETURN_EIGEN)]); + 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)]); - Rule returnType2_p = - (basisType_p[assign_a(retVal.type2)][assign_a(retVal.category2, RETURN_BASIS)]) | - ((*namespace_ret_p)[assign_a(retVal.namespaces2, namespaces_return)][clear_a(namespaces_return)] - >> (className_p[assign_a(retVal.type2)][assign_a(retVal.category2, RETURN_CLASS)]) >> - !ch_p('*') [assign_a(retVal.isPtr2,true)]) | - (eigenType_p[assign_a(retVal.type2)][assign_a(retVal.category2, 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)][assign_a(retVal.category1, RETURN_VOID)]; + Rule void_p = str_p("void")[assign_a(retVal.type1.name)][assign_a(retVal.type1.category, RETURN_VOID)]; - Rule returnType_p = void_p | pair_p | returnType1_p; + 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 = - (returnType_p >> methodName_p[assign_a(methodName)] >> + !templateArgValues_p >> + (returnValue_p >> methodName_p[assign_a(methodName)] >> '(' >> argumentList_p >> ')' >> !str_p("const")[assign_a(isConst,true)] >> ';' >> *comments_p) - [bl::bind(&Method::addOverload, - bl::var(cls.methods)[bl::var(methodName)], - verbose, - bl::var(isConst), - bl::var(methodName), - bl::var(args), - bl::var(retVal))] - [assign_a(isConst,isConst0)] - [assign_a(methodName,methodName0)] - [assign_a(args,args0)] - [assign_a(retVal,retVal0)]; + [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") >> returnType_p >> staticMethodName_p[assign_a(methodName)] >> + (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)], - verbose, - bl::var(methodName), - bl::var(args), - bl::var(retVal))] - [assign_a(methodName,methodName0)] - [assign_a(args,args0)] - [assign_a(retVal,retVal0)]; + verbose, bl::var(methodName), bl::var(args), bl::var(retVal), Qualified())] + [assign_a(retVal,retVal0)] + [clear_a(args)]; Rule functions_p = constructor_p | method_p | static_method_p; + // parse a full class + vector templateInstantiations; Rule class_p = - (str_p("")[assign_a(cls,cls0)]) - >> (!(templateInstantiations_p | templateList_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)] @@ -298,28 +298,25 @@ void Module::parseMarkup(const std::string& data) { [assign_a(constructor.name, cls.name)] [assign_a(cls.constructor, constructor)] [assign_a(cls.namespaces, namespaces)] - [assign_a(deconstructor.name,cls.name)] - [assign_a(cls.deconstructor, deconstructor)] - [bl::bind(&handle_possible_template, bl::var(classes), bl::var(cls), bl::var(templateArgument), bl::var(templateInstantiations))] - [assign_a(deconstructor,deconstructor0)] + [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)] - [clear_a(templateArgument)] - [clear_a(templateInstantiations)]; + [assign_a(cls,cls0)]; + // parse a global function + Qualified globalFunction; Rule global_function_p = - (returnType_p >> staticMethodName_p[assign_a(methodName)] >> + (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(methodName)], - verbose, - bl::var(methodName), - bl::var(args), - bl::var(retVal), - bl::var(namespaces))] - [assign_a(methodName,methodName0)] - [assign_a(args,args0)] - [assign_a(retVal,retVal0)]; + bl::var(global_functions)[bl::var(globalFunction.name)], + verbose, bl::var(globalFunction), bl::var(args), bl::var(retVal), Qualified())] + [assign_a(retVal,retVal0)] + [clear_a(globalFunction)] + [clear_a(args)]; Rule include_p = str_p("#include") >> ch_p('<') >> (*(anychar_p - '>'))[push_back_a(includes)] >> ch_p('>'); @@ -327,19 +324,21 @@ void Module::parseMarkup(const std::string& data) { #pragma clang diagnostic push #pragma clang diagnostic ignored "-Wuninitialized" #endif - - Rule namespace_def_p = - (str_p("namespace") - >> namespace_name_p[push_back_a(namespaces)] - >> ch_p('{') - >> *(include_p | class_p | templateSingleInstantiation_p | global_function_p | namespace_def_p | comments_p) - >> ch_p('}')) - [pop_a(namespaces)]; + + Rule namespace_def_p = + (str_p("namespace") + >> namespace_name_p[push_back_a(namespaces)] + >> ch_p('{') + >> *(include_p | class_p | templateSingleInstantiation_p | global_function_p | namespace_def_p | comments_p) + >> ch_p('}')) + [pop_a(namespaces)]; #ifdef __clang__ #pragma clang diagnostic pop #endif + // parse forward declaration + ForwardDeclaration fwDec0, fwDec; Rule forward_declaration_p = !(str_p("virtual")[assign_a(fwDec.isVirtual, true)]) >> str_p("class") @@ -367,7 +366,7 @@ void Module::parseMarkup(const std::string& data) { BOOST_SPIRIT_DEBUG_NODE(returnType2_p); BOOST_SPIRIT_DEBUG_NODE(pair_p); BOOST_SPIRIT_DEBUG_NODE(void_p); - BOOST_SPIRIT_DEBUG_NODE(returnType_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); @@ -388,67 +387,12 @@ void Module::parseMarkup(const std::string& data) { } // Post-process classes for serialization markers - BOOST_FOREACH(Class& cls, classes) { - Class::Methods::iterator serializable_it = cls.methods.find("serializable"); - if (serializable_it != cls.methods.end()) { -#ifndef WRAP_DISABLE_SERIALIZE - cls.isSerializable = true; -#else - cout << "Ignoring serializable() flag in class " << cls.name << endl; -#endif - cls.methods.erase(serializable_it); - } - - Class::Methods::iterator serialize_it = cls.methods.find("serialize"); - if (serialize_it != cls.methods.end()) { -#ifndef WRAP_DISABLE_SERIALIZE - cls.isSerializable = true; - cls.hasSerialization= true; -#else - cout << "Ignoring serialize() flag in class " << cls.name << endl; -#endif - cls.methods.erase(serialize_it); - } - } + BOOST_FOREACH(Class& cls, classes) + cls.erase_serialization(); // Explicitly add methods to the classes from parents so it shows in documentation BOOST_FOREACH(Class& cls, classes) - { - map inhereted = appendInheretedMethods(cls, classes); - cls.methods.insert(inhereted.begin(), inhereted.end()); - } -} - -/* ************************************************************************* */ -template -void verifyArguments(const vector& validArgs, const map& vt) { - typedef typename map::value_type Name_Method; - BOOST_FOREACH(const Name_Method& name_method, vt) { - const T& t = name_method.second; - BOOST_FOREACH(const ArgumentList& argList, t.argLists) { - BOOST_FOREACH(Argument arg, argList) { - string fullType = arg.qualifiedType("::"); - if(find(validArgs.begin(), validArgs.end(), fullType) - == validArgs.end()) - throw DependencyMissing(fullType, t.name); - } - } - } -} - -/* ************************************************************************* */ -template -void verifyReturnTypes(const vector& validtypes, const map& vt) { - typedef typename map::value_type Name_Method; - BOOST_FOREACH(const Name_Method& name_method, vt) { - const T& t = name_method.second; - BOOST_FOREACH(const ReturnValue& retval, t.returnVals) { - if (find(validtypes.begin(), validtypes.end(), retval.qualifiedType1("::")) == validtypes.end()) - throw DependencyMissing(retval.qualifiedType1("::"), t.name); - if (retval.isPair && find(validtypes.begin(), validtypes.end(), retval.qualifiedType2("::")) == validtypes.end()) - throw DependencyMissing(retval.qualifiedType2("::"), t.name); - } - } + cls.appendInheritedMethods(cls, classes); } /* ************************************************************************* */ @@ -486,21 +430,8 @@ void Module::matlab_code(const string& toolboxPath, const string& headerPath) co verifyReturnTypes(validTypes, global_functions); bool hasSerialiable = false; - BOOST_FOREACH(const Class& cls, expandedClasses) { - hasSerialiable |= cls.isSerializable; - // verify all of the function arguments - //TODO:verifyArguments(validTypes, cls.constructor.args_list); - verifyArguments(validTypes, cls.static_methods); - verifyArguments(validTypes, cls.methods); - - // verify function return types - verifyReturnTypes(validTypes, cls.static_methods); - verifyReturnTypes(validTypes, cls.methods); - - // verify parents - if(!cls.qualifiedParent.empty() && std::find(validTypes.begin(), validTypes.end(), wrap::qualifiedName("::", cls.qualifiedParent)) == validTypes.end()) - throw DependencyMissing(wrap::qualifiedName("::", cls.qualifiedParent), cls.qualifiedName("::")); - } + BOOST_FOREACH(const Class& cls, expandedClasses) + cls.verifyAll(validTypes,hasSerialiable); // Create type attributes table and check validity TypeAttributesTable typeAttributes; @@ -567,28 +498,7 @@ void Module::matlab_code(const string& toolboxPath, const string& headerPath) co wrapperFile.emit(true); } -/* ************************************************************************* */ -map Module::appendInheretedMethods(const Class& cls, const vector& classes) -{ - map methods; - if(!cls.qualifiedParent.empty()) - { - //Find Class - BOOST_FOREACH(const Class& parent, classes) { - //We found the class for our parent - if(parent.name == cls.qualifiedParent.back()) - { - Methods inhereted = appendInheretedMethods(parent, classes); - methods.insert(inhereted.begin(), inhereted.end()); - } - } - } else { - methods.insert(cls.methods.begin(), cls.methods.end()); - } - return methods; -} - /* ************************************************************************* */ void Module::finish_wrapper(FileWriter& file, const std::vector& functionNames) const { file.oss << "void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; diff --git a/wrap/Module.h b/wrap/Module.h index 93f699e14..8ef2bc4fd 100644 --- a/wrap/Module.h +++ b/wrap/Module.h @@ -53,9 +53,6 @@ struct Module { /// Dummy constructor that does no parsing - use only for testing Module(const std::string& moduleName, bool enable_verbose=true); - //Recursive method to append all methods inhereted from parent classes - std::map appendInheretedMethods(const Class& cls, const std::vector& classes); - /// MATLAB code generation: void matlab_code( const std::string& path, diff --git a/wrap/Qualified.h b/wrap/Qualified.h new file mode 100644 index 000000000..b23e9020d --- /dev/null +++ b/wrap/Qualified.h @@ -0,0 +1,77 @@ +/* ---------------------------------------------------------------------------- + + * 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 Qualified.h + * @brief Qualified name + * @author Frank Dellaert + * @date Nov 11, 2014 + **/ + +#pragma once + +#include +#include + +namespace wrap { + +/** + * Class to encapuslate a qualified name, i.e., with (nested) namespaces + */ +struct Qualified { + + std::vector namespaces; ///< Stack of namespaces + std::string name; ///< type name + + Qualified(const std::string& name_ = "") : + name(name_) { + } + + bool empty() const { + return namespaces.empty() && name.empty(); + } + + void clear() { + namespaces.clear(); + name.clear(); + } + + bool operator!=(const Qualified& other) const { + return other.name != name || other.namespaces != namespaces; + } + + /// 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; + 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"; + return result; + } + + friend std::ostream& operator<<(std::ostream& os, const Qualified& q) { + os << q.qualifiedName("::"); + return os; + } + +}; + +} // \namespace wrap + diff --git a/wrap/ReturnType.cpp b/wrap/ReturnType.cpp new file mode 100644 index 000000000..a317a5420 --- /dev/null +++ b/wrap/ReturnType.cpp @@ -0,0 +1,61 @@ +/** + * @file ReturnType.cpp + * @date Nov 13, 2014 + * @author Frank Dellaert + */ + +#include "ReturnType.h" +#include "utilities.h" +#include +#include + +using namespace std; +using namespace wrap; + +/* ************************************************************************* */ +string ReturnType::str(bool add_ptr) const { + return maybe_shared_ptr(add_ptr && isPtr, qualifiedName("::"), name); +} + +/* ************************************************************************* */ +void ReturnType::wrap_result(const string& out, const string& result, + FileWriter& wrapperFile, const TypeAttributesTable& typeAttributes) const { + + string cppType = qualifiedName("::"), matlabType = qualifiedName("."); + + if (category == CLASS) { + string objCopy, ptrType; + ptrType = "Shared" + name; + const bool isVirtual = typeAttributes.at(cppType).isVirtual; + if (isVirtual) { + if (isPtr) + objCopy = result; + else + objCopy = result + ".clone()"; + } else { + if (isPtr) + objCopy = result; + else + objCopy = ptrType + "(new " + cppType + "(" + result + "))"; + } + 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; + wrapperFile.oss << out << " = wrap_shared_ptr(ret,\"" << matlabType + << "\");\n"; + } else if (matlabType != "void") + 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; +} + +/* ************************************************************************* */ + diff --git a/wrap/ReturnType.h b/wrap/ReturnType.h new file mode 100644 index 000000000..1829fbc81 --- /dev/null +++ b/wrap/ReturnType.h @@ -0,0 +1,67 @@ +/** + * @file ReturnValue.h + * @brief Encapsulates a return type of a method + * @date Nov 13, 2014 + * @author Frank Dellaert + */ + +#include "Qualified.h" +#include "FileWriter.h" +#include "TypeAttributesTable.h" +#include "utilities.h" + +#pragma once + +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; + + bool isPtr; + return_category category; + + ReturnType() : + isPtr(false), category(CLASS) { + } + + ReturnType(const std::string& name) : + isPtr(false), category(CLASS) { + Qualified::name = name; + } + + void rename(const Qualified& q) { + name = q.name; + namespaces = q.namespaces; + } + + /// Check if this type is in a set of valid types + template + void verify(TYPES validtypes, const std::string& s) const { + std::string key = qualifiedName("::"); + if (find(validtypes.begin(), validtypes.end(), key) == validtypes.end()) + throw DependencyMissing(key, "checking return type of " + s); + } + +private: + + friend struct ReturnValue; + + std::string str(bool add_ptr) const; + + /// Example: out[1] = wrap_shared_ptr(pairResult.second,"Test", false); + void wrap_result(const std::string& out, const std::string& result, + FileWriter& wrapperFile, const TypeAttributesTable& typeAttributes) const; + + /// Creates typedef + void wrapTypeUnwrap(FileWriter& wrapperFile) const; + +}; + +} // \namespace wrap diff --git a/wrap/ReturnValue.cpp b/wrap/ReturnValue.cpp index 78e36d4da..54e585cad 100644 --- a/wrap/ReturnValue.cpp +++ b/wrap/ReturnValue.cpp @@ -1,153 +1,72 @@ /** * @file ReturnValue.cpp - * * @date Dec 1, 2011 * @author Alex Cunningham * @author Andrew Melim * @author Richard Roberts */ -#include - #include "ReturnValue.h" #include "utilities.h" +#include +#include using namespace std; using namespace wrap; /* ************************************************************************* */ -string ReturnValue::return_type(bool add_ptr, pairing p) const { - if (p==pair && isPair) { - string str = "pair< " + - maybe_shared_ptr(add_ptr || isPtr1, qualifiedType1("::"), type1) + ", " + - maybe_shared_ptr(add_ptr || isPtr2, qualifiedType2("::"), type2) + " >"; - return str; - } else - return maybe_shared_ptr(add_ptr && isPtr1, (p==arg2)? qualifiedType2("::") : qualifiedType1("::"), (p==arg2)? type2 : type1); +ReturnValue ReturnValue::expandTemplate(const TemplateSubstitution& ts) const { + ReturnValue instRetVal = *this; + instRetVal.type1 = ts(type1); + if (isPair) + instRetVal.type2 = ts(type2); + return instRetVal; +} + +/* ************************************************************************* */ +string ReturnValue::return_type(bool add_ptr) const { + if (isPair) + return "pair< " + type1.str(add_ptr) + ", " + type2.str(add_ptr) + " >"; + else + return type1.str(add_ptr); } /* ************************************************************************* */ string ReturnValue::matlab_returnType() const { - return isPair? "[first,second]" : "result"; + return isPair ? "[first,second]" : "result"; } /* ************************************************************************* */ -string ReturnValue::qualifiedType1(const string& delim) const { - string result; - BOOST_FOREACH(const string& ns, namespaces1) result += ns + delim; - return result + type1; -} - -/* ************************************************************************* */ -string ReturnValue::qualifiedType2(const string& delim) const { - string result; - BOOST_FOREACH(const string& ns, namespaces2) result += ns + delim; - return result + type2; -} - -/* ************************************************************************* */ -//TODO:Fix this -void ReturnValue::wrap_result(const string& result, FileWriter& file, const TypeAttributesTable& typeAttributes) const { - string cppType1 = qualifiedType1("::"), matlabType1 = qualifiedType1("."); - string cppType2 = qualifiedType2("::"), matlabType2 = qualifiedType2("."); - +void ReturnValue::wrap_result(const string& result, FileWriter& wrapperFile, + const TypeAttributesTable& typeAttributes) const { if (isPair) { // For a pair, store the returned pair so we do not evaluate the function twice - file.oss << " " << return_type(false, pair) << " pairResult = " << result << ";\n"; - - // first return value in pair - if (category1 == ReturnValue::CLASS) { // if we are going to make one - string objCopy, ptrType; - ptrType = "Shared" + type1; - const bool isVirtual = typeAttributes.at(cppType1).isVirtual; - if(isVirtual) { - if(isPtr1) - objCopy = "pairResult.first"; - else - objCopy = "pairResult.first.clone()"; - } else { - if(isPtr1) - objCopy = "pairResult.first"; - else - objCopy = ptrType + "(new " + cppType1 + "(pairResult.first))"; - } - file.oss << " out[0] = wrap_shared_ptr(" << objCopy << ",\"" << matlabType1 << "\", " << (isVirtual ? "true" : "false") << ");\n"; - } else if(isPtr1) { - file.oss << " Shared" << type1 <<"* ret = new Shared" << type1 << "(pairResult.first);" << endl; - file.oss << " out[0] = wrap_shared_ptr(ret,\"" << matlabType1 << "\", false);\n"; - } else // if basis type - file.oss << " out[0] = wrap< " << return_type(true,arg1) << " >(pairResult.first);\n"; - - // second return value in pair - if (category2 == ReturnValue::CLASS) { // if we are going to make one - string objCopy, ptrType; - ptrType = "Shared" + type2; - const bool isVirtual = typeAttributes.at(cppType2).isVirtual; - if(isVirtual) { - if(isPtr2) - objCopy = "pairResult.second"; - else - objCopy = "pairResult.second.clone()"; - } else { - if(isPtr2) - objCopy = "pairResult.second"; - else - objCopy = ptrType + "(new " + cppType2 + "(pairResult.second))"; - } - file.oss << " out[1] = wrap_shared_ptr(" << objCopy << ",\"" << matlabType2 << "\", " << (isVirtual ? "true" : "false") << ");\n"; - } else if(isPtr2) { - file.oss << " Shared" << type2 <<"* ret = new Shared" << type2 << "(pairResult.second);" << endl; - file.oss << " out[1] = wrap_shared_ptr(ret,\"" << matlabType2 << "\");\n"; - } else - file.oss << " out[1] = wrap< " << return_type(true,arg2) << " >(pairResult.second);\n"; + wrapperFile.oss << " " << return_type(true) << " pairResult = " << result + << ";\n"; + type1.wrap_result(" out[0]", "pairResult.first", wrapperFile, + typeAttributes); + type2.wrap_result(" out[1]", "pairResult.second", wrapperFile, + typeAttributes); } else { // Not a pair - - if (category1 == ReturnValue::CLASS) { - string objCopy, ptrType; - ptrType = "Shared" + type1; - const bool isVirtual = typeAttributes.at(cppType1).isVirtual; - if(isVirtual) { - if(isPtr1) - objCopy = result; - else - objCopy = result + ".clone()"; - } else { - if(isPtr1) - objCopy = result; - else - objCopy = ptrType + "(new " + cppType1 + "(" + result + "))"; - } - file.oss << " out[0] = wrap_shared_ptr(" << objCopy << ",\"" << matlabType1 << "\", " << (isVirtual ? "true" : "false") << ");\n"; - } else if(isPtr1) { - file.oss << " Shared" << type1 <<"* ret = new Shared" << type1 << "(" << result << ");" << endl; - file.oss << " out[0] = wrap_shared_ptr(ret,\"" << matlabType1 << "\");\n"; - } else if (matlabType1!="void") - file.oss << " out[0] = wrap< " << return_type(true,arg1) << " >(" << result << ");\n"; + type1.wrap_result(" out[0]", result, wrapperFile, typeAttributes); } } /* ************************************************************************* */ void ReturnValue::wrapTypeUnwrap(FileWriter& wrapperFile) const { - if(isPair) - { - if(category1 == ReturnValue::CLASS) - wrapperFile.oss << " typedef boost::shared_ptr<" << qualifiedType1("::") << "> Shared" << type1 << ";"<< endl; - if(category2 == ReturnValue::CLASS) - wrapperFile.oss << " typedef boost::shared_ptr<" << qualifiedType2("::") << "> Shared" << type2 << ";"<< endl; - } - else { - if (category1 == ReturnValue::CLASS) - wrapperFile.oss << " typedef boost::shared_ptr<" << qualifiedType1("::") << "> Shared" << type1 << ";"<< endl; - } + type1.wrapTypeUnwrap(wrapperFile); + if (isPair) + type2.wrapTypeUnwrap(wrapperFile); } + /* ************************************************************************* */ -void ReturnValue::emit_matlab(FileWriter& file) const { +void ReturnValue::emit_matlab(FileWriter& proxyFile) const { string output; if (isPair) - file.oss << "[ varargout{1} varargout{2} ] = "; - else if (category1 != ReturnValue::VOID) - file.oss << "varargout{1} = "; + proxyFile.oss << "[ varargout{1} varargout{2} ] = "; + else if (type1.category != ReturnType::VOID) + proxyFile.oss << "varargout{1} = "; } + /* ************************************************************************* */ - diff --git a/wrap/ReturnValue.h b/wrap/ReturnValue.h index 989f81109..abfcec2b0 100644 --- a/wrap/ReturnValue.h +++ b/wrap/ReturnValue.h @@ -2,59 +2,61 @@ * @file ReturnValue.h * * @brief Encapsulates a return value from a method - * * @date Dec 1, 2011 * @author Alex Cunningham * @author Richard Roberts */ +#include "ReturnType.h" +#include "TemplateSubstitution.h" #include "FileWriter.h" #include "TypeAttributesTable.h" - -#include +#include "utilities.h" #pragma once namespace wrap { /** - * Encapsulates return value of a method or function + * Encapsulates return type of a method or function, possibly a pair */ struct ReturnValue { - /// the different supported return value categories - typedef enum { - CLASS = 1, EIGEN = 2, BASIS = 3, VOID = 4 - } return_category; - - bool isPtr1, isPtr2, isPair; - return_category category1, category2; - std::string type1, type2; - std::vector namespaces1, namespaces2; + bool isPair; + ReturnType type1, type2; /// Constructor ReturnValue() : - isPtr1(false), isPtr2(false), isPair(false), category1(CLASS), category2( - CLASS) { + isPair(false) { } - typedef enum { - arg1, arg2, pair - } pairing; + /// Constructor + ReturnValue(const ReturnType& type) : + isPair(false), type1(type) { + } - std::string return_type(bool add_ptr, pairing p) const; + /// Substitute template argument + ReturnValue expandTemplate(const TemplateSubstitution& ts) const; - std::string qualifiedType1(const std::string& delim = "") const; - std::string qualifiedType2(const std::string& delim = "") const; + std::string return_type(bool add_ptr) const; std::string matlab_returnType() const; - void wrap_result(const std::string& result, FileWriter& file, + void wrap_result(const std::string& result, FileWriter& wrapperFile, const TypeAttributesTable& typeAttributes) const; void wrapTypeUnwrap(FileWriter& wrapperFile) const; - void emit_matlab(FileWriter& file) const; + void emit_matlab(FileWriter& proxyFile) const; + + friend std::ostream& operator<<(std::ostream& os, const ReturnValue& r) { + if (!r.isPair && r.type1.category == ReturnType::VOID) + os << "void"; + else + os << r.return_type(true); + return os; + } + }; } // \namespace wrap diff --git a/wrap/StaticMethod.cpp b/wrap/StaticMethod.cpp index 0c4cc7d75..d3bd75628 100644 --- a/wrap/StaticMethod.cpp +++ b/wrap/StaticMethod.cpp @@ -16,7 +16,7 @@ * @author Richard Roberts **/ -#include "StaticMethod.h" +#include "Method.h" #include "utilities.h" #include @@ -30,117 +30,146 @@ using namespace std; using namespace wrap; /* ************************************************************************* */ -void StaticMethod::addOverload(bool verbose, const std::string& name, - const ArgumentList& args, const ReturnValue& retVal) { - this->verbose = verbose; - this->name = name; - this->argLists.push_back(args); - this->returnVals.push_back(retVal); +void StaticMethod::addOverload(bool verbose, Str name, const ArgumentList& args, + const ReturnValue& retVal, const Qualified& instName) { + + Function::addOverload(verbose, name, instName); + SignatureOverloads::addOverload(args, retVal); } /* ************************************************************************* */ -void StaticMethod::proxy_wrapper_fragments(FileWriter& file, - FileWriter& wrapperFile, const string& cppClassName, - const std::string& matlabQualName, const std::string& matlabUniqueName, - const string& wrapperName, const TypeAttributesTable& typeAttributes, +void StaticMethod::proxy_header(FileWriter& proxyFile) const { + string upperName = matlabName(); + upperName[0] = toupper(upperName[0], locale()); + 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& functionNames) const { - string upperName = name; - upperName[0] = std::toupper(upperName[0], std::locale()); + // emit header, e.g., function varargout = templatedMethod(this, varargin) + proxy_header(proxyFile); - file.oss << " function varargout = " << upperName << "(varargin)\n"; - //Comments for documentation - string up_name = boost::to_upper_copy(name); - file.oss << " % " << up_name << " usage: "; - unsigned int argLCount = 0; - BOOST_FOREACH(ArgumentList argList, argLists) { - argList.emit_prototype(file, name); - if (argLCount != argLists.size() - 1) - file.oss << ", "; - else - file.oss << " : returns " - << returnVals[0].return_type(false, returnVals[0].pair) << endl; - argLCount++; - } + // Emit comments for documentation + string up_name = boost::to_upper_copy(matlabName()); + proxyFile.oss << " % " << up_name << " usage: "; + usage_fragment(proxyFile, matlabName()); - file.oss << " % " + // 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; - file.oss << " % " << "" << endl; - file.oss << " % " << "Usage" << endl; - BOOST_FOREACH(ArgumentList argList, argLists) { - file.oss << " % "; - argList.emit_prototype(file, up_name); - file.oss << endl; - } - - // Check arguments for all overloads - for (size_t overload = 0; overload < argLists.size(); ++overload) { + // Handle special case of single overload with all numeric arguments + if (nrOverloads() == 1 && argumentList(0).allScalar()) { // Output proxy matlab code - file.oss << " " << (overload == 0 ? "" : "else"); + // TODO: document why is it OK to not check arguments in this case + proxyFile.oss << " "; const int id = (int) functionNames.size(); - argLists[overload].emit_conditional_call(file, returnVals[overload], - wrapperName, id, true); // last bool is to indicate static ! + argumentList(0).emit_call(proxyFile, returnValue(0), wrapperName, id, + isStatic()); // Output C++ wrapper code const string wrapFunctionName = wrapper_fragment(wrapperFile, cppClassName, - matlabUniqueName, (int) overload, id, typeAttributes); + matlabUniqueName, 0, id, typeAttributes, templateArgValue_); // Add to function list functionNames.push_back(wrapFunctionName); - } - file.oss << " else\n"; - file.oss << " error('Arguments do not match any overload of function " - << matlabQualName << "." << upperName << "');" << endl; - file.oss << " end\n"; + } else { + // Check arguments for all overloads + for (size_t i = 0; i < nrOverloads(); ++i) { - file.oss << " end\n"; + // 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& file, - const string& cppClassName, const string& matlabUniqueName, int overload, - int id, const TypeAttributesTable& typeAttributes) const { +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 + "_" + const string wrapFunctionName = matlabUniqueName + "_" + name_ + "_" + boost::lexical_cast(id); - const ArgumentList& args = argLists[overload]; - const ReturnValue& returnVal = returnVals[overload]; + const ArgumentList& args = argumentList(overload); + const ReturnValue& returnVal = returnValue(overload); // call - file.oss << "void " << wrapFunctionName + wrapperFile.oss << "void " << wrapFunctionName << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; // start - file.oss << "{\n"; + wrapperFile.oss << "{\n"; - returnVal.wrapTypeUnwrap(file); + returnVal.wrapTypeUnwrap(wrapperFile); - file.oss << " typedef boost::shared_ptr<" << cppClassName << "> Shared;" - << endl; + wrapperFile.oss << " typedef boost::shared_ptr<" << cppClassName + << "> Shared;" << endl; - // check arguments - // NOTE: for static functions, there is no object passed - file.oss << " checkArguments(\"" << matlabUniqueName << "." << name - << "\",nargout,nargin," << args.size() << ");\n"; + // get call + // for static methods: cppClassName::staticMethod + // for instance methods: obj->instanceMethod + string expanded = wrapper_call(wrapperFile, cppClassName, matlabUniqueName, + args, returnVal, typeAttributes, instName); - // unwrap arguments, see Argument.cpp - 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 != "void") - returnVal.wrap_result(cppClassName + "::" + name + "(" + args.names() + ")", - file, typeAttributes); + expanded += ("(" + args.names() + ")"); + if (returnVal.type1.name != "void") + returnVal.wrap_result(expanded, wrapperFile, typeAttributes); else - file.oss << cppClassName + "::" + name + "(" + args.names() + ");\n"; + wrapperFile.oss << " " + expanded + ";\n"; // finish - file.oss << "}\n"; + 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 { + // check arguments + // NOTE: for static functions, there is no object passed + wrapperFile.oss << " checkArguments(\"" << matlabUniqueName << "." << name_ + << "\",nargout,nargin," << args.size() << ");\n"; + + // unwrap arguments, see Argument.cpp + args.matlab_unwrap(wrapperFile, 0); // We start at 0 because there is no self object + + // call method and wrap result + // example: out[0]=wrap(staticMethod(t)); + string expanded = cppClassName + "::" + name_; + if (!instName.empty()) + expanded += ("<" + instName.qualifiedName("::") + ">"); + + return expanded; +} + +/* ************************************************************************* */ diff --git a/wrap/StaticMethod.h b/wrap/StaticMethod.h index e1855f4c2..4a6fedbfc 100644 --- a/wrap/StaticMethod.h +++ b/wrap/StaticMethod.h @@ -19,44 +19,70 @@ #pragma once -#include "Argument.h" -#include "ReturnValue.h" -#include "TypeAttributesTable.h" +#include "Function.h" namespace wrap { /// StaticMethod class -struct StaticMethod { +struct StaticMethod: public Function, public SignatureOverloads { + + typedef const std::string& Str; /// Constructor creates empty object StaticMethod(bool verbosity = true) : - verbose(verbosity) { + Function(verbosity) { } - // Then the instance variables are set directly by the Module constructor - bool verbose; - std::string name; - std::vector argLists; - std::vector returnVals; + StaticMethod(const std::string& name, bool verbose = true) : + Function(name,verbose) { + } - // The first time this function is called, it initializes the class members - // with those in rhs, but in subsequent calls it adds additional argument - // lists as function overloads. - void addOverload(bool verbose, const std::string& name, - const ArgumentList& args, const ReturnValue& retVal); + virtual bool isStatic() const { + return true; + } + + void addOverload(bool verbose, Str name, const ArgumentList& args, + const ReturnValue& retVal, const Qualified& instName); + + // 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& validArgs) const { + SignatureOverloads::verifyArguments(validArgs, name_); + } + + void verifyReturnTypes(const std::vector& 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, - const std::string& cppClassName, const std::string& matlabQualName, - const std::string& matlabUniqueName, const std::string& wrapperName, - const TypeAttributesTable& typeAttributes, + Str cppClassName, Str matlabQualName, Str matlabUniqueName, + Str wrapperName, const TypeAttributesTable& typeAttributes, std::vector& functionNames) const; -private: - std::string wrapper_fragment(FileWriter& file, - const std::string& cppClassName, const std::string& matlabUniqueName, - int overload, int id, const TypeAttributesTable& typeAttributes) const; ///< cpp wrapper + 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]; + return os; + } + +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; }; } // \namespace wrap diff --git a/wrap/TemplateInstantiationTypedef.cpp b/wrap/TemplateInstantiationTypedef.cpp index 85615cef4..2007acdf1 100644 --- a/wrap/TemplateInstantiationTypedef.cpp +++ b/wrap/TemplateInstantiationTypedef.cpp @@ -19,43 +19,50 @@ #include "TemplateInstantiationTypedef.h" #include "utilities.h" +#include +#include +#include using namespace std; namespace wrap { - Class TemplateInstantiationTypedef::findAndExpand(const vector& classes) const { - // Find matching class - std::vector::const_iterator clsIt = classes.end(); - for(std::vector::const_iterator it = classes.begin(); it != classes.end(); ++it) { - if(it->name == className && it->namespaces == classNamespaces && it->templateArgs.size() == typeList.size()) { - clsIt = it; - break; - } +Class TemplateInstantiationTypedef::findAndExpand( + const vector& classes) const { + // Find matching class + boost::optional matchedClass; + BOOST_FOREACH(const Class& cls, classes) { + if (cls.name == class_.name && cls.namespaces == class_.namespaces + && cls.templateArgs.size() == typeList.size()) { + matchedClass.reset(cls); + break; } - - if(clsIt == classes.end()) - throw DependencyMissing(wrap::qualifiedName("::", classNamespaces, className), - "instantiation into typedef name " + wrap::qualifiedName("::", namespaces, name) + - ". Ensure that the typedef provides the correct number of template arguments."); - - // Instantiate it - Class classInst = *clsIt; - for(size_t i = 0; i < typeList.size(); ++i) - classInst = classInst.expandTemplate(classInst.templateArgs[i], typeList[i], namespaces, name); - - // Fix class properties - classInst.name = name; - classInst.templateArgs.clear(); - classInst.typedefName = clsIt->qualifiedName("::") + "<"; - if(typeList.size() > 0) - classInst.typedefName += wrap::qualifiedName("::", typeList[0]); - for(size_t i = 1; i < typeList.size(); ++i) - classInst.typedefName += (", " + wrap::qualifiedName("::", typeList[i])); - classInst.typedefName += ">"; - classInst.namespaces = namespaces; - - return classInst; } -} \ No newline at end of file + if (!matchedClass) + throw DependencyMissing(class_.qualifiedName("::"), + "instantiation into typedef name " + qualifiedName("::") + + ". Ensure that the typedef provides the correct number of template arguments."); + + // Instantiate it + Class classInst = *matchedClass; + for (size_t i = 0; i < typeList.size(); ++i) { + TemplateSubstitution ts(classInst.templateArgs[i], typeList[i], *this); + classInst = classInst.expandTemplate(ts); + } + + // Fix class properties + classInst.name = name; + classInst.templateArgs.clear(); + classInst.typedefName = matchedClass->qualifiedName("::") + "<"; + if (typeList.size() > 0) + classInst.typedefName += typeList[0].qualifiedName("::"); + for (size_t i = 1; i < typeList.size(); ++i) + classInst.typedefName += (", " + typeList[i].qualifiedName("::")); + classInst.typedefName += ">"; + classInst.namespaces = namespaces; + + return classInst; +} + +} diff --git a/wrap/TemplateInstantiationTypedef.h b/wrap/TemplateInstantiationTypedef.h index 2292f16ee..2a08d7a94 100644 --- a/wrap/TemplateInstantiationTypedef.h +++ b/wrap/TemplateInstantiationTypedef.h @@ -26,14 +26,11 @@ namespace wrap { -struct TemplateInstantiationTypedef { - std::vector classNamespaces; - std::string className; - std::vector namespaces; - std::string name; - std::vector > typeList; +struct TemplateInstantiationTypedef : public Qualified { + Qualified class_; + std::vector typeList; Class findAndExpand(const std::vector& classes) const; }; -} \ No newline at end of file +} diff --git a/wrap/TemplateSubstitution.h b/wrap/TemplateSubstitution.h new file mode 100644 index 000000000..b20a1af6f --- /dev/null +++ b/wrap/TemplateSubstitution.h @@ -0,0 +1,76 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file TemplateSubstitution.h + * @brief Auxiliary class for template substitutions + * @author Frank Dellaert + * @date Nov 13, 2014 + **/ + +#pragma once + +#include "ReturnType.h" +#include +#include + +namespace wrap { + +/** + * e.g. TemplateSubstitution("T", gtsam::Point2, gtsam::PriorFactorPoint2) + */ +class TemplateSubstitution { + + std::string templateArg_; + Qualified qualifiedType_, expandedClass_; + +public: + + TemplateSubstitution(const std::string& a, const Qualified& t, + const Qualified& e) : + templateArg_(a), qualifiedType_(t), expandedClass_(e) { + } + + std::string expandedClassName() const { + return expandedClass_.name; + } + + // Substitute if needed + Qualified operator()(const Qualified& type) const { + if (type.name == templateArg_ && type.namespaces.empty()) + return qualifiedType_; + else if (type.name == "This") + return expandedClass_; + else + return type; + } + + // Substitute if needed + ReturnType operator()(const ReturnType& type) const { + ReturnType instType = type; + if (type.name == templateArg_ && type.namespaces.empty()) + instType.rename(qualifiedType_); + else if (type.name == "This") + instType.rename(expandedClass_); + return instType; + } + + friend std::ostream& operator<<(std::ostream& os, + const TemplateSubstitution& ts) { + os << ts.templateArg_ << '/' << ts.qualifiedType_.qualifiedName("::") + << " (" << ts.expandedClass_.qualifiedName("::") << ")"; + return os; + } + +}; + +} // \namespace wrap + diff --git a/wrap/TypeAttributesTable.cpp b/wrap/TypeAttributesTable.cpp index 449b88aab..16055fca1 100644 --- a/wrap/TypeAttributesTable.cpp +++ b/wrap/TypeAttributesTable.cpp @@ -46,13 +46,17 @@ namespace wrap { void TypeAttributesTable::checkValidity(const vector& classes) const { BOOST_FOREACH(const Class& cls, classes) { // Check that class is virtual if it has a parent - if(!cls.qualifiedParent.empty() && !cls.isVirtual) - throw AttributeError(cls.qualifiedName("::"), "Has a base class so needs to be declared virtual, change to 'virtual class "+cls.name+" ...'"); + if (!cls.qualifiedParent.empty() && !cls.isVirtual) + throw AttributeError(cls.qualifiedName("::"), + "Has a base class so needs to be declared virtual, change to 'virtual class " + + cls.name + " ...'"); // Check that parent is virtual as well - if(!cls.qualifiedParent.empty() && !at(wrap::qualifiedName("::", cls.qualifiedParent)).isVirtual) - throw AttributeError(wrap::qualifiedName("::", cls.qualifiedParent), - "Is the base class of " + cls.qualifiedName("::") + ", so needs to be declared virtual"); + Qualified parent = cls.qualifiedParent; + if (!parent.empty() && !at(parent.qualifiedName("::")).isVirtual) + throw AttributeError(parent.qualifiedName("::"), + "Is the base class of " + cls.qualifiedName("::") + + ", so needs to be declared virtual"); } } -} \ No newline at end of file +} diff --git a/wrap/TypeAttributesTable.h b/wrap/TypeAttributesTable.h index 1ff6c121c..d2a346bfc 100644 --- a/wrap/TypeAttributesTable.h +++ b/wrap/TypeAttributesTable.h @@ -28,7 +28,7 @@ namespace wrap { // Forward declarations - struct Class; + 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 @@ -52,4 +52,4 @@ public: void checkValidity(const std::vector& classes) const; }; -} \ No newline at end of file +} diff --git a/wrap/tests/expected2/+gtsam/Point2.m b/wrap/tests/expected2/+gtsam/Point2.m new file mode 100644 index 000000000..308b35d9a --- /dev/null +++ b/wrap/tests/expected2/+gtsam/Point2.m @@ -0,0 +1,90 @@ +%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 +%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 = 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{:}); + 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{:}); + 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{:}); + 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{:}); + end + + end + + methods(Static = true) + end +end diff --git a/wrap/tests/expected2/+gtsam/Point3.m b/wrap/tests/expected2/+gtsam/Point3.m new file mode 100644 index 000000000..3ef336ff1 --- /dev/null +++ b/wrap/tests/expected2/+gtsam/Point3.m @@ -0,0 +1,61 @@ +%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 +% +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(11, 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}); + else + error('Arguments do not match any overload of gtsam.Point3 constructor'); + end + obj.ptr_gtsamPoint3 = my_ptr; + end + + function delete(obj) + geometry_wrapper(13, 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(14, this, varargin{:}); + 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(15, 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{:}); + end + + end +end diff --git a/wrap/tests/expected2/MyBase.m b/wrap/tests/expected2/MyBase.m new file mode 100644 index 000000000..d7bfe7e81 --- /dev/null +++ b/wrap/tests/expected2/MyBase.m @@ -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(41, varargin{2}); + end + geometry_wrapper(40, 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(42, 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 diff --git a/wrap/tests/expected2/MyFactorPosePoint2.m b/wrap/tests/expected2/MyFactorPosePoint2.m new file mode 100644 index 000000000..166e1514d --- /dev/null +++ b/wrap/tests/expected2/MyFactorPosePoint2.m @@ -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(69, 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}); + else + error('Arguments do not match any overload of MyFactorPosePoint2 constructor'); + end + obj.ptr_MyFactorPosePoint2 = my_ptr; + end + + function delete(obj) + geometry_wrapper(71, 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 diff --git a/wrap/tests/expected2/MyTemplatePoint2.m b/wrap/tests/expected2/MyTemplatePoint2.m new file mode 100644 index 000000000..5f1c69480 --- /dev/null +++ b/wrap/tests/expected2/MyTemplatePoint2.m @@ -0,0 +1,134 @@ +%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 > +%templatedMethodPoint2(Point2 t) : returns void +%templatedMethodPoint3(Point3 t) : returns void +% +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(44, varargin{2}); + end + base_ptr = geometry_wrapper(43, my_ptr); + elseif nargin == 0 + [ my_ptr, base_ptr ] = geometry_wrapper(45); + 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(46, 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(47, 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(48, 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(49, 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{:}); + 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{:}); + 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(52, 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(53, this, varargin{:}); + else + error('Arguments do not match any overload of function MyTemplatePoint2.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(54, 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 + % 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{:}); + else + error('Arguments do not match any overload of function MyTemplatePoint2.templatedMethod'); + end + end + + end + + methods(Static = true) + end +end diff --git a/wrap/tests/expected2/MyTemplatePoint3.m b/wrap/tests/expected2/MyTemplatePoint3.m new file mode 100644 index 000000000..848e224fd --- /dev/null +++ b/wrap/tests/expected2/MyTemplatePoint3.m @@ -0,0 +1,134 @@ +%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 diff --git a/wrap/tests/expected2/Test.m b/wrap/tests/expected2/Test.m new file mode 100644 index 000000000..ada5a868d --- /dev/null +++ b/wrap/tests/expected2/Test.m @@ -0,0 +1,218 @@ +%class Test, see Doxygen page for details +%at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html +% +%-------Constructors------- +%Test() +%Test(double a, Matrix b) +% +%-------Methods------- +%arg_EigenConstRef(Matrix value) : returns void +%create_MixedPtrs() : returns pair< Test, Test > +%create_ptrs() : returns pair< Test, Test > +%print() : returns void +%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 +%return_double(double value) : returns double +%return_field(Test t) : returns bool +%return_int(int value) : returns int +%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< Test, Test > +%return_size_t(size_t value) : returns size_t +%return_string(string value) : returns string +%return_vector1(Vector value) : returns Vector +%return_vector2(Vector value) : returns Vector +% +classdef Test < handle + properties + ptr_Test = 0 + end + methods + function obj = Test(varargin) + if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682) + my_ptr = varargin{2}; + geometry_wrapper(17, my_ptr); + elseif nargin == 0 + my_ptr = geometry_wrapper(18); + elseif nargin == 2 && isa(varargin{1},'double') && isa(varargin{2},'double') + my_ptr = geometry_wrapper(19, varargin{1}, varargin{2}); + else + error('Arguments do not match any overload of Test constructor'); + end + obj.ptr_Test = my_ptr; + end + + function delete(obj) + geometry_wrapper(20, obj.ptr_Test); + 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 = arg_EigenConstRef(this, varargin) + % 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{:}); + 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, 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{:}); + 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{:}); + 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{:}); + 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{:}); + 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{:}); + else + error('Arguments do not match any overload of function Test.return_Test'); + end + end + + function varargout = return_TestPtr(this, varargin) + % 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{:}); + else + error('Arguments do not match any overload of function Test.return_TestPtr'); + end + end + + 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{:}); + 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{:}); + 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{:}); + else + error('Arguments do not match any overload of function Test.return_field'); + end + end + + 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{:}); + 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{:}); + else + error('Arguments do not match any overload of function Test.return_matrix1'); + end + end + + function varargout = return_matrix2(this, varargin) + % 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{:}); + else + error('Arguments do not match any overload of function Test.return_matrix2'); + end + end + + 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{:}); + 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< 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{:}); + else + error('Arguments do not match any overload of function Test.return_ptrs'); + end + end + + 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{:}); + 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{:}); + else + error('Arguments do not match any overload of function Test.return_string'); + end + end + + 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{:}); + else + error('Arguments do not match any overload of function Test.return_vector1'); + end + end + + 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{:}); + else + error('Arguments do not match any overload of function Test.return_vector2'); + end + end + + end + + methods(Static = true) + end +end diff --git a/wrap/tests/expected2/aGlobalFunction.m b/wrap/tests/expected2/aGlobalFunction.m new file mode 100644 index 000000000..5cf9aafa1 --- /dev/null +++ b/wrap/tests/expected2/aGlobalFunction.m @@ -0,0 +1,6 @@ +function varargout = aGlobalFunction(varargin) + if length(varargin) == 0 + varargout{1} = geometry_wrapper(72, varargin{:}); + else + error('Arguments do not match any overload of function aGlobalFunction'); + end diff --git a/wrap/tests/expected2/geometry_wrapper.cpp b/wrap/tests/expected2/geometry_wrapper.cpp new file mode 100644 index 000000000..ab6ae5aa7 --- /dev/null +++ b/wrap/tests/expected2/geometry_wrapper.cpp @@ -0,0 +1,1127 @@ +#include +#include +#include + +#include + +typedef MyTemplate MyTemplatePoint2; +typedef MyTemplate MyTemplatePoint3; +typedef MyFactor MyFactorPosePoint2; + +typedef std::set*> Collector_gtsamPoint2; +static Collector_gtsamPoint2 collector_gtsamPoint2; +typedef std::set*> Collector_gtsamPoint3; +static Collector_gtsamPoint3 collector_gtsamPoint3; +typedef std::set*> Collector_Test; +static Collector_Test collector_Test; +typedef std::set*> Collector_MyBase; +static Collector_MyBase collector_MyBase; +typedef std::set*> Collector_MyTemplatePoint2; +static Collector_MyTemplatePoint2 collector_MyTemplatePoint2; +typedef std::set*> Collector_MyTemplatePoint3; +static Collector_MyTemplatePoint3 collector_MyTemplatePoint3; +typedef std::set*> Collector_MyFactorPosePoint2; +static Collector_MyFactorPosePoint2 collector_MyFactorPosePoint2; + +void _deleteAllObjects() +{ + mstream mout; + std::streambuf *outbuf = std::cout.rdbuf(&mout); + + bool anyDeleted = false; + { for(Collector_gtsamPoint2::iterator iter = collector_gtsamPoint2.begin(); + iter != collector_gtsamPoint2.end(); ) { + delete *iter; + collector_gtsamPoint2.erase(iter++); + anyDeleted = true; + } } + { for(Collector_gtsamPoint3::iterator iter = collector_gtsamPoint3.begin(); + iter != collector_gtsamPoint3.end(); ) { + delete *iter; + collector_gtsamPoint3.erase(iter++); + anyDeleted = true; + } } + { for(Collector_Test::iterator iter = collector_Test.begin(); + iter != collector_Test.end(); ) { + delete *iter; + collector_Test.erase(iter++); + anyDeleted = true; + } } + { for(Collector_MyBase::iterator iter = collector_MyBase.begin(); + iter != collector_MyBase.end(); ) { + delete *iter; + collector_MyBase.erase(iter++); + anyDeleted = true; + } } + { for(Collector_MyTemplatePoint2::iterator iter = collector_MyTemplatePoint2.begin(); + iter != collector_MyTemplatePoint2.end(); ) { + delete *iter; + collector_MyTemplatePoint2.erase(iter++); + anyDeleted = true; + } } + { for(Collector_MyTemplatePoint3::iterator iter = collector_MyTemplatePoint3.begin(); + iter != collector_MyTemplatePoint3.end(); ) { + delete *iter; + collector_MyTemplatePoint3.erase(iter++); + anyDeleted = true; + } } + { for(Collector_MyFactorPosePoint2::iterator iter = collector_MyFactorPosePoint2.begin(); + iter != collector_MyFactorPosePoint2.end(); ) { + delete *iter; + collector_MyFactorPosePoint2.erase(iter++); + anyDeleted = true; + } } + if(anyDeleted) + cout << + "WARNING: Wrap modules with variables in the workspace have been reloaded due to\n" + "calling destructors, call 'clear all' again if you plan to now recompile a wrap\n" + "module, so that your recompiled module is used instead of the old one." << endl; + std::cout.rdbuf(outbuf); +} + +void _geometry_RTTIRegister() { + const mxArray *alreadyCreated = mexGetVariablePtr("global", "gtsam_geometry_rttiRegistry_created"); + if(!alreadyCreated) { + std::map types; + types.insert(std::make_pair(typeid(MyBase).name(), "MyBase")); + types.insert(std::make_pair(typeid(MyTemplatePoint2).name(), "MyTemplatePoint2")); + types.insert(std::make_pair(typeid(MyTemplatePoint3).name(), "MyTemplatePoint3")); + + mxArray *registry = mexGetVariable("global", "gtsamwrap_rttiRegistry"); + if(!registry) + registry = mxCreateStructMatrix(1, 1, 0, NULL); + typedef std::pair StringPair; + BOOST_FOREACH(const StringPair& rtti_matlab, types) { + int fieldId = mxAddField(registry, rtti_matlab.first.c_str()); + if(fieldId < 0) + mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); + mxArray *matlabName = mxCreateString(rtti_matlab.second.c_str()); + mxSetFieldByNumber(registry, 0, fieldId, matlabName); + } + if(mexPutVariable("global", "gtsamwrap_rttiRegistry", registry) != 0) + mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); + mxDestroyArray(registry); + + mxArray *newAlreadyCreated = mxCreateNumericMatrix(0, 0, mxINT8_CLASS, mxREAL); + if(mexPutVariable("global", "gtsam_geometry_rttiRegistry_created", newAlreadyCreated) != 0) + mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); + mxDestroyArray(newAlreadyCreated); + } +} + +void gtsamPoint2_collectorInsertAndMakeBase_0(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + Shared *self = *reinterpret_cast (mxGetData(in[0])); + collector_gtsamPoint2.insert(self); +} + +void gtsamPoint2_constructor_1(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + Shared *self = new Shared(new gtsam::Point2()); + collector_gtsamPoint2.insert(self); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast (mxGetData(out[0])) = self; +} + +void gtsamPoint2_constructor_2(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + double x = unwrap< double >(in[0]); + double y = unwrap< double >(in[1]); + Shared *self = new Shared(new gtsam::Point2(x,y)); + collector_gtsamPoint2.insert(self); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast (mxGetData(out[0])) = self; +} + +void gtsamPoint2_deconstructor_3(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("delete_gtsamPoint2",nargout,nargin,1); + Shared *self = *reinterpret_cast(mxGetData(in[0])); + Collector_gtsamPoint2::iterator item; + item = collector_gtsamPoint2.find(self); + if(item != collector_gtsamPoint2.end()) { + delete self; + collector_gtsamPoint2.erase(item); + } +} + +void gtsamPoint2_argChar_4(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("argChar",nargout,nargin-1,1); + Shared obj = unwrap_shared_ptr(in[0], "ptr_gtsamPoint2"); + char a = unwrap< char >(in[1]); + obj->argChar(a); +} + +void gtsamPoint2_argUChar_5(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("argUChar",nargout,nargin-1,1); + Shared obj = unwrap_shared_ptr(in[0], "ptr_gtsamPoint2"); + unsigned char a = unwrap< unsigned char >(in[1]); + obj->argUChar(a); +} + +void gtsamPoint2_dim_6(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("dim",nargout,nargin-1,0); + Shared obj = unwrap_shared_ptr(in[0], "ptr_gtsamPoint2"); + out[0] = wrap< int >(obj->dim()); +} + +void gtsamPoint2_returnChar_7(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("returnChar",nargout,nargin-1,0); + Shared obj = unwrap_shared_ptr(in[0], "ptr_gtsamPoint2"); + out[0] = wrap< char >(obj->returnChar()); +} + +void gtsamPoint2_vectorConfusion_8(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr SharedVectorNotEigen; + typedef boost::shared_ptr Shared; + checkArguments("vectorConfusion",nargout,nargin-1,0); + Shared obj = unwrap_shared_ptr(in[0], "ptr_gtsamPoint2"); + out[0] = wrap_shared_ptr(SharedVectorNotEigen(new VectorNotEigen(obj->vectorConfusion())),"VectorNotEigen", false); +} + +void gtsamPoint2_x_9(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("x",nargout,nargin-1,0); + Shared obj = unwrap_shared_ptr(in[0], "ptr_gtsamPoint2"); + out[0] = wrap< double >(obj->x()); +} + +void gtsamPoint2_y_10(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("y",nargout,nargin-1,0); + Shared obj = unwrap_shared_ptr(in[0], "ptr_gtsamPoint2"); + out[0] = wrap< double >(obj->y()); +} + +void gtsamPoint3_collectorInsertAndMakeBase_11(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + Shared *self = *reinterpret_cast (mxGetData(in[0])); + collector_gtsamPoint3.insert(self); +} + +void gtsamPoint3_constructor_12(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + double x = unwrap< double >(in[0]); + double y = unwrap< double >(in[1]); + double z = unwrap< double >(in[2]); + Shared *self = new Shared(new gtsam::Point3(x,y,z)); + collector_gtsamPoint3.insert(self); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast (mxGetData(out[0])) = self; +} + +void gtsamPoint3_deconstructor_13(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("delete_gtsamPoint3",nargout,nargin,1); + Shared *self = *reinterpret_cast(mxGetData(in[0])); + Collector_gtsamPoint3::iterator item; + item = collector_gtsamPoint3.find(self); + if(item != collector_gtsamPoint3.end()) { + delete self; + collector_gtsamPoint3.erase(item); + } +} + +void gtsamPoint3_norm_14(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("norm",nargout,nargin-1,0); + Shared obj = unwrap_shared_ptr(in[0], "ptr_gtsamPoint3"); + out[0] = wrap< double >(obj->norm()); +} + +void gtsamPoint3_StaticFunctionRet_15(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr SharedPoint3; + typedef boost::shared_ptr Shared; + checkArguments("gtsamPoint3.StaticFunctionRet",nargout,nargin,1); + double z = unwrap< double >(in[0]); + out[0] = wrap_shared_ptr(SharedPoint3(new gtsam::Point3(gtsam::Point3::StaticFunctionRet(z))),"gtsam.Point3", false); +} + +void gtsamPoint3_staticFunction_16(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("gtsamPoint3.staticFunction",nargout,nargin,0); + out[0] = wrap< double >(gtsam::Point3::staticFunction()); +} + +void Test_collectorInsertAndMakeBase_17(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + Shared *self = *reinterpret_cast (mxGetData(in[0])); + collector_Test.insert(self); +} + +void Test_constructor_18(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + Shared *self = new Shared(new Test()); + collector_Test.insert(self); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast (mxGetData(out[0])) = self; +} + +void Test_constructor_19(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + double a = unwrap< double >(in[0]); + Matrix b = unwrap< Matrix >(in[1]); + Shared *self = new Shared(new Test(a,b)); + collector_Test.insert(self); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast (mxGetData(out[0])) = self; +} + +void Test_deconstructor_20(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("delete_Test",nargout,nargin,1); + Shared *self = *reinterpret_cast(mxGetData(in[0])); + Collector_Test::iterator item; + item = collector_Test.find(self); + if(item != collector_Test.end()) { + delete self; + collector_Test.erase(item); + } +} + +void Test_arg_EigenConstRef_21(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("arg_EigenConstRef",nargout,nargin-1,1); + Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + Matrix& value = *unwrap_shared_ptr< Matrix >(in[1], "ptr_Matrix"); + obj->arg_EigenConstRef(value); +} + +void Test_create_MixedPtrs_22(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr SharedTest; + typedef boost::shared_ptr SharedTest; + typedef boost::shared_ptr Shared; + checkArguments("create_MixedPtrs",nargout,nargin-1,0); + Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + pair< Test, SharedTest > pairResult = obj->create_MixedPtrs(); + out[0] = wrap_shared_ptr(SharedTest(new Test(pairResult.first)),"Test", false); + out[1] = wrap_shared_ptr(pairResult.second,"Test", false); +} + +void Test_create_ptrs_23(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr SharedTest; + typedef boost::shared_ptr SharedTest; + typedef boost::shared_ptr Shared; + checkArguments("create_ptrs",nargout,nargin-1,0); + Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + pair< SharedTest, SharedTest > pairResult = obj->create_ptrs(); + out[0] = wrap_shared_ptr(pairResult.first,"Test", false); + out[1] = wrap_shared_ptr(pairResult.second,"Test", false); +} + +void Test_print_24(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("print",nargout,nargin-1,0); + Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + obj->print(); +} + +void Test_return_Point2Ptr_25(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr SharedPoint2; + typedef boost::shared_ptr Shared; + checkArguments("return_Point2Ptr",nargout,nargin-1,1); + Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + bool value = unwrap< bool >(in[1]); + out[0] = wrap_shared_ptr(obj->return_Point2Ptr(value),"gtsam.Point2", false); +} + +void Test_return_Test_26(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr SharedTest; + typedef boost::shared_ptr Shared; + checkArguments("return_Test",nargout,nargin-1,1); + Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + boost::shared_ptr value = unwrap_shared_ptr< Test >(in[1], "ptr_Test"); + out[0] = wrap_shared_ptr(SharedTest(new Test(obj->return_Test(value))),"Test", false); +} + +void Test_return_TestPtr_27(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr SharedTest; + typedef boost::shared_ptr Shared; + checkArguments("return_TestPtr",nargout,nargin-1,1); + Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + boost::shared_ptr value = unwrap_shared_ptr< Test >(in[1], "ptr_Test"); + out[0] = wrap_shared_ptr(obj->return_TestPtr(value),"Test", false); +} + +void Test_return_bool_28(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("return_bool",nargout,nargin-1,1); + Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + bool value = unwrap< bool >(in[1]); + out[0] = wrap< bool >(obj->return_bool(value)); +} + +void Test_return_double_29(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("return_double",nargout,nargin-1,1); + Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + double value = unwrap< double >(in[1]); + out[0] = wrap< double >(obj->return_double(value)); +} + +void Test_return_field_30(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("return_field",nargout,nargin-1,1); + Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + Test& t = *unwrap_shared_ptr< Test >(in[1], "ptr_Test"); + out[0] = wrap< bool >(obj->return_field(t)); +} + +void Test_return_int_31(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("return_int",nargout,nargin-1,1); + Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + int value = unwrap< int >(in[1]); + out[0] = wrap< int >(obj->return_int(value)); +} + +void Test_return_matrix1_32(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("return_matrix1",nargout,nargin-1,1); + Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + Matrix value = unwrap< Matrix >(in[1]); + out[0] = wrap< Matrix >(obj->return_matrix1(value)); +} + +void Test_return_matrix2_33(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("return_matrix2",nargout,nargin-1,1); + Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + Matrix value = unwrap< Matrix >(in[1]); + out[0] = wrap< Matrix >(obj->return_matrix2(value)); +} + +void Test_return_pair_34(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("return_pair",nargout,nargin-1,2); + Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + Vector v = unwrap< Vector >(in[1]); + Matrix A = unwrap< Matrix >(in[2]); + pair< Vector, Matrix > pairResult = obj->return_pair(v,A); + out[0] = wrap< Vector >(pairResult.first); + out[1] = wrap< Matrix >(pairResult.second); +} + +void Test_return_ptrs_35(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr SharedTest; + typedef boost::shared_ptr SharedTest; + typedef boost::shared_ptr Shared; + checkArguments("return_ptrs",nargout,nargin-1,2); + Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + boost::shared_ptr p1 = unwrap_shared_ptr< Test >(in[1], "ptr_Test"); + boost::shared_ptr p2 = unwrap_shared_ptr< Test >(in[2], "ptr_Test"); + pair< SharedTest, SharedTest > pairResult = obj->return_ptrs(p1,p2); + out[0] = wrap_shared_ptr(pairResult.first,"Test", false); + out[1] = wrap_shared_ptr(pairResult.second,"Test", false); +} + +void Test_return_size_t_36(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("return_size_t",nargout,nargin-1,1); + Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + size_t value = unwrap< size_t >(in[1]); + out[0] = wrap< size_t >(obj->return_size_t(value)); +} + +void Test_return_string_37(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("return_string",nargout,nargin-1,1); + Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + string value = unwrap< string >(in[1]); + out[0] = wrap< string >(obj->return_string(value)); +} + +void Test_return_vector1_38(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("return_vector1",nargout,nargin-1,1); + Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + Vector value = unwrap< Vector >(in[1]); + out[0] = wrap< Vector >(obj->return_vector1(value)); +} + +void Test_return_vector2_39(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("return_vector2",nargout,nargin-1,1); + Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + Vector value = unwrap< Vector >(in[1]); + out[0] = wrap< Vector >(obj->return_vector2(value)); +} + +void MyBase_collectorInsertAndMakeBase_40(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + Shared *self = *reinterpret_cast (mxGetData(in[0])); + collector_MyBase.insert(self); +} + +void MyBase_upcastFromVoid_41(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + boost::shared_ptr *asVoid = *reinterpret_cast**> (mxGetData(in[0])); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + Shared *self = new Shared(boost::static_pointer_cast(*asVoid)); + *reinterpret_cast(mxGetData(out[0])) = self; +} + +void MyBase_deconstructor_42(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("delete_MyBase",nargout,nargin,1); + Shared *self = *reinterpret_cast(mxGetData(in[0])); + Collector_MyBase::iterator item; + item = collector_MyBase.find(self); + if(item != collector_MyBase.end()) { + delete self; + collector_MyBase.erase(item); + } +} + +void MyTemplatePoint2_collectorInsertAndMakeBase_43(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + Shared *self = *reinterpret_cast (mxGetData(in[0])); + collector_MyTemplatePoint2.insert(self); + + typedef boost::shared_ptr SharedBase; + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast(mxGetData(out[0])) = new SharedBase(*self); +} + +void MyTemplatePoint2_upcastFromVoid_44(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + boost::shared_ptr *asVoid = *reinterpret_cast**> (mxGetData(in[0])); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + Shared *self = new Shared(boost::static_pointer_cast(*asVoid)); + *reinterpret_cast(mxGetData(out[0])) = self; +} + +void MyTemplatePoint2_constructor_45(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + Shared *self = new Shared(new MyTemplatePoint2()); + collector_MyTemplatePoint2.insert(self); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast (mxGetData(out[0])) = self; + + typedef boost::shared_ptr SharedBase; + out[1] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast(mxGetData(out[1])) = new SharedBase(*self); +} + +void MyTemplatePoint2_deconstructor_46(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("delete_MyTemplatePoint2",nargout,nargin,1); + Shared *self = *reinterpret_cast(mxGetData(in[0])); + Collector_MyTemplatePoint2::iterator item; + item = collector_MyTemplatePoint2.find(self); + if(item != collector_MyTemplatePoint2.end()) { + delete self; + collector_MyTemplatePoint2.erase(item); + } +} + +void MyTemplatePoint2_accept_T_47(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("accept_T",nargout,nargin-1,1); + Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); + gtsam::Point2& value = *unwrap_shared_ptr< gtsam::Point2 >(in[1], "ptr_gtsamPoint2"); + obj->accept_T(value); +} + +void MyTemplatePoint2_accept_Tptr_48(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("accept_Tptr",nargout,nargin-1,1); + Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); + boost::shared_ptr value = unwrap_shared_ptr< gtsam::Point2 >(in[1], "ptr_gtsamPoint2"); + obj->accept_Tptr(value); +} + +void MyTemplatePoint2_create_MixedPtrs_49(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr SharedPoint2; + typedef boost::shared_ptr SharedPoint2; + typedef boost::shared_ptr Shared; + checkArguments("create_MixedPtrs",nargout,nargin-1,0); + Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); + pair< gtsam::Point2, SharedPoint2 > pairResult = obj->create_MixedPtrs(); + out[0] = wrap_shared_ptr(SharedPoint2(new gtsam::Point2(pairResult.first)),"gtsam.Point2", false); + out[1] = wrap_shared_ptr(pairResult.second,"gtsam.Point2", false); +} + +void MyTemplatePoint2_create_ptrs_50(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr SharedPoint2; + typedef boost::shared_ptr SharedPoint2; + typedef boost::shared_ptr Shared; + checkArguments("create_ptrs",nargout,nargin-1,0); + Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); + pair< SharedPoint2, SharedPoint2 > pairResult = obj->create_ptrs(); + out[0] = wrap_shared_ptr(pairResult.first,"gtsam.Point2", false); + out[1] = wrap_shared_ptr(pairResult.second,"gtsam.Point2", false); +} + +void MyTemplatePoint2_return_T_51(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr SharedPoint2; + typedef boost::shared_ptr Shared; + checkArguments("return_T",nargout,nargin-1,1); + Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); + boost::shared_ptr value = unwrap_shared_ptr< gtsam::Point2 >(in[1], "ptr_gtsamPoint2"); + out[0] = wrap_shared_ptr(SharedPoint2(new gtsam::Point2(obj->return_T(value))),"gtsam.Point2", false); +} + +void MyTemplatePoint2_return_Tptr_52(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr SharedPoint2; + typedef boost::shared_ptr Shared; + checkArguments("return_Tptr",nargout,nargin-1,1); + Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); + boost::shared_ptr value = unwrap_shared_ptr< gtsam::Point2 >(in[1], "ptr_gtsamPoint2"); + out[0] = wrap_shared_ptr(obj->return_Tptr(value),"gtsam.Point2", false); +} + +void MyTemplatePoint2_return_ptrs_53(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr SharedPoint2; + typedef boost::shared_ptr SharedPoint2; + typedef boost::shared_ptr Shared; + checkArguments("return_ptrs",nargout,nargin-1,2); + Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); + boost::shared_ptr p1 = unwrap_shared_ptr< gtsam::Point2 >(in[1], "ptr_gtsamPoint2"); + boost::shared_ptr p2 = unwrap_shared_ptr< gtsam::Point2 >(in[2], "ptr_gtsamPoint2"); + pair< SharedPoint2, SharedPoint2 > pairResult = obj->return_ptrs(p1,p2); + out[0] = wrap_shared_ptr(pairResult.first,"gtsam.Point2", false); + out[1] = wrap_shared_ptr(pairResult.second,"gtsam.Point2", false); +} + +void MyTemplatePoint2_templatedMethod_54(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("templatedMethod",nargout,nargin-1,1); + Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); + gtsam::Point2& t = *unwrap_shared_ptr< gtsam::Point2 >(in[1], "ptr_gtsamPoint2"); + obj->templatedMethod(t); +} + +void MyTemplatePoint2_templatedMethod_55(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("templatedMethod",nargout,nargin-1,1); + Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); + gtsam::Point3& t = *unwrap_shared_ptr< gtsam::Point3 >(in[1], "ptr_gtsamPoint3"); + obj->templatedMethod(t); +} + +void MyTemplatePoint3_collectorInsertAndMakeBase_56(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + Shared *self = *reinterpret_cast (mxGetData(in[0])); + collector_MyTemplatePoint3.insert(self); + + typedef boost::shared_ptr SharedBase; + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast(mxGetData(out[0])) = new SharedBase(*self); +} + +void MyTemplatePoint3_upcastFromVoid_57(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + boost::shared_ptr *asVoid = *reinterpret_cast**> (mxGetData(in[0])); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + Shared *self = new Shared(boost::static_pointer_cast(*asVoid)); + *reinterpret_cast(mxGetData(out[0])) = self; +} + +void MyTemplatePoint3_constructor_58(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + Shared *self = new Shared(new MyTemplatePoint3()); + collector_MyTemplatePoint3.insert(self); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast (mxGetData(out[0])) = self; + + typedef boost::shared_ptr SharedBase; + out[1] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast(mxGetData(out[1])) = new SharedBase(*self); +} + +void MyTemplatePoint3_deconstructor_59(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("delete_MyTemplatePoint3",nargout,nargin,1); + Shared *self = *reinterpret_cast(mxGetData(in[0])); + Collector_MyTemplatePoint3::iterator item; + item = collector_MyTemplatePoint3.find(self); + if(item != collector_MyTemplatePoint3.end()) { + delete self; + collector_MyTemplatePoint3.erase(item); + } +} + +void MyTemplatePoint3_accept_T_60(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("accept_T",nargout,nargin-1,1); + Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint3"); + gtsam::Point3& value = *unwrap_shared_ptr< gtsam::Point3 >(in[1], "ptr_gtsamPoint3"); + obj->accept_T(value); +} + +void MyTemplatePoint3_accept_Tptr_61(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("accept_Tptr",nargout,nargin-1,1); + Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint3"); + boost::shared_ptr value = unwrap_shared_ptr< gtsam::Point3 >(in[1], "ptr_gtsamPoint3"); + obj->accept_Tptr(value); +} + +void MyTemplatePoint3_create_MixedPtrs_62(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr SharedPoint3; + typedef boost::shared_ptr SharedPoint3; + typedef boost::shared_ptr Shared; + checkArguments("create_MixedPtrs",nargout,nargin-1,0); + Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint3"); + pair< gtsam::Point3, SharedPoint3 > pairResult = obj->create_MixedPtrs(); + out[0] = wrap_shared_ptr(SharedPoint3(new gtsam::Point3(pairResult.first)),"gtsam.Point3", false); + out[1] = wrap_shared_ptr(pairResult.second,"gtsam.Point3", false); +} + +void MyTemplatePoint3_create_ptrs_63(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr SharedPoint3; + typedef boost::shared_ptr SharedPoint3; + typedef boost::shared_ptr Shared; + checkArguments("create_ptrs",nargout,nargin-1,0); + Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint3"); + pair< SharedPoint3, SharedPoint3 > pairResult = obj->create_ptrs(); + out[0] = wrap_shared_ptr(pairResult.first,"gtsam.Point3", false); + out[1] = wrap_shared_ptr(pairResult.second,"gtsam.Point3", false); +} + +void MyTemplatePoint3_return_T_64(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr SharedPoint3; + typedef boost::shared_ptr Shared; + checkArguments("return_T",nargout,nargin-1,1); + Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint3"); + boost::shared_ptr value = unwrap_shared_ptr< gtsam::Point3 >(in[1], "ptr_gtsamPoint3"); + out[0] = wrap_shared_ptr(SharedPoint3(new gtsam::Point3(obj->return_T(value))),"gtsam.Point3", false); +} + +void MyTemplatePoint3_return_Tptr_65(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr SharedPoint3; + typedef boost::shared_ptr Shared; + checkArguments("return_Tptr",nargout,nargin-1,1); + Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint3"); + boost::shared_ptr value = unwrap_shared_ptr< gtsam::Point3 >(in[1], "ptr_gtsamPoint3"); + out[0] = wrap_shared_ptr(obj->return_Tptr(value),"gtsam.Point3", false); +} + +void MyTemplatePoint3_return_ptrs_66(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr SharedPoint3; + typedef boost::shared_ptr SharedPoint3; + typedef boost::shared_ptr Shared; + checkArguments("return_ptrs",nargout,nargin-1,2); + Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint3"); + boost::shared_ptr p1 = unwrap_shared_ptr< gtsam::Point3 >(in[1], "ptr_gtsamPoint3"); + boost::shared_ptr p2 = unwrap_shared_ptr< gtsam::Point3 >(in[2], "ptr_gtsamPoint3"); + pair< SharedPoint3, SharedPoint3 > pairResult = obj->return_ptrs(p1,p2); + out[0] = wrap_shared_ptr(pairResult.first,"gtsam.Point3", false); + out[1] = wrap_shared_ptr(pairResult.second,"gtsam.Point3", false); +} + +void MyTemplatePoint3_templatedMethod_67(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("templatedMethod",nargout,nargin-1,1); + Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint3"); + gtsam::Point2& t = *unwrap_shared_ptr< gtsam::Point2 >(in[1], "ptr_gtsamPoint2"); + obj->templatedMethod(t); +} + +void MyTemplatePoint3_templatedMethod_68(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("templatedMethod",nargout,nargin-1,1); + Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint3"); + gtsam::Point3& t = *unwrap_shared_ptr< gtsam::Point3 >(in[1], "ptr_gtsamPoint3"); + obj->templatedMethod(t); +} + +void MyFactorPosePoint2_collectorInsertAndMakeBase_69(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + Shared *self = *reinterpret_cast (mxGetData(in[0])); + collector_MyFactorPosePoint2.insert(self); +} + +void MyFactorPosePoint2_constructor_70(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + size_t key1 = unwrap< size_t >(in[0]); + size_t key2 = unwrap< size_t >(in[1]); + double measured = unwrap< double >(in[2]); + boost::shared_ptr noiseModel = unwrap_shared_ptr< gtsam::noiseModel::Base >(in[3], "ptr_gtsamnoiseModelBase"); + Shared *self = new Shared(new MyFactorPosePoint2(key1,key2,measured,noiseModel)); + collector_MyFactorPosePoint2.insert(self); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast (mxGetData(out[0])) = self; +} + +void MyFactorPosePoint2_deconstructor_71(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("delete_MyFactorPosePoint2",nargout,nargin,1); + Shared *self = *reinterpret_cast(mxGetData(in[0])); + Collector_MyFactorPosePoint2::iterator item; + item = collector_MyFactorPosePoint2.find(self); + if(item != collector_MyFactorPosePoint2.end()) { + delete self; + collector_MyFactorPosePoint2.erase(item); + } +} + +void aGlobalFunction_72(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("aGlobalFunction",nargout,nargin,0); + out[0] = wrap< Vector >(aGlobalFunction()); +} +void overloadedGlobalFunction_73(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("overloadedGlobalFunction",nargout,nargin,1); + int a = unwrap< int >(in[0]); + out[0] = wrap< Vector >(overloadedGlobalFunction(a)); +} +void overloadedGlobalFunction_74(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("overloadedGlobalFunction",nargout,nargin,2); + int a = unwrap< int >(in[0]); + double b = unwrap< double >(in[1]); + out[0] = wrap< Vector >(overloadedGlobalFunction(a,b)); +} + +void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mstream mout; + std::streambuf *outbuf = std::cout.rdbuf(&mout); + + _geometry_RTTIRegister(); + + int id = unwrap(in[0]); + + try { + switch(id) { + case 0: + gtsamPoint2_collectorInsertAndMakeBase_0(nargout, out, nargin-1, in+1); + break; + case 1: + gtsamPoint2_constructor_1(nargout, out, nargin-1, in+1); + break; + case 2: + gtsamPoint2_constructor_2(nargout, out, nargin-1, in+1); + break; + case 3: + gtsamPoint2_deconstructor_3(nargout, out, nargin-1, in+1); + break; + case 4: + gtsamPoint2_argChar_4(nargout, out, nargin-1, in+1); + break; + case 5: + gtsamPoint2_argUChar_5(nargout, out, nargin-1, in+1); + break; + case 6: + gtsamPoint2_dim_6(nargout, out, nargin-1, in+1); + break; + case 7: + gtsamPoint2_returnChar_7(nargout, out, nargin-1, in+1); + break; + case 8: + gtsamPoint2_vectorConfusion_8(nargout, out, nargin-1, in+1); + break; + case 9: + gtsamPoint2_x_9(nargout, out, nargin-1, in+1); + break; + case 10: + gtsamPoint2_y_10(nargout, out, nargin-1, in+1); + break; + case 11: + gtsamPoint3_collectorInsertAndMakeBase_11(nargout, out, nargin-1, in+1); + break; + case 12: + gtsamPoint3_constructor_12(nargout, out, nargin-1, in+1); + break; + case 13: + gtsamPoint3_deconstructor_13(nargout, out, nargin-1, in+1); + break; + case 14: + gtsamPoint3_norm_14(nargout, out, nargin-1, in+1); + break; + case 15: + gtsamPoint3_StaticFunctionRet_15(nargout, out, nargin-1, in+1); + break; + case 16: + gtsamPoint3_staticFunction_16(nargout, out, nargin-1, in+1); + break; + case 17: + Test_collectorInsertAndMakeBase_17(nargout, out, nargin-1, in+1); + break; + case 18: + Test_constructor_18(nargout, out, nargin-1, in+1); + break; + case 19: + Test_constructor_19(nargout, out, nargin-1, in+1); + break; + case 20: + Test_deconstructor_20(nargout, out, nargin-1, in+1); + break; + case 21: + Test_arg_EigenConstRef_21(nargout, out, nargin-1, in+1); + break; + case 22: + Test_create_MixedPtrs_22(nargout, out, nargin-1, in+1); + break; + case 23: + Test_create_ptrs_23(nargout, out, nargin-1, in+1); + break; + case 24: + Test_print_24(nargout, out, nargin-1, in+1); + break; + case 25: + Test_return_Point2Ptr_25(nargout, out, nargin-1, in+1); + break; + case 26: + Test_return_Test_26(nargout, out, nargin-1, in+1); + break; + case 27: + Test_return_TestPtr_27(nargout, out, nargin-1, in+1); + break; + case 28: + Test_return_bool_28(nargout, out, nargin-1, in+1); + break; + case 29: + Test_return_double_29(nargout, out, nargin-1, in+1); + break; + case 30: + Test_return_field_30(nargout, out, nargin-1, in+1); + break; + case 31: + Test_return_int_31(nargout, out, nargin-1, in+1); + break; + case 32: + Test_return_matrix1_32(nargout, out, nargin-1, in+1); + break; + case 33: + Test_return_matrix2_33(nargout, out, nargin-1, in+1); + break; + case 34: + Test_return_pair_34(nargout, out, nargin-1, in+1); + break; + case 35: + Test_return_ptrs_35(nargout, out, nargin-1, in+1); + break; + case 36: + Test_return_size_t_36(nargout, out, nargin-1, in+1); + break; + case 37: + Test_return_string_37(nargout, out, nargin-1, in+1); + break; + case 38: + Test_return_vector1_38(nargout, out, nargin-1, in+1); + break; + case 39: + Test_return_vector2_39(nargout, out, nargin-1, in+1); + break; + case 40: + MyBase_collectorInsertAndMakeBase_40(nargout, out, nargin-1, in+1); + break; + case 41: + MyBase_upcastFromVoid_41(nargout, out, nargin-1, in+1); + break; + case 42: + MyBase_deconstructor_42(nargout, out, nargin-1, in+1); + break; + case 43: + MyTemplatePoint2_collectorInsertAndMakeBase_43(nargout, out, nargin-1, in+1); + break; + case 44: + MyTemplatePoint2_upcastFromVoid_44(nargout, out, nargin-1, in+1); + break; + case 45: + MyTemplatePoint2_constructor_45(nargout, out, nargin-1, in+1); + break; + case 46: + MyTemplatePoint2_deconstructor_46(nargout, out, nargin-1, in+1); + break; + case 47: + MyTemplatePoint2_accept_T_47(nargout, out, nargin-1, in+1); + break; + case 48: + MyTemplatePoint2_accept_Tptr_48(nargout, out, nargin-1, in+1); + break; + case 49: + MyTemplatePoint2_create_MixedPtrs_49(nargout, out, nargin-1, in+1); + break; + case 50: + MyTemplatePoint2_create_ptrs_50(nargout, out, nargin-1, in+1); + break; + case 51: + MyTemplatePoint2_return_T_51(nargout, out, nargin-1, in+1); + break; + case 52: + MyTemplatePoint2_return_Tptr_52(nargout, out, nargin-1, in+1); + break; + case 53: + MyTemplatePoint2_return_ptrs_53(nargout, out, nargin-1, in+1); + break; + case 54: + MyTemplatePoint2_templatedMethod_54(nargout, out, nargin-1, in+1); + break; + case 55: + MyTemplatePoint2_templatedMethod_55(nargout, out, nargin-1, in+1); + break; + case 56: + MyTemplatePoint3_collectorInsertAndMakeBase_56(nargout, out, nargin-1, in+1); + break; + case 57: + MyTemplatePoint3_upcastFromVoid_57(nargout, out, nargin-1, in+1); + break; + case 58: + MyTemplatePoint3_constructor_58(nargout, out, nargin-1, in+1); + break; + case 59: + MyTemplatePoint3_deconstructor_59(nargout, out, nargin-1, in+1); + break; + case 60: + MyTemplatePoint3_accept_T_60(nargout, out, nargin-1, in+1); + break; + case 61: + MyTemplatePoint3_accept_Tptr_61(nargout, out, nargin-1, in+1); + break; + case 62: + MyTemplatePoint3_create_MixedPtrs_62(nargout, out, nargin-1, in+1); + break; + case 63: + MyTemplatePoint3_create_ptrs_63(nargout, out, nargin-1, in+1); + break; + case 64: + MyTemplatePoint3_return_T_64(nargout, out, nargin-1, in+1); + break; + case 65: + MyTemplatePoint3_return_Tptr_65(nargout, out, nargin-1, in+1); + break; + case 66: + MyTemplatePoint3_return_ptrs_66(nargout, out, nargin-1, in+1); + break; + case 67: + MyTemplatePoint3_templatedMethod_67(nargout, out, nargin-1, in+1); + break; + case 68: + MyTemplatePoint3_templatedMethod_68(nargout, out, nargin-1, in+1); + break; + case 69: + MyFactorPosePoint2_collectorInsertAndMakeBase_69(nargout, out, nargin-1, in+1); + break; + case 70: + MyFactorPosePoint2_constructor_70(nargout, out, nargin-1, in+1); + break; + case 71: + MyFactorPosePoint2_deconstructor_71(nargout, out, nargin-1, in+1); + break; + case 72: + aGlobalFunction_72(nargout, out, nargin-1, in+1); + break; + case 73: + overloadedGlobalFunction_73(nargout, out, nargin-1, in+1); + break; + case 74: + overloadedGlobalFunction_74(nargout, out, nargin-1, in+1); + break; + } + } catch(const std::exception& e) { + mexErrMsgTxt(("Exception from gtsam:\n" + std::string(e.what()) + "\n").c_str()); + } + + std::cout.rdbuf(outbuf); +} diff --git a/wrap/tests/expected2/overloadedGlobalFunction.m b/wrap/tests/expected2/overloadedGlobalFunction.m new file mode 100644 index 000000000..24758ed6e --- /dev/null +++ b/wrap/tests/expected2/overloadedGlobalFunction.m @@ -0,0 +1,8 @@ +function varargout = overloadedGlobalFunction(varargin) + if length(varargin) == 1 && isa(varargin{1},'numeric') + varargout{1} = geometry_wrapper(73, varargin{:}); + elseif length(varargin) == 2 && isa(varargin{1},'numeric') && isa(varargin{2},'double') + varargout{1} = geometry_wrapper(74, varargin{:}); + else + error('Arguments do not match any overload of function overloadedGlobalFunction'); + end diff --git a/wrap/tests/expected_namespaces/+ns2/ClassA.m b/wrap/tests/expected_namespaces/+ns2/ClassA.m index 9c064734e..14095899c 100644 --- a/wrap/tests/expected_namespaces/+ns2/ClassA.m +++ b/wrap/tests/expected_namespaces/+ns2/ClassA.m @@ -65,14 +65,7 @@ classdef ClassA < handle function varargout = Afunction(varargin) % AFUNCTION usage: afunction() : returns double % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Usage - % AFUNCTION() - if length(varargin) == 0 - varargout{1} = testNamespaces_wrapper(12, varargin{:}); - else - error('Arguments do not match any overload of function ns2.ClassA.Afunction'); - end + varargout{1} = testNamespaces_wrapper(12, varargin{:}); end end diff --git a/wrap/tests/geometry.h b/wrap/tests/geometry.h index bc233763d..5a6cee1a5 100644 --- a/wrap/tests/geometry.h +++ b/wrap/tests/geometry.h @@ -3,6 +3,8 @@ class VectorNotEigen; class ns::OtherClass; +namespace gtsam { + class Point2 { Point2(); Point2(double x, double y); @@ -23,12 +25,13 @@ class Point3 { // static functions - use static keyword and uppercase static double staticFunction(); - static Point3 StaticFunctionRet(double z); + static gtsam::Point3 StaticFunctionRet(double z); // enabling serialization functionality void serialize() const; // Just triggers a flag internally and removes actual function }; +} // another comment // another comment @@ -71,7 +74,7 @@ class Test { Test* return_TestPtr(Test* value) const; Test return_Test(Test* value) const; - Point2* return_Point2Ptr(bool value) const; + gtsam::Point2* return_Point2Ptr(bool value) const; pair create_ptrs () const; pair create_MixedPtrs () const; @@ -91,6 +94,38 @@ Vector aGlobalFunction(); Vector overloadedGlobalFunction(int a); Vector overloadedGlobalFunction(int a, double b); +// A base class +virtual class MyBase { + +}; + +// A templated class +template +virtual class MyTemplate : MyBase { + MyTemplate(); + + template + void templatedMethod(const ARG& t); + + // Stress test templates and pointer combinations + void accept_T(const T& value) const; + void accept_Tptr(T* value) const; + T* return_Tptr(T* value) const; + T return_T(T* value) const; + pair create_ptrs () const; + pair create_MixedPtrs () const; + pair return_ptrs (T* p1, T* p2) const; +}; + +// A doubly templated class +template +class MyFactor { + MyFactor(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base* noiseModel); +}; + +// and a typedef specializing it +typedef MyFactor MyFactorPosePoint2; + // comments at the end! // even more comments at the end! diff --git a/wrap/tests/testClass.cpp b/wrap/tests/testClass.cpp new file mode 100644 index 000000000..d68daf4ba --- /dev/null +++ b/wrap/tests/testClass.cpp @@ -0,0 +1,86 @@ +/* ---------------------------------------------------------------------------- + + * 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 testClass.cpp + * @brief Unit test for Class class + * @author Frank Dellaert + * @date Nov 12, 2014 + **/ + +#include +#include +#include + +using namespace std; +using namespace wrap; + +/* ************************************************************************* */ +// Constructor +TEST( Class, Constructor ) { + Class cls; +} + +/* ************************************************************************* */ +// test method overloading +TEST( Class, OverloadingMethod ) { + Class cls; + const string name = "method1"; + EXPECT(!cls.exists(name)); + + bool verbose = true, is_const = true; + ArgumentList args; + const ReturnValue retVal; + const string templateArgName; + vector templateArgValues; + cls.addMethod(verbose, is_const, name, args, retVal, templateArgName, + templateArgValues); + EXPECT_LONGS_EQUAL(1, cls.nrMethods()); + EXPECT(cls.exists(name)); + Method& method = cls.method(name); + EXPECT_LONGS_EQUAL(1, method.nrOverloads()); + + cls.addMethod(verbose, is_const, name, args, retVal, templateArgName, + templateArgValues); + EXPECT_LONGS_EQUAL(1, cls.nrMethods()); + EXPECT_LONGS_EQUAL(2, method.nrOverloads()); +} + +/* ************************************************************************* */ +// test templated methods +TEST( Class, TemplatedMethods ) { + Class cls; + const string name = "method"; + EXPECT(!cls.exists(name)); + + bool verbose = true, is_const = true; + ArgumentList args; + Argument arg; + arg.type.name = "T"; + args.push_back(arg); + const ReturnValue retVal(ReturnType("T")); + const string templateArgName("T"); + vector templateArgValues; + templateArgValues.push_back(Qualified("Point2")); + templateArgValues.push_back(Qualified("Point3")); + cls.addMethod(verbose, is_const, name, args, retVal, templateArgName, + templateArgValues); + EXPECT_LONGS_EQUAL(2, cls.nrMethods()); + EXPECT(cls.exists(name+"Point2")); + EXPECT(cls.exists(name+"Point3")); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/wrap/tests/testMethod.cpp b/wrap/tests/testMethod.cpp new file mode 100644 index 000000000..4067f3d85 --- /dev/null +++ b/wrap/tests/testMethod.cpp @@ -0,0 +1,48 @@ +/* ---------------------------------------------------------------------------- + + * 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 testMethod.cpp + * @brief Unit test for Method class + * @author Frank Dellaert + * @date Nov 12, 2014 + **/ + +#include +#include +#include + +using namespace std; +using namespace wrap; + +/* ************************************************************************* */ +// Constructor +TEST( Method, Constructor ) { + Method method; +} + +/* ************************************************************************* */ +// addOverload +TEST( Method, addOverload ) { + Method method("myName"); + bool verbose = true, is_const = true; + ArgumentList args; + const ReturnValue retVal(ReturnType("return_type")); + method.addOverload(verbose, is_const, "myName", args, retVal); + EXPECT_LONGS_EQUAL(1,method.nrOverloads()); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/wrap/tests/testWrap.cpp b/wrap/tests/testWrap.cpp index c6ce0903a..8fe862182 100644 --- a/wrap/tests/testWrap.cpp +++ b/wrap/tests/testWrap.cpp @@ -48,9 +48,9 @@ static const std::string headerPath = "/not_really_a_real_path/borg/gtsam/wrap"; /* ************************************************************************* */ TEST( wrap, ArgumentList ) { ArgumentList args; - Argument arg1; arg1.type = "double"; arg1.name = "x"; - Argument arg2; arg2.type = "double"; arg2.name = "y"; - Argument arg3; arg3.type = "double"; arg3.name = "z"; + Argument arg1; arg1.type.name = "double"; arg1.name = "x"; + Argument arg2; arg2.type.name = "double"; arg2.name = "y"; + Argument arg3; arg3.type.name = "double"; arg3.name = "z"; args.push_back(arg1); args.push_back(arg2); args.push_back(arg3); @@ -73,7 +73,7 @@ TEST( wrap, check_exception ) { } /* ************************************************************************* */ -TEST( wrap, small_parse ) { +TEST( wrap, Small ) { string moduleName("gtsam"); Module module(moduleName, true); @@ -92,68 +92,64 @@ TEST( wrap, small_parse ) { EXPECT(assert_equal("Point2", cls.name)); EXPECT(!cls.isVirtual); EXPECT(cls.namespaces.empty()); - LONGS_EQUAL(3, cls.methods.size()); + LONGS_EQUAL(3, cls.nrMethods()); LONGS_EQUAL(1, cls.static_methods.size()); // Method 1 - Method m1 = cls.methods.at("x"); - EXPECT(assert_equal("x", m1.name)); - EXPECT(m1.is_const_); - LONGS_EQUAL(1, m1.argLists.size()); - LONGS_EQUAL(1, m1.returnVals.size()); + Method m1 = cls.method("x"); + EXPECT(assert_equal("x", m1.name())); + EXPECT(m1.isConst()); + LONGS_EQUAL(1, m1.nrOverloads()); - ReturnValue rv1 = m1.returnVals.front(); + ReturnValue rv1 = m1.returnValue(0); EXPECT(!rv1.isPair); - EXPECT(!rv1.isPtr1); - EXPECT(assert_equal("double", rv1.type1)); - EXPECT_LONGS_EQUAL(ReturnValue::BASIS, rv1.category1); + EXPECT(!rv1.type1.isPtr); + EXPECT(assert_equal("double", rv1.type1.name)); + EXPECT_LONGS_EQUAL(ReturnType::BASIS, rv1.type1.category); // Method 2 - Method m2 = cls.methods.at("returnMatrix"); - EXPECT(assert_equal("returnMatrix", m2.name)); - EXPECT(m2.is_const_); - LONGS_EQUAL(1, m2.argLists.size()); - LONGS_EQUAL(1, m2.returnVals.size()); + Method m2 = cls.method("returnMatrix"); + EXPECT(assert_equal("returnMatrix", m2.name())); + EXPECT(m2.isConst()); + LONGS_EQUAL(1, m2.nrOverloads()); - ReturnValue rv2 = m2.returnVals.front(); + ReturnValue rv2 = m2.returnValue(0); EXPECT(!rv2.isPair); - EXPECT(!rv2.isPtr1); - EXPECT(assert_equal("Matrix", rv2.type1)); - EXPECT_LONGS_EQUAL(ReturnValue::EIGEN, rv2.category1); + EXPECT(!rv2.type1.isPtr); + EXPECT(assert_equal("Matrix", rv2.type1.name)); + EXPECT_LONGS_EQUAL(ReturnType::EIGEN, rv2.type1.category); // Method 3 - Method m3 = cls.methods.at("returnPoint2"); - EXPECT(assert_equal("returnPoint2", m3.name)); - EXPECT(m3.is_const_); - LONGS_EQUAL(1, m3.argLists.size()); - LONGS_EQUAL(1, m3.returnVals.size()); + Method m3 = cls.method("returnPoint2"); + EXPECT(assert_equal("returnPoint2", m3.name())); + EXPECT(m3.isConst()); + LONGS_EQUAL(1, m3.nrOverloads()); - ReturnValue rv3 = m3.returnVals.front(); + ReturnValue rv3 = m3.returnValue(0); EXPECT(!rv3.isPair); - EXPECT(!rv3.isPtr1); - EXPECT(assert_equal("Point2", rv3.type1)); - EXPECT_LONGS_EQUAL(ReturnValue::CLASS, rv3.category1); + EXPECT(!rv3.type1.isPtr); + EXPECT(assert_equal("Point2", rv3.type1.name)); + EXPECT_LONGS_EQUAL(ReturnType::CLASS, rv3.type1.category); // Static Method 1 // static Vector returnVector(); StaticMethod sm1 = cls.static_methods.at("returnVector"); - EXPECT(assert_equal("returnVector", sm1.name)); - LONGS_EQUAL(1, sm1.argLists.size()); - LONGS_EQUAL(1, sm1.returnVals.size()); + EXPECT(assert_equal("returnVector", sm1.name())); + LONGS_EQUAL(1, sm1.nrOverloads()); - ReturnValue rv4 = sm1.returnVals.front(); + ReturnValue rv4 = sm1.returnValue(0); EXPECT(!rv4.isPair); - EXPECT(!rv4.isPtr1); - EXPECT(assert_equal("Vector", rv4.type1)); - EXPECT_LONGS_EQUAL(ReturnValue::EIGEN, rv4.category1); + EXPECT(!rv4.type1.isPtr); + EXPECT(assert_equal("Vector", rv4.type1.name)); + EXPECT_LONGS_EQUAL(ReturnType::EIGEN, rv4.type1.category); } /* ************************************************************************* */ -TEST( wrap, parse_geometry ) { +TEST( wrap, Geometry ) { string markup_header_path = topdir + "/wrap/tests"; Module module(markup_header_path.c_str(), "geometry",enable_verbose); - EXPECT_LONGS_EQUAL(3, module.classes.size()); + EXPECT_LONGS_EQUAL(7, module.classes.size()); // forward declarations LONGS_EQUAL(2, module.forward_declarations.size()); @@ -164,9 +160,9 @@ TEST( wrap, parse_geometry ) { strvec exp_includes; exp_includes += "folder/path/to/Test.h"; EXPECT(assert_equal(exp_includes, module.includes)); - LONGS_EQUAL(3, module.classes.size()); + LONGS_EQUAL(7, module.classes.size()); - // Key for ReturnValue::return_category + // Key for ReturnType::return_category // CLASS = 1, // EIGEN = 2, // BASIS = 3, @@ -188,122 +184,126 @@ TEST( wrap, parse_geometry ) { Class cls = module.classes.at(0); EXPECT(assert_equal("Point2", cls.name)); - EXPECT_LONGS_EQUAL(2, cls.constructor.args_list.size()); - EXPECT_LONGS_EQUAL(7, cls.methods.size()); + EXPECT_LONGS_EQUAL(2, cls.constructor.nrOverloads()); + EXPECT_LONGS_EQUAL(7, cls.nrMethods()); { // char returnChar() const; - CHECK(cls.methods.find("returnChar") != cls.methods.end()); - Method m1 = cls.methods.find("returnChar")->second; - LONGS_EQUAL(1, m1.returnVals.size()); - EXPECT(assert_equal("char", m1.returnVals.front().type1)); - EXPECT_LONGS_EQUAL(ReturnValue::BASIS, m1.returnVals.front().category1); - EXPECT(!m1.returnVals.front().isPair); - EXPECT(assert_equal("returnChar", m1.name)); - LONGS_EQUAL(1, m1.argLists.size()); - EXPECT_LONGS_EQUAL(0, m1.argLists.front().size()); - EXPECT(m1.is_const_); + CHECK(cls.exists("returnChar")); + Method m1 = cls.method("returnChar"); + LONGS_EQUAL(1, m1.nrOverloads()); + EXPECT(assert_equal("char", m1.returnValue(0).type1.name)); + EXPECT_LONGS_EQUAL(ReturnType::BASIS, m1.returnValue(0).type1.category); + EXPECT(!m1.returnValue(0).isPair); + EXPECT(assert_equal("returnChar", m1.name())); + LONGS_EQUAL(1, m1.nrOverloads()); + EXPECT_LONGS_EQUAL(0, m1.argumentList(0).size()); + EXPECT(m1.isConst()); } { // VectorNotEigen vectorConfusion(); - CHECK(cls.methods.find("vectorConfusion") != cls.methods.end()); - Method m1 = cls.methods.find("vectorConfusion")->second; - LONGS_EQUAL(1, m1.returnVals.size()); - EXPECT(assert_equal("VectorNotEigen", m1.returnVals.front().type1)); - EXPECT_LONGS_EQUAL(ReturnValue::CLASS, m1.returnVals.front().category1); - EXPECT(!m1.returnVals.front().isPair); - EXPECT(assert_equal("vectorConfusion", m1.name)); - LONGS_EQUAL(1, m1.argLists.size()); - EXPECT_LONGS_EQUAL(0, m1.argLists.front().size()); - EXPECT(!m1.is_const_); + CHECK(cls.exists("vectorConfusion")); + Method m1 = cls.method("vectorConfusion"); + LONGS_EQUAL(1, m1.nrOverloads()); + EXPECT(assert_equal("VectorNotEigen", m1.returnValue(0).type1.name)); + EXPECT_LONGS_EQUAL(ReturnType::CLASS, m1.returnValue(0).type1.category); + EXPECT(!m1.returnValue(0).isPair); + EXPECT(assert_equal("vectorConfusion", m1.name())); + LONGS_EQUAL(1, m1.nrOverloads()); + EXPECT_LONGS_EQUAL(0, m1.argumentList(0).size()); + EXPECT(!m1.isConst()); } EXPECT_LONGS_EQUAL(0, cls.static_methods.size()); - EXPECT_LONGS_EQUAL(0, cls.namespaces.size()); + EXPECT_LONGS_EQUAL(1, cls.namespaces.size()); +#ifndef WRAP_DISABLE_SERIALIZE // check serialization flag EXPECT(cls.isSerializable); EXPECT(!cls.hasSerialization); - } +#endif + } // check second class, Point3 { Class cls = module.classes.at(1); EXPECT(assert_equal("Point3", cls.name)); - EXPECT_LONGS_EQUAL(1, cls.constructor.args_list.size()); - EXPECT_LONGS_EQUAL(1, cls.methods.size()); + EXPECT_LONGS_EQUAL(1, cls.constructor.nrOverloads()); + EXPECT_LONGS_EQUAL(1, cls.nrMethods()); EXPECT_LONGS_EQUAL(2, cls.static_methods.size()); - EXPECT_LONGS_EQUAL(0, cls.namespaces.size()); + EXPECT_LONGS_EQUAL(1, cls.namespaces.size()); // first constructor takes 3 doubles - ArgumentList c1 = cls.constructor.args_list.front(); + ArgumentList c1 = cls.constructor.argumentList(0); EXPECT_LONGS_EQUAL(3, c1.size()); // check first double argument Argument a1 = c1.front(); EXPECT(!a1.is_const); - EXPECT(assert_equal("double", a1.type)); + EXPECT(assert_equal("double", a1.type.name)); EXPECT(!a1.is_ref); EXPECT(assert_equal("x", a1.name)); // check method - CHECK(cls.methods.find("norm") != cls.methods.end()); - Method m1 = cls.methods.find("norm")->second; - LONGS_EQUAL(1, m1.returnVals.size()); - EXPECT(assert_equal("double", m1.returnVals.front().type1)); - EXPECT_LONGS_EQUAL(ReturnValue::BASIS, m1.returnVals.front().category1); - EXPECT(assert_equal("norm", m1.name)); - LONGS_EQUAL(1, m1.argLists.size()); - EXPECT_LONGS_EQUAL(0, m1.argLists.front().size()); - EXPECT(m1.is_const_); + CHECK(cls.exists("norm")); + Method m1 = cls.method("norm"); + LONGS_EQUAL(1, m1.nrOverloads()); + EXPECT(assert_equal("double", m1.returnValue(0).type1.name)); + EXPECT_LONGS_EQUAL(ReturnType::BASIS, m1.returnValue(0).type1.category); + EXPECT(assert_equal("norm", m1.name())); + LONGS_EQUAL(1, m1.nrOverloads()); + EXPECT_LONGS_EQUAL(0, m1.argumentList(0).size()); + EXPECT(m1.isConst()); +#ifndef WRAP_DISABLE_SERIALIZE // check serialization flag EXPECT(cls.isSerializable); EXPECT(cls.hasSerialization); - } +#endif + } // Test class is the third one { Class testCls = module.classes.at(2); - EXPECT_LONGS_EQUAL( 2, testCls.constructor.args_list.size()); - EXPECT_LONGS_EQUAL(19, testCls.methods.size()); + EXPECT_LONGS_EQUAL( 2, testCls.constructor.nrOverloads()); + EXPECT_LONGS_EQUAL(19, testCls.nrMethods()); EXPECT_LONGS_EQUAL( 0, testCls.static_methods.size()); EXPECT_LONGS_EQUAL( 0, testCls.namespaces.size()); // function to parse: pair return_pair (Vector v, Matrix A) const; - CHECK(testCls.methods.find("return_pair") != testCls.methods.end()); - Method m2 = testCls.methods.find("return_pair")->second; - LONGS_EQUAL(1, m2.returnVals.size()); - EXPECT(m2.returnVals.front().isPair); - EXPECT_LONGS_EQUAL(ReturnValue::EIGEN, m2.returnVals.front().category1); - EXPECT(assert_equal("Vector", m2.returnVals.front().type1)); - EXPECT_LONGS_EQUAL(ReturnValue::EIGEN, m2.returnVals.front().category2); - EXPECT(assert_equal("Matrix", m2.returnVals.front().type2)); + CHECK(testCls.exists("return_pair")); + Method m2 = testCls.method("return_pair"); + LONGS_EQUAL(1, m2.nrOverloads()); + EXPECT(m2.returnValue(0).isPair); + EXPECT_LONGS_EQUAL(ReturnType::EIGEN, m2.returnValue(0).type1.category); + EXPECT(assert_equal("Vector", m2.returnValue(0).type1.name)); + EXPECT_LONGS_EQUAL(ReturnType::EIGEN, m2.returnValue(0).type2.category); + EXPECT(assert_equal("Matrix", m2.returnValue(0).type2.name)); // checking pointer args and return values // pair return_ptrs (Test* p1, Test* p2) const; - CHECK(testCls.methods.find("return_ptrs") != testCls.methods.end()); - Method m3 = testCls.methods.find("return_ptrs")->second; - LONGS_EQUAL(1, m3.argLists.size()); - ArgumentList args = m3.argLists.front(); + CHECK(testCls.exists("return_ptrs")); + Method m3 = testCls.method("return_ptrs"); + LONGS_EQUAL(1, m3.nrOverloads()); + ArgumentList args = m3.argumentList(0); LONGS_EQUAL(2, args.size()); Argument arg1 = args.at(0); EXPECT(arg1.is_ptr); EXPECT(!arg1.is_const); EXPECT(!arg1.is_ref); - EXPECT(assert_equal("Test", arg1.type)); + EXPECT(assert_equal("Test", arg1.type.name)); EXPECT(assert_equal("p1", arg1.name)); - EXPECT(arg1.namespaces.empty()); + EXPECT(arg1.type.namespaces.empty()); Argument arg2 = args.at(1); EXPECT(arg2.is_ptr); EXPECT(!arg2.is_const); EXPECT(!arg2.is_ref); - EXPECT(assert_equal("Test", arg2.type)); + EXPECT(assert_equal("Test", arg2.type.name)); EXPECT(assert_equal("p2", arg2.name)); - EXPECT(arg2.namespaces.empty()); + EXPECT(arg2.type.namespaces.empty()); } // evaluate global functions @@ -312,12 +312,12 @@ TEST( wrap, parse_geometry ) { CHECK(module.global_functions.find("aGlobalFunction") != module.global_functions.end()); { GlobalFunction gfunc = module.global_functions.at("aGlobalFunction"); - EXPECT(assert_equal("aGlobalFunction", gfunc.name)); - LONGS_EQUAL(1, gfunc.returnVals.size()); - EXPECT(assert_equal("Vector", gfunc.returnVals.front().type1)); - EXPECT_LONGS_EQUAL(1, gfunc.argLists.size()); - LONGS_EQUAL(1, gfunc.namespaces.size()); - EXPECT(gfunc.namespaces.front().empty()); + EXPECT(assert_equal("aGlobalFunction", gfunc.name())); + LONGS_EQUAL(1, gfunc.nrOverloads()); + EXPECT(assert_equal("Vector", gfunc.returnValue(0).type1.name)); + EXPECT_LONGS_EQUAL(1, gfunc.nrOverloads()); + LONGS_EQUAL(1, gfunc.overloads.size()); + EXPECT(gfunc.overloads.front().namespaces.empty()); } } @@ -386,18 +386,17 @@ TEST( wrap, parse_namespaces ) { CHECK(module.global_functions.find("aGlobalFunction") != module.global_functions.end()); { GlobalFunction gfunc = module.global_functions.at("aGlobalFunction"); - EXPECT(assert_equal("aGlobalFunction", gfunc.name)); - LONGS_EQUAL(2, gfunc.returnVals.size()); - EXPECT(assert_equal("Vector", gfunc.returnVals.front().type1)); - EXPECT_LONGS_EQUAL(2, gfunc.argLists.size()); + EXPECT(assert_equal("aGlobalFunction", gfunc.name())); + LONGS_EQUAL(2, gfunc.nrOverloads()); + EXPECT(assert_equal("Vector", gfunc.returnValue(0).type1.name)); // check namespaces - LONGS_EQUAL(2, gfunc.namespaces.size()); + LONGS_EQUAL(2, gfunc.overloads.size()); strvec exp_namespaces1; exp_namespaces1 += "ns1"; - EXPECT(assert_equal(exp_namespaces1, gfunc.namespaces.at(0))); + EXPECT(assert_equal(exp_namespaces1, gfunc.overloads.at(0).namespaces)); strvec exp_namespaces2; exp_namespaces2 += "ns2"; - EXPECT(assert_equal(exp_namespaces2, gfunc.namespaces.at(1))); + EXPECT(assert_equal(exp_namespaces2, gfunc.overloads.at(1).namespaces)); } } @@ -443,13 +442,21 @@ TEST( wrap, matlab_code_geometry ) { // emit MATLAB code // make_geometry will not compile, use make testwrap to generate real make module.matlab_code("actual", headerPath); +#ifndef WRAP_DISABLE_SERIALIZE string epath = path + "/tests/expected/"; +#else + string epath = path + "/tests/expected2/"; +#endif string apath = "actual/"; EXPECT(files_equal(epath + "geometry_wrapper.cpp" , apath + "geometry_wrapper.cpp" )); - EXPECT(files_equal(epath + "Point2.m" , apath + "Point2.m" )); - EXPECT(files_equal(epath + "Point3.m" , apath + "Point3.m" )); + EXPECT(files_equal(epath + "+gtsam/Point2.m" , apath + "+gtsam/Point2.m" )); + EXPECT(files_equal(epath + "+gtsam/Point3.m" , apath + "+gtsam/Point3.m" )); EXPECT(files_equal(epath + "Test.m" , apath + "Test.m" )); + EXPECT(files_equal(epath + "MyBase.m" , apath + "MyBase.m" )); + EXPECT(files_equal(epath + "MyTemplatePoint2.m" , apath + "MyTemplatePoint2.m" )); + EXPECT(files_equal(epath + "MyTemplatePoint3.m" , apath + "MyTemplatePoint3.m" )); + EXPECT(files_equal(epath + "MyFactorPosePoint2.m" , apath + "MyFactorPosePoint2.m")); EXPECT(files_equal(epath + "aGlobalFunction.m" , apath + "aGlobalFunction.m" )); EXPECT(files_equal(epath + "overloadedGlobalFunction.m" , apath + "overloadedGlobalFunction.m" )); } diff --git a/wrap/utilities.cpp b/wrap/utilities.cpp index 47f7d10a6..51dc6f4c3 100644 --- a/wrap/utilities.cpp +++ b/wrap/utilities.cpp @@ -112,29 +112,23 @@ string maybe_shared_ptr(bool add, const string& qtype, const string& type) { string str = add? "Shared" : ""; if (add) str += type; else str += qtype; - - //if (add) str += ">"; return str; } /* ************************************************************************* */ -string qualifiedName(const string& separator, const vector& names, const string& finalName) { +string qualifiedName(const string& separator, const vector& names) { string result; - if(!names.empty()) { - for(size_t i = 0; i < names.size() - 1; ++i) + if (!names.empty()) { + for (size_t i = 0; i < names.size() - 1; ++i) result += (names[i] + separator); - if(finalName.empty()) - result += names.back(); - else - result += (names.back() + separator + finalName); - } else if(!finalName.empty()) { - result = finalName; + result += names.back(); } return result; } /* ************************************************************************* */ -void createNamespaceStructure(const std::vector& namespaces, const std::string& toolboxPath) { +void createNamespaceStructure(const std::vector& namespaces, + const std::string& toolboxPath) { using namespace boost::filesystem; path curPath = toolboxPath; BOOST_FOREACH(const string& subdir, namespaces) { diff --git a/wrap/utilities.h b/wrap/utilities.h index 7280dd297..a4f71b6ad 100644 --- a/wrap/utilities.h +++ b/wrap/utilities.h @@ -18,17 +18,20 @@ #pragma once +#include "FileWriter.h" + +#include +#include + +#include #include #include #include #include + //#include // on Linux GCC: fails with error regarding needing C++0x std flags //#include // same failure as above #include // works on Linux GCC -#include -#include - -#include "FileWriter.h" namespace wrap { @@ -123,12 +126,12 @@ bool assert_equal(const std::vector& expected, const std::vector& names, const std::string& finalName = ""); +std::string qualifiedName(const std::string& separator, const std::vector& names); /** creates the necessary folders for namespaces, as specified by a namespace stack */ -void createNamespaceStructure(const std::vector& namespaces, const std::string& toolboxPath); +void createNamespaceStructure(const std::vector& namespaces, + const std::string& toolboxPath); } // \namespace wrap