From 9d41178070a883f1fdc2f9b55449d845e3e3a274 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Mon, 19 Oct 2020 07:52:12 +0200 Subject: [PATCH 001/248] Use METIS system library if so selected --- CMakeLists.txt | 5 +++- cmake/HandleMetis.cmake | 44 ++++++++++++++++++++++++++++ cmake/HandlePrintConfiguration.cmake | 1 + gtsam/3rdparty/CMakeLists.txt | 5 +--- gtsam/CMakeLists.txt | 15 ++-------- 5 files changed, 53 insertions(+), 17 deletions(-) create mode 100644 cmake/HandleMetis.cmake diff --git a/CMakeLists.txt b/CMakeLists.txt index 35c487fd3..fa9e7d507 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -38,11 +38,14 @@ if(${CMAKE_SOURCE_DIR} STREQUAL ${CMAKE_BINARY_DIR}) message(FATAL_ERROR "In-source builds not allowed. Please make a new directory (called a build directory) and run CMake from there. You may need to remove CMakeCache.txt. ") endif() +include(cmake/HandleGeneralOptions.cmake) # CMake build options + +# Libraries: include(cmake/HandleBoost.cmake) # Boost include(cmake/HandleCCache.cmake) # ccache include(cmake/HandleCPack.cmake) # CPack include(cmake/HandleEigen.cmake) # Eigen3 -include(cmake/HandleGeneralOptions.cmake) # CMake build options +include(cmake/HandleMetis.cmake) # metis include(cmake/HandleMKL.cmake) # MKL include(cmake/HandleOpenMP.cmake) # OpenMP include(cmake/HandlePerfTools.cmake) # Google perftools diff --git a/cmake/HandleMetis.cmake b/cmake/HandleMetis.cmake new file mode 100644 index 000000000..9c29e5776 --- /dev/null +++ b/cmake/HandleMetis.cmake @@ -0,0 +1,44 @@ +############################################################################### +# Metis library + +# For both system or bundle version, a cmake target "metis-gtsam-if" is defined (interface library) + +# Dont try to use metis if GTSAM_SUPPORT_NESTED_DISSECTION is disabled: +if (NOT GTSAM_SUPPORT_NESTED_DISSECTION) + return() +endif() + +option(GTSAM_USE_SYSTEM_METIS "Find and use system-installed libmetis. If 'off', use the one bundled with GTSAM" OFF) + +if(GTSAM_USE_SYSTEM_METIS) + # Debian package: libmetis-dev + + find_path(METIS_INCLUDE_DIR metis.h REQUIRED) + find_library(METIS_LIBRARY metis REQUIRED) + + if(METIS_INCLUDE_DIR AND METIS_LIBRARY) + mark_as_advanced(METIS_INCLUDE_DIR) + mark_as_advanced(METIS_LIBRARY) + + add_library(metis-gtsam-if INTERFACE) + target_include_directories(metis-gtsam-if BEFORE INTERFACE ${METIS_INCLUDE_DIR}) + target_link_libraries(metis-gtsam-if INTERFACE ${METIS_LIBRARY}) + endif() +else() + # Bundled version: + option(GTSAM_BUILD_METIS_EXECUTABLES "Build metis library executables" OFF) + add_subdirectory(${GTSAM_SOURCE_DIR}/gtsam/3rdparty/metis) + + target_include_directories(metis-gtsam BEFORE PUBLIC + $ + $ + $ + $ + ) + + add_library(metis-gtsam-if INTERFACE) + target_link_libraries(metis-gtsam-if INTERFACE metis-gtsam) +endif() + +list(APPEND GTSAM_EXPORTED_TARGETS metis-gtsam-if) +install(TARGETS metis-gtsam-if EXPORT GTSAM-exports ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}) diff --git a/cmake/HandlePrintConfiguration.cmake b/cmake/HandlePrintConfiguration.cmake index 4ffd00e54..ad6ac5c5c 100644 --- a/cmake/HandlePrintConfiguration.cmake +++ b/cmake/HandlePrintConfiguration.cmake @@ -32,6 +32,7 @@ endif() print_build_options_for_target(gtsam) print_config("Use System Eigen" "${GTSAM_USE_SYSTEM_EIGEN} (Using version: ${GTSAM_EIGEN_VERSION})") +print_config("Use System Metis" "${GTSAM_USE_SYSTEM_METIS}") if(GTSAM_USE_TBB) print_config("Use Intel TBB" "Yes (Version: ${TBB_VERSION})") diff --git a/gtsam/3rdparty/CMakeLists.txt b/gtsam/3rdparty/CMakeLists.txt index 8b356393b..a1e917bbe 100644 --- a/gtsam/3rdparty/CMakeLists.txt +++ b/gtsam/3rdparty/CMakeLists.txt @@ -49,10 +49,7 @@ if(NOT GTSAM_USE_SYSTEM_EIGEN) endif() -option(GTSAM_BUILD_METIS_EXECUTABLES "Build metis library executables" OFF) -if(GTSAM_SUPPORT_NESTED_DISSECTION) - add_subdirectory(metis) -endif() +# metis: already handled in ROOT/cmake/HandleMetis.cmake add_subdirectory(ceres) diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index 37c4a1f88..91253b932 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -88,7 +88,8 @@ list(APPEND gtsam_srcs "${PROJECT_BINARY_DIR}/config.h" "${PROJECT_BINARY_DIR}/d install(FILES "${PROJECT_BINARY_DIR}/config.h" "${PROJECT_BINARY_DIR}/dllexport.h" DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/gtsam) if(GTSAM_SUPPORT_NESTED_DISSECTION) - list(APPEND GTSAM_ADDITIONAL_LIBRARIES metis-gtsam) + # target metis-gtsam-if is defined in both cases: embedded metis or system version: + list(APPEND GTSAM_ADDITIONAL_LIBRARIES metis-gtsam-if) endif() # Versions @@ -142,7 +143,7 @@ target_include_directories(gtsam BEFORE PUBLIC $ ) # 3rdparty libraries: use the "system" flag so they are included via "-isystem" -# and warnings (and warnings-considered-errors) in those headers are not +# and warnings (and warnings-considered-errors) in those headers are not # reported as warnings/errors in our targets: target_include_directories(gtsam SYSTEM BEFORE PUBLIC # SuiteSparse_config @@ -154,16 +155,6 @@ target_include_directories(gtsam SYSTEM BEFORE PUBLIC $ $ ) -if(GTSAM_SUPPORT_NESTED_DISSECTION) - target_include_directories(gtsam BEFORE PUBLIC - $ - $ - $ - $ - ) -endif() - - if(WIN32) # Add 'lib' prefix to static library to avoid filename collision with shared library if (NOT BUILD_SHARED_LIBS) From d4eaa15280275127a7fc053ed971aa5ed0383513 Mon Sep 17 00:00:00 2001 From: yotams Date: Thu, 22 Apr 2021 08:52:28 +0300 Subject: [PATCH 002/248] rolling shutter projection factor --- gtsam/base/Lie.h | 27 ++- gtsam/geometry/Pose3.cpp | 4 + gtsam/geometry/Pose3.h | 9 + .../slam/RollingShutterProjectionFactor.h | 214 ++++++++++++++++++ 4 files changed, 250 insertions(+), 4 deletions(-) create mode 100644 gtsam_unstable/slam/RollingShutterProjectionFactor.h diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index fe730c934..e4a66fad1 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -319,11 +319,30 @@ T expm(const Vector& x, int K=7) { } /** - * Linear interpolation between X and Y by coefficient t in [0, 1]. + * Linear interpolation and some extrapolation between X and Y by coefficient t in [0, 1.5], optinal jacobians calculations. */ -template -T interpolate(const T& X, const T& Y, double t) { - assert(t >= 0 && t <= 1); +template +T interpolate(const T& X, const T& Y, double t, OptionalJacobian Hx = boost::none, OptionalJacobian Hy = boost::none) { + assert(t >= 0 && t <= 1.5); + if (Hx && Hy) { + typedef Eigen::Matrix Jacobian; + typename traits::TangentVector tres; + T tres1; + Jacobian d1; + Jacobian d2; + + tres1 = traits::Between(X, Y, Hx, Hy); + tres = traits::Logmap(tres1, d1); + *Hx = d1 * (*Hx); + *Hy = d1 * (*Hy); + tres1 = traits::Expmap(t * tres, d1); + *Hx = t * d1 * (*Hx); + *Hy = t * d1 * (*Hy); + tres1 = traits::Compose(X, tres1, d1, d2); + *Hx = d1 + d2 * (*Hx); + *Hy = d2 * (*Hy); + return tres1; + } return traits::Compose(X, traits::Expmap(t * traits::Logmap(traits::Between(X, Y)))); } diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index c183e32ed..b2cdd0c87 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -423,4 +423,8 @@ std::ostream &operator<<(std::ostream &os, const Pose3& pose) { return os; } +Pose3 pose3_interp(const Pose3& X, const Pose3& Y, double t, Matrix& Hx, Matrix& Hy) { + return X.interp(t, Y, Hx, Hy); +} + } // namespace gtsam diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 318baab3d..f84d02965 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -353,6 +353,15 @@ public: return std::make_pair(0, 2); } + /** + * @brief Spherical Linear interpolation between *this and other + * @param s a value between 0 and 1.5 + * @param other final point of iterpolation geodesic on manifold + * @param Hx jacobian of the interpolation on this + & @param Hy jacobian of the interpolation on other + */ + Pose3 interp(double t, const Pose3& other, OptionalJacobian<6, 6> Hx = boost::none, OptionalJacobian<6, 6> Hy = boost::none) const; + /// Output stream operator GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Pose3& p); diff --git a/gtsam_unstable/slam/RollingShutterProjectionFactor.h b/gtsam_unstable/slam/RollingShutterProjectionFactor.h new file mode 100644 index 000000000..69b7e7376 --- /dev/null +++ b/gtsam_unstable/slam/RollingShutterProjectionFactor.h @@ -0,0 +1,214 @@ +/* ---------------------------------------------------------------------------- + + * 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 RollingShutterProjectionFactor.h + * @brief Basic bearing factor from 2D measurement for rolling shutter cameras + * @author Yotam Stern + */ + +#pragma once + +#include +#include +#include + +namespace gtsam { + + /** + * Non-linear factor for a constraint derived from a 2D measurement. The calibration is known here. + * i.e. the main building block for visual SLAM. + * this version takes rolling shutter information into account like so: consider camera A (pose A) and camera B, and Point2 from camera A. + * camera A has timestamp t_A for the exposure time of its first row, and so does camera B t_B, Point2 has timestamp t_p according to the timestamp + * corresponding to the time of exposure of the row in the camera it belongs to. + * let us define the interp_param = (t_p - t_A) / (t_B - t_A), we will use the pose interpolated between A and B by the interp_param to project + * the corresponding landmark to Point2. + * @addtogroup SLAM + */ + + class RollingShutterProjectionFactor: public NoiseModelFactor3 { + protected: + + // Keep a copy of measurement and calibration for I/O + Point2 measured_; ///< 2D measurement + double interp_param_; ///< interpolation parameter corresponding to the point2 measured + boost::shared_ptr K_; ///< shared pointer to calibration object + boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame + + // verbosity handling for Cheirality Exceptions + bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false) + bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions (default: false) + + public: + + /// shorthand for base class type + typedef NoiseModelFactor3 Base; + + /// shorthand for this class + typedef RollingShutterProjectionFactor This; + + /// shorthand for a smart pointer to a factor + typedef boost::shared_ptr shared_ptr; + + /// Default constructor + RollingShutterProjectionFactor() : + measured_(0, 0), interp_param_(0), throwCheirality_(false), verboseCheirality_(false) { + } + + /** + * Constructor + * @param measured is the 2 dimensional location of point in image (the measurement) + * @param interp_param is the rolling shutter parameter for the measurement + * @param model is the standard deviation + * @param poseKey_a is the index of the first camera + * @param poseKey_b is the index of the second camera + * @param pointKey is the index of the landmark + * @param K shared pointer to the constant calibration + * @param body_P_sensor is the transform from body to sensor frame (default identity) + */ + RollingShutterProjectionFactor(const Point2& measured, double interp_param, const SharedNoiseModel& model, + Key poseKey_a, Key poseKey_b, Key pointKey, const boost::shared_ptr& K, + boost::optional body_P_sensor = boost::none) : + Base(model, poseKey_a, poseKey_b, pointKey), measured_(measured), interp_param_(interp_param), K_(K), body_P_sensor_(body_P_sensor), + throwCheirality_(false), verboseCheirality_(false) {} + + /** + * Constructor with exception-handling flags + * @param measured is the 2 dimensional location of point in image (the measurement) + * @param interp_param is the rolling shutter parameter for the measurement + * @param model is the standard deviation + * @param poseKey_a is the index of the first camera + * @param poseKey_b is the index of the second camera + * @param pointKey is the index of the landmark + * @param K shared pointer to the constant calibration + * @param throwCheirality determines whether Cheirality exceptions are rethrown + * @param verboseCheirality determines whether exceptions are printed for Cheirality + * @param body_P_sensor is the transform from body to sensor frame (default identity) + */ + RollingShutterProjectionFactor(const Point2& measured, double interp_param, const SharedNoiseModel& model, + Key poseKey_a, Key poseKey_b, Key pointKey, const boost::shared_ptr& K, + bool throwCheirality, bool verboseCheirality, + boost::optional body_P_sensor = boost::none) : + Base(model, poseKey_a, poseKey_b, pointKey), measured_(measured), interp_param_(interp_param), K_(K), body_P_sensor_(body_P_sensor), + throwCheirality_(throwCheirality), verboseCheirality_(verboseCheirality) {} + + /** Virtual destructor */ + virtual ~RollingShutterProjectionFactor() {} + + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); } + + /** + * print + * @param s optional string naming the factor + * @param keyFormatter optional formatter useful for printing Symbols + */ + void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + std::cout << s << "RollingShutterProjectionFactor, z = "; + traits::Print(measured_); + std::cout << " rolling shutter interpolation param = " << interp_param_; + if(this->body_P_sensor_) + this->body_P_sensor_->print(" sensor pose in body frame: "); + Base::print("", keyFormatter); + } + + /// equals + virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const { + const This *e = dynamic_cast(&p); + return e + && Base::equals(p, tol) + && (interp_param_ == e->interp_param()) + && traits::Equals(this->measured_, e->measured_, tol) + && this->K_->equals(*e->K_, tol) + && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_))); + } + + /// Evaluate error h(x)-z and optionally derivatives + Vector evaluateError(const Pose3& pose_a, const Pose3& pose_b, const Point3& point, + boost::optional H1 = boost::none, boost::optional H2 = boost::none, boost::optional H3 = boost::none) const { + + Pose3 pose; + gtsam::Matrix Hprj; + + //pose = interpolate(pose_a, pose_b, interp_param_, H1, H2); + pose = pose_a.interp(interp_param_, pose_b, H1, H2); + try { + if(body_P_sensor_) { + if(H1 && H2) { + gtsam::Matrix H0; + PinholeCamera camera(pose.compose(*body_P_sensor_, H0), *K_); + Point2 reprojectionError(camera.project(point, Hprj, H3, boost::none) - measured_); + *H1 = Hprj * H0 * (*H1); + *H2 = Hprj * H0 * (*H2); + return reprojectionError; + } else { + PinholeCamera camera(pose.compose(*body_P_sensor_), *K_); + return camera.project(point, Hprj, H3, boost::none) - measured_; + } + } else { + PinholeCamera camera(pose, *K_); + Point2 reprojectionError(camera.project(point, Hprj, H3, boost::none) - measured_); + if (H1) *H1 = Hprj * (*H1); + if (H2) *H2 = Hprj * (*H2); + return reprojectionError; + } + } catch( CheiralityException& e) { + if (H1) *H1 = Matrix::Zero(2,6); + if (H2) *H2 = Matrix::Zero(2,6); + if (H3) *H3 = Matrix::Zero(2,3); + if (verboseCheirality_) + std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) << + " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl; + if (throwCheirality_) + throw CheiralityException(this->key2()); + } + return Vector2::Constant(2.0 * K_->fx()); + } + + /** return the measurement */ + const Point2& measured() const { + return measured_; + } + + /** return the calibration object */ + inline const boost::shared_ptr calibration() const { + return K_; + } + + /** returns the rolling shutter interp param*/ + inline double interp_param() const {return interp_param_; } + + /** return verbosity */ + inline bool verboseCheirality() const { return verboseCheirality_; } + + /** return flag for throwing cheirality exceptions */ + inline bool throwCheirality() const { return throwCheirality_; } + + private: + + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar & BOOST_SERIALIZATION_NVP(measured_); + ar & BOOST_SERIALIZATION_NVP(interp_param_); + ar & BOOST_SERIALIZATION_NVP(K_); + ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); + ar & BOOST_SERIALIZATION_NVP(throwCheirality_); + ar & BOOST_SERIALIZATION_NVP(verboseCheirality_); + } + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; // rolling shutter projection factor +} //namespace gtsam \ No newline at end of file From a637737d5e48bd58338a16b2e245bb0b6c5854f6 Mon Sep 17 00:00:00 2001 From: yetongumich Date: Wed, 12 May 2021 15:36:12 -0400 Subject: [PATCH 003/248] refactor tests and add comments --- gtsam/nonlinear/tests/testExpression.cpp | 81 ++++++++++++++---------- tests/testExpressionFactor.cpp | 59 +++++++++-------- 2 files changed, 81 insertions(+), 59 deletions(-) diff --git a/gtsam/nonlinear/tests/testExpression.cpp b/gtsam/nonlinear/tests/testExpression.cpp index 8fcf84a11..012a5ae9c 100644 --- a/gtsam/nonlinear/tests/testExpression.cpp +++ b/gtsam/nonlinear/tests/testExpression.cpp @@ -58,22 +58,23 @@ TEST(Expression, Constant) { /* ************************************************************************* */ // Leaf TEST(Expression, Leaf) { - Rot3_ R(100); + const Key key = 100; + Rot3_ R(key); Values values; - values.insert(100, someR); + values.insert(key, someR); Rot3 actual2 = R.value(values); EXPECT(assert_equal(someR, actual2)); } /* ************************************************************************* */ -// Many Leaves +// Test the function `createUnknowns` to create many leaves at once. TEST(Expression, Leaves) { Values values; - Point3 somePoint(1, 2, 3); + const 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))); + std::vector pointExpressions = createUnknowns(10, 'p', 1); + EXPECT(assert_equal(somePoint, pointExpressions.back().value(values))); } /* ************************************************************************* */ @@ -88,29 +89,34 @@ double f2(const Point3& p, OptionalJacobian<1, 3> H) { Vector f3(const Point3& p, OptionalJacobian H) { return p; } -Point3_ p(1); +Point3_ pointExpression(1); set expected = list_of(1); } // namespace unary +// Create a unary expression that takes another expression as a single argument. TEST(Expression, Unary1) { using namespace unary; - Expression e(f1, p); - EXPECT(expected == e.keys()); -} -TEST(Expression, Unary2) { - using namespace unary; - Double_ e(f2, p); - EXPECT(expected == e.keys()); + Expression unaryExpression(f1, pointExpression); + EXPECT(expected == unaryExpression.keys()); +} + +// Check that also works with a scalar return value. +TEST(Expression, Unary2) { + using namespace unary; + Double_ unaryExpression(f2, pointExpression); + EXPECT(expected == unaryExpression.keys()); } -/* ************************************************************************* */ // Unary(Leaf), dynamic TEST(Expression, Unary3) { using namespace unary; - // Expression e(f3, p); + // TODO(yetongumich): dynamic output arguments do not work yet! + // Expression unaryExpression(f3, pointExpression); + // EXPECT(expected == unaryExpression.keys()); } /* ************************************************************************* */ +// Simple test class that implements the `VectorSpace` protocol. class Class : public Point3 { public: enum {dimension = 3}; @@ -133,16 +139,20 @@ template<> struct traits : public internal::VectorSpace {}; // Nullary Method TEST(Expression, NullaryMethod) { // Create expression - Expression p(67); - Expression norm_(p, &Class::norm); + const Key key(67); + Expression classExpression(key); + + // Make expression from a class method, note how it differs from the function + // expressions by leading with the class expression in the constructor. + Expression norm_(classExpression, &Class::norm); // Create Values Values values; - values.insert(67, Class(3, 4, 5)); + values.insert(key, Class(3, 4, 5)); // Check dims as map std::map map; - norm_.dims(map); + norm_.dims(map); // TODO(yetongumich): Change to google style pointer convention. LONGS_EQUAL(1, map.size()); // Get value and Jacobians @@ -150,9 +160,10 @@ TEST(Expression, NullaryMethod) { double actual = norm_.value(values, H); // Check all - EXPECT(actual == sqrt(50)); + const double norm = sqrt(3*3 + 4*4 + 5*5); + EXPECT(actual == norm); Matrix expected(1, 3); - expected << 3.0 / sqrt(50.0), 4.0 / sqrt(50.0), 5.0 / sqrt(50.0); + expected << 3.0 / norm, 4.0 / norm, 5.0 / norm; EXPECT(assert_equal(expected, H[0])); } @@ -170,21 +181,21 @@ Point3_ p_cam(x, &Pose3::transformTo, p); } /* ************************************************************************* */ -// Check that creating an expression to double compiles +// Check that creating an expression to double compiles. TEST(Expression, BinaryToDouble) { using namespace binary; Double_ p_cam(doubleF, x, p); } /* ************************************************************************* */ -// keys +// Check keys of an expression created from class method. TEST(Expression, BinaryKeys) { set expected = list_of(1)(2); EXPECT(expected == binary::p_cam.keys()); } /* ************************************************************************* */ -// dimensions +// Check dimensions by calling `dims` method. TEST(Expression, BinaryDimensions) { map actual, expected = map_list_of(1, 6)(2, 3); binary::p_cam.dims(actual); @@ -192,7 +203,7 @@ TEST(Expression, BinaryDimensions) { } /* ************************************************************************* */ -// dimensions +// Check dimensions by calling `traceSize` method. TEST(Expression, BinaryTraceSize) { typedef internal::BinaryExpression Binary; size_t expectedTraceSize = sizeof(Binary::Record); @@ -247,6 +258,7 @@ TEST(Expression, TreeTraceSize) { } /* ************************************************************************* */ +// Test compose operation with * operator. TEST(Expression, compose1) { // Create expression Rot3_ R1(1), R2(2); @@ -258,7 +270,7 @@ TEST(Expression, compose1) { } /* ************************************************************************* */ -// Test compose with arguments referring to the same rotation +// Test compose with arguments referring to the same rotation. TEST(Expression, compose2) { // Create expression Rot3_ R1(1), R2(1); @@ -270,7 +282,7 @@ TEST(Expression, compose2) { } /* ************************************************************************* */ -// Test compose with one arguments referring to constant rotation +// Test compose with one arguments referring to constant rotation. TEST(Expression, compose3) { // Create expression Rot3_ R1(Rot3::identity()), R2(3); @@ -282,7 +294,7 @@ TEST(Expression, compose3) { } /* ************************************************************************* */ -// Test with ternary function +// Test with ternary function. Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3, OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2, OptionalJacobian<3, 3> H3) { // return dummy derivatives (not correct, but that's ok for testing here) @@ -306,6 +318,7 @@ TEST(Expression, ternary) { } /* ************************************************************************* */ +// Test scalar multiplication with * operator. TEST(Expression, ScalarMultiply) { const Key key(67); const Point3_ expr = 23 * Point3_(key); @@ -336,6 +349,7 @@ TEST(Expression, ScalarMultiply) { } /* ************************************************************************* */ +// Test sum with + operator. TEST(Expression, BinarySum) { const Key key(67); const Point3_ sum_ = Point3_(key) + Point3_(Point3(1, 1, 1)); @@ -366,6 +380,7 @@ TEST(Expression, BinarySum) { } /* ************************************************************************* */ +// Test sum of 3 variables with + operator. TEST(Expression, TripleSum) { const Key key(67); const Point3_ p1_(Point3(1, 1, 1)), p2_(key); @@ -387,6 +402,7 @@ TEST(Expression, TripleSum) { } /* ************************************************************************* */ +// Test sum with += operator. TEST(Expression, PlusEqual) { const Key key(67); const Point3_ p1_(Point3(1, 1, 1)), p2_(key); @@ -454,6 +470,7 @@ TEST(Expression, UnaryOfSum) { /* ************************************************************************* */ TEST(Expression, WeightedSum) { const Key key1(42), key2(67); + const Point3 point1(1, 0, 0), point2(0, 1, 0); const Point3_ weighted_sum_ = 17 * Point3_(key1) + 23 * Point3_(key2); map actual_dims, expected_dims = map_list_of(key1, 3)(key2, 3); @@ -461,11 +478,11 @@ TEST(Expression, WeightedSum) { EXPECT(actual_dims == expected_dims); Values values; - values.insert(key1, Point3(1, 0, 0)); - values.insert(key2, Point3(0, 1, 0)); + values.insert(key1, point1); + values.insert(key2, point2); // Check value - const Point3 expected = 17 * Point3(1, 0, 0) + 23 * Point3(0, 1, 0); + const Point3 expected = 17 * point1 + 23 * point2; EXPECT(assert_equal(expected, weighted_sum_.value(values))); // Check value + Jacobians diff --git a/tests/testExpressionFactor.cpp b/tests/testExpressionFactor.cpp index e3e37e7c7..657e77761 100644 --- a/tests/testExpressionFactor.cpp +++ b/tests/testExpressionFactor.cpp @@ -59,40 +59,42 @@ Point2_ p(2); TEST(ExpressionFactor, Leaf) { using namespace leaf; - // Create old-style factor to create expected value and derivatives + // Create old-style factor to create expected value and derivatives. PriorFactor old(2, Point2(0, 0), model); - // Concise version + // Create the equivalent factor with expression. ExpressionFactor f(model, Point2(0, 0), p); + + // Check values and derivatives. EXPECT_DOUBLES_EQUAL(old.error(values), f.error(values), 1e-9); EXPECT_LONGS_EQUAL(2, f.dim()); boost::shared_ptr gf2 = f.linearize(values); - EXPECT( assert_equal(*old.linearize(values), *gf2, 1e-9)); + EXPECT(assert_equal(*old.linearize(values), *gf2, 1e-9)); } /* ************************************************************************* */ -// non-zero noise model +// Test leaf expression with noise model of differnt variance. 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 + // Create old-style factor to create expected value and derivatives. PriorFactor old(2, Point2(0, 0), model); - // Concise version + // Create the equivalent factor with expression. ExpressionFactor f(model, Point2(0, 0), p); - // Check values and derivatives + // Check values and derivatives. 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)); + EXPECT(assert_equal(*old.linearize(values), *gf2, 1e-9)); EXPECT_CORRECT_FACTOR_JACOBIANS(f, values, 1e-5, 1e-5); // another way } /* ************************************************************************* */ -// Constrained noise model +// Test leaf expression with constrained noise model. TEST(ExpressionFactor, Constrained) { using namespace leaf; @@ -106,7 +108,7 @@ TEST(ExpressionFactor, Constrained) { 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)); + EXPECT(assert_equal(*old.linearize(values), *gf2, 1e-9)); } /* ************************************************************************* */ @@ -130,7 +132,7 @@ TEST(ExpressionFactor, Unary) { boost::shared_ptr gf = f.linearize(values); boost::shared_ptr jf = // boost::dynamic_pointer_cast(gf); - EXPECT( assert_equal(expected, *jf, 1e-9)); + EXPECT(assert_equal(expected, *jf, 1e-9)); } /* ************************************************************************* */ @@ -143,11 +145,13 @@ Vector9 wide(const Point3& p, OptionalJacobian<9,3> H) { if (H) *H << I_3x3, I_3x3, I_3x3; return v; } + typedef Eigen::Matrix Matrix9; Vector9 id9(const Vector9& v, OptionalJacobian<9,9> H) { if (H) *H = Matrix9::Identity(); return v; } + TEST(ExpressionFactor, Wide) { // Create some values Values values; @@ -208,6 +212,7 @@ TEST(ExpressionFactor, Binary) { EXPECT(assert_equal(expected25, (Matrix ) (*r)->dTdA1, 1e-9)); EXPECT(assert_equal(expected22, (Matrix ) (*r)->dTdA2, 1e-9)); } + /* ************************************************************************* */ // Unary(Binary(Leaf,Leaf)) TEST(ExpressionFactor, Shallow) { @@ -264,7 +269,7 @@ TEST(ExpressionFactor, Shallow) { 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)); + EXPECT(assert_equal(*expected, *gf2, 1e-9)); } /* ************************************************************************* */ @@ -297,7 +302,7 @@ TEST(ExpressionFactor, tree) { 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)); + EXPECT(assert_equal(*expected, *gf, 1e-9)); // Concise version ExpressionFactor f2(model, measured, @@ -305,14 +310,14 @@ TEST(ExpressionFactor, tree) { 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)); + EXPECT(assert_equal(*expected, *gf2, 1e-9)); // 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)); + EXPECT(assert_equal(*expected, *gf3, 1e-9)); } /* ************************************************************************* */ @@ -333,15 +338,15 @@ TEST(ExpressionFactor, Compose1) { // Check unwhitenedError std::vector H(2); Vector actual = f.unwhitenedError(values, H); - EXPECT( assert_equal(I_3x3, H[0],1e-9)); - EXPECT( assert_equal(I_3x3, H[1],1e-9)); + EXPECT(assert_equal(I_3x3, H[0],1e-9)); + EXPECT(assert_equal(I_3x3, H[1],1e-9)); // Check linearization JacobianFactor expected(1, I_3x3, 2, I_3x3, Z_3x1); boost::shared_ptr gf = f.linearize(values); boost::shared_ptr jf = // boost::dynamic_pointer_cast(gf); - EXPECT( assert_equal(expected, *jf,1e-9)); + EXPECT(assert_equal(expected, *jf,1e-9)); } /* ************************************************************************* */ @@ -363,14 +368,14 @@ TEST(ExpressionFactor, compose2) { std::vector H(1); Vector actual = f.unwhitenedError(values, H); EXPECT_LONGS_EQUAL(1, H.size()); - EXPECT( assert_equal(2*I_3x3, H[0],1e-9)); + EXPECT(assert_equal(2*I_3x3, H[0],1e-9)); // Check linearization JacobianFactor expected(1, 2 * I_3x3, Z_3x1); boost::shared_ptr gf = f.linearize(values); boost::shared_ptr jf = // boost::dynamic_pointer_cast(gf); - EXPECT( assert_equal(expected, *jf,1e-9)); + EXPECT(assert_equal(expected, *jf,1e-9)); } /* ************************************************************************* */ @@ -392,14 +397,14 @@ TEST(ExpressionFactor, compose3) { std::vector H(1); Vector actual = f.unwhitenedError(values, H); EXPECT_LONGS_EQUAL(1, H.size()); - EXPECT( assert_equal(I_3x3, H[0],1e-9)); + EXPECT(assert_equal(I_3x3, H[0],1e-9)); // Check linearization JacobianFactor expected(3, I_3x3, Z_3x1); boost::shared_ptr gf = f.linearize(values); boost::shared_ptr jf = // boost::dynamic_pointer_cast(gf); - EXPECT( assert_equal(expected, *jf,1e-9)); + EXPECT(assert_equal(expected, *jf,1e-9)); } /* ************************************************************************* */ @@ -435,16 +440,16 @@ TEST(ExpressionFactor, composeTernary) { std::vector H(3); Vector actual = f.unwhitenedError(values, H); EXPECT_LONGS_EQUAL(3, H.size()); - EXPECT( assert_equal(I_3x3, H[0],1e-9)); - EXPECT( assert_equal(I_3x3, H[1],1e-9)); - EXPECT( assert_equal(I_3x3, H[2],1e-9)); + EXPECT(assert_equal(I_3x3, H[0],1e-9)); + EXPECT(assert_equal(I_3x3, H[1],1e-9)); + EXPECT(assert_equal(I_3x3, H[2],1e-9)); // Check linearization JacobianFactor expected(1, I_3x3, 2, I_3x3, 3, I_3x3, Z_3x1); boost::shared_ptr gf = f.linearize(values); boost::shared_ptr jf = // boost::dynamic_pointer_cast(gf); - EXPECT( assert_equal(expected, *jf,1e-9)); + EXPECT(assert_equal(expected, *jf,1e-9)); } TEST(ExpressionFactor, tree_finite_differences) { @@ -636,7 +641,7 @@ TEST(ExpressionFactor, MultiplyWithInverseFunction) { class TestNaryFactor : public gtsam::ExpressionFactorN { + gtsam::Rot3, gtsam::Point3> { private: using This = TestNaryFactor; using Base = From 0fe12ec984dc487be74478701e60737eb3338e23 Mon Sep 17 00:00:00 2001 From: yetongumich Date: Wed, 12 May 2021 16:57:27 -0400 Subject: [PATCH 004/248] resolve some nits --- gtsam/nonlinear/tests/testExpression.cpp | 4 ++-- tests/testExpressionFactor.cpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/nonlinear/tests/testExpression.cpp b/gtsam/nonlinear/tests/testExpression.cpp index 012a5ae9c..aa922aa9a 100644 --- a/gtsam/nonlinear/tests/testExpression.cpp +++ b/gtsam/nonlinear/tests/testExpression.cpp @@ -203,7 +203,7 @@ TEST(Expression, BinaryDimensions) { } /* ************************************************************************* */ -// Check dimensions by calling `traceSize` method. +// Check dimensions of execution trace. TEST(Expression, BinaryTraceSize) { typedef internal::BinaryExpression Binary; size_t expectedTraceSize = sizeof(Binary::Record); @@ -470,7 +470,6 @@ TEST(Expression, UnaryOfSum) { /* ************************************************************************* */ TEST(Expression, WeightedSum) { const Key key1(42), key2(67); - const Point3 point1(1, 0, 0), point2(0, 1, 0); const Point3_ weighted_sum_ = 17 * Point3_(key1) + 23 * Point3_(key2); map actual_dims, expected_dims = map_list_of(key1, 3)(key2, 3); @@ -478,6 +477,7 @@ TEST(Expression, WeightedSum) { EXPECT(actual_dims == expected_dims); Values values; + const Point3 point1(1, 0, 0), point2(0, 1, 0); values.insert(key1, point1); values.insert(key2, point2); diff --git a/tests/testExpressionFactor.cpp b/tests/testExpressionFactor.cpp index 657e77761..87211dbd4 100644 --- a/tests/testExpressionFactor.cpp +++ b/tests/testExpressionFactor.cpp @@ -73,7 +73,7 @@ TEST(ExpressionFactor, Leaf) { } /* ************************************************************************* */ -// Test leaf expression with noise model of differnt variance. +// Test leaf expression with noise model of different variance. TEST(ExpressionFactor, Model) { using namespace leaf; From 9967c59ed0c9a1112bbffa7d84908154439e22cf Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Thu, 8 Jul 2021 14:30:16 +0200 Subject: [PATCH 005/248] Forward declaration for Set of Fisheye Cameras Forward declaration of camera vector for PinholeCamera and PinholeCamera. --- gtsam/geometry/triangulation.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 1df9efd22..b18c55818 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -20,6 +20,8 @@ #include #include +#include +#include #include #include #include @@ -499,6 +501,8 @@ TriangulationResult triangulateSafe(const CameraSet& cameras, // Vector of Cameras - used by the Python/MATLAB wrapper using CameraSetCal3Bundler = CameraSet>; using CameraSetCal3_S2 = CameraSet>; +using CameraSetCal3Fisheye = CameraSet>; +using CameraSetCal3Unified = CameraSet>; } // \namespace gtsam From dfd50f98c26791e68ab670c1768605fba9f20209 Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Thu, 8 Jul 2021 15:13:29 +0200 Subject: [PATCH 006/248] Extend python wrapper to include fisheye models. Extend python wrapper to include fisheye camera models Cal3Fisheye and Cal3Unified. --- gtsam/gtsam.i | 102 ++++++++++++++++++++++++++++++++++++++++++++------ 1 file changed, 90 insertions(+), 12 deletions(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 94d10953b..327574bf8 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -231,7 +231,7 @@ virtual class Value { }; #include -template +template virtual class GenericValue : gtsam::Value { void serializable() const; }; @@ -977,6 +977,52 @@ class Cal3Bundler { void pickle() const; }; +#include +class Cal3Fisheye { + // Standard Constructors + Cal3Fisheye(); + Cal3Fisheye(double fx, double fy, double s, double u0, double v0, double k1, double k2, double k3, double k4); + Cal3Fisheye(double fx, double fy, double s, double u0, double v0, double k1, double k2, double k3, double k4, double tol); + Cal3Fisheye(Vector v); + + // Testable + void print(string s = "Cal3Fisheye") const; + bool equals(const gtsam::Cal3Fisheye& rhs, double tol) const; + + // Manifold + static size_t Dim(); + size_t dim() const; + gtsam::Cal3Fisheye retract(Vector v) const; + Vector localCoordinates(const gtsam::Cal3Fisheye& c) const; + + // Action on Point2 + 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 skew() const; + double k1() const; + double k2() const; + double k3() const; + double k4() const; + double px() const; + double py() const; + gtsam::Point2 principalPoint() const; + Vector vector() const; + Vector k() const; + Matrix K() const; + Matrix inverse() const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; +}; + + #include class CalibratedCamera { // Standard Constructors and Named Constructors @@ -1085,6 +1131,7 @@ typedef gtsam::PinholeCamera PinholeCameraCal3_S2; typedef gtsam::PinholeCamera PinholeCameraCal3DS2; typedef gtsam::PinholeCamera PinholeCameraCal3Unified; typedef gtsam::PinholeCamera PinholeCameraCal3Bundler; +typedef gtsam::PinholeCamera PinholeCameraCal3Fisheye; template class CameraSet { @@ -1145,7 +1192,13 @@ gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3_S2& cameras, gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Bundler& cameras, const gtsam::Point2Vector& measurements, double rank_tol, bool optimize); - +gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Fisheye& cameras, + const gtsam::Point2Vector& measurements, double rank_tol, + bool optimize); +gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Unified& cameras, + const gtsam::Point2Vector& measurements, double rank_tol, + bool optimize); + //************************************************************************* // Symbolic //************************************************************************* @@ -2118,8 +2171,11 @@ class NonlinearFactorGraph { template , + gtsam::PinholeCameraCal3Bundler, + gtsam::PinholeCameraCal3Fisheye, + gtsam::PinholeCameraCal3Unified, gtsam::imuBias::ConstantBias}> void addPrior(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); @@ -2252,9 +2308,13 @@ class Values { void insert(size_t j, const gtsam::Cal3_S2& cal3_s2); void insert(size_t j, const gtsam::Cal3DS2& cal3ds2); void insert(size_t j, const gtsam::Cal3Bundler& cal3bundler); + void insert(size_t j, const gtsam::Cal3Fisheye& cal3fisheye); + void insert(size_t j, const gtsam::Cal3Unified& cal3unified); void insert(size_t j, const gtsam::EssentialMatrix& essential_matrix); void insert(size_t j, const gtsam::PinholeCameraCal3_S2& simple_camera); - void insert(size_t j, const gtsam::PinholeCamera& camera); + void insert(size_t j, const gtsam::PinholeCameraCal3Bundler& camera); + void insert(size_t j, const gtsam::PinholeCameraCal3Fisheye& camera); + void insert(size_t j, const gtsam::PinholeCameraCal3Unified& camera); void insert(size_t j, const gtsam::imuBias::ConstantBias& constant_bias); void insert(size_t j, const gtsam::NavState& nav_state); void insert(size_t j, double c); @@ -2272,9 +2332,13 @@ class Values { void update(size_t j, const gtsam::Cal3_S2& cal3_s2); void update(size_t j, const gtsam::Cal3DS2& cal3ds2); void update(size_t j, const gtsam::Cal3Bundler& cal3bundler); + void update(size_t j, const gtsam::Cal3Fisheye& cal3fisheye); + void update(size_t j, const gtsam::Cal3Unified& cal3unified); void update(size_t j, const gtsam::EssentialMatrix& essential_matrix); void update(size_t j, const gtsam::PinholeCameraCal3_S2& simple_camera); - void update(size_t j, const gtsam::PinholeCamera& camera); + void update(size_t j, const gtsam::PinholeCameraCal3Bundler& camera); + void update(size_t j, const gtsam::PinholeCameraCal3Fisheye& camera); + void update(size_t j, const gtsam::PinholeCameraCal3Unified& camera); void update(size_t j, const gtsam::imuBias::ConstantBias& constant_bias); void update(size_t j, const gtsam::NavState& nav_state); void update(size_t j, Vector vector); @@ -2294,10 +2358,14 @@ class Values { gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, - gtsam::EssentialMatrix, + gtsam::Cal3Fisheye, + gtsam::Cal3Unified, + gtsam::EssentialMatrix, gtsam::PinholeCameraCal3_S2, - gtsam::PinholeCamera, - gtsam::imuBias::ConstantBias, + gtsam::PinholeCameraCal3Bundler, + gtsam::PinholeCameraCal3Fisheye, + gtsam::PinholeCameraCal3Unified, + gtsam::imuBias::ConstantBias, gtsam::NavState, Vector, Matrix, @@ -2603,7 +2671,9 @@ class ISAM2 { template , + gtsam::PinholeCameraCal3_S2, gtsam::PinholeCameraCal3Bundler, + gtsam::Cal3Fisheye, gtsam::Cal3Unified, + gtsam::PinholeCameraCal3Fisheye, gtsam::PinholeCameraCal3Unified, Vector, Matrix}> VALUE calculateEstimate(size_t key) const; gtsam::Values calculateBestEstimate() const; @@ -2655,10 +2725,14 @@ template }> + gtsam::PinholeCameraCal3Bundler, + gtsam::PinholeCameraCal3Fisheye, + gtsam::PinholeCameraCal3Unified}> virtual class PriorFactor : gtsam::NoiseModelFactor { PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); T prior() const; @@ -2800,6 +2874,8 @@ virtual class GenericProjectionFactor : gtsam::NoiseModelFactor { }; typedef gtsam::GenericProjectionFactor GenericProjectionFactorCal3_S2; typedef gtsam::GenericProjectionFactor GenericProjectionFactorCal3DS2; +typedef gtsam::GenericProjectionFactor GenericProjectionFactorCal3Fisheye; +typedef gtsam::GenericProjectionFactor GenericProjectionFactorCal3Unified; #include @@ -2810,9 +2886,11 @@ virtual class GeneralSFMFactor : gtsam::NoiseModelFactor { }; typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3_S2; typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3DS2; -typedef gtsam::GeneralSFMFactor, gtsam::Point3> GeneralSFMFactorCal3Bundler; +typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3Bundler; +typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3Fisheye; +typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3Unified; -template +template virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor { GeneralSFMFactor2(const gtsam::Point2& measured, const gtsam::noiseModel::Base* model, size_t poseKey, size_t landmarkKey, size_t calibKey); gtsam::Point2 measured() const; From 55c12743fc29191c0e3d63c12b7c52f8284c402f Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Thu, 8 Jul 2021 15:45:11 +0200 Subject: [PATCH 007/248] Forward declaration of fisheye camera. Forward declaration of PinholeCal3Fisheye needed by Python wrapper. --- gtsam/geometry/SimpleCamera.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/gtsam/geometry/SimpleCamera.h b/gtsam/geometry/SimpleCamera.h index aa00222c7..2c34bdfa7 100644 --- a/gtsam/geometry/SimpleCamera.h +++ b/gtsam/geometry/SimpleCamera.h @@ -22,6 +22,7 @@ #include #include #include +#include #include #include @@ -33,7 +34,8 @@ namespace gtsam { using PinholeCameraCal3Bundler = gtsam::PinholeCamera; using PinholeCameraCal3DS2 = gtsam::PinholeCamera; using PinholeCameraCal3Unified = gtsam::PinholeCamera; - + using PinholeCameraCal3Fisheye = gtsam::PinholeCamera; + #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 /** * @deprecated: SimpleCamera for backwards compatability with GTSAM 3.x From c8fc3cd216020487be3850d58b56eca9ff74ec99 Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Thu, 8 Jul 2021 19:53:41 +0200 Subject: [PATCH 008/248] Unit test for equidistant fisheye --- python/gtsam/tests/test_Cal3Fisheye.py | 105 +++++++++++++++++++++++++ 1 file changed, 105 insertions(+) create mode 100644 python/gtsam/tests/test_Cal3Fisheye.py diff --git a/python/gtsam/tests/test_Cal3Fisheye.py b/python/gtsam/tests/test_Cal3Fisheye.py new file mode 100644 index 000000000..18c8ecd80 --- /dev/null +++ b/python/gtsam/tests/test_Cal3Fisheye.py @@ -0,0 +1,105 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Cal3Unified unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" +import unittest + +import numpy as np + +import gtsam +from gtsam.utils.test_case import GtsamTestCase + + +class TestCal3Fisheye(GtsamTestCase): + + def test_Cal3Fisheye(self): + K = gtsam.Cal3Fisheye() + self.assertEqual(K.fx(), 1.) + self.assertEqual(K.fy(), 1.) + + def test_distortion(self): + "Equidistant fisheye model of focal length f, defined as r/f = tan(theta)" + equidistant = gtsam.Cal3Fisheye() + x, y, z = 1, 0, 1 + u, v = equidistant.uncalibrate([x, y]) + x2, y2 = equidistant.calibrate([u, v]) + self.assertAlmostEqual(u, np.arctan2(x, z)) + self.assertAlmostEqual(v, 0) + self.assertAlmostEqual(x2, x) + self.assertAlmostEqual(y2, 0) + + def test_pinhole(self): + "Spatial equidistant camera projection" + x, y, z = 1.0, 0.0, 1.0 + u, v = np.arctan2(x, z), 0.0 + camera = gtsam.PinholeCameraCal3Fisheye() + + pt1 = camera.Project([x, y, z]) + self.gtsamAssertEquals(pt1, np.array([x/z, y/z])) + + pt2 = camera.project([x, y, z]) + self.gtsamAssertEquals(pt2, np.array([u, v])) + + obj1 = camera.backproject([u, v], z) + self.gtsamAssertEquals(obj1, np.array([x, y, z])) + + r1 = camera.range(np.array([x, y, z])) + self.assertEqual(r1, np.linalg.norm([x, y, z])) + + def test_generic_factor(self): + "Evaluate residual using pose and point as state variables" + objPoint = np.array([1, 0, 1]) + imgPoint = np.array([np.arctan2(objPoint[0], objPoint[2]), 0]) + graph = gtsam.NonlinearFactorGraph() + state = gtsam.Values() + measured = imgPoint + noiseModel = gtsam.noiseModel.Isotropic.Sigma(2, 1) + poseKey = gtsam.symbol_shorthand.P(0) + pointKey = gtsam.symbol_shorthand.L(0) + k = gtsam.Cal3Fisheye() + state.insert_pose3(poseKey, gtsam.Pose3()) + state.insert_point3(pointKey, gtsam.Point3(objPoint)) + factor = gtsam.GenericProjectionFactorCal3Fisheye(measured, noiseModel, poseKey, pointKey, k) + graph.add(factor) + score = graph.error(state) + self.assertAlmostEqual(score, 0) + + def test_sfm_factor2(self): + "Evaluate residual with camera, pose and point as state variables" + objPoint = np.array([1, 0, 1]) + imgPoint = np.array([np.arctan2(objPoint[0], objPoint[2]), 0]) + graph = gtsam.NonlinearFactorGraph() + state = gtsam.Values() + measured = imgPoint + noiseModel = gtsam.noiseModel.Isotropic.Sigma(2, 1) + cameraKey = gtsam.symbol_shorthand.K(0) + poseKey = gtsam.symbol_shorthand.P(0) + landmarkKey = gtsam.symbol_shorthand.L(0) + k = gtsam.Cal3Fisheye() + state.insert_cal3fisheye(cameraKey, k) + state.insert_pose3(poseKey, gtsam.Pose3()) + state.insert_point3(landmarkKey, gtsam.Point3(objPoint)) + factor = gtsam.GeneralSFMFactor2Cal3Fisheye(measured, noiseModel, poseKey, landmarkKey, cameraKey) + graph.add(factor) + score = graph.error(state) + self.assertAlmostEqual(score, 0) + + def test_retract(self): + expected = gtsam.Cal3Fisheye(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6, + 1e-3 + 7, 2.0*1e-3 + 8, 3.0*1e-3 + 9, 4.0*1e-3 + 10) + K = gtsam.Cal3Fisheye(100, 105, 0.0, 320, 240, + 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3) + d = np.array([2, 3, 4, 5, 6, 7, 8, 9, 10], order='F') + actual = K.retract(d) + self.gtsamAssertEquals(actual, expected) + np.testing.assert_allclose(d, K.localCoordinates(actual)) + + +if __name__ == "__main__": + unittest.main() From 19e8cde733c40e2e6dc0beb3c313c55b34e354fa Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Thu, 8 Jul 2021 19:59:56 +0200 Subject: [PATCH 009/248] Extend unit testing of omnidirectional projection Test projection function and factors using a stereoscopic (xi=1) reference model, i.e the image height is given by y = 2 f tan(theta/2). --- python/gtsam/tests/test_Cal3Unified.py | 83 ++++++++++++++++++++++++++ 1 file changed, 83 insertions(+) diff --git a/python/gtsam/tests/test_Cal3Unified.py b/python/gtsam/tests/test_Cal3Unified.py index fbf5f3565..ff9d65960 100644 --- a/python/gtsam/tests/test_Cal3Unified.py +++ b/python/gtsam/tests/test_Cal3Unified.py @@ -23,6 +23,89 @@ class TestCal3Unified(GtsamTestCase): self.assertEqual(K.fx(), 1.) self.assertEqual(K.fx(), 1.) + def test_distortion(self): + "Stereographic fisheye model of focal length f, defined as r/f = 2*tan(theta/2)" + fx, fy, s, u0, v0 = 2, 2, 0, 0, 0 + k1, k2, p1, p2 = 0, 0, 0, 0 + xi = 1 + stereographic = gtsam.Cal3Unified(fx, fy, s, u0, v0, k1, k2, p1, p2, xi) + x, y, z = 1, 0, 1 + u, v = stereographic.uncalibrate([x, y]) + r = np.linalg.norm([x, y, z]) + # Note: 2*tan(atan2(x, z)/2) = 2/(1+sqrt(x^2+z^2)) + self.assertAlmostEqual(2*np.tan(np.arctan2(x, z)/2), 2/(1+r)) + self.assertAlmostEqual(u, 2/(1+r)) + x2, y2 = stereographic.calibrate([u, v]) + self.assertAlmostEqual(x2, x) + + def test_pinhole(self): + "Spatial stereographic camera projection" + x, y, z = 1.0, 0.0, 1.0 + r = np.linalg.norm([x, y, z]) + u, v = 2/(1+r), 0.0 + objPoint = np.array([x, y, z]) + imgPoint = np.array([u, v]) + fx, fy, s, u0, v0 = 2, 2, 0, 0, 0 + k1, k2, p1, p2 = 0, 0, 0, 0 + xi = 1 + stereographic = gtsam.Cal3Unified(fx, fy, s, u0, v0, k1, k2, p1, p2, xi) + pose = gtsam.Pose3() + camera = gtsam.PinholeCameraCal3Unified(pose, stereographic) + pt1 = camera.Project(objPoint) + self.gtsamAssertEquals(pt1, np.array([x/z, y/z])) + pt2 = camera.project(objPoint) + self.gtsamAssertEquals(pt2, np.array([u, v])) + obj1 = camera.backproject([u, v], z) + self.gtsamAssertEquals(obj1, np.array([x, y, z])) + r1 = camera.range(np.array([x, y, z])) + self.assertEqual(r1, r) + + def test_generic_factor(self): + "Evaluate residual using pose and point as state variables" + fx, fy, s, u0, v0 = 2, 2, 0, 0, 0 + k1, k2, p1, p2 = 0, 0, 0, 0 + xi = 1 + objPoint = np.array([1, 0, 1]) + r = np.linalg.norm(objPoint) + imgPoint = np.array([2/(1+r), 0]) + graph = gtsam.NonlinearFactorGraph() + state = gtsam.Values() + measured = imgPoint + noiseModel = gtsam.noiseModel.Isotropic.Sigma(2, 1) + poseKey = gtsam.symbol_shorthand.P(0) + pointKey = gtsam.symbol_shorthand.L(0) + k = gtsam.Cal3Unified(fx, fy, s, u0, v0, k1, k2, p1, p2, xi) + state.insert_pose3(poseKey, gtsam.Pose3()) + state.insert_point3(pointKey, gtsam.Point3(objPoint)) + factor = gtsam.GenericProjectionFactorCal3Unified(measured, noiseModel, poseKey, pointKey, k) + graph.add(factor) + score = graph.error(state) + self.assertAlmostEqual(score, 0) + + def test_sfm_factor2(self): + "Evaluate residual with camera, pose and point as state variables" + fx, fy, s, u0, v0 = 2, 2, 0, 0, 0 + k1, k2, p1, p2 = 0, 0, 0, 0 + xi = 1 + objPoint = np.array([1, 0, 1]) + r = np.linalg.norm(objPoint) + imgPoint = np.array([2/(1+r), 0]) + graph = gtsam.NonlinearFactorGraph() + state = gtsam.Values() + measured = imgPoint + noiseModel = gtsam.noiseModel.Isotropic.Sigma(2, 1) + cameraKey = gtsam.symbol_shorthand.K(0) + poseKey = gtsam.symbol_shorthand.P(0) + landmarkKey = gtsam.symbol_shorthand.L(0) + k = gtsam.Cal3Unified(fx, fy, s, u0, v0, k1, k2, p1, p2, xi) + state.insert_cal3unified(cameraKey, k) + state.insert_pose3(poseKey, gtsam.Pose3()) + state.insert_point3(landmarkKey, gtsam.Point3(objPoint)) + factor = gtsam.GeneralSFMFactor2Cal3Unified(measured, noiseModel, poseKey, landmarkKey, cameraKey) + graph.add(factor) + score = graph.error(state) + self.assertAlmostEqual(score, 0) + def test_retract(self): expected = gtsam.Cal3Unified(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6, 1e-3 + 7, 2.0*1e-3 + 8, 3.0*1e-3 + 9, 4.0*1e-3 + 10, 0.1 + 1) From 2ecad47b9ef092379926f9784257c55dd245c24b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 8 Jul 2021 19:41:01 -0400 Subject: [PATCH 010/248] Added lots of tests for BetweenFactor --- gtsam/slam/tests/testBetweenFactor.cpp | 59 ++++++++++++++++++++++---- 1 file changed, 51 insertions(+), 8 deletions(-) diff --git a/gtsam/slam/tests/testBetweenFactor.cpp b/gtsam/slam/tests/testBetweenFactor.cpp index e99dba3bc..31842bf80 100644 --- a/gtsam/slam/tests/testBetweenFactor.cpp +++ b/gtsam/slam/tests/testBetweenFactor.cpp @@ -1,18 +1,19 @@ /** - * @file testBetweenFactor.cpp + * @file testBetweenFactor.cpp * @brief - * @author Duy-Nguyen Ta - * @date Aug 2, 2013 + * @author Duy-Nguyen Ta, Varun Agrawal + * @date Aug 2, 2013 */ +#include +#include #include +#include #include #include +#include #include -#include -#include - using namespace boost::placeholders; using namespace gtsam; using namespace gtsam::symbol_shorthand; @@ -48,7 +49,6 @@ TEST(BetweenFactor, Rot3) { } /* ************************************************************************* */ -/* // Constructor scalar TEST(BetweenFactor, ConstructorScalar) { SharedNoiseModel model; @@ -56,6 +56,7 @@ TEST(BetweenFactor, ConstructorScalar) { BetweenFactor factor(1, 2, measured_value, model); } +/* ************************************************************************* */ // Constructor vector3 TEST(BetweenFactor, ConstructorVector3) { SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 1.0); @@ -63,13 +64,55 @@ TEST(BetweenFactor, ConstructorVector3) { BetweenFactor factor(1, 2, measured_value, model); } +/* ************************************************************************* */ // Constructor dynamic sized vector TEST(BetweenFactor, ConstructorDynamicSizeVector) { SharedNoiseModel model = noiseModel::Isotropic::Sigma(5, 1.0); Vector measured_value(5); measured_value << 1, 2, 3, 4, 5; BetweenFactor factor(1, 2, measured_value, model); } -*/ + +/* ************************************************************************* */ +TEST(BetweenFactor, Point3Jacobians) { + SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 1.0); + Point3 measured_value(1, 2, 3); + BetweenFactor factor(1, 2, measured_value, model); + + Values values; + values.insert(1, Point3(0, 0, 0)); + values.insert(2, Point3(1, 2, 3)); + Vector3 error = factor.evaluateError(Point3(0, 0, 0), Point3(1, 2, 3)); + EXPECT(assert_equal(Vector3::Zero(), error, 1e-9)); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); +} + +/* ************************************************************************* */ +TEST(BetweenFactor, Rot3Jacobians) { + SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 1.0); + Rot3 measured_value = Rot3::Ry(M_PI_2); + BetweenFactor factor(1, 2, measured_value, model); + + Values values; + values.insert(1, Rot3()); + values.insert(2, Rot3::Ry(M_PI_2)); + Vector3 error = factor.evaluateError(Rot3(), Rot3::Ry(M_PI_2)); + EXPECT(assert_equal(Vector3::Zero(), error, 1e-9)); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); +} + +/* ************************************************************************* */ +TEST(BetweenFactor, Pose3Jacobians) { + SharedNoiseModel model = noiseModel::Isotropic::Sigma(6, 1.0); + Pose3 measured_value(Rot3(), Point3(1, 2, 3)); + BetweenFactor factor(1, 2, measured_value, model); + + Values values; + values.insert(1, Pose3()); + values.insert(2, Pose3(Rot3(), Point3(1, 2, 3))); + Vector3 error = factor.evaluateError(Pose3(), Pose3(Rot3(), Point3(1, 2, 3))); + EXPECT(assert_equal(Vector3::Zero(), error, 1e-9)); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); +} /* ************************************************************************* */ int main() { From a12b49de40aebb61966407f24411cf9b4d237478 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 8 Jul 2021 19:43:09 -0400 Subject: [PATCH 011/248] add Pose3 expmap to wrapper --- gtsam/gtsam.i | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 94d10953b..049a0110c 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -690,6 +690,7 @@ class Pose3 { // Lie Group static gtsam::Pose3 Expmap(Vector v); static Vector Logmap(const gtsam::Pose3& pose); + gtsam::Pose3 expmap(Vector v); Vector logmap(const gtsam::Pose3& pose); Matrix AdjointMap() const; Vector Adjoint(Vector xi) const; From d7d9ac0f0623f4d5d07bfa692df600a3729fc05b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 8 Jul 2021 19:43:25 -0400 Subject: [PATCH 012/248] typo fix --- gtsam/geometry/Pose3.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 318baab3d..d76e1b41a 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -142,7 +142,7 @@ public: static Vector6 Logmap(const Pose3& pose, OptionalJacobian<6, 6> Hpose = boost::none); /** - * Calculate Adjoint map, transforming a twist in the this pose's (i.e, body) frame to the world spatial frame + * Calculate Adjoint map, transforming a twist in this pose's (i.e, body) frame to the world spatial frame * Ad_pose is 6*6 matrix that when applied to twist xi \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$, returns Ad_pose(xi) */ Matrix6 AdjointMap() const; /// FIXME Not tested - marked as incorrect From bdeb60679b7ea8f9b1a7a53166da65590b007bcb Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Fri, 9 Jul 2021 11:14:10 +0200 Subject: [PATCH 013/248] Introduce setUpClass, python snake_case variables Test case fails if object depth z is not equal 1. --- python/gtsam/tests/test_Cal3Fisheye.py | 103 +++++++++++++------------ 1 file changed, 53 insertions(+), 50 deletions(-) diff --git a/python/gtsam/tests/test_Cal3Fisheye.py b/python/gtsam/tests/test_Cal3Fisheye.py index 18c8ecd80..23f7a9b8c 100644 --- a/python/gtsam/tests/test_Cal3Fisheye.py +++ b/python/gtsam/tests/test_Cal3Fisheye.py @@ -5,8 +5,9 @@ All Rights Reserved See LICENSE for the license information -Cal3Unified unit tests. +Cal3Fisheye unit tests. Author: Frank Dellaert & Duy Nguyen Ta (Python) +Refactored: Roderick Koehle """ import unittest @@ -14,78 +15,80 @@ import numpy as np import gtsam from gtsam.utils.test_case import GtsamTestCase - +from gtsam.symbol_shorthand import K, L, P class TestCal3Fisheye(GtsamTestCase): - + + @classmethod + def setUpClass(cls): + """ + Equidistant fisheye projection + + An equidistant fisheye projection with focal length f is defined + as the relation r/f = tan(theta), with r being the radius in the + image plane and theta the incident angle of the object point. + """ + x, y, z = 1.0, 0.0, 1.0 + # x, y, z = 0.5, 0.0, 2.0 <== Note: this example fails! + u, v = np.arctan2(x, z), 0.0 + cls.obj_point = np.array([x, y, z]) + cls.img_point = np.array([u, v]) + def test_Cal3Fisheye(self): K = gtsam.Cal3Fisheye() self.assertEqual(K.fx(), 1.) self.assertEqual(K.fy(), 1.) def test_distortion(self): - "Equidistant fisheye model of focal length f, defined as r/f = tan(theta)" + """Fisheye distortion and rectification""" equidistant = gtsam.Cal3Fisheye() - x, y, z = 1, 0, 1 - u, v = equidistant.uncalibrate([x, y]) - x2, y2 = equidistant.calibrate([u, v]) - self.assertAlmostEqual(u, np.arctan2(x, z)) - self.assertAlmostEqual(v, 0) - self.assertAlmostEqual(x2, x) - self.assertAlmostEqual(y2, 0) + perspective_pt = self.obj_point[0:2]/self.obj_point[2] + distorted_pt = equidistant.uncalibrate(perspective_pt) + rectified_pt = equidistant.calibrate(distorted_pt) + self.gtsamAssertEquals(distorted_pt, self.img_point) + self.gtsamAssertEquals(rectified_pt, perspective_pt) def test_pinhole(self): - "Spatial equidistant camera projection" - x, y, z = 1.0, 0.0, 1.0 - u, v = np.arctan2(x, z), 0.0 + """Spatial equidistant camera projection""" camera = gtsam.PinholeCameraCal3Fisheye() - - pt1 = camera.Project([x, y, z]) + pt1 = camera.Project(self.obj_point) # Perspective projection + pt2 = camera.project(self.obj_point) # Equidistant projection + x, y, z = self.obj_point + obj1 = camera.backproject(self.img_point, z) + r1 = camera.range(self.obj_point) + r = np.linalg.norm(self.obj_point) self.gtsamAssertEquals(pt1, np.array([x/z, y/z])) - - pt2 = camera.project([x, y, z]) - self.gtsamAssertEquals(pt2, np.array([u, v])) - - obj1 = camera.backproject([u, v], z) - self.gtsamAssertEquals(obj1, np.array([x, y, z])) - - r1 = camera.range(np.array([x, y, z])) - self.assertEqual(r1, np.linalg.norm([x, y, z])) + self.gtsamAssertEquals(pt2, self.img_point) + self.gtsamAssertEquals(obj1, self.obj_point) + self.assertEqual(r1, r) def test_generic_factor(self): - "Evaluate residual using pose and point as state variables" - objPoint = np.array([1, 0, 1]) - imgPoint = np.array([np.arctan2(objPoint[0], objPoint[2]), 0]) + """Evaluate residual using pose and point as state variables""" graph = gtsam.NonlinearFactorGraph() state = gtsam.Values() - measured = imgPoint - noiseModel = gtsam.noiseModel.Isotropic.Sigma(2, 1) - poseKey = gtsam.symbol_shorthand.P(0) - pointKey = gtsam.symbol_shorthand.L(0) + measured = self.img_point + noise_model = gtsam.noiseModel.Isotropic.Sigma(2, 1) + pose_key, point_key = P(0), L(0) k = gtsam.Cal3Fisheye() - state.insert_pose3(poseKey, gtsam.Pose3()) - state.insert_point3(pointKey, gtsam.Point3(objPoint)) - factor = gtsam.GenericProjectionFactorCal3Fisheye(measured, noiseModel, poseKey, pointKey, k) + state.insert_pose3(pose_key, gtsam.Pose3()) + state.insert_point3(point_key, self.obj_point) + factor = gtsam.GenericProjectionFactorCal3Fisheye(measured, noise_model, pose_key, point_key, k) graph.add(factor) score = graph.error(state) self.assertAlmostEqual(score, 0) def test_sfm_factor2(self): - "Evaluate residual with camera, pose and point as state variables" - objPoint = np.array([1, 0, 1]) - imgPoint = np.array([np.arctan2(objPoint[0], objPoint[2]), 0]) + """Evaluate residual with camera, pose and point as state variables""" graph = gtsam.NonlinearFactorGraph() state = gtsam.Values() - measured = imgPoint - noiseModel = gtsam.noiseModel.Isotropic.Sigma(2, 1) - cameraKey = gtsam.symbol_shorthand.K(0) - poseKey = gtsam.symbol_shorthand.P(0) - landmarkKey = gtsam.symbol_shorthand.L(0) + measured = self.img_point + noise_model = gtsam.noiseModel.Isotropic.Sigma(2, 1) + camera_key, pose_key, landmark_key = K(0), P(0), L(0) k = gtsam.Cal3Fisheye() - state.insert_cal3fisheye(cameraKey, k) - state.insert_pose3(poseKey, gtsam.Pose3()) - state.insert_point3(landmarkKey, gtsam.Point3(objPoint)) - factor = gtsam.GeneralSFMFactor2Cal3Fisheye(measured, noiseModel, poseKey, landmarkKey, cameraKey) + state.insert_cal3fisheye(camera_key, k) + state.insert_pose3(pose_key, gtsam.Pose3()) + state.insert_point3(landmark_key, gtsam.Point3(self.obj_point)) + factor = gtsam.GeneralSFMFactor2Cal3Fisheye(measured, noise_model, pose_key, landmark_key, camera_key) graph.add(factor) score = graph.error(state) self.assertAlmostEqual(score, 0) @@ -93,12 +96,12 @@ class TestCal3Fisheye(GtsamTestCase): def test_retract(self): expected = gtsam.Cal3Fisheye(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6, 1e-3 + 7, 2.0*1e-3 + 8, 3.0*1e-3 + 9, 4.0*1e-3 + 10) - K = gtsam.Cal3Fisheye(100, 105, 0.0, 320, 240, + k = gtsam.Cal3Fisheye(100, 105, 0.0, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3) d = np.array([2, 3, 4, 5, 6, 7, 8, 9, 10], order='F') - actual = K.retract(d) + actual = k.retract(d) self.gtsamAssertEquals(actual, expected) - np.testing.assert_allclose(d, K.localCoordinates(actual)) + np.testing.assert_allclose(d, k.localCoordinates(actual)) if __name__ == "__main__": From 6205057ccbd63c3705ac109b47104d14e10b41e9 Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Fri, 9 Jul 2021 11:17:38 +0200 Subject: [PATCH 014/248] Use of common setUpClass method --- python/gtsam/tests/test_Cal3Unified.py | 121 ++++++++++++------------- 1 file changed, 60 insertions(+), 61 deletions(-) diff --git a/python/gtsam/tests/test_Cal3Unified.py b/python/gtsam/tests/test_Cal3Unified.py index ff9d65960..f2c1ada48 100644 --- a/python/gtsam/tests/test_Cal3Unified.py +++ b/python/gtsam/tests/test_Cal3Unified.py @@ -14,94 +14,93 @@ import numpy as np import gtsam from gtsam.utils.test_case import GtsamTestCase +from gtsam.symbol_shorthand import K, L, P class TestCal3Unified(GtsamTestCase): + @classmethod + def setUpClass(cls): + """ + Stereographic fisheye projection + + An equidistant fisheye projection with focal length f is defined + as the relation r/f = 2*tan(theta/2), with r being the radius in the + image plane and theta the incident angle of the object point. + """ + x, y, z = 1.0, 0.0, 1.0 + r = np.linalg.norm([x, y, z]) + u, v = 2*x/(z+r), 0.0 + #u, v = 2*np.tan(np.arctan2(x, z)/2), 0.0 + cls.obj_point = np.array([x, y, z]) + cls.img_point = np.array([u, v]) + + fx, fy, s, u0, v0 = 2, 2, 0, 0, 0 + k1, k2, p1, p2 = 0, 0, 0, 0 + xi = 1 + cls.stereographic = gtsam.Cal3Unified(fx, fy, s, u0, v0, k1, k2, p1, p2, xi) + def test_Cal3Unified(self): K = gtsam.Cal3Unified() self.assertEqual(K.fx(), 1.) self.assertEqual(K.fx(), 1.) def test_distortion(self): - "Stereographic fisheye model of focal length f, defined as r/f = 2*tan(theta/2)" - fx, fy, s, u0, v0 = 2, 2, 0, 0, 0 - k1, k2, p1, p2 = 0, 0, 0, 0 - xi = 1 - stereographic = gtsam.Cal3Unified(fx, fy, s, u0, v0, k1, k2, p1, p2, xi) - x, y, z = 1, 0, 1 - u, v = stereographic.uncalibrate([x, y]) + """Stereographic fisheye model of focal length f, defined as r/f = 2*tan(theta/2)""" + x, y, z = self.obj_point r = np.linalg.norm([x, y, z]) - # Note: 2*tan(atan2(x, z)/2) = 2/(1+sqrt(x^2+z^2)) - self.assertAlmostEqual(2*np.tan(np.arctan2(x, z)/2), 2/(1+r)) - self.assertAlmostEqual(u, 2/(1+r)) - x2, y2 = stereographic.calibrate([u, v]) - self.assertAlmostEqual(x2, x) + # Note: 2*tan(atan2(x, z)/2) = 2*x/(z+sqrt(x^2+z^2)) + self.assertAlmostEqual(2*np.tan(np.arctan2(x, z)/2), 2*x/(z+r)) + perspective_pt = self.obj_point[0:2]/self.obj_point[2] + distorted_pt = self.stereographic.uncalibrate(perspective_pt) + rectified_pt = self.stereographic.calibrate(distorted_pt) + self.gtsamAssertEquals(distorted_pt, self.img_point) + self.gtsamAssertEquals(rectified_pt, perspective_pt) def test_pinhole(self): - "Spatial stereographic camera projection" - x, y, z = 1.0, 0.0, 1.0 - r = np.linalg.norm([x, y, z]) - u, v = 2/(1+r), 0.0 - objPoint = np.array([x, y, z]) - imgPoint = np.array([u, v]) - fx, fy, s, u0, v0 = 2, 2, 0, 0, 0 - k1, k2, p1, p2 = 0, 0, 0, 0 - xi = 1 - stereographic = gtsam.Cal3Unified(fx, fy, s, u0, v0, k1, k2, p1, p2, xi) + """Spatial stereographic camera projection""" + x, y, z = self.obj_point + u, v = self.img_point + r = np.linalg.norm(self.obj_point) pose = gtsam.Pose3() - camera = gtsam.PinholeCameraCal3Unified(pose, stereographic) - pt1 = camera.Project(objPoint) + camera = gtsam.PinholeCameraCal3Unified(pose, self.stereographic) + pt1 = camera.Project(self.obj_point) self.gtsamAssertEquals(pt1, np.array([x/z, y/z])) - pt2 = camera.project(objPoint) - self.gtsamAssertEquals(pt2, np.array([u, v])) - obj1 = camera.backproject([u, v], z) - self.gtsamAssertEquals(obj1, np.array([x, y, z])) - r1 = camera.range(np.array([x, y, z])) + pt2 = camera.project(self.obj_point) + self.gtsamAssertEquals(pt2, self.img_point) + obj1 = camera.backproject(self.img_point, z) + self.gtsamAssertEquals(obj1, self.obj_point) + r1 = camera.range(self.obj_point) self.assertEqual(r1, r) def test_generic_factor(self): - "Evaluate residual using pose and point as state variables" - fx, fy, s, u0, v0 = 2, 2, 0, 0, 0 - k1, k2, p1, p2 = 0, 0, 0, 0 - xi = 1 - objPoint = np.array([1, 0, 1]) - r = np.linalg.norm(objPoint) - imgPoint = np.array([2/(1+r), 0]) + """Evaluate residual using pose and point as state variables""" graph = gtsam.NonlinearFactorGraph() state = gtsam.Values() - measured = imgPoint - noiseModel = gtsam.noiseModel.Isotropic.Sigma(2, 1) - poseKey = gtsam.symbol_shorthand.P(0) - pointKey = gtsam.symbol_shorthand.L(0) - k = gtsam.Cal3Unified(fx, fy, s, u0, v0, k1, k2, p1, p2, xi) - state.insert_pose3(poseKey, gtsam.Pose3()) - state.insert_point3(pointKey, gtsam.Point3(objPoint)) - factor = gtsam.GenericProjectionFactorCal3Unified(measured, noiseModel, poseKey, pointKey, k) + measured = self.img_point + noise_model = gtsam.noiseModel.Isotropic.Sigma(2, 1) + pose_key, point_key = P(0), L(0) + k = self.stereographic + state.insert_pose3(pose_key, gtsam.Pose3()) + state.insert_point3(point_key, self.obj_point) + factor = gtsam.GenericProjectionFactorCal3Unified(measured, noise_model, pose_key, point_key, k) graph.add(factor) score = graph.error(state) self.assertAlmostEqual(score, 0) def test_sfm_factor2(self): - "Evaluate residual with camera, pose and point as state variables" - fx, fy, s, u0, v0 = 2, 2, 0, 0, 0 - k1, k2, p1, p2 = 0, 0, 0, 0 - xi = 1 - objPoint = np.array([1, 0, 1]) - r = np.linalg.norm(objPoint) - imgPoint = np.array([2/(1+r), 0]) + """Evaluate residual with camera, pose and point as state variables""" + r = np.linalg.norm(self.obj_point) graph = gtsam.NonlinearFactorGraph() state = gtsam.Values() - measured = imgPoint - noiseModel = gtsam.noiseModel.Isotropic.Sigma(2, 1) - cameraKey = gtsam.symbol_shorthand.K(0) - poseKey = gtsam.symbol_shorthand.P(0) - landmarkKey = gtsam.symbol_shorthand.L(0) - k = gtsam.Cal3Unified(fx, fy, s, u0, v0, k1, k2, p1, p2, xi) - state.insert_cal3unified(cameraKey, k) - state.insert_pose3(poseKey, gtsam.Pose3()) - state.insert_point3(landmarkKey, gtsam.Point3(objPoint)) - factor = gtsam.GeneralSFMFactor2Cal3Unified(measured, noiseModel, poseKey, landmarkKey, cameraKey) + measured = self.img_point + noise_model = gtsam.noiseModel.Isotropic.Sigma(2, 1) + camera_key, pose_key, landmark_key = K(0), P(0), L(0) + k = self.stereographic + state.insert_cal3unified(camera_key, k) + state.insert_pose3(pose_key, gtsam.Pose3()) + state.insert_point3(landmark_key, self.obj_point) + factor = gtsam.GeneralSFMFactor2Cal3Unified(measured, noise_model, pose_key, landmark_key, camera_key) graph.add(factor) score = graph.error(state) self.assertAlmostEqual(score, 0) From a411b664a198776f21aa6fd9a43732841e44add7 Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Fri, 9 Jul 2021 11:25:18 +0200 Subject: [PATCH 015/248] Correct tab to spaces to fix formatting --- gtsam/gtsam.i | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 327574bf8..ce251802c 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -2360,12 +2360,12 @@ class Values { gtsam::Cal3Bundler, gtsam::Cal3Fisheye, gtsam::Cal3Unified, - gtsam::EssentialMatrix, + gtsam::EssentialMatrix, gtsam::PinholeCameraCal3_S2, gtsam::PinholeCameraCal3Bundler, gtsam::PinholeCameraCal3Fisheye, gtsam::PinholeCameraCal3Unified, - gtsam::imuBias::ConstantBias, + gtsam::imuBias::ConstantBias, gtsam::NavState, Vector, Matrix, From 66af0079baa5de84618fdc1927dc55e8e9fcff81 Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Fri, 9 Jul 2021 12:39:31 +0200 Subject: [PATCH 016/248] Improved accuracy for analytic undistortion --- gtsam/geometry/Cal3Fisheye.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/gtsam/geometry/Cal3Fisheye.cpp b/gtsam/geometry/Cal3Fisheye.cpp index b9e60acee..3ee036eff 100644 --- a/gtsam/geometry/Cal3Fisheye.cpp +++ b/gtsam/geometry/Cal3Fisheye.cpp @@ -110,7 +110,9 @@ Point2 Cal3Fisheye::calibrate(const Point2& uv, OptionalJacobian<2, 9> Dcal, const double u = uv.x(), v = uv.y(); const double yd = (v - v0_) / fy_; const double xd = (u - s_ * yd - u0_) / fx_; - Point2 pi(xd, yd); + const double theta = sqrt(xd * xd + yd * yd); + const double scale = (theta > 0) ? tan(theta) / theta; + Point2 pi(scale * xd, scale * yd); // Perform newtons method, break when solution converges past tol_, // throw exception if max iterations are reached From 8b9e60156c5c895430624dfbdb27ae9fb1c5b472 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 9 Jul 2021 14:06:59 -0400 Subject: [PATCH 017/248] cleaner variables --- gtsam/slam/tests/testBetweenFactor.cpp | 31 +++++++++++++------------- 1 file changed, 16 insertions(+), 15 deletions(-) diff --git a/gtsam/slam/tests/testBetweenFactor.cpp b/gtsam/slam/tests/testBetweenFactor.cpp index 31842bf80..206d2b590 100644 --- a/gtsam/slam/tests/testBetweenFactor.cpp +++ b/gtsam/slam/tests/testBetweenFactor.cpp @@ -52,31 +52,31 @@ TEST(BetweenFactor, Rot3) { // Constructor scalar TEST(BetweenFactor, ConstructorScalar) { SharedNoiseModel model; - double measured_value = 0.0; - BetweenFactor factor(1, 2, measured_value, model); + double measured = 0.0; + BetweenFactor factor(1, 2, measured, model); } /* ************************************************************************* */ // Constructor vector3 TEST(BetweenFactor, ConstructorVector3) { SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 1.0); - Vector3 measured_value(1, 2, 3); - BetweenFactor factor(1, 2, measured_value, model); + Vector3 measured(1, 2, 3); + BetweenFactor factor(1, 2, measured, model); } /* ************************************************************************* */ // Constructor dynamic sized vector TEST(BetweenFactor, ConstructorDynamicSizeVector) { SharedNoiseModel model = noiseModel::Isotropic::Sigma(5, 1.0); - Vector measured_value(5); measured_value << 1, 2, 3, 4, 5; - BetweenFactor factor(1, 2, measured_value, model); + Vector measured(5); measured << 1, 2, 3, 4, 5; + BetweenFactor factor(1, 2, measured, model); } /* ************************************************************************* */ TEST(BetweenFactor, Point3Jacobians) { SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 1.0); - Point3 measured_value(1, 2, 3); - BetweenFactor factor(1, 2, measured_value, model); + Point3 measured(1, 2, 3); + BetweenFactor factor(1, 2, measured, model); Values values; values.insert(1, Point3(0, 0, 0)); @@ -89,8 +89,8 @@ TEST(BetweenFactor, Point3Jacobians) { /* ************************************************************************* */ TEST(BetweenFactor, Rot3Jacobians) { SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 1.0); - Rot3 measured_value = Rot3::Ry(M_PI_2); - BetweenFactor factor(1, 2, measured_value, model); + Rot3 measured = Rot3::Ry(M_PI_2); + BetweenFactor factor(1, 2, measured, model); Values values; values.insert(1, Rot3()); @@ -103,13 +103,14 @@ TEST(BetweenFactor, Rot3Jacobians) { /* ************************************************************************* */ TEST(BetweenFactor, Pose3Jacobians) { SharedNoiseModel model = noiseModel::Isotropic::Sigma(6, 1.0); - Pose3 measured_value(Rot3(), Point3(1, 2, 3)); - BetweenFactor factor(1, 2, measured_value, model); + Pose3 measured(Rot3(), Point3(1, 2, 3)); + BetweenFactor factor(1, 2, measured, model); + Pose3 pose1, pose2(Rot3(), Point3(1, 2, 3)); Values values; - values.insert(1, Pose3()); - values.insert(2, Pose3(Rot3(), Point3(1, 2, 3))); - Vector3 error = factor.evaluateError(Pose3(), Pose3(Rot3(), Point3(1, 2, 3))); + values.insert(1, pose1); + values.insert(2, pose2); + Vector3 error = factor.evaluateError(pose1, pose2); EXPECT(assert_equal(Vector3::Zero(), error, 1e-9)); EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); } From 2e4016932491bc7888cd9108e7667f7faf00e819 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 9 Jul 2021 14:07:19 -0400 Subject: [PATCH 018/248] fix dimension for Pose3 test --- gtsam/slam/tests/testBetweenFactor.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/slam/tests/testBetweenFactor.cpp b/gtsam/slam/tests/testBetweenFactor.cpp index 206d2b590..a1f8774e5 100644 --- a/gtsam/slam/tests/testBetweenFactor.cpp +++ b/gtsam/slam/tests/testBetweenFactor.cpp @@ -110,8 +110,8 @@ TEST(BetweenFactor, Pose3Jacobians) { Values values; values.insert(1, pose1); values.insert(2, pose2); - Vector3 error = factor.evaluateError(pose1, pose2); - EXPECT(assert_equal(Vector3::Zero(), error, 1e-9)); + Vector6 error = factor.evaluateError(pose1, pose2); + EXPECT(assert_equal(Vector6::Zero(), error, 1e-9)); EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); } From df579ec6a764f95f483a9f4acfa15b3ad008a58e Mon Sep 17 00:00:00 2001 From: Scott Date: Fri, 9 Jul 2021 13:07:08 -0700 Subject: [PATCH 019/248] Fix serialization of ISAM2 class --- gtsam/inference/VariableIndex.h | 10 ++++++++++ gtsam/nonlinear/ISAM2.h | 20 ++++++++++++++++++++ 2 files changed, 30 insertions(+) diff --git a/gtsam/inference/VariableIndex.h b/gtsam/inference/VariableIndex.h index f2ba3e31c..47438ecd6 100644 --- a/gtsam/inference/VariableIndex.h +++ b/gtsam/inference/VariableIndex.h @@ -182,6 +182,16 @@ protected: return item->second; } +private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + ar & BOOST_SERIALIZATION_NVP(index_); + ar & BOOST_SERIALIZATION_NVP(nFactors_); + ar & BOOST_SERIALIZATION_NVP(nEntries_); + } + /// @} }; diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 9f33e757f..92c2142a7 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -315,6 +315,26 @@ class GTSAM_EXPORT ISAM2 : public BayesTree { void removeVariables(const KeySet& unusedKeys); void updateDelta(bool forceFullSolve = false) const; + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + ar & boost::serialization::base_object >(*this); + ar & BOOST_SERIALIZATION_NVP(theta_); + ar & BOOST_SERIALIZATION_NVP(variableIndex_); + ar & BOOST_SERIALIZATION_NVP(delta_); + ar & BOOST_SERIALIZATION_NVP(deltaNewton_); + ar & BOOST_SERIALIZATION_NVP(RgProd_); + ar & BOOST_SERIALIZATION_NVP(deltaReplacedMask_); + ar & BOOST_SERIALIZATION_NVP(nonlinearFactors_); + ar & BOOST_SERIALIZATION_NVP(linearFactors_); + ar & BOOST_SERIALIZATION_NVP(doglegDelta_); + ar & BOOST_SERIALIZATION_NVP(fixedVariables_); + ar & BOOST_SERIALIZATION_NVP(update_count_); + } + }; // ISAM2 /// traits From 15478bf2783fbf4bac53e570bcd3bb61376a7d68 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Fri, 9 Jul 2021 17:33:33 -0400 Subject: [PATCH 020/248] Update ShonanAveraging.h --- gtsam/sfm/ShonanAveraging.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/gtsam/sfm/ShonanAveraging.h b/gtsam/sfm/ShonanAveraging.h index f00109cae..bff5ab471 100644 --- a/gtsam/sfm/ShonanAveraging.h +++ b/gtsam/sfm/ShonanAveraging.h @@ -430,6 +430,8 @@ class GTSAM_EXPORT ShonanAveraging2 : public ShonanAveraging<2> { const Parameters ¶meters = Parameters()); explicit ShonanAveraging2(std::string g2oFile, const Parameters ¶meters = Parameters()); + ShonanAveraging2(const gtsam::BetweenFactorPose2s &factors, + const gtsam::ShonanAveragingParameters2 ¶meters); }; class GTSAM_EXPORT ShonanAveraging3 : public ShonanAveraging<3> { From 3c1823349b6b90a7fce71bfeca229a56e26bed70 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Fri, 9 Jul 2021 17:38:10 -0400 Subject: [PATCH 021/248] add interface in C++, and helper extractRot2Measurements() --- gtsam/sfm/ShonanAveraging.cpp | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp index a982ef7da..f933f368d 100644 --- a/gtsam/sfm/ShonanAveraging.cpp +++ b/gtsam/sfm/ShonanAveraging.cpp @@ -944,6 +944,20 @@ ShonanAveraging2::ShonanAveraging2(string g2oFile, const Parameters ¶meters) parameters.getUseHuber()), parameters) {} +static ShonanAveraging2::Measurements extractRot2Measurements( + const BetweenFactorPose2s &factors) { + ShonanAveraging2::Measurements result; + result.reserve(factors.size()); + for (auto f : factors) result.push_back(convert(f)); + return result; +} + +ShonanAveraging2::ShonanAveraging2(const BetweenFactorPose2s &factors, + const Parameters ¶meters) + : ShonanAveraging<3>(maybeRobust(extractRot2Measurements(factors), + parameters.getUseHuber()), + parameters) {} + /* ************************************************************************* */ // Explicit instantiation for d=3 template class ShonanAveraging<3>; From 0e0d630c917ec6f80a8a037f61bd030842f7feb0 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Fri, 9 Jul 2021 17:39:38 -0400 Subject: [PATCH 022/248] fix typo --- gtsam/sfm/ShonanAveraging.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp index f933f368d..0de37be1e 100644 --- a/gtsam/sfm/ShonanAveraging.cpp +++ b/gtsam/sfm/ShonanAveraging.cpp @@ -954,7 +954,7 @@ static ShonanAveraging2::Measurements extractRot2Measurements( ShonanAveraging2::ShonanAveraging2(const BetweenFactorPose2s &factors, const Parameters ¶meters) - : ShonanAveraging<3>(maybeRobust(extractRot2Measurements(factors), + : ShonanAveraging<2>(maybeRobust(extractRot2Measurements(factors), parameters.getUseHuber()), parameters) {} From 3c8cdb4eeee70f0a95a8ccea54373262ea24b618 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Fri, 9 Jul 2021 17:42:59 -0400 Subject: [PATCH 023/248] add ShonanAveraging2 constructor to wrapper, that accepts BetweenFactorPose2s as input --- gtsam/gtsam.i | 2 ++ 1 file changed, 2 insertions(+) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 94d10953b..f17679739 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -3123,6 +3123,8 @@ class ShonanAveraging2 { ShonanAveraging2(string g2oFile); ShonanAveraging2(string g2oFile, const gtsam::ShonanAveragingParameters2 ¶meters); + ShonanAveraging2(const gtsam::BetweenFactorPose2s &factors, + const gtsam::ShonanAveragingParameters2 ¶meters); // Query properties size_t nrUnknowns() const; From 4bf2308ec595a96270952d81ce25e8d2d2d447fa Mon Sep 17 00:00:00 2001 From: John Lambert Date: Fri, 9 Jul 2021 17:48:26 -0400 Subject: [PATCH 024/248] add conversion function for Pose2 -> BinaryMeasurement --- gtsam/sfm/ShonanAveraging.cpp | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp index 0de37be1e..7810d0d47 100644 --- a/gtsam/sfm/ShonanAveraging.cpp +++ b/gtsam/sfm/ShonanAveraging.cpp @@ -944,11 +944,27 @@ ShonanAveraging2::ShonanAveraging2(string g2oFile, const Parameters ¶meters) parameters.getUseHuber()), parameters) {} +// Extract Rot2 measurement from Pose2 betweenfactors +// Modeled after similar function in dataset.cpp +static BinaryMeasurement convertPose2ToBinaryMeasurementRot2( + const BetweenFactor::shared_ptr &f) { + auto gaussian = + boost::dynamic_pointer_cast(f->noiseModel()); + if (!gaussian) + throw std::invalid_argument( + "parseMeasurements can only convert Pose2 measurements " + "with Gaussian noise models."); + const Matrix6 M = gaussian->covariance(); + auto model = noiseModel::Gaussian::Covariance(M.block<2, 2>(2, 2)); + return BinaryMeasurement(f->key1(), f->key2(), f->measured().rotation(), + model); +} + static ShonanAveraging2::Measurements extractRot2Measurements( const BetweenFactorPose2s &factors) { ShonanAveraging2::Measurements result; result.reserve(factors.size()); - for (auto f : factors) result.push_back(convert(f)); + for (auto f : factors) result.push_back(convertPose2ToBinaryMeasurementRot2(f)); return result; } From 641a01c72620c6736d917c470e85db26ae471980 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Fri, 9 Jul 2021 17:54:14 -0400 Subject: [PATCH 025/248] fix typo on 3x3 matrix def --- gtsam/sfm/ShonanAveraging.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp index 7810d0d47..88c393f16 100644 --- a/gtsam/sfm/ShonanAveraging.cpp +++ b/gtsam/sfm/ShonanAveraging.cpp @@ -954,7 +954,7 @@ static BinaryMeasurement convertPose2ToBinaryMeasurementRot2( throw std::invalid_argument( "parseMeasurements can only convert Pose2 measurements " "with Gaussian noise models."); - const Matrix6 M = gaussian->covariance(); + const Matrix3 M = gaussian->covariance(); auto model = noiseModel::Gaussian::Covariance(M.block<2, 2>(2, 2)); return BinaryMeasurement(f->key1(), f->key2(), f->measured().rotation(), model); From 8c68e215215f640a40cbf4b2d863fea64124f450 Mon Sep 17 00:00:00 2001 From: Scott Date: Fri, 9 Jul 2021 18:30:39 -0700 Subject: [PATCH 026/248] Added ISAM2 serialize test --- .../tests/testSerializationNonlinear.cpp | 79 +++++++++++++++++++ 1 file changed, 79 insertions(+) diff --git a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp index 90ebcbbba..037582654 100644 --- a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp +++ b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp @@ -17,6 +17,7 @@ */ #include +#include #include #include #include @@ -31,6 +32,30 @@ using namespace std; using namespace gtsam; using namespace gtsam::serializationTestHelpers; + +/* ************************************************************************* */ +// Create GUIDs for Noisemodels +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Base , "gtsam_noiseModel_mEstimator_Base"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Null , "gtsam_noiseModel_mEstimator_Null"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Fair , "gtsam_noiseModel_mEstimator_Fair"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Huber, "gtsam_noiseModel_mEstimator_Huber"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Tukey, "gtsam_noiseModel_mEstimator_Tukey"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic,"gtsam_noiseModel_Isotropic"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Robust, "gtsam_noiseModel_Robust"); +BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel"); +BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); + +/* ************************************************************************* */ +// Create GUIDs for factors +BOOST_CLASS_EXPORT_GUID(gtsam::PriorFactor, "gtsam::PriorFactor"); +BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor"); +BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor"); +BOOST_CLASS_EXPORT_GUID(gtsam::GaussianConditional , "gtsam::GaussianConditional"); + + /* ************************************************************************* */ // Export all classes derived from Value GTSAM_VALUE_EXPORT(gtsam::Cal3_S2); @@ -82,6 +107,60 @@ TEST (Serialization, TemplatedValues) { EXPECT(equalsBinary(values)); } +TEST(Serialization, ISAM2) { + + gtsam::ISAM2Params parameters; + gtsam::ISAM2 solver(parameters); + gtsam::NonlinearFactorGraph graph; + gtsam::Values initialValues; + initialValues.clear(); + + gtsam::Vector6 temp6; + for (int i = 0; i < 6; ++i) { + temp6[i] = 0.0001; + } + gtsam::noiseModel::Diagonal::shared_ptr noiseModel = gtsam::noiseModel::Diagonal::Sigmas(temp6); + + gtsam::Pose3 pose0(gtsam::Rot3(), gtsam::Point3(0, 0, 0)); + gtsam::Symbol symbol0('x', 0); + graph.add(gtsam::PriorFactor(symbol0, pose0, noiseModel)); + initialValues.insert(symbol0, pose0); + + solver.update(graph, initialValues, gtsam::FastVector()); + + std::string binaryPath = "saved_solver.dat"; + try { + std::ofstream outputStream(binaryPath); + boost::archive::binary_oarchive outputArchive(outputStream); + outputArchive << solver; + } catch(...) { + EXPECT(false); + } + + gtsam::ISAM2 solverFromDisk; + try { + std::ifstream ifs(binaryPath); + boost::archive::binary_iarchive inputArchive(ifs); + inputArchive >> solverFromDisk; + } catch(...) { + EXPECT(false); + } + + gtsam::Pose3 p1, p2; + try { + p1 = solver.calculateEstimate(symbol0); + } catch(std::exception &e) { + EXPECT(false); + } + + try { + p2 = solverFromDisk.calculateEstimate(symbol0); + } catch(std::exception &e) { + EXPECT(false); + } + EXPECT(assert_equal(p1, p2)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ From 7fc8f23367eeba02c7ac20464eaeee78ce9d5702 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Fri, 9 Jul 2021 23:34:55 -0400 Subject: [PATCH 027/248] use default parameters if none provided, and remove gtsam namespace prefix in .h file --- gtsam/sfm/ShonanAveraging.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/sfm/ShonanAveraging.h b/gtsam/sfm/ShonanAveraging.h index bff5ab471..de12de478 100644 --- a/gtsam/sfm/ShonanAveraging.h +++ b/gtsam/sfm/ShonanAveraging.h @@ -430,8 +430,8 @@ class GTSAM_EXPORT ShonanAveraging2 : public ShonanAveraging<2> { const Parameters ¶meters = Parameters()); explicit ShonanAveraging2(std::string g2oFile, const Parameters ¶meters = Parameters()); - ShonanAveraging2(const gtsam::BetweenFactorPose2s &factors, - const gtsam::ShonanAveragingParameters2 ¶meters); + ShonanAveraging2(const BetweenFactorPose2s &factors, + const Parameters ¶meters = Parameters()); }; class GTSAM_EXPORT ShonanAveraging3 : public ShonanAveraging<3> { From 64514387b42e04249b467c9d2b5bbb6807ded4b5 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Sat, 10 Jul 2021 00:57:01 -0400 Subject: [PATCH 028/248] check in python unit test for new functionality --- python/gtsam/tests/test_ShonanAveraging.py | 66 +++++++++++++++++++++- 1 file changed, 64 insertions(+), 2 deletions(-) diff --git a/python/gtsam/tests/test_ShonanAveraging.py b/python/gtsam/tests/test_ShonanAveraging.py index 4c423574d..fe0d7fc2b 100644 --- a/python/gtsam/tests/test_ShonanAveraging.py +++ b/python/gtsam/tests/test_ShonanAveraging.py @@ -13,14 +13,25 @@ Author: Frank Dellaert import unittest import gtsam -from gtsam import ShonanAveraging3, ShonanAveragingParameters3 +from gtsam import ( + BetweenFactorPose2, + LevenbergMarquardtParams, + Rot2, + Pose2, + ShonanAveraging2, + ShonanAveragingParameters2, + ShonanAveraging3, + ShonanAveragingParameters3 +) from gtsam.utils.test_case import GtsamTestCase DEFAULT_PARAMS = ShonanAveragingParameters3( gtsam.LevenbergMarquardtParams.CeresDefaults()) -def fromExampleName(name: str, parameters=DEFAULT_PARAMS): +def fromExampleName( + name: str, parameters: ShonanAveragingParameters3 = DEFAULT_PARAMS +) -> ShonanAveraging3: g2oFile = gtsam.findExampleDataFile(name) return ShonanAveraging3(g2oFile, parameters) @@ -133,6 +144,57 @@ class TestShonanAveraging(GtsamTestCase): self.assertAlmostEqual(3.0756, shonan.cost(initial), places=3) result, _lambdaMin = shonan.run(initial, 3, 3) self.assertAlmostEqual(0.0015, shonan.cost(result), places=3) + + def test_constructorBetweenFactorPose2s(self) -> None: + """Check if ShonanAveraging2 constructor works when not initialized from g2o file.""" + # map (i1,i2) pair to theta in degrees that parameterizes Rot2 object i2Ri1 + i2Ri1_dict = { + (1, 2): -43.86342, + (1, 5): -135.06351, + (2, 4): 72.64527, + (3, 5): 117.75967, + (6, 7): -31.73934, + (7, 10): 177.45901, + (6, 9): -133.58713, + (7, 8): -90.58668, + (0, 3): 127.02978, + (8, 10): -92.16361, + (4, 8): 51.63781, + (4, 6): 173.96384, + (4, 10): 139.59445, + (2, 3): 151.04022, + (3, 4): -78.39495, + (1, 4): 28.78185, + (6, 8): -122.32602, + (0, 2): -24.01045, + (5, 7): -53.93014, + (4, 5): -163.84535, + (2, 5): -91.20009, + (1, 3): 107.17680, + (7, 9): -102.35615, + (0, 1): 19.85297, + (5, 8): -144.51682, + (5, 6): -22.19079, + (5, 10): -56.56016, + } + i2Ri1_dict = {(i1,i2): Rot2.fromDegrees(theta_deg) for (i1,i2), theta_deg in i2Ri1_dict.items()} + lm_params = LevenbergMarquardtParams.CeresDefaults() + shonan_params = ShonanAveragingParameters2(lm_params) + shonan_params.setUseHuber(False) + shonan_params.setCertifyOptimality(True) + + noise_model = gtsam.noiseModel.Unit.Create(3) + between_factors = gtsam.BetweenFactorPose2s() + for (i1, i2), i2Ri1 in i2Ri1_dict.items(): + i2Ti1 = Pose2(i2Ri1, np.zeros(2)) + between_factors.append(BetweenFactorPose2(i2, i1, i2Ti1, noise_model)) + + obj = ShonanAveraging2(between_factors, shonan_params) + initial = obj.initializeRandomly() + result_values, _ = obj.run(initial, p_min, self._p_max) + + for i in range(11): + wRi = result_values.atRot2(i) if __name__ == '__main__': From 03049929a55d430bc67404bf8663d792ee9d62fb Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Sat, 10 Jul 2021 15:21:22 +0200 Subject: [PATCH 029/248] Add comment about initial guess in undistortion For the equidistant fisheye model, r/f = tan(theta), the Gauss-Newton search to model radial distortion is expected to converge faster by mapping the angular coordinate space into the respective tangent space of the perspective plane. This is consistent to the nPlaneToSpace initial projection used in the calibrate() function of the omnidirectional model (Cal3Unified). --- gtsam/geometry/Cal3Fisheye.cpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/Cal3Fisheye.cpp b/gtsam/geometry/Cal3Fisheye.cpp index 3ee036eff..52d475d5d 100644 --- a/gtsam/geometry/Cal3Fisheye.cpp +++ b/gtsam/geometry/Cal3Fisheye.cpp @@ -106,12 +106,20 @@ Point2 Cal3Fisheye::uncalibrate(const Point2& p, OptionalJacobian<2, 9> H1, /* ************************************************************************* */ Point2 Cal3Fisheye::calibrate(const Point2& uv, OptionalJacobian<2, 9> Dcal, OptionalJacobian<2, 2> Dp) const { - // initial gues just inverts the pinhole model + // Apply inverse camera matrix to map the pixel coordinate (u, v) + // of the equidistant fisheye image to angular coordinate space (xd, yd) + // with radius theta given in radians. const double u = uv.x(), v = uv.y(); const double yd = (v - v0_) / fy_; const double xd = (u - s_ * yd - u0_) / fx_; const double theta = sqrt(xd * xd + yd * yd); - const double scale = (theta > 0) ? tan(theta) / theta; + + // Provide initial guess for the Gauss-Newton search. + // The angular coordinates given by (xd, yd) are mapped back to + // the focal plane of the perspective undistorted projection pi. + // See Cal3Unified.calibrate() using the same pattern for the + // undistortion of omnidirectional fisheye projection. + const double scale = (theta > 0) ? tan(theta) / theta : 1.0; Point2 pi(scale * xd, scale * yd); // Perform newtons method, break when solution converges past tol_, From db801f164d2ff7c6c65d79058959c6dfb7832ef4 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Sat, 10 Jul 2021 10:31:27 -0400 Subject: [PATCH 030/248] add missing import to python unit test --- python/gtsam/tests/test_ShonanAveraging.py | 1 + 1 file changed, 1 insertion(+) diff --git a/python/gtsam/tests/test_ShonanAveraging.py b/python/gtsam/tests/test_ShonanAveraging.py index fe0d7fc2b..a873a6430 100644 --- a/python/gtsam/tests/test_ShonanAveraging.py +++ b/python/gtsam/tests/test_ShonanAveraging.py @@ -13,6 +13,7 @@ Author: Frank Dellaert import unittest import gtsam +import numpy as np from gtsam import ( BetweenFactorPose2, LevenbergMarquardtParams, From 8b86d7a51c803b6dbf52e67d5438e1b792c328d5 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Sat, 10 Jul 2021 11:36:36 -0400 Subject: [PATCH 031/248] improve docs about compiling without TBB --- INSTALL.md | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/INSTALL.md b/INSTALL.md index 520bddf3c..64857774a 100644 --- a/INSTALL.md +++ b/INSTALL.md @@ -20,9 +20,10 @@ $ make install Optional dependent libraries: - If TBB is installed and detectable by CMake GTSAM will use it automatically. Ensure that CMake prints "Use Intel TBB : Yes". To disable the use of TBB, - disable the CMake flag GTSAM_WITH_TBB (enabled by default). On Ubuntu, TBB - may be installed from the Ubuntu repositories, and for other platforms it - may be downloaded from https://www.threadingbuildingblocks.org/ + disable the CMake flag `GTSAM_WITH_TBB` (enabled by default) by providing + the argument `-DGTSAM_WITH_TBB=OFF` to `cmake`. On Ubuntu, TBB may be + installed from the Ubuntu repositories, and for other platforms it may be + downloaded from https://www.threadingbuildingblocks.org/ - GTSAM may be configured to use MKL by toggling `GTSAM_WITH_EIGEN_MKL` and `GTSAM_WITH_EIGEN_MKL_OPENMP` to `ON`; however, best performance is usually achieved with MKL disabled. We therefore advise you to benchmark your problem From 63236cf7afa2da29850ac45e310bc166f20e6f5a Mon Sep 17 00:00:00 2001 From: John Lambert Date: Sat, 10 Jul 2021 11:37:59 -0400 Subject: [PATCH 032/248] improve wrapper compilation instructions, when TBB not installed --- python/README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/python/README.md b/python/README.md index ec9808ce0..54436df93 100644 --- a/python/README.md +++ b/python/README.md @@ -24,6 +24,7 @@ For instructions on updating the version of the [wrap library](https://github.co ```bash cmake .. -DGTSAM_BUILD_PYTHON=1 -DGTSAM_PYTHON_VERSION=3.6.10 ``` + If you do not have TBB installed, you should also provide the argument `-DGTSAM_WITH_TBB=OFF`. - Build GTSAM and the wrapper with `make` (or `ninja` if you use `-GNinja`). - To install, simply run `make python-install` (`ninja python-install`). From aebb90573ab688287cf703f6ed4af97f5a2be962 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Sat, 10 Jul 2021 11:41:46 -0400 Subject: [PATCH 033/248] set pmin and pmax in unit test --- python/gtsam/tests/test_ShonanAveraging.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/python/gtsam/tests/test_ShonanAveraging.py b/python/gtsam/tests/test_ShonanAveraging.py index a873a6430..69e705545 100644 --- a/python/gtsam/tests/test_ShonanAveraging.py +++ b/python/gtsam/tests/test_ShonanAveraging.py @@ -148,6 +148,7 @@ class TestShonanAveraging(GtsamTestCase): def test_constructorBetweenFactorPose2s(self) -> None: """Check if ShonanAveraging2 constructor works when not initialized from g2o file.""" + num_images = 11 # map (i1,i2) pair to theta in degrees that parameterizes Rot2 object i2Ri1 i2Ri1_dict = { (1, 2): -43.86342, @@ -192,9 +193,9 @@ class TestShonanAveraging(GtsamTestCase): obj = ShonanAveraging2(between_factors, shonan_params) initial = obj.initializeRandomly() - result_values, _ = obj.run(initial, p_min, self._p_max) + result_values, _ = obj.run(initial, min_p=2, max_p=10) - for i in range(11): + for i in range(num_images): wRi = result_values.atRot2(i) From d54e234f93128c3c612e0ef0a2c65cb9a1182fae Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Sat, 10 Jul 2021 22:03:17 +0200 Subject: [PATCH 034/248] Add ambiguous calibrate/uncalibrate declarations. Without declaring calibrate / uncalibrated in the interface specification, the functions of the Base class Cal3DS2_Base is called. The layout of the optional Jacobian matrix is 2x10 in Cal3Unified and 2x9 in Cal3DS2_Base, so this are different function calls. --- gtsam/gtsam.i | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index ce251802c..97012c291 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -910,6 +910,12 @@ virtual class Cal3Unified : gtsam::Cal3DS2_Base { gtsam::Cal3Unified retract(Vector v) const; Vector localCoordinates(const gtsam::Cal3Unified& c) const; + // Action on Point2 + // Note: the signature of this functions differ from the functions + // with equal name in the base class. + gtsam::Point2 calibrate(const gtsam::Point2& p) const; + gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; + // enabling serialization functionality void serialize() const; From 3118fde6d3dce098ffbca641afb18488f97a9dbc Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Sat, 10 Jul 2021 23:00:24 +0200 Subject: [PATCH 035/248] Missing CameraSet binding specialisations Add pybind specialisations for CameraSetCal3Unified and CameraSetCal3Fisheye. --- python/gtsam/specializations.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/python/gtsam/specializations.h b/python/gtsam/specializations.h index be8eb8a6c..2e8612fec 100644 --- a/python/gtsam/specializations.h +++ b/python/gtsam/specializations.h @@ -32,4 +32,6 @@ py::bind_vector(m_, "IndexPairVector"); py::bind_map(m_, "KeyPairDoubleMap"); py::bind_vector > >(m_, "CameraSetCal3_S2"); py::bind_vector > >(m_, "CameraSetCal3Bundler"); +py::bind_vector > >(m_, "CameraSetCal3Unified"); +py::bind_vector > >(m_, "CameraSetCal3Fisheye"); py::bind_vector >(m_, "JacobianVector"); From 0a73961f5a2762100d30147db25fde150434d1a6 Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Sat, 10 Jul 2021 23:05:53 +0200 Subject: [PATCH 036/248] Update ignore list in CMakeFile --- python/CMakeLists.txt | 2 ++ 1 file changed, 2 insertions(+) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 5f51368e6..528732f4b 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -102,6 +102,8 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON) gtsam::BinaryMeasurementsUnit3 gtsam::CameraSetCal3_S2 gtsam::CameraSetCal3Bundler + gtsam::CameraSetCal3Unified + gtsam::CameraSetCal3Fisheye gtsam::KeyPairDoubleMap) pybind_wrap(gtsam_unstable_py # target From dc8b5e58ff2b8b48ba5b5fc329271daf3b8c2468 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 10 Jul 2021 21:01:20 -0400 Subject: [PATCH 037/248] replaced boost with std for placeholders, bind and function --- gtsam/base/Matrix.h | 4 +- gtsam/base/Testable.h | 7 +- gtsam/base/lieProxies.h | 2 +- gtsam/base/numericalDerivative.h | 345 +++++++++--------- gtsam/discrete/DecisionTree-inl.h | 4 +- gtsam/discrete/DecisionTree.h | 12 +- gtsam/geometry/Cyclic.h | 4 +- gtsam/inference/EliminateableFactorGraph.h | 4 +- gtsam/inference/LabeledSymbol.cpp | 45 ++- gtsam/inference/LabeledSymbol.h | 8 +- gtsam/inference/Symbol.cpp | 7 +- gtsam/inference/Symbol.h | 7 +- gtsam/linear/VectorValues.cpp | 4 +- gtsam/nonlinear/Expression-inl.h | 24 +- gtsam/nonlinear/Expression.h | 8 +- gtsam/nonlinear/NonlinearEquality.h | 10 +- gtsam/nonlinear/Values-inl.h | 26 +- gtsam/nonlinear/Values.h | 18 +- gtsam/nonlinear/internal/ExpressionNode.h | 2 +- gtsam_unstable/base/BTree.h | 8 +- gtsam_unstable/base/DSF.h | 2 +- gtsam_unstable/dynamics/FullIMUFactor.h | 4 +- gtsam_unstable/dynamics/IMUFactor.h | 4 +- gtsam_unstable/dynamics/SimpleHelicopter.h | 12 +- gtsam_unstable/dynamics/VelocityConstraint.h | 8 +- gtsam_unstable/linear/QPSParser.cpp | 47 ++- .../slam/EquivInertialNavFactor_GlobalVel.h | 68 ++-- .../EquivInertialNavFactor_GlobalVel_NoBias.h | 26 +- .../slam/InertialNavFactor_GlobalVelocity.h | 40 +- gtsam_unstable/slam/InvDepthFactorVariant1.h | 8 +- gtsam_unstable/slam/InvDepthFactorVariant2.h | 8 +- gtsam_unstable/slam/InvDepthFactorVariant3.h | 20 +- 32 files changed, 410 insertions(+), 386 deletions(-) diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index a3a14c6c3..013947bbd 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -29,7 +29,7 @@ #include #include -#include +#include #include #include @@ -489,7 +489,7 @@ struct MultiplyWithInverseFunction { // The function phi should calculate f(a)*b, with derivatives in a and b. // Naturally, the derivative in b is f(a). - typedef boost::function, OptionalJacobian)> Operator; diff --git a/gtsam/base/Testable.h b/gtsam/base/Testable.h index e8cdbd8e0..6062c7ae1 100644 --- a/gtsam/base/Testable.h +++ b/gtsam/base/Testable.h @@ -34,8 +34,9 @@ #pragma once #include -#include +#include #include +#include #include #define GTSAM_PRINT(x)((x).print(#x)) @@ -119,10 +120,10 @@ namespace gtsam { * Binary predicate on shared pointers */ template - struct equals_star : public std::function&, const boost::shared_ptr&)> { + struct equals_star : public std::function&, const std::shared_ptr&)> { double tol_; equals_star(double tol = 1e-9) : tol_(tol) {} - bool operator()(const boost::shared_ptr& expected, const boost::shared_ptr& actual) { + bool operator()(const std::shared_ptr& expected, const std::shared_ptr& actual) { if (!actual && !expected) return true; return actual && expected && traits::Equals(*actual,*expected, tol_); } diff --git a/gtsam/base/lieProxies.h b/gtsam/base/lieProxies.h index 820fc7333..c811eb58a 100644 --- a/gtsam/base/lieProxies.h +++ b/gtsam/base/lieProxies.h @@ -24,7 +24,7 @@ * * These should not be used outside of tests, as they are just remappings * of the original functions. We use these to avoid needing to do - * too much boost::bind magic or writing a bunch of separate proxy functions. + * too much std::bind magic or writing a bunch of separate proxy functions. * * Don't expect all classes to work for all of these functions. */ diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h index c624c900e..05b60033f 100644 --- a/gtsam/base/numericalDerivative.h +++ b/gtsam/base/numericalDerivative.h @@ -18,8 +18,7 @@ // \callgraph #pragma once -#include -#include +#include #include #include @@ -38,13 +37,13 @@ namespace gtsam { * 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, boost::placeholders::_1, boost::none) + * std::bind(bar, std::placeholders::_1, boost::none) * This syntax will fix the optional argument to boost::none, while using the first argument provided * * For member functions, such as below, with an instantiated copy instanceOfSomeClass * Foo SomeClass::bar(const Obj& a) * Use boost bind as follows to create a function pointer that uses the member function: - * boost::bind(&SomeClass::bar, ref(instanceOfSomeClass), boost::placeholders::_1) + * std::bind(&SomeClass::bar, ref(instanceOfSomeClass), std::placeholders::_1) * * For additional details, see the documentation: * http://www.boost.org/doc/libs/release/libs/bind/bind.html @@ -69,7 +68,7 @@ struct FixedSizeMatrix { template ::dimension> typename Eigen::Matrix numericalGradient( - boost::function h, const X& x, double delta = 1e-5) { + std::function h, const X& x, double delta = 1e-5) { double factor = 1.0 / (2.0 * delta); BOOST_STATIC_ASSERT_MSG( @@ -109,7 +108,7 @@ typename Eigen::Matrix numericalGradient( template ::dimension> // TODO Should compute fixed-size matrix typename internal::FixedSizeMatrix::type numericalDerivative11( - boost::function h, const X& x, double delta = 1e-5) { + std::function h, const X& x, double delta = 1e-5) { typedef typename internal::FixedSizeMatrix::type Matrix; BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), @@ -150,7 +149,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative11( template typename internal::FixedSizeMatrix::type numericalDerivative11(Y (*h)(const X&), const X& x, double delta = 1e-5) { - return numericalDerivative11(boost::bind(h, boost::placeholders::_1), x, + return numericalDerivative11(std::bind(h, std::placeholders::_1), x, delta); } @@ -164,14 +163,14 @@ typename internal::FixedSizeMatrix::type numericalDerivative11(Y (*h)(const * @tparam int N is the dimension of the X1 input value if variable dimension type but known at test time */ template::dimension> -typename internal::FixedSizeMatrix::type numericalDerivative21(const boost::function& h, +typename internal::FixedSizeMatrix::type numericalDerivative21(const std::function& h, const X1& x1, const X2& x2, double delta = 1e-5) { BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); return numericalDerivative11( - boost::bind(h, boost::placeholders::_1, boost::cref(x2)), x1, delta); + std::bind(h, std::placeholders::_1, std::cref(x2)), x1, delta); } /** use a raw C++ function pointer */ @@ -179,7 +178,7 @@ template typename internal::FixedSizeMatrix::type numericalDerivative21(Y (*h)(const X1&, const X2&), const X1& x1, const X2& x2, double delta = 1e-5) { return numericalDerivative21( - boost::bind(h, boost::placeholders::_1, boost::placeholders::_2), x1, x2, + std::bind(h, std::placeholders::_1, std::placeholders::_2), x1, x2, delta); } @@ -193,14 +192,14 @@ typename internal::FixedSizeMatrix::type numericalDerivative21(Y (*h)(cons * @tparam int N is the dimension of the X2 input value if variable dimension type but known at test time */ template::dimension> -typename internal::FixedSizeMatrix::type numericalDerivative22(boost::function h, +typename internal::FixedSizeMatrix::type numericalDerivative22(std::function h, const X1& x1, const X2& x2, double delta = 1e-5) { // BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), // "Template argument X1 must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X2 must be a manifold type."); return numericalDerivative11( - boost::bind(h, boost::cref(x1), boost::placeholders::_1), x2, delta); + std::bind(h, std::cref(x1), std::placeholders::_1), x2, delta); } /** use a raw C++ function pointer */ @@ -208,7 +207,7 @@ template typename internal::FixedSizeMatrix::type numericalDerivative22(Y (*h)(const X1&, const X2&), const X1& x1, const X2& x2, double delta = 1e-5) { return numericalDerivative22( - boost::bind(h, boost::placeholders::_1, boost::placeholders::_2), x1, x2, + std::bind(h, std::placeholders::_1, std::placeholders::_2), x1, x2, delta); } @@ -225,14 +224,14 @@ typename internal::FixedSizeMatrix::type numericalDerivative22(Y (*h)(cons */ template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative31( - boost::function h, const X1& x1, + std::function h, const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); return numericalDerivative11( - boost::bind(h, boost::placeholders::_1, boost::cref(x2), boost::cref(x3)), + std::bind(h, std::placeholders::_1, std::cref(x2), std::cref(x3)), x1, delta); } @@ -240,8 +239,8 @@ template typename internal::FixedSizeMatrix::type numericalDerivative31(Y (*h)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { return numericalDerivative31( - boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, - boost::placeholders::_3), + std::bind(h, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3), x1, x2, x3, delta); } @@ -258,14 +257,14 @@ typename internal::FixedSizeMatrix::type numericalDerivative31(Y (*h)(cons */ template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative32( - boost::function h, const X1& x1, + std::function h, const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X2 must be a manifold type."); return numericalDerivative11( - boost::bind(h, boost::cref(x1), boost::placeholders::_1, boost::cref(x3)), + std::bind(h, std::cref(x1), std::placeholders::_1, std::cref(x3)), x2, delta); } @@ -273,8 +272,8 @@ template inline typename internal::FixedSizeMatrix::type numericalDerivative32(Y (*h)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { return numericalDerivative32( - boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, - boost::placeholders::_3), + std::bind(h, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3), x1, x2, x3, delta); } @@ -291,14 +290,14 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative32(Y (* */ template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative33( - boost::function h, const X1& x1, + std::function h, const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X3 must be a manifold type."); return numericalDerivative11( - boost::bind(h, boost::cref(x1), boost::cref(x2), boost::placeholders::_1), + std::bind(h, std::cref(x1), std::cref(x2), std::placeholders::_1), x3, delta); } @@ -306,8 +305,8 @@ template inline typename internal::FixedSizeMatrix::type numericalDerivative33(Y (*h)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { return numericalDerivative33( - boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, - boost::placeholders::_3), + std::bind(h, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3), x1, x2, x3, delta); } @@ -324,15 +323,15 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative33(Y (* */ template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative41( - boost::function h, const X1& x1, + std::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) { BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); return numericalDerivative11( - boost::bind(h, boost::placeholders::_1, boost::cref(x2), boost::cref(x3), - boost::cref(x4)), + std::bind(h, std::placeholders::_1, std::cref(x2), std::cref(x3), + std::cref(x4)), x1, delta); } @@ -340,8 +339,8 @@ template inline typename internal::FixedSizeMatrix::type numericalDerivative41(Y (*h)(const X1&, const X2&, const X3&, const X4&), const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) { return numericalDerivative41( - boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, - boost::placeholders::_3, boost::placeholders::_4), + std::bind(h, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, std::placeholders::_4), x1, x2, x3, x4); } @@ -358,15 +357,15 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative41(Y (* */ template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative42( - boost::function h, const X1& x1, + std::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) { BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X2 must be a manifold type."); return numericalDerivative11( - boost::bind(h, boost::cref(x1), boost::placeholders::_1, boost::cref(x3), - boost::cref(x4)), + std::bind(h, std::cref(x1), std::placeholders::_1, std::cref(x3), + std::cref(x4)), x2, delta); } @@ -374,8 +373,8 @@ template inline typename internal::FixedSizeMatrix::type numericalDerivative42(Y (*h)(const X1&, const X2&, const X3&, const X4&), const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) { return numericalDerivative42( - boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, - boost::placeholders::_3, boost::placeholders::_4), + std::bind(h, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, std::placeholders::_4), x1, x2, x3, x4); } @@ -392,15 +391,15 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative42(Y (* */ template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative43( - boost::function h, const X1& x1, + std::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) { BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X3 must be a manifold type."); return numericalDerivative11( - boost::bind(h, boost::cref(x1), boost::cref(x2), boost::placeholders::_1, - boost::cref(x4)), + std::bind(h, std::cref(x1), std::cref(x2), std::placeholders::_1, + std::cref(x4)), x3, delta); } @@ -408,8 +407,8 @@ template inline typename internal::FixedSizeMatrix::type numericalDerivative43(Y (*h)(const X1&, const X2&, const X3&, const X4&), const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) { return numericalDerivative43( - boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, - boost::placeholders::_3, boost::placeholders::_4), + std::bind(h, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, std::placeholders::_4), x1, x2, x3, x4); } @@ -426,15 +425,15 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative43(Y (* */ template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative44( - boost::function h, const X1& x1, + std::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) { BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X4 must be a manifold type."); return numericalDerivative11( - boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), - boost::placeholders::_1), + std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3), + std::placeholders::_1), x4, delta); } @@ -442,8 +441,8 @@ template inline typename internal::FixedSizeMatrix::type numericalDerivative44(Y (*h)(const X1&, const X2&, const X3&, const X4&), const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) { return numericalDerivative44( - boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, - boost::placeholders::_3, boost::placeholders::_4), + std::bind(h, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, std::placeholders::_4), x1, x2, x3, x4); } @@ -461,15 +460,15 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative44(Y (* */ template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative51( - boost::function h, const X1& x1, + std::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); return numericalDerivative11( - boost::bind(h, boost::placeholders::_1, boost::cref(x2), boost::cref(x3), - boost::cref(x4), boost::cref(x5)), + std::bind(h, std::placeholders::_1, std::cref(x2), std::cref(x3), + std::cref(x4), std::cref(x5)), x1, delta); } @@ -477,9 +476,9 @@ template inline typename internal::FixedSizeMatrix::type numericalDerivative51(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&), const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { return numericalDerivative51( - boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, - boost::placeholders::_3, boost::placeholders::_4, - boost::placeholders::_5), + std::bind(h, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, std::placeholders::_4, + std::placeholders::_5), x1, x2, x3, x4, x5); } @@ -497,15 +496,15 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative51(Y (* */ template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative52( - boost::function h, const X1& x1, + std::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); return numericalDerivative11( - boost::bind(h, boost::cref(x1), boost::placeholders::_1, boost::cref(x3), - boost::cref(x4), boost::cref(x5)), + std::bind(h, std::cref(x1), std::placeholders::_1, std::cref(x3), + std::cref(x4), std::cref(x5)), x2, delta); } @@ -513,9 +512,9 @@ template inline typename internal::FixedSizeMatrix::type numericalDerivative52(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&), const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { return numericalDerivative52( - boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, - boost::placeholders::_3, boost::placeholders::_4, - boost::placeholders::_5), + std::bind(h, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, std::placeholders::_4, + std::placeholders::_5), x1, x2, x3, x4, x5); } @@ -533,15 +532,15 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative52(Y (* */ template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative53( - boost::function h, const X1& x1, + std::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); return numericalDerivative11( - boost::bind(h, boost::cref(x1), boost::cref(x2), boost::placeholders::_1, - boost::cref(x4), boost::cref(x5)), + std::bind(h, std::cref(x1), std::cref(x2), std::placeholders::_1, + std::cref(x4), std::cref(x5)), x3, delta); } @@ -549,9 +548,9 @@ template inline typename internal::FixedSizeMatrix::type numericalDerivative53(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&), const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { return numericalDerivative53( - boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, - boost::placeholders::_3, boost::placeholders::_4, - boost::placeholders::_5), + std::bind(h, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, std::placeholders::_4, + std::placeholders::_5), x1, x2, x3, x4, x5); } @@ -569,15 +568,15 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative53(Y (* */ template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative54( - boost::function h, const X1& x1, + std::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); return numericalDerivative11( - boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), - boost::placeholders::_1, boost::cref(x5)), + std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3), + std::placeholders::_1, std::cref(x5)), x4, delta); } @@ -585,9 +584,9 @@ template inline typename internal::FixedSizeMatrix::type numericalDerivative54(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&), const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { return numericalDerivative54( - boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, - boost::placeholders::_3, boost::placeholders::_4, - boost::placeholders::_5), + std::bind(h, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, std::placeholders::_4, + std::placeholders::_5), x1, x2, x3, x4, x5); } @@ -605,15 +604,15 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative54(Y (* */ template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative55( - boost::function h, const X1& x1, + std::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); return numericalDerivative11( - boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), - boost::cref(x4), boost::placeholders::_1), + std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3), + std::cref(x4), std::placeholders::_1), x5, delta); } @@ -621,9 +620,9 @@ template inline typename internal::FixedSizeMatrix::type numericalDerivative55(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&), const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { return numericalDerivative55( - boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, - boost::placeholders::_3, boost::placeholders::_4, - boost::placeholders::_5), + std::bind(h, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, std::placeholders::_4, + std::placeholders::_5), x1, x2, x3, x4, x5); } @@ -642,15 +641,15 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative55(Y (* */ template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative61( - boost::function h, const X1& x1, + std::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) { BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); return numericalDerivative11( - boost::bind(h, boost::placeholders::_1, boost::cref(x2), boost::cref(x3), - boost::cref(x4), boost::cref(x5), boost::cref(x6)), + std::bind(h, std::placeholders::_1, std::cref(x2), std::cref(x3), + std::cref(x4), std::cref(x5), std::cref(x6)), x1, delta); } @@ -658,9 +657,9 @@ template inline typename internal::FixedSizeMatrix::type numericalDerivative61(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&), const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) { return numericalDerivative61( - boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, - boost::placeholders::_3, boost::placeholders::_4, - boost::placeholders::_5, boost::placeholders::_6), + std::bind(h, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, std::placeholders::_4, + std::placeholders::_5, std::placeholders::_6), x1, x2, x3, x4, x5, x6); } @@ -679,15 +678,15 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative61(Y (* */ template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative62( - boost::function h, const X1& x1, + std::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) { BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); return numericalDerivative11( - boost::bind(h, boost::cref(x1), boost::placeholders::_1, boost::cref(x3), - boost::cref(x4), boost::cref(x5), boost::cref(x6)), + std::bind(h, std::cref(x1), std::placeholders::_1, std::cref(x3), + std::cref(x4), std::cref(x5), std::cref(x6)), x2, delta); } @@ -695,9 +694,9 @@ template inline typename internal::FixedSizeMatrix::type numericalDerivative62(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&), const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) { return numericalDerivative62( - boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, - boost::placeholders::_3, boost::placeholders::_4, - boost::placeholders::_5, boost::placeholders::_6), + std::bind(h, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, std::placeholders::_4, + std::placeholders::_5, std::placeholders::_6), x1, x2, x3, x4, x5, x6); } @@ -716,15 +715,15 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative62(Y (* */ template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative63( - boost::function h, const X1& x1, + std::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) { BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); return numericalDerivative11( - boost::bind(h, boost::cref(x1), boost::cref(x2), boost::placeholders::_1, - boost::cref(x4), boost::cref(x5), boost::cref(x6)), + std::bind(h, std::cref(x1), std::cref(x2), std::placeholders::_1, + std::cref(x4), std::cref(x5), std::cref(x6)), x3, delta); } @@ -732,9 +731,9 @@ template inline typename internal::FixedSizeMatrix::type numericalDerivative63(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&), const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) { return numericalDerivative63( - boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, - boost::placeholders::_3, boost::placeholders::_4, - boost::placeholders::_5, boost::placeholders::_6), + std::bind(h, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, std::placeholders::_4, + std::placeholders::_5, std::placeholders::_6), x1, x2, x3, x4, x5, x6); } @@ -753,15 +752,15 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative63(Y (* */ template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative64( - boost::function h, const X1& x1, + std::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) { BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); return numericalDerivative11( - boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), - boost::placeholders::_1, boost::cref(x5), boost::cref(x6)), + std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3), + std::placeholders::_1, std::cref(x5), std::cref(x6)), x4, delta); } @@ -769,9 +768,9 @@ template inline typename internal::FixedSizeMatrix::type numericalDerivative64(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&), const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) { return numericalDerivative64( - boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, - boost::placeholders::_3, boost::placeholders::_4, - boost::placeholders::_5, boost::placeholders::_6), + std::bind(h, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, std::placeholders::_4, + std::placeholders::_5, std::placeholders::_6), x1, x2, x3, x4, x5, x6); } @@ -790,15 +789,15 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative64(Y (* */ template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative65( - boost::function h, const X1& x1, + std::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) { BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); return numericalDerivative11( - boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), - boost::cref(x4), boost::placeholders::_1, boost::cref(x6)), + std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3), + std::cref(x4), std::placeholders::_1, std::cref(x6)), x5, delta); } @@ -806,9 +805,9 @@ template inline typename internal::FixedSizeMatrix::type numericalDerivative65(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&), const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) { return numericalDerivative65( - boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, - boost::placeholders::_3, boost::placeholders::_4, - boost::placeholders::_5, boost::placeholders::_6), + std::bind(h, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, std::placeholders::_4, + std::placeholders::_5, std::placeholders::_6), x1, x2, x3, x4, x5, x6); } @@ -827,7 +826,7 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative65(Y (* */ template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative66( - boost::function h, + std::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) { BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), @@ -835,8 +834,8 @@ typename internal::FixedSizeMatrix::type numericalDerivative66( BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); return numericalDerivative11( - boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), - boost::cref(x4), boost::cref(x5), boost::placeholders::_1), + std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3), + std::cref(x4), std::cref(x5), std::placeholders::_1), x6, delta); } @@ -844,9 +843,9 @@ template inline typename internal::FixedSizeMatrix::type numericalDerivative66(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&), const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) { return numericalDerivative66( - boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, - boost::placeholders::_3, boost::placeholders::_4, - boost::placeholders::_5, boost::placeholders::_6), + std::bind(h, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, std::placeholders::_4, + std::placeholders::_5, std::placeholders::_6), x1, x2, x3, x4, x5, x6); } @@ -859,22 +858,22 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative66(Y (* * @return n*n Hessian matrix computed via central differencing */ template -inline typename internal::FixedSizeMatrix::type numericalHessian(boost::function f, const X& x, +inline typename internal::FixedSizeMatrix::type numericalHessian(std::function f, const X& x, double delta = 1e-5) { BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X must be a manifold type."); typedef Eigen::Matrix::dimension, 1> VectorD; - typedef boost::function F; - typedef boost::function G; + typedef std::function F; + typedef std::function G; G ng = static_cast(numericalGradient ); return numericalDerivative11( - boost::bind(ng, f, boost::placeholders::_1, delta), x, delta); + std::bind(ng, f, std::placeholders::_1, delta), x, delta); } template inline typename internal::FixedSizeMatrix::type numericalHessian(double (*f)(const X&), const X& x, double delta = 1e-5) { - return numericalHessian(boost::function(f), x, delta); + return numericalHessian(std::function(f), x, delta); } /** Helper class that computes the derivative of f w.r.t. x1, centered about @@ -882,86 +881,86 @@ inline typename internal::FixedSizeMatrix::type numericalHessian(double (*f */ template class G_x1 { - const boost::function& f_; + const std::function& f_; X1 x1_; double delta_; public: typedef typename internal::FixedSizeMatrix::type Vector; - G_x1(const boost::function& f, const X1& x1, + G_x1(const std::function& f, const X1& x1, double delta) : f_(f), x1_(x1), delta_(delta) { } Vector operator()(const X2& x2) { return numericalGradient( - boost::bind(f_, boost::placeholders::_1, boost::cref(x2)), x1_, delta_); + std::bind(f_, std::placeholders::_1, std::cref(x2)), x1_, delta_); } }; template inline typename internal::FixedSizeMatrix::type numericalHessian212( - boost::function f, const X1& x1, const X2& x2, + std::function f, const X1& x1, const X2& x2, double delta = 1e-5) { typedef typename internal::FixedSizeMatrix::type Vector; G_x1 g_x1(f, x1, delta); return numericalDerivative11( - boost::function( - boost::bind(boost::ref(g_x1), boost::placeholders::_1)), + std::function( + std::bind(std::ref(g_x1), std::placeholders::_1)), x2, delta); } template inline typename internal::FixedSizeMatrix::type numericalHessian212(double (*f)(const X1&, const X2&), const X1& x1, const X2& x2, double delta = 1e-5) { - return numericalHessian212(boost::function(f), + return numericalHessian212(std::function(f), x1, x2, delta); } template inline typename internal::FixedSizeMatrix::type numericalHessian211( - boost::function f, const X1& x1, const X2& x2, + std::function f, const X1& x1, const X2& x2, double delta = 1e-5) { typedef typename internal::FixedSizeMatrix::type Vector; - Vector (*numGrad)(boost::function, const X1&, + Vector (*numGrad)(std::function, const X1&, double) = &numericalGradient; - boost::function f2( - boost::bind(f, boost::placeholders::_1, boost::cref(x2))); + std::function f2( + std::bind(f, std::placeholders::_1, std::cref(x2))); return numericalDerivative11( - boost::function( - boost::bind(numGrad, f2, boost::placeholders::_1, delta)), + std::function( + std::bind(numGrad, f2, std::placeholders::_1, delta)), x1, delta); } template inline typename internal::FixedSizeMatrix::type numericalHessian211(double (*f)(const X1&, const X2&), const X1& x1, const X2& x2, double delta = 1e-5) { - return numericalHessian211(boost::function(f), + return numericalHessian211(std::function(f), x1, x2, delta); } template inline typename internal::FixedSizeMatrix::type numericalHessian222( - boost::function f, const X1& x1, const X2& x2, + std::function f, const X1& x1, const X2& x2, double delta = 1e-5) { typedef typename internal::FixedSizeMatrix::type Vector; - Vector (*numGrad)(boost::function, const X2&, + Vector (*numGrad)(std::function, const X2&, double) = &numericalGradient; - boost::function f2( - boost::bind(f, boost::cref(x1), boost::placeholders::_1)); + std::function f2( + std::bind(f, std::cref(x1), std::placeholders::_1)); return numericalDerivative11( - boost::function( - boost::bind(numGrad, f2, boost::placeholders::_1, delta)), + std::function( + std::bind(numGrad, f2, std::placeholders::_1, delta)), x2, delta); } template inline typename internal::FixedSizeMatrix::type numericalHessian222(double (*f)(const X1&, const X2&), const X1& x1, const X2& x2, double delta = 1e-5) { - return numericalHessian222(boost::function(f), + return numericalHessian222(std::function(f), x1, x2, delta); } @@ -971,17 +970,17 @@ inline typename internal::FixedSizeMatrix::type numericalHessian222(doubl /* **************************************************************** */ template inline typename internal::FixedSizeMatrix::type numericalHessian311( - boost::function f, const X1& x1, + std::function f, const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { typedef typename internal::FixedSizeMatrix::type Vector; - Vector (*numGrad)(boost::function, const X1&, + Vector (*numGrad)(std::function, const X1&, double) = &numericalGradient; - boost::function f2(boost::bind( - f, boost::placeholders::_1, boost::cref(x2), boost::cref(x3))); + std::function f2(std::bind( + f, std::placeholders::_1, std::cref(x2), std::cref(x3))); return numericalDerivative11( - boost::function( - boost::bind(numGrad, f2, boost::placeholders::_1, delta)), + std::function( + std::bind(numGrad, f2, std::placeholders::_1, delta)), x1, delta); } @@ -989,24 +988,24 @@ template inline typename internal::FixedSizeMatrix::type numericalHessian311(double (*f)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { return numericalHessian311( - boost::function(f), x1, x2, x3, + std::function(f), x1, x2, x3, delta); } /* **************************************************************** */ template inline typename internal::FixedSizeMatrix::type numericalHessian322( - boost::function f, const X1& x1, + std::function f, const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { typedef typename internal::FixedSizeMatrix::type Vector; - Vector (*numGrad)(boost::function, const X2&, + Vector (*numGrad)(std::function, const X2&, double) = &numericalGradient; - boost::function f2(boost::bind( - f, boost::cref(x1), boost::placeholders::_1, boost::cref(x3))); + std::function f2(std::bind( + f, std::cref(x1), std::placeholders::_1, std::cref(x3))); return numericalDerivative11( - boost::function( - boost::bind(numGrad, f2, boost::placeholders::_1, delta)), + std::function( + std::bind(numGrad, f2, std::placeholders::_1, delta)), x2, delta); } @@ -1014,24 +1013,24 @@ template inline typename internal::FixedSizeMatrix::type numericalHessian322(double (*f)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { return numericalHessian322( - boost::function(f), x1, x2, x3, + std::function(f), x1, x2, x3, delta); } /* **************************************************************** */ template inline typename internal::FixedSizeMatrix::type numericalHessian333( - boost::function f, const X1& x1, + std::function f, const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { typedef typename internal::FixedSizeMatrix::type Vector; - Vector (*numGrad)(boost::function, const X3&, + Vector (*numGrad)(std::function, const X3&, double) = &numericalGradient; - boost::function f2(boost::bind( - f, boost::cref(x1), boost::cref(x2), boost::placeholders::_1)); + std::function f2(std::bind( + f, std::cref(x1), std::cref(x2), std::placeholders::_1)); return numericalDerivative11( - boost::function( - boost::bind(numGrad, f2, boost::placeholders::_1, delta)), + std::function( + std::bind(numGrad, f2, std::placeholders::_1, delta)), x3, delta); } @@ -1039,41 +1038,41 @@ template inline typename internal::FixedSizeMatrix::type numericalHessian333(double (*f)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { return numericalHessian333( - boost::function(f), x1, x2, x3, + std::function(f), x1, x2, x3, delta); } /* **************************************************************** */ template inline typename internal::FixedSizeMatrix::type numericalHessian312( - boost::function f, const X1& x1, + std::function f, const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { return numericalHessian212( - boost::function( - boost::bind(f, boost::placeholders::_1, boost::placeholders::_2, - boost::cref(x3))), + std::function( + std::bind(f, std::placeholders::_1, std::placeholders::_2, + std::cref(x3))), x1, x2, delta); } template inline typename internal::FixedSizeMatrix::type numericalHessian313( - boost::function f, const X1& x1, + std::function f, const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { return numericalHessian212( - boost::function( - boost::bind(f, boost::placeholders::_1, boost::cref(x2), - boost::placeholders::_2)), + std::function( + std::bind(f, std::placeholders::_1, std::cref(x2), + std::placeholders::_2)), x1, x3, delta); } template inline typename internal::FixedSizeMatrix::type numericalHessian323( - boost::function f, const X1& x1, + std::function f, const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { return numericalHessian212( - boost::function( - boost::bind(f, boost::cref(x1), boost::placeholders::_1, - boost::placeholders::_2)), + std::function( + std::bind(f, std::cref(x1), std::placeholders::_1, + std::placeholders::_2)), x2, x3, delta); } @@ -1082,7 +1081,7 @@ template inline typename internal::FixedSizeMatrix::type numericalHessian312(double (*f)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { return numericalHessian312( - boost::function(f), x1, x2, x3, + std::function(f), x1, x2, x3, delta); } @@ -1090,7 +1089,7 @@ template inline typename internal::FixedSizeMatrix::type numericalHessian313(double (*f)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { return numericalHessian313( - boost::function(f), x1, x2, x3, + std::function(f), x1, x2, x3, delta); } @@ -1098,7 +1097,7 @@ template inline typename internal::FixedSizeMatrix::type numericalHessian323(double (*f)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { return numericalHessian323( - boost::function(f), x1, x2, x3, + std::function(f), x1, x2, x3, delta); } diff --git a/gtsam/discrete/DecisionTree-inl.h b/gtsam/discrete/DecisionTree-inl.h index 80b226e3a..439889ebf 100644 --- a/gtsam/discrete/DecisionTree-inl.h +++ b/gtsam/discrete/DecisionTree-inl.h @@ -450,7 +450,7 @@ namespace gtsam { template template DecisionTree::DecisionTree(const DecisionTree& other, - const std::map& map, boost::function op) { + const std::map& map, std::function op) { root_ = convert(other.root_, map, op); } @@ -568,7 +568,7 @@ namespace gtsam { template typename DecisionTree::NodePtr DecisionTree::convert( const typename DecisionTree::NodePtr& f, const std::map& map, - boost::function op) { + std::function op) { typedef DecisionTree MX; typedef typename MX::Leaf MXLeaf; diff --git a/gtsam/discrete/DecisionTree.h b/gtsam/discrete/DecisionTree.h index ecf03ad5d..0ee0b8be0 100644 --- a/gtsam/discrete/DecisionTree.h +++ b/gtsam/discrete/DecisionTree.h @@ -20,10 +20,12 @@ #pragma once #include + #include +#include #include -#include #include +#include namespace gtsam { @@ -38,8 +40,8 @@ namespace gtsam { public: /** Handy typedefs for unary and binary function types */ - typedef boost::function Unary; - typedef boost::function Binary; + typedef std::function Unary; + typedef std::function Binary; /** A label annotated with cardinality */ typedef std::pair LabelC; @@ -107,7 +109,7 @@ namespace gtsam { /** Convert to a different type */ template NodePtr convert(const typename DecisionTree::NodePtr& f, const std::map& map, boost::function op); + L>& map, std::function op); /** Default constructor */ DecisionTree(); @@ -143,7 +145,7 @@ namespace gtsam { /** Convert from a different type */ template DecisionTree(const DecisionTree& other, - const std::map& map, boost::function op); + const std::map& map, std::function op); /// @} /// @name Testable diff --git a/gtsam/geometry/Cyclic.h b/gtsam/geometry/Cyclic.h index 0b81ff0f9..35578ffe0 100644 --- a/gtsam/geometry/Cyclic.h +++ b/gtsam/geometry/Cyclic.h @@ -17,7 +17,9 @@ #include #include -#include // for cout :-( + +#include +#include // for cout :-( namespace gtsam { diff --git a/gtsam/inference/EliminateableFactorGraph.h b/gtsam/inference/EliminateableFactorGraph.h index 316ca8ee4..edc4883e7 100644 --- a/gtsam/inference/EliminateableFactorGraph.h +++ b/gtsam/inference/EliminateableFactorGraph.h @@ -19,7 +19,7 @@ #pragma once #include -#include +#include #include #include @@ -86,7 +86,7 @@ namespace gtsam { typedef std::pair, boost::shared_ptr<_FactorType> > EliminationResult; /// The function type that does a single dense elimination step on a subgraph. - typedef boost::function Eliminate; + typedef std::function Eliminate; /// Typedef for an optional variable index as an argument to elimination functions typedef boost::optional OptionalVariableIndex; diff --git a/gtsam/inference/LabeledSymbol.cpp b/gtsam/inference/LabeledSymbol.cpp index 17ff6fd22..6a1739e20 100644 --- a/gtsam/inference/LabeledSymbol.cpp +++ b/gtsam/inference/LabeledSymbol.cpp @@ -15,15 +15,12 @@ * @author: Alex Cunningham */ -#include - -#include -#include - -#include - #include +#include +#include +#include + namespace gtsam { using namespace std; @@ -109,17 +106,37 @@ bool LabeledSymbol::operator!=(gtsam::Key comp) const { /* ************************************************************************* */ static LabeledSymbol make(gtsam::Key key) { return LabeledSymbol(key);} -boost::function LabeledSymbol::TypeTest(unsigned char c) { - return boost::bind(&LabeledSymbol::chr, boost::bind(make, boost::placeholders::_1)) == c; +std::function LabeledSymbol::TypeTest(unsigned char c) { + // Use lambda function to check equality + auto equals = [](unsigned char s, unsigned char c) { return s == c; }; + return std::bind( + equals, + std::bind(&LabeledSymbol::chr, std::bind(make, std::placeholders::_1)), + c); } -boost::function LabeledSymbol::LabelTest(unsigned char label) { - return boost::bind(&LabeledSymbol::label, boost::bind(make, boost::placeholders::_1)) == label; +std::function LabeledSymbol::LabelTest(unsigned char label) { + // Use lambda function to check equality + auto equals = [](unsigned char s, unsigned char c) { return s == c; }; + return std::bind( + equals, + std::bind(&LabeledSymbol::label, std::bind(make, std::placeholders::_1)), + label); } -boost::function LabeledSymbol::TypeLabelTest(unsigned char c, unsigned char label) { - return boost::bind(&LabeledSymbol::chr, boost::bind(make, boost::placeholders::_1)) == c && - boost::bind(&LabeledSymbol::label, boost::bind(make, boost::placeholders::_1)) == label; +std::function LabeledSymbol::TypeLabelTest(unsigned char c, unsigned char label) { + // Use lambda functions for && and == + auto logical_and = [](bool is_type, bool is_label) { return is_type == is_label; }; + auto equals = [](unsigned char s, unsigned char c) { return s == c; }; + return std::bind(logical_and, + std::bind(equals, + std::bind(&LabeledSymbol::chr, + std::bind(make, std::placeholders::_1)), + c), + std::bind(equals, + std::bind(&LabeledSymbol::label, + std::bind(make, std::placeholders::_1)), + label)); } /* ************************************************************************* */ diff --git a/gtsam/inference/LabeledSymbol.h b/gtsam/inference/LabeledSymbol.h index 15a2a2501..5aee4a0e2 100644 --- a/gtsam/inference/LabeledSymbol.h +++ b/gtsam/inference/LabeledSymbol.h @@ -19,8 +19,8 @@ #pragma once +#include #include -#include namespace gtsam { @@ -89,13 +89,13 @@ public: */ // Checks only the type - static boost::function TypeTest(unsigned char c); + static std::function TypeTest(unsigned char c); // Checks only the robot ID (label_) - static boost::function LabelTest(unsigned char label); + static std::function LabelTest(unsigned char label); // Checks both type and the robot ID - static boost::function TypeLabelTest(unsigned char c, unsigned char label); + static std::function TypeLabelTest(unsigned char c, unsigned char label); // Converts to upper/lower versions of labels LabeledSymbol upper() const { return LabeledSymbol(c_, toupper(label_), j_); } diff --git a/gtsam/inference/Symbol.cpp b/gtsam/inference/Symbol.cpp index 61e397f40..925ba9ce3 100644 --- a/gtsam/inference/Symbol.cpp +++ b/gtsam/inference/Symbol.cpp @@ -62,8 +62,11 @@ Symbol::operator std::string() const { static Symbol make(gtsam::Key key) { return Symbol(key);} -boost::function Symbol::ChrTest(unsigned char c) { - return boost::bind(&Symbol::chr, boost::bind(make, boost::placeholders::_1)) == c; +std::function Symbol::ChrTest(unsigned char c) { + auto equals = [](unsigned char s, unsigned char c) { return s == c; }; + return std::bind( + equals, std::bind(&Symbol::chr, std::bind(make, std::placeholders::_1)), + c); } GTSAM_EXPORT std::ostream &operator<<(std::ostream &os, const Symbol &symbol) { diff --git a/gtsam/inference/Symbol.h b/gtsam/inference/Symbol.h index 89fb0d161..017d5134a 100644 --- a/gtsam/inference/Symbol.h +++ b/gtsam/inference/Symbol.h @@ -18,11 +18,12 @@ #pragma once -#include #include +#include + #include -#include #include +#include namespace gtsam { @@ -114,7 +115,7 @@ public: * Values::filter() function to retrieve all key-value pairs with the * requested character. */ - static boost::function ChrTest(unsigned char c); + static std::function ChrTest(unsigned char c); /// Output stream operator that can be used with key_formatter (see Key.h). GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &, const Symbol &); diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp index f72799c0a..6a2514b35 100644 --- a/gtsam/linear/VectorValues.cpp +++ b/gtsam/linear/VectorValues.cpp @@ -38,8 +38,8 @@ namespace gtsam { { // Merge using predicate for comparing first of pair merge(first.begin(), first.end(), second.begin(), second.end(), inserter(values_, values_.end()), - boost::bind(&less::operator(), less(), boost::bind(&KeyValuePair::first, boost::placeholders::_1), - boost::bind(&KeyValuePair::first, boost::placeholders::_2))); + std::bind(&less::operator(), less(), std::bind(&KeyValuePair::first, std::placeholders::_1), + std::bind(&KeyValuePair::first, std::placeholders::_2))); if(size() != first.size() + second.size()) throw invalid_argument("Requested to merge two VectorValues that have one or more variables in common."); } diff --git a/gtsam/nonlinear/Expression-inl.h b/gtsam/nonlinear/Expression-inl.h index 85f2f14bc..cf2462dfc 100644 --- a/gtsam/nonlinear/Expression-inl.h +++ b/gtsam/nonlinear/Expression-inl.h @@ -83,8 +83,8 @@ template Expression::Expression(const Expression& expression, T (A::*method)(typename MakeOptionalJacobian::type) const) : root_( - new internal::UnaryExpression(boost::bind(method, - boost::placeholders::_1, boost::placeholders::_2), + new internal::UnaryExpression(std::bind(method, + std::placeholders::_1, std::placeholders::_2), expression)) { } @@ -97,9 +97,9 @@ Expression::Expression(const Expression& expression1, const Expression& expression2) : root_( new internal::BinaryExpression( - boost::bind(method, boost::placeholders::_1, - boost::placeholders::_2, boost::placeholders::_3, - boost::placeholders::_4), + std::bind(method, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3, + std::placeholders::_4), expression1, expression2)) { } @@ -114,10 +114,10 @@ Expression::Expression(const Expression& expression1, const Expression& expression2, const Expression& expression3) : root_( new internal::TernaryExpression( - boost::bind(method, boost::placeholders::_1, - boost::placeholders::_2, boost::placeholders::_3, - boost::placeholders::_4, boost::placeholders::_5, - boost::placeholders::_6), + std::bind(method, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3, + std::placeholders::_4, std::placeholders::_5, + std::placeholders::_6), expression1, expression2, expression3)) { } @@ -255,9 +255,9 @@ template Expression operator*(const Expression& expression1, const Expression& expression2) { return Expression( - boost::bind(internal::apply_compose(), boost::placeholders::_1, - boost::placeholders::_2, boost::placeholders::_3, - boost::placeholders::_4), + std::bind(internal::apply_compose(), std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3, + std::placeholders::_4), expression1, expression2); } diff --git a/gtsam/nonlinear/Expression.h b/gtsam/nonlinear/Expression.h index bda457595..eb828760d 100644 --- a/gtsam/nonlinear/Expression.h +++ b/gtsam/nonlinear/Expression.h @@ -68,20 +68,20 @@ public: // Expression::BinaryFunction,Point3>::type template struct UnaryFunction { - typedef boost::function< + typedef std::function< T(const A1&, typename MakeOptionalJacobian::type)> type; }; template struct BinaryFunction { - typedef boost::function< + typedef std::function< T(const A1&, const A2&, typename MakeOptionalJacobian::type, typename MakeOptionalJacobian::type)> type; }; template struct TernaryFunction { - typedef boost::function< + typedef std::function< T(const A1&, const A2&, const A3&, typename MakeOptionalJacobian::type, typename MakeOptionalJacobian::type, @@ -239,7 +239,7 @@ class BinarySumExpression : public Expression { */ template Expression linearExpression( - const boost::function& f, const Expression& expression, + const std::function& f, const Expression& expression, const Eigen::Matrix::dimension, traits::dimension>& dTdA) { // Use lambda to endow f with a linear Jacobian typename Expression::template UnaryFunction::type g = diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index f10ba93ae..6b9972156 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -69,7 +69,7 @@ public: /** * Function that compares two values */ - typedef boost::function CompareFunction; + typedef std::function CompareFunction; CompareFunction compare_; // bool (*compare_)(const T& a, const T& b); @@ -87,8 +87,8 @@ public: * Constructor - forces exact evaluation */ NonlinearEquality(Key j, const T& feasible, - const CompareFunction &_compare = boost::bind(traits::Equals, - boost::placeholders::_1, boost::placeholders::_2, 1e-9)) : + const CompareFunction &_compare = std::bind(traits::Equals, + std::placeholders::_1, std::placeholders::_2, 1e-9)) : Base(noiseModel::Constrained::All(traits::GetDimension(feasible)), j), feasible_(feasible), allow_error_(false), error_gain_(0.0), // compare_(_compare) { @@ -98,8 +98,8 @@ public: * Constructor - allows inexact evaluation */ NonlinearEquality(Key j, const T& feasible, double error_gain, - const CompareFunction &_compare = boost::bind(traits::Equals, - boost::placeholders::_1, boost::placeholders::_2, 1e-9)) : + const CompareFunction &_compare = std::bind(traits::Equals, + std::placeholders::_1, std::placeholders::_2, 1e-9)) : Base(noiseModel::Constrained::All(traits::GetDimension(feasible)), j), feasible_(feasible), allow_error_(true), error_gain_(error_gain), // compare_(_compare) { diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index eca7416c9..8ebdcab17 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -103,7 +103,7 @@ namespace gtsam { boost::transform_iterator< KeyValuePair(*)(Values::KeyValuePair), boost::filter_iterator< - boost::function, + std::function, Values::iterator> > iterator; @@ -113,7 +113,7 @@ namespace gtsam { boost::transform_iterator< ConstKeyValuePair(*)(Values::ConstKeyValuePair), boost::filter_iterator< - boost::function, + std::function, Values::const_iterator> > const_const_iterator; @@ -134,7 +134,7 @@ namespace gtsam { private: Filtered( - const boost::function& filter, + const std::function& filter, Values& values) : begin_( boost::make_transform_iterator( @@ -205,7 +205,7 @@ namespace gtsam { const_iterator begin_; const_iterator end_; ConstFiltered( - const boost::function& filter, + const std::function& filter, const Values& values) { // We remove the const from values to create a non-const Filtered // view, then pull the const_iterators out of it. @@ -236,35 +236,35 @@ namespace gtsam { /* ************************************************************************* */ Values::Filtered - inline Values::filter(const boost::function& filterFcn) { + inline Values::filter(const std::function& filterFcn) { return filter(filterFcn); } /* ************************************************************************* */ template Values::Filtered - Values::filter(const boost::function& filterFcn) { - return Filtered(boost::bind(&filterHelper, filterFcn, - boost::placeholders::_1), *this); + Values::filter(const std::function& filterFcn) { + return Filtered(std::bind(&filterHelper, filterFcn, + std::placeholders::_1), *this); } /* ************************************************************************* */ Values::ConstFiltered - inline Values::filter(const boost::function& filterFcn) const { + inline Values::filter(const std::function& filterFcn) const { return filter(filterFcn); } /* ************************************************************************* */ template Values::ConstFiltered - Values::filter(const boost::function& filterFcn) const { - return ConstFiltered(boost::bind(&filterHelper, - filterFcn, boost::placeholders::_1), *this); + Values::filter(const std::function& filterFcn) const { + return ConstFiltered(std::bind(&filterHelper, + filterFcn, std::placeholders::_1), *this); } /* ************************************************************************* */ template<> - inline bool Values::filterHelper(const boost::function filter, + inline bool Values::filterHelper(const std::function filter, const ConstKeyValuePair& key_value) { // Filter and check the type return filter(key_value.key); diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index 3b447ede1..33e9e7d82 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -108,19 +108,19 @@ namespace gtsam { /// Mutable forward iterator, with value type KeyValuePair typedef boost::transform_iterator< - boost::function1, KeyValueMap::iterator> iterator; + std::function, KeyValueMap::iterator> iterator; /// Const forward iterator, with value type ConstKeyValuePair typedef boost::transform_iterator< - boost::function1, KeyValueMap::const_iterator> const_iterator; + std::function, KeyValueMap::const_iterator> const_iterator; /// Mutable reverse iterator, with value type KeyValuePair typedef boost::transform_iterator< - boost::function1, KeyValueMap::reverse_iterator> reverse_iterator; + std::function, KeyValueMap::reverse_iterator> reverse_iterator; /// Const reverse iterator, with value type ConstKeyValuePair typedef boost::transform_iterator< - boost::function1, KeyValueMap::const_reverse_iterator> const_reverse_iterator; + std::function, KeyValueMap::const_reverse_iterator> const_reverse_iterator; typedef KeyValuePair value_type; @@ -321,7 +321,7 @@ namespace gtsam { * the original Values class. */ Filtered - filter(const boost::function& filterFcn); + filter(const std::function& filterFcn); /** * Return a filtered view of this Values class, without copying any data. @@ -344,7 +344,7 @@ namespace gtsam { */ template Filtered - filter(const boost::function& filterFcn = &_truePredicate); + filter(const std::function& filterFcn = &_truePredicate); /** * Return a filtered view of this Values class, without copying any data. @@ -360,7 +360,7 @@ namespace gtsam { * the original Values class. */ ConstFiltered - filter(const boost::function& filterFcn) const; + filter(const std::function& filterFcn) const; /** * Return a filtered view of this Values class, without copying any data. @@ -382,7 +382,7 @@ namespace gtsam { */ template ConstFiltered - filter(const boost::function& filterFcn = &_truePredicate) const; + filter(const std::function& filterFcn = &_truePredicate) const; // Count values of given type \c ValueType template @@ -399,7 +399,7 @@ namespace gtsam { // Filters based on ValueType (if not Value) and also based on the user- // supplied \c filter function. template - static bool filterHelper(const boost::function filter, const ConstKeyValuePair& key_value) { + static bool filterHelper(const std::function filter, const ConstKeyValuePair& key_value) { BOOST_STATIC_ASSERT((!boost::is_same::value)); // Filter and check the type return filter(key_value.key) && (dynamic_cast*>(&key_value.value)); diff --git a/gtsam/nonlinear/internal/ExpressionNode.h b/gtsam/nonlinear/internal/ExpressionNode.h index 2a1a07fb0..f6afb287e 100644 --- a/gtsam/nonlinear/internal/ExpressionNode.h +++ b/gtsam/nonlinear/internal/ExpressionNode.h @@ -57,7 +57,7 @@ T upAligned(T value, unsigned requiredAlignment = TraceAlignment) { * 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. + * are passed by value. This is the same way std::function works. * http://loki-lib.sourceforge.net/html/a00652.html */ template diff --git a/gtsam_unstable/base/BTree.h b/gtsam_unstable/base/BTree.h index 96424324b..9d854a169 100644 --- a/gtsam_unstable/base/BTree.h +++ b/gtsam_unstable/base/BTree.h @@ -20,7 +20,7 @@ #include #include #include -#include +#include namespace gtsam { @@ -260,7 +260,7 @@ namespace gtsam { } /** iterate over tree */ - void iter(boost::function f) const { + void iter(std::function f) const { if (!root_) return; left().iter(f); f(key(), value()); @@ -269,7 +269,7 @@ namespace gtsam { /** map key-values in tree over function f that computes a new value */ template - BTree map(boost::function f) const { + BTree map(std::function f) const { if (empty()) return BTree (); std::pair xd(key(), f(key(), value())); return BTree (left().map(f), xd, right().map(f)); @@ -282,7 +282,7 @@ namespace gtsam { * The associated values are passed to [f] in reverse sort order */ template - ACC fold(boost::function f, + ACC fold(std::function f, const ACC& a) const { if (!root_) return a; ACC ar = right().fold(f, a); // fold over right subtree diff --git a/gtsam_unstable/base/DSF.h b/gtsam_unstable/base/DSF.h index c7b8cd417..4ad7d3ea8 100644 --- a/gtsam_unstable/base/DSF.h +++ b/gtsam_unstable/base/DSF.h @@ -122,7 +122,7 @@ public: } // maps f over all keys, must be invertible - DSF map(boost::function func) const { + DSF map(std::function func) const { DSF t; for(const KeyLabel& pair: (Tree)*this) t = t.add(func(pair.first), func(pair.second)); diff --git a/gtsam_unstable/dynamics/FullIMUFactor.h b/gtsam_unstable/dynamics/FullIMUFactor.h index 337f2bc43..9f00f81d6 100644 --- a/gtsam_unstable/dynamics/FullIMUFactor.h +++ b/gtsam_unstable/dynamics/FullIMUFactor.h @@ -91,9 +91,9 @@ public: z.segment(3, 3).operator=(gyro_); // Strange syntax to work around ambiguous operator error with clang z.tail(3).operator=(x2.t()); // Strange syntax to work around ambiguous operator error with clang if (H1) *H1 = numericalDerivative21( - boost::bind(This::predict_proxy, boost::placeholders::_1, boost::placeholders::_2, dt_), x1, x2, 1e-5); + std::bind(This::predict_proxy, std::placeholders::_1, std::placeholders::_2, dt_), x1, x2, 1e-5); if (H2) *H2 = numericalDerivative22( - boost::bind(This::predict_proxy, boost::placeholders::_1, boost::placeholders::_2, dt_), x1, x2, 1e-5); + std::bind(This::predict_proxy, std::placeholders::_1, std::placeholders::_2, dt_), x1, x2, 1e-5); return z - predict_proxy(x1, x2, dt_); } diff --git a/gtsam_unstable/dynamics/IMUFactor.h b/gtsam_unstable/dynamics/IMUFactor.h index e082dee82..9a742b4f0 100644 --- a/gtsam_unstable/dynamics/IMUFactor.h +++ b/gtsam_unstable/dynamics/IMUFactor.h @@ -81,9 +81,9 @@ public: boost::optional H2 = boost::none) const override { const Vector6 meas = z(); if (H1) *H1 = numericalDerivative21( - boost::bind(This::predict_proxy, boost::placeholders::_1, boost::placeholders::_2, dt_, meas), x1, x2, 1e-5); + std::bind(This::predict_proxy, std::placeholders::_1, std::placeholders::_2, dt_, meas), x1, x2, 1e-5); if (H2) *H2 = numericalDerivative22( - boost::bind(This::predict_proxy, boost::placeholders::_1, boost::placeholders::_2, dt_, meas), x1, x2, 1e-5); + std::bind(This::predict_proxy, std::placeholders::_1, std::placeholders::_2, dt_, meas), x1, x2, 1e-5); return predict_proxy(x1, x2, dt_, meas); } diff --git a/gtsam_unstable/dynamics/SimpleHelicopter.h b/gtsam_unstable/dynamics/SimpleHelicopter.h index 976f448c5..bf3b95c0f 100644 --- a/gtsam_unstable/dynamics/SimpleHelicopter.h +++ b/gtsam_unstable/dynamics/SimpleHelicopter.h @@ -166,24 +166,24 @@ public: boost::optional H3 = boost::none) const { if (H1) { (*H1) = numericalDerivative31( - boost::function( - boost::bind(&DiscreteEulerPoincareHelicopter::computeError, *this, _1, _2, _3) + std::function( + std::bind(&DiscreteEulerPoincareHelicopter::computeError, *this, _1, _2, _3) ), xik, xik_1, gk, 1e-5 ); } if (H2) { (*H2) = numericalDerivative32( - boost::function( - boost::bind(&DiscreteEulerPoincareHelicopter::computeError, *this, _1, _2, _3) + std::function( + std::bind(&DiscreteEulerPoincareHelicopter::computeError, *this, _1, _2, _3) ), xik, xik_1, gk, 1e-5 ); } if (H3) { (*H3) = numericalDerivative33( - boost::function( - boost::bind(&DiscreteEulerPoincareHelicopter::computeError, *this, _1, _2, _3) + std::function( + std::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 840b7bba7..d24d06e79 100644 --- a/gtsam_unstable/dynamics/VelocityConstraint.h +++ b/gtsam_unstable/dynamics/VelocityConstraint.h @@ -86,11 +86,11 @@ public: boost::optional H1=boost::none, boost::optional H2=boost::none) const override { if (H1) *H1 = gtsam::numericalDerivative21( - boost::bind(VelocityConstraint::evaluateError_, boost::placeholders::_1, - boost::placeholders::_2, dt_, integration_mode_), x1, x2, 1e-5); + std::bind(VelocityConstraint::evaluateError_, std::placeholders::_1, + std::placeholders::_2, dt_, integration_mode_), x1, x2, 1e-5); if (H2) *H2 = gtsam::numericalDerivative22( - boost::bind(VelocityConstraint::evaluateError_, boost::placeholders::_1, - boost::placeholders::_2, dt_, integration_mode_), x1, x2, 1e-5); + std::bind(VelocityConstraint::evaluateError_, std::placeholders::_1, + std::placeholders::_2, dt_, integration_mode_), x1, x2, 1e-5); return evaluateError_(x1, x2, dt_, integration_mode_); } diff --git a/gtsam_unstable/linear/QPSParser.cpp b/gtsam_unstable/linear/QPSParser.cpp index 88697e075..c755f2451 100644 --- a/gtsam_unstable/linear/QPSParser.cpp +++ b/gtsam_unstable/linear/QPSParser.cpp @@ -24,7 +24,6 @@ #include #include -#include #include #include #include @@ -40,7 +39,7 @@ #include using boost::fusion::at_c; -using namespace boost::placeholders; +using namespace std::placeholders; using namespace std; namespace bf = boost::fusion; @@ -412,50 +411,50 @@ typedef qi::grammar> base_grammar; struct QPSParser::MPSGrammar : base_grammar { typedef std::vector Chars; QPSVisitor *rqp_; - boost::function const &)> setName; - boost::function const &)> + std::function const &)> setName; + std::function const &)> addRow; - boost::function const &)> rhsSingle; - boost::function)> rhsDouble; - boost::function const &)> rangeSingle; - boost::function)> rangeDouble; - boost::function)> colSingle; - boost::function const &)> colDouble; - boost::function const &)> addQuadTerm; - boost::function const &)> addBound; - boost::function const &)> addFreeBound; MPSGrammar(QPSVisitor *rqp) : base_grammar(start), rqp_(rqp), - setName(boost::bind(&QPSVisitor::setName, rqp, ::_1)), - addRow(boost::bind(&QPSVisitor::addRow, rqp, ::_1)), - rhsSingle(boost::bind(&QPSVisitor::addRHS, rqp, ::_1)), - rhsDouble(boost::bind(&QPSVisitor::addRHSDouble, rqp, ::_1)), - rangeSingle(boost::bind(&QPSVisitor::addRangeSingle, rqp, ::_1)), - rangeDouble(boost::bind(&QPSVisitor::addRangeDouble, rqp, ::_1)), - colSingle(boost::bind(&QPSVisitor::addColumn, rqp, ::_1)), - colDouble(boost::bind(&QPSVisitor::addColumnDouble, rqp, ::_1)), - addQuadTerm(boost::bind(&QPSVisitor::addQuadTerm, rqp, ::_1)), - addBound(boost::bind(&QPSVisitor::addBound, rqp, ::_1)), - addFreeBound(boost::bind(&QPSVisitor::addFreeBound, rqp, ::_1)) { + setName(std::bind(&QPSVisitor::setName, rqp, std::placeholders::_1)), + addRow(std::bind(&QPSVisitor::addRow, rqp, std::placeholders::_1)), + rhsSingle(std::bind(&QPSVisitor::addRHS, rqp, std::placeholders::_1)), + rhsDouble(std::bind(&QPSVisitor::addRHSDouble, rqp, std::placeholders::_1)), + rangeSingle(std::bind(&QPSVisitor::addRangeSingle, rqp, std::placeholders::_1)), + rangeDouble(std::bind(&QPSVisitor::addRangeDouble, rqp, std::placeholders::_1)), + colSingle(std::bind(&QPSVisitor::addColumn, rqp, std::placeholders::_1)), + colDouble(std::bind(&QPSVisitor::addColumnDouble, rqp, std::placeholders::_1)), + addQuadTerm(std::bind(&QPSVisitor::addQuadTerm, rqp, std::placeholders::_1)), + addBound(std::bind(&QPSVisitor::addBound, rqp, std::placeholders::_1)), + addFreeBound(std::bind(&QPSVisitor::addFreeBound, rqp, std::placeholders::_1)) { using namespace boost::spirit; using namespace boost::spirit::qi; character = lexeme[alnum | '_' | '-' | '.']; diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h index a6638c1d7..cabbfdbe8 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h @@ -309,12 +309,12 @@ public: // Jacobian w.r.t. Pose1 if (H1){ Matrix H1_Pose = numericalDerivative11( - boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, - this, boost::placeholders::_1, Vel1, Bias1, Pose2, Vel2), + std::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, + this, std::placeholders::_1, Vel1, Bias1, Pose2, Vel2), Pose1); Matrix H1_Vel = numericalDerivative11( - boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, - this, boost::placeholders::_1, Vel1, Bias1, Pose2, Vel2), + std::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, + this, std::placeholders::_1, Vel1, Bias1, Pose2, Vel2), Pose1); *H1 = stack(2, &H1_Pose, &H1_Vel); } @@ -323,12 +323,12 @@ public: if (H2){ 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, boost::placeholders::_1, Bias1, Pose2, Vel2), + std::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, + this, Pose1, std::placeholders::_1, Bias1, Pose2, Vel2), Vel1); Matrix H2_Vel = numericalDerivative11( - boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, - this, Pose1, boost::placeholders::_1, Bias1, Pose2, Vel2), + std::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, + this, Pose1, std::placeholders::_1, Bias1, Pose2, Vel2), Vel1); *H2 = stack(2, &H2_Pose, &H2_Vel); } @@ -336,12 +336,12 @@ public: // Jacobian w.r.t. IMUBias1 if (H3){ Matrix H3_Pose = numericalDerivative11( - boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, - this, Pose1, Vel1, boost::placeholders::_1, Pose2, Vel2), + std::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, + this, Pose1, Vel1, std::placeholders::_1, Pose2, Vel2), Bias1); Matrix H3_Vel = numericalDerivative11( - boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, - this, Pose1, Vel1, boost::placeholders::_1, Pose2, Vel2), + std::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, + this, Pose1, Vel1, std::placeholders::_1, Pose2, Vel2), Bias1); *H3 = stack(2, &H3_Pose, &H3_Vel); } @@ -349,12 +349,12 @@ public: // Jacobian w.r.t. Pose2 if (H4){ Matrix H4_Pose = numericalDerivative11( - boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, - this, Pose1, Vel1, Bias1, boost::placeholders::_1, Vel2), + std::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, + this, Pose1, Vel1, Bias1, std::placeholders::_1, Vel2), Pose2); Matrix H4_Vel = numericalDerivative11( - boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, - this, Pose1, Vel1, Bias1, boost::placeholders::_1, Vel2), + std::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, + this, Pose1, Vel1, Bias1, std::placeholders::_1, Vel2), Pose2); *H4 = stack(2, &H4_Pose, &H4_Vel); } @@ -363,12 +363,12 @@ public: if (H5){ 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, boost::placeholders::_1), + std::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, + this, Pose1, Vel1, Bias1, Pose2, std::placeholders::_1), Vel2); Matrix H5_Vel = numericalDerivative11( - boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, - this, Pose1, Vel1, Bias1, Pose2, boost::placeholders::_1), + std::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, + this, Pose1, Vel1, Bias1, Pose2, std::placeholders::_1), Vel2); *H5 = stack(2, &H5_Pose, &H5_Vel); } @@ -469,43 +469,43 @@ public: Matrix I_3x3 = I_3x3; Matrix H_pos_pos = numericalDerivative11( - boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, - boost::placeholders::_1, delta_vel_in_t0), + std::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, + std::placeholders::_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, boost::placeholders::_1), + std::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, + delta_pos_in_t0, std::placeholders::_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, boost::placeholders::_1, + std::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, + msr_acc_t, msr_dt, delta_angles, std::placeholders::_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, boost::placeholders::_1, delta_vel_in_t0, + std::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, + msr_acc_t, msr_dt, std::placeholders::_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, + std::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, - boost::placeholders::_1), + std::placeholders::_1), Bias_t0); Matrix H_vel_pos = Z_3x3; Matrix H_angles_angles = numericalDerivative11( - boost::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, - msr_dt, boost::placeholders::_1, flag_use_body_P_sensor, + std::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, + msr_dt, std::placeholders::_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, + std::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, msr_dt, delta_angles, flag_use_body_P_sensor, body_P_sensor, - boost::placeholders::_1), + std::placeholders::_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 40dc81c9a..0e2aebd7f 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h @@ -278,29 +278,29 @@ public: // TODO: Write analytical derivative calculations // Jacobian w.r.t. Pose1 if (H1){ - Matrix H1_Pose = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluatePoseError, this, _1, Vel1, Pose2, Vel2), Pose1); - Matrix H1_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluateVelocityError, this, _1, Vel1, Pose2, Vel2), Pose1); + Matrix H1_Pose = numericalDerivative11(std::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluatePoseError, this, _1, Vel1, Pose2, Vel2), Pose1); + Matrix H1_Vel = numericalDerivative11(std::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluateVelocityError, this, _1, Vel1, Pose2, Vel2), Pose1); *H1 = stack(2, &H1_Pose, &H1_Vel); } // Jacobian w.r.t. Vel1 if (H2){ - Matrix H2_Pose = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluatePoseError, this, Pose1, _1, Pose2, Vel2), Vel1); - Matrix H2_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluateVelocityError, this, Pose1, _1, Pose2, Vel2), Vel1); + Matrix H2_Pose = numericalDerivative11(std::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluatePoseError, this, Pose1, _1, Pose2, Vel2), Vel1); + Matrix H2_Vel = numericalDerivative11(std::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluateVelocityError, this, Pose1, _1, Pose2, Vel2), Vel1); *H2 = stack(2, &H2_Pose, &H2_Vel); } // Jacobian w.r.t. Pose2 if (H3){ - Matrix H3_Pose = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluatePoseError, this, Pose1, Vel1, _1, Vel2), Pose2); - Matrix H3_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluateVelocityError, this, Pose1, Vel1, _1, Vel2), Pose2); + Matrix H3_Pose = numericalDerivative11(std::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluatePoseError, this, Pose1, Vel1, _1, Vel2), Pose2); + Matrix H3_Vel = numericalDerivative11(std::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluateVelocityError, this, Pose1, Vel1, _1, Vel2), Pose2); *H3 = stack(2, &H3_Pose, &H3_Vel); } // Jacobian w.r.t. Vel2 if (H4){ - Matrix H4_Pose = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluatePoseError, this, Pose1, Vel1, Pose2, _1), Vel2); - Matrix H4_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluateVelocityError, this, Pose1, Vel1, Pose2, _1), Vel2); + Matrix H4_Pose = numericalDerivative11(std::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluatePoseError, this, Pose1, Vel1, Pose2, _1), Vel2); + Matrix H4_Vel = numericalDerivative11(std::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluateVelocityError, this, Pose1, Vel1, Pose2, _1), Vel2); *H4 = stack(2, &H4_Pose, &H4_Vel); } @@ -372,15 +372,15 @@ public: Matrix Z_3x3 = Z_3x3; Matrix I_3x3 = I_3x3; - 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(std::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, _1, delta_vel_in_t0), delta_pos_in_t0); + Matrix H_pos_vel = numericalDerivative11(std::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, delta_pos_in_t0, _1), delta_vel_in_t0); Matrix H_pos_angles = 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), 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), delta_angles); + Matrix H_vel_vel = numericalDerivative11(std::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, delta_angles, _1, flag_use_body_P_sensor, body_P_sensor), delta_vel_in_t0); + Matrix H_vel_angles = numericalDerivative11(std::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), delta_angles); 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), delta_angles); + Matrix H_angles_angles = numericalDerivative11(std::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, msr_dt, _1, flag_use_body_P_sensor, body_P_sensor), delta_angles); Matrix H_angles_pos = Z_3x3; Matrix H_angles_vel = Z_3x3; diff --git a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h index 3f526e934..0828fbd08 100644 --- a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h +++ b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h @@ -236,12 +236,12 @@ public: // Jacobian w.r.t. Pose1 if (H1){ Matrix H1_Pose = gtsam::numericalDerivative11( - boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, - this, boost::placeholders::_1, Vel1, Bias1, Pose2, Vel2), + std::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, + this, std::placeholders::_1, Vel1, Bias1, Pose2, Vel2), Pose1); Matrix H1_Vel = gtsam::numericalDerivative11( - boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, - this, boost::placeholders::_1, Vel1, Bias1, Pose2, Vel2), + std::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, + this, std::placeholders::_1, Vel1, Bias1, Pose2, Vel2), Pose1); *H1 = stack(2, &H1_Pose, &H1_Vel); } @@ -250,12 +250,12 @@ public: if (H2){ 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, boost::placeholders::_1, Bias1, Pose2, Vel2), + std::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, + this, Pose1, std::placeholders::_1, Bias1, Pose2, Vel2), Vel1); Matrix H2_Vel = gtsam::numericalDerivative11( - boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, - this, Pose1, boost::placeholders::_1, Bias1, Pose2, Vel2), + std::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, + this, Pose1, std::placeholders::_1, Bias1, Pose2, Vel2), Vel1); *H2 = stack(2, &H2_Pose, &H2_Vel); } @@ -263,12 +263,12 @@ public: // Jacobian w.r.t. IMUBias1 if (H3){ Matrix H3_Pose = gtsam::numericalDerivative11( - boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, - this, Pose1, Vel1, boost::placeholders::_1, Pose2, Vel2), + std::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, + this, Pose1, Vel1, std::placeholders::_1, Pose2, Vel2), Bias1); Matrix H3_Vel = gtsam::numericalDerivative11( - boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, - this, Pose1, Vel1, boost::placeholders::_1, Pose2, Vel2), + std::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, + this, Pose1, Vel1, std::placeholders::_1, Pose2, Vel2), Bias1); *H3 = stack(2, &H3_Pose, &H3_Vel); } @@ -276,12 +276,12 @@ public: // Jacobian w.r.t. Pose2 if (H4){ Matrix H4_Pose = gtsam::numericalDerivative11( - boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, - this, Pose1, Vel1, Bias1, boost::placeholders::_1, Vel2), + std::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, + this, Pose1, Vel1, Bias1, std::placeholders::_1, Vel2), Pose2); Matrix H4_Vel = gtsam::numericalDerivative11( - boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, - this, Pose1, Vel1, Bias1, boost::placeholders::_1, Vel2), + std::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, + this, Pose1, Vel1, Bias1, std::placeholders::_1, Vel2), Pose2); *H4 = stack(2, &H4_Pose, &H4_Vel); } @@ -290,12 +290,12 @@ public: if (H5){ 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, boost::placeholders::_1), + std::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, + this, Pose1, Vel1, Bias1, Pose2, std::placeholders::_1), Vel2); Matrix H5_Vel = gtsam::numericalDerivative11( - boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, - this, Pose1, Vel1, Bias1, Pose2, boost::placeholders::_1), + std::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, + this, Pose1, Vel1, Bias1, Pose2, std::placeholders::_1), Vel2); *H5 = stack(2, &H5_Pose, &H5_Vel); } diff --git a/gtsam_unstable/slam/InvDepthFactorVariant1.h b/gtsam_unstable/slam/InvDepthFactorVariant1.h index a9dcb8cef..40c09416c 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant1.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant1.h @@ -108,14 +108,14 @@ public: if (H1) { (*H1) = numericalDerivative11( - boost::bind(&InvDepthFactorVariant1::inverseDepthError, this, - boost::placeholders::_1, landmark), + std::bind(&InvDepthFactorVariant1::inverseDepthError, this, + std::placeholders::_1, landmark), pose); } if (H2) { (*H2) = numericalDerivative11( - boost::bind(&InvDepthFactorVariant1::inverseDepthError, this, pose, - boost::placeholders::_1), landmark); + std::bind(&InvDepthFactorVariant1::inverseDepthError, this, pose, + std::placeholders::_1), landmark); } return inverseDepthError(pose, landmark); diff --git a/gtsam_unstable/slam/InvDepthFactorVariant2.h b/gtsam_unstable/slam/InvDepthFactorVariant2.h index d1fc92b90..b1169580e 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant2.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant2.h @@ -111,13 +111,13 @@ public: if (H1) { (*H1) = numericalDerivative11( - boost::bind(&InvDepthFactorVariant2::inverseDepthError, this, - boost::placeholders::_1, landmark), pose); + std::bind(&InvDepthFactorVariant2::inverseDepthError, this, + std::placeholders::_1, landmark), pose); } if (H2) { (*H2) = numericalDerivative11( - boost::bind(&InvDepthFactorVariant2::inverseDepthError, this, pose, - boost::placeholders::_1), landmark); + std::bind(&InvDepthFactorVariant2::inverseDepthError, this, pose, + std::placeholders::_1), landmark); } return inverseDepthError(pose, landmark); diff --git a/gtsam_unstable/slam/InvDepthFactorVariant3.h b/gtsam_unstable/slam/InvDepthFactorVariant3.h index 4595a934b..98f2db2f3 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant3.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant3.h @@ -111,14 +111,14 @@ public: if(H1) { (*H1) = numericalDerivative11( - boost::bind(&InvDepthFactorVariant3a::inverseDepthError, this, - boost::placeholders::_1, landmark), + std::bind(&InvDepthFactorVariant3a::inverseDepthError, this, + std::placeholders::_1, landmark), pose); } if(H2) { (*H2) = numericalDerivative11( - boost::bind(&InvDepthFactorVariant3a::inverseDepthError, this, pose, - boost::placeholders::_1), + std::bind(&InvDepthFactorVariant3a::inverseDepthError, this, pose, + std::placeholders::_1), landmark); } @@ -238,20 +238,20 @@ public: if(H1) (*H1) = numericalDerivative11( - boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, - boost::placeholders::_1, pose2, landmark), + std::bind(&InvDepthFactorVariant3b::inverseDepthError, this, + std::placeholders::_1, pose2, landmark), pose1); if(H2) (*H2) = numericalDerivative11( - boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1, - boost::placeholders::_1, landmark), + std::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1, + std::placeholders::_1, landmark), pose2); if(H3) (*H3) = numericalDerivative11( - boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1, - pose2, boost::placeholders::_1), + std::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1, + pose2, std::placeholders::_1), landmark); return inverseDepthError(pose1, pose2, landmark); From d5890a2d61a7142e745c0227e1abbe67dcc83aab Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 10 Jul 2021 21:03:15 -0400 Subject: [PATCH 038/248] update all the tests --- .../tests/testAlgebraicDecisionTree.cpp | 2 +- gtsam/discrete/tests/testDecisionTree.cpp | 2 +- gtsam/geometry/tests/testCalibratedCamera.cpp | 7 +- gtsam/geometry/tests/testEssentialMatrix.cpp | 27 +++--- gtsam/geometry/tests/testOrientedPlane3.cpp | 12 +-- gtsam/geometry/tests/testPinholeCamera.cpp | 12 +-- gtsam/geometry/tests/testPinholePose.cpp | 4 +- gtsam/geometry/tests/testPoint3.cpp | 22 ++--- gtsam/geometry/tests/testPose3.cpp | 11 +-- gtsam/geometry/tests/testSO3.cpp | 27 +++--- gtsam/geometry/tests/testSO4.cpp | 6 +- gtsam/geometry/tests/testSOn.cpp | 4 +- gtsam/geometry/tests/testSimilarity3.cpp | 31 ++++--- gtsam/geometry/tests/testUnit3.cpp | 42 +++++---- gtsam/linear/tests/testGaussianBayesNet.cpp | 6 +- gtsam/linear/tests/testGaussianBayesTree.cpp | 7 +- gtsam/navigation/tests/testAHRSFactor.cpp | 47 +++++----- gtsam/navigation/tests/testAttitudeFactor.cpp | 9 +- gtsam/navigation/tests/testGPSFactor.cpp | 6 +- gtsam/navigation/tests/testImuBias.cpp | 13 +-- gtsam/navigation/tests/testImuFactor.cpp | 34 ++++---- gtsam/navigation/tests/testMagFactor.cpp | 18 ++-- gtsam/navigation/tests/testMagPoseFactor.cpp | 22 +++-- .../tests/testManifoldPreintegration.cpp | 19 ++-- gtsam/navigation/tests/testNavState.cpp | 48 +++++----- gtsam/navigation/tests/testScenario.cpp | 5 +- .../tests/testTangentPreintegration.cpp | 17 ++-- gtsam/nonlinear/tests/testExpression.cpp | 2 +- gtsam/nonlinear/tests/testValues.cpp | 6 +- gtsam/sam/tests/testRangeFactor.cpp | 18 ++-- gtsam/sfm/tests/testTranslationFactor.cpp | 7 +- gtsam/slam/tests/testBetweenFactor.cpp | 15 ++-- .../tests/testEssentialMatrixConstraint.cpp | 17 ++-- .../slam/tests/testEssentialMatrixFactor.cpp | 14 ++- gtsam/slam/tests/testOrientedPlane3Factor.cpp | 14 +-- gtsam/slam/tests/testReferenceFrameFactor.cpp | 16 ++-- gtsam/slam/tests/testRotateFactor.cpp | 11 ++- .../tests/testSmartProjectionPoseFactor.cpp | 7 +- gtsam/slam/tests/testTriangulationFactor.cpp | 10 ++- .../dynamics/tests/testSimpleHelicopter.cpp | 42 +++++---- gtsam_unstable/geometry/tests/testEvent.cpp | 8 +- .../slam/tests/testBetweenFactorEM.cpp | 6 +- .../slam/tests/testBiasedGPSFactor.cpp | 17 ++-- .../testEquivInertialNavFactor_GlobalVel.cpp | 2 +- .../tests/testGaussMarkov1stOrderFactor.cpp | 12 +-- .../testInertialNavFactor_GlobalVelocity.cpp | 45 +++++----- .../tests/testLocalOrientedPlane3Factor.cpp | 10 +-- .../slam/tests/testPartialPriorFactor.cpp | 26 +++--- .../slam/tests/testPoseBetweenFactor.cpp | 23 +++-- .../slam/tests/testPosePriorFactor.cpp | 13 ++- .../slam/tests/testPoseToPointFactor.h | 2 +- .../slam/tests/testProjectionFactorPPP.cpp | 24 ++--- .../slam/tests/testProjectionFactorPPPC.cpp | 22 ++--- .../tests/testRelativeElevationFactor.cpp | 87 ++++++++++++------- gtsam_unstable/slam/tests/testTSAMFactors.cpp | 27 +++--- .../testTransformBtwRobotsUnaryFactor.cpp | 12 ++- .../testTransformBtwRobotsUnaryFactorEM.cpp | 16 ++-- tests/testExpressionFactor.cpp | 24 +++-- tests/testSimulated3D.cpp | 24 +++-- 59 files changed, 557 insertions(+), 482 deletions(-) diff --git a/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp b/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp index 0261ef833..be720dbca 100644 --- a/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp +++ b/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp @@ -54,7 +54,7 @@ void dot(const T&f, const string& filename) { } /** I can't get this to work ! - class Mul: boost::function { + class Mul: std::function { inline double operator()(const double& a, const double& b) { return a * b; } diff --git a/gtsam/discrete/tests/testDecisionTree.cpp b/gtsam/discrete/tests/testDecisionTree.cpp index 71cb4abe3..96f503abc 100644 --- a/gtsam/discrete/tests/testDecisionTree.cpp +++ b/gtsam/discrete/tests/testDecisionTree.cpp @@ -196,7 +196,7 @@ TEST(DT, conversion) map ordering; ordering[A] = X; ordering[B] = Y; - boost::function op = convert; + std::function op = convert; BDT f2(f1, ordering, op); // f1.print("f1"); // f2.print("f2"); diff --git a/gtsam/geometry/tests/testCalibratedCamera.cpp b/gtsam/geometry/tests/testCalibratedCamera.cpp index 1db1926bc..0fb0754fe 100644 --- a/gtsam/geometry/tests/testCalibratedCamera.cpp +++ b/gtsam/geometry/tests/testCalibratedCamera.cpp @@ -20,12 +20,11 @@ #include #include -#include #include #include -using namespace boost::placeholders; +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -54,8 +53,8 @@ TEST(CalibratedCamera, Create) { EXPECT(assert_equal(camera, CalibratedCamera::Create(kDefaultPose, actualH))); // Check derivative - boost::function f = // - boost::bind(CalibratedCamera::Create, _1, boost::none); + std::function f = // + std::bind(CalibratedCamera::Create, std::placeholders::_1, boost::none); Matrix numericalH = numericalDerivative11(f, kDefaultPose); EXPECT(assert_equal(numericalH, actualH, 1e-9)); } diff --git a/gtsam/geometry/tests/testEssentialMatrix.cpp b/gtsam/geometry/tests/testEssentialMatrix.cpp index 7362cf7bf..ff8c61f35 100644 --- a/gtsam/geometry/tests/testEssentialMatrix.cpp +++ b/gtsam/geometry/tests/testEssentialMatrix.cpp @@ -5,16 +5,15 @@ * @date December 17, 2013 */ -#include -#include -#include -#include - -#include #include +#include +#include +#include +#include + #include -using namespace boost::placeholders; +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -42,15 +41,15 @@ TEST(EssentialMatrix, FromRotationAndDirection) { 1e-8)); Matrix expectedH1 = numericalDerivative11( - boost::bind(EssentialMatrix::FromRotationAndDirection, _1, trueDirection, boost::none, - boost::none), + std::bind(EssentialMatrix::FromRotationAndDirection, + std::placeholders::_1, trueDirection, boost::none, boost::none), trueRotation); EXPECT(assert_equal(expectedH1, actualH1, 1e-7)); Matrix expectedH2 = numericalDerivative11( - boost::bind(EssentialMatrix::FromRotationAndDirection, trueRotation, _1, boost::none, - boost::none), - trueDirection); + std::bind(EssentialMatrix::FromRotationAndDirection, trueRotation, + std::placeholders::_1, boost::none, boost::none), + trueDirection); EXPECT(assert_equal(expectedH2, actualH2, 1e-7)); } @@ -176,7 +175,7 @@ TEST (EssentialMatrix, FromPose3_a) { Pose3 pose(trueRotation, trueTranslation); // Pose between two cameras EXPECT(assert_equal(trueE, EssentialMatrix::FromPose3(pose, actualH), 1e-8)); Matrix expectedH = numericalDerivative11( - boost::bind(EssentialMatrix::FromPose3, _1, boost::none), pose); + std::bind(EssentialMatrix::FromPose3, std::placeholders::_1, boost::none), pose); EXPECT(assert_equal(expectedH, actualH, 1e-7)); } @@ -189,7 +188,7 @@ TEST (EssentialMatrix, FromPose3_b) { Pose3 pose(c1Rc2, c1Tc2); // Pose between two cameras EXPECT(assert_equal(E, EssentialMatrix::FromPose3(pose, actualH), 1e-8)); Matrix expectedH = numericalDerivative11( - boost::bind(EssentialMatrix::FromPose3, _1, boost::none), pose); + std::bind(EssentialMatrix::FromPose3, std::placeholders::_1, boost::none), pose); EXPECT(assert_equal(expectedH, actualH, 1e-5)); } diff --git a/gtsam/geometry/tests/testOrientedPlane3.cpp b/gtsam/geometry/tests/testOrientedPlane3.cpp index 5c7c6142e..533041a2c 100644 --- a/gtsam/geometry/tests/testOrientedPlane3.cpp +++ b/gtsam/geometry/tests/testOrientedPlane3.cpp @@ -21,10 +21,9 @@ #include #include #include -#include using namespace boost::assign; -using namespace boost::placeholders; +using namespace std::placeholders; using namespace gtsam; using namespace std; using boost::none; @@ -138,8 +137,9 @@ TEST(OrientedPlane3, errorVector) { Vector2(actual[0], actual[1]))); EXPECT(assert_equal(plane1.distance() - plane2.distance(), actual[2])); - boost::function f = - boost::bind(&OrientedPlane3::errorVector, _1, _2, boost::none, boost::none); + std::function f = + std::bind(&OrientedPlane3::errorVector, std::placeholders::_1, + std::placeholders::_2, boost::none, boost::none); expectedH1 = numericalDerivative21(f, plane1, plane2); expectedH2 = numericalDerivative22(f, plane1, plane2); EXPECT(assert_equal(expectedH1, actualH1, 1e-5)); @@ -150,8 +150,8 @@ TEST(OrientedPlane3, errorVector) { TEST(OrientedPlane3, jacobian_retract) { OrientedPlane3 plane(-1, 0.1, 0.2, 5); Matrix33 H_actual; - boost::function f = - boost::bind(&OrientedPlane3::retract, plane, _1, boost::none); + std::function f = std::bind( + &OrientedPlane3::retract, plane, std::placeholders::_1, boost::none); { Vector3 v(-0.1, 0.2, 0.3); plane.retract(v, H_actual); diff --git a/gtsam/geometry/tests/testPinholeCamera.cpp b/gtsam/geometry/tests/testPinholeCamera.cpp index 92deda6a5..0679a4609 100644 --- a/gtsam/geometry/tests/testPinholeCamera.cpp +++ b/gtsam/geometry/tests/testPinholeCamera.cpp @@ -22,13 +22,12 @@ #include #include -#include #include #include #include -using namespace boost::placeholders; +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -66,8 +65,9 @@ TEST(PinholeCamera, Create) { EXPECT(assert_equal(camera, Camera::Create(pose,K, actualH1, actualH2))); // Check derivative - boost::function f = // - boost::bind(Camera::Create,_1,_2,boost::none,boost::none); + std::function f = // + std::bind(Camera::Create, std::placeholders::_1, std::placeholders::_2, + boost::none, boost::none); Matrix numericalH1 = numericalDerivative21(f,pose,K); EXPECT(assert_equal(numericalH1, actualH1, 1e-9)); Matrix numericalH2 = numericalDerivative22(f,pose,K); @@ -81,8 +81,8 @@ TEST(PinholeCamera, Pose) { EXPECT(assert_equal(pose, camera.getPose(actualH))); // Check derivative - boost::function f = // - boost::bind(&Camera::getPose,_1,boost::none); + std::function f = // + std::bind(&Camera::getPose, std::placeholders::_1, boost::none); Matrix numericalH = numericalDerivative11(f,camera); EXPECT(assert_equal(numericalH, actualH, 1e-9)); } diff --git a/gtsam/geometry/tests/testPinholePose.cpp b/gtsam/geometry/tests/testPinholePose.cpp index 1cf2f4a3f..acfcd9f39 100644 --- a/gtsam/geometry/tests/testPinholePose.cpp +++ b/gtsam/geometry/tests/testPinholePose.cpp @@ -65,8 +65,8 @@ TEST(PinholeCamera, Pose) { EXPECT(assert_equal(pose, camera.getPose(actualH))); // Check derivative - boost::function f = // - boost::bind(&Camera::getPose,_1,boost::none); + std::function f = // + std::bind(&Camera::getPose,_1,boost::none); Matrix numericalH = numericalDerivative11(f,camera); EXPECT(assert_equal(numericalH, actualH, 1e-9)); } diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp index 79e44c0b3..315391ac8 100644 --- a/gtsam/geometry/tests/testPoint3.cpp +++ b/gtsam/geometry/tests/testPoint3.cpp @@ -14,14 +14,14 @@ * @brief Unit tests for Point3 class */ -#include +#include #include #include +#include -#include -#include +#include -using namespace boost::placeholders; +using namespace std::placeholders; using namespace gtsam; GTSAM_CONCEPT_TESTABLE_INST(Point3) @@ -101,7 +101,7 @@ TEST( Point3, dot) { // Use numerical derivatives to calculate the expected Jacobians Matrix H1, H2; - boost::function f = + std::function f = [](const Point3& p, const Point3& q) { return gtsam::dot(p, q); }; { gtsam::dot(p, q, H1, H2); @@ -123,8 +123,9 @@ TEST( Point3, dot) { /* ************************************************************************* */ TEST(Point3, cross) { Matrix aH1, aH2; - boost::function f = - boost::bind(>sam::cross, _1, _2, boost::none, boost::none); + std::function f = + std::bind(>sam::cross, std::placeholders::_1, std::placeholders::_2, + boost::none, boost::none); const Point3 omega(0, 1, 0), theta(4, 6, 8); cross(omega, theta, aH1, aH2); EXPECT(assert_equal(numericalDerivative21(f, omega, theta), aH1)); @@ -142,8 +143,9 @@ TEST( Point3, cross2) { // Use numerical derivatives to calculate the expected Jacobians Matrix H1, H2; - boost::function f = boost::bind(>sam::cross, _1, _2, // - boost::none, boost::none); + std::function f = + std::bind(>sam::cross, std::placeholders::_1, std::placeholders::_2, // + boost::none, boost::none); { gtsam::cross(p, q, H1, H2); EXPECT(assert_equal(numericalDerivative21(f, p, q), H1, 1e-9)); @@ -163,7 +165,7 @@ TEST (Point3, normalize) { Point3 expected(point / sqrt(14.0)); EXPECT(assert_equal(expected, normalize(point, actualH), 1e-8)); Matrix expectedH = numericalDerivative11( - boost::bind(gtsam::normalize, _1, boost::none), point); + std::bind(gtsam::normalize, std::placeholders::_1, boost::none), point); EXPECT(assert_equal(expectedH, actualH, 1e-8)); } diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 9ed76d4a6..11b8791d4 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -22,8 +22,7 @@ #include // for operator += using namespace boost::assign; -#include -using namespace boost::placeholders; +using namespace std::placeholders; #include #include @@ -215,7 +214,7 @@ TEST(Pose3, translation) { EXPECT(assert_equal(Point3(3.5, -8.2, 4.2), T.translation(actualH), 1e-8)); Matrix numericalH = numericalDerivative11( - boost::bind(&Pose3::translation, _1, boost::none), T); + std::bind(&Pose3::translation, std::placeholders::_1, boost::none), T); EXPECT(assert_equal(numericalH, actualH, 1e-6)); } @@ -226,7 +225,7 @@ TEST(Pose3, rotation) { EXPECT(assert_equal(R, T.rotation(actualH), 1e-8)); Matrix numericalH = numericalDerivative11( - boost::bind(&Pose3::rotation, _1, boost::none), T); + std::bind(&Pose3::rotation, std::placeholders::_1, boost::none), T); EXPECT(assert_equal(numericalH, actualH, 1e-6)); } @@ -1052,7 +1051,9 @@ TEST(Pose3, Create) { Matrix63 actualH1, actualH2; Pose3 actual = Pose3::Create(R, P2, actualH1, actualH2); EXPECT(assert_equal(T, actual)); - boost::function create = boost::bind(Pose3::Create,_1,_2,boost::none,boost::none); + std::function create = + std::bind(Pose3::Create, std::placeholders::_1, std::placeholders::_2, + boost::none, boost::none); EXPECT(assert_equal(numericalDerivative21(create, R, P2), actualH1, 1e-9)); EXPECT(assert_equal(numericalDerivative22(create, R, P2), actualH2, 1e-9)); } diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp index 58ad967d2..910d482b0 100644 --- a/gtsam/geometry/tests/testSO3.cpp +++ b/gtsam/geometry/tests/testSO3.cpp @@ -15,15 +15,12 @@ * @author Frank Dellaert **/ -#include - +#include #include #include +#include -#include -#include - -using namespace boost::placeholders; +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -211,7 +208,7 @@ TEST(SO3, ExpmapDerivative) { TEST(SO3, ExpmapDerivative2) { const Vector3 theta(0.1, 0, 0.1); const Matrix Jexpected = numericalDerivative11( - boost::bind(&SO3::Expmap, _1, boost::none), theta); + std::bind(&SO3::Expmap, std::placeholders::_1, boost::none), theta); CHECK(assert_equal(Jexpected, SO3::ExpmapDerivative(theta))); CHECK(assert_equal(Matrix3(Jexpected.transpose()), @@ -222,7 +219,7 @@ TEST(SO3, ExpmapDerivative2) { TEST(SO3, ExpmapDerivative3) { const Vector3 theta(10, 20, 30); const Matrix Jexpected = numericalDerivative11( - boost::bind(&SO3::Expmap, _1, boost::none), theta); + std::bind(&SO3::Expmap, std::placeholders::_1, boost::none), theta); CHECK(assert_equal(Jexpected, SO3::ExpmapDerivative(theta))); CHECK(assert_equal(Matrix3(Jexpected.transpose()), @@ -277,7 +274,7 @@ TEST(SO3, ExpmapDerivative5) { TEST(SO3, ExpmapDerivative6) { const Vector3 thetahat(0.1, 0, 0.1); const Matrix Jexpected = numericalDerivative11( - boost::bind(&SO3::Expmap, _1, boost::none), thetahat); + std::bind(&SO3::Expmap, std::placeholders::_1, boost::none), thetahat); Matrix3 Jactual; SO3::Expmap(thetahat, Jactual); EXPECT(assert_equal(Jexpected, Jactual)); @@ -288,7 +285,7 @@ TEST(SO3, LogmapDerivative) { const Vector3 thetahat(0.1, 0, 0.1); const SO3 R = SO3::Expmap(thetahat); // some rotation const Matrix Jexpected = numericalDerivative11( - boost::bind(&SO3::Logmap, _1, boost::none), R); + std::bind(&SO3::Logmap, std::placeholders::_1, boost::none), R); const Matrix3 Jactual = SO3::LogmapDerivative(thetahat); EXPECT(assert_equal(Jexpected, Jactual)); } @@ -298,7 +295,7 @@ TEST(SO3, JacobianLogmap) { const Vector3 thetahat(0.1, 0, 0.1); const SO3 R = SO3::Expmap(thetahat); // some rotation const Matrix Jexpected = numericalDerivative11( - boost::bind(&SO3::Logmap, _1, boost::none), R); + std::bind(&SO3::Logmap, std::placeholders::_1, boost::none), R); Matrix3 Jactual; SO3::Logmap(R, Jactual); EXPECT(assert_equal(Jexpected, Jactual)); @@ -308,7 +305,7 @@ TEST(SO3, JacobianLogmap) { TEST(SO3, ApplyDexp) { Matrix aH1, aH2; for (bool nearZeroApprox : {true, false}) { - boost::function f = + std::function f = [=](const Vector3& omega, const Vector3& v) { return so3::DexpFunctor(omega, nearZeroApprox).applyDexp(v); }; @@ -331,7 +328,7 @@ TEST(SO3, ApplyDexp) { TEST(SO3, ApplyInvDexp) { Matrix aH1, aH2; for (bool nearZeroApprox : {true, false}) { - boost::function f = + std::function f = [=](const Vector3& omega, const Vector3& v) { return so3::DexpFunctor(omega, nearZeroApprox).applyInvDexp(v); }; @@ -357,7 +354,7 @@ TEST(SO3, vec) { Matrix actualH; const Vector9 actual = R2.vec(actualH); CHECK(assert_equal(expected, actual)); - boost::function f = [](const SO3& Q) { return Q.vec(); }; + std::function f = [](const SO3& Q) { return Q.vec(); }; const Matrix numericalH = numericalDerivative11(f, R2, 1e-5); CHECK(assert_equal(numericalH, actualH)); } @@ -371,7 +368,7 @@ TEST(Matrix, compose) { Matrix actualH; const Matrix3 actual = so3::compose(M, R, actualH); CHECK(assert_equal(expected, actual)); - boost::function f = [R](const Matrix3& M) { + std::function f = [R](const Matrix3& M) { return so3::compose(M, R); }; Matrix numericalH = numericalDerivative11(f, M, 1e-2); diff --git a/gtsam/geometry/tests/testSO4.cpp b/gtsam/geometry/tests/testSO4.cpp index f771eea5f..5486755f7 100644 --- a/gtsam/geometry/tests/testSO4.cpp +++ b/gtsam/geometry/tests/testSO4.cpp @@ -166,7 +166,7 @@ TEST(SO4, vec) { Matrix actualH; const Vector16 actual = Q2.vec(actualH); EXPECT(assert_equal(expected, actual)); - boost::function f = [](const SO4& Q) { + std::function f = [](const SO4& Q) { return Q.vec(); }; const Matrix numericalH = numericalDerivative11(f, Q2, 1e-5); @@ -179,7 +179,7 @@ TEST(SO4, topLeft) { Matrix actualH; const Matrix3 actual = topLeft(Q3, actualH); EXPECT(assert_equal(expected, actual)); - boost::function f = [](const SO4& Q3) { + std::function f = [](const SO4& Q3) { return topLeft(Q3); }; const Matrix numericalH = numericalDerivative11(f, Q3, 1e-5); @@ -192,7 +192,7 @@ TEST(SO4, stiefel) { Matrix actualH; const Matrix43 actual = stiefel(Q3, actualH); EXPECT(assert_equal(expected, actual)); - boost::function f = [](const SO4& Q3) { + std::function f = [](const SO4& Q3) { return stiefel(Q3); }; const Matrix numericalH = numericalDerivative11(f, Q3, 1e-5); diff --git a/gtsam/geometry/tests/testSOn.cpp b/gtsam/geometry/tests/testSOn.cpp index 4d0ed98b3..d9d4da34c 100644 --- a/gtsam/geometry/tests/testSOn.cpp +++ b/gtsam/geometry/tests/testSOn.cpp @@ -189,7 +189,7 @@ Matrix RetractJacobian(size_t n) { return SOn::VectorizedGenerators(n); } /// Test Jacobian of Retract at origin TEST(SOn, RetractJacobian) { Matrix actualH = RetractJacobian(3); - boost::function h = [](const Vector &v) { + std::function h = [](const Vector &v) { return SOn::ChartAtOrigin::Retract(v).matrix(); }; Vector3 v; @@ -205,7 +205,7 @@ TEST(SOn, vec) { SOn Q = SOn::ChartAtOrigin::Retract(v); Matrix actualH; const Vector actual = Q.vec(actualH); - boost::function h = [](const SOn &Q) { return Q.vec(); }; + std::function h = [](const SOn &Q) { return Q.vec(); }; const Matrix H = numericalDerivative11(h, Q, 1e-5); CHECK(assert_equal(H, actualH)); } diff --git a/gtsam/geometry/tests/testSimilarity3.cpp b/gtsam/geometry/tests/testSimilarity3.cpp index 10a9b2ac4..428422072 100644 --- a/gtsam/geometry/tests/testSimilarity3.cpp +++ b/gtsam/geometry/tests/testSimilarity3.cpp @@ -16,24 +16,22 @@ * @author Zhaoyang Lv */ -#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 - -using namespace boost::placeholders; +using namespace std::placeholders; using namespace gtsam; using namespace std; using symbol_shorthand::X; @@ -243,8 +241,9 @@ TEST(Similarity3, GroupAction) { EXPECT(assert_equal(Point3(2, 6, 6), Td.transformFrom(pa))); // Test derivative - boost::function f = boost::bind( - &Similarity3::transformFrom, _1, _2, boost::none, boost::none); + // Use lambda to resolve overloaded method + std::function + f = [](const Similarity3& S, const Point3& p){ return S.transformFrom(p); }; Point3 q(1, 2, 3); for (const auto& T : { T1, T2, T3, T4, T5, T6 }) { diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp index 4d609380c..b548b9315 100644 --- a/gtsam/geometry/tests/testUnit3.cpp +++ b/gtsam/geometry/tests/testUnit3.cpp @@ -32,13 +32,12 @@ #include #include -#include #include #include using namespace boost::assign; -using namespace boost::placeholders; +using namespace std::placeholders; using namespace gtsam; using namespace std; using gtsam::symbol_shorthand::U; @@ -127,8 +126,9 @@ TEST(Unit3, dot) { // Use numerical derivatives to calculate the expected Jacobians Matrix H1, H2; - boost::function f = boost::bind(&Unit3::dot, _1, _2, // - boost::none, boost::none); + std::function f = + std::bind(&Unit3::dot, std::placeholders::_1, std::placeholders::_2, // + boost::none, boost::none); { p.dot(q, H1, H2); EXPECT(assert_equal(numericalDerivative21(f, p, q), H1, 1e-5)); @@ -158,13 +158,13 @@ TEST(Unit3, error) { // Use numerical derivatives to calculate the expected Jacobian { expected = numericalDerivative11( - boost::bind(&Unit3::error, &p, _1, boost::none), q); + std::bind(&Unit3::error, &p, std::placeholders::_1, boost::none), q); p.error(q, actual); EXPECT(assert_equal(expected.transpose(), actual, 1e-5)); } { expected = numericalDerivative11( - boost::bind(&Unit3::error, &p, _1, boost::none), r); + std::bind(&Unit3::error, &p, std::placeholders::_1, boost::none), r); p.error(r, actual); EXPECT(assert_equal(expected.transpose(), actual, 1e-5)); } @@ -185,25 +185,33 @@ TEST(Unit3, error2) { // Use numerical derivatives to calculate the expected Jacobian { expected = numericalDerivative21( - boost::bind(&Unit3::errorVector, _1, _2, boost::none, boost::none), p, q); + std::bind(&Unit3::errorVector, std::placeholders::_1, + std::placeholders::_2, boost::none, boost::none), + p, q); p.errorVector(q, actual, boost::none); EXPECT(assert_equal(expected, actual, 1e-5)); } { expected = numericalDerivative21( - boost::bind(&Unit3::errorVector, _1, _2, boost::none, boost::none), p, r); + std::bind(&Unit3::errorVector, std::placeholders::_1, + std::placeholders::_2, boost::none, boost::none), + p, r); p.errorVector(r, actual, boost::none); EXPECT(assert_equal(expected, actual, 1e-5)); } { expected = numericalDerivative22( - boost::bind(&Unit3::errorVector, _1, _2, boost::none, boost::none), p, q); + std::bind(&Unit3::errorVector, std::placeholders::_1, + std::placeholders::_2, boost::none, boost::none), + p, q); p.errorVector(q, boost::none, actual); EXPECT(assert_equal(expected, actual, 1e-5)); } { expected = numericalDerivative22( - boost::bind(&Unit3::errorVector, _1, _2, boost::none, boost::none), p, r); + std::bind(&Unit3::errorVector, std::placeholders::_1, + std::placeholders::_2, boost::none, boost::none), + p, r); p.errorVector(r, boost::none, actual); EXPECT(assert_equal(expected, actual, 1e-5)); } @@ -221,13 +229,13 @@ TEST(Unit3, distance) { // Use numerical derivatives to calculate the expected Jacobian { expected = numericalGradient( - boost::bind(&Unit3::distance, &p, _1, boost::none), q); + std::bind(&Unit3::distance, &p, std::placeholders::_1, boost::none), q); p.distance(q, actual); EXPECT(assert_equal(expected.transpose(), actual, 1e-5)); } { expected = numericalGradient( - boost::bind(&Unit3::distance, &p, _1, boost::none), r); + std::bind(&Unit3::distance, &p, std::placeholders::_1, boost::none), r); p.distance(r, actual); EXPECT(assert_equal(expected.transpose(), actual, 1e-5)); } @@ -319,7 +327,7 @@ TEST(Unit3, basis) { Matrix62 actualH; Matrix62 expectedH = numericalDerivative11( - boost::bind(BasisTest, _1, boost::none), p); + std::bind(BasisTest, std::placeholders::_1, boost::none), p); // without H, first time EXPECT(assert_equal(expected, p.basis(), 1e-6)); @@ -348,7 +356,7 @@ TEST(Unit3, basis_derivatives) { p.basis(actualH); Matrix62 expectedH = numericalDerivative11( - boost::bind(BasisTest, _1, boost::none), p); + std::bind(BasisTest, std::placeholders::_1, boost::none), p); EXPECT(assert_equal(expectedH, actualH, 1e-5)); } } @@ -376,8 +384,8 @@ TEST(Unit3, retract) { TEST (Unit3, jacobian_retract) { Matrix22 H; Unit3 p; - boost::function f = - boost::bind(&Unit3::retract, p, _1, boost::none); + std::function f = + std::bind(&Unit3::retract, p, std::placeholders::_1, boost::none); { Vector2 v (-0.2, 0.1); p.retract(v, H); @@ -440,7 +448,7 @@ TEST (Unit3, FromPoint3) { Unit3 expected(point); EXPECT(assert_equal(expected, Unit3::FromPoint3(point, actualH), 1e-5)); Matrix expectedH = numericalDerivative11( - boost::bind(Unit3::FromPoint3, _1, boost::none), point); + std::bind(Unit3::FromPoint3, std::placeholders::_1, boost::none), point); EXPECT(assert_equal(expectedH, actualH, 1e-5)); } diff --git a/gtsam/linear/tests/testGaussianBayesNet.cpp b/gtsam/linear/tests/testGaussianBayesNet.cpp index c88bf8731..00a338e54 100644 --- a/gtsam/linear/tests/testGaussianBayesNet.cpp +++ b/gtsam/linear/tests/testGaussianBayesNet.cpp @@ -32,7 +32,7 @@ #include using namespace boost::assign; -using namespace boost::placeholders; +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -270,11 +270,11 @@ TEST(GaussianBayesNet, ComputeSteepestDescentPoint) { // Compute the Hessian numerically Matrix hessian = numericalHessian( - boost::bind(&computeError, gbn, _1), Vector10::Zero()); + std::bind(&computeError, gbn, std::placeholders::_1), Vector10::Zero()); // Compute the gradient numerically Vector gradient = numericalGradient( - boost::bind(&computeError, gbn, _1), Vector10::Zero()); + std::bind(&computeError, gbn, std::placeholders::_1), Vector10::Zero()); // Compute the gradient using dense matrices Matrix augmentedHessian = GaussianFactorGraph(gbn).augmentedHessian(); diff --git a/gtsam/linear/tests/testGaussianBayesTree.cpp b/gtsam/linear/tests/testGaussianBayesTree.cpp index a6a60c19c..c5601af27 100644 --- a/gtsam/linear/tests/testGaussianBayesTree.cpp +++ b/gtsam/linear/tests/testGaussianBayesTree.cpp @@ -21,7 +21,6 @@ #include #include // for operator += #include // for operator += -#include #include #include @@ -30,7 +29,7 @@ #include using namespace boost::assign; -using namespace boost::placeholders; +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -260,11 +259,11 @@ TEST(GaussianBayesTree, ComputeSteepestDescentPointBT) { // Compute the Hessian numerically Matrix hessian = numericalHessian( - boost::bind(&computeError, bt, _1), Vector10::Zero()); + std::bind(&computeError, bt, std::placeholders::_1), Vector10::Zero()); // Compute the gradient numerically Vector gradient = numericalGradient( - boost::bind(&computeError, bt, _1), Vector10::Zero()); + std::bind(&computeError, bt, std::placeholders::_1), Vector10::Zero()); // Compute the gradient using dense matrices Matrix augmentedHessian = GaussianFactorGraph(bt).augmentedHessian(); diff --git a/gtsam/navigation/tests/testAHRSFactor.cpp b/gtsam/navigation/tests/testAHRSFactor.cpp index 828e264f4..a4d06d01a 100644 --- a/gtsam/navigation/tests/testAHRSFactor.cpp +++ b/gtsam/navigation/tests/testAHRSFactor.cpp @@ -25,10 +25,9 @@ #include #include -#include #include -using namespace boost::placeholders; +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -175,17 +174,17 @@ TEST(AHRSFactor, Error) { // Expected Jacobians Matrix H1e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, _1, x2, bias), x1); + std::bind(&callEvaluateError, factor, std::placeholders::_1, x2, bias), x1); Matrix H2e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, x1, _1, bias), x2); + std::bind(&callEvaluateError, factor, x1, std::placeholders::_1, bias), x2); Matrix H3e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, x1, x2, _1), bias); + std::bind(&callEvaluateError, factor, x1, x2, std::placeholders::_1), bias); // Check rotation Jacobians Matrix RH1e = numericalDerivative11( - boost::bind(&evaluateRotationError, factor, _1, x2, bias), x1); + std::bind(&evaluateRotationError, factor, std::placeholders::_1, x2, bias), x1); Matrix RH2e = numericalDerivative11( - boost::bind(&evaluateRotationError, factor, x1, _1, bias), x2); + std::bind(&evaluateRotationError, factor, x1, std::placeholders::_1, bias), x2); // Actual Jacobians Matrix H1a, H2a, H3a; @@ -234,19 +233,19 @@ TEST(AHRSFactor, ErrorWithBiases) { // Expected Jacobians Matrix H1e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, _1, x2, bias), x1); + std::bind(&callEvaluateError, factor, std::placeholders::_1, x2, bias), x1); Matrix H2e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, x1, _1, bias), x2); + std::bind(&callEvaluateError, factor, x1, std::placeholders::_1, bias), x2); Matrix H3e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, x1, x2, _1), bias); + std::bind(&callEvaluateError, factor, x1, x2, std::placeholders::_1), bias); // Check rotation Jacobians Matrix RH1e = numericalDerivative11( - boost::bind(&evaluateRotationError, factor, _1, x2, bias), x1); + std::bind(&evaluateRotationError, factor, std::placeholders::_1, x2, bias), x1); Matrix RH2e = numericalDerivative11( - boost::bind(&evaluateRotationError, factor, x1, _1, bias), x2); + std::bind(&evaluateRotationError, factor, x1, std::placeholders::_1, bias), x2); Matrix RH3e = numericalDerivative11( - boost::bind(&evaluateRotationError, factor, x1, x2, _1), bias); + std::bind(&evaluateRotationError, factor, x1, x2, std::placeholders::_1), bias); // Actual Jacobians Matrix H1a, H2a, H3a; @@ -269,7 +268,7 @@ TEST( AHRSFactor, PartialDerivativeExpmap ) { // Compute numerical derivatives Matrix expectedDelRdelBiasOmega = numericalDerivative11( - boost::bind(&evaluateRotation, measuredOmega, _1, deltaT), biasOmega); + std::bind(&evaluateRotation, measuredOmega, std::placeholders::_1, deltaT), biasOmega); const Matrix3 Jr = Rot3::ExpmapDerivative( (measuredOmega - biasOmega) * deltaT); @@ -294,7 +293,7 @@ TEST( AHRSFactor, PartialDerivativeLogmap ) { // Compute numerical derivatives Matrix expectedDelFdeltheta = numericalDerivative11( - boost::bind(&evaluateLogRotation, thetahat, _1), deltatheta); + std::bind(&evaluateLogRotation, thetahat, std::placeholders::_1), deltatheta); const Vector3 x = thetahat; // parametrization of so(3) const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^ @@ -368,7 +367,7 @@ TEST( AHRSFactor, FirstOrderPreIntegratedMeasurements ) { // Compute numerical derivatives Matrix expectedDelRdelBias = numericalDerivative11( - boost::bind(&evaluatePreintegratedMeasurementsRotation, _1, + std::bind(&evaluatePreintegratedMeasurementsRotation, std::placeholders::_1, measuredOmegas, deltaTs, Vector3(M_PI / 100.0, 0.0, 0.0)), bias); Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3); @@ -410,19 +409,19 @@ TEST( AHRSFactor, ErrorWithBiasesAndSensorBodyDisplacement ) { // Expected Jacobians Matrix H1e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, _1, x2, bias), x1); + std::bind(&callEvaluateError, factor, std::placeholders::_1, x2, bias), x1); Matrix H2e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, x1, _1, bias), x2); + std::bind(&callEvaluateError, factor, x1, std::placeholders::_1, bias), x2); Matrix H3e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, x1, x2, _1), bias); + std::bind(&callEvaluateError, factor, x1, x2, std::placeholders::_1), bias); // Check rotation Jacobians Matrix RH1e = numericalDerivative11( - boost::bind(&evaluateRotationError, factor, _1, x2, bias), x1); + std::bind(&evaluateRotationError, factor, std::placeholders::_1, x2, bias), x1); Matrix RH2e = numericalDerivative11( - boost::bind(&evaluateRotationError, factor, x1, _1, bias), x2); + std::bind(&evaluateRotationError, factor, x1, std::placeholders::_1, bias), x2); Matrix RH3e = numericalDerivative11( - boost::bind(&evaluateRotationError, factor, x1, x2, _1), bias); + std::bind(&evaluateRotationError, factor, x1, x2, std::placeholders::_1), bias); // Actual Jacobians Matrix H1a, H2a, H3a; @@ -459,8 +458,8 @@ TEST (AHRSFactor, predictTest) { // AHRSFactor::PreintegratedMeasurements::predict Matrix expectedH = numericalDerivative11( - boost::bind(&AHRSFactor::PreintegratedMeasurements::predict, - &pim, _1, boost::none), bias); + std::bind(&AHRSFactor::PreintegratedMeasurements::predict, + &pim, std::placeholders::_1, boost::none), bias); // Actual Jacobians Matrix H; diff --git a/gtsam/navigation/tests/testAttitudeFactor.cpp b/gtsam/navigation/tests/testAttitudeFactor.cpp index 2ab60fe6a..d49907cbf 100644 --- a/gtsam/navigation/tests/testAttitudeFactor.cpp +++ b/gtsam/navigation/tests/testAttitudeFactor.cpp @@ -25,7 +25,7 @@ #include #include -using namespace boost::placeholders; +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -50,8 +50,9 @@ TEST( Rot3AttitudeFactor, Constructor ) { EXPECT(assert_equal((Vector) Z_2x1,factor.evaluateError(nRb),1e-5)); // Calculate numerical derivatives - Matrix expectedH = numericalDerivative11( - boost::bind(&Rot3AttitudeFactor::evaluateError, &factor, _1, boost::none), + Matrix expectedH = numericalDerivative11( + std::bind(&Rot3AttitudeFactor::evaluateError, &factor, + std::placeholders::_1, boost::none), nRb); // Use the factor to calculate the derivative @@ -117,7 +118,7 @@ TEST( Pose3AttitudeFactor, Constructor ) { // Calculate numerical derivatives Matrix expectedH = numericalDerivative11( - boost::bind(&Pose3AttitudeFactor::evaluateError, &factor, _1, + std::bind(&Pose3AttitudeFactor::evaluateError, &factor, std::placeholders::_1, boost::none), T); // Use the factor to calculate the derivative diff --git a/gtsam/navigation/tests/testGPSFactor.cpp b/gtsam/navigation/tests/testGPSFactor.cpp index b486272ab..b784c0c94 100644 --- a/gtsam/navigation/tests/testGPSFactor.cpp +++ b/gtsam/navigation/tests/testGPSFactor.cpp @@ -27,7 +27,7 @@ #include #include -using namespace boost::placeholders; +using namespace std::placeholders; using namespace std; using namespace gtsam; using namespace GeographicLib; @@ -72,7 +72,7 @@ TEST( GPSFactor, Constructor ) { // Calculate numerical derivatives Matrix expectedH = numericalDerivative11( - boost::bind(&GPSFactor::evaluateError, &factor, _1, boost::none), T); + std::bind(&GPSFactor::evaluateError, &factor, _1, boost::none), T); // Use the factor to calculate the derivative Matrix actualH; @@ -101,7 +101,7 @@ TEST( GPSFactor2, Constructor ) { // Calculate numerical derivatives Matrix expectedH = numericalDerivative11( - boost::bind(&GPSFactor2::evaluateError, &factor, _1, boost::none), T); + std::bind(&GPSFactor2::evaluateError, &factor, _1, boost::none), T); // Use the factor to calculate the derivative Matrix actualH; diff --git a/gtsam/navigation/tests/testImuBias.cpp b/gtsam/navigation/tests/testImuBias.cpp index e7da2c81c..b486a4a98 100644 --- a/gtsam/navigation/tests/testImuBias.cpp +++ b/gtsam/navigation/tests/testImuBias.cpp @@ -19,9 +19,8 @@ #include #include -#include -using namespace boost::placeholders; +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -128,8 +127,9 @@ TEST(ImuBias, operatorSubB) { TEST(ImuBias, Correct1) { Matrix aH1, aH2; const Vector3 measurement(1, 2, 3); - boost::function f = boost::bind( - &Bias::correctAccelerometer, _1, _2, boost::none, boost::none); + std::function f = + std::bind(&Bias::correctAccelerometer, std::placeholders::_1, + std::placeholders::_2, boost::none, boost::none); bias1.correctAccelerometer(measurement, aH1, aH2); EXPECT(assert_equal(numericalDerivative21(f, bias1, measurement), aH1)); EXPECT(assert_equal(numericalDerivative22(f, bias1, measurement), aH2)); @@ -139,8 +139,9 @@ TEST(ImuBias, Correct1) { TEST(ImuBias, Correct2) { Matrix aH1, aH2; const Vector3 measurement(1, 2, 3); - boost::function f = - boost::bind(&Bias::correctGyroscope, _1, _2, boost::none, boost::none); + std::function f = + std::bind(&Bias::correctGyroscope, std::placeholders::_1, + std::placeholders::_2, boost::none, boost::none); bias1.correctGyroscope(measurement, aH1, aH2); EXPECT(assert_equal(numericalDerivative21(f, bias1, measurement), aH1)); EXPECT(assert_equal(numericalDerivative22(f, bias1, measurement), aH2)); diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index f19862772..801d87895 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -146,9 +146,9 @@ TEST(ImuFactor, PreintegratedMeasurements) { Matrix9 aH1, aH2; Matrix96 aH3; actual.computeError(x1, x2, bias, aH1, aH2, aH3); - boost::function f = - boost::bind(&PreintegrationBase::computeError, actual, - boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, + std::function f = + std::bind(&PreintegrationBase::computeError, actual, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, boost::none, boost::none, boost::none); EXPECT(assert_equal(numericalDerivative31(f, x1, x2, bias), aH1, 1e-9)); EXPECT(assert_equal(numericalDerivative32(f, x1, x2, bias), aH2, 1e-9)); @@ -204,20 +204,20 @@ TEST(ImuFactor, PreintegrationBaseMethods) { Matrix96 actualH; pim.biasCorrectedDelta(kZeroBias, actualH); Matrix expectedH = numericalDerivative11( - boost::bind(&PreintegrationBase::biasCorrectedDelta, pim, - boost::placeholders::_1, boost::none), kZeroBias); + std::bind(&PreintegrationBase::biasCorrectedDelta, pim, + std::placeholders::_1, boost::none), kZeroBias); EXPECT(assert_equal(expectedH, actualH)); Matrix9 aH1; Matrix96 aH2; NavState predictedState = pim.predict(state1, kZeroBias, aH1, aH2); Matrix eH1 = numericalDerivative11( - boost::bind(&PreintegrationBase::predict, pim, boost::placeholders::_1, + std::bind(&PreintegrationBase::predict, pim, std::placeholders::_1, kZeroBias, boost::none, boost::none), state1); EXPECT(assert_equal(eH1, aH1)); Matrix eH2 = numericalDerivative11( - boost::bind(&PreintegrationBase::predict, pim, state1, - boost::placeholders::_1, boost::none, boost::none), kZeroBias); + std::bind(&PreintegrationBase::predict, pim, state1, + std::placeholders::_1, boost::none, boost::none), kZeroBias); EXPECT(assert_equal(eH2, aH2)); } @@ -278,12 +278,12 @@ TEST(ImuFactor, ErrorAndJacobians) { // Make sure rotation part is correct when error is interpreted as axis-angle // Jacobians are around zero, so the rotation part is the same as: Matrix H1Rot3 = numericalDerivative11( - boost::bind(&evaluateRotationError, factor, boost::placeholders::_1, v1, x2, v2, kZeroBias), + std::bind(&evaluateRotationError, factor, std::placeholders::_1, v1, x2, v2, kZeroBias), x1); EXPECT(assert_equal(H1Rot3, H1a.topRows(3))); Matrix H3Rot3 = numericalDerivative11( - boost::bind(&evaluateRotationError, factor, x1, v1, boost::placeholders::_1, v2, kZeroBias), + std::bind(&evaluateRotationError, factor, x1, v1, std::placeholders::_1, v2, kZeroBias), x2); EXPECT(assert_equal(H3Rot3, H3a.topRows(3))); @@ -333,8 +333,8 @@ TEST(ImuFactor, ErrorAndJacobianWithBiases) { Matrix96 actualH; pim.biasCorrectedDelta(bias, actualH); Matrix expectedH = numericalDerivative11( - boost::bind(&PreintegrationBase::biasCorrectedDelta, pim, - boost::placeholders::_1, boost::none), bias); + std::bind(&PreintegrationBase::biasCorrectedDelta, pim, + std::placeholders::_1, boost::none), bias); EXPECT(assert_equal(expectedH, actualH)); // Create factor @@ -522,7 +522,7 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) { pim.correctMeasurementsBySensorPose(measuredAcc, measuredOmega, boost::none, D_correctedAcc_measuredOmega, boost::none); Matrix3 expectedD = numericalDerivative11( - boost::bind(correctedAcc, pim, measuredAcc, boost::placeholders::_1), + std::bind(correctedAcc, pim, measuredAcc, std::placeholders::_1), measuredOmega, 1e-6); EXPECT(assert_equal(expectedD, D_correctedAcc_measuredOmega, 1e-5)); @@ -534,15 +534,15 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) { // pim.updatedDeltaXij(measuredAcc, measuredOmega, dt, boost::none, G1, G2); // // Matrix93 expectedG1 = numericalDerivative21( -// boost::bind(&PreintegratedImuMeasurements::updatedDeltaXij, pim, -// boost::placeholders::_1, boost::placeholders::_2, +// std::bind(&PreintegratedImuMeasurements::updatedDeltaXij, pim, +// std::placeholders::_1, std::placeholders::_2, // dt, boost::none, boost::none, boost::none), measuredAcc, // measuredOmega, 1e-6); // EXPECT(assert_equal(expectedG1, G1, 1e-5)); // // Matrix93 expectedG2 = numericalDerivative22( -// boost::bind(&PreintegratedImuMeasurements::updatedDeltaXij, pim, -// boost::placeholders::_1, boost::placeholders::_2, +// std::bind(&PreintegratedImuMeasurements::updatedDeltaXij, pim, +// std::placeholders::_1, std::placeholders::_2, // dt, boost::none, boost::none, boost::none), measuredAcc, // measuredOmega, 1e-6); // EXPECT(assert_equal(expectedG2, G2, 1e-5)); diff --git a/gtsam/navigation/tests/testMagFactor.cpp b/gtsam/navigation/tests/testMagFactor.cpp index ad193b503..5107b3b6b 100644 --- a/gtsam/navigation/tests/testMagFactor.cpp +++ b/gtsam/navigation/tests/testMagFactor.cpp @@ -26,7 +26,7 @@ #include -using namespace boost::placeholders; +using namespace std::placeholders; using namespace std; using namespace gtsam; using namespace GeographicLib; @@ -64,7 +64,7 @@ TEST( MagFactor, unrotate ) { Point3 expected(22735.5, 314.502, 44202.5); EXPECT( assert_equal(expected, MagFactor::unrotate(theta,nM,H),1e-1)); EXPECT( assert_equal(numericalDerivative11 // - (boost::bind(&MagFactor::unrotate, _1, nM, none), theta), H, 1e-6)); + (std::bind(&MagFactor::unrotate, _1, nM, none), theta), H, 1e-6)); } // ************************************************************************* @@ -76,35 +76,35 @@ TEST( MagFactor, Factors ) { MagFactor f(1, measured, s, dir, bias, model); EXPECT( assert_equal(Z_3x1,f.evaluateError(theta,H1),1e-5)); EXPECT( assert_equal((Matrix)numericalDerivative11 // - (boost::bind(&MagFactor::evaluateError, &f, _1, none), theta), H1, 1e-7)); + (std::bind(&MagFactor::evaluateError, &f, _1, none), theta), H1, 1e-7)); // MagFactor1 MagFactor1 f1(1, measured, s, dir, bias, model); EXPECT( assert_equal(Z_3x1,f1.evaluateError(nRb,H1),1e-5)); EXPECT( assert_equal(numericalDerivative11 // - (boost::bind(&MagFactor1::evaluateError, &f1, _1, none), nRb), H1, 1e-7)); + (std::bind(&MagFactor1::evaluateError, &f1, _1, none), nRb), H1, 1e-7)); // MagFactor2 MagFactor2 f2(1, 2, measured, nRb, model); EXPECT( assert_equal(Z_3x1,f2.evaluateError(scaled,bias,H1,H2),1e-5)); EXPECT( assert_equal(numericalDerivative11 // - (boost::bind(&MagFactor2::evaluateError, &f2, _1, bias, none, none), scaled),// + (std::bind(&MagFactor2::evaluateError, &f2, _1, bias, none, none), scaled),// H1, 1e-7)); EXPECT( assert_equal(numericalDerivative11 // - (boost::bind(&MagFactor2::evaluateError, &f2, scaled, _1, none, none), bias),// + (std::bind(&MagFactor2::evaluateError, &f2, scaled, _1, none, none), bias),// H2, 1e-7)); // MagFactor2 MagFactor3 f3(1, 2, 3, measured, nRb, model); EXPECT(assert_equal(Z_3x1,f3.evaluateError(s,dir,bias,H1,H2,H3),1e-5)); EXPECT(assert_equal((Matrix)numericalDerivative11 // - (boost::bind(&MagFactor3::evaluateError, &f3, _1, dir, bias, none, none, none), s),// + (std::bind(&MagFactor3::evaluateError, &f3, _1, dir, bias, none, none, none), s),// H1, 1e-7)); EXPECT(assert_equal(numericalDerivative11 // - (boost::bind(&MagFactor3::evaluateError, &f3, s, _1, bias, none, none, none), dir),// + (std::bind(&MagFactor3::evaluateError, &f3, s, _1, bias, none, none, none), dir),// H2, 1e-7)); EXPECT(assert_equal(numericalDerivative11 // - (boost::bind(&MagFactor3::evaluateError, &f3, s, dir, _1, none, none, none), bias),// + (std::bind(&MagFactor3::evaluateError, &f3, s, dir, _1, none, none, none), bias),// H3, 1e-7)); } diff --git a/gtsam/navigation/tests/testMagPoseFactor.cpp b/gtsam/navigation/tests/testMagPoseFactor.cpp index 1a3c5b2a9..204c1d38f 100644 --- a/gtsam/navigation/tests/testMagPoseFactor.cpp +++ b/gtsam/navigation/tests/testMagPoseFactor.cpp @@ -17,9 +17,7 @@ #include #include -#include - -using namespace boost::placeholders; +using namespace std::placeholders; using namespace gtsam; // ***************************************************************************** @@ -78,8 +76,11 @@ TEST(MagPoseFactor, JacobianPose2) { // Error should be zero at the groundtruth pose. MagPoseFactor f(Symbol('X', 0), measured2, scale, dir2, bias2, model2, boost::none); CHECK(gtsam::assert_equal(Z_2x1, f.evaluateError(n_P2_b, H2), 1e-5)); - CHECK(gtsam::assert_equal(gtsam::numericalDerivative11 // - (boost::bind(&MagPoseFactor::evaluateError, &f, _1, boost::none), n_P2_b), H2, 1e-7)); + CHECK(gtsam::assert_equal(gtsam::numericalDerivative11 // + (std::bind(&MagPoseFactor::evaluateError, &f, + std::placeholders::_1, boost::none), + n_P2_b), + H2, 1e-7)); } // ***************************************************************************** @@ -89,8 +90,11 @@ TEST(MagPoseFactor, JacobianPose3) { // Error should be zero at the groundtruth pose. MagPoseFactor f(Symbol('X', 0), measured3, scale, dir3, bias3, model3, boost::none); CHECK(gtsam::assert_equal(Z_3x1, f.evaluateError(n_P3_b, H3), 1e-5)); - CHECK(gtsam::assert_equal(gtsam::numericalDerivative11 // - (boost::bind(&MagPoseFactor::evaluateError, &f, _1, boost::none), n_P3_b), H3, 1e-7)); + CHECK(gtsam::assert_equal(gtsam::numericalDerivative11 // + (std::bind(&MagPoseFactor::evaluateError, &f, + std::placeholders::_1, boost::none), + n_P3_b), + H3, 1e-7)); } // ***************************************************************************** @@ -104,7 +108,7 @@ TEST(MagPoseFactor, body_P_sensor2) { MagPoseFactor f = MagPoseFactor(Symbol('X', 0), sM, scale, dir2, bias2, model2, body_P2_sensor); CHECK(gtsam::assert_equal(Z_2x1, f.evaluateError(n_P2_b, H2), 1e-5)); CHECK(gtsam::assert_equal(gtsam::numericalDerivative11 // - (boost::bind(&MagPoseFactor::evaluateError, &f, _1, boost::none), n_P2_b), H2, 1e-7)); + (std::bind(&MagPoseFactor::evaluateError, &f, std::placeholders::_1, boost::none), n_P2_b), H2, 1e-7)); } // ***************************************************************************** @@ -118,7 +122,7 @@ TEST(MagPoseFactor, body_P_sensor3) { MagPoseFactor f = MagPoseFactor(Symbol('X', 0), sM, scale, dir3, bias3, model3, body_P3_sensor); CHECK(gtsam::assert_equal(Z_3x1, f.evaluateError(n_P3_b, H3), 1e-5)); CHECK(gtsam::assert_equal(gtsam::numericalDerivative11 // - (boost::bind(&MagPoseFactor::evaluateError, &f, _1, boost::none), n_P3_b), H3, 1e-7)); + (std::bind(&MagPoseFactor::evaluateError, &f, std::placeholders::_1, boost::none), n_P3_b), H3, 1e-7)); } // ***************************************************************************** diff --git a/gtsam/navigation/tests/testManifoldPreintegration.cpp b/gtsam/navigation/tests/testManifoldPreintegration.cpp index 625689ed7..7796ccbda 100644 --- a/gtsam/navigation/tests/testManifoldPreintegration.cpp +++ b/gtsam/navigation/tests/testManifoldPreintegration.cpp @@ -22,11 +22,10 @@ #include #include -#include #include "imuFactorTesting.h" -using namespace boost::placeholders; +using namespace std::placeholders; namespace testing { // Create default parameters with Z-down and above noise parameters @@ -43,21 +42,21 @@ static boost::shared_ptr Params() { TEST(ManifoldPreintegration, BiasCorrectionJacobians) { testing::SomeMeasurements measurements; - boost::function deltaRij = + std::function deltaRij = [=](const Vector3& a, const Vector3& w) { ManifoldPreintegration pim(testing::Params(), Bias(a, w)); testing::integrateMeasurements(measurements, &pim); return pim.deltaRij(); }; - boost::function deltaPij = + std::function deltaPij = [=](const Vector3& a, const Vector3& w) { ManifoldPreintegration pim(testing::Params(), Bias(a, w)); testing::integrateMeasurements(measurements, &pim); return pim.deltaPij(); }; - boost::function deltaVij = + std::function deltaVij = [=](const Vector3& a, const Vector3& w) { ManifoldPreintegration pim(testing::Params(), Bias(a, w)); testing::integrateMeasurements(measurements, &pim); @@ -98,10 +97,12 @@ TEST(ManifoldPreintegration, computeError) { Matrix9 aH1, aH2; Matrix96 aH3; pim.computeError(x1, x2, bias, aH1, aH2, aH3); - boost::function f = - boost::bind(&ManifoldPreintegration::computeError, pim, _1, _2, _3, - boost::none, boost::none, boost::none); + std::function + f = std::bind(&ManifoldPreintegration::computeError, pim, + std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, boost::none, boost::none, + boost::none); // NOTE(frank): tolerance of 1e-3 on H1 because approximate away from 0 EXPECT(assert_equal(numericalDerivative31(f, x1, x2, bias), aH1, 1e-9)); EXPECT(assert_equal(numericalDerivative32(f, x1, x2, bias), aH2, 1e-9)); diff --git a/gtsam/navigation/tests/testNavState.cpp b/gtsam/navigation/tests/testNavState.cpp index 86c708f5e..e7adb923d 100644 --- a/gtsam/navigation/tests/testNavState.cpp +++ b/gtsam/navigation/tests/testNavState.cpp @@ -23,7 +23,7 @@ #include #include -using namespace boost::placeholders; +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -39,9 +39,9 @@ static const Vector9 kZeroXi = Vector9::Zero(); /* ************************************************************************* */ TEST(NavState, Constructor) { - boost::function create = - boost::bind(&NavState::Create, _1, _2, _3, boost::none, boost::none, - boost::none); + std::function create = + std::bind(&NavState::Create, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, boost::none, boost::none, boost::none); Matrix aH1, aH2, aH3; EXPECT( assert_equal(kState1, @@ -59,9 +59,9 @@ TEST(NavState, Constructor) { /* ************************************************************************* */ TEST(NavState, Constructor2) { - boost::function construct = - boost::bind(&NavState::FromPoseVelocity, _1, _2, boost::none, - boost::none); + std::function construct = + std::bind(&NavState::FromPoseVelocity, std::placeholders::_1, + std::placeholders::_2, boost::none, boost::none); Matrix aH1, aH2; EXPECT( assert_equal(kState1, @@ -76,7 +76,7 @@ TEST( NavState, Attitude) { Rot3 actual = kState1.attitude(aH); EXPECT(assert_equal(actual, kAttitude)); eH = numericalDerivative11( - boost::bind(&NavState::attitude, _1, boost::none), kState1); + std::bind(&NavState::attitude, std::placeholders::_1, boost::none), kState1); EXPECT(assert_equal((Matrix )eH, aH)); } @@ -86,7 +86,8 @@ TEST( NavState, Position) { Point3 actual = kState1.position(aH); EXPECT(assert_equal(actual, kPosition)); eH = numericalDerivative11( - boost::bind(&NavState::position, _1, boost::none), kState1); + std::bind(&NavState::position, std::placeholders::_1, boost::none), + kState1); EXPECT(assert_equal((Matrix )eH, aH)); } @@ -96,7 +97,8 @@ TEST( NavState, Velocity) { Velocity3 actual = kState1.velocity(aH); EXPECT(assert_equal(actual, kVelocity)); eH = numericalDerivative11( - boost::bind(&NavState::velocity, _1, boost::none), kState1); + std::bind(&NavState::velocity, std::placeholders::_1, boost::none), + kState1); EXPECT(assert_equal((Matrix )eH, aH)); } @@ -106,7 +108,8 @@ TEST( NavState, BodyVelocity) { Velocity3 actual = kState1.bodyVelocity(aH); EXPECT(assert_equal(actual, kAttitude.unrotate(kVelocity))); eH = numericalDerivative11( - boost::bind(&NavState::bodyVelocity, _1, boost::none), kState1); + std::bind(&NavState::bodyVelocity, std::placeholders::_1, boost::none), + kState1); EXPECT(assert_equal((Matrix )eH, aH)); } @@ -137,8 +140,9 @@ TEST( NavState, Manifold ) { // Check retract derivatives Matrix9 aH1, aH2; kState1.retract(xi, aH1, aH2); - boost::function retract = - boost::bind(&NavState::retract, _1, _2, boost::none, boost::none); + std::function retract = + std::bind(&NavState::retract, std::placeholders::_1, + std::placeholders::_2, boost::none, boost::none); EXPECT(assert_equal(numericalDerivative21(retract, kState1, xi), aH1)); EXPECT(assert_equal(numericalDerivative22(retract, kState1, xi), aH2)); @@ -149,9 +153,9 @@ TEST( NavState, Manifold ) { EXPECT(assert_equal(numericalDerivative22(retract, state2, xi2), aH2)); // Check localCoordinates derivatives - boost::function local = - boost::bind(&NavState::localCoordinates, _1, _2, boost::none, - boost::none); + std::function local = + std::bind(&NavState::localCoordinates, std::placeholders::_1, + std::placeholders::_2, boost::none, boost::none); // from state1 to state2 kState1.localCoordinates(state2, aH1, aH2); EXPECT(assert_equal(numericalDerivative21(local, kState1, state2), aH1)); @@ -168,8 +172,9 @@ TEST( NavState, Manifold ) { /* ************************************************************************* */ static const double dt = 2.0; -boost::function coriolis = boost::bind( - &NavState::coriolis, _1, dt, kOmegaCoriolis, _2, boost::none); +std::function coriolis = + std::bind(&NavState::coriolis, std::placeholders::_1, dt, kOmegaCoriolis, + std::placeholders::_2, boost::none); TEST(NavState, Coriolis) { Matrix9 aH; @@ -244,9 +249,10 @@ TEST(NavState, CorrectPIM) { xi << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4, -0.1, -0.2, -0.3; double dt = 0.5; Matrix9 aH1, aH2; - boost::function correctPIM = - boost::bind(&NavState::correctPIM, _1, _2, dt, kGravity, kOmegaCoriolis, - false, boost::none, boost::none); + std::function correctPIM = + std::bind(&NavState::correctPIM, std::placeholders::_1, + std::placeholders::_2, dt, kGravity, kOmegaCoriolis, false, + boost::none, boost::none); kState1.correctPIM(xi, dt, kGravity, kOmegaCoriolis, false, aH1, aH2); EXPECT(assert_equal(numericalDerivative21(correctPIM, kState1, xi), aH1)); EXPECT(assert_equal(numericalDerivative22(correctPIM, kState1, xi), aH2)); diff --git a/gtsam/navigation/tests/testScenario.cpp b/gtsam/navigation/tests/testScenario.cpp index d0bae3690..0df51956b 100644 --- a/gtsam/navigation/tests/testScenario.cpp +++ b/gtsam/navigation/tests/testScenario.cpp @@ -19,10 +19,9 @@ #include #include -#include #include -using namespace boost::placeholders; +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -148,7 +147,7 @@ TEST(Scenario, Accelerating) { { // Check acceleration in nav Matrix expected = numericalDerivative11( - boost::bind(&Scenario::velocity_n, scenario, _1), T); + std::bind(&Scenario::velocity_n, scenario, std::placeholders::_1), T); EXPECT(assert_equal(Vector3(expected), scenario.acceleration_n(T), 1e-9)); } diff --git a/gtsam/navigation/tests/testTangentPreintegration.cpp b/gtsam/navigation/tests/testTangentPreintegration.cpp index 9bb988b42..ada059094 100644 --- a/gtsam/navigation/tests/testTangentPreintegration.cpp +++ b/gtsam/navigation/tests/testTangentPreintegration.cpp @@ -22,11 +22,10 @@ #include #include -#include #include "imuFactorTesting.h" -using namespace boost::placeholders; +using namespace std::placeholders; static const double kDt = 0.1; @@ -78,7 +77,7 @@ TEST(TangentPreintegration, UpdateEstimate2) { TEST(ImuFactor, BiasCorrectionJacobians) { testing::SomeMeasurements measurements; - boost::function preintegrated = + std::function preintegrated = [=](const Vector3& a, const Vector3& w) { TangentPreintegration pim(testing::Params(), Bias(a, w)); testing::integrateMeasurements(measurements, &pim); @@ -105,10 +104,12 @@ TEST(TangentPreintegration, computeError) { Matrix9 aH1, aH2; Matrix96 aH3; pim.computeError(x1, x2, bias, aH1, aH2, aH3); - boost::function f = - boost::bind(&TangentPreintegration::computeError, pim, _1, _2, _3, - boost::none, boost::none, boost::none); + std::function + f = std::bind(&TangentPreintegration::computeError, pim, + std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, boost::none, boost::none, + boost::none); // NOTE(frank): tolerance of 1e-3 on H1 because approximate away from 0 EXPECT(assert_equal(numericalDerivative31(f, x1, x2, bias), aH1, 1e-9)); EXPECT(assert_equal(numericalDerivative32(f, x1, x2, bias), aH2, 1e-9)); @@ -121,7 +122,7 @@ TEST(TangentPreintegration, Compose) { TangentPreintegration pim(testing::Params()); testing::integrateMeasurements(measurements, &pim); - boost::function f = + std::function f = [pim](const Vector9& zeta01, const Vector9& zeta12) { return TangentPreintegration::Compose(zeta01, zeta12, pim.deltaTij()); }; diff --git a/gtsam/nonlinear/tests/testExpression.cpp b/gtsam/nonlinear/tests/testExpression.cpp index 8fcf84a11..7bb802186 100644 --- a/gtsam/nonlinear/tests/testExpression.cpp +++ b/gtsam/nonlinear/tests/testExpression.cpp @@ -495,7 +495,7 @@ TEST(Expression, Subtract) { /* ************************************************************************* */ TEST(Expression, LinearExpression) { const Key key(67); - const boost::function f = [](const Point3& p) { return (Vector3)p; }; + const std::function f = [](const Point3& p) { return (Vector3)p; }; const Matrix3 kIdentity = I_3x3; const Expression linear_ = linearExpression(f, Point3_(key), kIdentity); diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index e9076a7d7..b894f4816 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -34,7 +34,7 @@ #include using namespace boost::assign; -using namespace boost::placeholders; +using namespace std::placeholders; using namespace gtsam; using namespace std; static double inf = std::numeric_limits::infinity(); @@ -336,7 +336,7 @@ TEST(Values, filter) { // Filter by key int i = 0; - Values::Filtered filtered = values.filter(boost::bind(std::greater_equal(), _1, 2)); + Values::Filtered filtered = values.filter(std::bind(std::greater_equal(), std::placeholders::_1, 2)); EXPECT_LONGS_EQUAL(2, (long)filtered.size()); for(const auto key_value: filtered) { if(i == 0) { @@ -364,7 +364,7 @@ TEST(Values, filter) { EXPECT(assert_equal(expectedSubValues1, actualSubValues1)); // ConstFilter by Key - Values::ConstFiltered constfiltered = values.filter(boost::bind(std::greater_equal(), _1, 2)); + Values::ConstFiltered constfiltered = values.filter(std::bind(std::greater_equal(), std::placeholders::_1, 2)); EXPECT_LONGS_EQUAL(2, (long)constfiltered.size()); Values fromconstfiltered(constfiltered); EXPECT(assert_equal(expectedSubValues1, fromconstfiltered)); diff --git a/gtsam/sam/tests/testRangeFactor.cpp b/gtsam/sam/tests/testRangeFactor.cpp index 2af5825db..5f5d4f4dd 100644 --- a/gtsam/sam/tests/testRangeFactor.cpp +++ b/gtsam/sam/tests/testRangeFactor.cpp @@ -28,7 +28,7 @@ #include #include -using namespace boost::placeholders; +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -265,9 +265,9 @@ TEST( RangeFactor, Jacobian2D ) { // Use numerical derivatives to calculate the Jacobians Matrix H1Expected, H2Expected; H1Expected = numericalDerivative11( - boost::bind(&factorError2D, _1, point, factor), pose); + std::bind(&factorError2D, std::placeholders::_1, point, factor), pose); H2Expected = numericalDerivative11( - boost::bind(&factorError2D, pose, _1, factor), point); + std::bind(&factorError2D, pose, std::placeholders::_1, factor), point); // Verify the Jacobians are correct CHECK(assert_equal(H1Expected, H1Actual, 1e-9)); @@ -296,9 +296,9 @@ TEST( RangeFactor, Jacobian2DWithTransform ) { // Use numerical derivatives to calculate the Jacobians Matrix H1Expected, H2Expected; H1Expected = numericalDerivative11( - boost::bind(&factorErrorWithTransform2D, _1, point, factor), pose); + std::bind(&factorErrorWithTransform2D, std::placeholders::_1, point, factor), pose); H2Expected = numericalDerivative11( - boost::bind(&factorErrorWithTransform2D, pose, _1, factor), point); + std::bind(&factorErrorWithTransform2D, pose, std::placeholders::_1, factor), point); // Verify the Jacobians are correct CHECK(assert_equal(H1Expected, H1Actual, 1e-9)); @@ -323,9 +323,9 @@ TEST( RangeFactor, Jacobian3D ) { // Use numerical derivatives to calculate the Jacobians Matrix H1Expected, H2Expected; H1Expected = numericalDerivative11( - boost::bind(&factorError3D, _1, point, factor), pose); + std::bind(&factorError3D, std::placeholders::_1, point, factor), pose); H2Expected = numericalDerivative11( - boost::bind(&factorError3D, pose, _1, factor), point); + std::bind(&factorError3D, pose, std::placeholders::_1, factor), point); // Verify the Jacobians are correct CHECK(assert_equal(H1Expected, H1Actual, 1e-9)); @@ -355,9 +355,9 @@ TEST( RangeFactor, Jacobian3DWithTransform ) { // Use numerical derivatives to calculate the Jacobians Matrix H1Expected, H2Expected; H1Expected = numericalDerivative11( - boost::bind(&factorErrorWithTransform3D, _1, point, factor), pose); + std::bind(&factorErrorWithTransform3D, std::placeholders::_1, point, factor), pose); H2Expected = numericalDerivative11( - boost::bind(&factorErrorWithTransform3D, pose, _1, factor), point); + std::bind(&factorErrorWithTransform3D, pose, std::placeholders::_1, factor), point); // Verify the Jacobians are correct CHECK(assert_equal(H1Expected, H1Actual, 1e-9)); diff --git a/gtsam/sfm/tests/testTranslationFactor.cpp b/gtsam/sfm/tests/testTranslationFactor.cpp index 05ae76faa..818f2bce5 100644 --- a/gtsam/sfm/tests/testTranslationFactor.cpp +++ b/gtsam/sfm/tests/testTranslationFactor.cpp @@ -20,10 +20,9 @@ #include #include -#include #include -using namespace boost::placeholders; +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -91,9 +90,9 @@ TEST(TranslationFactor, Jacobian) { // Use numerical derivatives to calculate the Jacobians Matrix H1Expected, H2Expected; H1Expected = numericalDerivative11( - boost::bind(&factorError, _1, T2, factor), T1); + std::bind(&factorError, std::placeholders::_1, T2, factor), T1); H2Expected = numericalDerivative11( - boost::bind(&factorError, T1, _1, factor), T2); + std::bind(&factorError, T1, std::placeholders::_1, factor), T2); // Verify the Jacobians are correct EXPECT(assert_equal(H1Expected, H1Actual, 1e-9)); diff --git a/gtsam/slam/tests/testBetweenFactor.cpp b/gtsam/slam/tests/testBetweenFactor.cpp index a1f8774e5..6897943ec 100644 --- a/gtsam/slam/tests/testBetweenFactor.cpp +++ b/gtsam/slam/tests/testBetweenFactor.cpp @@ -14,7 +14,7 @@ #include #include -using namespace boost::placeholders; +using namespace std::placeholders; using namespace gtsam; using namespace gtsam::symbol_shorthand; using namespace gtsam::noiseModel; @@ -35,15 +35,16 @@ TEST(BetweenFactor, Rot3) { Vector expected = Rot3::Logmap(measured.inverse() * R1.between(R2)); EXPECT(assert_equal(expected,actual/*, 1e-100*/)); // Uncomment to make unit test fail - Matrix numericalH1 = numericalDerivative21( - boost::function(boost::bind( - &BetweenFactor::evaluateError, factor, _1, _2, boost::none, - boost::none)), R1, R2, 1e-5); + Matrix numericalH1 = numericalDerivative21( + std::function(std::bind( + &BetweenFactor::evaluateError, factor, std::placeholders::_1, + std::placeholders::_2, boost::none, boost::none)), + R1, R2, 1e-5); EXPECT(assert_equal(numericalH1,actualH1, 1E-5)); Matrix numericalH2 = numericalDerivative22( - boost::function(boost::bind( - &BetweenFactor::evaluateError, factor, _1, _2, boost::none, + std::function(std::bind( + &BetweenFactor::evaluateError, factor, std::placeholders::_1, std::placeholders::_2, boost::none, boost::none)), R1, R2, 1e-5); EXPECT(assert_equal(numericalH2,actualH2, 1E-5)); } diff --git a/gtsam/slam/tests/testEssentialMatrixConstraint.cpp b/gtsam/slam/tests/testEssentialMatrixConstraint.cpp index fb2172107..080239b35 100644 --- a/gtsam/slam/tests/testEssentialMatrixConstraint.cpp +++ b/gtsam/slam/tests/testEssentialMatrixConstraint.cpp @@ -23,10 +23,9 @@ #include #include -#include #include -using namespace boost::placeholders; +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -53,12 +52,14 @@ TEST( EssentialMatrixConstraint, test ) { CHECK(assert_equal(expected, actual, 1e-8)); // Calculate numerical derivatives - Matrix expectedH1 = numericalDerivative11( - boost::bind(&EssentialMatrixConstraint::evaluateError, &factor, _1, pose2, - boost::none, boost::none), pose1); - Matrix expectedH2 = numericalDerivative11( - boost::bind(&EssentialMatrixConstraint::evaluateError, &factor, pose1, _1, - boost::none, boost::none), pose2); + Matrix expectedH1 = numericalDerivative11( + std::bind(&EssentialMatrixConstraint::evaluateError, &factor, + std::placeholders::_1, pose2, boost::none, boost::none), + pose1); + Matrix expectedH2 = numericalDerivative11( + std::bind(&EssentialMatrixConstraint::evaluateError, &factor, pose1, + std::placeholders::_1, boost::none, boost::none), + pose2); // Use the factor to calculate the derivative Matrix actualH1; diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index 79a86865d..03775a70f 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -17,9 +17,7 @@ #include #include -#include - -using namespace boost::placeholders; +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -104,8 +102,8 @@ TEST(EssentialMatrixFactor, factor) { TEST(EssentialMatrixFactor, ExpressionFactor) { Key key(1); for (size_t i = 0; i < 5; i++) { - boost::function)> f = - boost::bind(&EssentialMatrix::error, _1, vA(i), vB(i), _2); + std::function)> f = + std::bind(&EssentialMatrix::error, std::placeholders::_1, vA(i), vB(i), std::placeholders::_2); Expression E_(key); // leaf expression Expression expr(f, E_); // unary expression @@ -130,9 +128,9 @@ TEST(EssentialMatrixFactor, ExpressionFactor) { TEST(EssentialMatrixFactor, ExpressionFactorRotationOnly) { Key key(1); for (size_t i = 0; i < 5; i++) { - boost::function)> f = - boost::bind(&EssentialMatrix::error, _1, vA(i), vB(i), _2); - boost::function)> f = + std::bind(&EssentialMatrix::error, std::placeholders::_1, vA(i), vB(i), std::placeholders::_2); + std::function, OptionalJacobian<5, 2>)> g; diff --git a/gtsam/slam/tests/testOrientedPlane3Factor.cpp b/gtsam/slam/tests/testOrientedPlane3Factor.cpp index 022dc859e..35c750408 100644 --- a/gtsam/slam/tests/testOrientedPlane3Factor.cpp +++ b/gtsam/slam/tests/testOrientedPlane3Factor.cpp @@ -27,10 +27,9 @@ #include #include -#include using namespace boost::assign; -using namespace boost::placeholders; +using namespace std::placeholders; using namespace gtsam; using namespace std; @@ -145,8 +144,9 @@ TEST( OrientedPlane3Factor, Derivatives ) { OrientedPlane3Factor factor(p.planeCoefficients(), noise, poseKey, planeKey); // Calculate numerical derivatives - boost::function f = boost::bind( - &OrientedPlane3Factor::evaluateError, factor, _1, _2, boost::none, boost::none); + std::function f = std::bind( + &OrientedPlane3Factor::evaluateError, factor, std::placeholders::_1, + std::placeholders::_2, boost::none, boost::none); Matrix numericalH1 = numericalDerivative21(f, poseLin, pLin); Matrix numericalH2 = numericalDerivative22(f, poseLin, pLin); @@ -184,15 +184,15 @@ TEST( OrientedPlane3DirectionPrior, Constructor ) { // Calculate numerical derivatives Matrix expectedH1 = numericalDerivative11( - boost::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, _1, + std::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, std::placeholders::_1, boost::none), T1); Matrix expectedH2 = numericalDerivative11( - boost::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, _1, + std::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, std::placeholders::_1, boost::none), T2); Matrix expectedH3 = numericalDerivative11( - boost::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, _1, + std::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, std::placeholders::_1, boost::none), T3); // Use the factor to calculate the derivative diff --git a/gtsam/slam/tests/testReferenceFrameFactor.cpp b/gtsam/slam/tests/testReferenceFrameFactor.cpp index 075710ae7..b5800a414 100644 --- a/gtsam/slam/tests/testReferenceFrameFactor.cpp +++ b/gtsam/slam/tests/testReferenceFrameFactor.cpp @@ -16,8 +16,6 @@ #include -#include - #include #include @@ -32,7 +30,7 @@ using namespace std; using namespace boost; -using namespace boost::placeholders; +using namespace std::placeholders; using namespace gtsam; typedef gtsam::ReferenceFrameFactor PointReferenceFrameFactor; @@ -70,13 +68,13 @@ TEST( ReferenceFrameFactor, jacobians ) { Matrix numericalDT, numericalDL, numericalDF; numericalDF = numericalDerivative31( - boost::bind(evaluateError_, tc, _1, _2, _3), + std::bind(evaluateError_, tc, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), global, trans, local, 1e-5); numericalDT = numericalDerivative32( - boost::bind(evaluateError_, tc, _1, _2, _3), + std::bind(evaluateError_, tc, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), global, trans, local, 1e-5); numericalDL = numericalDerivative33( - boost::bind(evaluateError_, tc, _1, _2, _3), + std::bind(evaluateError_, tc, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), global, trans, local, 1e-5); EXPECT(assert_equal(numericalDF, actualDF)); @@ -102,13 +100,13 @@ TEST( ReferenceFrameFactor, jacobians_zero ) { Matrix numericalDT, numericalDL, numericalDF; numericalDF = numericalDerivative31( - boost::bind(evaluateError_, tc, _1, _2, _3), + std::bind(evaluateError_, tc, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), global, trans, local, 1e-5); numericalDT = numericalDerivative32( - boost::bind(evaluateError_, tc, _1, _2, _3), + std::bind(evaluateError_, tc, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), global, trans, local, 1e-5); numericalDL = numericalDerivative33( - boost::bind(evaluateError_, tc, _1, _2, _3), + std::bind(evaluateError_, tc, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), global, trans, local, 1e-5); EXPECT(assert_equal(numericalDF, actualDF)); diff --git a/gtsam/slam/tests/testRotateFactor.cpp b/gtsam/slam/tests/testRotateFactor.cpp index e4ecafd42..b8fd01730 100644 --- a/gtsam/slam/tests/testRotateFactor.cpp +++ b/gtsam/slam/tests/testRotateFactor.cpp @@ -13,12 +13,11 @@ #include #include -#include #include using namespace std; using namespace boost::assign; -using namespace boost::placeholders; +using namespace std::placeholders; using namespace gtsam; static const double kDegree = M_PI / 180; @@ -73,13 +72,13 @@ TEST (RotateFactor, test) { // Use numerical derivatives to calculate the expected Jacobian { expected = numericalDerivative11( - boost::bind(&RotateFactor::evaluateError, &f, _1, boost::none), iRc); + std::bind(&RotateFactor::evaluateError, &f, std::placeholders::_1, boost::none), iRc); f.evaluateError(iRc, actual); EXPECT(assert_equal(expected, actual, 1e-9)); } { expected = numericalDerivative11( - boost::bind(&RotateFactor::evaluateError, &f, _1, boost::none), R); + std::bind(&RotateFactor::evaluateError, &f, std::placeholders::_1, boost::none), R); f.evaluateError(R, actual); EXPECT(assert_equal(expected, actual, 1e-9)); } @@ -144,14 +143,14 @@ TEST (RotateDirectionsFactor, test) { // Use numerical derivatives to calculate the expected Jacobian { expected = numericalDerivative11( - boost::bind(&RotateDirectionsFactor::evaluateError, &f, _1, + std::bind(&RotateDirectionsFactor::evaluateError, &f, std::placeholders::_1, boost::none), iRc); f.evaluateError(iRc, actual); EXPECT(assert_equal(expected, actual, 1e-9)); } { expected = numericalDerivative11( - boost::bind(&RotateDirectionsFactor::evaluateError, &f, _1, + std::bind(&RotateDirectionsFactor::evaluateError, &f, std::placeholders::_1, boost::none), R); f.evaluateError(R, actual); EXPECT(assert_equal(expected, actual, 1e-9)); diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index 26caa6b75..c7f220c10 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -27,11 +27,10 @@ #include #include #include -#include #include using namespace boost::assign; -using namespace boost::placeholders; +using namespace std::placeholders; static const double rankTol = 1.0; // Create a noise model for the pixel error @@ -132,8 +131,8 @@ TEST( SmartProjectionPoseFactor, noiseless ) { EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1e-7); // Calculate expected derivative for point (easiest to check) - boost::function f = // - boost::bind(&SmartFactor::whitenedError, factor, cameras, _1); + std::function f = // + std::bind(&SmartFactor::whitenedError, factor, cameras, std::placeholders::_1); // Calculate using computeEP Matrix actualE; diff --git a/gtsam/slam/tests/testTriangulationFactor.cpp b/gtsam/slam/tests/testTriangulationFactor.cpp index 9fe087c2f..ad88c88fc 100644 --- a/gtsam/slam/tests/testTriangulationFactor.cpp +++ b/gtsam/slam/tests/testTriangulationFactor.cpp @@ -32,7 +32,7 @@ using namespace std; using namespace gtsam; using namespace boost::assign; -using namespace boost::placeholders; +using namespace std::placeholders; // Some common constants static const boost::shared_ptr sharedCal = // @@ -62,7 +62,7 @@ TEST( triangulation, TriangulationFactor ) { factor.evaluateError(landmark, HActual); Matrix HExpected = numericalDerivative11( - boost::bind(&Factor::evaluateError, &factor, _1, boost::none), landmark); + std::bind(&Factor::evaluateError, &factor, std::placeholders::_1, boost::none), landmark); // Verify the Jacobians are correct CHECK(assert_equal(HExpected, HActual, 1e-3)); @@ -86,13 +86,15 @@ TEST( triangulation, TriangulationFactorStereo ) { factor.evaluateError(landmark, HActual); Matrix HExpected = numericalDerivative11( - boost::bind(&Factor::evaluateError, &factor, _1, boost::none), landmark); + std::bind(&Factor::evaluateError, &factor, std::placeholders::_1, boost::none), landmark); // Verify the Jacobians are correct CHECK(assert_equal(HExpected, HActual, 1e-3)); // compare same problem against expression factor - Expression::UnaryFunction::type f = boost::bind(&StereoCamera::project2, camera2, _1, boost::none, _2); + Expression::UnaryFunction::type f = + std::bind(&StereoCamera::project2, camera2, std::placeholders::_1, + boost::none, std::placeholders::_2); Expression point_(pointKey); Expression project2_(f, point_); diff --git a/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp b/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp index ede11c7fb..882d5423a 100644 --- a/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp +++ b/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp @@ -3,14 +3,13 @@ * @author Duy-Nguyen Ta */ -#include #include #include #include #include /* ************************************************************************* */ -using namespace boost::placeholders; +using namespace std::placeholders; using namespace gtsam; using namespace gtsam::symbol_shorthand; @@ -58,19 +57,28 @@ TEST( Reconstruction, evaluateError) { assert_equal(Z_6x1, constraint.evaluateError(g2, g1, V1_g1, H1, H2, H3), tol)); Matrix numericalH1 = numericalDerivative31( - boost::function( - boost::bind(&Reconstruction::evaluateError, constraint, _1, _2, _3, - boost::none, boost::none, boost::none)), g2, g1, V1_g1, 1e-5); + std::function( + std::bind(&Reconstruction::evaluateError, constraint, + std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, boost::none, boost::none, + boost::none)), + g2, g1, V1_g1, 1e-5); Matrix numericalH2 = numericalDerivative32( - boost::function( - boost::bind(&Reconstruction::evaluateError, constraint, _1, _2, _3, - boost::none, boost::none, boost::none)), g2, g1, V1_g1, 1e-5); + std::function( + std::bind(&Reconstruction::evaluateError, constraint, + std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, boost::none, boost::none, + boost::none)), + g2, g1, V1_g1, 1e-5); Matrix numericalH3 = numericalDerivative33( - boost::function( - boost::bind(&Reconstruction::evaluateError, constraint, _1, _2, _3, - boost::none, boost::none, boost::none)), g2, g1, V1_g1, 1e-5); + std::function( + std::bind(&Reconstruction::evaluateError, constraint, + std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, boost::none, boost::none, + boost::none)), + g2, g1, V1_g1, 1e-5); EXPECT(assert_equal(numericalH1,H1,1e-5)); EXPECT(assert_equal(numericalH2,H2,1e-5)); @@ -111,22 +119,22 @@ TEST( DiscreteEulerPoincareHelicopter, evaluateError) { EXPECT(assert_equal(Z_6x1, constraint.evaluateError(expectedv2, V1_g1, g2, H1, H2, H3), 1e0)); Matrix numericalH1 = numericalDerivative31( - boost::function( - boost::bind(&DiscreteEulerPoincareHelicopter::evaluateError, constraint, _1, _2, _3, boost::none, boost::none, boost::none) + std::function( + std::bind(&DiscreteEulerPoincareHelicopter::evaluateError, constraint, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, boost::none, boost::none, boost::none) ), expectedv2, V1_g1, g2, 1e-5 ); Matrix numericalH2 = numericalDerivative32( - boost::function( - boost::bind(&DiscreteEulerPoincareHelicopter::evaluateError, constraint, _1, _2, _3, boost::none, boost::none, boost::none) + std::function( + std::bind(&DiscreteEulerPoincareHelicopter::evaluateError, constraint, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, boost::none, boost::none, boost::none) ), expectedv2, V1_g1, g2, 1e-5 ); Matrix numericalH3 = numericalDerivative33( - boost::function( - boost::bind(&DiscreteEulerPoincareHelicopter::evaluateError, constraint, _1, _2, _3, boost::none, boost::none, boost::none) + std::function( + std::bind(&DiscreteEulerPoincareHelicopter::evaluateError, constraint, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, boost::none, boost::none, boost::none) ), expectedv2, V1_g1, g2, 1e-5 ); diff --git a/gtsam_unstable/geometry/tests/testEvent.cpp b/gtsam_unstable/geometry/tests/testEvent.cpp index 3a599e6c5..f1bbc3970 100644 --- a/gtsam_unstable/geometry/tests/testEvent.cpp +++ b/gtsam_unstable/geometry/tests/testEvent.cpp @@ -23,9 +23,7 @@ #include -#include - -using namespace boost::placeholders; +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -67,11 +65,11 @@ TEST(Event, Derivatives) { Matrix13 actualH2; kToa(exampleEvent, microphoneAt0, actualH1, actualH2); Matrix expectedH1 = numericalDerivative11( - boost::bind(kToa, _1, microphoneAt0, boost::none, boost::none), + std::bind(kToa, std::placeholders::_1, microphoneAt0, boost::none, boost::none), exampleEvent); EXPECT(assert_equal(expectedH1, actualH1, 1e-8)); Matrix expectedH2 = numericalDerivative11( - boost::bind(kToa, exampleEvent, _1, boost::none, boost::none), + std::bind(kToa, exampleEvent, std::placeholders::_1, boost::none, boost::none), microphoneAt0); EXPECT(assert_equal(expectedH2, actualH2, 1e-8)); } diff --git a/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp b/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp index 70368cc0e..4d6e1912a 100644 --- a/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp +++ b/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp @@ -190,12 +190,12 @@ TEST (BetweenFactorEM, 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, key1, key2, f), p1, stepsize); - Matrix H2_expected = gtsam::numericalDerivative11(boost::bind(&predictionError, p1, _1, key1, key2, f), p2, stepsize); + Matrix H1_expected = gtsam::numericalDerivative11(std::bind(&predictionError, _1, p2, key1, key2, f), p1, stepsize); + Matrix H2_expected = gtsam::numericalDerivative11(std::bind(&predictionError, p1, _1, key1, key2, 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, key1, key2, h), p1, stepsize); + Matrix H1_expected_stnd = gtsam::numericalDerivative11(std::bind(&predictionError_standard, _1, p2, key1, key2, h), p1, stepsize); // CHECK( assert_equal(H1_expected_stnd, H1_actual_stnd, 1e-5)); // // diff --git a/gtsam_unstable/slam/tests/testBiasedGPSFactor.cpp b/gtsam_unstable/slam/tests/testBiasedGPSFactor.cpp index 0eb8b274b..59c4fdf53 100644 --- a/gtsam_unstable/slam/tests/testBiasedGPSFactor.cpp +++ b/gtsam_unstable/slam/tests/testBiasedGPSFactor.cpp @@ -10,10 +10,9 @@ #include #include -#include #include -using namespace boost::placeholders; +using namespace std::placeholders; using namespace gtsam; using namespace gtsam::symbol_shorthand; using namespace gtsam::noiseModel; @@ -68,15 +67,17 @@ TEST(BiasedGPSFactor, jacobian) { factor.evaluateError(pose,bias, actualH1, actualH2); Matrix numericalH1 = numericalDerivative21( - boost::function(boost::bind( - &BiasedGPSFactor::evaluateError, factor, _1, _2, boost::none, - boost::none)), pose, bias, 1e-5); + std::function(std::bind( + &BiasedGPSFactor::evaluateError, factor, std::placeholders::_1, + std::placeholders::_2, boost::none, boost::none)), + pose, bias, 1e-5); EXPECT(assert_equal(numericalH1,actualH1, 1E-5)); Matrix numericalH2 = numericalDerivative22( - boost::function(boost::bind( - &BiasedGPSFactor::evaluateError, factor, _1, _2, boost::none, - boost::none)), pose, bias, 1e-5); + std::function(std::bind( + &BiasedGPSFactor::evaluateError, factor, std::placeholders::_1, + std::placeholders::_2, boost::none, boost::none)), + pose, bias, 1e-5); EXPECT(assert_equal(numericalH2,actualH2, 1E-5)); } diff --git a/gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp b/gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp index 95b9b2f88..644283512 100644 --- a/gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp +++ b/gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp @@ -26,7 +26,7 @@ #include #include -using namespace boost::placeholders; +using namespace std::placeholders; using namespace std; using namespace gtsam; diff --git a/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp b/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp index 74134612d..8692cf584 100644 --- a/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp +++ b/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp @@ -23,9 +23,7 @@ #include #include -#include - -using namespace boost::placeholders; +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -108,9 +106,13 @@ TEST (GaussMarkovFactor, jacobian ) { // Calculate the Jacobian matrices H1 and H2 using the numerical derivative function Matrix numerical_H1, numerical_H2; numerical_H1 = numericalDerivative21( - boost::bind(&predictionError, _1, _2, factor), v1_upd, v2_upd); + std::bind(&predictionError, std::placeholders::_1, std::placeholders::_2, + factor), + v1_upd, v2_upd); numerical_H2 = numericalDerivative22( - boost::bind(&predictionError, _1, _2, factor), v1_upd, v2_upd); + std::bind(&predictionError, std::placeholders::_1, std::placeholders::_2, + factor), + v1_upd, v2_upd); // Verify they are equal for this choice of state CHECK( assert_equal(numerical_H1, computed_H1, 1e-9)); diff --git a/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp b/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp index aae59035b..e68b2fe5f 100644 --- a/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp +++ b/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp @@ -16,7 +16,6 @@ */ #include -#include #include #include #include @@ -26,7 +25,7 @@ #include #include -using namespace boost::placeholders; +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -251,7 +250,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, // // Vector3 v(predictionRq(angles, q)); // -// J_expected = numericalDerivative11(boost::bind(&predictionRq, _1, q), angles); +// J_expected = numericalDerivative11(std::bind(&predictionRq, std::placeholders::_1, q), angles); // // cout<<"J_hyp"<( - boost::bind(&predictionErrorPose, _1, Vel1, Bias1, Pose2, Vel2, factor), + std::bind(&predictionErrorPose, std::placeholders::_1, Vel1, Bias1, Pose2, Vel2, factor), Pose1); H2_expectedPose = numericalDerivative11( - boost::bind(&predictionErrorPose, Pose1, _1, Bias1, Pose2, Vel2, factor), + std::bind(&predictionErrorPose, Pose1, std::placeholders::_1, Bias1, Pose2, Vel2, factor), Vel1); H3_expectedPose = numericalDerivative11( - boost::bind(&predictionErrorPose, Pose1, Vel1, _1, Pose2, Vel2, factor), + std::bind(&predictionErrorPose, Pose1, Vel1, std::placeholders::_1, Pose2, Vel2, factor), Bias1); H4_expectedPose = numericalDerivative11( - boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, _1, Vel2, factor), + std::bind(&predictionErrorPose, Pose1, Vel1, Bias1, std::placeholders::_1, Vel2, factor), Pose2); H5_expectedPose = numericalDerivative11( - boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, Pose2, _1, factor), + std::bind(&predictionErrorPose, Pose1, Vel1, Bias1, Pose2, std::placeholders::_1, factor), Vel2); // Verify they are equal for this choice of state @@ -346,19 +345,19 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, Matrix H1_expectedVel, H2_expectedVel, H3_expectedVel, H4_expectedVel, H5_expectedVel; H1_expectedVel = numericalDerivative11( - boost::bind(&predictionErrorVel, _1, Vel1, Bias1, Pose2, Vel2, factor), + std::bind(&predictionErrorVel, std::placeholders::_1, Vel1, Bias1, Pose2, Vel2, factor), Pose1); H2_expectedVel = numericalDerivative11( - boost::bind(&predictionErrorVel, Pose1, _1, Bias1, Pose2, Vel2, factor), + std::bind(&predictionErrorVel, Pose1, std::placeholders::_1, Bias1, Pose2, Vel2, factor), Vel1); H3_expectedVel = numericalDerivative11( - boost::bind(&predictionErrorVel, Pose1, Vel1, _1, Pose2, Vel2, factor), + std::bind(&predictionErrorVel, Pose1, Vel1, std::placeholders::_1, Pose2, Vel2, factor), Bias1); H4_expectedVel = numericalDerivative11( - boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, _1, Vel2, factor), + std::bind(&predictionErrorVel, Pose1, Vel1, Bias1, std::placeholders::_1, Vel2, factor), Pose2); H5_expectedVel = numericalDerivative11( - boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, Pose2, _1, factor), + std::bind(&predictionErrorVel, Pose1, Vel1, Bias1, Pose2, std::placeholders::_1, factor), Vel2); // Verify they are equal for this choice of state @@ -644,19 +643,19 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, Matrix H1_expectedPose, H2_expectedPose, H3_expectedPose, H4_expectedPose, H5_expectedPose; H1_expectedPose = numericalDerivative11( - boost::bind(&predictionErrorPose, _1, Vel1, Bias1, Pose2, Vel2, factor), + std::bind(&predictionErrorPose, std::placeholders::_1, Vel1, Bias1, Pose2, Vel2, factor), Pose1); H2_expectedPose = numericalDerivative11( - boost::bind(&predictionErrorPose, Pose1, _1, Bias1, Pose2, Vel2, factor), + std::bind(&predictionErrorPose, Pose1, std::placeholders::_1, Bias1, Pose2, Vel2, factor), Vel1); H3_expectedPose = numericalDerivative11( - boost::bind(&predictionErrorPose, Pose1, Vel1, _1, Pose2, Vel2, factor), + std::bind(&predictionErrorPose, Pose1, Vel1, std::placeholders::_1, Pose2, Vel2, factor), Bias1); H4_expectedPose = numericalDerivative11( - boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, _1, Vel2, factor), + std::bind(&predictionErrorPose, Pose1, Vel1, Bias1, std::placeholders::_1, Vel2, factor), Pose2); H5_expectedPose = numericalDerivative11( - boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, Pose2, _1, factor), + std::bind(&predictionErrorPose, Pose1, Vel1, Bias1, Pose2, std::placeholders::_1, factor), Vel2); // Verify they are equal for this choice of state @@ -678,19 +677,19 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, Matrix H1_expectedVel, H2_expectedVel, H3_expectedVel, H4_expectedVel, H5_expectedVel; H1_expectedVel = numericalDerivative11( - boost::bind(&predictionErrorVel, _1, Vel1, Bias1, Pose2, Vel2, factor), + std::bind(&predictionErrorVel, std::placeholders::_1, Vel1, Bias1, Pose2, Vel2, factor), Pose1); H2_expectedVel = numericalDerivative11( - boost::bind(&predictionErrorVel, Pose1, _1, Bias1, Pose2, Vel2, factor), + std::bind(&predictionErrorVel, Pose1, std::placeholders::_1, Bias1, Pose2, Vel2, factor), Vel1); H3_expectedVel = numericalDerivative11( - boost::bind(&predictionErrorVel, Pose1, Vel1, _1, Pose2, Vel2, factor), + std::bind(&predictionErrorVel, Pose1, Vel1, std::placeholders::_1, Pose2, Vel2, factor), Bias1); H4_expectedVel = numericalDerivative11( - boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, _1, Vel2, factor), + std::bind(&predictionErrorVel, Pose1, Vel1, Bias1, std::placeholders::_1, Vel2, factor), Pose2); H5_expectedVel = numericalDerivative11( - boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, Pose2, _1, factor), + std::bind(&predictionErrorVel, Pose1, Vel1, Bias1, Pose2, std::placeholders::_1, factor), Vel2); // Verify they are equal for this choice of state diff --git a/gtsam_unstable/slam/tests/testLocalOrientedPlane3Factor.cpp b/gtsam_unstable/slam/tests/testLocalOrientedPlane3Factor.cpp index 6bbfb55ae..b97d56c7e 100644 --- a/gtsam_unstable/slam/tests/testLocalOrientedPlane3Factor.cpp +++ b/gtsam_unstable/slam/tests/testLocalOrientedPlane3Factor.cpp @@ -24,9 +24,7 @@ #include -#include - -using namespace boost::placeholders; +using namespace std::placeholders; using namespace gtsam; using namespace std; @@ -143,8 +141,10 @@ TEST(LocalOrientedPlane3Factor, Derivatives) { LocalOrientedPlane3Factor factor(p, noise, poseKey, anchorPoseKey, planeKey); // Calculate numerical derivatives - auto f = boost::bind(&LocalOrientedPlane3Factor::evaluateError, factor, - _1, _2, _3, boost::none, boost::none, boost::none); + auto f = + std::bind(&LocalOrientedPlane3Factor::evaluateError, factor, + std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, boost::none, boost::none, boost::none); Matrix numericalH1 = numericalDerivative31(f, poseLin, anchorPoseLin, pLin); Matrix numericalH2 = numericalDerivative32 #include -#include #include -using namespace boost::placeholders; +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -83,7 +82,9 @@ TEST(PartialPriorFactor, JacobianPartialTranslation2) { // Calculate numerical derivatives. Matrix expectedH1 = numericalDerivative11( - boost::bind(&TestPartialPriorFactor2::evaluateError, &factor, _1, boost::none), pose); + std::bind(&TestPartialPriorFactor2::evaluateError, &factor, + std::placeholders::_1, boost::none), + pose); // Use the factor to calculate the derivative. Matrix actualH1; @@ -99,13 +100,16 @@ TEST(PartialPriorFactor, JacobianFullTranslation2) { Pose2 measurement(-6.0, 3.5, 0.123); // Prior on x component of translation. - TestPartialPriorFactor2 factor(poseKey, { 0, 1 }, measurement.translation(), NM::Isotropic::Sigma(2, 0.25)); + TestPartialPriorFactor2 factor(poseKey, {0, 1}, measurement.translation(), + NM::Isotropic::Sigma(2, 0.25)); Pose2 pose = measurement; // Zero-error linearization point. // Calculate numerical derivatives. Matrix expectedH1 = numericalDerivative11( - boost::bind(&TestPartialPriorFactor2::evaluateError, &factor, _1, boost::none), pose); + std::bind(&TestPartialPriorFactor2::evaluateError, &factor, + std::placeholders::_1, boost::none), + pose); // Use the factor to calculate the derivative. Matrix actualH1; @@ -127,7 +131,7 @@ TEST(PartialPriorFactor, JacobianTheta) { // Calculate numerical derivatives. Matrix expectedH1 = numericalDerivative11( - boost::bind(&TestPartialPriorFactor2::evaluateError, &factor, _1, boost::none), pose); + std::bind(&TestPartialPriorFactor2::evaluateError, &factor, std::placeholders::_1, boost::none), pose); // Use the factor to calculate the derivative. Matrix actualH1; @@ -178,7 +182,7 @@ TEST(PartialPriorFactor, JacobianAtIdentity3) { // Calculate numerical derivatives. Matrix expectedH1 = numericalDerivative11( - boost::bind(&TestPartialPriorFactor3::evaluateError, &factor, _1, boost::none), pose); + std::bind(&TestPartialPriorFactor3::evaluateError, &factor, std::placeholders::_1, boost::none), pose); // Use the factor to calculate the derivative. Matrix actualH1; @@ -200,7 +204,7 @@ TEST(PartialPriorFactor, JacobianPartialTranslation3) { // Calculate numerical derivatives. Matrix expectedH1 = numericalDerivative11( - boost::bind(&TestPartialPriorFactor3::evaluateError, &factor, _1, boost::none), pose); + std::bind(&TestPartialPriorFactor3::evaluateError, &factor, std::placeholders::_1, boost::none), pose); // Use the factor to calculate the derivative. Matrix actualH1; @@ -224,7 +228,7 @@ TEST(PartialPriorFactor, JacobianFullTranslation3) { // Calculate numerical derivatives. Matrix expectedH1 = numericalDerivative11( - boost::bind(&TestPartialPriorFactor3::evaluateError, &factor, _1, boost::none), pose); + std::bind(&TestPartialPriorFactor3::evaluateError, &factor, std::placeholders::_1, boost::none), pose); // Use the factor to calculate the derivative. Matrix actualH1; @@ -248,7 +252,7 @@ TEST(PartialPriorFactor, JacobianTxTz3) { // Calculate numerical derivatives. Matrix expectedH1 = numericalDerivative11( - boost::bind(&TestPartialPriorFactor3::evaluateError, &factor, _1, boost::none), pose); + std::bind(&TestPartialPriorFactor3::evaluateError, &factor, std::placeholders::_1, boost::none), pose); // Use the factor to calculate the derivative. Matrix actualH1; @@ -271,7 +275,7 @@ TEST(PartialPriorFactor, JacobianFullRotation3) { // Calculate numerical derivatives. Matrix expectedH1 = numericalDerivative11( - boost::bind(&TestPartialPriorFactor3::evaluateError, &factor, _1, boost::none), pose); + std::bind(&TestPartialPriorFactor3::evaluateError, &factor, std::placeholders::_1, boost::none), pose); // Use the factor to calculate the derivative. Matrix actualH1; diff --git a/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp b/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp index 6f7725eed..cd5bf1d9e 100644 --- a/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp +++ b/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp @@ -22,10 +22,9 @@ #include #include -#include #include -using namespace boost::placeholders; +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -199,8 +198,14 @@ 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( + std::bind(&TestPoseBetweenFactor::evaluateError, &factor, + std::placeholders::_1, pose2, boost::none, boost::none), + pose1); + Matrix expectedH2 = numericalDerivative11( + std::bind(&TestPoseBetweenFactor::evaluateError, &factor, pose1, + std::placeholders::_1, boost::none, boost::none), + pose2); // Use the factor to calculate the derivative Matrix actualH1; @@ -228,8 +233,14 @@ 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( + std::bind(&TestPoseBetweenFactor::evaluateError, &factor, + std::placeholders::_1, pose2, boost::none, boost::none), + pose1); + Matrix expectedH2 = numericalDerivative11( + std::bind(&TestPoseBetweenFactor::evaluateError, &factor, pose1, + std::placeholders::_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 cc2615517..dbbc02a8b 100644 --- a/gtsam_unstable/slam/tests/testPosePriorFactor.cpp +++ b/gtsam_unstable/slam/tests/testPosePriorFactor.cpp @@ -22,10 +22,9 @@ #include #include -#include #include -using namespace boost::placeholders; +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -188,7 +187,10 @@ 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( + std::bind(&TestPosePriorFactor::evaluateError, &factor, + std::placeholders::_1, boost::none), + pose); // Use the factor to calculate the derivative Matrix actualH1; @@ -212,7 +214,10 @@ 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( + std::bind(&TestPosePriorFactor::evaluateError, &factor, + std::placeholders::_1, boost::none), + pose); // Use the factor to calculate the derivative Matrix actualH1; diff --git a/gtsam_unstable/slam/tests/testPoseToPointFactor.h b/gtsam_unstable/slam/tests/testPoseToPointFactor.h index 8f8563e9d..e0e5c4581 100644 --- a/gtsam_unstable/slam/tests/testPoseToPointFactor.h +++ b/gtsam_unstable/slam/tests/testPoseToPointFactor.h @@ -63,7 +63,7 @@ TEST(PoseToPointFactor, jacobian) { PoseToPointFactor factor(pose_key, point_key, l_meas, noise); // Calculate numerical derivatives - auto f = boost::bind(&PoseToPointFactor::evaluateError, factor, _1, _2, + auto f = std::bind(&PoseToPointFactor::evaluateError, factor, _1, _2, boost::none, boost::none); Matrix numerical_H1 = numericalDerivative21(f, p, l); Matrix numerical_H2 = numericalDerivative22(f, p, l); diff --git a/gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp b/gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp index 8a65e5e57..c05f83a23 100644 --- a/gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp +++ b/gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp @@ -28,9 +28,7 @@ #include -#include - -using namespace boost::placeholders; +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -178,10 +176,13 @@ TEST( ProjectionFactorPPP, Jacobian ) { CHECK(assert_equal(H3Expected, H3Actual, 1e-3)); // Verify H2 with numerical derivative - Matrix H2Expected = numericalDerivative32( - boost::function( - boost::bind(&TestProjectionFactor::evaluateError, &factor, _1, _2, _3, - boost::none, boost::none, boost::none)), pose, Pose3(), point); + Matrix H2Expected = numericalDerivative32( + std::function( + std::bind(&TestProjectionFactor::evaluateError, &factor, + std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, boost::none, boost::none, + boost::none)), + pose, Pose3(), point); CHECK(assert_equal(H2Expected, H2Actual, 1e-5)); } @@ -214,9 +215,12 @@ TEST( ProjectionFactorPPP, JacobianWithTransform ) { // Verify H2 with numerical derivative Matrix H2Expected = numericalDerivative32( - boost::function( - boost::bind(&TestProjectionFactor::evaluateError, &factor, _1, _2, _3, - boost::none, boost::none, boost::none)), pose, body_P_sensor, point); + std::function( + std::bind(&TestProjectionFactor::evaluateError, &factor, + std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, boost::none, boost::none, + boost::none)), + pose, body_P_sensor, point); CHECK(assert_equal(H2Expected, H2Actual, 1e-5)); diff --git a/gtsam_unstable/slam/tests/testProjectionFactorPPPC.cpp b/gtsam_unstable/slam/tests/testProjectionFactorPPPC.cpp index 232f8de17..6490dfc75 100644 --- a/gtsam_unstable/slam/tests/testProjectionFactorPPPC.cpp +++ b/gtsam_unstable/slam/tests/testProjectionFactorPPPC.cpp @@ -28,9 +28,7 @@ #include -#include - -using namespace boost::placeholders; +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -138,12 +136,16 @@ TEST( ProjectionFactorPPPC, Jacobian ) { // Verify H2 and H4 with numerical derivatives Matrix H2Expected = numericalDerivative11( - boost::bind(&TestProjectionFactor::evaluateError, &factor, pose, _1, point, - *K1, boost::none, boost::none, boost::none, boost::none), Pose3()); + std::bind(&TestProjectionFactor::evaluateError, &factor, pose, + std::placeholders::_1, point, *K1, boost::none, boost::none, + boost::none, boost::none), + Pose3()); Matrix H4Expected = numericalDerivative11( - boost::bind(&TestProjectionFactor::evaluateError, &factor, pose, Pose3(), point, - _1, boost::none, boost::none, boost::none, boost::none), *K1); + std::bind(&TestProjectionFactor::evaluateError, &factor, pose, Pose3(), + point, std::placeholders::_1, boost::none, boost::none, + boost::none, boost::none), + *K1); CHECK(assert_equal(H2Expected, H2Actual, 1e-5)); CHECK(assert_equal(H4Expected, H4Actual, 1e-5)); @@ -174,12 +176,12 @@ TEST( ProjectionFactorPPPC, JacobianWithTransform ) { // Verify H2 and H4 with numerical derivatives Matrix H2Expected = numericalDerivative11( - boost::bind(&TestProjectionFactor::evaluateError, &factor, pose, _1, point, + std::bind(&TestProjectionFactor::evaluateError, &factor, pose, std::placeholders::_1, point, *K1, boost::none, boost::none, boost::none, boost::none), body_P_sensor); Matrix H4Expected = numericalDerivative11( - boost::bind(&TestProjectionFactor::evaluateError, &factor, pose, body_P_sensor, point, - _1, boost::none, boost::none, boost::none, boost::none), *K1); + std::bind(&TestProjectionFactor::evaluateError, &factor, pose, body_P_sensor, point, + std::placeholders::_1, boost::none, boost::none, boost::none, boost::none), *K1); CHECK(assert_equal(H2Expected, H2Actual, 1e-5)); CHECK(assert_equal(H4Expected, H4Actual, 1e-5)); diff --git a/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp b/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp index 2fda9debb..25ca3339b 100644 --- a/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp +++ b/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp @@ -5,14 +5,13 @@ * @author Alex Cunningham */ -#include #include #include #include -using namespace boost::placeholders; +using namespace std::placeholders; using namespace gtsam; SharedNoiseModel model1 = noiseModel::Unit::Create(1); @@ -37,10 +36,14 @@ TEST( testRelativeElevationFactor, level_zero_error ) { RelativeElevationFactor factor(poseKey, pointKey, measured, model1); Matrix actH1, actH2; EXPECT(assert_equal(Z_1x1, factor.evaluateError(pose1, point1, actH1, actH2))); - Matrix expH1 = numericalDerivative21( - boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5); - Matrix expH2 = numericalDerivative22( - boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5); + Matrix expH1 = numericalDerivative21( + std::bind(evalFactorError, factor, std::placeholders::_1, + std::placeholders::_2), + pose1, point1, 1e-5); + Matrix expH2 = numericalDerivative22( + std::bind(evalFactorError, factor, std::placeholders::_1, + std::placeholders::_2), + pose1, point1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH2, actH2, tol)); } @@ -52,10 +55,14 @@ TEST( testRelativeElevationFactor, level_positive ) { RelativeElevationFactor factor(poseKey, pointKey, measured, model1); Matrix actH1, actH2; EXPECT(assert_equal((Vector(1) << 2.0).finished(), factor.evaluateError(pose1, point1, actH1, actH2))); - Matrix expH1 = numericalDerivative21( - boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5); - Matrix expH2 = numericalDerivative22( - boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5); + Matrix expH1 = numericalDerivative21( + std::bind(evalFactorError, factor, std::placeholders::_1, + std::placeholders::_2), + pose1, point1, 1e-5); + Matrix expH2 = numericalDerivative22( + std::bind(evalFactorError, factor, std::placeholders::_1, + std::placeholders::_2), + pose1, point1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH2, actH2, tol)); } @@ -67,10 +74,14 @@ TEST( testRelativeElevationFactor, level_negative ) { RelativeElevationFactor factor(poseKey, pointKey, measured, model1); Matrix actH1, actH2; EXPECT(assert_equal((Vector(1) << 3.0).finished(), factor.evaluateError(pose1, point1, actH1, actH2))); - Matrix expH1 = numericalDerivative21( - boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5); - Matrix expH2 = numericalDerivative22( - boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5); + Matrix expH1 = numericalDerivative21( + std::bind(evalFactorError, factor, std::placeholders::_1, + std::placeholders::_2), + pose1, point1, 1e-5); + Matrix expH2 = numericalDerivative22( + std::bind(evalFactorError, factor, std::placeholders::_1, + std::placeholders::_2), + pose1, point1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH2, actH2, tol)); } @@ -82,10 +93,14 @@ TEST( testRelativeElevationFactor, rotated_zero_error ) { RelativeElevationFactor factor(poseKey, pointKey, measured, model1); Matrix actH1, actH2; EXPECT(assert_equal(Z_1x1, factor.evaluateError(pose2, point1, actH1, actH2))); - Matrix expH1 = numericalDerivative21( - boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5); - Matrix expH2 = numericalDerivative22( - boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5); + Matrix expH1 = numericalDerivative21( + std::bind(evalFactorError, factor, std::placeholders::_1, + std::placeholders::_2), + pose2, point1, 1e-5); + Matrix expH2 = numericalDerivative22( + std::bind(evalFactorError, factor, std::placeholders::_1, + std::placeholders::_2), + pose2, point1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH2, actH2, tol)); } @@ -97,10 +112,14 @@ TEST( testRelativeElevationFactor, rotated_positive ) { RelativeElevationFactor factor(poseKey, pointKey, measured, model1); Matrix actH1, actH2; EXPECT(assert_equal((Vector(1) << 2.0).finished(), factor.evaluateError(pose2, point1, actH1, actH2))); - Matrix expH1 = numericalDerivative21( - boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5); - Matrix expH2 = numericalDerivative22( - boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5); + Matrix expH1 = numericalDerivative21( + std::bind(evalFactorError, factor, std::placeholders::_1, + std::placeholders::_2), + pose2, point1, 1e-5); + Matrix expH2 = numericalDerivative22( + std::bind(evalFactorError, factor, std::placeholders::_1, + std::placeholders::_2), + pose2, point1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH2, actH2, tol)); } @@ -112,10 +131,14 @@ TEST( testRelativeElevationFactor, rotated_negative1 ) { RelativeElevationFactor factor(poseKey, pointKey, measured, model1); Matrix actH1, actH2; EXPECT(assert_equal((Vector(1) << 3.0).finished(), factor.evaluateError(pose2, point1, actH1, actH2))); - Matrix expH1 = numericalDerivative21( - boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5); - Matrix expH2 = numericalDerivative22( - boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5); + Matrix expH1 = numericalDerivative21( + std::bind(evalFactorError, factor, std::placeholders::_1, + std::placeholders::_2), + pose2, point1, 1e-5); + Matrix expH2 = numericalDerivative22( + std::bind(evalFactorError, factor, std::placeholders::_1, + std::placeholders::_2), + pose2, point1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH2, actH2, tol)); } @@ -127,10 +150,14 @@ TEST( testRelativeElevationFactor, rotated_negative2 ) { RelativeElevationFactor factor(poseKey, pointKey, measured, model1); Matrix actH1, actH2; EXPECT(assert_equal((Vector(1) << 3.0).finished(), factor.evaluateError(pose3, point1, actH1, actH2))); - Matrix expH1 = numericalDerivative21( - boost::bind(evalFactorError, factor, _1, _2), pose3, point1, 1e-5); - Matrix expH2 = numericalDerivative22( - boost::bind(evalFactorError, factor, _1, _2), pose3, point1, 1e-5); + Matrix expH1 = numericalDerivative21( + std::bind(evalFactorError, factor, std::placeholders::_1, + std::placeholders::_2), + pose3, point1, 1e-5); + Matrix expH2 = numericalDerivative22( + std::bind(evalFactorError, factor, std::placeholders::_1, + std::placeholders::_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 77f82bca4..fbb21e191 100644 --- a/gtsam_unstable/slam/tests/testTSAMFactors.cpp +++ b/gtsam_unstable/slam/tests/testTSAMFactors.cpp @@ -19,10 +19,9 @@ #include #include -#include #include -using namespace boost::placeholders; +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -48,10 +47,10 @@ TEST( DeltaFactor, all ) { // Use numerical derivatives to calculate the Jacobians Matrix H1Expected, H2Expected; H1Expected = numericalDerivative11( - boost::bind(&DeltaFactor::evaluateError, &factor, _1, point, boost::none, + std::bind(&DeltaFactor::evaluateError, &factor, std::placeholders::_1, point, boost::none, boost::none), pose); H2Expected = numericalDerivative11( - boost::bind(&DeltaFactor::evaluateError, &factor, pose, _1, boost::none, + std::bind(&DeltaFactor::evaluateError, &factor, pose, std::placeholders::_1, boost::none, boost::none), point); // Verify the Jacobians are correct @@ -82,17 +81,17 @@ TEST( DeltaFactorBase, all ) { // Use numerical derivatives to calculate the Jacobians Matrix H1Expected, H2Expected, H3Expected, H4Expected; H1Expected = numericalDerivative11( - boost::bind(&DeltaFactorBase::evaluateError, &factor, _1, pose, base2, + std::bind(&DeltaFactorBase::evaluateError, &factor, std::placeholders::_1, pose, base2, point, boost::none, boost::none, boost::none, boost::none), base1); H2Expected = numericalDerivative11( - boost::bind(&DeltaFactorBase::evaluateError, &factor, base1, _1, base2, + std::bind(&DeltaFactorBase::evaluateError, &factor, base1, std::placeholders::_1, base2, point, boost::none, boost::none, boost::none, boost::none), pose); H3Expected = numericalDerivative11( - boost::bind(&DeltaFactorBase::evaluateError, &factor, base1, pose, _1, + std::bind(&DeltaFactorBase::evaluateError, &factor, base1, pose, std::placeholders::_1, point, boost::none, boost::none, boost::none, boost::none), base2); H4Expected = numericalDerivative11( - boost::bind(&DeltaFactorBase::evaluateError, &factor, base1, pose, base2, - _1, boost::none, boost::none, boost::none, boost::none), point); + std::bind(&DeltaFactorBase::evaluateError, &factor, base1, pose, base2, + std::placeholders::_1, boost::none, boost::none, boost::none, boost::none), point); // Verify the Jacobians are correct EXPECT(assert_equal(H1Expected, H1Actual, 1e-9)); @@ -123,17 +122,17 @@ TEST( OdometryFactorBase, all ) { // Use numerical derivatives to calculate the Jacobians Matrix H1Expected, H2Expected, H3Expected, H4Expected; H1Expected = numericalDerivative11( - boost::bind(&OdometryFactorBase::evaluateError, &factor, _1, pose1, base2, + std::bind(&OdometryFactorBase::evaluateError, &factor, std::placeholders::_1, pose1, base2, pose2, boost::none, boost::none, boost::none, boost::none), base1); H2Expected = numericalDerivative11( - boost::bind(&OdometryFactorBase::evaluateError, &factor, base1, _1, base2, + std::bind(&OdometryFactorBase::evaluateError, &factor, base1, std::placeholders::_1, base2, pose2, boost::none, boost::none, boost::none, boost::none), pose1); H3Expected = numericalDerivative11( - boost::bind(&OdometryFactorBase::evaluateError, &factor, base1, pose1, _1, + std::bind(&OdometryFactorBase::evaluateError, &factor, base1, pose1, std::placeholders::_1, pose2, boost::none, boost::none, boost::none, boost::none), base2); H4Expected = numericalDerivative11( - boost::bind(&OdometryFactorBase::evaluateError, &factor, base1, pose1, - base2, _1, boost::none, boost::none, boost::none, boost::none), + std::bind(&OdometryFactorBase::evaluateError, &factor, base1, pose1, + base2, std::placeholders::_1, boost::none, boost::none, boost::none, boost::none), pose2); // Verify the Jacobians are correct diff --git a/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp b/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp index d9e945b78..36914f88f 100644 --- a/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp +++ b/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp @@ -18,9 +18,7 @@ #include #include -#include - -using namespace boost::placeholders; +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -233,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(std::bind(&predictionError, std::placeholders::_1, key, g), orgA_T_orgB, stepsize); // CHECK( assert_equal(H1_expected, H1_actual, 1e-5)); } @@ -287,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(std::bind(&predictionError, std::placeholders::_1, p2, keyA, keyB, f), p1, stepsize); +// Matrix H2_expected = gtsam::numericalDerivative11(std::bind(&predictionError, p1, std::placeholders::_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(std::bind(&predictionError_standard, std::placeholders::_1, p2, keyA, keyB, h), p1, stepsize); // CHECK( assert_equal(H1_expected_stnd, H1_actual_stnd, 1e-5)); // // diff --git a/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp b/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp index 2fd282091..657a9fb34 100644 --- a/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp +++ b/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp @@ -18,9 +18,7 @@ #include #include -#include - -using namespace boost::placeholders; +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -262,8 +260,10 @@ 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); -// CHECK( assert_equal(H1_expected, H1_actual, 1e-5)); + Matrix H1_expected = gtsam::numericalDerivative11( + std::bind(&predictionError, std::placeholders::_1, key, g), orgA_T_orgB, + stepsize); + // CHECK( assert_equal(H1_expected, H1_actual, 1e-5)); } /////* ************************************************************************** */ //TEST (TransformBtwRobotsUnaryFactorEM, jacobian ) { @@ -312,12 +312,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(std::bind(&predictionError, std::placeholders::_1, p2, keyA, keyB, f), p1, stepsize); +// Matrix H2_expected = gtsam::numericalDerivative11(std::bind(&predictionError, p1, std::placeholders::_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(std::bind(&predictionError_standard, std::placeholders::_1, p2, keyA, keyB, h), p1, stepsize); // CHECK( assert_equal(H1_expected_stnd, H1_actual_stnd, 1e-5)); // // diff --git a/tests/testExpressionFactor.cpp b/tests/testExpressionFactor.cpp index 6bb5751bf..e9eac22eb 100644 --- a/tests/testExpressionFactor.cpp +++ b/tests/testExpressionFactor.cpp @@ -17,22 +17,19 @@ * @brief unit tests for Block Automatic Differentiation */ -#include -#include -#include -#include -#include +#include +#include #include #include +#include #include -#include - -#include +#include +#include +#include #include using boost::assign::list_of; -#include -using namespace boost::placeholders; +using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -621,9 +618,10 @@ TEST(ExpressionFactor, MultiplyWithInverseFunction) { Matrix3 A; const Vector Ab = f(a, b, H1, A); CHECK(assert_equal(A * b, Ab)); - CHECK(assert_equal(numericalDerivative11( - boost::bind(f, _1, b, boost::none, boost::none), a), - H1)); + CHECK(assert_equal( + numericalDerivative11( + std::bind(f, std::placeholders::_1, b, boost::none, boost::none), a), + H1)); Values values; values.insert(0, a); diff --git a/tests/testSimulated3D.cpp b/tests/testSimulated3D.cpp index 342c353bc..2bc381f7a 100644 --- a/tests/testSimulated3D.cpp +++ b/tests/testSimulated3D.cpp @@ -26,7 +26,7 @@ #include -using namespace boost::placeholders; +using namespace std::placeholders; using namespace gtsam; // Convenience for named keys @@ -46,7 +46,7 @@ TEST( simulated3D, Values ) TEST( simulated3D, Dprior ) { Point3 x(1,-9, 7); - Matrix numerical = numericalDerivative11(boost::bind(simulated3D::prior, _1, boost::none),x); + Matrix numerical = numericalDerivative11(std::bind(simulated3D::prior, std::placeholders::_1, boost::none),x); Matrix computed; simulated3D::prior(x,computed); EXPECT(assert_equal(numerical,computed,1e-9)); @@ -55,13 +55,19 @@ TEST( simulated3D, Dprior ) /* ************************************************************************* */ TEST( simulated3D, DOdo ) { - Point3 x1(1,-9,7),x2(-5,6,7); - Matrix H1,H2; - simulated3D::odo(x1,x2,H1,H2); - Matrix A1 = numericalDerivative21(boost::bind(simulated3D::odo, _1, _2, boost::none, boost::none),x1,x2); - EXPECT(assert_equal(A1,H1,1e-9)); - Matrix A2 = numericalDerivative22(boost::bind(simulated3D::odo, _1, _2, boost::none, boost::none),x1,x2); - EXPECT(assert_equal(A2,H2,1e-9)); + Point3 x1(1, -9, 7), x2(-5, 6, 7); + Matrix H1, H2; + simulated3D::odo(x1, x2, H1, H2); + Matrix A1 = numericalDerivative21( + std::bind(simulated3D::odo, std::placeholders::_1, std::placeholders::_2, + boost::none, boost::none), + x1, x2); + EXPECT(assert_equal(A1, H1, 1e-9)); + Matrix A2 = numericalDerivative22( + std::bind(simulated3D::odo, std::placeholders::_1, std::placeholders::_2, + boost::none, boost::none), + x1, x2); + EXPECT(assert_equal(A2, H2, 1e-9)); } From ddfb45efb03544636f69268f85a703816a730c57 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Sun, 11 Jul 2021 01:02:36 -0600 Subject: [PATCH 039/248] fix typo in block indexing, 3x3 covariance for Pose2 should have just 1x1 block for theta --- gtsam/sfm/ShonanAveraging.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp index 88c393f16..76fd1bfc7 100644 --- a/gtsam/sfm/ShonanAveraging.cpp +++ b/gtsam/sfm/ShonanAveraging.cpp @@ -955,7 +955,7 @@ static BinaryMeasurement convertPose2ToBinaryMeasurementRot2( "parseMeasurements can only convert Pose2 measurements " "with Gaussian noise models."); const Matrix3 M = gaussian->covariance(); - auto model = noiseModel::Gaussian::Covariance(M.block<2, 2>(2, 2)); + auto model = noiseModel::Gaussian::Covariance(M.block<1, 1>(2, 2)); return BinaryMeasurement(f->key1(), f->key2(), f->measured().rotation(), model); } From 941594c94be1d1223d7fbcf9d1181f2c73411574 Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Sun, 11 Jul 2021 14:11:40 +0200 Subject: [PATCH 040/248] Testing CameraSet and triangulatePoint3 Currently triangulatePoint3 returns wrong results for fisheye models. The template for PinholePose may be implemented for a fixed size of variable dimensions. --- python/gtsam/tests/test_Cal3Fisheye.py | 40 +++++++++++++++++++++++++- 1 file changed, 39 insertions(+), 1 deletion(-) diff --git a/python/gtsam/tests/test_Cal3Fisheye.py b/python/gtsam/tests/test_Cal3Fisheye.py index 23f7a9b8c..d731204ef 100644 --- a/python/gtsam/tests/test_Cal3Fisheye.py +++ b/python/gtsam/tests/test_Cal3Fisheye.py @@ -29,7 +29,7 @@ class TestCal3Fisheye(GtsamTestCase): image plane and theta the incident angle of the object point. """ x, y, z = 1.0, 0.0, 1.0 - # x, y, z = 0.5, 0.0, 2.0 <== Note: this example fails! + # x, y, z = 0.5, 0.0, 2.0 u, v = np.arctan2(x, z), 0.0 cls.obj_point = np.array([x, y, z]) cls.img_point = np.array([u, v]) @@ -93,6 +93,44 @@ class TestCal3Fisheye(GtsamTestCase): score = graph.error(state) self.assertAlmostEqual(score, 0) + @unittest.skip("triangulatePoint3 currently seems to require perspective projections.") + def test_triangulation_skipped(self): + """Estimate spatial point from image measurements""" + p1 = [-1.0, 0.0, -1.0] + p2 = [ 1.0, 0.0, -1.0] + q1 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0) + q2 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0) + obj_point = np.array([0.0, 0.0, 0.0]) + pose1 = gtsam.Pose3(q1, p1) + pose2 = gtsam.Pose3(q2, p2) + camera1 = gtsam.PinholeCameraCal3Fisheye(pose1) + camera2 = gtsam.PinholeCameraCal3Fisheye(pose2) + cameras = gtsam.CameraSetCal3Fisheye([camera1, camera2]) + measurements = gtsam.Point2Vector([k.project(obj_point) for k in cameras]) + triangulated = gtsam.triangulatePoint3(cameras, measurements, rank_tol=1e-9, optimize=True) + self.gtsamAssertEquals(measurements[0], self.img_point) + self.gtsamAssertEquals(triangulated, obj_point) + + def test_triangulation_rectify(self): + """Estimate spatial point from image measurements using rectification""" + p1 = [-1.0, 0.0, -1.0] + p2 = [ 1.0, 0.0, -1.0] + q1 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0) + q2 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0) + obj_point = np.array([0.0, 0.0, 0.0]) + pose1 = gtsam.Pose3(q1, p1) + pose2 = gtsam.Pose3(q2, p2) + camera1 = gtsam.PinholeCameraCal3Fisheye(pose1) + camera2 = gtsam.PinholeCameraCal3Fisheye(pose2) + cameras = gtsam.CameraSetCal3Fisheye([camera1, camera2]) + measurements = gtsam.Point2Vector([k.project(obj_point) for k in cameras]) + rectified = gtsam.Point2Vector([k.calibration().calibrate(pt) for k, pt in zip(cameras, measurements)]) + shared_cal = gtsam.Cal3_S2() + poses = gtsam.Pose3Vector([pose1, pose2]) + triangulated = gtsam.triangulatePoint3(poses, shared_cal, rectified, rank_tol=1e-9, optimize=False) + self.gtsamAssertEquals(measurements[0], self.img_point) + self.gtsamAssertEquals(triangulated, obj_point) + def test_retract(self): expected = gtsam.Cal3Fisheye(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6, 1e-3 + 7, 2.0*1e-3 + 8, 3.0*1e-3 + 9, 4.0*1e-3 + 10) From c595767caeba831701298ab410a693048b2fe1db Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Sun, 11 Jul 2021 14:14:08 +0200 Subject: [PATCH 041/248] Unittest, triangulation for Cal3Unified --- python/gtsam/tests/test_Cal3Unified.py | 38 ++++++++++++++++++++++++++ 1 file changed, 38 insertions(+) diff --git a/python/gtsam/tests/test_Cal3Unified.py b/python/gtsam/tests/test_Cal3Unified.py index f2c1ada48..0b09fc3ae 100644 --- a/python/gtsam/tests/test_Cal3Unified.py +++ b/python/gtsam/tests/test_Cal3Unified.py @@ -105,6 +105,44 @@ class TestCal3Unified(GtsamTestCase): score = graph.error(state) self.assertAlmostEqual(score, 0) + @unittest.skip("triangulatePoint3 currently seems to require perspective projections.") + def test_triangulation(self): + """Estimate spatial point from image measurements""" + p1 = [-1.0, 0.0, -1.0] + p2 = [ 1.0, 0.0, -1.0] + q1 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0) + q2 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0) + obj_point = np.array([0.0, 0.0, 0.0]) + pose1 = gtsam.Pose3(q1, p1) + pose2 = gtsam.Pose3(q2, p2) + camera1 = gtsam.PinholeCameraCal3Unified(pose1, self.stereographic) + camera2 = gtsam.PinholeCameraCal3Unified(pose2, self.stereographic) + cameras = gtsam.CameraSetCal3Unified([camera1, camera2]) + measurements = gtsam.Point2Vector([k.project(obj_point) for k in cameras]) + triangulated = gtsam.triangulatePoint3(cameras, measurements, rank_tol=1e-9, optimize=True) + self.gtsamAssertEquals(measurements[0], self.img_point) + self.gtsamAssertEquals(triangulated, obj_point) + + def test_triangulation_rectify(self): + """Estimate spatial point from image measurements using rectification""" + p1 = [-1.0, 0.0, -1.0] + p2 = [ 1.0, 0.0, -1.0] + q1 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0) + q2 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0) + obj_point = np.array([0.0, 0.0, 0.0]) + pose1 = gtsam.Pose3(q1, p1) + pose2 = gtsam.Pose3(q2, p2) + camera1 = gtsam.PinholeCameraCal3Unified(pose1, self.stereographic) + camera2 = gtsam.PinholeCameraCal3Unified(pose2, self.stereographic) + cameras = gtsam.CameraSetCal3Unified([camera1, camera2]) + measurements = gtsam.Point2Vector([k.project(obj_point) for k in cameras]) + rectified = gtsam.Point2Vector([k.calibration().calibrate(pt) for k, pt in zip(cameras, measurements)]) + shared_cal = gtsam.Cal3_S2() + poses = gtsam.Pose3Vector([pose1, pose2]) + triangulated = gtsam.triangulatePoint3(poses, shared_cal, rectified, rank_tol=1e-9, optimize=False) + self.gtsamAssertEquals(measurements[0], self.img_point) + self.gtsamAssertEquals(triangulated, obj_point) + def test_retract(self): expected = gtsam.Cal3Unified(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6, 1e-3 + 7, 2.0*1e-3 + 8, 3.0*1e-3 + 9, 4.0*1e-3 + 10, 0.1 + 1) From 9bafebb5218f40ec40dfbaa2b0f7576fe27ec683 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 11 Jul 2021 08:09:59 -0700 Subject: [PATCH 042/248] break interface file into multiple files --- gtsam/base/base.i | 113 ++ gtsam/geometry/geometry.i | 942 +++++++++ gtsam/gtsam.i | 3520 +-------------------------------- gtsam/linear/linear.i | 653 ++++++ gtsam/navigation/navigation.i | 355 ++++ gtsam/nonlinear/nonlinear.i | 769 +++++++ gtsam/sam/sam.i | 96 + gtsam/sfm/sfm.i | 209 ++ gtsam/slam/slam.i | 325 +++ gtsam/symbolic/symbolic.i | 201 ++ 10 files changed, 3709 insertions(+), 3474 deletions(-) create mode 100644 gtsam/base/base.i create mode 100644 gtsam/geometry/geometry.i create mode 100644 gtsam/linear/linear.i create mode 100644 gtsam/navigation/navigation.i create mode 100644 gtsam/nonlinear/nonlinear.i create mode 100644 gtsam/sam/sam.i create mode 100644 gtsam/sfm/sfm.i create mode 100644 gtsam/slam/slam.i create mode 100644 gtsam/symbolic/symbolic.i diff --git a/gtsam/base/base.i b/gtsam/base/base.i new file mode 100644 index 000000000..09278ff5b --- /dev/null +++ b/gtsam/base/base.i @@ -0,0 +1,113 @@ +//************************************************************************* +// base +//************************************************************************* + +namespace gtsam { + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// ##### + +#include +bool isDebugVersion(); + +#include +class IndexPair { + IndexPair(); + IndexPair(size_t i, size_t j); + size_t i() const; + size_t j() const; +}; + +// template +// class DSFMap { +// DSFMap(); +// KEY find(const KEY& key) const; +// void merge(const KEY& x, const KEY& y); +// std::map sets(); +// }; + +class IndexPairSet { + IndexPairSet(); + // common STL methods + size_t size() const; + bool empty() const; + void clear(); + + // structure specific methods + void insert(gtsam::IndexPair key); + bool erase(gtsam::IndexPair key); // returns true if value was removed + bool count(gtsam::IndexPair key) const; // returns true if value exists +}; + +class IndexPairVector { + IndexPairVector(); + IndexPairVector(const gtsam::IndexPairVector& other); + + // common STL methods + size_t size() const; + bool empty() const; + void clear(); + + // structure specific methods + gtsam::IndexPair at(size_t i) const; + void push_back(gtsam::IndexPair key) const; +}; + +gtsam::IndexPairVector IndexPairSetAsArray(gtsam::IndexPairSet& set); + +class IndexPairSetMap { + IndexPairSetMap(); + // common STL methods + size_t size() const; + bool empty() const; + void clear(); + + // structure specific methods + gtsam::IndexPairSet at(gtsam::IndexPair& key); +}; + +class DSFMapIndexPair { + DSFMapIndexPair(); + gtsam::IndexPair find(const gtsam::IndexPair& key) const; + void merge(const gtsam::IndexPair& x, const gtsam::IndexPair& y); + gtsam::IndexPairSetMap sets(); +}; + +#include +bool linear_independent(Matrix A, Matrix B, double tol); + +#include +virtual class Value { + // No constructors because this is an abstract class + + // Testable + void print(string s = "") const; + + // Manifold + size_t dim() const; +}; + +#include +template +virtual class GenericValue : gtsam::Value { + void serializable() const; +}; + +} // namespace gtsam \ No newline at end of file diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i new file mode 100644 index 000000000..a39c22797 --- /dev/null +++ b/gtsam/geometry/geometry.i @@ -0,0 +1,942 @@ +//************************************************************************* +// geometry +//************************************************************************* + +namespace gtsam { + +#include +class Point2 { + // Standard Constructors + Point2(); + Point2(double x, double y); + Point2(Vector v); + + // Testable + void print(string s = "") const; + bool equals(const gtsam::Point2& point, double tol) const; + + // Group + static gtsam::Point2 identity(); + + // Standard Interface + double x() const; + double y() const; + Vector vector() const; + double distance(const gtsam::Point2& p2) const; + double norm() const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; +}; + +// std::vector +class Point2Vector { + // Constructors + Point2Vector(); + Point2Vector(const gtsam::Point2Vector& v); + + // Capacity + size_t size() const; + size_t max_size() const; + void resize(size_t sz); + size_t capacity() const; + bool empty() const; + void reserve(size_t n); + + // Element access + gtsam::Point2 at(size_t n) const; + gtsam::Point2 front() const; + gtsam::Point2 back() const; + + // Modifiers + void assign(size_t n, const gtsam::Point2& u); + void push_back(const gtsam::Point2& x); + void pop_back(); +}; + +#include +class StereoPoint2 { + // Standard Constructors + StereoPoint2(); + StereoPoint2(double uL, double uR, double v); + + // Testable + void print(string s = "") const; + bool equals(const gtsam::StereoPoint2& point, double tol) const; + + // Group + static gtsam::StereoPoint2 identity(); + gtsam::StereoPoint2 inverse() const; + gtsam::StereoPoint2 compose(const gtsam::StereoPoint2& p2) const; + gtsam::StereoPoint2 between(const gtsam::StereoPoint2& p2) const; + + // Operator Overloads + gtsam::StereoPoint2 operator-() const; + // gtsam::StereoPoint2 operator+(Vector b) const; //TODO Mixed types not yet + // supported + gtsam::StereoPoint2 operator+(const gtsam::StereoPoint2& p2) const; + gtsam::StereoPoint2 operator-(const gtsam::StereoPoint2& p2) const; + + // Manifold + gtsam::StereoPoint2 retract(Vector v) const; + Vector localCoordinates(const gtsam::StereoPoint2& p) const; + + // Lie Group + static gtsam::StereoPoint2 Expmap(Vector v); + static Vector Logmap(const gtsam::StereoPoint2& p); + + // Standard Interface + Vector vector() const; + double uL() const; + double uR() const; + double v() const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; +}; + +#include +class Point3 { + // Standard Constructors + Point3(); + Point3(double x, double y, double z); + Point3(Vector v); + + // Testable + void print(string s = "") const; + bool equals(const gtsam::Point3& p, double tol) const; + + // Group + static gtsam::Point3 identity(); + + // Standard Interface + Vector vector() const; + double x() const; + double y() const; + double z() const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; +}; + +class Point3Pairs { + Point3Pairs(); + size_t size() const; + bool empty() const; + gtsam::Point3Pair at(size_t n) const; + void push_back(const gtsam::Point3Pair& point_pair); +}; + +#include +class Rot2 { + // Standard Constructors and Named Constructors + Rot2(); + Rot2(double theta); + static gtsam::Rot2 fromAngle(double theta); + static gtsam::Rot2 fromDegrees(double theta); + static gtsam::Rot2 fromCosSin(double c, double s); + + // Testable + void print(string s = "theta") const; + bool equals(const gtsam::Rot2& rot, double tol) const; + + // Group + static gtsam::Rot2 identity(); + gtsam::Rot2 inverse(); + gtsam::Rot2 compose(const gtsam::Rot2& p2) const; + gtsam::Rot2 between(const gtsam::Rot2& p2) const; + + // Operator Overloads + gtsam::Rot2 operator*(const gtsam::Rot2& p2) const; + + // Manifold + gtsam::Rot2 retract(Vector v) const; + Vector localCoordinates(const gtsam::Rot2& p) const; + + // Lie Group + static gtsam::Rot2 Expmap(Vector v); + static Vector Logmap(const gtsam::Rot2& p); + Vector logmap(const gtsam::Rot2& p); + + // Group Action on Point2 + gtsam::Point2 rotate(const gtsam::Point2& point) const; + gtsam::Point2 unrotate(const gtsam::Point2& point) const; + + // Standard Interface + static gtsam::Rot2 relativeBearing( + const gtsam::Point2& d); // Ignoring derivative + static gtsam::Rot2 atan2(double y, double x); + double theta() const; + double degrees() const; + double c() const; + double s() const; + Matrix matrix() const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; +}; + +#include +class SO3 { + // Standard Constructors + SO3(); + SO3(Matrix R); + static gtsam::SO3 FromMatrix(Matrix R); + static gtsam::SO3 AxisAngle(const Vector axis, double theta); + static gtsam::SO3 ClosestTo(const Matrix M); + + // Testable + void print(string s = "") const; + bool equals(const gtsam::SO3& other, double tol) const; + + // Group + static gtsam::SO3 identity(); + gtsam::SO3 inverse() const; + gtsam::SO3 between(const gtsam::SO3& R) const; + gtsam::SO3 compose(const gtsam::SO3& R) const; + + // Operator Overloads + gtsam::SO3 operator*(const gtsam::SO3& R) const; + + // Manifold + gtsam::SO3 retract(Vector v) const; + Vector localCoordinates(const gtsam::SO3& R) const; + static gtsam::SO3 Expmap(Vector v); + + // Other methods + Vector vec() const; + Matrix matrix() const; +}; + +#include +class SO4 { + // Standard Constructors + SO4(); + SO4(Matrix R); + static gtsam::SO4 FromMatrix(Matrix R); + + // Testable + void print(string s = "") const; + bool equals(const gtsam::SO4& other, double tol) const; + + // Group + static gtsam::SO4 identity(); + gtsam::SO4 inverse() const; + gtsam::SO4 between(const gtsam::SO4& Q) const; + gtsam::SO4 compose(const gtsam::SO4& Q) const; + + // Operator Overloads + gtsam::SO4 operator*(const gtsam::SO4& Q) const; + + // Manifold + gtsam::SO4 retract(Vector v) const; + Vector localCoordinates(const gtsam::SO4& Q) const; + static gtsam::SO4 Expmap(Vector v); + + // Other methods + Vector vec() const; + Matrix matrix() const; +}; + +#include +class SOn { + // Standard Constructors + SOn(size_t n); + static gtsam::SOn FromMatrix(Matrix R); + static gtsam::SOn Lift(size_t n, Matrix R); + + // Testable + void print(string s = "") const; + bool equals(const gtsam::SOn& other, double tol) const; + + // Group + static gtsam::SOn identity(); + gtsam::SOn inverse() const; + gtsam::SOn between(const gtsam::SOn& Q) const; + gtsam::SOn compose(const gtsam::SOn& Q) const; + + // Operator Overloads + gtsam::SOn operator*(const gtsam::SOn& Q) const; + + // Manifold + gtsam::SOn retract(Vector v) const; + Vector localCoordinates(const gtsam::SOn& Q) const; + static gtsam::SOn Expmap(Vector v); + + // Other methods + Vector vec() const; + Matrix matrix() const; + + // enabling serialization functionality + void serialize() const; +}; + +#include +class Quaternion { + double w() const; + double x() const; + double y() const; + double z() const; + Vector coeffs() const; +}; + +#include +class Rot3 { + // Standard Constructors and Named Constructors + Rot3(); + Rot3(Matrix R); + Rot3(const gtsam::Point3& col1, const gtsam::Point3& col2, + const gtsam::Point3& col3); + Rot3(double R11, double R12, double R13, double R21, double R22, double R23, + double R31, double R32, double R33); + Rot3(double w, double x, double y, double z); + + static gtsam::Rot3 Rx(double t); + static gtsam::Rot3 Ry(double t); + static gtsam::Rot3 Rz(double t); + static gtsam::Rot3 RzRyRx(double x, double y, double z); + static gtsam::Rot3 RzRyRx(Vector xyz); + static gtsam::Rot3 Yaw( + double t); // positive yaw is to right (as in aircraft heading) + static gtsam::Rot3 Pitch( + double t); // positive pitch is up (increasing aircraft altitude) + static gtsam::Rot3 Roll( + double t); // positive roll is to right (increasing yaw in aircraft) + static gtsam::Rot3 Ypr(double y, double p, double r); + static gtsam::Rot3 Quaternion(double w, double x, double y, double z); + static gtsam::Rot3 AxisAngle(const gtsam::Point3& axis, double angle); + static gtsam::Rot3 Rodrigues(Vector v); + static gtsam::Rot3 Rodrigues(double wx, double wy, double wz); + static gtsam::Rot3 ClosestTo(const Matrix M); + + // Testable + void print(string s = "") const; + bool equals(const gtsam::Rot3& rot, double tol) const; + + // Group + static gtsam::Rot3 identity(); + gtsam::Rot3 inverse() const; + gtsam::Rot3 compose(const gtsam::Rot3& p2) const; + gtsam::Rot3 between(const gtsam::Rot3& p2) const; + + // Operator Overloads + gtsam::Rot3 operator*(const gtsam::Rot3& p2) const; + + // Manifold + // gtsam::Rot3 retractCayley(Vector v) const; // TODO, does not exist in both + // Matrix and Quaternion options + gtsam::Rot3 retract(Vector v) const; + Vector localCoordinates(const gtsam::Rot3& p) const; + + // Group Action on Point3 + gtsam::Point3 rotate(const gtsam::Point3& p) const; + gtsam::Point3 unrotate(const gtsam::Point3& p) const; + + // Standard Interface + static gtsam::Rot3 Expmap(Vector v); + static Vector Logmap(const gtsam::Rot3& p); + Vector logmap(const gtsam::Rot3& p); + Matrix matrix() const; + Matrix transpose() const; + gtsam::Point3 column(size_t index) const; + Vector xyz() const; + Vector ypr() const; + Vector rpy() const; + double roll() const; + double pitch() const; + double yaw() const; + pair axisAngle() const; + gtsam::Quaternion toQuaternion() const; + Vector quaternion() const; + gtsam::Rot3 slerp(double t, const gtsam::Rot3& other) const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; +}; + +#include +class Pose2 { + // Standard Constructor + Pose2(); + Pose2(const gtsam::Pose2& other); + Pose2(double x, double y, double theta); + Pose2(double theta, const gtsam::Point2& t); + Pose2(const gtsam::Rot2& r, const gtsam::Point2& t); + Pose2(Vector v); + + // Testable + void print(string s = "") const; + bool equals(const gtsam::Pose2& pose, double tol) const; + + // Group + static gtsam::Pose2 identity(); + gtsam::Pose2 inverse() const; + gtsam::Pose2 compose(const gtsam::Pose2& p2) const; + gtsam::Pose2 between(const gtsam::Pose2& p2) const; + + // Operator Overloads + gtsam::Pose2 operator*(const gtsam::Pose2& p2) const; + + // Manifold + gtsam::Pose2 retract(Vector v) const; + Vector localCoordinates(const gtsam::Pose2& p) const; + + // Lie Group + static gtsam::Pose2 Expmap(Vector v); + static Vector Logmap(const gtsam::Pose2& p); + Vector logmap(const gtsam::Pose2& p); + static Matrix ExpmapDerivative(Vector v); + static Matrix LogmapDerivative(const gtsam::Pose2& v); + Matrix AdjointMap() const; + Vector Adjoint(Vector xi) const; + static Matrix adjointMap_(Vector v); + static Vector adjoint_(Vector xi, Vector y); + static Vector adjointTranspose(Vector xi, Vector y); + static Matrix wedge(double vx, double vy, double w); + + // Group Actions on Point2 + gtsam::Point2 transformFrom(const gtsam::Point2& p) const; + gtsam::Point2 transformTo(const gtsam::Point2& p) const; + + // Standard Interface + double x() const; + double y() const; + double theta() const; + gtsam::Rot2 bearing(const gtsam::Point2& point) const; + double range(const gtsam::Point2& point) const; + gtsam::Point2 translation() const; + gtsam::Rot2 rotation() const; + Matrix matrix() const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; +}; + +#include +class Pose3 { + // Standard Constructors + Pose3(); + Pose3(const gtsam::Pose3& other); + Pose3(const gtsam::Rot3& r, const gtsam::Point3& t); + Pose3(const gtsam::Pose2& pose2); + Pose3(Matrix mat); + + // Testable + void print(string s = "") const; + bool equals(const gtsam::Pose3& pose, double tol) const; + + // Group + static gtsam::Pose3 identity(); + gtsam::Pose3 inverse() const; + gtsam::Pose3 compose(const gtsam::Pose3& pose) const; + gtsam::Pose3 between(const gtsam::Pose3& pose) const; + + // Operator Overloads + gtsam::Pose3 operator*(const gtsam::Pose3& pose) const; + + // Manifold + gtsam::Pose3 retract(Vector v) const; + Vector localCoordinates(const gtsam::Pose3& pose) const; + + // Lie Group + static gtsam::Pose3 Expmap(Vector v); + static Vector Logmap(const gtsam::Pose3& pose); + Vector logmap(const gtsam::Pose3& pose); + Matrix AdjointMap() const; + Vector Adjoint(Vector xi) const; + static Matrix adjointMap_(Vector xi); + static Vector adjoint_(Vector xi, Vector y); + static Vector adjointTranspose(Vector xi, Vector y); + static Matrix ExpmapDerivative(Vector xi); + static Matrix LogmapDerivative(const gtsam::Pose3& xi); + static Matrix wedge(double wx, double wy, double wz, double vx, double vy, + double vz); + + // Group Action on Point3 + gtsam::Point3 transformFrom(const gtsam::Point3& point) const; + gtsam::Point3 transformTo(const gtsam::Point3& point) const; + + // Standard Interface + gtsam::Rot3 rotation() const; + gtsam::Point3 translation() const; + double x() const; + double y() const; + double z() const; + Matrix matrix() const; + gtsam::Pose3 transformPoseFrom(const gtsam::Pose3& pose) const; + gtsam::Pose3 transformPoseTo(const gtsam::Pose3& pose) const; + double range(const gtsam::Point3& point); + double range(const gtsam::Pose3& pose); + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; +}; + +class Pose3Pairs { + Pose3Pairs(); + size_t size() const; + bool empty() const; + gtsam::Pose3Pair at(size_t n) const; + void push_back(const gtsam::Pose3Pair& pose_pair); +}; + +class Pose3Vector { + Pose3Vector(); + size_t size() const; + bool empty() const; + gtsam::Pose3 at(size_t n) const; + void push_back(const gtsam::Pose3& pose); +}; + +#include +class Unit3 { + // Standard Constructors + Unit3(); + Unit3(const gtsam::Point3& pose); + + // Testable + void print(string s = "") const; + bool equals(const gtsam::Unit3& pose, double tol) const; + + // Other functionality + Matrix basis() const; + Matrix skew() const; + gtsam::Point3 point3() const; + + // Manifold + static size_t Dim(); + size_t dim() const; + gtsam::Unit3 retract(Vector v) const; + Vector localCoordinates(const gtsam::Unit3& s) const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; + + // enabling function to compare objects + bool equals(const gtsam::Unit3& expected, double tol) const; +}; + +#include +class EssentialMatrix { + // Standard Constructors + EssentialMatrix(const gtsam::Rot3& aRb, const gtsam::Unit3& aTb); + + // Testable + void print(string s = "") const; + bool equals(const gtsam::EssentialMatrix& pose, double tol) const; + + // Manifold + static size_t Dim(); + size_t dim() const; + gtsam::EssentialMatrix retract(Vector v) const; + Vector localCoordinates(const gtsam::EssentialMatrix& s) const; + + // Other methods: + gtsam::Rot3 rotation() const; + gtsam::Unit3 direction() const; + Matrix matrix() const; + double error(Vector vA, Vector vB); +}; + +#include +class Cal3_S2 { + // Standard Constructors + Cal3_S2(); + Cal3_S2(double fx, double fy, double s, double u0, double v0); + Cal3_S2(Vector v); + Cal3_S2(double fov, int w, int h); + + // Testable + void print(string s = "Cal3_S2") const; + bool equals(const gtsam::Cal3_S2& rhs, double tol) const; + + // Manifold + static size_t Dim(); + size_t dim() const; + gtsam::Cal3_S2 retract(Vector v) const; + Vector localCoordinates(const gtsam::Cal3_S2& c) const; + + // Action on Point2 + 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 skew() const; + double px() const; + double py() const; + gtsam::Point2 principalPoint() const; + Vector vector() const; + Matrix K() const; + Matrix inverse() const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; +}; + +#include +virtual class Cal3DS2_Base { + // Standard Constructors + Cal3DS2_Base(); + + // Testable + void print(string s = "") const; + + // Standard Interface + double fx() const; + double fy() const; + double skew() const; + double px() const; + double py() const; + double k1() const; + double k2() const; + Matrix K() const; + Vector k() const; + Vector vector() const; + + // Action on Point2 + gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; + gtsam::Point2 calibrate(const gtsam::Point2& p) const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; +}; + +#include +virtual class Cal3DS2 : gtsam::Cal3DS2_Base { + // Standard Constructors + Cal3DS2(); + Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, + double k2); + Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, + double k2, double p1, double p2); + Cal3DS2(Vector v); + + // Testable + bool equals(const gtsam::Cal3DS2& rhs, double tol) const; + + // Manifold + size_t dim() const; + static size_t Dim(); + gtsam::Cal3DS2 retract(Vector v) const; + Vector localCoordinates(const gtsam::Cal3DS2& c) const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; +}; + +#include +virtual class Cal3Unified : gtsam::Cal3DS2_Base { + // Standard Constructors + Cal3Unified(); + Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1, + double k2); + Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1, + double k2, double p1, double p2, double xi); + Cal3Unified(Vector v); + + // Testable + bool equals(const gtsam::Cal3Unified& rhs, double tol) const; + + // Standard Interface + double xi() const; + gtsam::Point2 spaceToNPlane(const gtsam::Point2& p) const; + gtsam::Point2 nPlaneToSpace(const gtsam::Point2& p) const; + + // Manifold + size_t dim() const; + static size_t Dim(); + gtsam::Cal3Unified retract(Vector v) const; + Vector localCoordinates(const gtsam::Cal3Unified& c) const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; +}; + +#include +class Cal3_S2Stereo { + // Standard Constructors + Cal3_S2Stereo(); + Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b); + Cal3_S2Stereo(Vector v); + + // Testable + void print(string s = "") const; + bool equals(const gtsam::Cal3_S2Stereo& K, double tol) const; + + // Standard Interface + double fx() const; + double fy() const; + double skew() const; + double px() const; + double py() const; + gtsam::Point2 principalPoint() const; + double baseline() const; +}; + +#include +class Cal3Bundler { + // Standard Constructors + Cal3Bundler(); + Cal3Bundler(double fx, double k1, double k2, double u0, double v0); + Cal3Bundler(double fx, double k1, double k2, double u0, double v0, + double tol); + + // 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) const; + gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; + + // Standard Interface + double fx() const; + double fy() const; + double k1() const; + double k2() const; + double px() const; + double py() const; + Vector vector() const; + Vector k() const; + Matrix K() const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; +}; + +#include +class CalibratedCamera { + // Standard Constructors and Named Constructors + CalibratedCamera(); + CalibratedCamera(const gtsam::Pose3& pose); + CalibratedCamera(Vector v); + static gtsam::CalibratedCamera Level(const gtsam::Pose2& pose2, + double height); + + // Testable + void print(string s = "CalibratedCamera") const; + bool equals(const gtsam::CalibratedCamera& camera, double tol) const; + + // Manifold + static size_t Dim(); + size_t dim() const; + gtsam::CalibratedCamera retract(Vector d) const; + Vector localCoordinates(const gtsam::CalibratedCamera& T2) const; + + // Action on Point3 + gtsam::Point2 project(const gtsam::Point3& point) const; + static gtsam::Point2 Project(const gtsam::Point3& cameraPoint); + + // Standard Interface + gtsam::Pose3 pose() const; + double range(const gtsam::Point3& point) const; + double range(const gtsam::Pose3& pose) const; + double range(const gtsam::CalibratedCamera& camera) const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; +}; + +#include +template +class PinholeCamera { + // Standard Constructors and Named Constructors + PinholeCamera(); + PinholeCamera(const gtsam::Pose3& pose); + PinholeCamera(const gtsam::Pose3& pose, const CALIBRATION& K); + static This Level(const CALIBRATION& K, const gtsam::Pose2& pose, + double height); + static This Level(const gtsam::Pose2& pose, double height); + static This Lookat(const gtsam::Point3& eye, const gtsam::Point3& target, + const gtsam::Point3& upVector, const CALIBRATION& K); + + // Testable + void print(string s = "PinholeCamera") const; + bool equals(const This& camera, double tol) const; + + // Standard Interface + gtsam::Pose3 pose() const; + CALIBRATION calibration() const; + + // Manifold + This retract(Vector d) const; + Vector localCoordinates(const This& T2) const; + size_t dim() const; + static size_t Dim(); + + // Transformations and measurement functions + static gtsam::Point2 Project(const gtsam::Point3& cameraPoint); + pair projectSafe(const gtsam::Point3& pw) const; + gtsam::Point2 project(const gtsam::Point3& point); + gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const; + double range(const gtsam::Point3& point); + double range(const gtsam::Pose3& pose); + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; +}; + +#include +class Similarity3 { + // Standard Constructors + Similarity3(); + Similarity3(double s); + Similarity3(const gtsam::Rot3& R, const gtsam::Point3& t, double s); + Similarity3(const Matrix& R, const Vector& t, double s); + Similarity3(const Matrix& T); + + gtsam::Point3 transformFrom(const gtsam::Point3& p) const; + gtsam::Pose3 transformFrom(const gtsam::Pose3& T); + + static gtsam::Similarity3 Align(const gtsam::Point3Pairs& abPointPairs); + static gtsam::Similarity3 Align(const gtsam::Pose3Pairs& abPosePairs); + + // Standard Interface + const Matrix matrix() const; + const gtsam::Rot3& rotation(); + const gtsam::Point3& translation(); + double scale() const; +}; + +// Forward declaration of PinholeCameraCalX is defined here. +#include +// Some typedefs for common camera types +// PinholeCameraCal3_S2 is the same as SimpleCamera above +typedef gtsam::PinholeCamera PinholeCameraCal3_S2; +typedef gtsam::PinholeCamera PinholeCameraCal3DS2; +typedef gtsam::PinholeCamera PinholeCameraCal3Unified; +typedef gtsam::PinholeCamera PinholeCameraCal3Bundler; + +template +class CameraSet { + CameraSet(); + + // structure specific methods + T at(size_t i) const; + void push_back(const T& cam); +}; + +#include +class StereoCamera { + // Standard Constructors and Named Constructors + StereoCamera(); + StereoCamera(const gtsam::Pose3& pose, const gtsam::Cal3_S2Stereo* K); + + // Testable + void print(string s = "") const; + bool equals(const gtsam::StereoCamera& camera, double tol) const; + + // Standard Interface + gtsam::Pose3 pose() const; + double baseline() const; + gtsam::Cal3_S2Stereo calibration() const; + + // Manifold + gtsam::StereoCamera retract(Vector d) const; + Vector localCoordinates(const gtsam::StereoCamera& T2) const; + size_t dim() const; + static size_t Dim(); + + // Transformations and measurement functions + gtsam::StereoPoint2 project(const gtsam::Point3& point); + gtsam::Point3 backproject(const gtsam::StereoPoint2& p) const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; +}; + +#include + +// Templates appear not yet supported for free functions - issue raised at +// borglab/wrap#14 to add support +gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, + gtsam::Cal3_S2* sharedCal, + const gtsam::Point2Vector& measurements, + double rank_tol, bool optimize); +gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, + gtsam::Cal3DS2* sharedCal, + const gtsam::Point2Vector& measurements, + double rank_tol, bool optimize); +gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, + gtsam::Cal3Bundler* sharedCal, + const gtsam::Point2Vector& measurements, + double rank_tol, bool optimize); +gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3_S2& cameras, + const gtsam::Point2Vector& measurements, + double rank_tol, bool optimize); +gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Bundler& cameras, + const gtsam::Point2Vector& measurements, + double rank_tol, bool optimize); + +#include +template +class BearingRange { + BearingRange(const BEARING& b, const RANGE& r); + BEARING bearing() const; + RANGE range() const; + static This Measure(const POSE& pose, const POINT& point); + static BEARING MeasureBearing(const POSE& pose, const POINT& point); + static RANGE MeasureRange(const POSE& pose, const POINT& point); + void print(string s = "") const; +}; + +typedef gtsam::BearingRange + BearingRange2D; + +} // namespace gtsam diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 049a0110c..a55581e50 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -2,10 +2,12 @@ * GTSAM Wrap Module Definition * - * These are the current classes available through the matlab and python wrappers, + * These are the current classes available through the matlab and python + wrappers, * add more functions/classes as they are available. * - * Please refer to the wrapping docs: https://github.com/borglab/wrap/blob/master/README.md + * Please refer to the wrapping docs: + https://github.com/borglab/wrap/blob/master/README.md */ namespace gtsam { @@ -61,8 +63,8 @@ class KeySet { // structure specific methods void insert(size_t key); void merge(const gtsam::KeySet& other); - bool erase(size_t key); // returns true if value was removed - bool count(size_t key) const; // returns true if value exists + bool erase(size_t key); // returns true if value was removed + bool count(size_t key) const; // returns true if value exists void serialize() const; @@ -123,8 +125,8 @@ class FactorIndexSet { // structure specific methods void insert(size_t factorIndex); - bool erase(size_t factorIndex); // returns true if value was removed - bool count(size_t factorIndex) const; // returns true if value exists + bool erase(size_t factorIndex); // returns true if value was removed + bool count(size_t factorIndex) const; // returns true if value exists }; // Actually a vector @@ -144,3483 +146,53 @@ class FactorIndices { void push_back(size_t factorIndex) const; }; -//************************************************************************* -// base -//************************************************************************* - -/** gtsam namespace functions */ - -#include -bool isDebugVersion(); - -#include -class IndexPair { - IndexPair(); - IndexPair(size_t i, size_t j); - size_t i() const; - size_t j() const; -}; - -// template -// class DSFMap { -// DSFMap(); -// KEY find(const KEY& key) const; -// void merge(const KEY& x, const KEY& y); -// std::map sets(); -// }; - -class IndexPairSet { - IndexPairSet(); - // common STL methods - size_t size() const; - bool empty() const; - void clear(); - - // structure specific methods - void insert(gtsam::IndexPair key); - bool erase(gtsam::IndexPair key); // returns true if value was removed - bool count(gtsam::IndexPair key) const; // returns true if value exists -}; - -class IndexPairVector { - IndexPairVector(); - IndexPairVector(const gtsam::IndexPairVector& other); - - // common STL methods - size_t size() const; - bool empty() const; - void clear(); - - // structure specific methods - gtsam::IndexPair at(size_t i) const; - void push_back(gtsam::IndexPair key) const; -}; - -gtsam::IndexPairVector IndexPairSetAsArray(gtsam::IndexPairSet& set); - -class IndexPairSetMap { - IndexPairSetMap(); - // common STL methods - size_t size() const; - bool empty() const; - void clear(); - - // structure specific methods - gtsam::IndexPairSet at(gtsam::IndexPair& key); -}; - -class DSFMapIndexPair { - DSFMapIndexPair(); - gtsam::IndexPair find(const gtsam::IndexPair& key) const; - void merge(const gtsam::IndexPair& x, const gtsam::IndexPair& y); - gtsam::IndexPairSetMap sets(); -}; - -#include -bool linear_independent(Matrix A, Matrix B, double tol); - -#include -virtual class Value { - // No constructors because this is an abstract class - - // Testable - void print(string s = "") const; - - // Manifold - size_t dim() const; -}; - -#include -template -virtual class GenericValue : gtsam::Value { - void serializable() const; -}; - -//************************************************************************* -// geometry -//************************************************************************* - -#include -class Point2 { - // Standard Constructors - Point2(); - Point2(double x, double y); - Point2(Vector v); - - // Testable - void print(string s = "") const; - bool equals(const gtsam::Point2& point, double tol) const; - - // Group - static gtsam::Point2 identity(); - - // Standard Interface - double x() const; - double y() const; - Vector vector() const; - double distance(const gtsam::Point2& p2) const; - double norm() const; - - // enabling serialization functionality - void serialize() const; - - // enable pickling in python - void pickle() const; -}; - -// std::vector -class Point2Vector -{ - // Constructors - Point2Vector(); - Point2Vector(const gtsam::Point2Vector& v); - - //Capacity - size_t size() const; - size_t max_size() const; - void resize(size_t sz); - size_t capacity() const; - bool empty() const; - void reserve(size_t n); - - //Element access - gtsam::Point2 at(size_t n) const; - gtsam::Point2 front() const; - gtsam::Point2 back() const; - - //Modifiers - void assign(size_t n, const gtsam::Point2& u); - void push_back(const gtsam::Point2& x); - void pop_back(); -}; - -#include -class StereoPoint2 { - // Standard Constructors - StereoPoint2(); - StereoPoint2(double uL, double uR, double v); - - // Testable - void print(string s = "") const; - bool equals(const gtsam::StereoPoint2& point, double tol) const; - - // Group - static gtsam::StereoPoint2 identity(); - gtsam::StereoPoint2 inverse() const; - gtsam::StereoPoint2 compose(const gtsam::StereoPoint2& p2) const; - gtsam::StereoPoint2 between(const gtsam::StereoPoint2& p2) const; - - // Operator Overloads - gtsam::StereoPoint2 operator-() const; - // gtsam::StereoPoint2 operator+(Vector b) const; //TODO Mixed types not yet supported - gtsam::StereoPoint2 operator+(const gtsam::StereoPoint2& p2) const; - gtsam::StereoPoint2 operator-(const gtsam::StereoPoint2& p2) const; - - // Manifold - gtsam::StereoPoint2 retract(Vector v) const; - Vector localCoordinates(const gtsam::StereoPoint2& p) const; - - // Lie Group - static gtsam::StereoPoint2 Expmap(Vector v); - static Vector Logmap(const gtsam::StereoPoint2& p); - - // Standard Interface - Vector vector() const; - double uL() const; - double uR() const; - double v() const; - - // enabling serialization functionality - void serialize() const; - - // enable pickling in python - void pickle() const; -}; - -#include -class Point3 { - // Standard Constructors - Point3(); - Point3(double x, double y, double z); - Point3(Vector v); - - // Testable - void print(string s = "") const; - bool equals(const gtsam::Point3& p, double tol) const; - - // Group - static gtsam::Point3 identity(); - - // Standard Interface - Vector vector() const; - double x() const; - double y() const; - double z() const; - - // enabling serialization functionality - void serialize() const; - - // enable pickling in python - void pickle() const; -}; - -class Point3Pairs { - Point3Pairs(); - size_t size() const; - bool empty() const; - gtsam::Point3Pair at(size_t n) const; - void push_back(const gtsam::Point3Pair& point_pair); -}; - -#include -class Rot2 { - // Standard Constructors and Named Constructors - Rot2(); - Rot2(double theta); - static gtsam::Rot2 fromAngle(double theta); - static gtsam::Rot2 fromDegrees(double theta); - static gtsam::Rot2 fromCosSin(double c, double s); - - // Testable - void print(string s = "theta") const; - bool equals(const gtsam::Rot2& rot, double tol) const; - - // Group - static gtsam::Rot2 identity(); - gtsam::Rot2 inverse(); - gtsam::Rot2 compose(const gtsam::Rot2& p2) const; - gtsam::Rot2 between(const gtsam::Rot2& p2) const; - - // Operator Overloads - gtsam::Rot2 operator*(const gtsam::Rot2& p2) const; - - // Manifold - gtsam::Rot2 retract(Vector v) const; - Vector localCoordinates(const gtsam::Rot2& p) const; - - // Lie Group - static gtsam::Rot2 Expmap(Vector v); - static Vector Logmap(const gtsam::Rot2& p); - Vector logmap(const gtsam::Rot2& p); - - // Group Action on Point2 - gtsam::Point2 rotate(const gtsam::Point2& point) const; - gtsam::Point2 unrotate(const gtsam::Point2& point) const; - - // Standard Interface - static gtsam::Rot2 relativeBearing(const gtsam::Point2& d); // Ignoring derivative - static gtsam::Rot2 atan2(double y, double x); - double theta() const; - double degrees() const; - double c() const; - double s() const; - Matrix matrix() const; - - // enabling serialization functionality - void serialize() const; - - // enable pickling in python - void pickle() const; -}; - -#include -class SO3 { - // Standard Constructors - SO3(); - SO3(Matrix R); - static gtsam::SO3 FromMatrix(Matrix R); - static gtsam::SO3 AxisAngle(const Vector axis, double theta); - static gtsam::SO3 ClosestTo(const Matrix M); - - // Testable - void print(string s = "") const; - bool equals(const gtsam::SO3& other, double tol) const; - - // Group - static gtsam::SO3 identity(); - gtsam::SO3 inverse() const; - gtsam::SO3 between(const gtsam::SO3& R) const; - gtsam::SO3 compose(const gtsam::SO3& R) const; - - // Operator Overloads - gtsam::SO3 operator*(const gtsam::SO3& R) const; - - // Manifold - gtsam::SO3 retract(Vector v) const; - Vector localCoordinates(const gtsam::SO3& R) const; - static gtsam::SO3 Expmap(Vector v); - - // Other methods - Vector vec() const; - Matrix matrix() const; -}; - -#include -class SO4 { - // Standard Constructors - SO4(); - SO4(Matrix R); - static gtsam::SO4 FromMatrix(Matrix R); - - // Testable - void print(string s = "") const; - bool equals(const gtsam::SO4& other, double tol) const; - - // Group - static gtsam::SO4 identity(); - gtsam::SO4 inverse() const; - gtsam::SO4 between(const gtsam::SO4& Q) const; - gtsam::SO4 compose(const gtsam::SO4& Q) const; - - // Operator Overloads - gtsam::SO4 operator*(const gtsam::SO4& Q) const; - - // Manifold - gtsam::SO4 retract(Vector v) const; - Vector localCoordinates(const gtsam::SO4& Q) const; - static gtsam::SO4 Expmap(Vector v); - - // Other methods - Vector vec() const; - Matrix matrix() const; -}; - -#include -class SOn { - // Standard Constructors - SOn(size_t n); - static gtsam::SOn FromMatrix(Matrix R); - static gtsam::SOn Lift(size_t n, Matrix R); - - // Testable - void print(string s = "") const; - bool equals(const gtsam::SOn& other, double tol) const; - - // Group - static gtsam::SOn identity(); - gtsam::SOn inverse() const; - gtsam::SOn between(const gtsam::SOn& Q) const; - gtsam::SOn compose(const gtsam::SOn& Q) const; - - // Operator Overloads - gtsam::SOn operator*(const gtsam::SOn& Q) const; - - // Manifold - gtsam::SOn retract(Vector v) const; - Vector localCoordinates(const gtsam::SOn& Q) const; - static gtsam::SOn Expmap(Vector v); - - // Other methods - Vector vec() const; - Matrix matrix() const; - - // enabling serialization functionality - void serialize() const; -}; - -#include -class Quaternion { - double w() const; - double x() const; - double y() const; - double z() const; - Vector coeffs() const; -}; - -#include -class Rot3 { - // Standard Constructors and Named Constructors - Rot3(); - Rot3(Matrix R); - Rot3(const gtsam::Point3& col1, const gtsam::Point3& col2, const gtsam::Point3& col3); - Rot3(double R11, double R12, double R13, - double R21, double R22, double R23, - double R31, double R32, double R33); - Rot3(double w, double x, double y, double z); - - static gtsam::Rot3 Rx(double t); - static gtsam::Rot3 Ry(double t); - static gtsam::Rot3 Rz(double t); - static gtsam::Rot3 RzRyRx(double x, double y, double z); - static gtsam::Rot3 RzRyRx(Vector xyz); - static gtsam::Rot3 Yaw(double t); // positive yaw is to right (as in aircraft heading) - static gtsam::Rot3 Pitch(double t); // positive pitch is up (increasing aircraft altitude) - static gtsam::Rot3 Roll(double t); // positive roll is to right (increasing yaw in aircraft) - static gtsam::Rot3 Ypr(double y, double p, double r); - static gtsam::Rot3 Quaternion(double w, double x, double y, double z); - static gtsam::Rot3 AxisAngle(const gtsam::Point3& axis, double angle); - static gtsam::Rot3 Rodrigues(Vector v); - static gtsam::Rot3 Rodrigues(double wx, double wy, double wz); - static gtsam::Rot3 ClosestTo(const Matrix M); - - // Testable - void print(string s = "") const; - bool equals(const gtsam::Rot3& rot, double tol) const; - - // Group - static gtsam::Rot3 identity(); - gtsam::Rot3 inverse() const; - gtsam::Rot3 compose(const gtsam::Rot3& p2) const; - gtsam::Rot3 between(const gtsam::Rot3& p2) const; - - // Operator Overloads - gtsam::Rot3 operator*(const gtsam::Rot3& p2) const; - - // Manifold - //gtsam::Rot3 retractCayley(Vector v) const; // TODO, does not exist in both Matrix and Quaternion options - gtsam::Rot3 retract(Vector v) const; - Vector localCoordinates(const gtsam::Rot3& p) const; - - // Group Action on Point3 - gtsam::Point3 rotate(const gtsam::Point3& p) const; - gtsam::Point3 unrotate(const gtsam::Point3& p) const; - - // Standard Interface - static gtsam::Rot3 Expmap(Vector v); - static Vector Logmap(const gtsam::Rot3& p); - Vector logmap(const gtsam::Rot3& p); - Matrix matrix() const; - Matrix transpose() const; - gtsam::Point3 column(size_t index) const; - Vector xyz() const; - Vector ypr() const; - Vector rpy() const; - double roll() const; - double pitch() const; - double yaw() const; - pair axisAngle() const; - gtsam::Quaternion toQuaternion() const; - Vector quaternion() const; - gtsam::Rot3 slerp(double t, const gtsam::Rot3& other) const; - - // enabling serialization functionality - void serialize() const; - - // enable pickling in python - void pickle() const; -}; - -#include -class Pose2 { - // Standard Constructor - Pose2(); - Pose2(const gtsam::Pose2& other); - Pose2(double x, double y, double theta); - Pose2(double theta, const gtsam::Point2& t); - Pose2(const gtsam::Rot2& r, const gtsam::Point2& t); - Pose2(Vector v); - - // Testable - void print(string s = "") const; - bool equals(const gtsam::Pose2& pose, double tol) const; - - // Group - static gtsam::Pose2 identity(); - gtsam::Pose2 inverse() const; - gtsam::Pose2 compose(const gtsam::Pose2& p2) const; - gtsam::Pose2 between(const gtsam::Pose2& p2) const; - - // Operator Overloads - gtsam::Pose2 operator*(const gtsam::Pose2& p2) const; - - // Manifold - gtsam::Pose2 retract(Vector v) const; - Vector localCoordinates(const gtsam::Pose2& p) const; - - // Lie Group - static gtsam::Pose2 Expmap(Vector v); - static Vector Logmap(const gtsam::Pose2& p); - Vector logmap(const gtsam::Pose2& p); - static Matrix ExpmapDerivative(Vector v); - static Matrix LogmapDerivative(const gtsam::Pose2& v); - Matrix AdjointMap() const; - Vector Adjoint(Vector xi) const; - static Matrix adjointMap_(Vector v); - static Vector adjoint_(Vector xi, Vector y); - static Vector adjointTranspose(Vector xi, Vector y); - static Matrix wedge(double vx, double vy, double w); - - // Group Actions on Point2 - gtsam::Point2 transformFrom(const gtsam::Point2& p) const; - gtsam::Point2 transformTo(const gtsam::Point2& p) const; - - // Standard Interface - double x() const; - double y() const; - double theta() const; - gtsam::Rot2 bearing(const gtsam::Point2& point) const; - double range(const gtsam::Point2& point) const; - gtsam::Point2 translation() const; - gtsam::Rot2 rotation() const; - Matrix matrix() const; - - // enabling serialization functionality - void serialize() const; - - // enable pickling in python - void pickle() const; -}; - -#include -class Pose3 { - // Standard Constructors - Pose3(); - Pose3(const gtsam::Pose3& other); - Pose3(const gtsam::Rot3& r, const gtsam::Point3& t); - Pose3(const gtsam::Pose2& pose2); - Pose3(Matrix mat); - - // Testable - void print(string s = "") const; - bool equals(const gtsam::Pose3& pose, double tol) const; - - // Group - static gtsam::Pose3 identity(); - gtsam::Pose3 inverse() const; - gtsam::Pose3 compose(const gtsam::Pose3& pose) const; - gtsam::Pose3 between(const gtsam::Pose3& pose) const; - - // Operator Overloads - gtsam::Pose3 operator*(const gtsam::Pose3& pose) const; - - // Manifold - gtsam::Pose3 retract(Vector v) const; - Vector localCoordinates(const gtsam::Pose3& pose) const; - - // Lie Group - static gtsam::Pose3 Expmap(Vector v); - static Vector Logmap(const gtsam::Pose3& pose); - gtsam::Pose3 expmap(Vector v); - Vector logmap(const gtsam::Pose3& pose); - Matrix AdjointMap() const; - Vector Adjoint(Vector xi) const; - static Matrix adjointMap_(Vector xi); - static Vector adjoint_(Vector xi, Vector y); - static Vector adjointTranspose(Vector xi, Vector y); - static Matrix ExpmapDerivative(Vector xi); - static Matrix LogmapDerivative(const gtsam::Pose3& xi); - static Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz); - - // Group Action on Point3 - gtsam::Point3 transformFrom(const gtsam::Point3& point) const; - gtsam::Point3 transformTo(const gtsam::Point3& point) const; - - // Standard Interface - gtsam::Rot3 rotation() const; - gtsam::Point3 translation() const; - double x() const; - double y() const; - double z() const; - Matrix matrix() const; - gtsam::Pose3 transformPoseFrom(const gtsam::Pose3& pose) const; - gtsam::Pose3 transformPoseTo(const gtsam::Pose3& pose) const; - double range(const gtsam::Point3& point); - double range(const gtsam::Pose3& pose); - - // enabling serialization functionality - void serialize() const; - - // enable pickling in python - void pickle() const; -}; - -class Pose3Pairs { - Pose3Pairs(); - size_t size() const; - bool empty() const; - gtsam::Pose3Pair at(size_t n) const; - void push_back(const gtsam::Pose3Pair& pose_pair); -}; - -class Pose3Vector -{ - Pose3Vector(); - size_t size() const; - bool empty() const; - gtsam::Pose3 at(size_t n) const; - void push_back(const gtsam::Pose3& pose); -}; - -#include -class Unit3 { - // Standard Constructors - Unit3(); - Unit3(const gtsam::Point3& pose); - - // Testable - void print(string s = "") const; - bool equals(const gtsam::Unit3& pose, double tol) const; - - // Other functionality - Matrix basis() const; - Matrix skew() const; - gtsam::Point3 point3() const; - - // Manifold - static size_t Dim(); - size_t dim() const; - gtsam::Unit3 retract(Vector v) const; - Vector localCoordinates(const gtsam::Unit3& s) const; - - // enabling serialization functionality - void serialize() const; - - // enable pickling in python - void pickle() const; - - // enabling function to compare objects - bool equals(const gtsam::Unit3& expected, double tol) const; -}; - -#include -class EssentialMatrix { - // Standard Constructors - EssentialMatrix(const gtsam::Rot3& aRb, const gtsam::Unit3& aTb); - - // Testable - void print(string s = "") const; - bool equals(const gtsam::EssentialMatrix& pose, double tol) const; - - // Manifold - static size_t Dim(); - size_t dim() const; - gtsam::EssentialMatrix retract(Vector v) const; - Vector localCoordinates(const gtsam::EssentialMatrix& s) const; - - // Other methods: - gtsam::Rot3 rotation() const; - gtsam::Unit3 direction() const; - Matrix matrix() const; - double error(Vector vA, Vector vB); -}; - -#include -class Cal3_S2 { - // Standard Constructors - Cal3_S2(); - Cal3_S2(double fx, double fy, double s, double u0, double v0); - Cal3_S2(Vector v); - Cal3_S2(double fov, int w, int h); - - // Testable - void print(string s = "Cal3_S2") const; - bool equals(const gtsam::Cal3_S2& rhs, double tol) const; - - // Manifold - static size_t Dim(); - size_t dim() const; - gtsam::Cal3_S2 retract(Vector v) const; - Vector localCoordinates(const gtsam::Cal3_S2& c) const; - - // Action on Point2 - 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 skew() const; - double px() const; - double py() const; - gtsam::Point2 principalPoint() const; - Vector vector() const; - Matrix K() const; - Matrix inverse() const; - - // enabling serialization functionality - void serialize() const; - - // enable pickling in python - void pickle() const; -}; - -#include -virtual class Cal3DS2_Base { - // Standard Constructors - Cal3DS2_Base(); - - // Testable - void print(string s = "") const; - - // Standard Interface - double fx() const; - double fy() const; - double skew() const; - double px() const; - double py() const; - double k1() const; - double k2() const; - Matrix K() const; - Vector k() const; - Vector vector() const; - - // Action on Point2 - gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; - gtsam::Point2 calibrate(const gtsam::Point2& p) const; - - // enabling serialization functionality - void serialize() const; - - // enable pickling in python - void pickle() const; -}; - -#include -virtual class Cal3DS2 : gtsam::Cal3DS2_Base { - // Standard Constructors - Cal3DS2(); - Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2); - Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2, double p1, double p2); - Cal3DS2(Vector v); - - // Testable - bool equals(const gtsam::Cal3DS2& rhs, double tol) const; - - // Manifold - size_t dim() const; - static size_t Dim(); - gtsam::Cal3DS2 retract(Vector v) const; - Vector localCoordinates(const gtsam::Cal3DS2& c) const; - - // enabling serialization functionality - void serialize() const; - - // enable pickling in python - void pickle() const; -}; - -#include -virtual class Cal3Unified : gtsam::Cal3DS2_Base { - // Standard Constructors - Cal3Unified(); - Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1, double k2); - Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1, double k2, double p1, double p2, double xi); - Cal3Unified(Vector v); - - // Testable - bool equals(const gtsam::Cal3Unified& rhs, double tol) const; - - // Standard Interface - double xi() const; - gtsam::Point2 spaceToNPlane(const gtsam::Point2& p) const; - gtsam::Point2 nPlaneToSpace(const gtsam::Point2& p) const; - - // Manifold - size_t dim() const; - static size_t Dim(); - gtsam::Cal3Unified retract(Vector v) const; - Vector localCoordinates(const gtsam::Cal3Unified& c) const; - - // enabling serialization functionality - void serialize() const; - - // enable pickling in python - void pickle() const; -}; - -#include -class Cal3_S2Stereo { - // Standard Constructors - Cal3_S2Stereo(); - Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b); - Cal3_S2Stereo(Vector v); - - // Testable - void print(string s = "") const; - bool equals(const gtsam::Cal3_S2Stereo& K, double tol) const; - - // Standard Interface - double fx() const; - double fy() const; - double skew() const; - double px() const; - double py() const; - gtsam::Point2 principalPoint() const; - double baseline() const; -}; - -#include -class Cal3Bundler { - // Standard Constructors - Cal3Bundler(); - Cal3Bundler(double fx, double k1, double k2, double u0, double v0); - Cal3Bundler(double fx, double k1, double k2, double u0, double v0, double tol); - - // 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) const; - gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; - - // Standard Interface - double fx() const; - double fy() const; - double k1() const; - double k2() const; - double px() const; - double py() const; - Vector vector() const; - Vector k() const; - Matrix K() const; - - // enabling serialization functionality - void serialize() const; - - // enable pickling in python - void pickle() const; -}; - -#include -class CalibratedCamera { - // Standard Constructors and Named Constructors - CalibratedCamera(); - CalibratedCamera(const gtsam::Pose3& pose); - CalibratedCamera(Vector v); - static gtsam::CalibratedCamera Level(const gtsam::Pose2& pose2, double height); - - // Testable - void print(string s = "CalibratedCamera") const; - bool equals(const gtsam::CalibratedCamera& camera, double tol) const; - - // Manifold - static size_t Dim(); - size_t dim() const; - gtsam::CalibratedCamera retract(Vector d) const; - Vector localCoordinates(const gtsam::CalibratedCamera& T2) const; - - // Action on Point3 - gtsam::Point2 project(const gtsam::Point3& point) const; - static gtsam::Point2 Project(const gtsam::Point3& cameraPoint); - - // Standard Interface - gtsam::Pose3 pose() const; - double range(const gtsam::Point3& point) const; - double range(const gtsam::Pose3& pose) const; - double range(const gtsam::CalibratedCamera& camera) const; - - // enabling serialization functionality - void serialize() const; - - // enable pickling in python - void pickle() const; -}; - -#include -template -class PinholeCamera { - // Standard Constructors and Named Constructors - PinholeCamera(); - PinholeCamera(const gtsam::Pose3& pose); - PinholeCamera(const gtsam::Pose3& pose, const CALIBRATION& K); - static This Level(const CALIBRATION& K, const gtsam::Pose2& pose, double height); - static This Level(const gtsam::Pose2& pose, double height); - static This Lookat(const gtsam::Point3& eye, const gtsam::Point3& target, - const gtsam::Point3& upVector, const CALIBRATION& K); - - // Testable - void print(string s = "PinholeCamera") const; - bool equals(const This& camera, double tol) const; - - // Standard Interface - gtsam::Pose3 pose() const; - CALIBRATION calibration() const; - - // Manifold - This retract(Vector d) const; - Vector localCoordinates(const This& T2) const; - size_t dim() const; - static size_t Dim(); - - // Transformations and measurement functions - static gtsam::Point2 Project(const gtsam::Point3& cameraPoint); - pair projectSafe(const gtsam::Point3& pw) const; - gtsam::Point2 project(const gtsam::Point3& point); - gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const; - double range(const gtsam::Point3& point); - double range(const gtsam::Pose3& pose); - - // enabling serialization functionality - void serialize() const; - - // enable pickling in python - void pickle() const; -}; - - -#include -class Similarity3 { - // Standard Constructors - Similarity3(); - Similarity3(double s); - Similarity3(const gtsam::Rot3& R, const gtsam::Point3& t, double s); - Similarity3(const Matrix& R, const Vector& t, double s); - Similarity3(const Matrix& T); - - gtsam::Point3 transformFrom(const gtsam::Point3& p) const; - gtsam::Pose3 transformFrom(const gtsam::Pose3& T); - - static gtsam::Similarity3 Align(const gtsam::Point3Pairs & abPointPairs); - static gtsam::Similarity3 Align(const gtsam::Pose3Pairs & abPosePairs); - - // Standard Interface - const Matrix matrix() const; - const gtsam::Rot3& rotation(); - const gtsam::Point3& translation(); - double scale() const; -}; - - -// Forward declaration of PinholeCameraCalX is defined here. -#include -// Some typedefs for common camera types -// PinholeCameraCal3_S2 is the same as SimpleCamera above -typedef gtsam::PinholeCamera PinholeCameraCal3_S2; -typedef gtsam::PinholeCamera PinholeCameraCal3DS2; -typedef gtsam::PinholeCamera PinholeCameraCal3Unified; -typedef gtsam::PinholeCamera PinholeCameraCal3Bundler; - -template -class CameraSet { - CameraSet(); - - // structure specific methods - T at(size_t i) const; - void push_back(const T& cam); -}; - -#include -class StereoCamera { - // Standard Constructors and Named Constructors - StereoCamera(); - StereoCamera(const gtsam::Pose3& pose, const gtsam::Cal3_S2Stereo* K); - - // Testable - void print(string s = "") const; - bool equals(const gtsam::StereoCamera& camera, double tol) const; - - // Standard Interface - gtsam::Pose3 pose() const; - double baseline() const; - gtsam::Cal3_S2Stereo calibration() const; - - // Manifold - gtsam::StereoCamera retract(Vector d) const; - Vector localCoordinates(const gtsam::StereoCamera& T2) const; - size_t dim() const; - static size_t Dim(); - - // Transformations and measurement functions - gtsam::StereoPoint2 project(const gtsam::Point3& point); - gtsam::Point3 backproject(const gtsam::StereoPoint2& p) const; - - // enabling serialization functionality - void serialize() const; - - // enable pickling in python - void pickle() const; -}; - -#include - -// Templates appear not yet supported for free functions - issue raised at borglab/wrap#14 to add support -gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, - gtsam::Cal3_S2* sharedCal, const gtsam::Point2Vector& measurements, - double rank_tol, bool optimize); -gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, - gtsam::Cal3DS2* sharedCal, const gtsam::Point2Vector& measurements, - double rank_tol, bool optimize); -gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, - gtsam::Cal3Bundler* sharedCal, const gtsam::Point2Vector& measurements, - double rank_tol, bool optimize); -gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3_S2& cameras, - const gtsam::Point2Vector& measurements, double rank_tol, - bool optimize); -gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Bundler& cameras, - const gtsam::Point2Vector& measurements, double rank_tol, - bool optimize); - -//************************************************************************* -// Symbolic -//************************************************************************* - -#include -virtual class SymbolicFactor { - // Standard Constructors and Named Constructors - SymbolicFactor(const gtsam::SymbolicFactor& f); - SymbolicFactor(); - SymbolicFactor(size_t j); - SymbolicFactor(size_t j1, size_t j2); - SymbolicFactor(size_t j1, size_t j2, size_t j3); - SymbolicFactor(size_t j1, size_t j2, size_t j3, size_t j4); - SymbolicFactor(size_t j1, size_t j2, size_t j3, size_t j4, size_t j5); - SymbolicFactor(size_t j1, size_t j2, size_t j3, size_t j4, size_t j5, size_t j6); - static gtsam::SymbolicFactor FromKeys(const gtsam::KeyVector& js); - - // From Factor - size_t size() const; - void print(string s = "SymbolicFactor", - const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; - bool equals(const gtsam::SymbolicFactor& other, double tol) const; - gtsam::KeyVector keys(); -}; - -#include -virtual class SymbolicFactorGraph { - SymbolicFactorGraph(); - SymbolicFactorGraph(const gtsam::SymbolicBayesNet& bayesNet); - SymbolicFactorGraph(const gtsam::SymbolicBayesTree& bayesTree); - - // From FactorGraph - void push_back(gtsam::SymbolicFactor* factor); - void print(string s = "SymbolicFactorGraph", - const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; - bool equals(const gtsam::SymbolicFactorGraph& rhs, double tol) const; - size_t size() const; - bool exists(size_t idx) const; - - // Standard interface - gtsam::KeySet keys() const; - void push_back(const gtsam::SymbolicFactorGraph& graph); - void push_back(const gtsam::SymbolicBayesNet& bayesNet); - void push_back(const gtsam::SymbolicBayesTree& bayesTree); - - //Advanced Interface - void push_factor(size_t key); - void push_factor(size_t key1, size_t key2); - void push_factor(size_t key1, size_t key2, size_t key3); - void push_factor(size_t key1, size_t key2, size_t key3, size_t key4); - - gtsam::SymbolicBayesNet* eliminateSequential(); - gtsam::SymbolicBayesNet* eliminateSequential(const gtsam::Ordering& ordering); - gtsam::SymbolicBayesTree* eliminateMultifrontal(); - gtsam::SymbolicBayesTree* eliminateMultifrontal(const gtsam::Ordering& ordering); - pair eliminatePartialSequential( - const gtsam::Ordering& ordering); - pair eliminatePartialSequential( - const gtsam::KeyVector& keys); - pair eliminatePartialMultifrontal( - const gtsam::Ordering& ordering); - pair eliminatePartialMultifrontal( - const gtsam::KeyVector& keys); - gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& ordering); - gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& key_vector); - gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& ordering, - const gtsam::Ordering& marginalizedVariableOrdering); - gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& key_vector, - const gtsam::Ordering& marginalizedVariableOrdering); - gtsam::SymbolicFactorGraph* marginal(const gtsam::KeyVector& key_vector); -}; - -#include -virtual class SymbolicConditional : gtsam::SymbolicFactor { - // Standard Constructors and Named Constructors - SymbolicConditional(); - SymbolicConditional(const gtsam::SymbolicConditional& other); - SymbolicConditional(size_t key); - SymbolicConditional(size_t key, size_t parent); - SymbolicConditional(size_t key, size_t parent1, size_t parent2); - SymbolicConditional(size_t key, size_t parent1, size_t parent2, size_t parent3); - static gtsam::SymbolicConditional FromKeys(const gtsam::KeyVector& keys, size_t nrFrontals); - - // Testable - void print(string s = "", const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; - bool equals(const gtsam::SymbolicConditional& other, double tol) const; - - // Standard interface - size_t nrFrontals() const; - size_t nrParents() const; -}; - -#include -class SymbolicBayesNet { - SymbolicBayesNet(); - SymbolicBayesNet(const gtsam::SymbolicBayesNet& other); - // Testable - void print(string s = "SymbolicBayesNet", - const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; - bool equals(const gtsam::SymbolicBayesNet& other, double tol) const; - - // Standard interface - size_t size() const; - void saveGraph(string s) const; - gtsam::SymbolicConditional* at(size_t idx) const; - gtsam::SymbolicConditional* front() const; - gtsam::SymbolicConditional* back() const; - void push_back(gtsam::SymbolicConditional* conditional); - void push_back(const gtsam::SymbolicBayesNet& bayesNet); -}; - -#include -class SymbolicBayesTree { - - //Constructors - SymbolicBayesTree(); - SymbolicBayesTree(const gtsam::SymbolicBayesTree& other); - - // Testable - void print(string s = "", const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter); - bool equals(const gtsam::SymbolicBayesTree& other, double tol) const; - - //Standard Interface - //size_t findParentClique(const gtsam::IndexVector& parents) const; - size_t size(); - void saveGraph(string s) const; - void clear(); - void deleteCachedShortcuts(); - size_t numCachedSeparatorMarginals() const; - - gtsam::SymbolicConditional* marginalFactor(size_t key) const; - gtsam::SymbolicFactorGraph* joint(size_t key1, size_t key2) const; - gtsam::SymbolicBayesNet* jointBayesNet(size_t key1, size_t key2) const; -}; - -class SymbolicBayesTreeClique { - SymbolicBayesTreeClique(); - // SymbolicBayesTreeClique(gtsam::sharedConditional* conditional); - - bool equals(const gtsam::SymbolicBayesTreeClique& other, double tol) const; - void print(string s = "", - const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; - size_t numCachedSeparatorMarginals() const; - // gtsam::sharedConditional* conditional() const; - bool isRoot() const; - size_t treeSize() const; - gtsam::SymbolicBayesTreeClique* parent() const; - -// // TODO: need wrapped versions graphs, BayesNet -// BayesNet shortcut(derived_ptr root, Eliminate function) const; -// FactorGraph marginal(derived_ptr root, Eliminate function) const; -// FactorGraph joint(derived_ptr C2, derived_ptr root, Eliminate function) const; -// - void deleteCachedShortcuts(); -}; - -#include -class VariableIndex { - // Standard Constructors and Named Constructors - VariableIndex(); - // TODO: Templetize constructor when wrap supports it - //template - //VariableIndex(const T& factorGraph, size_t nVariables); - //VariableIndex(const T& factorGraph); - VariableIndex(const gtsam::SymbolicFactorGraph& sfg); - VariableIndex(const gtsam::GaussianFactorGraph& gfg); - VariableIndex(const gtsam::NonlinearFactorGraph& fg); - VariableIndex(const gtsam::VariableIndex& other); - - // Testable - bool equals(const gtsam::VariableIndex& other, double tol) const; - void print(string s = "VariableIndex: ", - const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; - - // Standard interface - size_t size() const; - size_t nFactors() const; - size_t nEntries() const; -}; - -//************************************************************************* -// linear -//************************************************************************* - -namespace noiseModel { -#include -virtual class Base { - void print(string s = "") const; - // Methods below are available for all noise models. However, can't add them - // because wrap (incorrectly) thinks robust classes derive from this Base as well. - // bool isConstrained() const; - // bool isUnit() const; - // size_t dim() const; - // Vector sigmas() const; -}; - -virtual class Gaussian : gtsam::noiseModel::Base { - static gtsam::noiseModel::Gaussian* Information(Matrix R); - static gtsam::noiseModel::Gaussian* SqrtInformation(Matrix R); - static gtsam::noiseModel::Gaussian* Covariance(Matrix R); - - bool equals(gtsam::noiseModel::Base& expected, double tol); - - // access to noise model - Matrix R() const; - Matrix information() const; - Matrix covariance() const; - - // Whitening operations - Vector whiten(Vector v) const; - Vector unwhiten(Vector v) const; - Matrix Whiten(Matrix H) const; - - // enabling serialization functionality - void serializable() const; -}; - -virtual class Diagonal : gtsam::noiseModel::Gaussian { - static gtsam::noiseModel::Diagonal* Sigmas(Vector sigmas); - static gtsam::noiseModel::Diagonal* Variances(Vector variances); - static gtsam::noiseModel::Diagonal* Precisions(Vector precisions); - Matrix R() const; - - // access to noise model - Vector sigmas() const; - Vector invsigmas() const; - Vector precisions() const; - - // enabling serialization functionality - void serializable() const; -}; - -virtual class Constrained : gtsam::noiseModel::Diagonal { - static gtsam::noiseModel::Constrained* MixedSigmas(Vector mu, Vector sigmas); - static gtsam::noiseModel::Constrained* MixedSigmas(double m, Vector sigmas); - static gtsam::noiseModel::Constrained* MixedVariances(Vector mu, Vector variances); - static gtsam::noiseModel::Constrained* MixedVariances(Vector variances); - static gtsam::noiseModel::Constrained* MixedPrecisions(Vector mu, Vector precisions); - static gtsam::noiseModel::Constrained* MixedPrecisions(Vector precisions); - - static gtsam::noiseModel::Constrained* All(size_t dim); - static gtsam::noiseModel::Constrained* All(size_t dim, double mu); - - gtsam::noiseModel::Constrained* unit() const; - - // enabling serialization functionality - void serializable() const; -}; - -virtual class Isotropic : gtsam::noiseModel::Diagonal { - static gtsam::noiseModel::Isotropic* Sigma(size_t dim, double sigma); - static gtsam::noiseModel::Isotropic* Variance(size_t dim, double varianace); - static gtsam::noiseModel::Isotropic* Precision(size_t dim, double precision); - - // access to noise model - double sigma() const; - - // enabling serialization functionality - void serializable() const; -}; - -virtual class Unit : gtsam::noiseModel::Isotropic { - static gtsam::noiseModel::Unit* Create(size_t dim); - - // enabling serialization functionality - void serializable() const; -}; - -namespace mEstimator { -virtual class Base { - void print(string s = "") const; -}; - -virtual class Null: gtsam::noiseModel::mEstimator::Base { - Null(); - static gtsam::noiseModel::mEstimator::Null* Create(); - - // enabling serialization functionality - void serializable() const; - - double weight(double error) const; - double loss(double error) const; -}; - -virtual class Fair: gtsam::noiseModel::mEstimator::Base { - Fair(double c); - static gtsam::noiseModel::mEstimator::Fair* Create(double c); - - // enabling serialization functionality - void serializable() const; - - double weight(double error) const; - double loss(double error) const; -}; - -virtual class Huber: gtsam::noiseModel::mEstimator::Base { - Huber(double k); - static gtsam::noiseModel::mEstimator::Huber* Create(double k); - - // enabling serialization functionality - void serializable() const; - - double weight(double error) const; - double loss(double error) const; -}; - -virtual class Cauchy: gtsam::noiseModel::mEstimator::Base { - Cauchy(double k); - static gtsam::noiseModel::mEstimator::Cauchy* Create(double k); - - // enabling serialization functionality - void serializable() const; - - double weight(double error) const; - double loss(double error) const; -}; - -virtual class Tukey: gtsam::noiseModel::mEstimator::Base { - Tukey(double k); - static gtsam::noiseModel::mEstimator::Tukey* Create(double k); - - // enabling serialization functionality - void serializable() const; - - double weight(double error) const; - double loss(double error) const; -}; - -virtual class Welsch: gtsam::noiseModel::mEstimator::Base { - Welsch(double k); - static gtsam::noiseModel::mEstimator::Welsch* Create(double k); - - // enabling serialization functionality - void serializable() const; - - double weight(double error) const; - double loss(double error) const; -}; - -virtual class GemanMcClure: gtsam::noiseModel::mEstimator::Base { - GemanMcClure(double c); - static gtsam::noiseModel::mEstimator::GemanMcClure* Create(double c); - - // enabling serialization functionality - void serializable() const; - - double weight(double error) const; - double loss(double error) const; -}; - -virtual class DCS: gtsam::noiseModel::mEstimator::Base { - DCS(double c); - static gtsam::noiseModel::mEstimator::DCS* Create(double c); - - // enabling serialization functionality - void serializable() const; - - double weight(double error) const; - double loss(double error) const; -}; - -virtual class L2WithDeadZone: gtsam::noiseModel::mEstimator::Base { - L2WithDeadZone(double k); - static gtsam::noiseModel::mEstimator::L2WithDeadZone* Create(double k); - - // enabling serialization functionality - void serializable() const; - - double weight(double error) const; - double loss(double error) const; -}; - -}///\namespace mEstimator - -virtual class Robust : gtsam::noiseModel::Base { - Robust(const gtsam::noiseModel::mEstimator::Base* robust, const gtsam::noiseModel::Base* noise); - static gtsam::noiseModel::Robust* Create(const gtsam::noiseModel::mEstimator::Base* robust, const gtsam::noiseModel::Base* noise); - - // enabling serialization functionality - void serializable() const; -}; - -}///\namespace noiseModel - -#include -class Sampler { - // Constructors - Sampler(gtsam::noiseModel::Diagonal* model, int seed); - Sampler(Vector sigmas, int seed); - - // Standard Interface - size_t dim() const; - Vector sigmas() const; - gtsam::noiseModel::Diagonal* model() const; - Vector sample(); -}; - -#include -class VectorValues { - //Constructors - VectorValues(); - VectorValues(const gtsam::VectorValues& other); - - //Named Constructors - static gtsam::VectorValues Zero(const gtsam::VectorValues& model); - - //Standard Interface - size_t size() const; - size_t dim(size_t j) const; - bool exists(size_t j) const; - void print(string s = "VectorValues", - const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; - bool equals(const gtsam::VectorValues& expected, double tol) const; - void insert(size_t j, Vector value); - Vector vector() const; - Vector at(size_t j) const; - void update(const gtsam::VectorValues& values); - - //Advanced Interface - void setZero(); - - gtsam::VectorValues add(const gtsam::VectorValues& c) const; - void addInPlace(const gtsam::VectorValues& c); - gtsam::VectorValues subtract(const gtsam::VectorValues& c) const; - gtsam::VectorValues scale(double a) const; - void scaleInPlace(double a); - - bool hasSameStructure(const gtsam::VectorValues& other) const; - double dot(const gtsam::VectorValues& V) const; - double norm() const; - double squaredNorm() const; - - // enabling serialization functionality - void serialize() const; - - // enable pickling in python - void pickle() const; -}; - -#include -virtual class GaussianFactor { - gtsam::KeyVector keys() const; - void print(string s = "", const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; - bool equals(const gtsam::GaussianFactor& lf, double tol) const; - double error(const gtsam::VectorValues& c) const; - gtsam::GaussianFactor* clone() const; - gtsam::GaussianFactor* negate() const; - Matrix augmentedInformation() const; - Matrix information() const; - Matrix augmentedJacobian() const; - pair jacobian() const; - size_t size() const; - bool empty() const; -}; - -#include -virtual class JacobianFactor : gtsam::GaussianFactor { - //Constructors - JacobianFactor(); - JacobianFactor(const gtsam::GaussianFactor& factor); - JacobianFactor(Vector b_in); - JacobianFactor(size_t i1, Matrix A1, Vector b, - const gtsam::noiseModel::Diagonal* model); - JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, Vector b, - const gtsam::noiseModel::Diagonal* model); - JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, size_t i3, Matrix A3, - Vector b, const gtsam::noiseModel::Diagonal* model); - JacobianFactor(const gtsam::GaussianFactorGraph& graph); - - //Testable - void print(string s = "", const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; - void printKeys(string s) const; - bool equals(const gtsam::GaussianFactor& lf, double tol) const; - size_t size() const; - Vector unweighted_error(const gtsam::VectorValues& c) const; - Vector error_vector(const gtsam::VectorValues& c) const; - double error(const gtsam::VectorValues& c) const; - - //Standard Interface - Matrix getA() const; - Vector getb() const; - size_t rows() const; - size_t cols() const; - bool isConstrained() const; - pair jacobianUnweighted() const; - Matrix augmentedJacobianUnweighted() const; - - void transposeMultiplyAdd(double alpha, Vector e, gtsam::VectorValues& x) const; - gtsam::JacobianFactor whiten() const; - - pair eliminate(const gtsam::Ordering& keys) const; - - void setModel(bool anyConstrained, Vector sigmas); - - gtsam::noiseModel::Diagonal* get_model() const; - - // enabling serialization functionality - void serialize() const; - - // enable pickling in python - void pickle() const; -}; - -#include -virtual class HessianFactor : gtsam::GaussianFactor { - //Constructors - HessianFactor(); - HessianFactor(const gtsam::GaussianFactor& factor); - HessianFactor(size_t j, Matrix G, Vector g, double f); - HessianFactor(size_t j, Vector mu, Matrix Sigma); - HessianFactor(size_t j1, size_t j2, Matrix G11, Matrix G12, Vector g1, Matrix G22, - Vector g2, double f); - HessianFactor(size_t j1, size_t j2, size_t j3, Matrix G11, Matrix G12, Matrix G13, - Vector g1, Matrix G22, Matrix G23, Vector g2, Matrix G33, Vector g3, - double f); - HessianFactor(const gtsam::GaussianFactorGraph& factors); - - //Testable - size_t size() const; - void print(string s = "", const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; - void printKeys(string s) const; - bool equals(const gtsam::GaussianFactor& lf, double tol) const; - double error(const gtsam::VectorValues& c) const; - - //Standard Interface - size_t rows() const; - Matrix information() const; - double constantTerm() const; - Vector linearTerm() const; - - // enabling serialization functionality - void serialize() const; - - // enable pickling in python - void pickle() const; -}; - -#include -class GaussianFactorGraph { - GaussianFactorGraph(); - GaussianFactorGraph(const gtsam::GaussianBayesNet& bayesNet); - GaussianFactorGraph(const gtsam::GaussianBayesTree& bayesTree); - - // From FactorGraph - void print(string s = "", const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; - bool equals(const gtsam::GaussianFactorGraph& lfgraph, double tol) const; - size_t size() const; - gtsam::GaussianFactor* at(size_t idx) const; - gtsam::KeySet keys() const; - gtsam::KeyVector keyVector() const; - bool exists(size_t idx) const; - - // Building the graph - void push_back(const gtsam::GaussianFactor* factor); - void push_back(const gtsam::GaussianConditional* conditional); - void push_back(const gtsam::GaussianFactorGraph& graph); - void push_back(const gtsam::GaussianBayesNet& bayesNet); - void push_back(const gtsam::GaussianBayesTree& bayesTree); - void add(const gtsam::GaussianFactor& factor); - void add(Vector b); - void add(size_t key1, Matrix A1, Vector b, const gtsam::noiseModel::Diagonal* model); - void add(size_t key1, Matrix A1, size_t key2, Matrix A2, Vector b, - const gtsam::noiseModel::Diagonal* model); - void add(size_t key1, Matrix A1, size_t key2, Matrix A2, size_t key3, Matrix A3, - Vector b, const gtsam::noiseModel::Diagonal* model); - - // error and probability - double error(const gtsam::VectorValues& c) const; - double probPrime(const gtsam::VectorValues& c) const; - - gtsam::GaussianFactorGraph clone() const; - gtsam::GaussianFactorGraph negate() const; - - // Optimizing and linear algebra - gtsam::VectorValues optimize() const; - gtsam::VectorValues optimize(const gtsam::Ordering& ordering) const; - gtsam::VectorValues optimizeGradientSearch() const; - gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const; - gtsam::VectorValues gradientAtZero() const; - - // Elimination and marginals - gtsam::GaussianBayesNet* eliminateSequential(); - gtsam::GaussianBayesNet* eliminateSequential(const gtsam::Ordering& ordering); - gtsam::GaussianBayesTree* eliminateMultifrontal(); - gtsam::GaussianBayesTree* eliminateMultifrontal(const gtsam::Ordering& ordering); - pair eliminatePartialSequential( - const gtsam::Ordering& ordering); - pair eliminatePartialSequential( - const gtsam::KeyVector& keys); - pair eliminatePartialMultifrontal( - const gtsam::Ordering& ordering); - pair eliminatePartialMultifrontal( - const gtsam::KeyVector& keys); - gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& ordering); - gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& key_vector); - gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& ordering, - const gtsam::Ordering& marginalizedVariableOrdering); - gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& key_vector, - const gtsam::Ordering& marginalizedVariableOrdering); - gtsam::GaussianFactorGraph* marginal(const gtsam::KeyVector& key_vector); - - // Conversion to matrices - Matrix sparseJacobian_() const; - Matrix augmentedJacobian() const; - Matrix augmentedJacobian(const gtsam::Ordering& ordering) const; - pair jacobian() const; - pair jacobian(const gtsam::Ordering& ordering) const; - Matrix augmentedHessian() const; - Matrix augmentedHessian(const gtsam::Ordering& ordering) const; - pair hessian() const; - pair hessian(const gtsam::Ordering& ordering) const; - - // enabling serialization functionality - void serialize() const; - - // enable pickling in python - void pickle() const; -}; - -#include -virtual class GaussianConditional : gtsam::JacobianFactor { - //Constructors - GaussianConditional(size_t key, Vector d, Matrix R, const gtsam::noiseModel::Diagonal* sigmas); - GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, - const gtsam::noiseModel::Diagonal* sigmas); - GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, - size_t name2, Matrix T, const gtsam::noiseModel::Diagonal* sigmas); - - //Constructors with no noise model - GaussianConditional(size_t key, Vector d, Matrix R); - GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S); - GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, - size_t name2, Matrix T); - - //Standard Interface - void print(string s = "GaussianConditional", - const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; - bool equals(const gtsam::GaussianConditional& cg, double tol) const; - - // Advanced Interface - gtsam::VectorValues solve(const gtsam::VectorValues& parents) const; - gtsam::VectorValues solveOtherRHS(const gtsam::VectorValues& parents, - const gtsam::VectorValues& rhs) const; - void solveTransposeInPlace(gtsam::VectorValues& gy) const; - void scaleFrontalsBySigma(gtsam::VectorValues& gy) const; - Matrix R() const; - Matrix S() const; - Vector d() const; - - // enabling serialization functionality - void serialize() const; -}; - -#include -virtual class GaussianDensity : gtsam::GaussianConditional { - //Constructors - GaussianDensity(size_t key, Vector d, Matrix R, const gtsam::noiseModel::Diagonal* sigmas); - - //Standard Interface - void print(string s = "GaussianDensity", - const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; - bool equals(const gtsam::GaussianDensity &cg, double tol) const; - Vector mean() const; - Matrix covariance() const; -}; - -#include -virtual class GaussianBayesNet { - //Constructors - GaussianBayesNet(); - GaussianBayesNet(const gtsam::GaussianConditional* conditional); - - // Testable - void print(string s = "", const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; - bool equals(const gtsam::GaussianBayesNet& other, double tol) const; - size_t size() const; - - // FactorGraph derived interface - // size_t size() const; - gtsam::GaussianConditional* at(size_t idx) const; - gtsam::KeySet keys() const; - bool exists(size_t idx) const; - - void saveGraph(const string& s) const; - - gtsam::GaussianConditional* front() const; - gtsam::GaussianConditional* back() const; - void push_back(gtsam::GaussianConditional* conditional); - void push_back(const gtsam::GaussianBayesNet& bayesNet); - - gtsam::VectorValues optimize() const; - gtsam::VectorValues optimize(gtsam::VectorValues& solutionForMissing) const; - gtsam::VectorValues optimizeGradientSearch() const; - gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const; - gtsam::VectorValues gradientAtZero() const; - double error(const gtsam::VectorValues& x) const; - double determinant() const; - double logDeterminant() const; - gtsam::VectorValues backSubstitute(const gtsam::VectorValues& gx) const; - gtsam::VectorValues backSubstituteTranspose(const gtsam::VectorValues& gx) const; -}; - -#include -virtual class GaussianBayesTree { - // Standard Constructors and Named Constructors - GaussianBayesTree(); - GaussianBayesTree(const gtsam::GaussianBayesTree& other); - bool equals(const gtsam::GaussianBayesTree& other, double tol) const; - void print(string s = "", const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter); - size_t size() const; - bool empty() const; - size_t numCachedSeparatorMarginals() const; - void saveGraph(string s) const; - - gtsam::VectorValues optimize() const; - gtsam::VectorValues optimizeGradientSearch() const; - gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const; - gtsam::VectorValues gradientAtZero() const; - double error(const gtsam::VectorValues& x) const; - double determinant() const; - double logDeterminant() const; - Matrix marginalCovariance(size_t key) const; - gtsam::GaussianConditional* marginalFactor(size_t key) const; - gtsam::GaussianFactorGraph* joint(size_t key1, size_t key2) const; - gtsam::GaussianBayesNet* jointBayesNet(size_t key1, size_t key2) const; -}; - -#include -class Errors { - //Constructors - Errors(); - Errors(const gtsam::VectorValues& V); - - //Testable - void print(string s = "Errors"); - bool equals(const gtsam::Errors& expected, double tol) const; -}; - -#include -class GaussianISAM { - //Constructor - GaussianISAM(); - - //Standard Interface - void update(const gtsam::GaussianFactorGraph& newFactors); - void saveGraph(string s) const; - void clear(); -}; - -#include -virtual class IterativeOptimizationParameters { - string getVerbosity() const; - void setVerbosity(string s) ; -}; - -//virtual class IterativeSolver { -// IterativeSolver(); -// gtsam::VectorValues optimize (); -//}; - -#include -virtual class ConjugateGradientParameters : gtsam::IterativeOptimizationParameters { - ConjugateGradientParameters(); - int getMinIterations() const ; - int getMaxIterations() const ; - int getReset() const; - double getEpsilon_rel() const; - double getEpsilon_abs() const; - - void setMinIterations(int value); - void setMaxIterations(int value); - void setReset(int value); - void setEpsilon_rel(double value); - void setEpsilon_abs(double value); -}; - -#include -virtual class PreconditionerParameters { - PreconditionerParameters(); -}; - -virtual class DummyPreconditionerParameters : gtsam::PreconditionerParameters { - DummyPreconditionerParameters(); -}; - -#include -virtual class PCGSolverParameters : gtsam::ConjugateGradientParameters { - PCGSolverParameters(); - void print(string s = ""); - void setPreconditionerParams(gtsam::PreconditionerParameters* preconditioner); -}; - -#include -virtual class SubgraphSolverParameters : gtsam::ConjugateGradientParameters { - SubgraphSolverParameters(); -}; - -virtual class SubgraphSolver { - SubgraphSolver(const gtsam::GaussianFactorGraph &A, const gtsam::SubgraphSolverParameters ¶meters, const gtsam::Ordering& ordering); - SubgraphSolver(const gtsam::GaussianFactorGraph &Ab1, const gtsam::GaussianFactorGraph* Ab2, const gtsam::SubgraphSolverParameters ¶meters, const gtsam::Ordering& ordering); - gtsam::VectorValues optimize() const; -}; - -#include -class KalmanFilter { - KalmanFilter(size_t n); - // gtsam::GaussianDensity* init(Vector x0, const gtsam::SharedDiagonal& P0); - gtsam::GaussianDensity* init(Vector x0, Matrix P0); - void print(string s = "") const; - static size_t step(gtsam::GaussianDensity* p); - gtsam::GaussianDensity* predict(gtsam::GaussianDensity* p, Matrix F, - Matrix B, Vector u, const gtsam::noiseModel::Diagonal* modelQ); - gtsam::GaussianDensity* predictQ(gtsam::GaussianDensity* p, Matrix F, - Matrix B, Vector u, Matrix Q); - gtsam::GaussianDensity* predict2(gtsam::GaussianDensity* p, Matrix A0, - Matrix A1, Vector b, const gtsam::noiseModel::Diagonal* model); - gtsam::GaussianDensity* update(gtsam::GaussianDensity* p, Matrix H, - Vector z, const gtsam::noiseModel::Diagonal* model); - gtsam::GaussianDensity* updateQ(gtsam::GaussianDensity* p, Matrix H, - Vector z, Matrix Q); -}; - -//************************************************************************* -// nonlinear -//************************************************************************* - -#include - -class Symbol { - Symbol(); - Symbol(char c, uint64_t j); - Symbol(size_t key); - - size_t key() const; - void print(const string& s = "") const; - bool equals(const gtsam::Symbol& expected, double tol) const; - - char chr() const; - uint64_t index() const; - string string() const; -}; - -size_t symbol(char chr, size_t index); -char symbolChr(size_t key); -size_t symbolIndex(size_t key); - -namespace symbol_shorthand { - size_t A(size_t j); - size_t B(size_t j); - size_t C(size_t j); - size_t D(size_t j); - size_t E(size_t j); - size_t F(size_t j); - size_t G(size_t j); - size_t H(size_t j); - size_t I(size_t j); - size_t J(size_t j); - size_t K(size_t j); - size_t L(size_t j); - size_t M(size_t j); - size_t N(size_t j); - size_t O(size_t j); - size_t P(size_t j); - size_t Q(size_t j); - size_t R(size_t j); - size_t S(size_t j); - size_t T(size_t j); - size_t U(size_t j); - size_t V(size_t j); - size_t W(size_t j); - size_t X(size_t j); - size_t Y(size_t j); - size_t Z(size_t j); -}///\namespace symbol - -// Default keyformatter -void PrintKeyList (const gtsam::KeyList& keys); -void PrintKeyList (const gtsam::KeyList& keys, string s); -void PrintKeyVector(const gtsam::KeyVector& keys); -void PrintKeyVector(const gtsam::KeyVector& keys, string s); -void PrintKeySet (const gtsam::KeySet& keys); -void PrintKeySet (const gtsam::KeySet& keys, string s); - -#include -class LabeledSymbol { - LabeledSymbol(size_t full_key); - LabeledSymbol(const gtsam::LabeledSymbol& key); - LabeledSymbol(unsigned char valType, unsigned char label, size_t j); - - size_t key() const; - unsigned char label() const; - unsigned char chr() const; - size_t index() const; - - gtsam::LabeledSymbol upper() const; - gtsam::LabeledSymbol lower() const; - gtsam::LabeledSymbol newChr(unsigned char c) const; - gtsam::LabeledSymbol newLabel(unsigned char label) const; - - void print(string s = "") const; -}; - -size_t mrsymbol(unsigned char c, unsigned char label, size_t j); -unsigned char mrsymbolChr(size_t key); -unsigned char mrsymbolLabel(size_t key); -size_t mrsymbolIndex(size_t key); - -#include -class Ordering { - // Standard Constructors and Named Constructors - Ordering(); - Ordering(const gtsam::Ordering& other); - - // Testable - void print(string s = "", const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; - bool equals(const gtsam::Ordering& ord, double tol) const; - - // Standard interface - size_t size() const; - size_t at(size_t key) const; - void push_back(size_t key); - - // enabling serialization functionality - void serialize() const; - - // enable pickling in python - void pickle() const; -}; - -#include -class NonlinearFactorGraph { - NonlinearFactorGraph(); - NonlinearFactorGraph(const gtsam::NonlinearFactorGraph& graph); - - // FactorGraph - void print(string s = "NonlinearFactorGraph: ", - const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; - bool equals(const gtsam::NonlinearFactorGraph& fg, double tol) const; - size_t size() const; - bool empty() const; - void remove(size_t i); - void replace(size_t i, gtsam::NonlinearFactor* factors); - void resize(size_t size); - size_t nrFactors() const; - gtsam::NonlinearFactor* at(size_t idx) const; - void push_back(const gtsam::NonlinearFactorGraph& factors); - void push_back(gtsam::NonlinearFactor* factor); - void add(gtsam::NonlinearFactor* factor); - bool exists(size_t idx) const; - gtsam::KeySet keys() const; - gtsam::KeyVector keyVector() const; - - template , - gtsam::imuBias::ConstantBias}> - void addPrior(size_t key, const T& prior, - const gtsam::noiseModel::Base* noiseModel); - - // NonlinearFactorGraph - void printErrors(const gtsam::Values& values) const; - double error(const gtsam::Values& values) const; - double probPrime(const gtsam::Values& values) const; - gtsam::Ordering orderingCOLAMD() const; - // Ordering* orderingCOLAMDConstrained(const gtsam::Values& c, const std::map& constraints) const; - gtsam::GaussianFactorGraph* linearize(const gtsam::Values& values) const; - gtsam::NonlinearFactorGraph clone() const; - - // enabling serialization functionality - void serialize() const; - - // enable pickling in python - void pickle() const; - - void saveGraph(const string& s) const; -}; - -#include -virtual class NonlinearFactor { - // Factor base class - size_t size() const; - gtsam::KeyVector keys() const; - void print(string s = "", const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; - void printKeys(string s) const; - // NonlinearFactor - bool equals(const gtsam::NonlinearFactor& other, double tol) const; - double error(const gtsam::Values& c) const; - size_t dim() const; - bool active(const gtsam::Values& c) const; - gtsam::GaussianFactor* linearize(const gtsam::Values& c) const; - gtsam::NonlinearFactor* clone() const; - gtsam::NonlinearFactor* rekey(const gtsam::KeyVector& newKeys) const; -}; - -#include -virtual class NoiseModelFactor: gtsam::NonlinearFactor { - bool equals(const gtsam::NoiseModelFactor& other, double tol) const; - gtsam::noiseModel::Base* noiseModel() const; - Vector unwhitenedError(const gtsam::Values& x) const; - Vector whitenedError(const gtsam::Values& x) const; -}; - -#include -virtual class CustomFactor: gtsam::NoiseModelFactor { - /* - * Note CustomFactor will not be wrapped for MATLAB, as there is no supporting machinery there. - * This is achieved by adding `gtsam::CustomFactor` to the ignore list in `matlab/CMakeLists.txt`. - */ - CustomFactor(); - /* - * Example: - * ``` - * def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]): - * - * if not H is None: - * - * H[0] = J1 # 2-d numpy array for a Jacobian block - * H[1] = J2 - * ... - * return error # 1-d numpy array - * - * cf = CustomFactor(noise_model, keys, error_func) - * ``` - */ - CustomFactor(const gtsam::SharedNoiseModel& noiseModel, const gtsam::KeyVector& keys, - const gtsam::CustomErrorFunction& errorFunction); - - void print(string s = "", gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter); -}; - -#include -class Values { - Values(); - Values(const gtsam::Values& other); - - size_t size() const; - bool empty() const; - void clear(); - size_t dim() const; - - void print(string s = "", const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; - bool equals(const gtsam::Values& other, double tol) const; - - void insert(const gtsam::Values& values); - void update(const gtsam::Values& values); - void erase(size_t j); - void swap(gtsam::Values& values); - - bool exists(size_t j) const; - gtsam::KeyVector keys() const; - - gtsam::VectorValues zeroVectors() const; - - gtsam::Values retract(const gtsam::VectorValues& delta) const; - gtsam::VectorValues localCoordinates(const gtsam::Values& cp) const; - - // enabling serialization functionality - void serialize() const; - - // enable pickling in python - void pickle() 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; - - // The order is important: Vector has to precede Point2/Point3 so `atVector` - // can work for those fixed-size vectors. - void insert(size_t j, Vector vector); - void insert(size_t j, Matrix matrix); - void insert(size_t j, const gtsam::Point2& point2); - void insert(size_t j, const gtsam::Point3& point3); - void insert(size_t j, const gtsam::Rot2& rot2); - void insert(size_t j, const gtsam::Pose2& pose2); - void insert(size_t j, const gtsam::SO3& R); - void insert(size_t j, const gtsam::SO4& Q); - void insert(size_t j, const gtsam::SOn& P); - void insert(size_t j, const gtsam::Rot3& rot3); - void insert(size_t j, const gtsam::Pose3& pose3); - void insert(size_t j, const gtsam::Unit3& unit3); - void insert(size_t j, const gtsam::Cal3_S2& cal3_s2); - void insert(size_t j, const gtsam::Cal3DS2& cal3ds2); - void insert(size_t j, const gtsam::Cal3Bundler& cal3bundler); - void insert(size_t j, const gtsam::EssentialMatrix& essential_matrix); - void insert(size_t j, const gtsam::PinholeCameraCal3_S2& simple_camera); - void insert(size_t j, const gtsam::PinholeCamera& camera); - void insert(size_t j, const gtsam::imuBias::ConstantBias& constant_bias); - void insert(size_t j, const gtsam::NavState& nav_state); - void insert(size_t j, double c); - - void update(size_t j, const gtsam::Point2& point2); - void update(size_t j, const gtsam::Point3& point3); - void update(size_t j, const gtsam::Rot2& rot2); - void update(size_t j, const gtsam::Pose2& pose2); - void update(size_t j, const gtsam::SO3& R); - void update(size_t j, const gtsam::SO4& Q); - void update(size_t j, const gtsam::SOn& P); - void update(size_t j, const gtsam::Rot3& rot3); - void update(size_t j, const gtsam::Pose3& pose3); - void update(size_t j, const gtsam::Unit3& unit3); - void update(size_t j, const gtsam::Cal3_S2& cal3_s2); - void update(size_t j, const gtsam::Cal3DS2& cal3ds2); - void update(size_t j, const gtsam::Cal3Bundler& cal3bundler); - void update(size_t j, const gtsam::EssentialMatrix& essential_matrix); - void update(size_t j, const gtsam::PinholeCameraCal3_S2& simple_camera); - void update(size_t j, const gtsam::PinholeCamera& camera); - void update(size_t j, const gtsam::imuBias::ConstantBias& constant_bias); - void update(size_t j, const gtsam::NavState& nav_state); - void update(size_t j, Vector vector); - void update(size_t j, Matrix matrix); - void update(size_t j, double c); - - template , - gtsam::imuBias::ConstantBias, - gtsam::NavState, - Vector, - Matrix, - double}> - T at(size_t j); - -}; - -#include -class Marginals { - Marginals(const gtsam::NonlinearFactorGraph& graph, - const gtsam::Values& solution); - Marginals(const gtsam::GaussianFactorGraph& gfgraph, - const gtsam::Values& solution); - Marginals(const gtsam::GaussianFactorGraph& gfgraph, - const gtsam::VectorValues& solutionvec); - - void print(string s = "Marginals: ", const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; - Matrix marginalCovariance(size_t variable) const; - Matrix marginalInformation(size_t variable) const; - gtsam::JointMarginal jointMarginalCovariance(const gtsam::KeyVector& variables) const; - gtsam::JointMarginal jointMarginalInformation(const gtsam::KeyVector& variables) const; -}; - -class JointMarginal { - Matrix at(size_t iVariable, size_t jVariable) const; - Matrix fullMatrix() const; - void print(string s = "", gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter) const; -}; - -#include -virtual class LinearContainerFactor : gtsam::NonlinearFactor { - - LinearContainerFactor(gtsam::GaussianFactor* factor, const gtsam::Values& linearizationPoint); - LinearContainerFactor(gtsam::GaussianFactor* factor); - - gtsam::GaussianFactor* factor() const; -// const boost::optional& linearizationPoint() const; - - bool isJacobian() const; - gtsam::JacobianFactor* toJacobian() const; - gtsam::HessianFactor* toHessian() const; - - static gtsam::NonlinearFactorGraph ConvertLinearGraph(const gtsam::GaussianFactorGraph& linear_graph, - const gtsam::Values& linearizationPoint); - - static gtsam::NonlinearFactorGraph ConvertLinearGraph(const gtsam::GaussianFactorGraph& linear_graph); - - // enabling serialization functionality - void serializable() const; -}; // \class LinearContainerFactor - -// Summarization functionality -//#include -// -//// Uses partial QR approach by default -//gtsam::GaussianFactorGraph summarize( -// const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values, -// const gtsam::KeySet& saved_keys); -// -//gtsam::NonlinearFactorGraph summarizeAsNonlinearContainer( -// const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values, -// const gtsam::KeySet& saved_keys); - -//************************************************************************* -// Nonlinear optimizers -//************************************************************************* -#include -virtual class NonlinearOptimizerParams { - NonlinearOptimizerParams(); - void print(string s = "") const; - - int getMaxIterations() const; - double getRelativeErrorTol() const; - double getAbsoluteErrorTol() const; - double getErrorTol() const; - string getVerbosity() const; - - void setMaxIterations(int value); - void setRelativeErrorTol(double value); - void setAbsoluteErrorTol(double value); - void setErrorTol(double value); - void setVerbosity(string s); - - string getLinearSolverType() const; - void setLinearSolverType(string solver); - - void setIterativeParams(gtsam::IterativeOptimizationParameters* params); - void setOrdering(const gtsam::Ordering& ordering); - string getOrderingType() const; - void setOrderingType(string ordering); - - bool isMultifrontal() const; - bool isSequential() const; - bool isCholmod() const; - bool isIterative() const; -}; - -bool checkConvergence(double relativeErrorTreshold, - double absoluteErrorTreshold, double errorThreshold, - double currentError, double newError); -bool checkConvergence(const gtsam::NonlinearOptimizerParams& params, - double currentError, double newError); - -#include -virtual class GaussNewtonParams : gtsam::NonlinearOptimizerParams { - GaussNewtonParams(); -}; - -#include -virtual class LevenbergMarquardtParams : gtsam::NonlinearOptimizerParams { - LevenbergMarquardtParams(); - - bool getDiagonalDamping() const; - double getlambdaFactor() const; - double getlambdaInitial() const; - double getlambdaLowerBound() const; - double getlambdaUpperBound() const; - bool getUseFixedLambdaFactor(); - string getLogFile() const; - string getVerbosityLM() const; - - void setDiagonalDamping(bool flag); - void setlambdaFactor(double value); - void setlambdaInitial(double value); - void setlambdaLowerBound(double value); - void setlambdaUpperBound(double value); - void setUseFixedLambdaFactor(bool flag); - void setLogFile(string s); - void setVerbosityLM(string s); - - static gtsam::LevenbergMarquardtParams LegacyDefaults(); - static gtsam::LevenbergMarquardtParams CeresDefaults(); - - static gtsam::LevenbergMarquardtParams EnsureHasOrdering( - gtsam::LevenbergMarquardtParams params, - const gtsam::NonlinearFactorGraph& graph); - static gtsam::LevenbergMarquardtParams ReplaceOrdering( - gtsam::LevenbergMarquardtParams params, const gtsam::Ordering& ordering); -}; - -#include -virtual class DoglegParams : gtsam::NonlinearOptimizerParams { - DoglegParams(); - - double getDeltaInitial() const; - string getVerbosityDL() const; - - void setDeltaInitial(double deltaInitial) const; - void setVerbosityDL(string verbosityDL) const; -}; - -#include -virtual class NonlinearOptimizer { - gtsam::Values optimize(); - gtsam::Values optimizeSafely(); - double error() const; - int iterations() const; - gtsam::Values values() const; - gtsam::NonlinearFactorGraph graph() const; - gtsam::GaussianFactorGraph* iterate() const; -}; - -#include -virtual class GaussNewtonOptimizer : gtsam::NonlinearOptimizer { - GaussNewtonOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues); - GaussNewtonOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues, const gtsam::GaussNewtonParams& params); -}; - -#include -virtual class DoglegOptimizer : gtsam::NonlinearOptimizer { - DoglegOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues); - DoglegOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues, const gtsam::DoglegParams& params); - double getDelta() const; -}; - -#include -virtual class LevenbergMarquardtOptimizer : gtsam::NonlinearOptimizer { - LevenbergMarquardtOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues); - LevenbergMarquardtOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues, const gtsam::LevenbergMarquardtParams& params); - double lambda() const; - void print(string s = "") const; -}; - -#include -class ISAM2GaussNewtonParams { - ISAM2GaussNewtonParams(); - - void print(string s = "") const; - - /** Getters and Setters for all properties */ - double getWildfireThreshold() const; - void setWildfireThreshold(double wildfireThreshold); -}; - -class ISAM2DoglegParams { - ISAM2DoglegParams(); - - void print(string s = "") const; - - /** Getters and Setters for all properties */ - double getWildfireThreshold() const; - void setWildfireThreshold(double wildfireThreshold); - double getInitialDelta() const; - void setInitialDelta(double initialDelta); - string getAdaptationMode() const; - void setAdaptationMode(string adaptationMode); - bool isVerbose() const; - void setVerbose(bool verbose); -}; - -class ISAM2ThresholdMapValue { - ISAM2ThresholdMapValue(char c, Vector thresholds); - ISAM2ThresholdMapValue(const gtsam::ISAM2ThresholdMapValue& other); -}; - -class ISAM2ThresholdMap { - ISAM2ThresholdMap(); - ISAM2ThresholdMap(const gtsam::ISAM2ThresholdMap& other); - - // Note: no print function - - // common STL methods - size_t size() const; - bool empty() const; - void clear(); - - // structure specific methods - void insert(const gtsam::ISAM2ThresholdMapValue& value) const; -}; - -class ISAM2Params { - ISAM2Params(); - - void print(string s = "") const; - - /** Getters and Setters for all properties */ - void setOptimizationParams(const gtsam::ISAM2GaussNewtonParams& gauss_newton__params); - void setOptimizationParams(const gtsam::ISAM2DoglegParams& dogleg_params); - void setRelinearizeThreshold(double threshold); - void setRelinearizeThreshold(const gtsam::ISAM2ThresholdMap& threshold_map); - int getRelinearizeSkip() const; - void setRelinearizeSkip(int relinearizeSkip); - bool isEnableRelinearization() const; - void setEnableRelinearization(bool enableRelinearization); - bool isEvaluateNonlinearError() const; - void setEvaluateNonlinearError(bool evaluateNonlinearError); - string getFactorization() const; - void setFactorization(string factorization); - bool isCacheLinearizedFactors() const; - void setCacheLinearizedFactors(bool cacheLinearizedFactors); - bool isEnableDetailedResults() const; - void setEnableDetailedResults(bool enableDetailedResults); - bool isEnablePartialRelinearizationCheck() const; - void setEnablePartialRelinearizationCheck(bool enablePartialRelinearizationCheck); -}; - -class ISAM2Clique { - - //Constructors - ISAM2Clique(); - - //Standard Interface - Vector gradientContribution() const; - void print(string s = "", gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter); -}; - -class ISAM2Result { - ISAM2Result(); - - void print(string s = "") const; - - /** Getters and Setters for all properties */ - size_t getVariablesRelinearized() const; - size_t getVariablesReeliminated() const; - size_t getCliques() const; - double getErrorBefore() const; - double getErrorAfter() const; -}; - -class ISAM2 { - ISAM2(); - ISAM2(const gtsam::ISAM2Params& params); - ISAM2(const gtsam::ISAM2& other); - - bool equals(const gtsam::ISAM2& other, double tol) const; - void print(string s = "", const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; - void printStats() const; - void saveGraph(string s) const; - - gtsam::ISAM2Result update(); - gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta); - gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices); - gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices, const gtsam::KeyGroupMap& constrainedKeys); - gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices, gtsam::KeyGroupMap& constrainedKeys, const gtsam::KeyList& noRelinKeys); - gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices, gtsam::KeyGroupMap& constrainedKeys, const gtsam::KeyList& noRelinKeys, const gtsam::KeyList& extraReelimKeys); - gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices, gtsam::KeyGroupMap& constrainedKeys, const gtsam::KeyList& noRelinKeys, const gtsam::KeyList& extraReelimKeys, bool force_relinearize); - - gtsam::Values getLinearizationPoint() const; - gtsam::Values calculateEstimate() const; - template , - Vector, Matrix}> - VALUE calculateEstimate(size_t key) const; - gtsam::Values calculateBestEstimate() const; - Matrix marginalCovariance(size_t key) const; - gtsam::VectorValues getDelta() const; - gtsam::NonlinearFactorGraph getFactorsUnsafe() const; - gtsam::VariableIndex getVariableIndex() const; - gtsam::ISAM2Params params() const; -}; - -#include -class NonlinearISAM { - NonlinearISAM(); - NonlinearISAM(int reorderInterval); - void print(string s = "", const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; - void printStats() const; - void saveGraph(string s) const; - gtsam::Values estimate() const; - Matrix marginalCovariance(size_t key) const; - int reorderInterval() const; - int reorderCounter() const; - void update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& initialValues); - void reorder_relinearize(); - - // These might be expensive as instead of a reference the wrapper will make a copy - gtsam::GaussianISAM bayesTree() const; - gtsam::Values getLinearizationPoint() const; - gtsam::NonlinearFactorGraph getFactorsUnsafe() const; -}; - -//************************************************************************* -// Nonlinear factor types -//************************************************************************* -#include -template }> -virtual class PriorFactor : gtsam::NoiseModelFactor { - PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); - T prior() const; - - // enabling serialization functionality - void serialize() const; - - // enable pickling in python - void pickle() const; -}; - -#include -template -virtual class BetweenFactor : gtsam::NoiseModelFactor { - BetweenFactor(size_t key1, size_t key2, const T& relativePose, const gtsam::noiseModel::Base* noiseModel); - T measured() const; - - // enabling serialization functionality - void serialize() const; - - // enable pickling in python - void pickle() const; -}; - -#include -template -virtual class NonlinearEquality : gtsam::NoiseModelFactor { - // Constructor - forces exact evaluation - NonlinearEquality(size_t j, const T& feasible); - // Constructor - allows inexact evaluation - NonlinearEquality(size_t j, const T& feasible, double error_gain); - - // enabling serialization functionality - void serialize() const; -}; - -#include -template -virtual class RangeFactor : gtsam::NoiseModelFactor { - RangeFactor(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base* noiseModel); - - // enabling serialization functionality - void serialize() const; -}; - -typedef gtsam::RangeFactor RangeFactor2D; -typedef gtsam::RangeFactor RangeFactor3D; -typedef gtsam::RangeFactor RangeFactorPose2; -typedef gtsam::RangeFactor RangeFactorPose3; -typedef gtsam::RangeFactor RangeFactorCalibratedCameraPoint; -typedef gtsam::RangeFactor RangeFactorSimpleCameraPoint; -typedef gtsam::RangeFactor RangeFactorCalibratedCamera; -typedef gtsam::RangeFactor RangeFactorSimpleCamera; - - -#include -template -virtual class RangeFactorWithTransform : gtsam::NoiseModelFactor { - RangeFactorWithTransform(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base* noiseModel, const POSE& body_T_sensor); - - // enabling serialization functionality - void serialize() const; -}; - -typedef gtsam::RangeFactorWithTransform RangeFactorWithTransform2D; -typedef gtsam::RangeFactorWithTransform RangeFactorWithTransform3D; -typedef gtsam::RangeFactorWithTransform RangeFactorWithTransformPose2; -typedef gtsam::RangeFactorWithTransform RangeFactorWithTransformPose3; - -#include -template -virtual class BearingFactor : gtsam::NoiseModelFactor { - BearingFactor(size_t key1, size_t key2, const BEARING& measured, const gtsam::noiseModel::Base* noiseModel); - - // enabling serialization functionality - void serialize() const; -}; - -typedef gtsam::BearingFactor BearingFactor2D; -typedef gtsam::BearingFactor BearingFactor3D; -typedef gtsam::BearingFactor BearingFactorPose2; - -#include -template -class BearingRange { - BearingRange(const BEARING& b, const RANGE& r); - BEARING bearing() const; - RANGE range() const; - static This Measure(const POSE& pose, const POINT& point); - static BEARING MeasureBearing(const POSE& pose, const POINT& point); - static RANGE MeasureRange(const POSE& pose, const POINT& point); - void print(string s = "") const; -}; - -typedef gtsam::BearingRange BearingRange2D; - -#include -template -virtual class BearingRangeFactor : gtsam::NoiseModelFactor { - BearingRangeFactor(size_t poseKey, size_t pointKey, - const BEARING& measuredBearing, const RANGE& measuredRange, - const gtsam::noiseModel::Base* noiseModel); - - gtsam::BearingRange measured() const; - - // enabling serialization functionality - void serialize() const; -}; - -typedef gtsam::BearingRangeFactor BearingRangeFactor2D; -typedef gtsam::BearingRangeFactor BearingRangeFactorPose2; - - -#include -template -virtual class GenericProjectionFactor : gtsam::NoiseModelFactor { - GenericProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel, - size_t poseKey, size_t pointKey, const CALIBRATION* k); - GenericProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel, - size_t poseKey, size_t pointKey, const CALIBRATION* k, const POSE& body_P_sensor); - - GenericProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel, - size_t poseKey, size_t pointKey, const CALIBRATION* k, bool throwCheirality, bool verboseCheirality); - GenericProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel, - size_t poseKey, size_t pointKey, const CALIBRATION* k, bool throwCheirality, bool verboseCheirality, - const POSE& body_P_sensor); - - gtsam::Point2 measured() const; - CALIBRATION* calibration() const; - bool verboseCheirality() const; - bool throwCheirality() const; - - // enabling serialization functionality - void serialize() const; -}; -typedef gtsam::GenericProjectionFactor GenericProjectionFactorCal3_S2; -typedef gtsam::GenericProjectionFactor GenericProjectionFactorCal3DS2; - - -#include -template -virtual class GeneralSFMFactor : gtsam::NoiseModelFactor { - GeneralSFMFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* model, size_t cameraKey, size_t landmarkKey); - gtsam::Point2 measured() const; -}; -typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3_S2; -typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3DS2; -typedef gtsam::GeneralSFMFactor, gtsam::Point3> GeneralSFMFactorCal3Bundler; - -template -virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor { - GeneralSFMFactor2(const gtsam::Point2& measured, const gtsam::noiseModel::Base* model, size_t poseKey, size_t landmarkKey, size_t calibKey); - gtsam::Point2 measured() const; - - // enabling serialization functionality - void serialize() const; -}; - -#include - -/// Linearization mode: what factor to linearize to -enum LinearizationMode { HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD }; - -/// How to manage degeneracy -enum DegeneracyMode { IGNORE_DEGENERACY, ZERO_ON_DEGENERACY, HANDLE_INFINITY }; - -class SmartProjectionParams { - SmartProjectionParams(); - void setLinearizationMode(gtsam::LinearizationMode linMode); - void setDegeneracyMode(gtsam::DegeneracyMode degMode); - void setRankTolerance(double rankTol); - void setEnableEPI(bool enableEPI); - void setLandmarkDistanceThreshold(bool landmarkDistanceThreshold); - void setDynamicOutlierRejectionThreshold(bool dynOutRejectionThreshold); -}; - -#include -template -virtual class SmartProjectionPoseFactor: gtsam::NonlinearFactor { - - SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise, - const CALIBRATION* K); - SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise, - const CALIBRATION* K, - const gtsam::Pose3& body_P_sensor); - SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise, - const CALIBRATION* K, - const gtsam::SmartProjectionParams& params); - SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise, - const CALIBRATION* K, - const gtsam::Pose3& body_P_sensor, - const gtsam::SmartProjectionParams& params); - - void add(const gtsam::Point2& measured_i, size_t poseKey_i); - - // enabling serialization functionality - void serialize() const; -}; - -typedef gtsam::SmartProjectionPoseFactor SmartProjectionPose3Factor; - - -#include -template -virtual class GenericStereoFactor : gtsam::NoiseModelFactor { - GenericStereoFactor(const gtsam::StereoPoint2& measured, const gtsam::noiseModel::Base* noiseModel, - size_t poseKey, size_t landmarkKey, const gtsam::Cal3_S2Stereo* K); - gtsam::StereoPoint2 measured() const; - gtsam::Cal3_S2Stereo* calibration() const; - - // enabling serialization functionality - void serialize() const; -}; -typedef gtsam::GenericStereoFactor GenericStereoFactor3D; - -#include -template -virtual class PoseTranslationPrior : gtsam::NoiseModelFactor { - PoseTranslationPrior(size_t key, const POSE& pose_z, const gtsam::noiseModel::Base* noiseModel); -}; - -typedef gtsam::PoseTranslationPrior PoseTranslationPrior2D; -typedef gtsam::PoseTranslationPrior PoseTranslationPrior3D; - -#include -template -virtual class PoseRotationPrior : gtsam::NoiseModelFactor { - PoseRotationPrior(size_t key, const POSE& pose_z, const gtsam::noiseModel::Base* noiseModel); -}; - -typedef gtsam::PoseRotationPrior PoseRotationPrior2D; -typedef gtsam::PoseRotationPrior PoseRotationPrior3D; - -#include -virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor { - EssentialMatrixFactor(size_t key, const gtsam::Point2& pA, const gtsam::Point2& pB, - const gtsam::noiseModel::Base* noiseModel); -}; - -#include - -class SfmTrack { - SfmTrack(); - SfmTrack(const gtsam::Point3& pt); - const Point3& point3() const; - - double r; - double g; - double b; - - std::vector> measurements; - - size_t number_measurements() const; - pair measurement(size_t idx) const; - pair siftIndex(size_t idx) const; - void add_measurement(size_t idx, const gtsam::Point2& m); - - // enabling serialization functionality - void serialize() const; - - // enable pickling in python - void pickle() const; - - // enabling function to compare objects - bool equals(const gtsam::SfmTrack& expected, double tol) const; -}; - -class SfmData { - SfmData(); - size_t number_cameras() const; - size_t number_tracks() const; - gtsam::PinholeCamera camera(size_t idx) const; - gtsam::SfmTrack track(size_t idx) const; - void add_track(const gtsam::SfmTrack& t) ; - void add_camera(const gtsam::SfmCamera& cam); - - // enabling serialization functionality - void serialize() const; - - // enable pickling in python - void pickle() const; - - // enabling function to compare objects - bool equals(const gtsam::SfmData& expected, double tol) const; -}; - -gtsam::SfmData readBal(string filename); -bool writeBAL(string filename, gtsam::SfmData& data); -gtsam::Values initialCamerasEstimate(const gtsam::SfmData& db); -gtsam::Values initialCamerasAndPointsEstimate(const gtsam::SfmData& db); - -pair load2D(string filename, - gtsam::noiseModel::Diagonal* model, int maxIndex, bool addNoise, bool smart); -pair load2D(string filename, - gtsam::noiseModel::Diagonal* model, int maxIndex, bool addNoise); -pair load2D(string filename, - gtsam::noiseModel::Diagonal* model, int maxIndex); -pair load2D(string filename, - gtsam::noiseModel::Diagonal* model); -pair load2D(string filename); -pair load2D_robust(string filename, - gtsam::noiseModel::Base* model, int maxIndex); -void save2D(const gtsam::NonlinearFactorGraph& graph, - const gtsam::Values& config, gtsam::noiseModel::Diagonal* model, - string filename); - -// std::vector::shared_ptr> -// Ignored by pybind -> will be List[BetweenFactorPose2] -class BetweenFactorPose2s -{ - BetweenFactorPose2s(); - size_t size() const; - gtsam::BetweenFactor* at(size_t i) const; - void push_back(const gtsam::BetweenFactor* factor); -}; -gtsam::BetweenFactorPose2s parse2DFactors(string filename); - -// std::vector::shared_ptr> -// Ignored by pybind -> will be List[BetweenFactorPose3] -class BetweenFactorPose3s -{ - BetweenFactorPose3s(); - size_t size() const; - gtsam::BetweenFactor* at(size_t i) const; - void push_back(const gtsam::BetweenFactor* factor); -}; -gtsam::BetweenFactorPose3s parse3DFactors(string filename); - -pair load3D(string filename); - -pair readG2o(string filename); -pair readG2o(string filename, bool is3D); -void writeG2o(const gtsam::NonlinearFactorGraph& graph, - const gtsam::Values& estimate, string filename); - -#include -class InitializePose3 { - static gtsam::Values computeOrientationsChordal( - const gtsam::NonlinearFactorGraph& pose3Graph); - static gtsam::Values computeOrientationsGradient( - const gtsam::NonlinearFactorGraph& pose3Graph, - const gtsam::Values& givenGuess, size_t maxIter, const bool setRefFrame); - static gtsam::Values computeOrientationsGradient( - const gtsam::NonlinearFactorGraph& pose3Graph, - const gtsam::Values& givenGuess); - static gtsam::NonlinearFactorGraph buildPose3graph( - const gtsam::NonlinearFactorGraph& graph); - static gtsam::Values initializeOrientations( - const gtsam::NonlinearFactorGraph& graph); - static gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph, - const gtsam::Values& givenGuess, - bool useGradient); - static gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph); -}; - -#include -template -virtual class KarcherMeanFactor : gtsam::NonlinearFactor { - KarcherMeanFactor(const gtsam::KeyVector& keys); -}; - -#include -gtsam::noiseModel::Isotropic* ConvertNoiseModel( - gtsam::noiseModel::Base* model, size_t d); - -template -virtual class FrobeniusFactor : gtsam::NoiseModelFactor { - FrobeniusFactor(size_t key1, size_t key2); - FrobeniusFactor(size_t key1, size_t key2, gtsam::noiseModel::Base* model); - - Vector evaluateError(const T& R1, const T& R2); -}; - -template -virtual class FrobeniusBetweenFactor : gtsam::NoiseModelFactor { - FrobeniusBetweenFactor(size_t key1, size_t key2, const T& R12); - FrobeniusBetweenFactor(size_t key1, size_t key2, const T& R12, gtsam::noiseModel::Base* model); - - Vector evaluateError(const T& R1, const T& R2); -}; - -#include - -virtual class ShonanFactor3 : gtsam::NoiseModelFactor { - ShonanFactor3(size_t key1, size_t key2, const gtsam::Rot3 &R12, - size_t p); - ShonanFactor3(size_t key1, size_t key2, const gtsam::Rot3 &R12, - size_t p, gtsam::noiseModel::Base *model); - Vector evaluateError(const gtsam::SOn &Q1, const gtsam::SOn &Q2); -}; - -#include -template -class BinaryMeasurement { - BinaryMeasurement(size_t key1, size_t key2, const T& measured, - const gtsam::noiseModel::Base* model); - size_t key1() const; - size_t key2() const; - T measured() const; - gtsam::noiseModel::Base* noiseModel() const; -}; - -typedef gtsam::BinaryMeasurement BinaryMeasurementUnit3; -typedef gtsam::BinaryMeasurement BinaryMeasurementRot3; - -class BinaryMeasurementsUnit3 { - BinaryMeasurementsUnit3(); - size_t size() const; - gtsam::BinaryMeasurement at(size_t idx) const; - void push_back(const gtsam::BinaryMeasurement& measurement); -}; - -#include - -// TODO(frank): copy/pasta below until we have integer template paremeters in wrap! - -class ShonanAveragingParameters2 { - ShonanAveragingParameters2(const gtsam::LevenbergMarquardtParams& lm); - ShonanAveragingParameters2(const gtsam::LevenbergMarquardtParams& lm, string method); - gtsam::LevenbergMarquardtParams getLMParams() const; - void setOptimalityThreshold(double value); - double getOptimalityThreshold() const; - void setAnchor(size_t index, const gtsam::Rot2& value); - pair getAnchor(); - void setAnchorWeight(double value); - double getAnchorWeight() const; - void setKarcherWeight(double value); - double getKarcherWeight() const; - void setGaugesWeight(double value); - double getGaugesWeight() const; - void setUseHuber(bool value); - bool getUseHuber() const; - void setCertifyOptimality(bool value); - bool getCertifyOptimality() const; -}; - -class ShonanAveragingParameters3 { - ShonanAveragingParameters3(const gtsam::LevenbergMarquardtParams& lm); - ShonanAveragingParameters3(const gtsam::LevenbergMarquardtParams& lm, string method); - gtsam::LevenbergMarquardtParams getLMParams() const; - void setOptimalityThreshold(double value); - double getOptimalityThreshold() const; - void setAnchor(size_t index, const gtsam::Rot3& value); - pair getAnchor(); - void setAnchorWeight(double value); - double getAnchorWeight() const; - void setKarcherWeight(double value); - double getKarcherWeight() const; - void setGaugesWeight(double value); - double getGaugesWeight() const; - void setUseHuber(bool value); - bool getUseHuber() const; - void setCertifyOptimality(bool value); - bool getCertifyOptimality() const; -}; - -class ShonanAveraging2 { - ShonanAveraging2(string g2oFile); - ShonanAveraging2(string g2oFile, - const gtsam::ShonanAveragingParameters2 ¶meters); - - // Query properties - size_t nrUnknowns() const; - size_t nrMeasurements() const; - gtsam::Rot2 measured(size_t i); - gtsam::KeyVector keys(size_t i); - - // Matrix API (advanced use, debugging) - Matrix denseD() const; - Matrix denseQ() const; - Matrix denseL() const; - // Matrix computeLambda_(Matrix S) const; - Matrix computeLambda_(const gtsam::Values& values) const; - Matrix computeA_(const gtsam::Values& values) const; - double computeMinEigenValue(const gtsam::Values& values) const; - gtsam::Values initializeWithDescent(size_t p, const gtsam::Values& values, - const Vector& minEigenVector, double minEigenValue) const; - - // Advanced API - gtsam::NonlinearFactorGraph buildGraphAt(size_t p) const; - gtsam::Values initializeRandomlyAt(size_t p) const; - double costAt(size_t p, const gtsam::Values& values) const; - pair computeMinEigenVector(const gtsam::Values& values) const; - bool checkOptimality(const gtsam::Values& values) const; - gtsam::LevenbergMarquardtOptimizer* createOptimizerAt(size_t p, const gtsam::Values& initial); - // gtsam::Values tryOptimizingAt(size_t p) const; - gtsam::Values tryOptimizingAt(size_t p, const gtsam::Values& initial) const; - gtsam::Values projectFrom(size_t p, const gtsam::Values& values) const; - gtsam::Values roundSolution(const gtsam::Values& values) const; - - // Basic API - double cost(const gtsam::Values& values) const; - gtsam::Values initializeRandomly() const; - pair run(const gtsam::Values& initial, size_t min_p, size_t max_p) const; -}; - -class ShonanAveraging3 { - ShonanAveraging3(string g2oFile); - ShonanAveraging3(string g2oFile, - const gtsam::ShonanAveragingParameters3 ¶meters); - - // TODO(frank): deprecate once we land pybind wrapper - ShonanAveraging3(const gtsam::BetweenFactorPose3s &factors); - ShonanAveraging3(const gtsam::BetweenFactorPose3s &factors, - const gtsam::ShonanAveragingParameters3 ¶meters); - - // Query properties - size_t nrUnknowns() const; - size_t nrMeasurements() const; - gtsam::Rot3 measured(size_t i); - gtsam::KeyVector keys(size_t i); - - // Matrix API (advanced use, debugging) - Matrix denseD() const; - Matrix denseQ() const; - Matrix denseL() const; - // Matrix computeLambda_(Matrix S) const; - Matrix computeLambda_(const gtsam::Values& values) const; - Matrix computeA_(const gtsam::Values& values) const; - double computeMinEigenValue(const gtsam::Values& values) const; - gtsam::Values initializeWithDescent(size_t p, const gtsam::Values& values, - const Vector& minEigenVector, double minEigenValue) const; - - // Advanced API - gtsam::NonlinearFactorGraph buildGraphAt(size_t p) const; - gtsam::Values initializeRandomlyAt(size_t p) const; - double costAt(size_t p, const gtsam::Values& values) const; - pair computeMinEigenVector(const gtsam::Values& values) const; - bool checkOptimality(const gtsam::Values& values) const; - gtsam::LevenbergMarquardtOptimizer* createOptimizerAt(size_t p, const gtsam::Values& initial); - // gtsam::Values tryOptimizingAt(size_t p) const; - gtsam::Values tryOptimizingAt(size_t p, const gtsam::Values& initial) const; - gtsam::Values projectFrom(size_t p, const gtsam::Values& values) const; - gtsam::Values roundSolution(const gtsam::Values& values) const; - - // Basic API - double cost(const gtsam::Values& values) const; - gtsam::Values initializeRandomly() const; - pair run(const gtsam::Values& initial, size_t min_p, size_t max_p) const; -}; - -#include - -class KeyPairDoubleMap { - KeyPairDoubleMap(); - KeyPairDoubleMap(const gtsam::KeyPairDoubleMap& other); - - size_t size() const; - bool empty() const; - void clear(); - size_t at(const pair& keypair) const; -}; - -class MFAS { - MFAS(const gtsam::BinaryMeasurementsUnit3& relativeTranslations, - const gtsam::Unit3& projectionDirection); - - gtsam::KeyPairDoubleMap computeOutlierWeights() const; - gtsam::KeyVector computeOrdering() const; -}; - -#include -class TranslationRecovery { - TranslationRecovery(const gtsam::BinaryMeasurementsUnit3 &relativeTranslations, - const gtsam::LevenbergMarquardtParams &lmParams); - TranslationRecovery( - const gtsam::BinaryMeasurementsUnit3 & relativeTranslations); // default LevenbergMarquardtParams - gtsam::Values run(const double scale) const; - gtsam::Values run() const; // default scale = 1.0 -}; - -//************************************************************************* -// Navigation -//************************************************************************* -namespace imuBias { -#include - -class ConstantBias { - // Constructors - ConstantBias(); - ConstantBias(Vector biasAcc, Vector biasGyro); - - // Testable - void print(string s = "") const; - bool equals(const gtsam::imuBias::ConstantBias& expected, double tol) const; - - // Group - static gtsam::imuBias::ConstantBias identity(); - gtsam::imuBias::ConstantBias inverse() const; - gtsam::imuBias::ConstantBias compose(const gtsam::imuBias::ConstantBias& b) const; - gtsam::imuBias::ConstantBias between(const gtsam::imuBias::ConstantBias& b) const; - - // Operator Overloads - gtsam::imuBias::ConstantBias operator-() const; - gtsam::imuBias::ConstantBias operator+(const gtsam::imuBias::ConstantBias& b) const; - gtsam::imuBias::ConstantBias operator-(const gtsam::imuBias::ConstantBias& b) const; - - // Manifold - gtsam::imuBias::ConstantBias retract(Vector v) const; - Vector localCoordinates(const gtsam::imuBias::ConstantBias& b) const; - - // Lie Group - static gtsam::imuBias::ConstantBias Expmap(Vector v); - static Vector Logmap(const gtsam::imuBias::ConstantBias& b); - - // Standard Interface - Vector vector() const; - Vector accelerometer() const; - Vector gyroscope() const; - Vector correctAccelerometer(Vector measurement) const; - Vector correctGyroscope(Vector measurement) const; -}; - -}///\namespace imuBias - -#include -class NavState { - // Constructors - NavState(); - NavState(const gtsam::Rot3& R, const gtsam::Point3& t, Vector v); - NavState(const gtsam::Pose3& pose, Vector v); - - // Testable - void print(string s = "") const; - bool equals(const gtsam::NavState& expected, double tol) const; - - // Access - gtsam::Rot3 attitude() const; - gtsam::Point3 position() const; - Vector velocity() const; - gtsam::Pose3 pose() const; - - gtsam::NavState retract(const Vector& x) const; - Vector localCoordinates(const gtsam::NavState& g) const; -}; - -#include -virtual class PreintegratedRotationParams { - PreintegratedRotationParams(); - - // Testable - void print(string s = "") const; - bool equals(const gtsam::PreintegratedRotationParams& expected, double tol); - - void setGyroscopeCovariance(Matrix cov); - void setOmegaCoriolis(Vector omega); - void setBodyPSensor(const gtsam::Pose3& pose); - - Matrix getGyroscopeCovariance() const; - - boost::optional getOmegaCoriolis() const; - boost::optional getBodyPSensor() const; -}; - -#include -virtual class PreintegrationParams : gtsam::PreintegratedRotationParams { - PreintegrationParams(Vector n_gravity); - - static gtsam::PreintegrationParams* MakeSharedD(double g); - static gtsam::PreintegrationParams* MakeSharedU(double g); - static gtsam::PreintegrationParams* MakeSharedD(); // default g = 9.81 - static gtsam::PreintegrationParams* MakeSharedU(); // default g = 9.81 - - // Testable - void print(string s = "") const; - bool equals(const gtsam::PreintegrationParams& expected, double tol); - - void setAccelerometerCovariance(Matrix cov); - void setIntegrationCovariance(Matrix cov); - void setUse2ndOrderCoriolis(bool flag); - - Matrix getAccelerometerCovariance() const; - Matrix getIntegrationCovariance() const; - bool getUse2ndOrderCoriolis() const; -}; - -#include -class PreintegratedImuMeasurements { - // Constructors - PreintegratedImuMeasurements(const gtsam::PreintegrationParams* params); - PreintegratedImuMeasurements(const gtsam::PreintegrationParams* params, - const gtsam::imuBias::ConstantBias& bias); - - // Testable - void print(string s = "") const; - bool equals(const gtsam::PreintegratedImuMeasurements& expected, double tol); - - // Standard Interface - void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, - double deltaT); - void resetIntegration(); - void resetIntegrationAndSetBias(const gtsam::imuBias::ConstantBias& biasHat); - - Matrix preintMeasCov() const; - Vector preintegrated() const; - double deltaTij() const; - gtsam::Rot3 deltaRij() const; - Vector deltaPij() const; - Vector deltaVij() const; - gtsam::imuBias::ConstantBias biasHat() const; - Vector biasHatVector() const; - gtsam::NavState predict(const gtsam::NavState& state_i, - const gtsam::imuBias::ConstantBias& bias) const; -}; - -virtual class ImuFactor: gtsam::NonlinearFactor { - ImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, - size_t bias, - const gtsam::PreintegratedImuMeasurements& preintegratedMeasurements); - - // Standard Interface - gtsam::PreintegratedImuMeasurements preintegratedMeasurements() const; - Vector evaluateError(const gtsam::Pose3& pose_i, Vector vel_i, - const gtsam::Pose3& pose_j, Vector vel_j, - const gtsam::imuBias::ConstantBias& bias); -}; - -#include -virtual class PreintegrationCombinedParams : gtsam::PreintegrationParams { - PreintegrationCombinedParams(Vector n_gravity); - - static gtsam::PreintegrationCombinedParams* MakeSharedD(double g); - static gtsam::PreintegrationCombinedParams* MakeSharedU(double g); - static gtsam::PreintegrationCombinedParams* MakeSharedD(); // default g = 9.81 - static gtsam::PreintegrationCombinedParams* MakeSharedU(); // default g = 9.81 - - // Testable - void print(string s = "") const; - bool equals(const gtsam::PreintegrationCombinedParams& expected, double tol); - - void setBiasAccCovariance(Matrix cov); - void setBiasOmegaCovariance(Matrix cov); - void setBiasAccOmegaInt(Matrix cov); - - Matrix getBiasAccCovariance() const ; - Matrix getBiasOmegaCovariance() const ; - Matrix getBiasAccOmegaInt() const; - -}; - -class PreintegratedCombinedMeasurements { -// Constructors - PreintegratedCombinedMeasurements(const gtsam::PreintegrationCombinedParams* params); - PreintegratedCombinedMeasurements(const gtsam::PreintegrationCombinedParams* params, - const gtsam::imuBias::ConstantBias& bias); - // Testable - void print(string s = "Preintegrated Measurements:") const; - bool equals(const gtsam::PreintegratedCombinedMeasurements& expected, - double tol); - - // Standard Interface - void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, - double deltaT); - void resetIntegration(); - void resetIntegrationAndSetBias(const gtsam::imuBias::ConstantBias& biasHat); - - Matrix preintMeasCov() const; - double deltaTij() const; - gtsam::Rot3 deltaRij() const; - Vector deltaPij() const; - Vector deltaVij() const; - gtsam::imuBias::ConstantBias biasHat() const; - Vector biasHatVector() const; - gtsam::NavState predict(const gtsam::NavState& state_i, - const gtsam::imuBias::ConstantBias& bias) const; -}; - -virtual class CombinedImuFactor: gtsam::NonlinearFactor { - CombinedImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, - size_t bias_i, size_t bias_j, - const gtsam::PreintegratedCombinedMeasurements& CombinedPreintegratedMeasurements); - - // Standard Interface - gtsam::PreintegratedCombinedMeasurements preintegratedMeasurements() const; - Vector evaluateError(const gtsam::Pose3& pose_i, Vector vel_i, - const gtsam::Pose3& pose_j, Vector vel_j, - const gtsam::imuBias::ConstantBias& bias_i, - const gtsam::imuBias::ConstantBias& bias_j); -}; - -#include -class PreintegratedAhrsMeasurements { - // Standard Constructor - PreintegratedAhrsMeasurements(Vector bias, Matrix measuredOmegaCovariance); - PreintegratedAhrsMeasurements(const gtsam::PreintegratedAhrsMeasurements& rhs); - - // Testable - void print(string s = "Preintegrated Measurements: ") const; - bool equals(const gtsam::PreintegratedAhrsMeasurements& expected, double tol); - - // get Data - gtsam::Rot3 deltaRij() const; - double deltaTij() const; - Vector biasHat() const; - - // Standard Interface - void integrateMeasurement(Vector measuredOmega, double deltaT); - void resetIntegration() ; -}; - -virtual class AHRSFactor : gtsam::NonlinearFactor { - AHRSFactor(size_t rot_i, size_t rot_j,size_t bias, - const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, Vector omegaCoriolis); - AHRSFactor(size_t rot_i, size_t rot_j, size_t bias, - const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, Vector omegaCoriolis, - const gtsam::Pose3& body_P_sensor); - - // Standard Interface - gtsam::PreintegratedAhrsMeasurements preintegratedMeasurements() const; - Vector evaluateError(const gtsam::Rot3& rot_i, const gtsam::Rot3& rot_j, - Vector bias) const; - gtsam::Rot3 predict(const gtsam::Rot3& rot_i, Vector bias, - const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, - Vector omegaCoriolis) const; -}; - -#include -//virtual class AttitudeFactor : gtsam::NonlinearFactor { -// AttitudeFactor(const Unit3& nZ, const Unit3& bRef); -// AttitudeFactor(); -//}; -virtual class Rot3AttitudeFactor : gtsam::NonlinearFactor{ - Rot3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model, - const gtsam::Unit3& bRef); - Rot3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model); - Rot3AttitudeFactor(); - void print(string s = "", const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; - bool equals(const gtsam::NonlinearFactor& expected, double tol) const; - gtsam::Unit3 nZ() const; - gtsam::Unit3 bRef() const; -}; - -virtual class Pose3AttitudeFactor : gtsam::NonlinearFactor { - Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, - const gtsam::noiseModel::Diagonal* model, - const gtsam::Unit3& bRef); - Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, - const gtsam::noiseModel::Diagonal* model); - Pose3AttitudeFactor(); - void print(string s = "", const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; - bool equals(const gtsam::NonlinearFactor& expected, double tol) const; - gtsam::Unit3 nZ() const; - gtsam::Unit3 bRef() const; -}; - -#include -virtual class GPSFactor : gtsam::NonlinearFactor{ - GPSFactor(size_t key, const gtsam::Point3& gpsIn, - const gtsam::noiseModel::Base* model); - - // Testable - void print(string s = "", const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; - bool equals(const gtsam::GPSFactor& expected, double tol); - - // Standard Interface - gtsam::Point3 measurementIn() const; -}; - -virtual class GPSFactor2 : gtsam::NonlinearFactor { - GPSFactor2(size_t key, const gtsam::Point3& gpsIn, - const gtsam::noiseModel::Base* model); - - // Testable - void print(string s = "", const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; - bool equals(const gtsam::GPSFactor2& expected, double tol); - - // Standard Interface - gtsam::Point3 measurementIn() const; -}; - -#include -virtual class Scenario { - gtsam::Pose3 pose(double t) const; - Vector omega_b(double t) const; - Vector velocity_n(double t) const; - Vector acceleration_n(double t) const; - gtsam::Rot3 rotation(double t) const; - gtsam::NavState navState(double t) const; - Vector velocity_b(double t) const; - Vector acceleration_b(double t) const; -}; - -virtual class ConstantTwistScenario : gtsam::Scenario { - ConstantTwistScenario(Vector w, Vector v); - ConstantTwistScenario(Vector w, Vector v, - const gtsam::Pose3& nTb0); -}; - -virtual class AcceleratingScenario : gtsam::Scenario { - AcceleratingScenario(const gtsam::Rot3& nRb, const gtsam::Point3& p0, - Vector v0, Vector a_n, - Vector omega_b); -}; - -#include -class ScenarioRunner { - ScenarioRunner(const gtsam::Scenario& scenario, - const gtsam::PreintegrationParams* p, - double imuSampleTime, - const gtsam::imuBias::ConstantBias& bias); - Vector gravity_n() const; - Vector actualAngularVelocity(double t) const; - Vector actualSpecificForce(double t) const; - Vector measuredAngularVelocity(double t) const; - Vector measuredSpecificForce(double t) const; - double imuSampleTime() const; - gtsam::PreintegratedImuMeasurements integrate( - double T, const gtsam::imuBias::ConstantBias& estimatedBias, - bool corrupted) const; - gtsam::NavState predict( - const gtsam::PreintegratedImuMeasurements& pim, - const gtsam::imuBias::ConstantBias& estimatedBias) const; - Matrix estimateCovariance( - double T, size_t N, - const gtsam::imuBias::ConstantBias& estimatedBias) const; - Matrix estimateNoiseCovariance(size_t N) const; -}; - //************************************************************************* // Utilities //************************************************************************* namespace utilities { - #include - gtsam::KeyList createKeyList(Vector I); - gtsam::KeyList createKeyList(string s, Vector I); - gtsam::KeyVector createKeyVector(Vector I); - gtsam::KeyVector createKeyVector(string s, Vector I); - gtsam::KeySet createKeySet(Vector I); - gtsam::KeySet createKeySet(string s, Vector I); - Matrix extractPoint2(const gtsam::Values& values); - Matrix extractPoint3(const gtsam::Values& values); - gtsam::Values allPose2s(gtsam::Values& values); - Matrix extractPose2(const gtsam::Values& values); - gtsam::Values allPose3s(gtsam::Values& values); - Matrix extractPose3(const gtsam::Values& values); - void perturbPoint2(gtsam::Values& values, double sigma, int seed); - void perturbPose2 (gtsam::Values& values, double sigmaT, double sigmaR, int seed); - void perturbPoint3(gtsam::Values& values, double sigma, int seed); - void insertBackprojections(gtsam::Values& values, const gtsam::PinholeCameraCal3_S2& c, Vector J, Matrix Z, double depth); - void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K); - void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K, const gtsam::Pose3& body_P_sensor); - Matrix reprojectionErrors(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values); - gtsam::Values localToWorld(const gtsam::Values& local, const gtsam::Pose2& base); - gtsam::Values localToWorld(const gtsam::Values& local, const gtsam::Pose2& base, const gtsam::KeyVector& keys); - -} //\namespace utilities - #include +gtsam::KeyList createKeyList(Vector I); +gtsam::KeyList createKeyList(string s, Vector I); +gtsam::KeyVector createKeyVector(Vector I); +gtsam::KeyVector createKeyVector(string s, Vector I); +gtsam::KeySet createKeySet(Vector I); +gtsam::KeySet createKeySet(string s, Vector I); +Matrix extractPoint2(const gtsam::Values& values); +Matrix extractPoint3(const gtsam::Values& values); +gtsam::Values allPose2s(gtsam::Values& values); +Matrix extractPose2(const gtsam::Values& values); +gtsam::Values allPose3s(gtsam::Values& values); +Matrix extractPose3(const gtsam::Values& values); +void perturbPoint2(gtsam::Values& values, double sigma, int seed); +void perturbPose2(gtsam::Values& values, double sigmaT, double sigmaR, + int seed); +void perturbPoint3(gtsam::Values& values, double sigma, int seed); +void insertBackprojections(gtsam::Values& values, + const gtsam::PinholeCamera& c, + Vector J, Matrix Z, double depth); +void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, + Vector J, Matrix Z, + const gtsam::noiseModel::Base* model, + const gtsam::Cal3_S2* K); +void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, + Vector J, Matrix Z, + const gtsam::noiseModel::Base* model, + const gtsam::Cal3_S2* K, + const gtsam::Pose3& body_P_sensor); +Matrix reprojectionErrors(const gtsam::NonlinearFactorGraph& graph, + const gtsam::Values& values); +gtsam::Values localToWorld(const gtsam::Values& local, + const gtsam::Pose2& base); +gtsam::Values localToWorld(const gtsam::Values& local, const gtsam::Pose2& base, + const gtsam::KeyVector& keys); + +} // namespace utilities + class RedirectCout { RedirectCout(); string str(); }; -} +} // namespace gtsam diff --git a/gtsam/linear/linear.i b/gtsam/linear/linear.i new file mode 100644 index 000000000..63047bf4f --- /dev/null +++ b/gtsam/linear/linear.i @@ -0,0 +1,653 @@ +//************************************************************************* +// linear +//************************************************************************* +namespace gtsam { + +namespace noiseModel { +#include +virtual class Base { + void print(string s = "") const; + // Methods below are available for all noise models. However, can't add them + // because wrap (incorrectly) thinks robust classes derive from this Base as well. + // bool isConstrained() const; + // bool isUnit() const; + // size_t dim() const; + // Vector sigmas() const; +}; + +virtual class Gaussian : gtsam::noiseModel::Base { + static gtsam::noiseModel::Gaussian* Information(Matrix R); + static gtsam::noiseModel::Gaussian* SqrtInformation(Matrix R); + static gtsam::noiseModel::Gaussian* Covariance(Matrix R); + + bool equals(gtsam::noiseModel::Base& expected, double tol); + + // access to noise model + Matrix R() const; + Matrix information() const; + Matrix covariance() const; + + // Whitening operations + Vector whiten(Vector v) const; + Vector unwhiten(Vector v) const; + Matrix Whiten(Matrix H) const; + + // enabling serialization functionality + void serializable() const; +}; + +virtual class Diagonal : gtsam::noiseModel::Gaussian { + static gtsam::noiseModel::Diagonal* Sigmas(Vector sigmas); + static gtsam::noiseModel::Diagonal* Variances(Vector variances); + static gtsam::noiseModel::Diagonal* Precisions(Vector precisions); + Matrix R() const; + + // access to noise model + Vector sigmas() const; + Vector invsigmas() const; + Vector precisions() const; + + // enabling serialization functionality + void serializable() const; +}; + +virtual class Constrained : gtsam::noiseModel::Diagonal { + static gtsam::noiseModel::Constrained* MixedSigmas(Vector mu, Vector sigmas); + static gtsam::noiseModel::Constrained* MixedSigmas(double m, Vector sigmas); + static gtsam::noiseModel::Constrained* MixedVariances(Vector mu, Vector variances); + static gtsam::noiseModel::Constrained* MixedVariances(Vector variances); + static gtsam::noiseModel::Constrained* MixedPrecisions(Vector mu, Vector precisions); + static gtsam::noiseModel::Constrained* MixedPrecisions(Vector precisions); + + static gtsam::noiseModel::Constrained* All(size_t dim); + static gtsam::noiseModel::Constrained* All(size_t dim, double mu); + + gtsam::noiseModel::Constrained* unit() const; + + // enabling serialization functionality + void serializable() const; +}; + +virtual class Isotropic : gtsam::noiseModel::Diagonal { + static gtsam::noiseModel::Isotropic* Sigma(size_t dim, double sigma); + static gtsam::noiseModel::Isotropic* Variance(size_t dim, double varianace); + static gtsam::noiseModel::Isotropic* Precision(size_t dim, double precision); + + // access to noise model + double sigma() const; + + // enabling serialization functionality + void serializable() const; +}; + +virtual class Unit : gtsam::noiseModel::Isotropic { + static gtsam::noiseModel::Unit* Create(size_t dim); + + // enabling serialization functionality + void serializable() const; +}; + +namespace mEstimator { +virtual class Base { + void print(string s = "") const; +}; + +virtual class Null: gtsam::noiseModel::mEstimator::Base { + Null(); + static gtsam::noiseModel::mEstimator::Null* Create(); + + // enabling serialization functionality + void serializable() const; + + double weight(double error) const; + double loss(double error) const; +}; + +virtual class Fair: gtsam::noiseModel::mEstimator::Base { + Fair(double c); + static gtsam::noiseModel::mEstimator::Fair* Create(double c); + + // enabling serialization functionality + void serializable() const; + + double weight(double error) const; + double loss(double error) const; +}; + +virtual class Huber: gtsam::noiseModel::mEstimator::Base { + Huber(double k); + static gtsam::noiseModel::mEstimator::Huber* Create(double k); + + // enabling serialization functionality + void serializable() const; + + double weight(double error) const; + double loss(double error) const; +}; + +virtual class Cauchy: gtsam::noiseModel::mEstimator::Base { + Cauchy(double k); + static gtsam::noiseModel::mEstimator::Cauchy* Create(double k); + + // enabling serialization functionality + void serializable() const; + + double weight(double error) const; + double loss(double error) const; +}; + +virtual class Tukey: gtsam::noiseModel::mEstimator::Base { + Tukey(double k); + static gtsam::noiseModel::mEstimator::Tukey* Create(double k); + + // enabling serialization functionality + void serializable() const; + + double weight(double error) const; + double loss(double error) const; +}; + +virtual class Welsch: gtsam::noiseModel::mEstimator::Base { + Welsch(double k); + static gtsam::noiseModel::mEstimator::Welsch* Create(double k); + + // enabling serialization functionality + void serializable() const; + + double weight(double error) const; + double loss(double error) const; +}; + +virtual class GemanMcClure: gtsam::noiseModel::mEstimator::Base { + GemanMcClure(double c); + static gtsam::noiseModel::mEstimator::GemanMcClure* Create(double c); + + // enabling serialization functionality + void serializable() const; + + double weight(double error) const; + double loss(double error) const; +}; + +virtual class DCS: gtsam::noiseModel::mEstimator::Base { + DCS(double c); + static gtsam::noiseModel::mEstimator::DCS* Create(double c); + + // enabling serialization functionality + void serializable() const; + + double weight(double error) const; + double loss(double error) const; +}; + +virtual class L2WithDeadZone: gtsam::noiseModel::mEstimator::Base { + L2WithDeadZone(double k); + static gtsam::noiseModel::mEstimator::L2WithDeadZone* Create(double k); + + // enabling serialization functionality + void serializable() const; + + double weight(double error) const; + double loss(double error) const; +}; + +}///\namespace mEstimator + +virtual class Robust : gtsam::noiseModel::Base { + Robust(const gtsam::noiseModel::mEstimator::Base* robust, const gtsam::noiseModel::Base* noise); + static gtsam::noiseModel::Robust* Create(const gtsam::noiseModel::mEstimator::Base* robust, const gtsam::noiseModel::Base* noise); + + // enabling serialization functionality + void serializable() const; +}; + +}///\namespace noiseModel + +#include +class Sampler { + // Constructors + Sampler(gtsam::noiseModel::Diagonal* model, int seed); + Sampler(Vector sigmas, int seed); + + // Standard Interface + size_t dim() const; + Vector sigmas() const; + gtsam::noiseModel::Diagonal* model() const; + Vector sample(); +}; + +#include +class VectorValues { + //Constructors + VectorValues(); + VectorValues(const gtsam::VectorValues& other); + + //Named Constructors + static gtsam::VectorValues Zero(const gtsam::VectorValues& model); + + //Standard Interface + size_t size() const; + size_t dim(size_t j) const; + bool exists(size_t j) const; + void print(string s = "VectorValues", + const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + bool equals(const gtsam::VectorValues& expected, double tol) const; + void insert(size_t j, Vector value); + Vector vector() const; + Vector at(size_t j) const; + void update(const gtsam::VectorValues& values); + + //Advanced Interface + void setZero(); + + gtsam::VectorValues add(const gtsam::VectorValues& c) const; + void addInPlace(const gtsam::VectorValues& c); + gtsam::VectorValues subtract(const gtsam::VectorValues& c) const; + gtsam::VectorValues scale(double a) const; + void scaleInPlace(double a); + + bool hasSameStructure(const gtsam::VectorValues& other) const; + double dot(const gtsam::VectorValues& V) const; + double norm() const; + double squaredNorm() const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; +}; + +#include +virtual class GaussianFactor { + gtsam::KeyVector keys() const; + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + bool equals(const gtsam::GaussianFactor& lf, double tol) const; + double error(const gtsam::VectorValues& c) const; + gtsam::GaussianFactor* clone() const; + gtsam::GaussianFactor* negate() const; + Matrix augmentedInformation() const; + Matrix information() const; + Matrix augmentedJacobian() const; + pair jacobian() const; + size_t size() const; + bool empty() const; +}; + +#include +virtual class JacobianFactor : gtsam::GaussianFactor { + //Constructors + JacobianFactor(); + JacobianFactor(const gtsam::GaussianFactor& factor); + JacobianFactor(Vector b_in); + JacobianFactor(size_t i1, Matrix A1, Vector b, + const gtsam::noiseModel::Diagonal* model); + JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, Vector b, + const gtsam::noiseModel::Diagonal* model); + JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, size_t i3, Matrix A3, + Vector b, const gtsam::noiseModel::Diagonal* model); + JacobianFactor(const gtsam::GaussianFactorGraph& graph); + + //Testable + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + void printKeys(string s) const; + bool equals(const gtsam::GaussianFactor& lf, double tol) const; + size_t size() const; + Vector unweighted_error(const gtsam::VectorValues& c) const; + Vector error_vector(const gtsam::VectorValues& c) const; + double error(const gtsam::VectorValues& c) const; + + //Standard Interface + Matrix getA() const; + Vector getb() const; + size_t rows() const; + size_t cols() const; + bool isConstrained() const; + pair jacobianUnweighted() const; + Matrix augmentedJacobianUnweighted() const; + + void transposeMultiplyAdd(double alpha, Vector e, gtsam::VectorValues& x) const; + gtsam::JacobianFactor whiten() const; + + pair eliminate(const gtsam::Ordering& keys) const; + + void setModel(bool anyConstrained, Vector sigmas); + + gtsam::noiseModel::Diagonal* get_model() const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; +}; + +#include +virtual class HessianFactor : gtsam::GaussianFactor { + //Constructors + HessianFactor(); + HessianFactor(const gtsam::GaussianFactor& factor); + HessianFactor(size_t j, Matrix G, Vector g, double f); + HessianFactor(size_t j, Vector mu, Matrix Sigma); + HessianFactor(size_t j1, size_t j2, Matrix G11, Matrix G12, Vector g1, Matrix G22, + Vector g2, double f); + HessianFactor(size_t j1, size_t j2, size_t j3, Matrix G11, Matrix G12, Matrix G13, + Vector g1, Matrix G22, Matrix G23, Vector g2, Matrix G33, Vector g3, + double f); + HessianFactor(const gtsam::GaussianFactorGraph& factors); + + //Testable + size_t size() const; + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + void printKeys(string s) const; + bool equals(const gtsam::GaussianFactor& lf, double tol) const; + double error(const gtsam::VectorValues& c) const; + + //Standard Interface + size_t rows() const; + Matrix information() const; + double constantTerm() const; + Vector linearTerm() const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; +}; + +#include +class GaussianFactorGraph { + GaussianFactorGraph(); + GaussianFactorGraph(const gtsam::GaussianBayesNet& bayesNet); + GaussianFactorGraph(const gtsam::GaussianBayesTree& bayesTree); + + // From FactorGraph + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + bool equals(const gtsam::GaussianFactorGraph& lfgraph, double tol) const; + size_t size() const; + gtsam::GaussianFactor* at(size_t idx) const; + gtsam::KeySet keys() const; + gtsam::KeyVector keyVector() const; + bool exists(size_t idx) const; + + // Building the graph + void push_back(const gtsam::GaussianFactor* factor); + void push_back(const gtsam::GaussianConditional* conditional); + void push_back(const gtsam::GaussianFactorGraph& graph); + void push_back(const gtsam::GaussianBayesNet& bayesNet); + void push_back(const gtsam::GaussianBayesTree& bayesTree); + void add(const gtsam::GaussianFactor& factor); + void add(Vector b); + void add(size_t key1, Matrix A1, Vector b, const gtsam::noiseModel::Diagonal* model); + void add(size_t key1, Matrix A1, size_t key2, Matrix A2, Vector b, + const gtsam::noiseModel::Diagonal* model); + void add(size_t key1, Matrix A1, size_t key2, Matrix A2, size_t key3, Matrix A3, + Vector b, const gtsam::noiseModel::Diagonal* model); + + // error and probability + double error(const gtsam::VectorValues& c) const; + double probPrime(const gtsam::VectorValues& c) const; + + gtsam::GaussianFactorGraph clone() const; + gtsam::GaussianFactorGraph negate() const; + + // Optimizing and linear algebra + gtsam::VectorValues optimize() const; + gtsam::VectorValues optimize(const gtsam::Ordering& ordering) const; + gtsam::VectorValues optimizeGradientSearch() const; + gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const; + gtsam::VectorValues gradientAtZero() const; + + // Elimination and marginals + gtsam::GaussianBayesNet* eliminateSequential(); + gtsam::GaussianBayesNet* eliminateSequential(const gtsam::Ordering& ordering); + gtsam::GaussianBayesTree* eliminateMultifrontal(); + gtsam::GaussianBayesTree* eliminateMultifrontal(const gtsam::Ordering& ordering); + pair eliminatePartialSequential( + const gtsam::Ordering& ordering); + pair eliminatePartialSequential( + const gtsam::KeyVector& keys); + pair eliminatePartialMultifrontal( + const gtsam::Ordering& ordering); + pair eliminatePartialMultifrontal( + const gtsam::KeyVector& keys); + gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& ordering); + gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& key_vector); + gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& ordering, + const gtsam::Ordering& marginalizedVariableOrdering); + gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& key_vector, + const gtsam::Ordering& marginalizedVariableOrdering); + gtsam::GaussianFactorGraph* marginal(const gtsam::KeyVector& key_vector); + + // Conversion to matrices + Matrix sparseJacobian_() const; + Matrix augmentedJacobian() const; + Matrix augmentedJacobian(const gtsam::Ordering& ordering) const; + pair jacobian() const; + pair jacobian(const gtsam::Ordering& ordering) const; + Matrix augmentedHessian() const; + Matrix augmentedHessian(const gtsam::Ordering& ordering) const; + pair hessian() const; + pair hessian(const gtsam::Ordering& ordering) const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; +}; + +#include +virtual class GaussianConditional : gtsam::JacobianFactor { + //Constructors + GaussianConditional(size_t key, Vector d, Matrix R, const gtsam::noiseModel::Diagonal* sigmas); + GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, + const gtsam::noiseModel::Diagonal* sigmas); + GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, + size_t name2, Matrix T, const gtsam::noiseModel::Diagonal* sigmas); + + //Constructors with no noise model + GaussianConditional(size_t key, Vector d, Matrix R); + GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S); + GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, + size_t name2, Matrix T); + + //Standard Interface + void print(string s = "GaussianConditional", + const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + bool equals(const gtsam::GaussianConditional& cg, double tol) const; + + // Advanced Interface + gtsam::VectorValues solve(const gtsam::VectorValues& parents) const; + gtsam::VectorValues solveOtherRHS(const gtsam::VectorValues& parents, + const gtsam::VectorValues& rhs) const; + void solveTransposeInPlace(gtsam::VectorValues& gy) const; + void scaleFrontalsBySigma(gtsam::VectorValues& gy) const; + Matrix R() const; + Matrix S() const; + Vector d() const; + + // enabling serialization functionality + void serialize() const; +}; + +#include +virtual class GaussianDensity : gtsam::GaussianConditional { + //Constructors + GaussianDensity(size_t key, Vector d, Matrix R, const gtsam::noiseModel::Diagonal* sigmas); + + //Standard Interface + void print(string s = "GaussianDensity", + const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + bool equals(const gtsam::GaussianDensity &cg, double tol) const; + Vector mean() const; + Matrix covariance() const; +}; + +#include +virtual class GaussianBayesNet { + //Constructors + GaussianBayesNet(); + GaussianBayesNet(const gtsam::GaussianConditional* conditional); + + // Testable + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + bool equals(const gtsam::GaussianBayesNet& other, double tol) const; + size_t size() const; + + // FactorGraph derived interface + // size_t size() const; + gtsam::GaussianConditional* at(size_t idx) const; + gtsam::KeySet keys() const; + bool exists(size_t idx) const; + + void saveGraph(const string& s) const; + + gtsam::GaussianConditional* front() const; + gtsam::GaussianConditional* back() const; + void push_back(gtsam::GaussianConditional* conditional); + void push_back(const gtsam::GaussianBayesNet& bayesNet); + + gtsam::VectorValues optimize() const; + gtsam::VectorValues optimize(gtsam::VectorValues& solutionForMissing) const; + gtsam::VectorValues optimizeGradientSearch() const; + gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const; + gtsam::VectorValues gradientAtZero() const; + double error(const gtsam::VectorValues& x) const; + double determinant() const; + double logDeterminant() const; + gtsam::VectorValues backSubstitute(const gtsam::VectorValues& gx) const; + gtsam::VectorValues backSubstituteTranspose(const gtsam::VectorValues& gx) const; +}; + +#include +virtual class GaussianBayesTree { + // Standard Constructors and Named Constructors + GaussianBayesTree(); + GaussianBayesTree(const gtsam::GaussianBayesTree& other); + bool equals(const gtsam::GaussianBayesTree& other, double tol) const; + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter); + size_t size() const; + bool empty() const; + size_t numCachedSeparatorMarginals() const; + void saveGraph(string s) const; + + gtsam::VectorValues optimize() const; + gtsam::VectorValues optimizeGradientSearch() const; + gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const; + gtsam::VectorValues gradientAtZero() const; + double error(const gtsam::VectorValues& x) const; + double determinant() const; + double logDeterminant() const; + Matrix marginalCovariance(size_t key) const; + gtsam::GaussianConditional* marginalFactor(size_t key) const; + gtsam::GaussianFactorGraph* joint(size_t key1, size_t key2) const; + gtsam::GaussianBayesNet* jointBayesNet(size_t key1, size_t key2) const; +}; + +#include +class Errors { + //Constructors + Errors(); + Errors(const gtsam::VectorValues& V); + + //Testable + void print(string s = "Errors"); + bool equals(const gtsam::Errors& expected, double tol) const; +}; + +#include +class GaussianISAM { + //Constructor + GaussianISAM(); + + //Standard Interface + void update(const gtsam::GaussianFactorGraph& newFactors); + void saveGraph(string s) const; + void clear(); +}; + +#include +virtual class IterativeOptimizationParameters { + string getVerbosity() const; + void setVerbosity(string s) ; +}; + +//virtual class IterativeSolver { +// IterativeSolver(); +// gtsam::VectorValues optimize (); +//}; + +#include +virtual class ConjugateGradientParameters : gtsam::IterativeOptimizationParameters { + ConjugateGradientParameters(); + int getMinIterations() const ; + int getMaxIterations() const ; + int getReset() const; + double getEpsilon_rel() const; + double getEpsilon_abs() const; + + void setMinIterations(int value); + void setMaxIterations(int value); + void setReset(int value); + void setEpsilon_rel(double value); + void setEpsilon_abs(double value); +}; + +#include +virtual class PreconditionerParameters { + PreconditionerParameters(); +}; + +virtual class DummyPreconditionerParameters : gtsam::PreconditionerParameters { + DummyPreconditionerParameters(); +}; + +#include +virtual class PCGSolverParameters : gtsam::ConjugateGradientParameters { + PCGSolverParameters(); + void print(string s = ""); + void setPreconditionerParams(gtsam::PreconditionerParameters* preconditioner); +}; + +#include +virtual class SubgraphSolverParameters : gtsam::ConjugateGradientParameters { + SubgraphSolverParameters(); +}; + +virtual class SubgraphSolver { + SubgraphSolver(const gtsam::GaussianFactorGraph &A, const gtsam::SubgraphSolverParameters ¶meters, const gtsam::Ordering& ordering); + SubgraphSolver(const gtsam::GaussianFactorGraph &Ab1, const gtsam::GaussianFactorGraph* Ab2, const gtsam::SubgraphSolverParameters ¶meters, const gtsam::Ordering& ordering); + gtsam::VectorValues optimize() const; +}; + +#include +class KalmanFilter { + KalmanFilter(size_t n); + // gtsam::GaussianDensity* init(Vector x0, const gtsam::SharedDiagonal& P0); + gtsam::GaussianDensity* init(Vector x0, Matrix P0); + void print(string s = "") const; + static size_t step(gtsam::GaussianDensity* p); + gtsam::GaussianDensity* predict(gtsam::GaussianDensity* p, Matrix F, + Matrix B, Vector u, const gtsam::noiseModel::Diagonal* modelQ); + gtsam::GaussianDensity* predictQ(gtsam::GaussianDensity* p, Matrix F, + Matrix B, Vector u, Matrix Q); + gtsam::GaussianDensity* predict2(gtsam::GaussianDensity* p, Matrix A0, + Matrix A1, Vector b, const gtsam::noiseModel::Diagonal* model); + gtsam::GaussianDensity* update(gtsam::GaussianDensity* p, Matrix H, + Vector z, const gtsam::noiseModel::Diagonal* model); + gtsam::GaussianDensity* updateQ(gtsam::GaussianDensity* p, Matrix H, + Vector z, Matrix Q); +}; + +} \ No newline at end of file diff --git a/gtsam/navigation/navigation.i b/gtsam/navigation/navigation.i new file mode 100644 index 000000000..48a5a35de --- /dev/null +++ b/gtsam/navigation/navigation.i @@ -0,0 +1,355 @@ +//************************************************************************* +// Navigation +//************************************************************************* + +namespace gtsam { + +namespace imuBias { +#include + +class ConstantBias { + // Constructors + ConstantBias(); + ConstantBias(Vector biasAcc, Vector biasGyro); + + // Testable + void print(string s = "") const; + bool equals(const gtsam::imuBias::ConstantBias& expected, double tol) const; + + // Group + static gtsam::imuBias::ConstantBias identity(); + gtsam::imuBias::ConstantBias inverse() const; + gtsam::imuBias::ConstantBias compose(const gtsam::imuBias::ConstantBias& b) const; + gtsam::imuBias::ConstantBias between(const gtsam::imuBias::ConstantBias& b) const; + + // Operator Overloads + gtsam::imuBias::ConstantBias operator-() const; + gtsam::imuBias::ConstantBias operator+(const gtsam::imuBias::ConstantBias& b) const; + gtsam::imuBias::ConstantBias operator-(const gtsam::imuBias::ConstantBias& b) const; + + // Manifold + gtsam::imuBias::ConstantBias retract(Vector v) const; + Vector localCoordinates(const gtsam::imuBias::ConstantBias& b) const; + + // Lie Group + static gtsam::imuBias::ConstantBias Expmap(Vector v); + static Vector Logmap(const gtsam::imuBias::ConstantBias& b); + + // Standard Interface + Vector vector() const; + Vector accelerometer() const; + Vector gyroscope() const; + Vector correctAccelerometer(Vector measurement) const; + Vector correctGyroscope(Vector measurement) const; +}; + +}///\namespace imuBias + +#include +class NavState { + // Constructors + NavState(); + NavState(const gtsam::Rot3& R, const gtsam::Point3& t, Vector v); + NavState(const gtsam::Pose3& pose, Vector v); + + // Testable + void print(string s = "") const; + bool equals(const gtsam::NavState& expected, double tol) const; + + // Access + gtsam::Rot3 attitude() const; + gtsam::Point3 position() const; + Vector velocity() const; + gtsam::Pose3 pose() const; + + gtsam::NavState retract(const Vector& x) const; + Vector localCoordinates(const gtsam::NavState& g) const; +}; + +#include +virtual class PreintegratedRotationParams { + PreintegratedRotationParams(); + + // Testable + void print(string s = "") const; + bool equals(const gtsam::PreintegratedRotationParams& expected, double tol); + + void setGyroscopeCovariance(Matrix cov); + void setOmegaCoriolis(Vector omega); + void setBodyPSensor(const gtsam::Pose3& pose); + + Matrix getGyroscopeCovariance() const; + + boost::optional getOmegaCoriolis() const; + boost::optional getBodyPSensor() const; +}; + +#include +virtual class PreintegrationParams : gtsam::PreintegratedRotationParams { + PreintegrationParams(Vector n_gravity); + + static gtsam::PreintegrationParams* MakeSharedD(double g); + static gtsam::PreintegrationParams* MakeSharedU(double g); + static gtsam::PreintegrationParams* MakeSharedD(); // default g = 9.81 + static gtsam::PreintegrationParams* MakeSharedU(); // default g = 9.81 + + // Testable + void print(string s = "") const; + bool equals(const gtsam::PreintegrationParams& expected, double tol); + + void setAccelerometerCovariance(Matrix cov); + void setIntegrationCovariance(Matrix cov); + void setUse2ndOrderCoriolis(bool flag); + + Matrix getAccelerometerCovariance() const; + Matrix getIntegrationCovariance() const; + bool getUse2ndOrderCoriolis() const; +}; + +#include +class PreintegratedImuMeasurements { + // Constructors + PreintegratedImuMeasurements(const gtsam::PreintegrationParams* params); + PreintegratedImuMeasurements(const gtsam::PreintegrationParams* params, + const gtsam::imuBias::ConstantBias& bias); + + // Testable + void print(string s = "") const; + bool equals(const gtsam::PreintegratedImuMeasurements& expected, double tol); + + // Standard Interface + void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, + double deltaT); + void resetIntegration(); + void resetIntegrationAndSetBias(const gtsam::imuBias::ConstantBias& biasHat); + + Matrix preintMeasCov() const; + Vector preintegrated() const; + double deltaTij() const; + gtsam::Rot3 deltaRij() const; + Vector deltaPij() const; + Vector deltaVij() const; + gtsam::imuBias::ConstantBias biasHat() const; + Vector biasHatVector() const; + gtsam::NavState predict(const gtsam::NavState& state_i, + const gtsam::imuBias::ConstantBias& bias) const; +}; + +virtual class ImuFactor: gtsam::NonlinearFactor { + ImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, + size_t bias, + const gtsam::PreintegratedImuMeasurements& preintegratedMeasurements); + + // Standard Interface + gtsam::PreintegratedImuMeasurements preintegratedMeasurements() const; + Vector evaluateError(const gtsam::Pose3& pose_i, Vector vel_i, + const gtsam::Pose3& pose_j, Vector vel_j, + const gtsam::imuBias::ConstantBias& bias); +}; + +#include +virtual class PreintegrationCombinedParams : gtsam::PreintegrationParams { + PreintegrationCombinedParams(Vector n_gravity); + + static gtsam::PreintegrationCombinedParams* MakeSharedD(double g); + static gtsam::PreintegrationCombinedParams* MakeSharedU(double g); + static gtsam::PreintegrationCombinedParams* MakeSharedD(); // default g = 9.81 + static gtsam::PreintegrationCombinedParams* MakeSharedU(); // default g = 9.81 + + // Testable + void print(string s = "") const; + bool equals(const gtsam::PreintegrationCombinedParams& expected, double tol); + + void setBiasAccCovariance(Matrix cov); + void setBiasOmegaCovariance(Matrix cov); + void setBiasAccOmegaInt(Matrix cov); + + Matrix getBiasAccCovariance() const ; + Matrix getBiasOmegaCovariance() const ; + Matrix getBiasAccOmegaInt() const; + +}; + +class PreintegratedCombinedMeasurements { +// Constructors + PreintegratedCombinedMeasurements(const gtsam::PreintegrationCombinedParams* params); + PreintegratedCombinedMeasurements(const gtsam::PreintegrationCombinedParams* params, + const gtsam::imuBias::ConstantBias& bias); + // Testable + void print(string s = "Preintegrated Measurements:") const; + bool equals(const gtsam::PreintegratedCombinedMeasurements& expected, + double tol); + + // Standard Interface + void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, + double deltaT); + void resetIntegration(); + void resetIntegrationAndSetBias(const gtsam::imuBias::ConstantBias& biasHat); + + Matrix preintMeasCov() const; + double deltaTij() const; + gtsam::Rot3 deltaRij() const; + Vector deltaPij() const; + Vector deltaVij() const; + gtsam::imuBias::ConstantBias biasHat() const; + Vector biasHatVector() const; + gtsam::NavState predict(const gtsam::NavState& state_i, + const gtsam::imuBias::ConstantBias& bias) const; +}; + +virtual class CombinedImuFactor: gtsam::NonlinearFactor { + CombinedImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, + size_t bias_i, size_t bias_j, + const gtsam::PreintegratedCombinedMeasurements& CombinedPreintegratedMeasurements); + + // Standard Interface + gtsam::PreintegratedCombinedMeasurements preintegratedMeasurements() const; + Vector evaluateError(const gtsam::Pose3& pose_i, Vector vel_i, + const gtsam::Pose3& pose_j, Vector vel_j, + const gtsam::imuBias::ConstantBias& bias_i, + const gtsam::imuBias::ConstantBias& bias_j); +}; + +#include +class PreintegratedAhrsMeasurements { + // Standard Constructor + PreintegratedAhrsMeasurements(Vector bias, Matrix measuredOmegaCovariance); + PreintegratedAhrsMeasurements(const gtsam::PreintegratedAhrsMeasurements& rhs); + + // Testable + void print(string s = "Preintegrated Measurements: ") const; + bool equals(const gtsam::PreintegratedAhrsMeasurements& expected, double tol); + + // get Data + gtsam::Rot3 deltaRij() const; + double deltaTij() const; + Vector biasHat() const; + + // Standard Interface + void integrateMeasurement(Vector measuredOmega, double deltaT); + void resetIntegration() ; +}; + +virtual class AHRSFactor : gtsam::NonlinearFactor { + AHRSFactor(size_t rot_i, size_t rot_j,size_t bias, + const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, Vector omegaCoriolis); + AHRSFactor(size_t rot_i, size_t rot_j, size_t bias, + const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, Vector omegaCoriolis, + const gtsam::Pose3& body_P_sensor); + + // Standard Interface + gtsam::PreintegratedAhrsMeasurements preintegratedMeasurements() const; + Vector evaluateError(const gtsam::Rot3& rot_i, const gtsam::Rot3& rot_j, + Vector bias) const; + gtsam::Rot3 predict(const gtsam::Rot3& rot_i, Vector bias, + const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, + Vector omegaCoriolis) const; +}; + +#include +//virtual class AttitudeFactor : gtsam::NonlinearFactor { +// AttitudeFactor(const Unit3& nZ, const Unit3& bRef); +// AttitudeFactor(); +//}; +virtual class Rot3AttitudeFactor : gtsam::NonlinearFactor{ + Rot3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model, + const gtsam::Unit3& bRef); + Rot3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model); + Rot3AttitudeFactor(); + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + bool equals(const gtsam::NonlinearFactor& expected, double tol) const; + gtsam::Unit3 nZ() const; + gtsam::Unit3 bRef() const; +}; + +virtual class Pose3AttitudeFactor : gtsam::NonlinearFactor { + Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, + const gtsam::noiseModel::Diagonal* model, + const gtsam::Unit3& bRef); + Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, + const gtsam::noiseModel::Diagonal* model); + Pose3AttitudeFactor(); + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + bool equals(const gtsam::NonlinearFactor& expected, double tol) const; + gtsam::Unit3 nZ() const; + gtsam::Unit3 bRef() const; +}; + +#include +virtual class GPSFactor : gtsam::NonlinearFactor{ + GPSFactor(size_t key, const gtsam::Point3& gpsIn, + const gtsam::noiseModel::Base* model); + + // Testable + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + bool equals(const gtsam::GPSFactor& expected, double tol); + + // Standard Interface + gtsam::Point3 measurementIn() const; +}; + +virtual class GPSFactor2 : gtsam::NonlinearFactor { + GPSFactor2(size_t key, const gtsam::Point3& gpsIn, + const gtsam::noiseModel::Base* model); + + // Testable + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + bool equals(const gtsam::GPSFactor2& expected, double tol); + + // Standard Interface + gtsam::Point3 measurementIn() const; +}; + +#include +virtual class Scenario { + gtsam::Pose3 pose(double t) const; + Vector omega_b(double t) const; + Vector velocity_n(double t) const; + Vector acceleration_n(double t) const; + gtsam::Rot3 rotation(double t) const; + gtsam::NavState navState(double t) const; + Vector velocity_b(double t) const; + Vector acceleration_b(double t) const; +}; + +virtual class ConstantTwistScenario : gtsam::Scenario { + ConstantTwistScenario(Vector w, Vector v); + ConstantTwistScenario(Vector w, Vector v, + const gtsam::Pose3& nTb0); +}; + +virtual class AcceleratingScenario : gtsam::Scenario { + AcceleratingScenario(const gtsam::Rot3& nRb, const gtsam::Point3& p0, + Vector v0, Vector a_n, + Vector omega_b); +}; + +#include +class ScenarioRunner { + ScenarioRunner(const gtsam::Scenario& scenario, + const gtsam::PreintegrationParams* p, + double imuSampleTime, + const gtsam::imuBias::ConstantBias& bias); + Vector gravity_n() const; + Vector actualAngularVelocity(double t) const; + Vector actualSpecificForce(double t) const; + Vector measuredAngularVelocity(double t) const; + Vector measuredSpecificForce(double t) const; + double imuSampleTime() const; + gtsam::PreintegratedImuMeasurements integrate( + double T, const gtsam::imuBias::ConstantBias& estimatedBias, + bool corrupted) const; + gtsam::NavState predict( + const gtsam::PreintegratedImuMeasurements& pim, + const gtsam::imuBias::ConstantBias& estimatedBias) const; + Matrix estimateCovariance( + double T, size_t N, + const gtsam::imuBias::ConstantBias& estimatedBias) const; + Matrix estimateNoiseCovariance(size_t N) const; +}; + +} diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i new file mode 100644 index 000000000..e22ac5954 --- /dev/null +++ b/gtsam/nonlinear/nonlinear.i @@ -0,0 +1,769 @@ +//************************************************************************* +// nonlinear +//************************************************************************* + +namespace gtsam { + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +class Symbol { + Symbol(); + Symbol(char c, uint64_t j); + Symbol(size_t key); + + size_t key() const; + void print(const string& s = "") const; + bool equals(const gtsam::Symbol& expected, double tol) const; + + char chr() const; + uint64_t index() const; + string string() const; +}; + +size_t symbol(char chr, size_t index); +char symbolChr(size_t key); +size_t symbolIndex(size_t key); + +namespace symbol_shorthand { +size_t A(size_t j); +size_t B(size_t j); +size_t C(size_t j); +size_t D(size_t j); +size_t E(size_t j); +size_t F(size_t j); +size_t G(size_t j); +size_t H(size_t j); +size_t I(size_t j); +size_t J(size_t j); +size_t K(size_t j); +size_t L(size_t j); +size_t M(size_t j); +size_t N(size_t j); +size_t O(size_t j); +size_t P(size_t j); +size_t Q(size_t j); +size_t R(size_t j); +size_t S(size_t j); +size_t T(size_t j); +size_t U(size_t j); +size_t V(size_t j); +size_t W(size_t j); +size_t X(size_t j); +size_t Y(size_t j); +size_t Z(size_t j); +} // namespace symbol_shorthand + +// Default keyformatter +void PrintKeyList(const gtsam::KeyList& keys); +void PrintKeyList(const gtsam::KeyList& keys, string s); +void PrintKeyVector(const gtsam::KeyVector& keys); +void PrintKeyVector(const gtsam::KeyVector& keys, string s); +void PrintKeySet(const gtsam::KeySet& keys); +void PrintKeySet(const gtsam::KeySet& keys, string s); + +#include +class LabeledSymbol { + LabeledSymbol(size_t full_key); + LabeledSymbol(const gtsam::LabeledSymbol& key); + LabeledSymbol(unsigned char valType, unsigned char label, size_t j); + + size_t key() const; + unsigned char label() const; + unsigned char chr() const; + size_t index() const; + + gtsam::LabeledSymbol upper() const; + gtsam::LabeledSymbol lower() const; + gtsam::LabeledSymbol newChr(unsigned char c) const; + gtsam::LabeledSymbol newLabel(unsigned char label) const; + + void print(string s = "") const; +}; + +size_t mrsymbol(unsigned char c, unsigned char label, size_t j); +unsigned char mrsymbolChr(size_t key); +unsigned char mrsymbolLabel(size_t key); +size_t mrsymbolIndex(size_t key); + +#include +class Ordering { + // Standard Constructors and Named Constructors + Ordering(); + Ordering(const gtsam::Ordering& other); + + // Testable + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + bool equals(const gtsam::Ordering& ord, double tol) const; + + // Standard interface + size_t size() const; + size_t at(size_t key) const; + void push_back(size_t key); + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; +}; + +#include +class NonlinearFactorGraph { + NonlinearFactorGraph(); + NonlinearFactorGraph(const gtsam::NonlinearFactorGraph& graph); + + // FactorGraph + void print(string s = "NonlinearFactorGraph: ", + const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + bool equals(const gtsam::NonlinearFactorGraph& fg, double tol) const; + size_t size() const; + bool empty() const; + void remove(size_t i); + void replace(size_t i, gtsam::NonlinearFactor* factors); + void resize(size_t size); + size_t nrFactors() const; + gtsam::NonlinearFactor* at(size_t idx) const; + void push_back(const gtsam::NonlinearFactorGraph& factors); + void push_back(gtsam::NonlinearFactor* factor); + void add(gtsam::NonlinearFactor* factor); + bool exists(size_t idx) const; + gtsam::KeySet keys() const; + gtsam::KeyVector keyVector() const; + + template , + gtsam::PinholeCamera, + gtsam::imuBias::ConstantBias}> + void addPrior(size_t key, const T& prior, + const gtsam::noiseModel::Base* noiseModel); + + // NonlinearFactorGraph + void printErrors(const gtsam::Values& values) const; + double error(const gtsam::Values& values) const; + double probPrime(const gtsam::Values& values) const; + gtsam::Ordering orderingCOLAMD() const; + // Ordering* orderingCOLAMDConstrained(const gtsam::Values& c, const + // std::map& constraints) const; + gtsam::GaussianFactorGraph* linearize(const gtsam::Values& values) const; + gtsam::NonlinearFactorGraph clone() const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; + + void saveGraph(const string& s) const; +}; + +#include +virtual class NonlinearFactor { + // Factor base class + size_t size() const; + gtsam::KeyVector keys() const; + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + void printKeys(string s) const; + // NonlinearFactor + bool equals(const gtsam::NonlinearFactor& other, double tol) const; + double error(const gtsam::Values& c) const; + size_t dim() const; + bool active(const gtsam::Values& c) const; + gtsam::GaussianFactor* linearize(const gtsam::Values& c) const; + gtsam::NonlinearFactor* clone() const; + gtsam::NonlinearFactor* rekey(const gtsam::KeyVector& newKeys) const; +}; + +#include +virtual class NoiseModelFactor : gtsam::NonlinearFactor { + bool equals(const gtsam::NoiseModelFactor& other, double tol) const; + gtsam::noiseModel::Base* noiseModel() const; + Vector unwhitenedError(const gtsam::Values& x) const; + Vector whitenedError(const gtsam::Values& x) const; +}; + +#include +virtual class CustomFactor : gtsam::NoiseModelFactor { + /* + * Note CustomFactor will not be wrapped for MATLAB, as there is no supporting + * machinery there. This is achieved by adding `gtsam::CustomFactor` to the + * ignore list in `matlab/CMakeLists.txt`. + */ + CustomFactor(); + /* + * Example: + * ``` + * def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]): + * + * if not H is None: + * + * H[0] = J1 # 2-d numpy array for a Jacobian block + * H[1] = J2 + * ... + * return error # 1-d numpy array + * + * cf = CustomFactor(noise_model, keys, error_func) + * ``` + */ + CustomFactor(const gtsam::SharedNoiseModel& noiseModel, + const gtsam::KeyVector& keys, + const gtsam::CustomErrorFunction& errorFunction); + + void print(string s = "", + gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter); +}; + +#include +class Values { + Values(); + Values(const gtsam::Values& other); + + size_t size() const; + bool empty() const; + void clear(); + size_t dim() const; + + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + bool equals(const gtsam::Values& other, double tol) const; + + void insert(const gtsam::Values& values); + void update(const gtsam::Values& values); + void erase(size_t j); + void swap(gtsam::Values& values); + + bool exists(size_t j) const; + gtsam::KeyVector keys() const; + + gtsam::VectorValues zeroVectors() const; + + gtsam::Values retract(const gtsam::VectorValues& delta) const; + gtsam::VectorValues localCoordinates(const gtsam::Values& cp) const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() 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; + + // The order is important: Vector has to precede Point2/Point3 so `atVector` + // can work for those fixed-size vectors. + void insert(size_t j, Vector vector); + void insert(size_t j, Matrix matrix); + void insert(size_t j, const gtsam::Point2& point2); + void insert(size_t j, const gtsam::Point3& point3); + void insert(size_t j, const gtsam::Rot2& rot2); + void insert(size_t j, const gtsam::Pose2& pose2); + void insert(size_t j, const gtsam::SO3& R); + void insert(size_t j, const gtsam::SO4& Q); + void insert(size_t j, const gtsam::SOn& P); + void insert(size_t j, const gtsam::Rot3& rot3); + void insert(size_t j, const gtsam::Pose3& pose3); + void insert(size_t j, const gtsam::Unit3& unit3); + void insert(size_t j, const gtsam::Cal3_S2& cal3_s2); + void insert(size_t j, const gtsam::Cal3DS2& cal3ds2); + void insert(size_t j, const gtsam::Cal3Bundler& cal3bundler); + void insert(size_t j, const gtsam::EssentialMatrix& essential_matrix); + void insert(size_t j, + const gtsam::PinholeCamera& simple_camera); + void insert(size_t j, const gtsam::PinholeCamera& camera); + void insert(size_t j, const gtsam::imuBias::ConstantBias& constant_bias); + void insert(size_t j, const gtsam::NavState& nav_state); + void insert(size_t j, double c); + + void update(size_t j, const gtsam::Point2& point2); + void update(size_t j, const gtsam::Point3& point3); + void update(size_t j, const gtsam::Rot2& rot2); + void update(size_t j, const gtsam::Pose2& pose2); + void update(size_t j, const gtsam::SO3& R); + void update(size_t j, const gtsam::SO4& Q); + void update(size_t j, const gtsam::SOn& P); + void update(size_t j, const gtsam::Rot3& rot3); + void update(size_t j, const gtsam::Pose3& pose3); + void update(size_t j, const gtsam::Unit3& unit3); + void update(size_t j, const gtsam::Cal3_S2& cal3_s2); + void update(size_t j, const gtsam::Cal3DS2& cal3ds2); + void update(size_t j, const gtsam::Cal3Bundler& cal3bundler); + void update(size_t j, const gtsam::EssentialMatrix& essential_matrix); + void update(size_t j, + const gtsam::PinholeCamera& simple_camera); + void update(size_t j, const gtsam::PinholeCamera& camera); + void update(size_t j, const gtsam::imuBias::ConstantBias& constant_bias); + void update(size_t j, const gtsam::NavState& nav_state); + void update(size_t j, Vector vector); + void update(size_t j, Matrix matrix); + void update(size_t j, double c); + + template , + gtsam::PinholeCamera, + gtsam::imuBias::ConstantBias, + gtsam::NavState, + Vector, + Matrix, + double}> + T at(size_t j); +}; + +#include +class Marginals { + Marginals(const gtsam::NonlinearFactorGraph& graph, + const gtsam::Values& solution); + Marginals(const gtsam::GaussianFactorGraph& gfgraph, + const gtsam::Values& solution); + Marginals(const gtsam::GaussianFactorGraph& gfgraph, + const gtsam::VectorValues& solutionvec); + + void print(string s = "Marginals: ", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + Matrix marginalCovariance(size_t variable) const; + Matrix marginalInformation(size_t variable) const; + gtsam::JointMarginal jointMarginalCovariance( + const gtsam::KeyVector& variables) const; + gtsam::JointMarginal jointMarginalInformation( + const gtsam::KeyVector& variables) const; +}; + +class JointMarginal { + Matrix at(size_t iVariable, size_t jVariable) const; + Matrix fullMatrix() const; + void print(string s = "", gtsam::KeyFormatter keyFormatter = + gtsam::DefaultKeyFormatter) const; +}; + +#include +virtual class LinearContainerFactor : gtsam::NonlinearFactor { + LinearContainerFactor(gtsam::GaussianFactor* factor, + const gtsam::Values& linearizationPoint); + LinearContainerFactor(gtsam::GaussianFactor* factor); + + gtsam::GaussianFactor* factor() const; + // const boost::optional& linearizationPoint() const; + + bool isJacobian() const; + gtsam::JacobianFactor* toJacobian() const; + gtsam::HessianFactor* toHessian() const; + + static gtsam::NonlinearFactorGraph ConvertLinearGraph( + const gtsam::GaussianFactorGraph& linear_graph, + const gtsam::Values& linearizationPoint); + + static gtsam::NonlinearFactorGraph ConvertLinearGraph( + const gtsam::GaussianFactorGraph& linear_graph); + + // enabling serialization functionality + void serializable() const; +}; // \class LinearContainerFactor + +// Summarization functionality +//#include +// +//// Uses partial QR approach by default +// gtsam::GaussianFactorGraph summarize( +// const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values, +// const gtsam::KeySet& saved_keys); +// +// gtsam::NonlinearFactorGraph summarizeAsNonlinearContainer( +// const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values, +// const gtsam::KeySet& saved_keys); + +//************************************************************************* +// Nonlinear optimizers +//************************************************************************* +#include +virtual class NonlinearOptimizerParams { + NonlinearOptimizerParams(); + void print(string s = "") const; + + int getMaxIterations() const; + double getRelativeErrorTol() const; + double getAbsoluteErrorTol() const; + double getErrorTol() const; + string getVerbosity() const; + + void setMaxIterations(int value); + void setRelativeErrorTol(double value); + void setAbsoluteErrorTol(double value); + void setErrorTol(double value); + void setVerbosity(string s); + + string getLinearSolverType() const; + void setLinearSolverType(string solver); + + void setIterativeParams(gtsam::IterativeOptimizationParameters* params); + void setOrdering(const gtsam::Ordering& ordering); + string getOrderingType() const; + void setOrderingType(string ordering); + + bool isMultifrontal() const; + bool isSequential() const; + bool isCholmod() const; + bool isIterative() const; +}; + +bool checkConvergence(double relativeErrorTreshold, + double absoluteErrorTreshold, double errorThreshold, + double currentError, double newError); +bool checkConvergence(const gtsam::NonlinearOptimizerParams& params, + double currentError, double newError); + +#include +virtual class GaussNewtonParams : gtsam::NonlinearOptimizerParams { + GaussNewtonParams(); +}; + +#include +virtual class LevenbergMarquardtParams : gtsam::NonlinearOptimizerParams { + LevenbergMarquardtParams(); + + bool getDiagonalDamping() const; + double getlambdaFactor() const; + double getlambdaInitial() const; + double getlambdaLowerBound() const; + double getlambdaUpperBound() const; + bool getUseFixedLambdaFactor(); + string getLogFile() const; + string getVerbosityLM() const; + + void setDiagonalDamping(bool flag); + void setlambdaFactor(double value); + void setlambdaInitial(double value); + void setlambdaLowerBound(double value); + void setlambdaUpperBound(double value); + void setUseFixedLambdaFactor(bool flag); + void setLogFile(string s); + void setVerbosityLM(string s); + + static gtsam::LevenbergMarquardtParams LegacyDefaults(); + static gtsam::LevenbergMarquardtParams CeresDefaults(); + + static gtsam::LevenbergMarquardtParams EnsureHasOrdering( + gtsam::LevenbergMarquardtParams params, + const gtsam::NonlinearFactorGraph& graph); + static gtsam::LevenbergMarquardtParams ReplaceOrdering( + gtsam::LevenbergMarquardtParams params, const gtsam::Ordering& ordering); +}; + +#include +virtual class DoglegParams : gtsam::NonlinearOptimizerParams { + DoglegParams(); + + double getDeltaInitial() const; + string getVerbosityDL() const; + + void setDeltaInitial(double deltaInitial) const; + void setVerbosityDL(string verbosityDL) const; +}; + +#include +virtual class NonlinearOptimizer { + gtsam::Values optimize(); + gtsam::Values optimizeSafely(); + double error() const; + int iterations() const; + gtsam::Values values() const; + gtsam::NonlinearFactorGraph graph() const; + gtsam::GaussianFactorGraph* iterate() const; +}; + +#include +virtual class GaussNewtonOptimizer : gtsam::NonlinearOptimizer { + GaussNewtonOptimizer(const gtsam::NonlinearFactorGraph& graph, + const gtsam::Values& initialValues); + GaussNewtonOptimizer(const gtsam::NonlinearFactorGraph& graph, + const gtsam::Values& initialValues, + const gtsam::GaussNewtonParams& params); +}; + +#include +virtual class DoglegOptimizer : gtsam::NonlinearOptimizer { + DoglegOptimizer(const gtsam::NonlinearFactorGraph& graph, + const gtsam::Values& initialValues); + DoglegOptimizer(const gtsam::NonlinearFactorGraph& graph, + const gtsam::Values& initialValues, + const gtsam::DoglegParams& params); + double getDelta() const; +}; + +#include +virtual class LevenbergMarquardtOptimizer : gtsam::NonlinearOptimizer { + LevenbergMarquardtOptimizer(const gtsam::NonlinearFactorGraph& graph, + const gtsam::Values& initialValues); + LevenbergMarquardtOptimizer(const gtsam::NonlinearFactorGraph& graph, + const gtsam::Values& initialValues, + const gtsam::LevenbergMarquardtParams& params); + double lambda() const; + void print(string s = "") const; +}; + +#include +class ISAM2GaussNewtonParams { + ISAM2GaussNewtonParams(); + + void print(string s = "") const; + + /** Getters and Setters for all properties */ + double getWildfireThreshold() const; + void setWildfireThreshold(double wildfireThreshold); +}; + +class ISAM2DoglegParams { + ISAM2DoglegParams(); + + void print(string s = "") const; + + /** Getters and Setters for all properties */ + double getWildfireThreshold() const; + void setWildfireThreshold(double wildfireThreshold); + double getInitialDelta() const; + void setInitialDelta(double initialDelta); + string getAdaptationMode() const; + void setAdaptationMode(string adaptationMode); + bool isVerbose() const; + void setVerbose(bool verbose); +}; + +class ISAM2ThresholdMapValue { + ISAM2ThresholdMapValue(char c, Vector thresholds); + ISAM2ThresholdMapValue(const gtsam::ISAM2ThresholdMapValue& other); +}; + +class ISAM2ThresholdMap { + ISAM2ThresholdMap(); + ISAM2ThresholdMap(const gtsam::ISAM2ThresholdMap& other); + + // Note: no print function + + // common STL methods + size_t size() const; + bool empty() const; + void clear(); + + // structure specific methods + void insert(const gtsam::ISAM2ThresholdMapValue& value) const; +}; + +class ISAM2Params { + ISAM2Params(); + + void print(string s = "") const; + + /** Getters and Setters for all properties */ + void setOptimizationParams( + const gtsam::ISAM2GaussNewtonParams& gauss_newton__params); + void setOptimizationParams(const gtsam::ISAM2DoglegParams& dogleg_params); + void setRelinearizeThreshold(double threshold); + void setRelinearizeThreshold(const gtsam::ISAM2ThresholdMap& threshold_map); + int getRelinearizeSkip() const; + void setRelinearizeSkip(int relinearizeSkip); + bool isEnableRelinearization() const; + void setEnableRelinearization(bool enableRelinearization); + bool isEvaluateNonlinearError() const; + void setEvaluateNonlinearError(bool evaluateNonlinearError); + string getFactorization() const; + void setFactorization(string factorization); + bool isCacheLinearizedFactors() const; + void setCacheLinearizedFactors(bool cacheLinearizedFactors); + bool isEnableDetailedResults() const; + void setEnableDetailedResults(bool enableDetailedResults); + bool isEnablePartialRelinearizationCheck() const; + void setEnablePartialRelinearizationCheck( + bool enablePartialRelinearizationCheck); +}; + +class ISAM2Clique { + // Constructors + ISAM2Clique(); + + // Standard Interface + Vector gradientContribution() const; + void print(string s = "", + gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter); +}; + +class ISAM2Result { + ISAM2Result(); + + void print(string s = "") const; + + /** Getters and Setters for all properties */ + size_t getVariablesRelinearized() const; + size_t getVariablesReeliminated() const; + size_t getCliques() const; + double getErrorBefore() const; + double getErrorAfter() const; +}; + +class ISAM2 { + ISAM2(); + ISAM2(const gtsam::ISAM2Params& params); + ISAM2(const gtsam::ISAM2& other); + + bool equals(const gtsam::ISAM2& other, double tol) const; + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + void printStats() const; + void saveGraph(string s) const; + + gtsam::ISAM2Result update(); + gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, + const gtsam::Values& newTheta); + gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, + const gtsam::Values& newTheta, + const gtsam::FactorIndices& removeFactorIndices); + gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, + const gtsam::Values& newTheta, + const gtsam::FactorIndices& removeFactorIndices, + const gtsam::KeyGroupMap& constrainedKeys); + gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, + const gtsam::Values& newTheta, + const gtsam::FactorIndices& removeFactorIndices, + gtsam::KeyGroupMap& constrainedKeys, + const gtsam::KeyList& noRelinKeys); + gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, + const gtsam::Values& newTheta, + const gtsam::FactorIndices& removeFactorIndices, + gtsam::KeyGroupMap& constrainedKeys, + const gtsam::KeyList& noRelinKeys, + const gtsam::KeyList& extraReelimKeys); + gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, + const gtsam::Values& newTheta, + const gtsam::FactorIndices& removeFactorIndices, + gtsam::KeyGroupMap& constrainedKeys, + const gtsam::KeyList& noRelinKeys, + const gtsam::KeyList& extraReelimKeys, + bool force_relinearize); + + gtsam::Values getLinearizationPoint() const; + gtsam::Values calculateEstimate() const; + template , + gtsam::PinholeCamera, Vector, Matrix}> + VALUE calculateEstimate(size_t key) const; + gtsam::Values calculateBestEstimate() const; + Matrix marginalCovariance(size_t key) const; + gtsam::VectorValues getDelta() const; + gtsam::NonlinearFactorGraph getFactorsUnsafe() const; + gtsam::VariableIndex getVariableIndex() const; + gtsam::ISAM2Params params() const; +}; + +#include +class NonlinearISAM { + NonlinearISAM(); + NonlinearISAM(int reorderInterval); + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + void printStats() const; + void saveGraph(string s) const; + gtsam::Values estimate() const; + Matrix marginalCovariance(size_t key) const; + int reorderInterval() const; + int reorderCounter() const; + void update(const gtsam::NonlinearFactorGraph& newFactors, + const gtsam::Values& initialValues); + void reorder_relinearize(); + + // These might be expensive as instead of a reference the wrapper will make a + // copy + gtsam::GaussianISAM bayesTree() const; + gtsam::Values getLinearizationPoint() const; + gtsam::NonlinearFactorGraph getFactorsUnsafe() const; +}; + +//************************************************************************* +// Nonlinear factor types +//************************************************************************* +#include +template , + gtsam::imuBias::ConstantBias, + gtsam::PinholeCamera}> +virtual class PriorFactor : gtsam::NoiseModelFactor { + PriorFactor(size_t key, const T& prior, + const gtsam::noiseModel::Base* noiseModel); + T prior() const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; +}; + +#include +template , + gtsam::imuBias::ConstantBias}> +virtual class NonlinearEquality : gtsam::NoiseModelFactor { + // Constructor - forces exact evaluation + NonlinearEquality(size_t j, const T& feasible); + // Constructor - allows inexact evaluation + NonlinearEquality(size_t j, const T& feasible, double error_gain); + + // enabling serialization functionality + void serialize() const; +}; + +} // namespace gtsam diff --git a/gtsam/sam/sam.i b/gtsam/sam/sam.i new file mode 100644 index 000000000..370e1c3ea --- /dev/null +++ b/gtsam/sam/sam.i @@ -0,0 +1,96 @@ +//************************************************************************* +// sam +//************************************************************************* + +namespace gtsam { + +#include +#include +#include +#include +#include + +// ##### + +#include +template +virtual class RangeFactor : gtsam::NoiseModelFactor { + RangeFactor(size_t key1, size_t key2, double measured, + const gtsam::noiseModel::Base* noiseModel); + + // enabling serialization functionality + void serialize() const; +}; + +typedef gtsam::RangeFactor RangeFactor2D; +typedef gtsam::RangeFactor RangeFactor3D; +typedef gtsam::RangeFactor RangeFactorPose2; +typedef gtsam::RangeFactor RangeFactorPose3; +typedef gtsam::RangeFactor + RangeFactorCalibratedCameraPoint; +typedef gtsam::RangeFactor, gtsam::Point3> + RangeFactorSimpleCameraPoint; +typedef gtsam::RangeFactor + RangeFactorCalibratedCamera; +typedef gtsam::RangeFactor, + gtsam::PinholeCamera> + RangeFactorSimpleCamera; + +#include +template +virtual class RangeFactorWithTransform : gtsam::NoiseModelFactor { + RangeFactorWithTransform(size_t key1, size_t key2, double measured, + const gtsam::noiseModel::Base* noiseModel, + const POSE& body_T_sensor); + + // enabling serialization functionality + void serialize() const; +}; + +typedef gtsam::RangeFactorWithTransform + RangeFactorWithTransform2D; +typedef gtsam::RangeFactorWithTransform + RangeFactorWithTransform3D; +typedef gtsam::RangeFactorWithTransform + RangeFactorWithTransformPose2; +typedef gtsam::RangeFactorWithTransform + RangeFactorWithTransformPose3; + +#include +template +virtual class BearingFactor : gtsam::NoiseModelFactor { + BearingFactor(size_t key1, size_t key2, const BEARING& measured, + const gtsam::noiseModel::Base* noiseModel); + + // enabling serialization functionality + void serialize() const; +}; + +typedef gtsam::BearingFactor + BearingFactor2D; +typedef gtsam::BearingFactor + BearingFactor3D; +typedef gtsam::BearingFactor + BearingFactorPose2; + +#include +template +virtual class BearingRangeFactor : gtsam::NoiseModelFactor { + BearingRangeFactor(size_t poseKey, size_t pointKey, + const BEARING& measuredBearing, const RANGE& measuredRange, + const gtsam::noiseModel::Base* noiseModel); + + gtsam::BearingRange measured() const; + + // enabling serialization functionality + void serialize() const; +}; + +typedef gtsam::BearingRangeFactor + BearingRangeFactor2D; +typedef gtsam::BearingRangeFactor + BearingRangeFactorPose2; + +} // namespace gtsam diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i new file mode 100644 index 000000000..c86de8d88 --- /dev/null +++ b/gtsam/sfm/sfm.i @@ -0,0 +1,209 @@ +//************************************************************************* +// sfm +//************************************************************************* + +namespace gtsam { + +// ##### + +#include + +virtual class ShonanFactor3 : gtsam::NoiseModelFactor { + ShonanFactor3(size_t key1, size_t key2, const gtsam::Rot3& R12, size_t p); + ShonanFactor3(size_t key1, size_t key2, const gtsam::Rot3& R12, size_t p, + gtsam::noiseModel::Base* model); + Vector evaluateError(const gtsam::SOn& Q1, const gtsam::SOn& Q2); +}; + +#include +template +class BinaryMeasurement { + BinaryMeasurement(size_t key1, size_t key2, const T& measured, + const gtsam::noiseModel::Base* model); + size_t key1() const; + size_t key2() const; + T measured() const; + gtsam::noiseModel::Base* noiseModel() const; +}; + +typedef gtsam::BinaryMeasurement BinaryMeasurementUnit3; +typedef gtsam::BinaryMeasurement BinaryMeasurementRot3; + +class BinaryMeasurementsUnit3 { + BinaryMeasurementsUnit3(); + size_t size() const; + gtsam::BinaryMeasurement at(size_t idx) const; + void push_back(const gtsam::BinaryMeasurement& measurement); +}; + +#include + +// TODO(frank): copy/pasta below until we have integer template paremeters in +// wrap! + +class ShonanAveragingParameters2 { + ShonanAveragingParameters2(const gtsam::LevenbergMarquardtParams& lm); + ShonanAveragingParameters2(const gtsam::LevenbergMarquardtParams& lm, + string method); + gtsam::LevenbergMarquardtParams getLMParams() const; + void setOptimalityThreshold(double value); + double getOptimalityThreshold() const; + void setAnchor(size_t index, const gtsam::Rot2& value); + pair getAnchor(); + void setAnchorWeight(double value); + double getAnchorWeight() const; + void setKarcherWeight(double value); + double getKarcherWeight() const; + void setGaugesWeight(double value); + double getGaugesWeight() const; + void setUseHuber(bool value); + bool getUseHuber() const; + void setCertifyOptimality(bool value); + bool getCertifyOptimality() const; +}; + +class ShonanAveragingParameters3 { + ShonanAveragingParameters3(const gtsam::LevenbergMarquardtParams& lm); + ShonanAveragingParameters3(const gtsam::LevenbergMarquardtParams& lm, + string method); + gtsam::LevenbergMarquardtParams getLMParams() const; + void setOptimalityThreshold(double value); + double getOptimalityThreshold() const; + void setAnchor(size_t index, const gtsam::Rot3& value); + pair getAnchor(); + void setAnchorWeight(double value); + double getAnchorWeight() const; + void setKarcherWeight(double value); + double getKarcherWeight() const; + void setGaugesWeight(double value); + double getGaugesWeight() const; + void setUseHuber(bool value); + bool getUseHuber() const; + void setCertifyOptimality(bool value); + bool getCertifyOptimality() const; +}; + +class ShonanAveraging2 { + ShonanAveraging2(string g2oFile); + ShonanAveraging2(string g2oFile, + const gtsam::ShonanAveragingParameters2& parameters); + + // Query properties + size_t nrUnknowns() const; + size_t nrMeasurements() const; + gtsam::Rot2 measured(size_t i); + gtsam::KeyVector keys(size_t i); + + // Matrix API (advanced use, debugging) + Matrix denseD() const; + Matrix denseQ() const; + Matrix denseL() const; + // Matrix computeLambda_(Matrix S) const; + Matrix computeLambda_(const gtsam::Values& values) const; + Matrix computeA_(const gtsam::Values& values) const; + double computeMinEigenValue(const gtsam::Values& values) const; + gtsam::Values initializeWithDescent(size_t p, const gtsam::Values& values, + const Vector& minEigenVector, + double minEigenValue) const; + + // Advanced API + gtsam::NonlinearFactorGraph buildGraphAt(size_t p) const; + gtsam::Values initializeRandomlyAt(size_t p) const; + double costAt(size_t p, const gtsam::Values& values) const; + pair computeMinEigenVector(const gtsam::Values& values) const; + bool checkOptimality(const gtsam::Values& values) const; + gtsam::LevenbergMarquardtOptimizer* createOptimizerAt( + size_t p, const gtsam::Values& initial); + // gtsam::Values tryOptimizingAt(size_t p) const; + gtsam::Values tryOptimizingAt(size_t p, const gtsam::Values& initial) const; + gtsam::Values projectFrom(size_t p, const gtsam::Values& values) const; + gtsam::Values roundSolution(const gtsam::Values& values) const; + + // Basic API + double cost(const gtsam::Values& values) const; + gtsam::Values initializeRandomly() const; + pair run(const gtsam::Values& initial, size_t min_p, + size_t max_p) const; +}; + +class ShonanAveraging3 { + ShonanAveraging3(string g2oFile); + ShonanAveraging3(string g2oFile, + const gtsam::ShonanAveragingParameters3& parameters); + + // TODO(frank): deprecate once we land pybind wrapper + ShonanAveraging3(const gtsam::BetweenFactorPose3s& factors); + ShonanAveraging3(const gtsam::BetweenFactorPose3s& factors, + const gtsam::ShonanAveragingParameters3& parameters); + + // Query properties + size_t nrUnknowns() const; + size_t nrMeasurements() const; + gtsam::Rot3 measured(size_t i); + gtsam::KeyVector keys(size_t i); + + // Matrix API (advanced use, debugging) + Matrix denseD() const; + Matrix denseQ() const; + Matrix denseL() const; + // Matrix computeLambda_(Matrix S) const; + Matrix computeLambda_(const gtsam::Values& values) const; + Matrix computeA_(const gtsam::Values& values) const; + double computeMinEigenValue(const gtsam::Values& values) const; + gtsam::Values initializeWithDescent(size_t p, const gtsam::Values& values, + const Vector& minEigenVector, + double minEigenValue) const; + + // Advanced API + gtsam::NonlinearFactorGraph buildGraphAt(size_t p) const; + gtsam::Values initializeRandomlyAt(size_t p) const; + double costAt(size_t p, const gtsam::Values& values) const; + pair computeMinEigenVector(const gtsam::Values& values) const; + bool checkOptimality(const gtsam::Values& values) const; + gtsam::LevenbergMarquardtOptimizer* createOptimizerAt( + size_t p, const gtsam::Values& initial); + // gtsam::Values tryOptimizingAt(size_t p) const; + gtsam::Values tryOptimizingAt(size_t p, const gtsam::Values& initial) const; + gtsam::Values projectFrom(size_t p, const gtsam::Values& values) const; + gtsam::Values roundSolution(const gtsam::Values& values) const; + + // Basic API + double cost(const gtsam::Values& values) const; + gtsam::Values initializeRandomly() const; + pair run(const gtsam::Values& initial, size_t min_p, + size_t max_p) const; +}; + +#include + +class KeyPairDoubleMap { + KeyPairDoubleMap(); + KeyPairDoubleMap(const gtsam::KeyPairDoubleMap& other); + + size_t size() const; + bool empty() const; + void clear(); + size_t at(const pair& keypair) const; +}; + +class MFAS { + MFAS(const gtsam::BinaryMeasurementsUnit3& relativeTranslations, + const gtsam::Unit3& projectionDirection); + + gtsam::KeyPairDoubleMap computeOutlierWeights() const; + gtsam::KeyVector computeOrdering() const; +}; + +#include +class TranslationRecovery { + TranslationRecovery( + const gtsam::BinaryMeasurementsUnit3& relativeTranslations, + const gtsam::LevenbergMarquardtParams& lmParams); + TranslationRecovery( + const gtsam::BinaryMeasurementsUnit3& + relativeTranslations); // default LevenbergMarquardtParams + gtsam::Values run(const double scale) const; + gtsam::Values run() const; // default scale = 1.0 +}; + +} // namespace gtsam diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i new file mode 100644 index 000000000..457e907b9 --- /dev/null +++ b/gtsam/slam/slam.i @@ -0,0 +1,325 @@ +//************************************************************************* +// slam +//************************************************************************* + +namespace gtsam { + +#include +#include +#include + +// ###### + +#include +template +virtual class BetweenFactor : gtsam::NoiseModelFactor { + BetweenFactor(size_t key1, size_t key2, const T& relativePose, + const gtsam::noiseModel::Base* noiseModel); + T measured() const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; +}; + +#include +template +virtual class GenericProjectionFactor : gtsam::NoiseModelFactor { + GenericProjectionFactor(const gtsam::Point2& measured, + const gtsam::noiseModel::Base* noiseModel, + size_t poseKey, size_t pointKey, + const CALIBRATION* k); + GenericProjectionFactor(const gtsam::Point2& measured, + const gtsam::noiseModel::Base* noiseModel, + size_t poseKey, size_t pointKey, const CALIBRATION* k, + const POSE& body_P_sensor); + + GenericProjectionFactor(const gtsam::Point2& measured, + const gtsam::noiseModel::Base* noiseModel, + size_t poseKey, size_t pointKey, const CALIBRATION* k, + bool throwCheirality, bool verboseCheirality); + GenericProjectionFactor(const gtsam::Point2& measured, + const gtsam::noiseModel::Base* noiseModel, + size_t poseKey, size_t pointKey, const CALIBRATION* k, + bool throwCheirality, bool verboseCheirality, + const POSE& body_P_sensor); + + gtsam::Point2 measured() const; + CALIBRATION* calibration() const; + bool verboseCheirality() const; + bool throwCheirality() const; + + // enabling serialization functionality + void serialize() const; +}; +typedef gtsam::GenericProjectionFactor + GenericProjectionFactorCal3_S2; +typedef gtsam::GenericProjectionFactor + GenericProjectionFactorCal3DS2; + +#include +template +virtual class GeneralSFMFactor : gtsam::NoiseModelFactor { + GeneralSFMFactor(const gtsam::Point2& measured, + const gtsam::noiseModel::Base* model, size_t cameraKey, + size_t landmarkKey); + gtsam::Point2 measured() const; +}; +typedef gtsam::GeneralSFMFactor, + gtsam::Point3> + GeneralSFMFactorCal3_S2; +typedef gtsam::GeneralSFMFactor, + gtsam::Point3> + GeneralSFMFactorCal3DS2; +typedef gtsam::GeneralSFMFactor, + gtsam::Point3> + GeneralSFMFactorCal3Bundler; + +template +virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor { + GeneralSFMFactor2(const gtsam::Point2& measured, + const gtsam::noiseModel::Base* model, size_t poseKey, + size_t landmarkKey, size_t calibKey); + gtsam::Point2 measured() const; + + // enabling serialization functionality + void serialize() const; +}; + +#include + +/// Linearization mode: what factor to linearize to +enum LinearizationMode { HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD }; + +/// How to manage degeneracy +enum DegeneracyMode { IGNORE_DEGENERACY, ZERO_ON_DEGENERACY, HANDLE_INFINITY }; + +class SmartProjectionParams { + SmartProjectionParams(); + void setLinearizationMode(gtsam::LinearizationMode linMode); + void setDegeneracyMode(gtsam::DegeneracyMode degMode); + void setRankTolerance(double rankTol); + void setEnableEPI(bool enableEPI); + void setLandmarkDistanceThreshold(bool landmarkDistanceThreshold); + void setDynamicOutlierRejectionThreshold(bool dynOutRejectionThreshold); +}; + +#include +template +virtual class SmartProjectionPoseFactor : gtsam::NonlinearFactor { + SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise, + const CALIBRATION* K); + SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise, + const CALIBRATION* K, + const gtsam::Pose3& body_P_sensor); + SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise, + const CALIBRATION* K, + const gtsam::SmartProjectionParams& params); + SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise, + const CALIBRATION* K, + const gtsam::Pose3& body_P_sensor, + const gtsam::SmartProjectionParams& params); + + void add(const gtsam::Point2& measured_i, size_t poseKey_i); + + // enabling serialization functionality + void serialize() const; +}; + +typedef gtsam::SmartProjectionPoseFactor + SmartProjectionPose3Factor; + +#include +template +virtual class GenericStereoFactor : gtsam::NoiseModelFactor { + GenericStereoFactor(const gtsam::StereoPoint2& measured, + const gtsam::noiseModel::Base* noiseModel, size_t poseKey, + size_t landmarkKey, const gtsam::Cal3_S2Stereo* K); + gtsam::StereoPoint2 measured() const; + gtsam::Cal3_S2Stereo* calibration() const; + + // enabling serialization functionality + void serialize() const; +}; +typedef gtsam::GenericStereoFactor + GenericStereoFactor3D; + +#include +template +virtual class PoseTranslationPrior : gtsam::NoiseModelFactor { + PoseTranslationPrior(size_t key, const POSE& pose_z, + const gtsam::noiseModel::Base* noiseModel); +}; + +typedef gtsam::PoseTranslationPrior PoseTranslationPrior2D; +typedef gtsam::PoseTranslationPrior PoseTranslationPrior3D; + +#include +template +virtual class PoseRotationPrior : gtsam::NoiseModelFactor { + PoseRotationPrior(size_t key, const POSE& pose_z, + const gtsam::noiseModel::Base* noiseModel); +}; + +typedef gtsam::PoseRotationPrior PoseRotationPrior2D; +typedef gtsam::PoseRotationPrior PoseRotationPrior3D; + +#include +virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor { + EssentialMatrixFactor(size_t key, const gtsam::Point2& pA, + const gtsam::Point2& pB, + const gtsam::noiseModel::Base* noiseModel); +}; + +#include + +class SfmTrack { + SfmTrack(); + SfmTrack(const gtsam::Point3& pt); + const Point3& point3() const; + + double r; + double g; + double b; + + std::vector> measurements; + + size_t number_measurements() const; + pair measurement(size_t idx) const; + pair siftIndex(size_t idx) const; + void add_measurement(size_t idx, const gtsam::Point2& m); + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; + + // enabling function to compare objects + bool equals(const gtsam::SfmTrack& expected, double tol) const; +}; + +class SfmData { + SfmData(); + size_t number_cameras() const; + size_t number_tracks() const; + gtsam::PinholeCamera camera(size_t idx) const; + gtsam::SfmTrack track(size_t idx) const; + void add_track(const gtsam::SfmTrack& t); + void add_camera(const gtsam::SfmCamera& cam); + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; + + // enabling function to compare objects + bool equals(const gtsam::SfmData& expected, double tol) const; +}; + +gtsam::SfmData readBal(string filename); +bool writeBAL(string filename, gtsam::SfmData& data); +gtsam::Values initialCamerasEstimate(const gtsam::SfmData& db); +gtsam::Values initialCamerasAndPointsEstimate(const gtsam::SfmData& db); + +pair load2D( + string filename, gtsam::noiseModel::Diagonal* model, int maxIndex, + bool addNoise, bool smart); +pair load2D( + string filename, gtsam::noiseModel::Diagonal* model, int maxIndex, + bool addNoise); +pair load2D( + string filename, gtsam::noiseModel::Diagonal* model, int maxIndex); +pair load2D( + string filename, gtsam::noiseModel::Diagonal* model); +pair load2D(string filename); +pair load2D_robust( + string filename, gtsam::noiseModel::Base* model, int maxIndex); +void save2D(const gtsam::NonlinearFactorGraph& graph, + const gtsam::Values& config, gtsam::noiseModel::Diagonal* model, + string filename); + +// std::vector::shared_ptr> +// Ignored by pybind -> will be List[BetweenFactorPose2] +class BetweenFactorPose2s { + BetweenFactorPose2s(); + size_t size() const; + gtsam::BetweenFactor* at(size_t i) const; + void push_back(const gtsam::BetweenFactor* factor); +}; +gtsam::BetweenFactorPose2s parse2DFactors(string filename); + +// std::vector::shared_ptr> +// Ignored by pybind -> will be List[BetweenFactorPose3] +class BetweenFactorPose3s { + BetweenFactorPose3s(); + size_t size() const; + gtsam::BetweenFactor* at(size_t i) const; + void push_back(const gtsam::BetweenFactor* factor); +}; +gtsam::BetweenFactorPose3s parse3DFactors(string filename); + +pair load3D(string filename); + +pair readG2o(string filename); +pair readG2o(string filename, + bool is3D); +void writeG2o(const gtsam::NonlinearFactorGraph& graph, + const gtsam::Values& estimate, string filename); + +#include +class InitializePose3 { + static gtsam::Values computeOrientationsChordal( + const gtsam::NonlinearFactorGraph& pose3Graph); + static gtsam::Values computeOrientationsGradient( + const gtsam::NonlinearFactorGraph& pose3Graph, + const gtsam::Values& givenGuess, size_t maxIter, const bool setRefFrame); + static gtsam::Values computeOrientationsGradient( + const gtsam::NonlinearFactorGraph& pose3Graph, + const gtsam::Values& givenGuess); + static gtsam::NonlinearFactorGraph buildPose3graph( + const gtsam::NonlinearFactorGraph& graph); + static gtsam::Values initializeOrientations( + const gtsam::NonlinearFactorGraph& graph); + static gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph, + const gtsam::Values& givenGuess, + bool useGradient); + static gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph); +}; + +#include +template +virtual class KarcherMeanFactor : gtsam::NonlinearFactor { + KarcherMeanFactor(const gtsam::KeyVector& keys); +}; + +#include +gtsam::noiseModel::Isotropic* ConvertNoiseModel(gtsam::noiseModel::Base* model, + size_t d); + +template +virtual class FrobeniusFactor : gtsam::NoiseModelFactor { + FrobeniusFactor(size_t key1, size_t key2); + FrobeniusFactor(size_t key1, size_t key2, gtsam::noiseModel::Base* model); + + Vector evaluateError(const T& R1, const T& R2); +}; + +template +virtual class FrobeniusBetweenFactor : gtsam::NoiseModelFactor { + FrobeniusBetweenFactor(size_t key1, size_t key2, const T& R12); + FrobeniusBetweenFactor(size_t key1, size_t key2, const T& R12, + gtsam::noiseModel::Base* model); + + Vector evaluateError(const T& R1, const T& R2); +}; + +} // namespace gtsam diff --git a/gtsam/symbolic/symbolic.i b/gtsam/symbolic/symbolic.i new file mode 100644 index 000000000..4e7cca68a --- /dev/null +++ b/gtsam/symbolic/symbolic.i @@ -0,0 +1,201 @@ +//************************************************************************* +// Symbolic +//************************************************************************* +namespace gtsam { + +#include +#include + +// ################### + +#include +virtual class SymbolicFactor { + // Standard Constructors and Named Constructors + SymbolicFactor(const gtsam::SymbolicFactor& f); + SymbolicFactor(); + SymbolicFactor(size_t j); + SymbolicFactor(size_t j1, size_t j2); + SymbolicFactor(size_t j1, size_t j2, size_t j3); + SymbolicFactor(size_t j1, size_t j2, size_t j3, size_t j4); + SymbolicFactor(size_t j1, size_t j2, size_t j3, size_t j4, size_t j5); + SymbolicFactor(size_t j1, size_t j2, size_t j3, size_t j4, size_t j5, + size_t j6); + static gtsam::SymbolicFactor FromKeys(const gtsam::KeyVector& js); + + // From Factor + size_t size() const; + void print(string s = "SymbolicFactor", + const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + bool equals(const gtsam::SymbolicFactor& other, double tol) const; + gtsam::KeyVector keys(); +}; + +#include +virtual class SymbolicFactorGraph { + SymbolicFactorGraph(); + SymbolicFactorGraph(const gtsam::SymbolicBayesNet& bayesNet); + SymbolicFactorGraph(const gtsam::SymbolicBayesTree& bayesTree); + + // From FactorGraph + void push_back(gtsam::SymbolicFactor* factor); + void print(string s = "SymbolicFactorGraph", + const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + bool equals(const gtsam::SymbolicFactorGraph& rhs, double tol) const; + size_t size() const; + bool exists(size_t idx) const; + + // Standard interface + gtsam::KeySet keys() const; + void push_back(const gtsam::SymbolicFactorGraph& graph); + void push_back(const gtsam::SymbolicBayesNet& bayesNet); + void push_back(const gtsam::SymbolicBayesTree& bayesTree); + + // Advanced Interface + void push_factor(size_t key); + void push_factor(size_t key1, size_t key2); + void push_factor(size_t key1, size_t key2, size_t key3); + void push_factor(size_t key1, size_t key2, size_t key3, size_t key4); + + gtsam::SymbolicBayesNet* eliminateSequential(); + gtsam::SymbolicBayesNet* eliminateSequential(const gtsam::Ordering& ordering); + gtsam::SymbolicBayesTree* eliminateMultifrontal(); + gtsam::SymbolicBayesTree* eliminateMultifrontal( + const gtsam::Ordering& ordering); + pair + eliminatePartialSequential(const gtsam::Ordering& ordering); + pair + eliminatePartialSequential(const gtsam::KeyVector& keys); + pair + eliminatePartialMultifrontal(const gtsam::Ordering& ordering); + pair + eliminatePartialMultifrontal(const gtsam::KeyVector& keys); + gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet( + const gtsam::Ordering& ordering); + gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet( + const gtsam::KeyVector& key_vector); + gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet( + const gtsam::Ordering& ordering, + const gtsam::Ordering& marginalizedVariableOrdering); + gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet( + const gtsam::KeyVector& key_vector, + const gtsam::Ordering& marginalizedVariableOrdering); + gtsam::SymbolicFactorGraph* marginal(const gtsam::KeyVector& key_vector); +}; + +#include +virtual class SymbolicConditional : gtsam::SymbolicFactor { + // Standard Constructors and Named Constructors + SymbolicConditional(); + SymbolicConditional(const gtsam::SymbolicConditional& other); + SymbolicConditional(size_t key); + SymbolicConditional(size_t key, size_t parent); + SymbolicConditional(size_t key, size_t parent1, size_t parent2); + SymbolicConditional(size_t key, size_t parent1, size_t parent2, + size_t parent3); + static gtsam::SymbolicConditional FromKeys(const gtsam::KeyVector& keys, + size_t nrFrontals); + + // Testable + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + bool equals(const gtsam::SymbolicConditional& other, double tol) const; + + // Standard interface + size_t nrFrontals() const; + size_t nrParents() const; +}; + +#include +class SymbolicBayesNet { + SymbolicBayesNet(); + SymbolicBayesNet(const gtsam::SymbolicBayesNet& other); + // Testable + void print(string s = "SymbolicBayesNet", + const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + bool equals(const gtsam::SymbolicBayesNet& other, double tol) const; + + // Standard interface + size_t size() const; + void saveGraph(string s) const; + gtsam::SymbolicConditional* at(size_t idx) const; + gtsam::SymbolicConditional* front() const; + gtsam::SymbolicConditional* back() const; + void push_back(gtsam::SymbolicConditional* conditional); + void push_back(const gtsam::SymbolicBayesNet& bayesNet); +}; + +#include +class SymbolicBayesTree { + // Constructors + SymbolicBayesTree(); + SymbolicBayesTree(const gtsam::SymbolicBayesTree& other); + + // Testable + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter); + bool equals(const gtsam::SymbolicBayesTree& other, double tol) const; + + // Standard Interface + // size_t findParentClique(const gtsam::IndexVector& parents) const; + size_t size(); + void saveGraph(string s) const; + void clear(); + void deleteCachedShortcuts(); + size_t numCachedSeparatorMarginals() const; + + gtsam::SymbolicConditional* marginalFactor(size_t key) const; + gtsam::SymbolicFactorGraph* joint(size_t key1, size_t key2) const; + gtsam::SymbolicBayesNet* jointBayesNet(size_t key1, size_t key2) const; +}; + +class SymbolicBayesTreeClique { + SymbolicBayesTreeClique(); + // SymbolicBayesTreeClique(gtsam::sharedConditional* conditional); + + bool equals(const gtsam::SymbolicBayesTreeClique& other, double tol) const; + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + size_t numCachedSeparatorMarginals() const; + // gtsam::sharedConditional* conditional() const; + bool isRoot() const; + size_t treeSize() const; + gtsam::SymbolicBayesTreeClique* parent() const; + + // // TODO: need wrapped versions graphs, BayesNet + // BayesNet shortcut(derived_ptr root, Eliminate function) + // const; FactorGraph marginal(derived_ptr root, Eliminate + // function) const; FactorGraph joint(derived_ptr C2, derived_ptr + // root, Eliminate function) const; + // + void deleteCachedShortcuts(); +}; + +#include +class VariableIndex { + // Standard Constructors and Named Constructors + VariableIndex(); + // TODO: Templetize constructor when wrap supports it + // template + // VariableIndex(const T& factorGraph, size_t nVariables); + // VariableIndex(const T& factorGraph); + VariableIndex(const gtsam::SymbolicFactorGraph& sfg); + VariableIndex(const gtsam::GaussianFactorGraph& gfg); + VariableIndex(const gtsam::NonlinearFactorGraph& fg); + VariableIndex(const gtsam::VariableIndex& other); + + // Testable + bool equals(const gtsam::VariableIndex& other, double tol) const; + void print(string s = "VariableIndex: ", + const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + + // Standard interface + size_t size() const; + size_t nFactors() const; + size_t nEntries() const; +}; + +} // namespace gtsam From f33e6a801f5cb9a316dc29b0c96e6ac3f605d16c Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 11 Jul 2021 08:10:05 -0700 Subject: [PATCH 043/248] break up preamble and specializations so there are no duplicate includes --- python/gtsam/preamble.h | 30 ------------------- python/gtsam/preamble/base.h | 16 +++++++++++ python/gtsam/preamble/geometry.h | 21 ++++++++++++++ python/gtsam/preamble/gtsam.h | 17 +++++++++++ python/gtsam/preamble/linear.h | 12 ++++++++ python/gtsam/preamble/navigation.h | 18 ++++++++++++ python/gtsam/preamble/nonlinear.h | 12 ++++++++ python/gtsam/preamble/sam.h | 12 ++++++++ python/gtsam/preamble/sfm.h | 12 ++++++++ python/gtsam/preamble/slam.h | 17 +++++++++++ python/gtsam/preamble/symbolic.h | 12 ++++++++ python/gtsam/specializations.h | 35 ----------------------- python/gtsam/specializations/base.h | 17 +++++++++++ python/gtsam/specializations/geometry.h | 23 +++++++++++++++ python/gtsam/specializations/gtsam.h | 20 +++++++++++++ python/gtsam/specializations/linear.h | 12 ++++++++ python/gtsam/specializations/navigation.h | 12 ++++++++ python/gtsam/specializations/nonlinear.h | 12 ++++++++ python/gtsam/specializations/sam.h | 12 ++++++++ python/gtsam/specializations/sfm.h | 16 +++++++++++ python/gtsam/specializations/slam.h | 19 ++++++++++++ python/gtsam/specializations/symbolic.h | 12 ++++++++ 22 files changed, 304 insertions(+), 65 deletions(-) delete mode 100644 python/gtsam/preamble.h create mode 100644 python/gtsam/preamble/base.h create mode 100644 python/gtsam/preamble/geometry.h create mode 100644 python/gtsam/preamble/gtsam.h create mode 100644 python/gtsam/preamble/linear.h create mode 100644 python/gtsam/preamble/navigation.h create mode 100644 python/gtsam/preamble/nonlinear.h create mode 100644 python/gtsam/preamble/sam.h create mode 100644 python/gtsam/preamble/sfm.h create mode 100644 python/gtsam/preamble/slam.h create mode 100644 python/gtsam/preamble/symbolic.h delete mode 100644 python/gtsam/specializations.h create mode 100644 python/gtsam/specializations/base.h create mode 100644 python/gtsam/specializations/geometry.h create mode 100644 python/gtsam/specializations/gtsam.h create mode 100644 python/gtsam/specializations/linear.h create mode 100644 python/gtsam/specializations/navigation.h create mode 100644 python/gtsam/specializations/nonlinear.h create mode 100644 python/gtsam/specializations/sam.h create mode 100644 python/gtsam/specializations/sfm.h create mode 100644 python/gtsam/specializations/slam.h create mode 100644 python/gtsam/specializations/symbolic.h diff --git a/python/gtsam/preamble.h b/python/gtsam/preamble.h deleted file mode 100644 index 7294a6ac8..000000000 --- a/python/gtsam/preamble.h +++ /dev/null @@ -1,30 +0,0 @@ -/* Please refer to: https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html - * These are required to save one copy operation on Python calls. - * - * NOTES - * ================= - * - * `PYBIND11_MAKE_OPAQUE` will mark the type as "opaque" for the pybind11 automatic STL binding, - * such that the raw objects can be accessed in Python. Without this they will be automatically - * converted to a Python object, and all mutations on Python side will not be reflected on C++. - */ -#ifdef GTSAM_ALLOCATOR_TBB -PYBIND11_MAKE_OPAQUE(std::vector>); -#else -PYBIND11_MAKE_OPAQUE(std::vector); -#endif -PYBIND11_MAKE_OPAQUE(std::vector >); -PYBIND11_MAKE_OPAQUE(gtsam::Point3Pairs); -PYBIND11_MAKE_OPAQUE(gtsam::Pose3Pairs); -PYBIND11_MAKE_OPAQUE(std::vector); -PYBIND11_MAKE_OPAQUE(std::vector > >); -PYBIND11_MAKE_OPAQUE(std::vector > >); -PYBIND11_MAKE_OPAQUE(std::vector); -PYBIND11_MAKE_OPAQUE(gtsam::CameraSet >); -PYBIND11_MAKE_OPAQUE(gtsam::CameraSet >); -PYBIND11_MAKE_OPAQUE(std::vector); // JacobianVector - -// TODO(fan): This is to fix the Argument-dependent lookup (ADL) of std::pair. We should find a way to NOT do this. -namespace std { - using gtsam::operator<<; -} diff --git a/python/gtsam/preamble/base.h b/python/gtsam/preamble/base.h new file mode 100644 index 000000000..626b47ae4 --- /dev/null +++ b/python/gtsam/preamble/base.h @@ -0,0 +1,16 @@ +/* Please refer to: + * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `PYBIND11_MAKE_OPAQUE` will mark the type as "opaque" for the pybind11 + * automatic STL binding, such that the raw objects can be accessed in Python. + * Without this they will be automatically converted to a Python object, and all + * mutations on Python side will not be reflected on C++. + */ + +PYBIND11_MAKE_OPAQUE(std::vector); + +PYBIND11_MAKE_OPAQUE(std::vector); // JacobianVector diff --git a/python/gtsam/preamble/geometry.h b/python/gtsam/preamble/geometry.h new file mode 100644 index 000000000..498c7dc58 --- /dev/null +++ b/python/gtsam/preamble/geometry.h @@ -0,0 +1,21 @@ +/* Please refer to: + * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `PYBIND11_MAKE_OPAQUE` will mark the type as "opaque" for the pybind11 + * automatic STL binding, such that the raw objects can be accessed in Python. + * Without this they will be automatically converted to a Python object, and all + * mutations on Python side will not be reflected on C++. + */ + +PYBIND11_MAKE_OPAQUE( + std::vector>); +PYBIND11_MAKE_OPAQUE(gtsam::Point3Pairs); +PYBIND11_MAKE_OPAQUE(gtsam::Pose3Pairs); +PYBIND11_MAKE_OPAQUE(std::vector); +PYBIND11_MAKE_OPAQUE( + gtsam::CameraSet>); +PYBIND11_MAKE_OPAQUE(gtsam::CameraSet>); diff --git a/python/gtsam/preamble/gtsam.h b/python/gtsam/preamble/gtsam.h new file mode 100644 index 000000000..ec39c410a --- /dev/null +++ b/python/gtsam/preamble/gtsam.h @@ -0,0 +1,17 @@ +/* Please refer to: + * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `PYBIND11_MAKE_OPAQUE` will mark the type as "opaque" for the pybind11 + * automatic STL binding, such that the raw objects can be accessed in Python. + * Without this they will be automatically converted to a Python object, and all + * mutations on Python side will not be reflected on C++. + */ +#ifdef GTSAM_ALLOCATOR_TBB +PYBIND11_MAKE_OPAQUE(std::vector>); +#else +PYBIND11_MAKE_OPAQUE(std::vector); +#endif diff --git a/python/gtsam/preamble/linear.h b/python/gtsam/preamble/linear.h new file mode 100644 index 000000000..a34e73058 --- /dev/null +++ b/python/gtsam/preamble/linear.h @@ -0,0 +1,12 @@ +/* Please refer to: + * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `PYBIND11_MAKE_OPAQUE` will mark the type as "opaque" for the pybind11 + * automatic STL binding, such that the raw objects can be accessed in Python. + * Without this they will be automatically converted to a Python object, and all + * mutations on Python side will not be reflected on C++. + */ \ No newline at end of file diff --git a/python/gtsam/preamble/navigation.h b/python/gtsam/preamble/navigation.h new file mode 100644 index 000000000..b647ef029 --- /dev/null +++ b/python/gtsam/preamble/navigation.h @@ -0,0 +1,18 @@ +/* Please refer to: + * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `PYBIND11_MAKE_OPAQUE` will mark the type as "opaque" for the pybind11 + * automatic STL binding, such that the raw objects can be accessed in Python. + * Without this they will be automatically converted to a Python object, and all + * mutations on Python side will not be reflected on C++. + */ + +// TODO(fan): This is to fix the Argument-dependent lookup (ADL) of std::pair. +// We should find a way to NOT do this. +namespace std { +using gtsam::operator<<; +} diff --git a/python/gtsam/preamble/nonlinear.h b/python/gtsam/preamble/nonlinear.h new file mode 100644 index 000000000..a34e73058 --- /dev/null +++ b/python/gtsam/preamble/nonlinear.h @@ -0,0 +1,12 @@ +/* Please refer to: + * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `PYBIND11_MAKE_OPAQUE` will mark the type as "opaque" for the pybind11 + * automatic STL binding, such that the raw objects can be accessed in Python. + * Without this they will be automatically converted to a Python object, and all + * mutations on Python side will not be reflected on C++. + */ \ No newline at end of file diff --git a/python/gtsam/preamble/sam.h b/python/gtsam/preamble/sam.h new file mode 100644 index 000000000..a34e73058 --- /dev/null +++ b/python/gtsam/preamble/sam.h @@ -0,0 +1,12 @@ +/* Please refer to: + * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `PYBIND11_MAKE_OPAQUE` will mark the type as "opaque" for the pybind11 + * automatic STL binding, such that the raw objects can be accessed in Python. + * Without this they will be automatically converted to a Python object, and all + * mutations on Python side will not be reflected on C++. + */ \ No newline at end of file diff --git a/python/gtsam/preamble/sfm.h b/python/gtsam/preamble/sfm.h new file mode 100644 index 000000000..a34e73058 --- /dev/null +++ b/python/gtsam/preamble/sfm.h @@ -0,0 +1,12 @@ +/* Please refer to: + * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `PYBIND11_MAKE_OPAQUE` will mark the type as "opaque" for the pybind11 + * automatic STL binding, such that the raw objects can be accessed in Python. + * Without this they will be automatically converted to a Python object, and all + * mutations on Python side will not be reflected on C++. + */ \ No newline at end of file diff --git a/python/gtsam/preamble/slam.h b/python/gtsam/preamble/slam.h new file mode 100644 index 000000000..34dbb4b7a --- /dev/null +++ b/python/gtsam/preamble/slam.h @@ -0,0 +1,17 @@ +/* Please refer to: + * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `PYBIND11_MAKE_OPAQUE` will mark the type as "opaque" for the pybind11 + * automatic STL binding, such that the raw objects can be accessed in Python. + * Without this they will be automatically converted to a Python object, and all + * mutations on Python side will not be reflected on C++. + */ + +PYBIND11_MAKE_OPAQUE( + std::vector > >); +PYBIND11_MAKE_OPAQUE( + std::vector > >); diff --git a/python/gtsam/preamble/symbolic.h b/python/gtsam/preamble/symbolic.h new file mode 100644 index 000000000..a34e73058 --- /dev/null +++ b/python/gtsam/preamble/symbolic.h @@ -0,0 +1,12 @@ +/* Please refer to: + * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `PYBIND11_MAKE_OPAQUE` will mark the type as "opaque" for the pybind11 + * automatic STL binding, such that the raw objects can be accessed in Python. + * Without this they will be automatically converted to a Python object, and all + * mutations on Python side will not be reflected on C++. + */ \ No newline at end of file diff --git a/python/gtsam/specializations.h b/python/gtsam/specializations.h deleted file mode 100644 index be8eb8a6c..000000000 --- a/python/gtsam/specializations.h +++ /dev/null @@ -1,35 +0,0 @@ -/* Please refer to: https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html - * These are required to save one copy operation on Python calls. - * - * NOTES - * ================= - * - * `PYBIND11_MAKE_OPAQUE` will mark the type as "opaque" for the pybind11 automatic STL binding, - * such that the raw objects can be accessed in Python. Without this they will be automatically - * converted to a Python object, and all mutations on Python side will not be reflected on C++. - * - * `py::bind_vector` and similar machinery gives the std container a Python-like interface, but - * without the `` copying mechanism. Combined with `PYBIND11_MAKE_OPAQUE` this - * allows the types to be modified with Python, and saves one copy operation. - */ -#ifdef GTSAM_ALLOCATOR_TBB -py::bind_vector > >(m_, "KeyVector"); -py::implicitly_convertible > >(); -#else -py::bind_vector >(m_, "KeyVector"); -py::implicitly_convertible >(); -#endif - -py::bind_vector > >(m_, "Point2Vector"); -py::bind_vector >(m_, "Point3Pairs"); -py::bind_vector >(m_, "Pose3Pairs"); -py::bind_vector >(m_, "Pose3Vector"); -py::bind_vector > > >(m_, "BetweenFactorPose3s"); -py::bind_vector > > >(m_, "BetweenFactorPose2s"); -py::bind_vector > >(m_, "BinaryMeasurementsUnit3"); -py::bind_map(m_, "IndexPairSetMap"); -py::bind_vector(m_, "IndexPairVector"); -py::bind_map(m_, "KeyPairDoubleMap"); -py::bind_vector > >(m_, "CameraSetCal3_S2"); -py::bind_vector > >(m_, "CameraSetCal3Bundler"); -py::bind_vector >(m_, "JacobianVector"); diff --git a/python/gtsam/specializations/base.h b/python/gtsam/specializations/base.h new file mode 100644 index 000000000..9eefdeca8 --- /dev/null +++ b/python/gtsam/specializations/base.h @@ -0,0 +1,17 @@ +/* Please refer to: + * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `py::bind_vector` and similar machinery gives the std container a Python-like + * interface, but without the `` copying mechanism. Combined + * with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python, + * and saves one copy operation. + */ + +py::bind_map(m_, "IndexPairSetMap"); +py::bind_vector(m_, "IndexPairVector"); + +py::bind_vector >(m_, "JacobianVector"); diff --git a/python/gtsam/specializations/geometry.h b/python/gtsam/specializations/geometry.h new file mode 100644 index 000000000..3d22581d9 --- /dev/null +++ b/python/gtsam/specializations/geometry.h @@ -0,0 +1,23 @@ +/* Please refer to: + * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `py::bind_vector` and similar machinery gives the std container a Python-like + * interface, but without the `` copying mechanism. Combined + * with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python, + * and saves one copy operation. + */ + +py::bind_vector< + std::vector>>( + m_, "Point2Vector"); +py::bind_vector>(m_, "Point3Pairs"); +py::bind_vector>(m_, "Pose3Pairs"); +py::bind_vector>(m_, "Pose3Vector"); +py::bind_vector>>( + m_, "CameraSetCal3_S2"); +py::bind_vector>>( + m_, "CameraSetCal3Bundler"); diff --git a/python/gtsam/specializations/gtsam.h b/python/gtsam/specializations/gtsam.h new file mode 100644 index 000000000..490d71902 --- /dev/null +++ b/python/gtsam/specializations/gtsam.h @@ -0,0 +1,20 @@ +/* Please refer to: + * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `py::bind_vector` and similar machinery gives the std container a Python-like + * interface, but without the `` copying mechanism. Combined + * with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python, + * and saves one copy operation. + */ + +#ifdef GTSAM_ALLOCATOR_TBB +py::bind_vector > >(m_, "KeyVector"); +py::implicitly_convertible > >(); +#else +py::bind_vector >(m_, "KeyVector"); +py::implicitly_convertible >(); +#endif diff --git a/python/gtsam/specializations/linear.h b/python/gtsam/specializations/linear.h new file mode 100644 index 000000000..d46ccdc66 --- /dev/null +++ b/python/gtsam/specializations/linear.h @@ -0,0 +1,12 @@ +/* Please refer to: + * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `py::bind_vector` and similar machinery gives the std container a Python-like + * interface, but without the `` copying mechanism. Combined + * with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python, + * and saves one copy operation. + */ \ No newline at end of file diff --git a/python/gtsam/specializations/navigation.h b/python/gtsam/specializations/navigation.h new file mode 100644 index 000000000..d46ccdc66 --- /dev/null +++ b/python/gtsam/specializations/navigation.h @@ -0,0 +1,12 @@ +/* Please refer to: + * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `py::bind_vector` and similar machinery gives the std container a Python-like + * interface, but without the `` copying mechanism. Combined + * with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python, + * and saves one copy operation. + */ \ No newline at end of file diff --git a/python/gtsam/specializations/nonlinear.h b/python/gtsam/specializations/nonlinear.h new file mode 100644 index 000000000..d46ccdc66 --- /dev/null +++ b/python/gtsam/specializations/nonlinear.h @@ -0,0 +1,12 @@ +/* Please refer to: + * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `py::bind_vector` and similar machinery gives the std container a Python-like + * interface, but without the `` copying mechanism. Combined + * with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python, + * and saves one copy operation. + */ \ No newline at end of file diff --git a/python/gtsam/specializations/sam.h b/python/gtsam/specializations/sam.h new file mode 100644 index 000000000..d46ccdc66 --- /dev/null +++ b/python/gtsam/specializations/sam.h @@ -0,0 +1,12 @@ +/* Please refer to: + * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `py::bind_vector` and similar machinery gives the std container a Python-like + * interface, but without the `` copying mechanism. Combined + * with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python, + * and saves one copy operation. + */ \ No newline at end of file diff --git a/python/gtsam/specializations/sfm.h b/python/gtsam/specializations/sfm.h new file mode 100644 index 000000000..6de15217f --- /dev/null +++ b/python/gtsam/specializations/sfm.h @@ -0,0 +1,16 @@ +/* Please refer to: + * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `py::bind_vector` and similar machinery gives the std container a Python-like + * interface, but without the `` copying mechanism. Combined + * with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python, + * and saves one copy operation. + */ + +py::bind_vector > >( + m_, "BinaryMeasurementsUnit3"); +py::bind_map(m_, "KeyPairDoubleMap"); diff --git a/python/gtsam/specializations/slam.h b/python/gtsam/specializations/slam.h new file mode 100644 index 000000000..198485a72 --- /dev/null +++ b/python/gtsam/specializations/slam.h @@ -0,0 +1,19 @@ +/* Please refer to: + * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `py::bind_vector` and similar machinery gives the std container a Python-like + * interface, but without the `` copying mechanism. Combined + * with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python, + * and saves one copy operation. + */ + +py::bind_vector< + std::vector > > >( + m_, "BetweenFactorPose3s"); +py::bind_vector< + std::vector > > >( + m_, "BetweenFactorPose2s"); diff --git a/python/gtsam/specializations/symbolic.h b/python/gtsam/specializations/symbolic.h new file mode 100644 index 000000000..d46ccdc66 --- /dev/null +++ b/python/gtsam/specializations/symbolic.h @@ -0,0 +1,12 @@ +/* Please refer to: + * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `py::bind_vector` and similar machinery gives the std container a Python-like + * interface, but without the `` copying mechanism. Combined + * with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python, + * and saves one copy operation. + */ \ No newline at end of file From 54063934fab2d041cd54b60c2c0ba7ac624290b6 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 11 Jul 2021 08:10:06 -0700 Subject: [PATCH 044/248] update template for wrapper --- python/gtsam/gtsam.tpl | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/python/gtsam/gtsam.tpl b/python/gtsam/gtsam.tpl index b800f7c35..93e7ffbaf 100644 --- a/python/gtsam/gtsam.tpl +++ b/python/gtsam/gtsam.tpl @@ -18,7 +18,7 @@ #include #include "gtsam/config.h" #include "gtsam/base/serialization.h" -#include "gtsam/nonlinear/utilities.h" // for RedirectCout. +#include "gtsam/base/utilities.h" // for RedirectCout. // These are the included headers listed in `gtsam.i` {includes} @@ -32,20 +32,24 @@ // Preamble for STL classes // TODO(fan): make this automatic -#include "python/gtsam/preamble.h" +#include "python/gtsam/preamble/{module_name}.h" using namespace std; namespace py = pybind11; -PYBIND11_MODULE({module_name}, m_) {{ +{submodules} + +{module_def} {{ m_.doc() = "pybind11 wrapper of {module_name}"; +{submodules_init} + {wrapped_namespace} // Specializations for STL classes // TODO(fan): make this automatic -#include "python/gtsam/specializations.h" +#include "python/gtsam/specializations/{module_name}.h" }} From 86c47d52d571d427323288197a5b8a07209cbb7a Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 11 Jul 2021 08:10:08 -0700 Subject: [PATCH 045/248] move RedirectCout to base/utilities.h --- gtsam/base/utilities.h | 29 +++++++++++++++++++++++++++++ gtsam/nonlinear/utilities.h | 25 ------------------------- 2 files changed, 29 insertions(+), 25 deletions(-) create mode 100644 gtsam/base/utilities.h diff --git a/gtsam/base/utilities.h b/gtsam/base/utilities.h new file mode 100644 index 000000000..8eb5617a8 --- /dev/null +++ b/gtsam/base/utilities.h @@ -0,0 +1,29 @@ +#pragma once + +namespace gtsam { +/** + * For Python __str__(). + * Redirect std cout to a string stream so we can return a string representation + * of an object when it prints to cout. + * https://stackoverflow.com/questions/5419356/redirect-stdout-stderr-to-a-string + */ +struct RedirectCout { + /// constructor -- redirect stdout buffer to a stringstream buffer + RedirectCout() : ssBuffer_(), coutBuffer_(std::cout.rdbuf(ssBuffer_.rdbuf())) {} + + /// return the string + std::string str() const { + return ssBuffer_.str(); + } + + /// destructor -- redirect stdout buffer to its original buffer + ~RedirectCout() { + std::cout.rdbuf(coutBuffer_); + } + +private: + std::stringstream ssBuffer_; + std::streambuf* coutBuffer_; +}; + +} diff --git a/gtsam/nonlinear/utilities.h b/gtsam/nonlinear/utilities.h index ffc279872..4e79e20ff 100644 --- a/gtsam/nonlinear/utilities.h +++ b/gtsam/nonlinear/utilities.h @@ -260,30 +260,5 @@ Values localToWorld(const Values& local, const Pose2& base, } // namespace utilities -/** - * For Python __str__(). - * Redirect std cout to a string stream so we can return a string representation - * of an object when it prints to cout. - * https://stackoverflow.com/questions/5419356/redirect-stdout-stderr-to-a-string - */ -struct RedirectCout { - /// constructor -- redirect stdout buffer to a stringstream buffer - RedirectCout() : ssBuffer_(), coutBuffer_(std::cout.rdbuf(ssBuffer_.rdbuf())) {} - - /// return the string - std::string str() const { - return ssBuffer_.str(); - } - - /// destructor -- redirect stdout buffer to its original buffer - ~RedirectCout() { - std::cout.rdbuf(coutBuffer_); - } - -private: - std::stringstream ssBuffer_; - std::streambuf* coutBuffer_; -}; - } From e8e3094556f2c28e55b03d3438c30fa36c37b8c4 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 11 Jul 2021 08:10:09 -0700 Subject: [PATCH 046/248] update CMake --- python/CMakeLists.txt | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 5f51368e6..676479bd5 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -50,8 +50,22 @@ set(ignore gtsam::BinaryMeasurementsUnit3 gtsam::KeyPairDoubleMap) +set(interface_headers + ${PROJECT_SOURCE_DIR}/gtsam/gtsam.i + ${PROJECT_SOURCE_DIR}/gtsam/base/base.i + ${PROJECT_SOURCE_DIR}/gtsam/geometry/geometry.i + ${PROJECT_SOURCE_DIR}/gtsam/linear/linear.i + ${PROJECT_SOURCE_DIR}/gtsam/nonlinear/nonlinear.i + ${PROJECT_SOURCE_DIR}/gtsam/symbolic/symbolic.i + ${PROJECT_SOURCE_DIR}/gtsam/sam/sam.i + ${PROJECT_SOURCE_DIR}/gtsam/slam/slam.i + ${PROJECT_SOURCE_DIR}/gtsam/sfm/sfm.i + ${PROJECT_SOURCE_DIR}/gtsam/navigation/navigation.i +) + + pybind_wrap(gtsam_py # target - ${PROJECT_SOURCE_DIR}/gtsam/gtsam.i # interface_header + "${interface_headers}" # interface_headers "gtsam.cpp" # generated_cpp "gtsam" # module_name "gtsam" # top_namespace From fe95b8b9709db56d67790a294270be8a0f27e001 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 11 Jul 2021 08:10:09 -0700 Subject: [PATCH 047/248] wrapper updates --- wrap/cmake/PybindWrap.cmake | 157 +++++++++++----------- wrap/gtwrap/pybind_wrapper.py | 75 +++++++++-- wrap/scripts/pybind_wrap.py | 11 +- wrap/templates/pybind_wrapper.tpl.example | 8 +- 4 files changed, 157 insertions(+), 94 deletions(-) diff --git a/wrap/cmake/PybindWrap.cmake b/wrap/cmake/PybindWrap.cmake index 331dfff8c..f341c2f98 100644 --- a/wrap/cmake/PybindWrap.cmake +++ b/wrap/cmake/PybindWrap.cmake @@ -13,15 +13,14 @@ gtwrap_get_python_version(${WRAP_PYTHON_VERSION}) message(STATUS "Setting Python version for wrapper") set(PYBIND11_PYTHON_VERSION ${WRAP_PYTHON_VERSION}) -# User-friendly Pybind11 wrapping and installing function. -# Builds a Pybind11 module from the provided interface_header. -# For example, for the interface header gtsam.h, this will -# build the wrap module 'gtsam_py.cc'. +# User-friendly Pybind11 wrapping and installing function. Builds a Pybind11 +# module from the provided interface_headers. For example, for the interface +# header gtsam.h, this will build the wrap module 'gtsam_py.cc'. # # Arguments: # ~~~ # target: The Make target -# interface_header: The relative path to the wrapper interface definition file. +# interface_headers: List of paths to the wrapper interface definition files. The top level interface file should be first. # generated_cpp: The name of the cpp file which is generated from the tpl file. # module_name: The name of the Python module to use. # top_namespace: The C++ namespace under which the code to be wrapped exists. @@ -31,16 +30,17 @@ set(PYBIND11_PYTHON_VERSION ${WRAP_PYTHON_VERSION}) # libs: Libraries to link with. # dependencies: Dependencies which need to be built before the wrapper. # use_boost (optional): Flag indicating whether to include Boost. -function(pybind_wrap - target - interface_header - generated_cpp - module_name - top_namespace - ignore_classes - module_template - libs - dependencies) +function( + pybind_wrap + target + interface_headers + generated_cpp + module_name + top_namespace + ignore_classes + module_template + libs + dependencies) set(ExtraMacroArgs ${ARGN}) list(GET ExtraMacroArgs 0 USE_BOOST) if(USE_BOOST) @@ -49,57 +49,62 @@ function(pybind_wrap set(_WRAP_BOOST_ARG "") endif(USE_BOOST) - if (UNIX) + if(UNIX) set(GTWRAP_PATH_SEPARATOR ":") else() set(GTWRAP_PATH_SEPARATOR ";") endif() - add_custom_command(OUTPUT ${generated_cpp} - COMMAND ${CMAKE_COMMAND} -E env "PYTHONPATH=${GTWRAP_PACKAGE_DIR}${GTWRAP_PATH_SEPARATOR}$ENV{PYTHONPATH}" - ${PYTHON_EXECUTABLE} - ${PYBIND_WRAP_SCRIPT} - --src - ${interface_header} - --out - ${generated_cpp} - --module_name - ${module_name} - --top_module_namespaces - "${top_namespace}" - --ignore - ${ignore_classes} - --template - ${module_template} - ${_WRAP_BOOST_ARG} - DEPENDS ${interface_header} ${module_template} - VERBATIM) - add_custom_target(pybind_wrap_${module_name} ALL DEPENDS ${generated_cpp}) + # Convert .i file names to .cpp file names. + foreach(filepath ${interface_headers}) + get_filename_component(interface ${filepath} NAME) + string(REPLACE ".i" ".cpp" cpp_file ${interface}) + list(APPEND cpp_files ${cpp_file}) + endforeach() + + add_custom_command( + OUTPUT ${cpp_files} + COMMAND + ${CMAKE_COMMAND} -E env + "PYTHONPATH=${GTWRAP_PACKAGE_DIR}${GTWRAP_PATH_SEPARATOR}$ENV{PYTHONPATH}" + ${PYTHON_EXECUTABLE} ${PYBIND_WRAP_SCRIPT} --src "${interface_headers}" + --out "${generated_cpp}" --module_name ${module_name} + --top_module_namespaces "${top_namespace}" --ignore ${ignore_classes} + --template ${module_template} ${_WRAP_BOOST_ARG} + DEPENDS "${interface_headers}" ${module_template} + VERBATIM) + + add_custom_target(pybind_wrap_${module_name} ALL DEPENDS ${cpp_files}) # Late dependency injection, to make sure this gets called whenever the # interface header or the wrap library are updated. # ~~~ # See: https://stackoverflow.com/questions/40032593/cmake-does-not-rebuild-dependent-after-prerequisite-changes # ~~~ - add_custom_command(OUTPUT ${generated_cpp} - DEPENDS ${interface_header} - # @GTWRAP_SOURCE_DIR@/gtwrap/interface_parser.py - # @GTWRAP_SOURCE_DIR@/gtwrap/pybind_wrapper.py - # @GTWRAP_SOURCE_DIR@/gtwrap/template_instantiator.py - APPEND) + add_custom_command( + OUTPUT ${cpp_files} + DEPENDS ${interface_headers} + # @GTWRAP_SOURCE_DIR@/gtwrap/interface_parser.py + # @GTWRAP_SOURCE_DIR@/gtwrap/pybind_wrapper.py + # @GTWRAP_SOURCE_DIR@/gtwrap/template_instantiator.py + APPEND) - pybind11_add_module(${target} ${generated_cpp}) + pybind11_add_module(${target} "${cpp_files}") if(APPLE) - # `type_info` objects will become "weak private external" if the templated class is initialized implicitly even if we explicitly - # export them with `WRAP_EXPORT`. If that happens, the `type_info` for the same templated class will diverge between shared - # libraries, causing `dynamic_cast` to fail. This is mitigated by telling Clang to mimic the MSVC behavior. - # See https://developer.apple.com/library/archive/technotes/tn2185/_index.html#//apple_ref/doc/uid/DTS10004200-CH1-SUBSECTION2 + # `type_info` objects will become "weak private external" if the templated + # class is initialized implicitly even if we explicitly export them with + # `WRAP_EXPORT`. If that happens, the `type_info` for the same templated + # class will diverge between shared libraries, causing `dynamic_cast` to + # fail. This is mitigated by telling Clang to mimic the MSVC behavior. See + # https://developer.apple.com/library/archive/technotes/tn2185/_index.html#//apple_ref/doc/uid/DTS10004200-CH1-SUBSECTION2 # https://github.com/CppMicroServices/CppMicroServices/pull/82/files # https://www.russellmcc.com/posts/2013-08-03-rtti.html target_compile_options(${target} PRIVATE "-fvisibility-ms-compat") endif() + add_dependencies(${target} pybind_wrap_${module_name}) + if(NOT "${libs}" STREQUAL "") target_link_libraries(${target} PRIVATE "${libs}") endif() @@ -121,10 +126,7 @@ endfunction() # dest_directory: The destination directory to install to. # patterns: list of file patterns to install # ~~~ -function(install_python_scripts - source_directory - dest_directory - patterns) +function(install_python_scripts source_directory dest_directory patterns) set(patterns_args "") set(exclude_patterns "") @@ -144,17 +146,19 @@ function(install_python_scripts # there is one get_filename_component(location "${dest_directory}" PATH) get_filename_component(name "${dest_directory}" NAME) - install(DIRECTORY "${source_directory}" - DESTINATION "${location}/${name}${build_type_tag}" - CONFIGURATIONS "${build_type}" - FILES_MATCHING ${patterns_args} - PATTERN "${exclude_patterns}" EXCLUDE) + install( + DIRECTORY "${source_directory}" + DESTINATION "${location}/${name}${build_type_tag}" + CONFIGURATIONS "${build_type}" + FILES_MATCHING ${patterns_args} + PATTERN "${exclude_patterns}" EXCLUDE) endforeach() else() - install(DIRECTORY "${source_directory}" - DESTINATION "${dest_directory}" - FILES_MATCHING ${patterns_args} - PATTERN "${exclude_patterns}" EXCLUDE) + install( + DIRECTORY "${source_directory}" + DESTINATION "${dest_directory}" + FILES_MATCHING ${patterns_args} + PATTERN "${exclude_patterns}" EXCLUDE) endif() endfunction() @@ -172,13 +176,14 @@ function(install_python_files source_files dest_directory) foreach(build_type ${CMAKE_CONFIGURATION_TYPES}) string(TOUPPER "${build_type}" build_type_upper) set(build_type_tag "") - # Split up filename to strip trailing '/' in WRAP_PY_INSTALL_PATH if - # there is one + # Split up filename to strip trailing '/' in WRAP_PY_INSTALL_PATH if there + # is one get_filename_component(location "${dest_directory}" PATH) get_filename_component(name "${dest_directory}" NAME) - install(FILES "${source_files}" - DESTINATION "${location}/${name}${build_type_tag}" - CONFIGURATIONS "${build_type}") + install( + FILES "${source_files}" + DESTINATION "${location}/${name}${build_type_tag}" + CONFIGURATIONS "${build_type}") endforeach() else() install(FILES "${source_files}" DESTINATION "${dest_directory}") @@ -194,18 +199,19 @@ function(create_symlinks source_folder dest_folder) return() endif() - file(GLOB files - LIST_DIRECTORIES true - RELATIVE "${source_folder}" - "${source_folder}/*") + file( + GLOB files + LIST_DIRECTORIES true + RELATIVE "${source_folder}" + "${source_folder}/*") foreach(path_file ${files}) get_filename_component(folder ${path_file} PATH) get_filename_component(ext ${path_file} EXT) set(ignored_ext ".tpl" ".h") - list (FIND ignored_ext "${ext}" _index) - if (${_index} GREATER -1) + list(FIND ignored_ext "${ext}" _index) + if(${_index} GREATER -1) continue() - endif () + endif() # Create REAL folder file(MAKE_DIRECTORY "${dest_folder}") @@ -224,9 +230,10 @@ function(create_symlinks source_folder dest_folder) endif() # cmake-format: on - execute_process(COMMAND ${command} - RESULT_VARIABLE result - ERROR_VARIABLE output) + execute_process( + COMMAND ${command} + RESULT_VARIABLE result + ERROR_VARIABLE output) if(NOT ${result} EQUAL 0) message( diff --git a/wrap/gtwrap/pybind_wrapper.py b/wrap/gtwrap/pybind_wrapper.py index 0e1b3c7ea..40571263a 100755 --- a/wrap/gtwrap/pybind_wrapper.py +++ b/wrap/gtwrap/pybind_wrapper.py @@ -13,6 +13,7 @@ Author: Duy Nguyen Ta, Fan Jiang, Matthew Sklar, Varun Agrawal, and Frank Dellae # pylint: disable=too-many-arguments, too-many-instance-attributes, no-self-use, no-else-return, too-many-arguments, unused-format-string-argument, line-too-long import re +from pathlib import Path import gtwrap.interface_parser as parser import gtwrap.template_instantiator as instantiator @@ -32,7 +33,7 @@ class PybindWrapper: self.top_module_namespaces = top_module_namespaces self.use_boost = use_boost self.ignore_classes = ignore_classes - self._serializing_classes = list() + self._serializing_classes = [] self.module_template = module_template self.python_keywords = [ 'lambda', 'False', 'def', 'if', 'raise', 'None', 'del', 'import', @@ -160,7 +161,7 @@ class PybindWrapper: 'self->print', 'py::scoped_ostream_redirect output; self->print') - # Make __repr__() call print() internally + # Make __repr__() call .print() internally ret += '''{prefix}.def("__repr__", [](const {cpp_class}& self{opt_comma}{args_signature_with_names}){{ gtsam::RedirectCout redirect; @@ -557,8 +558,15 @@ class PybindWrapper: ) return wrapped, includes - def wrap(self, content): - """Wrap the code in the interface file.""" + def wrap_file(self, content, module_name=None, submodules=None): + """ + Wrap the code in the interface file. + + Args: + content: The contents of the interface file. + module_name: The name of the module. + submodules: List of other interface file names that should be linked to. + """ # Parse the contents of the interface file module = parser.Module.parseString(content) # Instantiate all templates @@ -574,23 +582,74 @@ class PybindWrapper: if ',' in cpp_class: new_name = re.sub("[,:<> ]", "", cpp_class) boost_class_export += "typedef {cpp_class} {new_name};\n".format( # noqa - cpp_class=cpp_class, - new_name=new_name, - ) + cpp_class=cpp_class, new_name=new_name) + boost_class_export += "BOOST_CLASS_EXPORT({new_name})\n".format( new_name=new_name, ) + # Reset the serializing classes list + self._serializing_classes = [] + holder_type = "PYBIND11_DECLARE_HOLDER_TYPE(TYPE_PLACEHOLDER_DONOTUSE, " \ "{shared_ptr_type}::shared_ptr);" include_boost = "#include " if self.use_boost else "" + submodules_init = [] + + if submodules is not None: + module_def = "PYBIND11_MODULE({0}, m_)".format(module_name) + + for idx, submodule in enumerate(submodules): + submodules[idx] = "void {0}(py::module_ &);".format(submodule) + submodules_init.append("{0}(m_);".format(submodule)) + + else: + module_def = "void {0}(py::module_ &m_)".format(module_name) + submodules = [] + return self.module_template.format( include_boost=include_boost, - module_name=self.module_name, + module_def=module_def, + module_name=module_name, includes=includes, holder_type=holder_type.format( shared_ptr_type=('boost' if self.use_boost else 'std')) if self.use_boost else "", wrapped_namespace=wrapped_namespace, boost_class_export=boost_class_export, + submodules="\n".join(submodules), + submodules_init="\n".join(submodules_init), ) + + def wrap(self, sources, main_output): + """ + Wrap all the source interface files. + + Args: + sources: List of all interface files. + main_output: The name for the main module. + """ + main_module = sources[0] + submodules = [] + for source in sources[1:]: + filename = Path(source).name + module_name = Path(source).stem + # Read in the complete interface (.i) file + with open(source, "r") as f: + content = f.read() + submodules.append(module_name) + cc_content = self.wrap_file(content, module_name=module_name) + + # Generate the C++ code which Pybind11 will use. + with open(filename.replace(".i", ".cpp"), "w") as f: + f.write(cc_content) + + with open(main_module, "r") as f: + content = f.read() + cc_content = self.wrap_file(content, + module_name=self.module_name, + submodules=submodules) + + # Generate the C++ code which Pybind11 will use. + with open(main_output, "w") as f: + f.write(cc_content) diff --git a/wrap/scripts/pybind_wrap.py b/wrap/scripts/pybind_wrap.py index 7f2f8d419..c82a1d24c 100644 --- a/wrap/scripts/pybind_wrap.py +++ b/wrap/scripts/pybind_wrap.py @@ -67,10 +67,6 @@ def main(): if top_module_namespaces[0]: top_module_namespaces = [''] + top_module_namespaces - # Read in the complete interface (.i) file - with open(args.src, "r") as f: - content = f.read() - with open(args.template, "r") as f: template_content = f.read() @@ -83,11 +79,8 @@ def main(): ) # Wrap the code and get back the cpp/cc code. - cc_content = wrapper.wrap(content) - - # Generate the C++ code which Pybind11 will use. - with open(args.out, "w") as f: - f.write(cc_content) + sources = args.src.split(';') + wrapper.wrap(sources, args.out) if __name__ == "__main__": diff --git a/wrap/templates/pybind_wrapper.tpl.example b/wrap/templates/pybind_wrapper.tpl.example index bf5b33490..485aa8d00 100644 --- a/wrap/templates/pybind_wrapper.tpl.example +++ b/wrap/templates/pybind_wrapper.tpl.example @@ -7,7 +7,7 @@ #include #include #include "gtsam/base/serialization.h" -#include "gtsam/nonlinear/utilities.h" // for RedirectCout. +#include "gtsam/base/utilities.h" // for RedirectCout. {includes} #include @@ -22,9 +22,13 @@ using namespace std; namespace py = pybind11; -PYBIND11_MODULE({module_name}, m_) {{ +{submodules} + +{module_def} {{ m_.doc() = "pybind11 wrapper of {module_name}"; +{submodules_init} + {wrapped_namespace} #include "python/specializations.h" From 0989aed0cfa2bfbec198dfa25d7bc7abcb6d9f7f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 11 Jul 2021 08:10:10 -0700 Subject: [PATCH 048/248] enable CI builds --- .github/workflows/build-python.yml | 28 +++++++++++++--------------- 1 file changed, 13 insertions(+), 15 deletions(-) diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index 1f87b5119..addde8c64 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -19,22 +19,20 @@ jobs: # Github Actions requires a single row to be added to the build matrix. # See https://help.github.com/en/articles/workflow-syntax-for-github-actions. name: [ - # ubuntu-18.04-gcc-5, #TODO Enable once the Python wrapper is optimized for memory + ubuntu-18.04-gcc-5, ubuntu-18.04-gcc-9, ubuntu-18.04-clang-9, macOS-10.15-xcode-11.3.1, - # ubuntu-18.04-gcc-5-tbb, + ubuntu-18.04-gcc-5-tbb, ] - #TODO update wrapper to prevent OOM - # build_type: [Debug, Release] - build_type: [Release] + build_type: [Debug, Release] python_version: [3] include: - # - name: ubuntu-18.04-gcc-5 - # os: ubuntu-18.04 - # compiler: gcc - # version: "5" + - name: ubuntu-18.04-gcc-5 + os: ubuntu-18.04 + compiler: gcc + version: "5" - name: ubuntu-18.04-gcc-9 os: ubuntu-18.04 @@ -46,7 +44,7 @@ jobs: compiler: clang version: "9" - #NOTE temporarily added this as it is a required check. + # NOTE temporarily added this as it is a required check. - name: ubuntu-18.04-clang-9 os: ubuntu-18.04 compiler: clang @@ -59,11 +57,11 @@ jobs: compiler: xcode version: "11.3.1" - # - name: ubuntu-18.04-gcc-5-tbb - # os: ubuntu-18.04 - # compiler: gcc - # version: "5" - # flag: tbb + - name: ubuntu-18.04-gcc-5-tbb + os: ubuntu-18.04 + compiler: gcc + version: "5" + flag: tbb steps: - name: Checkout From 17842dcea7ed5ae377a5b9f57fdfb45a3a2b0291 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 11 Jul 2021 08:10:11 -0700 Subject: [PATCH 049/248] fixes --- gtsam/geometry/geometry.i | 1 + python/gtsam_unstable/gtsam_unstable.tpl | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index a39c22797..6217d9e80 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -459,6 +459,7 @@ class Pose3 { // Lie Group static gtsam::Pose3 Expmap(Vector v); static Vector Logmap(const gtsam::Pose3& pose); + gtsam::Pose3 expmap(Vector v); Vector logmap(const gtsam::Pose3& pose); Matrix AdjointMap() const; Vector Adjoint(Vector xi) const; diff --git a/python/gtsam_unstable/gtsam_unstable.tpl b/python/gtsam_unstable/gtsam_unstable.tpl index c1033ba43..aa7ac6bdb 100644 --- a/python/gtsam_unstable/gtsam_unstable.tpl +++ b/python/gtsam_unstable/gtsam_unstable.tpl @@ -16,7 +16,7 @@ #include #include #include "gtsam/base/serialization.h" -#include "gtsam/nonlinear/utilities.h" // for RedirectCout. +#include "gtsam/base/utilities.h" // for RedirectCout. // These are the included headers listed in `gtsam_unstable.i` {includes} From 4c410fcd0ebe1869400f6ea601e21792da74f019 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 11 Jul 2021 08:10:35 -0700 Subject: [PATCH 050/248] Squashed 'wrap/' changes from 07330d100..d9ae5ce03 d9ae5ce03 Merge pull request #118 from borglab/feature/matlab-multi-files 9adddf7dd update the main script for matlab wrapping 0b0398d46 remove debug statements since they aren't needed for now df064a364 support for parsing mutiple interface files for Matlab wrapping 1929e197c add test for parsing multiple interface files bac442056 Merge pull request #117 from borglab/fix/matlab-refactor 331f4a8ce update tests to remove redundant code 5426e3af4 generate all content from within the wrap function f78612bf9 make directory check common b7acd7a1f fixed import and test setup 88007b153 Merge pull request #116 from borglab/feature/matlab-refactor a074896e6 utils -> mixins 414557e00 structure 187100439 update gitignore adbc55aea don't use class attributes in matlab wrapper f45ba5b2d broke down some large functions into smaller ones 7756f0548 add mixin for checks and call method to wrap global functions a318e2a67 Merge pull request #115 from borglab/feature/multiple-modules b02b74c3d convert matlab_wrapper to a submodule be8641e83 improved function naming in tests 02ddbfbb0 update tests and docs dfbded2c7 small fixes e9ec5af07 update docs d124e2cfb wrap multiple files 7c7342f86 update cmake to take in new changes for multiple modules 54850f724 Merge pull request #114 from borglab/fix/remove-py35 71ee98321 add mypy annotations ccaea6294 remove support for python 3.5 git-subtree-dir: wrap git-subtree-split: d9ae5ce036c4315db3c28b12db9c73eae246f314 --- .github/workflows/linux-ci.yml | 2 +- .github/workflows/macos-ci.yml | 2 +- .gitignore | 2 +- CMakeLists.txt | 2 +- DOCS.md | 8 +- README.md | 4 +- cmake/PybindWrap.cmake | 157 ++-- gtwrap/interface_parser/__init__.py | 2 +- gtwrap/interface_parser/classes.py | 12 +- gtwrap/interface_parser/declaration.py | 2 +- gtwrap/interface_parser/enum.py | 2 +- gtwrap/interface_parser/function.py | 12 +- gtwrap/interface_parser/module.py | 3 +- gtwrap/interface_parser/namespace.py | 6 +- gtwrap/interface_parser/template.py | 4 +- gtwrap/interface_parser/tokens.py | 6 +- gtwrap/interface_parser/type.py | 5 +- gtwrap/interface_parser/variable.py | 6 +- gtwrap/matlab_wrapper/__init__.py | 3 + gtwrap/matlab_wrapper/mixins.py | 222 +++++ gtwrap/matlab_wrapper/templates.py | 166 ++++ .../wrapper.py} | 847 ++++++------------ gtwrap/pybind_wrapper.py | 75 +- gtwrap/template_instantiator.py | 23 +- scripts/matlab_wrap.py | 40 +- scripts/pybind_wrap.py | 11 +- templates/pybind_wrapper.tpl.example | 8 +- tests/expected/matlab/+gtsam/Class1.m | 36 + tests/expected/matlab/+gtsam/Class2.m | 36 + tests/expected/matlab/+gtsam/ClassA.m | 36 + tests/expected/matlab/class_wrapper.cpp | 17 +- tests/expected/matlab/functions_wrapper.cpp | 103 +-- tests/expected/matlab/geometry_wrapper.cpp | 103 +-- tests/expected/matlab/inheritance_wrapper.cpp | 221 ++--- .../matlab/multiple_files_wrapper.cpp | 229 +++++ tests/expected/matlab/namespaces_wrapper.cpp | 161 +--- .../expected/matlab/special_cases_wrapper.cpp | 224 +---- tests/fixtures/part1.i | 11 + tests/fixtures/part2.i | 7 + tests/test_matlab_wrapper.py | 173 ++-- tests/test_pybind_wrapper.py | 58 +- 41 files changed, 1419 insertions(+), 1628 deletions(-) create mode 100644 gtwrap/matlab_wrapper/__init__.py create mode 100644 gtwrap/matlab_wrapper/mixins.py create mode 100644 gtwrap/matlab_wrapper/templates.py rename gtwrap/{matlab_wrapper.py => matlab_wrapper/wrapper.py} (68%) create mode 100644 tests/expected/matlab/+gtsam/Class1.m create mode 100644 tests/expected/matlab/+gtsam/Class2.m create mode 100644 tests/expected/matlab/+gtsam/ClassA.m create mode 100644 tests/expected/matlab/multiple_files_wrapper.cpp create mode 100644 tests/fixtures/part1.i create mode 100644 tests/fixtures/part2.i diff --git a/.github/workflows/linux-ci.yml b/.github/workflows/linux-ci.yml index 0ca9ba8f5..34623385e 100644 --- a/.github/workflows/linux-ci.yml +++ b/.github/workflows/linux-ci.yml @@ -10,7 +10,7 @@ jobs: strategy: fail-fast: false matrix: - python-version: [3.5, 3.6, 3.7, 3.8, 3.9] + python-version: [3.6, 3.7, 3.8, 3.9] steps: - name: Checkout diff --git a/.github/workflows/macos-ci.yml b/.github/workflows/macos-ci.yml index b0ccb3fbe..3910d28d8 100644 --- a/.github/workflows/macos-ci.yml +++ b/.github/workflows/macos-ci.yml @@ -10,7 +10,7 @@ jobs: strategy: fail-fast: false matrix: - python-version: [3.5, 3.6, 3.7, 3.8, 3.9] + python-version: [3.6, 3.7, 3.8, 3.9] steps: - name: Checkout diff --git a/.gitignore b/.gitignore index 8e2bafa7a..9f79deafa 100644 --- a/.gitignore +++ b/.gitignore @@ -8,4 +8,4 @@ __pycache__/ # Files related to code coverage stats **/.coverage -gtwrap/matlab_wrapper.tpl +gtwrap/matlab_wrapper/matlab_wrapper.tpl diff --git a/CMakeLists.txt b/CMakeLists.txt index 9e03da060..2a11a760d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -58,7 +58,7 @@ if(NOT DEFINED GTWRAP_INCLUDE_NAME) endif() configure_file(${PROJECT_SOURCE_DIR}/templates/matlab_wrapper.tpl.in - ${PROJECT_SOURCE_DIR}/gtwrap/matlab_wrapper.tpl) + ${PROJECT_SOURCE_DIR}/gtwrap/matlab_wrapper/matlab_wrapper.tpl) # Install the gtwrap python package as a directory so it can be found by CMake # for wrapping. diff --git a/DOCS.md b/DOCS.md index 8537ddd27..c8285baef 100644 --- a/DOCS.md +++ b/DOCS.md @@ -192,12 +192,14 @@ The python wrapper supports keyword arguments for functions/methods. Hence, the - **DO NOT** re-define an overriden function already declared in the external (forward-declared) base class. This will cause an ambiguity problem in the Pybind header file. +- Splitting wrapper over multiple files + - The Pybind11 wrapper supports splitting the wrapping code over multiple files. + - To be able to use classes from another module, simply import the C++ header file in that wrapper file. + - Unfortunately, this means that aliases can no longer be used. + - Similarly, there can be multiple `preamble.h` and `specializations.h` files. Each of these should match the module file name. ### TODO -- Default values for arguments. - - WORKAROUND: make multiple versions of the same function for different configurations of default arguments. - Handle `gtsam::Rot3M` conversions to quaternions. - Parse return of const ref arguments. - Parse `std::string` variants and convert directly to special string. -- Add enum support. - Add generalized serialization support via `boost.serialization` with hooks to MATLAB save/load. diff --git a/README.md b/README.md index 442fc2f93..a04a2ef2d 100644 --- a/README.md +++ b/README.md @@ -29,8 +29,10 @@ Using `wrap` in your project is straightforward from here. In your `CMakeLists.t ```cmake find_package(gtwrap) +set(interface_files ${PROJECT_SOURCE_DIR}/cpp/${PROJECT_NAME}.h) + pybind_wrap(${PROJECT_NAME}_py # target - ${PROJECT_SOURCE_DIR}/cpp/${PROJECT_NAME}.h # interface header file + "${interface_files}" # list of interface header files "${PROJECT_NAME}.cpp" # the generated cpp "${PROJECT_NAME}" # module_name "${PROJECT_MODULE_NAME}" # top namespace in the cpp file e.g. gtsam diff --git a/cmake/PybindWrap.cmake b/cmake/PybindWrap.cmake index 331dfff8c..f341c2f98 100644 --- a/cmake/PybindWrap.cmake +++ b/cmake/PybindWrap.cmake @@ -13,15 +13,14 @@ gtwrap_get_python_version(${WRAP_PYTHON_VERSION}) message(STATUS "Setting Python version for wrapper") set(PYBIND11_PYTHON_VERSION ${WRAP_PYTHON_VERSION}) -# User-friendly Pybind11 wrapping and installing function. -# Builds a Pybind11 module from the provided interface_header. -# For example, for the interface header gtsam.h, this will -# build the wrap module 'gtsam_py.cc'. +# User-friendly Pybind11 wrapping and installing function. Builds a Pybind11 +# module from the provided interface_headers. For example, for the interface +# header gtsam.h, this will build the wrap module 'gtsam_py.cc'. # # Arguments: # ~~~ # target: The Make target -# interface_header: The relative path to the wrapper interface definition file. +# interface_headers: List of paths to the wrapper interface definition files. The top level interface file should be first. # generated_cpp: The name of the cpp file which is generated from the tpl file. # module_name: The name of the Python module to use. # top_namespace: The C++ namespace under which the code to be wrapped exists. @@ -31,16 +30,17 @@ set(PYBIND11_PYTHON_VERSION ${WRAP_PYTHON_VERSION}) # libs: Libraries to link with. # dependencies: Dependencies which need to be built before the wrapper. # use_boost (optional): Flag indicating whether to include Boost. -function(pybind_wrap - target - interface_header - generated_cpp - module_name - top_namespace - ignore_classes - module_template - libs - dependencies) +function( + pybind_wrap + target + interface_headers + generated_cpp + module_name + top_namespace + ignore_classes + module_template + libs + dependencies) set(ExtraMacroArgs ${ARGN}) list(GET ExtraMacroArgs 0 USE_BOOST) if(USE_BOOST) @@ -49,57 +49,62 @@ function(pybind_wrap set(_WRAP_BOOST_ARG "") endif(USE_BOOST) - if (UNIX) + if(UNIX) set(GTWRAP_PATH_SEPARATOR ":") else() set(GTWRAP_PATH_SEPARATOR ";") endif() - add_custom_command(OUTPUT ${generated_cpp} - COMMAND ${CMAKE_COMMAND} -E env "PYTHONPATH=${GTWRAP_PACKAGE_DIR}${GTWRAP_PATH_SEPARATOR}$ENV{PYTHONPATH}" - ${PYTHON_EXECUTABLE} - ${PYBIND_WRAP_SCRIPT} - --src - ${interface_header} - --out - ${generated_cpp} - --module_name - ${module_name} - --top_module_namespaces - "${top_namespace}" - --ignore - ${ignore_classes} - --template - ${module_template} - ${_WRAP_BOOST_ARG} - DEPENDS ${interface_header} ${module_template} - VERBATIM) - add_custom_target(pybind_wrap_${module_name} ALL DEPENDS ${generated_cpp}) + # Convert .i file names to .cpp file names. + foreach(filepath ${interface_headers}) + get_filename_component(interface ${filepath} NAME) + string(REPLACE ".i" ".cpp" cpp_file ${interface}) + list(APPEND cpp_files ${cpp_file}) + endforeach() + + add_custom_command( + OUTPUT ${cpp_files} + COMMAND + ${CMAKE_COMMAND} -E env + "PYTHONPATH=${GTWRAP_PACKAGE_DIR}${GTWRAP_PATH_SEPARATOR}$ENV{PYTHONPATH}" + ${PYTHON_EXECUTABLE} ${PYBIND_WRAP_SCRIPT} --src "${interface_headers}" + --out "${generated_cpp}" --module_name ${module_name} + --top_module_namespaces "${top_namespace}" --ignore ${ignore_classes} + --template ${module_template} ${_WRAP_BOOST_ARG} + DEPENDS "${interface_headers}" ${module_template} + VERBATIM) + + add_custom_target(pybind_wrap_${module_name} ALL DEPENDS ${cpp_files}) # Late dependency injection, to make sure this gets called whenever the # interface header or the wrap library are updated. # ~~~ # See: https://stackoverflow.com/questions/40032593/cmake-does-not-rebuild-dependent-after-prerequisite-changes # ~~~ - add_custom_command(OUTPUT ${generated_cpp} - DEPENDS ${interface_header} - # @GTWRAP_SOURCE_DIR@/gtwrap/interface_parser.py - # @GTWRAP_SOURCE_DIR@/gtwrap/pybind_wrapper.py - # @GTWRAP_SOURCE_DIR@/gtwrap/template_instantiator.py - APPEND) + add_custom_command( + OUTPUT ${cpp_files} + DEPENDS ${interface_headers} + # @GTWRAP_SOURCE_DIR@/gtwrap/interface_parser.py + # @GTWRAP_SOURCE_DIR@/gtwrap/pybind_wrapper.py + # @GTWRAP_SOURCE_DIR@/gtwrap/template_instantiator.py + APPEND) - pybind11_add_module(${target} ${generated_cpp}) + pybind11_add_module(${target} "${cpp_files}") if(APPLE) - # `type_info` objects will become "weak private external" if the templated class is initialized implicitly even if we explicitly - # export them with `WRAP_EXPORT`. If that happens, the `type_info` for the same templated class will diverge between shared - # libraries, causing `dynamic_cast` to fail. This is mitigated by telling Clang to mimic the MSVC behavior. - # See https://developer.apple.com/library/archive/technotes/tn2185/_index.html#//apple_ref/doc/uid/DTS10004200-CH1-SUBSECTION2 + # `type_info` objects will become "weak private external" if the templated + # class is initialized implicitly even if we explicitly export them with + # `WRAP_EXPORT`. If that happens, the `type_info` for the same templated + # class will diverge between shared libraries, causing `dynamic_cast` to + # fail. This is mitigated by telling Clang to mimic the MSVC behavior. See + # https://developer.apple.com/library/archive/technotes/tn2185/_index.html#//apple_ref/doc/uid/DTS10004200-CH1-SUBSECTION2 # https://github.com/CppMicroServices/CppMicroServices/pull/82/files # https://www.russellmcc.com/posts/2013-08-03-rtti.html target_compile_options(${target} PRIVATE "-fvisibility-ms-compat") endif() + add_dependencies(${target} pybind_wrap_${module_name}) + if(NOT "${libs}" STREQUAL "") target_link_libraries(${target} PRIVATE "${libs}") endif() @@ -121,10 +126,7 @@ endfunction() # dest_directory: The destination directory to install to. # patterns: list of file patterns to install # ~~~ -function(install_python_scripts - source_directory - dest_directory - patterns) +function(install_python_scripts source_directory dest_directory patterns) set(patterns_args "") set(exclude_patterns "") @@ -144,17 +146,19 @@ function(install_python_scripts # there is one get_filename_component(location "${dest_directory}" PATH) get_filename_component(name "${dest_directory}" NAME) - install(DIRECTORY "${source_directory}" - DESTINATION "${location}/${name}${build_type_tag}" - CONFIGURATIONS "${build_type}" - FILES_MATCHING ${patterns_args} - PATTERN "${exclude_patterns}" EXCLUDE) + install( + DIRECTORY "${source_directory}" + DESTINATION "${location}/${name}${build_type_tag}" + CONFIGURATIONS "${build_type}" + FILES_MATCHING ${patterns_args} + PATTERN "${exclude_patterns}" EXCLUDE) endforeach() else() - install(DIRECTORY "${source_directory}" - DESTINATION "${dest_directory}" - FILES_MATCHING ${patterns_args} - PATTERN "${exclude_patterns}" EXCLUDE) + install( + DIRECTORY "${source_directory}" + DESTINATION "${dest_directory}" + FILES_MATCHING ${patterns_args} + PATTERN "${exclude_patterns}" EXCLUDE) endif() endfunction() @@ -172,13 +176,14 @@ function(install_python_files source_files dest_directory) foreach(build_type ${CMAKE_CONFIGURATION_TYPES}) string(TOUPPER "${build_type}" build_type_upper) set(build_type_tag "") - # Split up filename to strip trailing '/' in WRAP_PY_INSTALL_PATH if - # there is one + # Split up filename to strip trailing '/' in WRAP_PY_INSTALL_PATH if there + # is one get_filename_component(location "${dest_directory}" PATH) get_filename_component(name "${dest_directory}" NAME) - install(FILES "${source_files}" - DESTINATION "${location}/${name}${build_type_tag}" - CONFIGURATIONS "${build_type}") + install( + FILES "${source_files}" + DESTINATION "${location}/${name}${build_type_tag}" + CONFIGURATIONS "${build_type}") endforeach() else() install(FILES "${source_files}" DESTINATION "${dest_directory}") @@ -194,18 +199,19 @@ function(create_symlinks source_folder dest_folder) return() endif() - file(GLOB files - LIST_DIRECTORIES true - RELATIVE "${source_folder}" - "${source_folder}/*") + file( + GLOB files + LIST_DIRECTORIES true + RELATIVE "${source_folder}" + "${source_folder}/*") foreach(path_file ${files}) get_filename_component(folder ${path_file} PATH) get_filename_component(ext ${path_file} EXT) set(ignored_ext ".tpl" ".h") - list (FIND ignored_ext "${ext}" _index) - if (${_index} GREATER -1) + list(FIND ignored_ext "${ext}" _index) + if(${_index} GREATER -1) continue() - endif () + endif() # Create REAL folder file(MAKE_DIRECTORY "${dest_folder}") @@ -224,9 +230,10 @@ function(create_symlinks source_folder dest_folder) endif() # cmake-format: on - execute_process(COMMAND ${command} - RESULT_VARIABLE result - ERROR_VARIABLE output) + execute_process( + COMMAND ${command} + RESULT_VARIABLE result + ERROR_VARIABLE output) if(NOT ${result} EQUAL 0) message( diff --git a/gtwrap/interface_parser/__init__.py b/gtwrap/interface_parser/__init__.py index 0f87eaaa9..3be52d7d9 100644 --- a/gtwrap/interface_parser/__init__.py +++ b/gtwrap/interface_parser/__init__.py @@ -12,7 +12,7 @@ Author: Duy Nguyen Ta, Fan Jiang, Matthew Sklar, Varun Agrawal, and Frank Dellae import sys -import pyparsing +import pyparsing # type: ignore from .classes import * from .declaration import * diff --git a/gtwrap/interface_parser/classes.py b/gtwrap/interface_parser/classes.py index ea7a3b3c3..3e6a0fc3c 100644 --- a/gtwrap/interface_parser/classes.py +++ b/gtwrap/interface_parser/classes.py @@ -12,7 +12,7 @@ Author: Duy Nguyen Ta, Fan Jiang, Matthew Sklar, Varun Agrawal, and Frank Dellae from typing import Iterable, List, Union -from pyparsing import Literal, Optional, ZeroOrMore +from pyparsing import Literal, Optional, ZeroOrMore # type: ignore from .enum import Enum from .function import ArgumentList, ReturnType @@ -233,7 +233,7 @@ class Class: self.static_methods = [] self.properties = [] self.operators = [] - self.enums = [] + self.enums: List[Enum] = [] for m in members: if isinstance(m, Constructor): self.ctors.append(m) @@ -274,7 +274,7 @@ class Class: def __init__( self, - template: Template, + template: Union[Template, None], is_virtual: str, name: str, parent_class: list, @@ -292,16 +292,16 @@ class Class: if parent_class: # If it is in an iterable, extract the parent class. if isinstance(parent_class, Iterable): - parent_class = parent_class[0] + parent_class = parent_class[0] # type: ignore # If the base class is a TemplatedType, # we want the instantiated Typename if isinstance(parent_class, TemplatedType): - parent_class = parent_class.typename + parent_class = parent_class.typename # type: ignore self.parent_class = parent_class else: - self.parent_class = '' + self.parent_class = '' # type: ignore self.ctors = ctors self.methods = methods diff --git a/gtwrap/interface_parser/declaration.py b/gtwrap/interface_parser/declaration.py index 292d6aeaa..f47ee6e05 100644 --- a/gtwrap/interface_parser/declaration.py +++ b/gtwrap/interface_parser/declaration.py @@ -10,7 +10,7 @@ Classes and rules for declarations such as includes and forward declarations. Author: Duy Nguyen Ta, Fan Jiang, Matthew Sklar, Varun Agrawal, and Frank Dellaert """ -from pyparsing import CharsNotIn, Optional +from pyparsing import CharsNotIn, Optional # type: ignore from .tokens import (CLASS, COLON, INCLUDE, LOPBRACK, ROPBRACK, SEMI_COLON, VIRTUAL) diff --git a/gtwrap/interface_parser/enum.py b/gtwrap/interface_parser/enum.py index fca7080ef..265e1ad61 100644 --- a/gtwrap/interface_parser/enum.py +++ b/gtwrap/interface_parser/enum.py @@ -10,7 +10,7 @@ Parser class and rules for parsing C++ enums. Author: Varun Agrawal """ -from pyparsing import delimitedList +from pyparsing import delimitedList # type: ignore from .tokens import ENUM, IDENT, LBRACE, RBRACE, SEMI_COLON from .type import Typename diff --git a/gtwrap/interface_parser/function.py b/gtwrap/interface_parser/function.py index 3b9a5d4ad..995aba10e 100644 --- a/gtwrap/interface_parser/function.py +++ b/gtwrap/interface_parser/function.py @@ -10,9 +10,9 @@ Parser classes and rules for parsing C++ functions. Author: Duy Nguyen Ta, Fan Jiang, Matthew Sklar, Varun Agrawal, and Frank Dellaert """ -from typing import Iterable, List, Union +from typing import Any, Iterable, List, Union -from pyparsing import Optional, ParseResults, delimitedList +from pyparsing import Optional, ParseResults, delimitedList # type: ignore from .template import Template from .tokens import (COMMA, DEFAULT_ARG, EQUAL, IDENT, LOPBRACK, LPAREN, PAIR, @@ -42,12 +42,12 @@ class Argument: name: str, default: ParseResults = None): if isinstance(ctype, Iterable): - self.ctype = ctype[0] + self.ctype = ctype[0] # type: ignore else: self.ctype = ctype self.name = name self.default = default - self.parent = None # type: Union[ArgumentList, None] + self.parent: Union[ArgumentList, None] = None def __repr__(self) -> str: return self.to_cpp() @@ -70,7 +70,7 @@ class ArgumentList: arg.parent = self # The parent object which contains the argument list # E.g. Method, StaticMethod, Template, Constructor, GlobalFunction - self.parent = None + self.parent: Any = None @staticmethod def from_parse_result(parse_result: ParseResults): @@ -123,7 +123,7 @@ class ReturnType: self.type2 = type2 # The parent object which contains the return type # E.g. Method, StaticMethod, Template, Constructor, GlobalFunction - self.parent = None + self.parent: Any = None def is_void(self) -> bool: """ diff --git a/gtwrap/interface_parser/module.py b/gtwrap/interface_parser/module.py index 6412098b8..7912c40d5 100644 --- a/gtwrap/interface_parser/module.py +++ b/gtwrap/interface_parser/module.py @@ -12,7 +12,8 @@ Author: Duy Nguyen Ta, Fan Jiang, Matthew Sklar, Varun Agrawal, and Frank Dellae # pylint: disable=unnecessary-lambda, unused-import, expression-not-assigned, no-else-return, protected-access, too-few-public-methods, too-many-arguments -from pyparsing import ParseResults, ZeroOrMore, cppStyleComment, stringEnd +from pyparsing import (ParseResults, ZeroOrMore, # type: ignore + cppStyleComment, stringEnd) from .classes import Class from .declaration import ForwardDeclaration, Include diff --git a/gtwrap/interface_parser/namespace.py b/gtwrap/interface_parser/namespace.py index 575d98237..9c135ffe8 100644 --- a/gtwrap/interface_parser/namespace.py +++ b/gtwrap/interface_parser/namespace.py @@ -14,7 +14,7 @@ Author: Duy Nguyen Ta, Fan Jiang, Matthew Sklar, Varun Agrawal, and Frank Dellae from typing import List, Union -from pyparsing import Forward, ParseResults, ZeroOrMore +from pyparsing import Forward, ParseResults, ZeroOrMore # type: ignore from .classes import Class, collect_namespaces from .declaration import ForwardDeclaration, Include @@ -93,7 +93,7 @@ class Namespace: return Namespace(t.name, content) def find_class_or_function( - self, typename: Typename) -> Union[Class, GlobalFunction]: + self, typename: Typename) -> Union[Class, GlobalFunction, ForwardDeclaration]: """ Find the Class or GlobalFunction object given its typename. We have to traverse the tree of namespaces. @@ -115,7 +115,7 @@ class Namespace: return res[0] def top_level(self) -> "Namespace": - """Return the top leve namespace.""" + """Return the top level namespace.""" if self.name == '' or self.parent == '': return self else: diff --git a/gtwrap/interface_parser/template.py b/gtwrap/interface_parser/template.py index dc9d0ce44..fd9de830a 100644 --- a/gtwrap/interface_parser/template.py +++ b/gtwrap/interface_parser/template.py @@ -12,11 +12,11 @@ Author: Duy Nguyen Ta, Fan Jiang, Matthew Sklar, Varun Agrawal, and Frank Dellae from typing import List -from pyparsing import Optional, ParseResults, delimitedList +from pyparsing import Optional, ParseResults, delimitedList # type: ignore from .tokens import (EQUAL, IDENT, LBRACE, LOPBRACK, RBRACE, ROPBRACK, SEMI_COLON, TEMPLATE, TYPEDEF) -from .type import Typename, TemplatedType +from .type import TemplatedType, Typename class Template: diff --git a/gtwrap/interface_parser/tokens.py b/gtwrap/interface_parser/tokens.py index 4eba95900..0f8d38d86 100644 --- a/gtwrap/interface_parser/tokens.py +++ b/gtwrap/interface_parser/tokens.py @@ -10,9 +10,9 @@ All the token definitions. Author: Duy Nguyen Ta, Fan Jiang, Matthew Sklar, Varun Agrawal, and Frank Dellaert """ -from pyparsing import (Keyword, Literal, OneOrMore, Or, QuotedString, Suppress, - Word, alphanums, alphas, nestedExpr, nums, - originalTextFor, printables) +from pyparsing import (Keyword, Literal, OneOrMore, Or, # type: ignore + QuotedString, Suppress, Word, alphanums, alphas, + nestedExpr, nums, originalTextFor, printables) # rule for identifiers (e.g. variable names) IDENT = Word(alphas + '_', alphanums + '_') ^ Word(nums) diff --git a/gtwrap/interface_parser/type.py b/gtwrap/interface_parser/type.py index b9f2bd8f7..0b9be6501 100644 --- a/gtwrap/interface_parser/type.py +++ b/gtwrap/interface_parser/type.py @@ -14,7 +14,8 @@ Author: Duy Nguyen Ta, Fan Jiang, Matthew Sklar, Varun Agrawal, and Frank Dellae from typing import Iterable, List, Union -from pyparsing import Forward, Optional, Or, ParseResults, delimitedList +from pyparsing import (Forward, Optional, Or, ParseResults, # type: ignore + delimitedList) from .tokens import (BASIS_TYPES, CONST, IDENT, LOPBRACK, RAW_POINTER, REF, ROPBRACK, SHARED_POINTER) @@ -48,7 +49,7 @@ class Typename: def __init__(self, t: ParseResults, - instantiations: Union[tuple, list, str, ParseResults] = ()): + instantiations: Iterable[ParseResults] = ()): self.name = t[-1] # the name is the last element in this list self.namespaces = t[:-1] diff --git a/gtwrap/interface_parser/variable.py b/gtwrap/interface_parser/variable.py index fcb02666f..3779cf74f 100644 --- a/gtwrap/interface_parser/variable.py +++ b/gtwrap/interface_parser/variable.py @@ -10,7 +10,9 @@ Parser classes and rules for parsing C++ variables. Author: Varun Agrawal, Gerry Chen """ -from pyparsing import Optional, ParseResults +from typing import List + +from pyparsing import Optional, ParseResults # type: ignore from .tokens import DEFAULT_ARG, EQUAL, IDENT, SEMI_COLON from .type import TemplatedType, Type @@ -40,7 +42,7 @@ class Variable: t.default[0] if isinstance(t.default, ParseResults) else None)) def __init__(self, - ctype: Type, + ctype: List[Type], name: str, default: ParseResults = None, parent=''): diff --git a/gtwrap/matlab_wrapper/__init__.py b/gtwrap/matlab_wrapper/__init__.py new file mode 100644 index 000000000..f10338c1c --- /dev/null +++ b/gtwrap/matlab_wrapper/__init__.py @@ -0,0 +1,3 @@ +"""Package to wrap C++ code to Matlab via MEX.""" + +from .wrapper import MatlabWrapper diff --git a/gtwrap/matlab_wrapper/mixins.py b/gtwrap/matlab_wrapper/mixins.py new file mode 100644 index 000000000..061cea283 --- /dev/null +++ b/gtwrap/matlab_wrapper/mixins.py @@ -0,0 +1,222 @@ +"""Mixins for reducing the amount of boilerplate in the main wrapper class.""" + +import gtwrap.interface_parser as parser +import gtwrap.template_instantiator as instantiator + + +class CheckMixin: + """Mixin to provide various checks.""" + # Data types that are primitive types + not_ptr_type = ['int', 'double', 'bool', 'char', 'unsigned char', 'size_t'] + # Ignore the namespace for these datatypes + ignore_namespace = ['Matrix', 'Vector', 'Point2', 'Point3'] + # Methods that should be ignored + ignore_methods = ['pickle'] + # Methods that should not be wrapped directly + whitelist = ['serializable', 'serialize'] + # Datatypes that do not need to be checked in methods + not_check_type: list = [] + + def _has_serialization(self, cls): + for m in cls.methods: + if m.name in self.whitelist: + return True + return False + + def is_shared_ptr(self, arg_type): + """ + Determine if the `interface_parser.Type` should be treated as a + shared pointer in the wrapper. + """ + return arg_type.is_shared_ptr or ( + arg_type.typename.name not in self.not_ptr_type + and arg_type.typename.name not in self.ignore_namespace + and arg_type.typename.name != 'string') + + def is_ptr(self, arg_type): + """ + Determine if the `interface_parser.Type` should be treated as a + raw pointer in the wrapper. + """ + return arg_type.is_ptr or ( + arg_type.typename.name not in self.not_ptr_type + and arg_type.typename.name not in self.ignore_namespace + and arg_type.typename.name != 'string') + + def is_ref(self, arg_type): + """ + Determine if the `interface_parser.Type` should be treated as a + reference in the wrapper. + """ + return arg_type.typename.name not in self.ignore_namespace and \ + arg_type.typename.name not in self.not_ptr_type and \ + arg_type.is_ref + + +class FormatMixin: + """Mixin to provide formatting utilities.""" + def _clean_class_name(self, instantiated_class): + """Reformatted the C++ class name to fit Matlab defined naming + standards + """ + if len(instantiated_class.ctors) != 0: + return instantiated_class.ctors[0].name + + return instantiated_class.name + + def _format_type_name(self, + type_name, + separator='::', + include_namespace=True, + constructor=False, + method=False): + """ + Args: + type_name: an interface_parser.Typename to reformat + separator: the statement to add between namespaces and typename + include_namespace: whether to include namespaces when reformatting + constructor: if the typename will be in a constructor + method: if the typename will be in a method + + Raises: + constructor and method cannot both be true + """ + if constructor and method: + raise ValueError( + 'Constructor and method parameters cannot both be True') + + formatted_type_name = '' + name = type_name.name + + if include_namespace: + for namespace in type_name.namespaces: + if name not in self.ignore_namespace and namespace != '': + formatted_type_name += namespace + separator + + if constructor: + formatted_type_name += self.data_type.get(name) or name + elif method: + formatted_type_name += self.data_type_param.get(name) or name + else: + formatted_type_name += name + + if separator == "::": # C++ + templates = [] + for idx in range(len(type_name.instantiations)): + template = '{}'.format( + self._format_type_name(type_name.instantiations[idx], + include_namespace=include_namespace, + constructor=constructor, + method=method)) + templates.append(template) + + if len(templates) > 0: # If there are no templates + formatted_type_name += '<{}>'.format(','.join(templates)) + + else: + for idx in range(len(type_name.instantiations)): + formatted_type_name += '{}'.format( + self._format_type_name(type_name.instantiations[idx], + separator=separator, + include_namespace=False, + constructor=constructor, + method=method)) + + return formatted_type_name + + def _format_return_type(self, + return_type, + include_namespace=False, + separator="::"): + """Format return_type. + + Args: + return_type: an interface_parser.ReturnType to reformat + include_namespace: whether to include namespaces when reformatting + """ + return_wrap = '' + + if self._return_count(return_type) == 1: + return_wrap = self._format_type_name( + return_type.type1.typename, + separator=separator, + include_namespace=include_namespace) + else: + return_wrap = 'pair< {type1}, {type2} >'.format( + type1=self._format_type_name( + return_type.type1.typename, + separator=separator, + include_namespace=include_namespace), + type2=self._format_type_name( + return_type.type2.typename, + separator=separator, + include_namespace=include_namespace)) + + return return_wrap + + def _format_class_name(self, instantiated_class, separator=''): + """Format a template_instantiator.InstantiatedClass name.""" + if instantiated_class.parent == '': + parent_full_ns = [''] + else: + parent_full_ns = instantiated_class.parent.full_namespaces() + # class_name = instantiated_class.parent.name + # + # if class_name != '': + # class_name += separator + # + # class_name += instantiated_class.name + parentname = "".join([separator + x + for x in parent_full_ns]) + separator + + class_name = parentname[2 * len(separator):] + + class_name += instantiated_class.name + + return class_name + + def _format_static_method(self, static_method, separator=''): + """Example: + + gtsamPoint3.staticFunction + """ + method = '' + + if isinstance(static_method, parser.StaticMethod): + method += "".join([separator + x for x in static_method.parent.namespaces()]) + \ + separator + static_method.parent.name + separator + + return method[2 * len(separator):] + + def _format_instance_method(self, instance_method, separator=''): + """Example: + + gtsamPoint3.staticFunction + """ + method = '' + + if isinstance(instance_method, instantiator.InstantiatedMethod): + method_list = [ + separator + x + for x in instance_method.parent.parent.full_namespaces() + ] + method += "".join(method_list) + separator + + method += instance_method.parent.name + separator + method += instance_method.original.name + method += "<" + instance_method.instantiations.to_cpp() + ">" + + return method[2 * len(separator):] + + def _format_global_method(self, static_method, separator=''): + """Example: + + gtsamPoint3.staticFunction + """ + method = '' + + if isinstance(static_method, parser.GlobalFunction): + method += "".join([separator + x for x in static_method.parent.full_namespaces()]) + \ + separator + + return method[2 * len(separator):] diff --git a/gtwrap/matlab_wrapper/templates.py b/gtwrap/matlab_wrapper/templates.py new file mode 100644 index 000000000..7aaf8f487 --- /dev/null +++ b/gtwrap/matlab_wrapper/templates.py @@ -0,0 +1,166 @@ +import textwrap + + +class WrapperTemplate: + """Class to encapsulate string templates for use in wrapper generation""" + boost_headers = textwrap.dedent(""" + #include + #include + #include + """) + + typdef_collectors = textwrap.dedent('''\ + typedef std::set*> Collector_{class_name}; + static Collector_{class_name} collector_{class_name}; + ''') + + delete_obj = textwrap.indent(textwrap.dedent('''\ + {{ for(Collector_{class_name}::iterator iter = collector_{class_name}.begin(); + iter != collector_{class_name}.end(); ) {{ + delete *iter; + collector_{class_name}.erase(iter++); + anyDeleted = true; + }} }} + '''), + prefix=' ') + + delete_all_objects = textwrap.dedent(''' + void _deleteAllObjects() + {{ + mstream mout; + std::streambuf *outbuf = std::cout.rdbuf(&mout);\n + bool anyDeleted = false; + {delete_objs} + 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); + }} + ''') + + rtti_register = textwrap.dedent('''\ + void _{module_name}_RTTIRegister() {{ + const mxArray *alreadyCreated = mexGetVariablePtr("global", "gtsam_{module_name}_rttiRegistry_created"); + if(!alreadyCreated) {{ + std::map types; + + {rtti_classes} + + mxArray *registry = mexGetVariable("global", "gtsamwrap_rttiRegistry"); + if(!registry) + registry = mxCreateStructMatrix(1, 1, 0, NULL); + typedef std::pair StringPair; + for(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); + }} + }} + ''') + + collector_function_upcast_from_void = textwrap.dedent('''\ + void {class_name}_upcastFromVoid_{id}(int nargout, mxArray *out[], int nargin, const mxArray *in[]) {{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr<{cpp_name}> 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<{cpp_name}>(*asVoid)); + *reinterpret_cast(mxGetData(out[0])) = self; + }}\n + ''') + + class_serialize_method = textwrap.dedent('''\ + function varargout = string_serialize(this, varargin) + % STRING_SERIALIZE usage: string_serialize() : returns string + % Doxygen can be found at https://gtsam.org/doxygen/ + if length(varargin) == 0 + varargout{{1}} = {wrapper}({wrapper_id}, this, varargin{{:}}); + else + error('Arguments do not match any overload of function {class_name}.string_serialize'); + end + end\n + function sobj = saveobj(obj) + % SAVEOBJ Saves the object to a matlab-readable format + sobj = obj.string_serialize(); + end + ''') + + collector_function_serialize = textwrap.indent(textwrap.dedent("""\ + typedef boost::shared_ptr<{full_name}> Shared; + checkArguments("string_serialize",nargout,nargin-1,0); + Shared obj = unwrap_shared_ptr<{full_name}>(in[0], "ptr_{namespace}{class_name}"); + ostringstream out_archive_stream; + boost::archive::text_oarchive out_archive(out_archive_stream); + out_archive << *obj; + out[0] = wrap< string >(out_archive_stream.str()); + """), + prefix=' ') + + collector_function_deserialize = textwrap.indent(textwrap.dedent("""\ + typedef boost::shared_ptr<{full_name}> Shared; + checkArguments("{namespace}{class_name}.string_deserialize",nargout,nargin,1); + string serialized = unwrap< string >(in[0]); + istringstream in_archive_stream(serialized); + boost::archive::text_iarchive in_archive(in_archive_stream); + Shared output(new {full_name}()); + in_archive >> *output; + out[0] = wrap_shared_ptr(output,"{namespace}.{class_name}", false); + """), + prefix=' ') + + mex_function = textwrap.dedent(''' + void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) + {{ + mstream mout; + std::streambuf *outbuf = std::cout.rdbuf(&mout);\n + _{module_name}_RTTIRegister();\n + int id = unwrap(in[0]);\n + try {{ + switch(id) {{ + {cases} }} + }} catch(const std::exception& e) {{ + mexErrMsgTxt(("Exception from gtsam:\\n" + std::string(e.what()) + "\\n").c_str()); + }}\n + std::cout.rdbuf(outbuf); + }} + ''') + + collector_function_shared_return = textwrap.indent(textwrap.dedent('''\ + {{ + boost::shared_ptr<{name}> shared({shared_obj}); + out[{id}] = wrap_shared_ptr(shared,"{name}"); + }}{new_line}'''), + prefix=' ') + + matlab_deserialize = textwrap.indent(textwrap.dedent("""\ + function varargout = string_deserialize(varargin) + % STRING_DESERIALIZE usage: string_deserialize() : returns {class_name} + % Doxygen can be found at https://gtsam.org/doxygen/ + if length(varargin) == 1 + varargout{{1}} = {wrapper}({id}, varargin{{:}}); + else + error('Arguments do not match any overload of function {class_name}.string_deserialize'); + end + end\n + function obj = loadobj(sobj) + % LOADOBJ Saves the object to a matlab-readable format + obj = {class_name}.string_deserialize(sobj); + end + """), + prefix=' ') diff --git a/gtwrap/matlab_wrapper.py b/gtwrap/matlab_wrapper/wrapper.py similarity index 68% rename from gtwrap/matlab_wrapper.py rename to gtwrap/matlab_wrapper/wrapper.py index de6221bbc..b040d2731 100755 --- a/gtwrap/matlab_wrapper.py +++ b/gtwrap/matlab_wrapper/wrapper.py @@ -7,16 +7,19 @@ that Matlab's MEX compiler can use. import os import os.path as osp -import sys import textwrap from functools import partial, reduce from typing import Dict, Iterable, List, Union +from loguru import logger + import gtwrap.interface_parser as parser import gtwrap.template_instantiator as instantiator +from gtwrap.matlab_wrapper.mixins import CheckMixin, FormatMixin +from gtwrap.matlab_wrapper.templates import WrapperTemplate -class MatlabWrapper(object): +class MatlabWrapper(CheckMixin, FormatMixin): """ Wrap the given C++ code into Matlab. Attributes @@ -25,89 +28,75 @@ class MatlabWrapper(object): top_module_namespace: C++ namespace for the top module (default '') ignore_classes: A list of classes to ignore (default []) """ - # Map the data type to its Matlab class. - # Found in Argument.cpp in old wrapper - data_type = { - 'string': 'char', - 'char': 'char', - 'unsigned char': 'unsigned char', - 'Vector': 'double', - 'Matrix': 'double', - 'int': 'numeric', - 'size_t': 'numeric', - 'bool': 'logical' - } - # Map the data type into the type used in Matlab methods. - # Found in matlab.h in old wrapper - data_type_param = { - 'string': 'char', - 'char': 'char', - 'unsigned char': 'unsigned char', - 'size_t': 'int', - 'int': 'int', - 'double': 'double', - 'Point2': 'double', - 'Point3': 'double', - 'Vector': 'double', - 'Matrix': 'double', - 'bool': 'bool' - } - # Methods that should not be wrapped directly - whitelist = ['serializable', 'serialize'] - # Methods that should be ignored - ignore_methods = ['pickle'] - # Datatypes that do not need to be checked in methods - not_check_type = [] # type: list - # Data types that are primitive types - not_ptr_type = ['int', 'double', 'bool', 'char', 'unsigned char', 'size_t'] - # Ignore the namespace for these datatypes - ignore_namespace = ['Matrix', 'Vector', 'Point2', 'Point3'] - # The amount of times the wrapper has created a call to geometry_wrapper - wrapper_id = 0 - # Map each wrapper id to what its collector function namespace, class, type, and string format - wrapper_map = {} - # Set of all the includes in the namespace - includes = {} # type: Dict[parser.Include, int] - # Set of all classes in the namespace - classes = [ - ] # type: List[Union[parser.Class, instantiator.InstantiatedClass]] - classes_elems = { - } # type: Dict[Union[parser.Class, instantiator.InstantiatedClass], int] - # Id for ordering global functions in the wrapper - global_function_id = 0 - # Files and their content - content = [] # type: List[str] - - # Ensure the template file is always picked up from the correct directory. - dir_path = osp.dirname(osp.realpath(__file__)) - with open(osp.join(dir_path, "matlab_wrapper.tpl")) as f: - wrapper_file_header = f.read() - def __init__(self, module_name, top_module_namespace='', ignore_classes=()): + super().__init__() + self.module_name = module_name self.top_module_namespace = top_module_namespace self.ignore_classes = ignore_classes self.verbose = False - def _debug(self, message): - if not self.verbose: - return - print(message, file=sys.stderr) + # Map the data type to its Matlab class. + # Found in Argument.cpp in old wrapper + self.data_type = { + 'string': 'char', + 'char': 'char', + 'unsigned char': 'unsigned char', + 'Vector': 'double', + 'Matrix': 'double', + 'int': 'numeric', + 'size_t': 'numeric', + 'bool': 'logical' + } + # Map the data type into the type used in Matlab methods. + # Found in matlab.h in old wrapper + self.data_type_param = { + 'string': 'char', + 'char': 'char', + 'unsigned char': 'unsigned char', + 'size_t': 'int', + 'int': 'int', + 'double': 'double', + 'Point2': 'double', + 'Point3': 'double', + 'Vector': 'double', + 'Matrix': 'double', + 'bool': 'bool' + } + # The amount of times the wrapper has created a call to geometry_wrapper + self.wrapper_id = 0 + # Map each wrapper id to its collector function namespace, class, type, and string format + self.wrapper_map: Dict = {} + # Set of all the includes in the namespace + self.includes: List[parser.Include] = [] + # Set of all classes in the namespace + self.classes: List[Union[parser.Class, + instantiator.InstantiatedClass]] = [] + self.classes_elems: Dict[Union[parser.Class, + instantiator.InstantiatedClass], + int] = {} + # Id for ordering global functions in the wrapper + self.global_function_id = 0 + # Files and their content + self.content: List[str] = [] - def _add_include(self, include): - self.includes[include] = 0 + # Ensure the template file is always picked up from the correct directory. + dir_path = osp.dirname(osp.realpath(__file__)) + with open(osp.join(dir_path, "matlab_wrapper.tpl")) as f: + self.wrapper_file_headers = f.read() - def _add_class(self, instantiated_class): + def add_class(self, instantiated_class): + """Add `instantiated_class` to the list of classes.""" if self.classes_elems.get(instantiated_class) is None: self.classes_elems[instantiated_class] = 0 self.classes.append(instantiated_class) def _update_wrapper_id(self, collector_function=None, id_diff=0): - """Get and define wrapper ids. - + """ + Get and define wrapper ids. Generates the map of id -> collector function. Args: @@ -150,34 +139,6 @@ class MatlabWrapper(object): """ return x + '\n' + ('' if y == '' else ' ') + y - def _is_shared_ptr(self, arg_type): - """ - Determine if the `interface_parser.Type` should be treated as a - shared pointer in the wrapper. - """ - return arg_type.is_shared_ptr or ( - arg_type.typename.name not in self.not_ptr_type - and arg_type.typename.name not in self.ignore_namespace - and arg_type.typename.name != 'string') - - def _is_ptr(self, arg_type): - """ - Determine if the `interface_parser.Type` should be treated as a - raw pointer in the wrapper. - """ - return arg_type.is_ptr or ( - arg_type.typename.name not in self.not_ptr_type - and arg_type.typename.name not in self.ignore_namespace - and arg_type.typename.name != 'string') - - def _is_ref(self, arg_type): - """Determine if the interface_parser.Type should be treated as a - reference in the wrapper. - """ - return arg_type.typename.name not in self.ignore_namespace and \ - arg_type.typename.name not in self.not_ptr_type and \ - arg_type.is_ref - def _group_methods(self, methods): """Group overloaded methods together""" method_map = {} @@ -190,181 +151,10 @@ class MatlabWrapper(object): method_map[method.name] = len(method_out) method_out.append([method]) else: - self._debug("[_group_methods] Merging {} with {}".format( - method_index, method.name)) method_out[method_index].append(method) return method_out - def _clean_class_name(self, instantiated_class): - """Reformatted the C++ class name to fit Matlab defined naming - standards - """ - if len(instantiated_class.ctors) != 0: - return instantiated_class.ctors[0].name - - return instantiated_class.name - - @classmethod - def _format_type_name(cls, - type_name, - separator='::', - include_namespace=True, - constructor=False, - method=False): - """ - Args: - type_name: an interface_parser.Typename to reformat - separator: the statement to add between namespaces and typename - include_namespace: whether to include namespaces when reformatting - constructor: if the typename will be in a constructor - method: if the typename will be in a method - - Raises: - constructor and method cannot both be true - """ - if constructor and method: - raise Exception( - 'Constructor and method parameters cannot both be True') - - formatted_type_name = '' - name = type_name.name - - if include_namespace: - for namespace in type_name.namespaces: - if name not in cls.ignore_namespace and namespace != '': - formatted_type_name += namespace + separator - - #self._debug("formatted_ns: {}, ns: {}".format(formatted_type_name, type_name.namespaces)) - if constructor: - formatted_type_name += cls.data_type.get(name) or name - elif method: - formatted_type_name += cls.data_type_param.get(name) or name - else: - formatted_type_name += name - - if separator == "::": # C++ - templates = [] - for idx in range(len(type_name.instantiations)): - template = '{}'.format( - cls._format_type_name(type_name.instantiations[idx], - include_namespace=include_namespace, - constructor=constructor, - method=method)) - templates.append(template) - - if len(templates) > 0: # If there are no templates - formatted_type_name += '<{}>'.format(','.join(templates)) - - else: - for idx in range(len(type_name.instantiations)): - formatted_type_name += '{}'.format( - cls._format_type_name(type_name.instantiations[idx], - separator=separator, - include_namespace=False, - constructor=constructor, - method=method)) - - return formatted_type_name - - @classmethod - def _format_return_type(cls, - return_type, - include_namespace=False, - separator="::"): - """Format return_type. - - Args: - return_type: an interface_parser.ReturnType to reformat - include_namespace: whether to include namespaces when reformatting - """ - return_wrap = '' - - if cls._return_count(return_type) == 1: - return_wrap = cls._format_type_name( - return_type.type1.typename, - separator=separator, - include_namespace=include_namespace) - else: - return_wrap = 'pair< {type1}, {type2} >'.format( - type1=cls._format_type_name( - return_type.type1.typename, - separator=separator, - include_namespace=include_namespace), - type2=cls._format_type_name( - return_type.type2.typename, - separator=separator, - include_namespace=include_namespace)) - - return return_wrap - - def _format_class_name(self, instantiated_class, separator=''): - """Format a template_instantiator.InstantiatedClass name.""" - if instantiated_class.parent == '': - parent_full_ns = [''] - else: - parent_full_ns = instantiated_class.parent.full_namespaces() - # class_name = instantiated_class.parent.name - # - # if class_name != '': - # class_name += separator - # - # class_name += instantiated_class.name - parentname = "".join([separator + x - for x in parent_full_ns]) + separator - - class_name = parentname[2 * len(separator):] - - class_name += instantiated_class.name - - return class_name - - def _format_static_method(self, static_method, separator=''): - """Example: - - gtsamPoint3.staticFunction - """ - method = '' - - if isinstance(static_method, parser.StaticMethod): - method += "".join([separator + x for x in static_method.parent.namespaces()]) + \ - separator + static_method.parent.name + separator - - return method[2 * len(separator):] - - def _format_instance_method(self, instance_method, separator=''): - """Example: - - gtsamPoint3.staticFunction - """ - method = '' - - if isinstance(instance_method, instantiator.InstantiatedMethod): - method_list = [ - separator + x - for x in instance_method.parent.parent.full_namespaces() - ] - method += "".join(method_list) + separator - - method += instance_method.parent.name + separator - method += instance_method.original.name - method += "<" + instance_method.instantiations.to_cpp() + ">" - - return method[2 * len(separator):] - - def _format_global_method(self, static_method, separator=''): - """Example: - - gtsamPoint3.staticFunction - """ - method = '' - - if isinstance(static_method, parser.GlobalFunction): - method += "".join([separator + x for x in static_method.parent.full_namespaces()]) + \ - separator - - return method[2 * len(separator):] - def _wrap_args(self, args): """Wrap an interface_parser.ArgumentList into a list of arguments. @@ -520,7 +310,7 @@ class MatlabWrapper(object): if params != '': params += ',' - if self._is_ref(arg.ctype): # and not constructor: + if self.is_ref(arg.ctype): # and not constructor: ctype_camel = self._format_type_name(arg.ctype.typename, separator='') body_args += textwrap.indent(textwrap.dedent('''\ @@ -531,7 +321,7 @@ class MatlabWrapper(object): id=arg_id)), prefix=' ') - elif (self._is_shared_ptr(arg.ctype) or self._is_ptr(arg.ctype)) and \ + elif (self.is_shared_ptr(arg.ctype) or self.is_ptr(arg.ctype)) and \ arg.ctype.typename.name not in self.ignore_namespace: if arg.ctype.is_shared_ptr: call_type = arg.ctype.is_shared_ptr @@ -665,22 +455,13 @@ class MatlabWrapper(object): return comment - def generate_matlab_wrapper(self): - """Generate the C++ file for the wrapper.""" - file_name = self._wrapper_name() + '.cpp' - - wrapper_file = self.wrapper_file_header - - return file_name, wrapper_file - def wrap_method(self, methods): - """Wrap methods in the body of a class.""" + """ + Wrap methods in the body of a class. + """ if not isinstance(methods, list): methods = [methods] - # for method in methods: - # output = '' - return '' def wrap_methods(self, methods, global_funcs=False, global_ns=None): @@ -697,10 +478,6 @@ class MatlabWrapper(object): continue if global_funcs: - self._debug("[wrap_methods] wrapping: {}..{}={}".format( - method[0].parent.name, method[0].name, - type(method[0].parent.name))) - method_text = self.wrap_global_function(method) self.content.append(("".join([ '+' + x + '/' for x in global_ns.full_namespaces()[1:] @@ -838,11 +615,6 @@ class MatlabWrapper(object): base_obj = '' - if has_parent: - self._debug("class: {} ns: {}".format( - parent_name, - self._format_class_name(inst_class.parent, separator="."))) - if has_parent: base_obj = ' obj = obj@{parent_name}(uint64(5139824614673773682), base_ptr);'.format( parent_name=parent_name) @@ -850,9 +622,6 @@ class MatlabWrapper(object): if base_obj: base_obj = '\n' + base_obj - self._debug("class: {}, name: {}".format( - inst_class.name, self._format_class_name(inst_class, - separator="."))) methods_wrap += textwrap.indent(textwrap.dedent('''\ else error('Arguments do not match any overload of {class_name_doc} constructor'); @@ -1101,27 +870,12 @@ class MatlabWrapper(object): prefix=" ") if serialize: - method_text += textwrap.indent(textwrap.dedent("""\ - function varargout = string_deserialize(varargin) - % STRING_DESERIALIZE usage: string_deserialize() : returns {class_name} - % Doxygen can be found at https://gtsam.org/doxygen/ - if length(varargin) == 1 - varargout{{1}} = {wrapper}({id}, varargin{{:}}); - else - error('Arguments do not match any overload of function {class_name}.string_deserialize'); - end - end\n - function obj = loadobj(sobj) - % LOADOBJ Saves the object to a matlab-readable format - obj = {class_name}.string_deserialize(sobj); - end - """).format( + method_text += WrapperTemplate.matlab_deserialize.format( class_name=namespace_name + '.' + instantiated_class.name, wrapper=self._wrapper_name(), id=self._update_wrapper_id( (namespace_name, instantiated_class, 'string_deserialize', - 'deserialize'))), - prefix=' ') + 'deserialize'))) return method_text @@ -1213,33 +967,32 @@ class MatlabWrapper(object): return file_name + '.m', content_text - def wrap_namespace(self, namespace, parent=()): + def wrap_namespace(self, namespace): """Wrap a namespace by wrapping all of its components. Args: namespace: the interface_parser.namespace instance of the namespace parent: parent namespace """ - test_output = '' namespaces = namespace.full_namespaces() inner_namespace = namespace.name != '' wrapped = [] - self._debug("wrapping ns: {}, parent: {}".format( - namespace.full_namespaces(), parent)) - matlab_wrapper = self.generate_matlab_wrapper() - self.content.append((matlab_wrapper[0], matlab_wrapper[1])) + cpp_filename = self._wrapper_name() + '.cpp' + self.content.append((cpp_filename, self.wrapper_file_headers)) current_scope = [] namespace_scope = [] for element in namespace.content: if isinstance(element, parser.Include): - self._add_include(element) + self.includes.append(element) + elif isinstance(element, parser.Namespace): - self.wrap_namespace(element, namespaces) + self.wrap_namespace(element) + elif isinstance(element, instantiator.InstantiatedClass): - self._add_class(element) + self.add_class(element) if inner_namespace: class_text = self.wrap_instantiated_class( @@ -1265,7 +1018,7 @@ class MatlabWrapper(object): if isinstance(func, parser.GlobalFunction) ] - test_output += self.wrap_methods(all_funcs, True, global_ns=namespace) + self.wrap_methods(all_funcs, True, global_ns=namespace) return wrapped @@ -1277,16 +1030,12 @@ class MatlabWrapper(object): """Wrap the collector function which returns a shared pointer.""" new_line = '\n' if new_line else '' - return textwrap.indent(textwrap.dedent('''\ - {{ - boost::shared_ptr<{name}> shared({shared_obj}); - out[{id}] = wrap_shared_ptr(shared,"{name}"); - }}{new_line}''').format(name=self._format_type_name( - return_type_name, include_namespace=False), - shared_obj=shared_obj, - id=func_id, - new_line=new_line), - prefix=' ') + return WrapperTemplate.collector_function_shared_return.format( + name=self._format_type_name(return_type_name, + include_namespace=False), + shared_obj=shared_obj, + id=func_id, + new_line=new_line) def wrap_collector_function_return_types(self, return_type, func_id): """ @@ -1296,7 +1045,7 @@ class MatlabWrapper(object): pair_value = 'first' if func_id == 0 else 'second' new_line = '\n' if func_id == 0 else '' - if self._is_shared_ptr(return_type) or self._is_ptr(return_type): + if self.is_shared_ptr(return_type) or self.is_ptr(return_type): shared_obj = 'pairResult.' + pair_value if not (return_type.is_shared_ptr or return_type.is_ptr): @@ -1355,16 +1104,12 @@ class MatlabWrapper(object): method_name = self._format_static_method(method, '::') method_name += method.name - if "MeasureRange" in method_name: - self._debug("method: {}, method: {}, inst: {}".format( - method_name, method.name, method.parent.to_cpp())) - obj = ' ' if return_1_name == 'void' else '' obj += '{}{}({})'.format(obj_start, method_name, params) if return_1_name != 'void': if return_count == 1: - if self._is_shared_ptr(return_1) or self._is_ptr(return_1): + if self.is_shared_ptr(return_1) or self.is_ptr(return_1): sep_method_name = partial(self._format_type_name, return_1.typename, include_namespace=True) @@ -1377,12 +1122,6 @@ class MatlabWrapper(object): shared_obj = '{obj},"{method_name_sep}"'.format( obj=obj, method_name_sep=sep_method_name('.')) else: - self._debug("Non-PTR: {}, {}".format( - return_1, type(return_1))) - self._debug("Inner type is: {}, {}".format( - return_1.typename.name, sep_method_name('.'))) - self._debug("Inner type instantiations: {}".format( - return_1.typename.instantiations)) method_name_sep_dot = sep_method_name('.') shared_obj_template = 'boost::make_shared<{method_name_sep_col}>({obj}),' \ '"{method_name_sep_dot}"' @@ -1417,16 +1156,8 @@ class MatlabWrapper(object): """ Add function to upcast type from void type. """ - return textwrap.dedent('''\ - void {class_name}_upcastFromVoid_{id}(int nargout, mxArray *out[], int nargin, const mxArray *in[]) {{ - mexAtExit(&_deleteAllObjects); - typedef boost::shared_ptr<{cpp_name}> 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<{cpp_name}>(*asVoid)); - *reinterpret_cast(mxGetData(out[0])) = self; - }}\n - ''').format(class_name=class_name, cpp_name=cpp_name, id=func_id) + return WrapperTemplate.collector_function_upcast_from_void.format( + class_name=class_name, cpp_name=cpp_name, id=func_id) def generate_collector_function(self, func_id): """ @@ -1610,158 +1341,109 @@ class MatlabWrapper(object): else: next_case = None - mex_function = textwrap.dedent(''' - void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) - {{ - mstream mout; - std::streambuf *outbuf = std::cout.rdbuf(&mout);\n - _{module_name}_RTTIRegister();\n - int id = unwrap(in[0]);\n - try {{ - switch(id) {{ - {cases} }} - }} catch(const std::exception& e) {{ - mexErrMsgTxt(("Exception from gtsam:\\n" + std::string(e.what()) + "\\n").c_str()); - }}\n - std::cout.rdbuf(outbuf); - }} - ''').format(module_name=self.module_name, cases=cases) + mex_function = WrapperTemplate.mex_function.format( + module_name=self.module_name, cases=cases) return mex_function - def generate_wrapper(self, namespace): - """Generate the c++ wrapper.""" - # Includes - wrapper_file = self.wrapper_file_header + textwrap.dedent(""" - #include - #include - #include \n - """) - - assert namespace - - includes_list = sorted(list(self.includes.keys()), - key=lambda include: include.header) - - # Check the number of includes. - # If no includes, do nothing, if 1 then just append newline. - # if more than one, concatenate them with newlines. - if len(includes_list) == 0: - pass - elif len(includes_list) == 1: - wrapper_file += (str(includes_list[0]) + '\n') + def get_class_name(self, cls): + """Get the name of the class `cls` taking template instantiations into account.""" + if cls.instantiations: + class_name_sep = cls.name else: - wrapper_file += reduce(lambda x, y: str(x) + '\n' + str(y), - includes_list) - wrapper_file += '\n' + class_name_sep = cls.to_cpp() - typedef_instances = '\n' - typedef_collectors = '' + class_name = self._format_class_name(cls) + + return class_name, class_name_sep + + def generate_preamble(self): + """ + Generate the preamble of the wrapper file, which includes + the Boost exports, typedefs for collectors, and + the _deleteAllObjects and _RTTIRegister functions. + """ + delete_objs = '' + typedef_instances = [] boost_class_export_guid = '' - delete_objs = textwrap.dedent('''\ - void _deleteAllObjects() - { - mstream mout; - std::streambuf *outbuf = std::cout.rdbuf(&mout);\n - bool anyDeleted = false; - ''') - rtti_reg_start = textwrap.dedent('''\ - void _{module_name}_RTTIRegister() {{ - const mxArray *alreadyCreated = mexGetVariablePtr("global", "gtsam_{module_name}_rttiRegistry_created"); - if(!alreadyCreated) {{ - std::map types; - ''').format(module_name=self.module_name) - rtti_reg_mid = '' - rtti_reg_end = textwrap.indent( - textwrap.dedent(''' - mxArray *registry = mexGetVariable("global", "gtsamwrap_rttiRegistry"); - if(!registry) - registry = mxCreateStructMatrix(1, 1, 0, NULL); - typedef std::pair StringPair; - for(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); - '''), - prefix=' ') + ' \n' + textwrap.dedent('''\ - 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); - } - } - ''') - ptr_ctor_frag = '' + typedef_collectors = '' + rtti_classes = '' for cls in self.classes: - uninstantiated_name = "::".join( - cls.namespaces()[1:]) + "::" + cls.name - self._debug("Cls: {} -> {}".format(cls.name, uninstantiated_name)) - + # Check if class is in ignore list. + # If so, then skip + uninstantiated_name = "::".join(cls.namespaces()[1:] + [cls.name]) if uninstantiated_name in self.ignore_classes: - self._debug("Ignoring: {} -> {}".format( - cls.name, uninstantiated_name)) continue - def _has_serialization(cls): - for m in cls.methods: - if m.name in self.whitelist: - return True - return False + class_name, class_name_sep = self.get_class_name(cls) + # If a class has instantiations, then declare the typedef for each instance if cls.instantiations: cls_insts = '' - for i, inst in enumerate(cls.instantiations): if i != 0: cls_insts += ', ' cls_insts += self._format_type_name(inst) - typedef_instances += 'typedef {original_class_name} {class_name_sep};\n' \ + typedef_instances.append('typedef {original_class_name} {class_name_sep};' \ .format(original_class_name=cls.to_cpp(), - class_name_sep=cls.name) + class_name_sep=cls.name)) - class_name_sep = cls.name - class_name = self._format_class_name(cls) + # Get the Boost exports for serialization + if cls.original.namespaces() and self._has_serialization(cls): + boost_class_export_guid += 'BOOST_CLASS_EXPORT_GUID({}, "{}");\n'.format( + class_name_sep, class_name) - if len(cls.original.namespaces()) > 1 and _has_serialization( - cls): - boost_class_export_guid += 'BOOST_CLASS_EXPORT_GUID({}, "{}");\n'.format( - class_name_sep, class_name) - else: - class_name_sep = cls.to_cpp() - class_name = self._format_class_name(cls) + # Typedef and declare the collector objects. + typedef_collectors += WrapperTemplate.typdef_collectors.format( + class_name_sep=class_name_sep, class_name=class_name) - if len(cls.original.namespaces()) > 1 and _has_serialization( - cls): - boost_class_export_guid += 'BOOST_CLASS_EXPORT_GUID({}, "{}");\n'.format( - class_name_sep, class_name) - - typedef_collectors += textwrap.dedent('''\ - typedef std::set*> Collector_{class_name}; - static Collector_{class_name} collector_{class_name}; - ''').format(class_name_sep=class_name_sep, class_name=class_name) - delete_objs += textwrap.indent(textwrap.dedent('''\ - {{ for(Collector_{class_name}::iterator iter = collector_{class_name}.begin(); - iter != collector_{class_name}.end(); ) {{ - delete *iter; - collector_{class_name}.erase(iter++); - anyDeleted = true; - }} }} - ''').format(class_name=class_name), - prefix=' ') + # Generate the _deleteAllObjects method + delete_objs += WrapperTemplate.delete_obj.format( + class_name=class_name) if cls.is_virtual: - rtti_reg_mid += ' types.insert(std::make_pair(typeid({}).name(), "{}"));\n' \ + class_name, class_name_sep = self.get_class_name(cls) + rtti_classes += ' types.insert(std::make_pair(typeid({}).name(), "{}"));\n' \ .format(class_name_sep, class_name) + # Generate the typedef instances string + typedef_instances = "\n".join(typedef_instances) + + # Generate the full deleteAllObjects function + delete_all_objs = WrapperTemplate.delete_all_objects.format( + delete_objs=delete_objs) + + # Generate the full RTTIRegister function + rtti_register = WrapperTemplate.rtti_register.format( + module_name=self.module_name, rtti_classes=rtti_classes) + + return typedef_instances, boost_class_export_guid, \ + typedef_collectors, delete_all_objs, rtti_register + + def generate_wrapper(self, namespace): + """Generate the c++ wrapper.""" + assert namespace, "Namespace if empty" + + # Generate the header includes + includes_list = sorted(self.includes, + key=lambda include: include.header) + includes = textwrap.dedent("""\ + {wrapper_file_headers} + {boost_headers} + {includes_list} + """).format(wrapper_file_headers=self.wrapper_file_headers.strip(), + boost_headers=WrapperTemplate.boost_headers, + includes_list='\n'.join(map(str, includes_list))) + + preamble = self.generate_preamble() + typedef_instances, boost_class_export_guid, \ + typedef_collectors, delete_all_objs, \ + rtti_register = preamble + + ptr_ctor_frag = '' set_next_case = False for idx in range(self.wrapper_id): @@ -1784,24 +1466,20 @@ class MatlabWrapper(object): ptr_ctor_frag += self.wrap_collector_function_upcast_from_void( id_val[1].name, idx, id_val[1].to_cpp()) - wrapper_file += textwrap.dedent('''\ + wrapper_file = textwrap.dedent('''\ + {includes} {typedef_instances} {boost_class_export_guid} {typedefs_collectors} - {delete_objs} 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); - }}\n + {delete_all_objs} {rtti_register} {pointer_constructor_fragment}{mex_function}''') \ - .format(typedef_instances=typedef_instances, + .format(includes=includes, + typedef_instances=typedef_instances, boost_class_export_guid=boost_class_export_guid, typedefs_collectors=typedef_collectors, - delete_objs=delete_objs, - rtti_register=rtti_reg_start + rtti_reg_mid + rtti_reg_end, + delete_all_objs=delete_all_objs, + rtti_register=rtti_register, pointer_constructor_fragment=ptr_ctor_frag, mex_function=self.mex_function()) @@ -1815,23 +1493,10 @@ class MatlabWrapper(object): wrapper_id = self._update_wrapper_id( (namespace_name, inst_class, 'string_serialize', 'serialize')) - return textwrap.dedent('''\ - function varargout = string_serialize(this, varargin) - % STRING_SERIALIZE usage: string_serialize() : returns string - % Doxygen can be found at https://gtsam.org/doxygen/ - if length(varargin) == 0 - varargout{{1}} = {wrapper}({wrapper_id}, this, varargin{{:}}); - else - error('Arguments do not match any overload of function {class_name}.string_serialize'); - end - end\n - function sobj = saveobj(obj) - % SAVEOBJ Saves the object to a matlab-readable format - sobj = obj.string_serialize(); - end - ''').format(wrapper=self._wrapper_name(), - wrapper_id=wrapper_id, - class_name=namespace_name + '.' + class_name) + return WrapperTemplate.class_serialize_method.format( + wrapper=self._wrapper_name(), + wrapper_id=wrapper_id, + class_name=namespace_name + '.' + class_name) def wrap_collector_function_serialize(self, class_name, @@ -1840,18 +1505,8 @@ class MatlabWrapper(object): """ Wrap the serizalize collector function. """ - return textwrap.indent(textwrap.dedent("""\ - typedef boost::shared_ptr<{full_name}> Shared; - checkArguments("string_serialize",nargout,nargin-1,0); - Shared obj = unwrap_shared_ptr<{full_name}>(in[0], "ptr_{namespace}{class_name}"); - ostringstream out_archive_stream; - boost::archive::text_oarchive out_archive(out_archive_stream); - out_archive << *obj; - out[0] = wrap< string >(out_archive_stream.str()); - """).format(class_name=class_name, - full_name=full_name, - namespace=namespace), - prefix=' ') + return WrapperTemplate.collector_function_serialize.format( + class_name=class_name, full_name=full_name, namespace=namespace) def wrap_collector_function_deserialize(self, class_name, @@ -1860,87 +1515,85 @@ class MatlabWrapper(object): """ Wrap the deserizalize collector function. """ - return textwrap.indent(textwrap.dedent("""\ - typedef boost::shared_ptr<{full_name}> Shared; - checkArguments("{namespace}{class_name}.string_deserialize",nargout,nargin,1); - string serialized = unwrap< string >(in[0]); - istringstream in_archive_stream(serialized); - boost::archive::text_iarchive in_archive(in_archive_stream); - Shared output(new {full_name}()); - in_archive >> *output; - out[0] = wrap_shared_ptr(output,"{namespace}.{class_name}", false); - """).format(class_name=class_name, - full_name=full_name, - namespace=namespace), - prefix=' ') + return WrapperTemplate.collector_function_deserialize.format( + class_name=class_name, full_name=full_name, namespace=namespace) - def wrap(self, content): + def generate_content(self, cc_content, path): + """ + Generate files and folders from matlab wrapper content. + + Args: + cc_content: The content to generate formatted as + (file_name, file_content) or + (folder_name, [(file_name, file_content)]) + path: The path to the files parent folder within the main folder + """ + for c in cc_content: + if isinstance(c, list): + if len(c) == 0: + continue + + path_to_folder = osp.join(path, c[0][0]) + + if not osp.isdir(path_to_folder): + try: + os.makedirs(path_to_folder, exist_ok=True) + except OSError: + pass + + for sub_content in c: + self.generate_content(sub_content[1], path_to_folder) + + elif isinstance(c[1], list): + path_to_folder = osp.join(path, c[0]) + + if not osp.isdir(path_to_folder): + try: + os.makedirs(path_to_folder, exist_ok=True) + except OSError: + pass + for sub_content in c[1]: + path_to_file = osp.join(path_to_folder, sub_content[0]) + with open(path_to_file, 'w') as f: + f.write(sub_content[1]) + else: + path_to_file = osp.join(path, c[0]) + + if not osp.isdir(path_to_file): + try: + os.mkdir(path) + except OSError: + pass + + with open(path_to_file, 'w') as f: + f.write(c[1]) + + def wrap(self, files, path): """High level function to wrap the project.""" - # Parse the contents of the interface file - parsed_result = parser.Module.parseString(content) - # Instantiate the module - module = instantiator.instantiate_namespace(parsed_result) - self.wrap_namespace(module) - self.generate_wrapper(module) + modules = {} + for file in files: + with open(file, 'r') as f: + content = f.read() + + # Parse the contents of the interface file + parsed_result = parser.Module.parseString(content) + # print(parsed_result) + + # Instantiate the module + module = instantiator.instantiate_namespace(parsed_result) + + if module.name in modules: + modules[module. + name].content[0].content += module.content[0].content + else: + modules[module.name] = module + + for module in modules.values(): + # Wrap the full namespace + self.wrap_namespace(module) + self.generate_wrapper(module) + + # Generate the corresponding .m and .cpp files + self.generate_content(self.content, path) return self.content - - -def generate_content(cc_content, path, verbose=False): - """ - Generate files and folders from matlab wrapper content. - - Args: - cc_content: The content to generate formatted as - (file_name, file_content) or - (folder_name, [(file_name, file_content)]) - path: The path to the files parent folder within the main folder - """ - def _debug(message): - if not verbose: - return - print(message, file=sys.stderr) - - for c in cc_content: - if isinstance(c, list): - if len(c) == 0: - continue - _debug("c object: {}".format(c[0][0])) - path_to_folder = osp.join(path, c[0][0]) - - if not os.path.isdir(path_to_folder): - try: - os.makedirs(path_to_folder, exist_ok=True) - except OSError: - pass - - for sub_content in c: - _debug("sub object: {}".format(sub_content[1][0][0])) - generate_content(sub_content[1], path_to_folder) - - elif isinstance(c[1], list): - path_to_folder = osp.join(path, c[0]) - - _debug("[generate_content_global]: {}".format(path_to_folder)) - if not os.path.isdir(path_to_folder): - try: - os.makedirs(path_to_folder, exist_ok=True) - except OSError: - pass - for sub_content in c[1]: - path_to_file = osp.join(path_to_folder, sub_content[0]) - _debug("[generate_global_method]: {}".format(path_to_file)) - with open(path_to_file, 'w') as f: - f.write(sub_content[1]) - else: - path_to_file = osp.join(path, c[0]) - - _debug("[generate_content]: {}".format(path_to_file)) - if not os.path.isdir(path_to_file): - try: - os.mkdir(path) - except OSError: - pass - - with open(path_to_file, 'w') as f: - f.write(c[1]) diff --git a/gtwrap/pybind_wrapper.py b/gtwrap/pybind_wrapper.py index 0e1b3c7ea..40571263a 100755 --- a/gtwrap/pybind_wrapper.py +++ b/gtwrap/pybind_wrapper.py @@ -13,6 +13,7 @@ Author: Duy Nguyen Ta, Fan Jiang, Matthew Sklar, Varun Agrawal, and Frank Dellae # pylint: disable=too-many-arguments, too-many-instance-attributes, no-self-use, no-else-return, too-many-arguments, unused-format-string-argument, line-too-long import re +from pathlib import Path import gtwrap.interface_parser as parser import gtwrap.template_instantiator as instantiator @@ -32,7 +33,7 @@ class PybindWrapper: self.top_module_namespaces = top_module_namespaces self.use_boost = use_boost self.ignore_classes = ignore_classes - self._serializing_classes = list() + self._serializing_classes = [] self.module_template = module_template self.python_keywords = [ 'lambda', 'False', 'def', 'if', 'raise', 'None', 'del', 'import', @@ -160,7 +161,7 @@ class PybindWrapper: 'self->print', 'py::scoped_ostream_redirect output; self->print') - # Make __repr__() call print() internally + # Make __repr__() call .print() internally ret += '''{prefix}.def("__repr__", [](const {cpp_class}& self{opt_comma}{args_signature_with_names}){{ gtsam::RedirectCout redirect; @@ -557,8 +558,15 @@ class PybindWrapper: ) return wrapped, includes - def wrap(self, content): - """Wrap the code in the interface file.""" + def wrap_file(self, content, module_name=None, submodules=None): + """ + Wrap the code in the interface file. + + Args: + content: The contents of the interface file. + module_name: The name of the module. + submodules: List of other interface file names that should be linked to. + """ # Parse the contents of the interface file module = parser.Module.parseString(content) # Instantiate all templates @@ -574,23 +582,74 @@ class PybindWrapper: if ',' in cpp_class: new_name = re.sub("[,:<> ]", "", cpp_class) boost_class_export += "typedef {cpp_class} {new_name};\n".format( # noqa - cpp_class=cpp_class, - new_name=new_name, - ) + cpp_class=cpp_class, new_name=new_name) + boost_class_export += "BOOST_CLASS_EXPORT({new_name})\n".format( new_name=new_name, ) + # Reset the serializing classes list + self._serializing_classes = [] + holder_type = "PYBIND11_DECLARE_HOLDER_TYPE(TYPE_PLACEHOLDER_DONOTUSE, " \ "{shared_ptr_type}::shared_ptr);" include_boost = "#include " if self.use_boost else "" + submodules_init = [] + + if submodules is not None: + module_def = "PYBIND11_MODULE({0}, m_)".format(module_name) + + for idx, submodule in enumerate(submodules): + submodules[idx] = "void {0}(py::module_ &);".format(submodule) + submodules_init.append("{0}(m_);".format(submodule)) + + else: + module_def = "void {0}(py::module_ &m_)".format(module_name) + submodules = [] + return self.module_template.format( include_boost=include_boost, - module_name=self.module_name, + module_def=module_def, + module_name=module_name, includes=includes, holder_type=holder_type.format( shared_ptr_type=('boost' if self.use_boost else 'std')) if self.use_boost else "", wrapped_namespace=wrapped_namespace, boost_class_export=boost_class_export, + submodules="\n".join(submodules), + submodules_init="\n".join(submodules_init), ) + + def wrap(self, sources, main_output): + """ + Wrap all the source interface files. + + Args: + sources: List of all interface files. + main_output: The name for the main module. + """ + main_module = sources[0] + submodules = [] + for source in sources[1:]: + filename = Path(source).name + module_name = Path(source).stem + # Read in the complete interface (.i) file + with open(source, "r") as f: + content = f.read() + submodules.append(module_name) + cc_content = self.wrap_file(content, module_name=module_name) + + # Generate the C++ code which Pybind11 will use. + with open(filename.replace(".i", ".cpp"), "w") as f: + f.write(cc_content) + + with open(main_module, "r") as f: + content = f.read() + cc_content = self.wrap_file(content, + module_name=self.module_name, + submodules=submodules) + + # Generate the C++ code which Pybind11 will use. + with open(main_output, "w") as f: + f.write(cc_content) diff --git a/gtwrap/template_instantiator.py b/gtwrap/template_instantiator.py index c47424648..87729cfa6 100644 --- a/gtwrap/template_instantiator.py +++ b/gtwrap/template_instantiator.py @@ -4,7 +4,7 @@ import itertools from copy import deepcopy -from typing import List +from typing import Iterable, List import gtwrap.interface_parser as parser @@ -29,12 +29,13 @@ def instantiate_type(ctype: parser.Type, ctype = deepcopy(ctype) # Check if the return type has template parameters - if len(ctype.typename.instantiations) > 0: + if ctype.typename.instantiations: for idx, instantiation in enumerate(ctype.typename.instantiations): if instantiation.name in template_typenames: template_idx = template_typenames.index(instantiation.name) - ctype.typename.instantiations[idx] = instantiations[ - template_idx] + ctype.typename.instantiations[ + idx] = instantiations[ # type: ignore + template_idx] return ctype @@ -212,7 +213,9 @@ class InstantiatedMethod(parser.Method): void func(X x, Y y); } """ - def __init__(self, original, instantiations: List[parser.Typename] = ''): + def __init__(self, + original, + instantiations: Iterable[parser.Typename] = ()): self.original = original self.instantiations = instantiations self.template = '' @@ -278,7 +281,7 @@ class InstantiatedClass(parser.Class): self.original = original self.instantiations = instantiations - self.template = '' + self.template = None self.is_virtual = original.is_virtual self.parent_class = original.parent_class self.parent = original.parent @@ -318,7 +321,7 @@ class InstantiatedClass(parser.Class): self.methods = [] for method in instantiated_methods: if not method.template: - self.methods.append(InstantiatedMethod(method, '')) + self.methods.append(InstantiatedMethod(method, ())) else: instantiations = [] # Get all combinations of template parameters @@ -342,9 +345,9 @@ class InstantiatedClass(parser.Class): ) def __repr__(self): - return "{virtual} class {name} [{cpp_class}]: {parent_class}\n"\ - "{ctors}\n{static_methods}\n{methods}".format( - virtual="virtual" if self.is_virtual else '', + return "{virtual}Class {cpp_class} : {parent_class}\n"\ + "{ctors}\n{static_methods}\n{methods}\n{operators}".format( + virtual="virtual " if self.is_virtual else '', name=self.name, cpp_class=self.to_cpp(), parent_class=self.parent, diff --git a/scripts/matlab_wrap.py b/scripts/matlab_wrap.py index be6043947..0f6664a63 100644 --- a/scripts/matlab_wrap.py +++ b/scripts/matlab_wrap.py @@ -1,5 +1,4 @@ #!/usr/bin/env python3 - """ Helper script to wrap C++ to Matlab. This script is installed via CMake to the user's binary directory @@ -7,19 +6,24 @@ and invoked during the wrapping by CMake. """ import argparse -import os import sys -from gtwrap.matlab_wrapper import MatlabWrapper, generate_content +from gtwrap.matlab_wrapper import MatlabWrapper if __name__ == "__main__": arg_parser = argparse.ArgumentParser( formatter_class=argparse.ArgumentDefaultsHelpFormatter) - arg_parser.add_argument("--src", type=str, required=True, + arg_parser.add_argument("--src", + type=str, + required=True, help="Input interface .h file.") - arg_parser.add_argument("--module_name", type=str, required=True, + arg_parser.add_argument("--module_name", + type=str, + required=True, help="Name of the C++ class being wrapped.") - arg_parser.add_argument("--out", type=str, required=True, + arg_parser.add_argument("--out", + type=str, + required=True, help="Name of the output folder.") arg_parser.add_argument( "--top_module_namespaces", @@ -33,28 +37,22 @@ if __name__ == "__main__": "`.Class` of the corresponding C++ `ns1::ns2::ns3::Class`" ", and `from import ns4` gives you access to a Python " "`ns4.Class` of the C++ `ns1::ns2::ns3::ns4::Class`. ") - arg_parser.add_argument("--ignore", - nargs='*', - type=str, - help="A space-separated list of classes to ignore. " - "Class names must include their full namespaces.") + arg_parser.add_argument( + "--ignore", + nargs='*', + type=str, + help="A space-separated list of classes to ignore. " + "Class names must include their full namespaces.") args = arg_parser.parse_args() top_module_namespaces = args.top_module_namespaces.split("::") if top_module_namespaces[0]: top_module_namespaces = [''] + top_module_namespaces - with open(args.src, 'r') as f: - content = f.read() - - if not os.path.exists(args.src): - os.mkdir(args.src) - - print("Ignoring classes: {}".format(args.ignore), file=sys.stderr) + print("[MatlabWrapper] Ignoring classes: {}".format(args.ignore), file=sys.stderr) wrapper = MatlabWrapper(module_name=args.module_name, top_module_namespace=top_module_namespaces, ignore_classes=args.ignore) - cc_content = wrapper.wrap(content) - - generate_content(cc_content, args.out) + sources = args.src.split(';') + cc_content = wrapper.wrap(sources, path=args.out) diff --git a/scripts/pybind_wrap.py b/scripts/pybind_wrap.py index 7f2f8d419..c82a1d24c 100644 --- a/scripts/pybind_wrap.py +++ b/scripts/pybind_wrap.py @@ -67,10 +67,6 @@ def main(): if top_module_namespaces[0]: top_module_namespaces = [''] + top_module_namespaces - # Read in the complete interface (.i) file - with open(args.src, "r") as f: - content = f.read() - with open(args.template, "r") as f: template_content = f.read() @@ -83,11 +79,8 @@ def main(): ) # Wrap the code and get back the cpp/cc code. - cc_content = wrapper.wrap(content) - - # Generate the C++ code which Pybind11 will use. - with open(args.out, "w") as f: - f.write(cc_content) + sources = args.src.split(';') + wrapper.wrap(sources, args.out) if __name__ == "__main__": diff --git a/templates/pybind_wrapper.tpl.example b/templates/pybind_wrapper.tpl.example index bf5b33490..485aa8d00 100644 --- a/templates/pybind_wrapper.tpl.example +++ b/templates/pybind_wrapper.tpl.example @@ -7,7 +7,7 @@ #include #include #include "gtsam/base/serialization.h" -#include "gtsam/nonlinear/utilities.h" // for RedirectCout. +#include "gtsam/base/utilities.h" // for RedirectCout. {includes} #include @@ -22,9 +22,13 @@ using namespace std; namespace py = pybind11; -PYBIND11_MODULE({module_name}, m_) {{ +{submodules} + +{module_def} {{ m_.doc() = "pybind11 wrapper of {module_name}"; +{submodules_init} + {wrapped_namespace} #include "python/specializations.h" diff --git a/tests/expected/matlab/+gtsam/Class1.m b/tests/expected/matlab/+gtsam/Class1.m new file mode 100644 index 000000000..00dd5ca74 --- /dev/null +++ b/tests/expected/matlab/+gtsam/Class1.m @@ -0,0 +1,36 @@ +%class Class1, see Doxygen page for details +%at https://gtsam.org/doxygen/ +% +%-------Constructors------- +%Class1() +% +classdef Class1 < handle + properties + ptr_gtsamClass1 = 0 + end + methods + function obj = Class1(varargin) + if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682) + my_ptr = varargin{2}; + multiple_files_wrapper(0, my_ptr); + elseif nargin == 0 + my_ptr = multiple_files_wrapper(1); + else + error('Arguments do not match any overload of gtsam.Class1 constructor'); + end + obj.ptr_gtsamClass1 = my_ptr; + end + + function delete(obj) + multiple_files_wrapper(2, obj.ptr_gtsamClass1); + 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/tests/expected/matlab/+gtsam/Class2.m b/tests/expected/matlab/+gtsam/Class2.m new file mode 100644 index 000000000..93279e156 --- /dev/null +++ b/tests/expected/matlab/+gtsam/Class2.m @@ -0,0 +1,36 @@ +%class Class2, see Doxygen page for details +%at https://gtsam.org/doxygen/ +% +%-------Constructors------- +%Class2() +% +classdef Class2 < handle + properties + ptr_gtsamClass2 = 0 + end + methods + function obj = Class2(varargin) + if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682) + my_ptr = varargin{2}; + multiple_files_wrapper(3, my_ptr); + elseif nargin == 0 + my_ptr = multiple_files_wrapper(4); + else + error('Arguments do not match any overload of gtsam.Class2 constructor'); + end + obj.ptr_gtsamClass2 = my_ptr; + end + + function delete(obj) + multiple_files_wrapper(5, obj.ptr_gtsamClass2); + 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/tests/expected/matlab/+gtsam/ClassA.m b/tests/expected/matlab/+gtsam/ClassA.m new file mode 100644 index 000000000..3210e93c6 --- /dev/null +++ b/tests/expected/matlab/+gtsam/ClassA.m @@ -0,0 +1,36 @@ +%class ClassA, see Doxygen page for details +%at https://gtsam.org/doxygen/ +% +%-------Constructors------- +%ClassA() +% +classdef ClassA < handle + properties + ptr_gtsamClassA = 0 + end + methods + function obj = ClassA(varargin) + if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682) + my_ptr = varargin{2}; + multiple_files_wrapper(6, my_ptr); + elseif nargin == 0 + my_ptr = multiple_files_wrapper(7); + else + error('Arguments do not match any overload of gtsam.ClassA constructor'); + end + obj.ptr_gtsamClassA = my_ptr; + end + + function delete(obj) + multiple_files_wrapper(8, obj.ptr_gtsamClassA); + 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/tests/expected/matlab/class_wrapper.cpp b/tests/expected/matlab/class_wrapper.cpp index e644ac00f..fab9c1450 100644 --- a/tests/expected/matlab/class_wrapper.cpp +++ b/tests/expected/matlab/class_wrapper.cpp @@ -7,7 +7,6 @@ #include - typedef Fun FunDouble; typedef PrimitiveRef PrimitiveRefDouble; typedef MyVector<3> MyVector3; @@ -16,7 +15,6 @@ typedef MultipleTemplates MultipleTemplatesIntDouble; typedef MultipleTemplates MultipleTemplatesIntFloat; typedef MyFactor MyFactorPosePoint2; - typedef std::set*> Collector_FunRange; static Collector_FunRange collector_FunRange; typedef std::set*> Collector_FunDouble; @@ -38,6 +36,7 @@ static Collector_ForwardKinematics collector_ForwardKinematics; typedef std::set*> Collector_MyFactorPosePoint2; static Collector_MyFactorPosePoint2 collector_MyFactorPosePoint2; + void _deleteAllObjects() { mstream mout; @@ -104,6 +103,7 @@ void _deleteAllObjects() collector_MyFactorPosePoint2.erase(iter++); anyDeleted = true; } } + if(anyDeleted) cout << "WARNING: Wrap modules with variables in the workspace have been reloaded due to\n" @@ -117,24 +117,29 @@ void _class_RTTIRegister() { if(!alreadyCreated) { std::map types; + + mxArray *registry = mexGetVariable("global", "gtsamwrap_rttiRegistry"); if(!registry) registry = mxCreateStructMatrix(1, 1, 0, NULL); typedef std::pair StringPair; for(const StringPair& rtti_matlab: types) { int fieldId = mxAddField(registry, rtti_matlab.first.c_str()); - if(fieldId < 0) + 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) + 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) + if(mexPutVariable("global", "gtsam_geometry_rttiRegistry_created", newAlreadyCreated) != 0) { mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); + } mxDestroyArray(newAlreadyCreated); } } diff --git a/tests/expected/matlab/functions_wrapper.cpp b/tests/expected/matlab/functions_wrapper.cpp index ae7f49c41..d0f0f8ca6 100644 --- a/tests/expected/matlab/functions_wrapper.cpp +++ b/tests/expected/matlab/functions_wrapper.cpp @@ -5,38 +5,11 @@ #include #include -#include -typedef Fun FunDouble; -typedef PrimitiveRef PrimitiveRefDouble; -typedef MyVector<3> MyVector3; -typedef MyVector<12> MyVector12; -typedef MultipleTemplates MultipleTemplatesIntDouble; -typedef MultipleTemplates MultipleTemplatesIntFloat; -typedef MyFactor MyFactorPosePoint2; -typedef std::set*> Collector_FunRange; -static Collector_FunRange collector_FunRange; -typedef std::set*> Collector_FunDouble; -static Collector_FunDouble collector_FunDouble; -typedef std::set*> Collector_Test; -static Collector_Test collector_Test; -typedef std::set*> Collector_PrimitiveRefDouble; -static Collector_PrimitiveRefDouble collector_PrimitiveRefDouble; -typedef std::set*> Collector_MyVector3; -static Collector_MyVector3 collector_MyVector3; -typedef std::set*> Collector_MyVector12; -static Collector_MyVector12 collector_MyVector12; -typedef std::set*> Collector_MultipleTemplatesIntDouble; -static Collector_MultipleTemplatesIntDouble collector_MultipleTemplatesIntDouble; -typedef std::set*> Collector_MultipleTemplatesIntFloat; -static Collector_MultipleTemplatesIntFloat collector_MultipleTemplatesIntFloat; -typedef std::set*> Collector_ForwardKinematics; -static Collector_ForwardKinematics collector_ForwardKinematics; -typedef std::set*> Collector_MyFactorPosePoint2; -static Collector_MyFactorPosePoint2 collector_MyFactorPosePoint2; + void _deleteAllObjects() { @@ -44,66 +17,7 @@ void _deleteAllObjects() std::streambuf *outbuf = std::cout.rdbuf(&mout); bool anyDeleted = false; - { for(Collector_FunRange::iterator iter = collector_FunRange.begin(); - iter != collector_FunRange.end(); ) { - delete *iter; - collector_FunRange.erase(iter++); - anyDeleted = true; - } } - { for(Collector_FunDouble::iterator iter = collector_FunDouble.begin(); - iter != collector_FunDouble.end(); ) { - delete *iter; - collector_FunDouble.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_PrimitiveRefDouble::iterator iter = collector_PrimitiveRefDouble.begin(); - iter != collector_PrimitiveRefDouble.end(); ) { - delete *iter; - collector_PrimitiveRefDouble.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MyVector3::iterator iter = collector_MyVector3.begin(); - iter != collector_MyVector3.end(); ) { - delete *iter; - collector_MyVector3.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MyVector12::iterator iter = collector_MyVector12.begin(); - iter != collector_MyVector12.end(); ) { - delete *iter; - collector_MyVector12.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MultipleTemplatesIntDouble::iterator iter = collector_MultipleTemplatesIntDouble.begin(); - iter != collector_MultipleTemplatesIntDouble.end(); ) { - delete *iter; - collector_MultipleTemplatesIntDouble.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MultipleTemplatesIntFloat::iterator iter = collector_MultipleTemplatesIntFloat.begin(); - iter != collector_MultipleTemplatesIntFloat.end(); ) { - delete *iter; - collector_MultipleTemplatesIntFloat.erase(iter++); - anyDeleted = true; - } } - { for(Collector_ForwardKinematics::iterator iter = collector_ForwardKinematics.begin(); - iter != collector_ForwardKinematics.end(); ) { - delete *iter; - collector_ForwardKinematics.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" @@ -117,24 +31,29 @@ void _functions_RTTIRegister() { if(!alreadyCreated) { std::map types; + + mxArray *registry = mexGetVariable("global", "gtsamwrap_rttiRegistry"); if(!registry) registry = mxCreateStructMatrix(1, 1, 0, NULL); typedef std::pair StringPair; for(const StringPair& rtti_matlab: types) { int fieldId = mxAddField(registry, rtti_matlab.first.c_str()); - if(fieldId < 0) + 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) + 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) + if(mexPutVariable("global", "gtsam_geometry_rttiRegistry_created", newAlreadyCreated) != 0) { mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); + } mxDestroyArray(newAlreadyCreated); } } diff --git a/tests/expected/matlab/geometry_wrapper.cpp b/tests/expected/matlab/geometry_wrapper.cpp index 4d8a7c789..81631390c 100644 --- a/tests/expected/matlab/geometry_wrapper.cpp +++ b/tests/expected/matlab/geometry_wrapper.cpp @@ -5,112 +5,25 @@ #include #include -#include #include #include -typedef Fun FunDouble; -typedef PrimitiveRef PrimitiveRefDouble; -typedef MyVector<3> MyVector3; -typedef MyVector<12> MyVector12; -typedef MultipleTemplates MultipleTemplatesIntDouble; -typedef MultipleTemplates MultipleTemplatesIntFloat; -typedef MyFactor MyFactorPosePoint2; BOOST_CLASS_EXPORT_GUID(gtsam::Point2, "gtsamPoint2"); BOOST_CLASS_EXPORT_GUID(gtsam::Point3, "gtsamPoint3"); -typedef std::set*> Collector_FunRange; -static Collector_FunRange collector_FunRange; -typedef std::set*> Collector_FunDouble; -static Collector_FunDouble collector_FunDouble; -typedef std::set*> Collector_Test; -static Collector_Test collector_Test; -typedef std::set*> Collector_PrimitiveRefDouble; -static Collector_PrimitiveRefDouble collector_PrimitiveRefDouble; -typedef std::set*> Collector_MyVector3; -static Collector_MyVector3 collector_MyVector3; -typedef std::set*> Collector_MyVector12; -static Collector_MyVector12 collector_MyVector12; -typedef std::set*> Collector_MultipleTemplatesIntDouble; -static Collector_MultipleTemplatesIntDouble collector_MultipleTemplatesIntDouble; -typedef std::set*> Collector_MultipleTemplatesIntFloat; -static Collector_MultipleTemplatesIntFloat collector_MultipleTemplatesIntFloat; -typedef std::set*> Collector_ForwardKinematics; -static Collector_ForwardKinematics collector_ForwardKinematics; -typedef std::set*> Collector_MyFactorPosePoint2; -static Collector_MyFactorPosePoint2 collector_MyFactorPosePoint2; typedef std::set*> Collector_gtsamPoint2; static Collector_gtsamPoint2 collector_gtsamPoint2; typedef std::set*> Collector_gtsamPoint3; static Collector_gtsamPoint3 collector_gtsamPoint3; + void _deleteAllObjects() { mstream mout; std::streambuf *outbuf = std::cout.rdbuf(&mout); bool anyDeleted = false; - { for(Collector_FunRange::iterator iter = collector_FunRange.begin(); - iter != collector_FunRange.end(); ) { - delete *iter; - collector_FunRange.erase(iter++); - anyDeleted = true; - } } - { for(Collector_FunDouble::iterator iter = collector_FunDouble.begin(); - iter != collector_FunDouble.end(); ) { - delete *iter; - collector_FunDouble.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_PrimitiveRefDouble::iterator iter = collector_PrimitiveRefDouble.begin(); - iter != collector_PrimitiveRefDouble.end(); ) { - delete *iter; - collector_PrimitiveRefDouble.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MyVector3::iterator iter = collector_MyVector3.begin(); - iter != collector_MyVector3.end(); ) { - delete *iter; - collector_MyVector3.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MyVector12::iterator iter = collector_MyVector12.begin(); - iter != collector_MyVector12.end(); ) { - delete *iter; - collector_MyVector12.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MultipleTemplatesIntDouble::iterator iter = collector_MultipleTemplatesIntDouble.begin(); - iter != collector_MultipleTemplatesIntDouble.end(); ) { - delete *iter; - collector_MultipleTemplatesIntDouble.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MultipleTemplatesIntFloat::iterator iter = collector_MultipleTemplatesIntFloat.begin(); - iter != collector_MultipleTemplatesIntFloat.end(); ) { - delete *iter; - collector_MultipleTemplatesIntFloat.erase(iter++); - anyDeleted = true; - } } - { for(Collector_ForwardKinematics::iterator iter = collector_ForwardKinematics.begin(); - iter != collector_ForwardKinematics.end(); ) { - delete *iter; - collector_ForwardKinematics.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MyFactorPosePoint2::iterator iter = collector_MyFactorPosePoint2.begin(); - iter != collector_MyFactorPosePoint2.end(); ) { - delete *iter; - collector_MyFactorPosePoint2.erase(iter++); - anyDeleted = true; - } } { for(Collector_gtsamPoint2::iterator iter = collector_gtsamPoint2.begin(); iter != collector_gtsamPoint2.end(); ) { delete *iter; @@ -123,6 +36,7 @@ void _deleteAllObjects() collector_gtsamPoint3.erase(iter++); anyDeleted = true; } } + if(anyDeleted) cout << "WARNING: Wrap modules with variables in the workspace have been reloaded due to\n" @@ -136,24 +50,29 @@ void _geometry_RTTIRegister() { if(!alreadyCreated) { std::map types; + + mxArray *registry = mexGetVariable("global", "gtsamwrap_rttiRegistry"); if(!registry) registry = mxCreateStructMatrix(1, 1, 0, NULL); typedef std::pair StringPair; for(const StringPair& rtti_matlab: types) { int fieldId = mxAddField(registry, rtti_matlab.first.c_str()); - if(fieldId < 0) + 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) + 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) + if(mexPutVariable("global", "gtsam_geometry_rttiRegistry_created", newAlreadyCreated) != 0) { mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); + } mxDestroyArray(newAlreadyCreated); } } diff --git a/tests/expected/matlab/inheritance_wrapper.cpp b/tests/expected/matlab/inheritance_wrapper.cpp index 077df4830..8e61ac8c6 100644 --- a/tests/expected/matlab/inheritance_wrapper.cpp +++ b/tests/expected/matlab/inheritance_wrapper.cpp @@ -5,47 +5,11 @@ #include #include -#include -#include -#include -typedef Fun FunDouble; -typedef PrimitiveRef PrimitiveRefDouble; -typedef MyVector<3> MyVector3; -typedef MyVector<12> MyVector12; -typedef MultipleTemplates MultipleTemplatesIntDouble; -typedef MultipleTemplates MultipleTemplatesIntFloat; -typedef MyFactor MyFactorPosePoint2; + typedef MyTemplate MyTemplatePoint2; typedef MyTemplate MyTemplateMatrix; -BOOST_CLASS_EXPORT_GUID(gtsam::Point2, "gtsamPoint2"); -BOOST_CLASS_EXPORT_GUID(gtsam::Point3, "gtsamPoint3"); - -typedef std::set*> Collector_FunRange; -static Collector_FunRange collector_FunRange; -typedef std::set*> Collector_FunDouble; -static Collector_FunDouble collector_FunDouble; -typedef std::set*> Collector_Test; -static Collector_Test collector_Test; -typedef std::set*> Collector_PrimitiveRefDouble; -static Collector_PrimitiveRefDouble collector_PrimitiveRefDouble; -typedef std::set*> Collector_MyVector3; -static Collector_MyVector3 collector_MyVector3; -typedef std::set*> Collector_MyVector12; -static Collector_MyVector12 collector_MyVector12; -typedef std::set*> Collector_MultipleTemplatesIntDouble; -static Collector_MultipleTemplatesIntDouble collector_MultipleTemplatesIntDouble; -typedef std::set*> Collector_MultipleTemplatesIntFloat; -static Collector_MultipleTemplatesIntFloat collector_MultipleTemplatesIntFloat; -typedef std::set*> Collector_ForwardKinematics; -static Collector_ForwardKinematics collector_ForwardKinematics; -typedef std::set*> Collector_MyFactorPosePoint2; -static Collector_MyFactorPosePoint2 collector_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_MyBase; static Collector_MyBase collector_MyBase; typedef std::set*> Collector_MyTemplatePoint2; @@ -55,84 +19,13 @@ static Collector_MyTemplateMatrix collector_MyTemplateMatrix; typedef std::set*> Collector_ForwardKinematicsFactor; static Collector_ForwardKinematicsFactor collector_ForwardKinematicsFactor; + void _deleteAllObjects() { mstream mout; std::streambuf *outbuf = std::cout.rdbuf(&mout); bool anyDeleted = false; - { for(Collector_FunRange::iterator iter = collector_FunRange.begin(); - iter != collector_FunRange.end(); ) { - delete *iter; - collector_FunRange.erase(iter++); - anyDeleted = true; - } } - { for(Collector_FunDouble::iterator iter = collector_FunDouble.begin(); - iter != collector_FunDouble.end(); ) { - delete *iter; - collector_FunDouble.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_PrimitiveRefDouble::iterator iter = collector_PrimitiveRefDouble.begin(); - iter != collector_PrimitiveRefDouble.end(); ) { - delete *iter; - collector_PrimitiveRefDouble.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MyVector3::iterator iter = collector_MyVector3.begin(); - iter != collector_MyVector3.end(); ) { - delete *iter; - collector_MyVector3.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MyVector12::iterator iter = collector_MyVector12.begin(); - iter != collector_MyVector12.end(); ) { - delete *iter; - collector_MyVector12.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MultipleTemplatesIntDouble::iterator iter = collector_MultipleTemplatesIntDouble.begin(); - iter != collector_MultipleTemplatesIntDouble.end(); ) { - delete *iter; - collector_MultipleTemplatesIntDouble.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MultipleTemplatesIntFloat::iterator iter = collector_MultipleTemplatesIntFloat.begin(); - iter != collector_MultipleTemplatesIntFloat.end(); ) { - delete *iter; - collector_MultipleTemplatesIntFloat.erase(iter++); - anyDeleted = true; - } } - { for(Collector_ForwardKinematics::iterator iter = collector_ForwardKinematics.begin(); - iter != collector_ForwardKinematics.end(); ) { - delete *iter; - collector_ForwardKinematics.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MyFactorPosePoint2::iterator iter = collector_MyFactorPosePoint2.begin(); - iter != collector_MyFactorPosePoint2.end(); ) { - delete *iter; - collector_MyFactorPosePoint2.erase(iter++); - anyDeleted = true; - } } - { 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_MyBase::iterator iter = collector_MyBase.begin(); iter != collector_MyBase.end(); ) { delete *iter; @@ -157,6 +50,7 @@ void _deleteAllObjects() collector_ForwardKinematicsFactor.erase(iter++); anyDeleted = true; } } + if(anyDeleted) cout << "WARNING: Wrap modules with variables in the workspace have been reloaded due to\n" @@ -169,42 +63,38 @@ void _inheritance_RTTIRegister() { const mxArray *alreadyCreated = mexGetVariablePtr("global", "gtsam_inheritance_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(MyTemplateMatrix).name(), "MyTemplateMatrix")); types.insert(std::make_pair(typeid(ForwardKinematicsFactor).name(), "ForwardKinematicsFactor")); + mxArray *registry = mexGetVariable("global", "gtsamwrap_rttiRegistry"); if(!registry) registry = mxCreateStructMatrix(1, 1, 0, NULL); typedef std::pair StringPair; for(const StringPair& rtti_matlab: types) { int fieldId = mxAddField(registry, rtti_matlab.first.c_str()); - if(fieldId < 0) + 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) + 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) + 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 MyBase_collectorInsertAndMakeBase_0(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); @@ -214,6 +104,15 @@ void MyBase_collectorInsertAndMakeBase_0(int nargout, mxArray *out[], int nargin collector_MyBase.insert(self); } +void MyBase_upcastFromVoid_1(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_2(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { typedef boost::shared_ptr Shared; @@ -227,19 +126,6 @@ void MyBase_deconstructor_2(int nargout, mxArray *out[], int nargin, const mxArr } } -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 MyTemplatePoint2_collectorInsertAndMakeBase_3(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); @@ -253,6 +139,15 @@ void MyTemplatePoint2_collectorInsertAndMakeBase_3(int nargout, mxArray *out[], *reinterpret_cast(mxGetData(out[0])) = new SharedBase(*self); } +void MyTemplatePoint2_upcastFromVoid_4(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_5(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); @@ -399,20 +294,6 @@ void MyTemplatePoint2_Level_18(int nargout, mxArray *out[], int nargin, const mx out[0] = wrap_shared_ptr(boost::make_shared>(MyTemplate::Level(K)),"MyTemplatePoint2", false); } -void gtsamPoint3_constructor_19(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 MyTemplateMatrix_collectorInsertAndMakeBase_19(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); @@ -426,6 +307,15 @@ void MyTemplateMatrix_collectorInsertAndMakeBase_19(int nargout, mxArray *out[], *reinterpret_cast(mxGetData(out[0])) = new SharedBase(*self); } +void MyTemplateMatrix_upcastFromVoid_20(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 MyTemplateMatrix_constructor_21(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); @@ -572,14 +462,6 @@ void MyTemplateMatrix_Level_34(int nargout, mxArray *out[], int nargin, const mx out[0] = wrap_shared_ptr(boost::make_shared>(MyTemplate::Level(K)),"MyTemplateMatrix", false); } -void Test_return_vector2_35(int nargout, mxArray *out[], int nargin, const mxArray *in[]) -{ - checkArguments("return_vector2",nargout,nargin-1,1); - auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); - Vector value = unwrap< Vector >(in[1]); - out[0] = wrap< Vector >(obj->return_vector2(value)); -} - void ForwardKinematicsFactor_collectorInsertAndMakeBase_35(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); @@ -593,6 +475,15 @@ void ForwardKinematicsFactor_collectorInsertAndMakeBase_35(int nargout, mxArray *reinterpret_cast(mxGetData(out[0])) = new SharedBase(*self); } +void ForwardKinematicsFactor_upcastFromVoid_36(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 ForwardKinematicsFactor_deconstructor_37(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { typedef boost::shared_ptr Shared; @@ -619,19 +510,19 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) try { switch(id) { case 0: - gtsamPoint2_collectorInsertAndMakeBase_0(nargout, out, nargin-1, in+1); + MyBase_collectorInsertAndMakeBase_0(nargout, out, nargin-1, in+1); break; case 1: - MyBase_collectorInsertAndMakeBase_0(nargout, out, nargin-1, in+1); + MyBase_upcastFromVoid_1(nargout, out, nargin-1, in+1); break; case 2: MyBase_deconstructor_2(nargout, out, nargin-1, in+1); break; case 3: - gtsamPoint2_deconstructor_3(nargout, out, nargin-1, in+1); + MyTemplatePoint2_collectorInsertAndMakeBase_3(nargout, out, nargin-1, in+1); break; case 4: - MyTemplatePoint2_collectorInsertAndMakeBase_3(nargout, out, nargin-1, in+1); + MyTemplatePoint2_upcastFromVoid_4(nargout, out, nargin-1, in+1); break; case 5: MyTemplatePoint2_constructor_5(nargout, out, nargin-1, in+1); @@ -676,10 +567,10 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) MyTemplatePoint2_Level_18(nargout, out, nargin-1, in+1); break; case 19: - gtsamPoint3_constructor_19(nargout, out, nargin-1, in+1); + MyTemplateMatrix_collectorInsertAndMakeBase_19(nargout, out, nargin-1, in+1); break; case 20: - MyTemplateMatrix_collectorInsertAndMakeBase_19(nargout, out, nargin-1, in+1); + MyTemplateMatrix_upcastFromVoid_20(nargout, out, nargin-1, in+1); break; case 21: MyTemplateMatrix_constructor_21(nargout, out, nargin-1, in+1); @@ -724,10 +615,10 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) MyTemplateMatrix_Level_34(nargout, out, nargin-1, in+1); break; case 35: - Test_return_vector2_35(nargout, out, nargin-1, in+1); + ForwardKinematicsFactor_collectorInsertAndMakeBase_35(nargout, out, nargin-1, in+1); break; case 36: - ForwardKinematicsFactor_collectorInsertAndMakeBase_35(nargout, out, nargin-1, in+1); + ForwardKinematicsFactor_upcastFromVoid_36(nargout, out, nargin-1, in+1); break; case 37: ForwardKinematicsFactor_deconstructor_37(nargout, out, nargin-1, in+1); diff --git a/tests/expected/matlab/multiple_files_wrapper.cpp b/tests/expected/matlab/multiple_files_wrapper.cpp new file mode 100644 index 000000000..66ab7ff73 --- /dev/null +++ b/tests/expected/matlab/multiple_files_wrapper.cpp @@ -0,0 +1,229 @@ +#include +#include + +#include +#include +#include + + + + + +typedef std::set*> Collector_gtsamClass1; +static Collector_gtsamClass1 collector_gtsamClass1; +typedef std::set*> Collector_gtsamClass2; +static Collector_gtsamClass2 collector_gtsamClass2; +typedef std::set*> Collector_gtsamClassA; +static Collector_gtsamClassA collector_gtsamClassA; + + +void _deleteAllObjects() +{ + mstream mout; + std::streambuf *outbuf = std::cout.rdbuf(&mout); + + bool anyDeleted = false; + { for(Collector_gtsamClass1::iterator iter = collector_gtsamClass1.begin(); + iter != collector_gtsamClass1.end(); ) { + delete *iter; + collector_gtsamClass1.erase(iter++); + anyDeleted = true; + } } + { for(Collector_gtsamClass2::iterator iter = collector_gtsamClass2.begin(); + iter != collector_gtsamClass2.end(); ) { + delete *iter; + collector_gtsamClass2.erase(iter++); + anyDeleted = true; + } } + { for(Collector_gtsamClassA::iterator iter = collector_gtsamClassA.begin(); + iter != collector_gtsamClassA.end(); ) { + delete *iter; + collector_gtsamClassA.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 _multiple_files_RTTIRegister() { + const mxArray *alreadyCreated = mexGetVariablePtr("global", "gtsam_multiple_files_rttiRegistry_created"); + if(!alreadyCreated) { + std::map types; + + + + mxArray *registry = mexGetVariable("global", "gtsamwrap_rttiRegistry"); + if(!registry) + registry = mxCreateStructMatrix(1, 1, 0, NULL); + typedef std::pair StringPair; + for(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 gtsamClass1_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_gtsamClass1.insert(self); +} + +void gtsamClass1_constructor_1(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + Shared *self = new Shared(new gtsam::Class1()); + collector_gtsamClass1.insert(self); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast (mxGetData(out[0])) = self; +} + +void gtsamClass1_deconstructor_2(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("delete_gtsamClass1",nargout,nargin,1); + Shared *self = *reinterpret_cast(mxGetData(in[0])); + Collector_gtsamClass1::iterator item; + item = collector_gtsamClass1.find(self); + if(item != collector_gtsamClass1.end()) { + delete self; + collector_gtsamClass1.erase(item); + } +} + +void gtsamClass2_collectorInsertAndMakeBase_3(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + Shared *self = *reinterpret_cast (mxGetData(in[0])); + collector_gtsamClass2.insert(self); +} + +void gtsamClass2_constructor_4(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + Shared *self = new Shared(new gtsam::Class2()); + collector_gtsamClass2.insert(self); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast (mxGetData(out[0])) = self; +} + +void gtsamClass2_deconstructor_5(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("delete_gtsamClass2",nargout,nargin,1); + Shared *self = *reinterpret_cast(mxGetData(in[0])); + Collector_gtsamClass2::iterator item; + item = collector_gtsamClass2.find(self); + if(item != collector_gtsamClass2.end()) { + delete self; + collector_gtsamClass2.erase(item); + } +} + +void gtsamClassA_collectorInsertAndMakeBase_6(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + Shared *self = *reinterpret_cast (mxGetData(in[0])); + collector_gtsamClassA.insert(self); +} + +void gtsamClassA_constructor_7(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + Shared *self = new Shared(new gtsam::ClassA()); + collector_gtsamClassA.insert(self); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast (mxGetData(out[0])) = self; +} + +void gtsamClassA_deconstructor_8(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("delete_gtsamClassA",nargout,nargin,1); + Shared *self = *reinterpret_cast(mxGetData(in[0])); + Collector_gtsamClassA::iterator item; + item = collector_gtsamClassA.find(self); + if(item != collector_gtsamClassA.end()) { + delete self; + collector_gtsamClassA.erase(item); + } +} + + +void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mstream mout; + std::streambuf *outbuf = std::cout.rdbuf(&mout); + + _multiple_files_RTTIRegister(); + + int id = unwrap(in[0]); + + try { + switch(id) { + case 0: + gtsamClass1_collectorInsertAndMakeBase_0(nargout, out, nargin-1, in+1); + break; + case 1: + gtsamClass1_constructor_1(nargout, out, nargin-1, in+1); + break; + case 2: + gtsamClass1_deconstructor_2(nargout, out, nargin-1, in+1); + break; + case 3: + gtsamClass2_collectorInsertAndMakeBase_3(nargout, out, nargin-1, in+1); + break; + case 4: + gtsamClass2_constructor_4(nargout, out, nargin-1, in+1); + break; + case 5: + gtsamClass2_deconstructor_5(nargout, out, nargin-1, in+1); + break; + case 6: + gtsamClassA_collectorInsertAndMakeBase_6(nargout, out, nargin-1, in+1); + break; + case 7: + gtsamClassA_constructor_7(nargout, out, nargin-1, in+1); + break; + case 8: + gtsamClassA_deconstructor_8(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/tests/expected/matlab/namespaces_wrapper.cpp b/tests/expected/matlab/namespaces_wrapper.cpp index 8f6e415e2..604ede5da 100644 --- a/tests/expected/matlab/namespaces_wrapper.cpp +++ b/tests/expected/matlab/namespaces_wrapper.cpp @@ -5,9 +5,6 @@ #include #include -#include -#include -#include #include #include #include @@ -15,51 +12,8 @@ #include #include -typedef Fun FunDouble; -typedef PrimitiveRef PrimitiveRefDouble; -typedef MyVector<3> MyVector3; -typedef MyVector<12> MyVector12; -typedef MultipleTemplates MultipleTemplatesIntDouble; -typedef MultipleTemplates MultipleTemplatesIntFloat; -typedef MyFactor MyFactorPosePoint2; -typedef MyTemplate MyTemplatePoint2; -typedef MyTemplate MyTemplateMatrix; -BOOST_CLASS_EXPORT_GUID(gtsam::Point2, "gtsamPoint2"); -BOOST_CLASS_EXPORT_GUID(gtsam::Point3, "gtsamPoint3"); -typedef std::set*> Collector_FunRange; -static Collector_FunRange collector_FunRange; -typedef std::set*> Collector_FunDouble; -static Collector_FunDouble collector_FunDouble; -typedef std::set*> Collector_Test; -static Collector_Test collector_Test; -typedef std::set*> Collector_PrimitiveRefDouble; -static Collector_PrimitiveRefDouble collector_PrimitiveRefDouble; -typedef std::set*> Collector_MyVector3; -static Collector_MyVector3 collector_MyVector3; -typedef std::set*> Collector_MyVector12; -static Collector_MyVector12 collector_MyVector12; -typedef std::set*> Collector_MultipleTemplatesIntDouble; -static Collector_MultipleTemplatesIntDouble collector_MultipleTemplatesIntDouble; -typedef std::set*> Collector_MultipleTemplatesIntFloat; -static Collector_MultipleTemplatesIntFloat collector_MultipleTemplatesIntFloat; -typedef std::set*> Collector_ForwardKinematics; -static Collector_ForwardKinematics collector_ForwardKinematics; -typedef std::set*> Collector_MyFactorPosePoint2; -static Collector_MyFactorPosePoint2 collector_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_MyBase; -static Collector_MyBase collector_MyBase; -typedef std::set*> Collector_MyTemplatePoint2; -static Collector_MyTemplatePoint2 collector_MyTemplatePoint2; -typedef std::set*> Collector_MyTemplateMatrix; -static Collector_MyTemplateMatrix collector_MyTemplateMatrix; -typedef std::set*> Collector_ForwardKinematicsFactor; -static Collector_ForwardKinematicsFactor collector_ForwardKinematicsFactor; typedef std::set*> Collector_ns1ClassA; static Collector_ns1ClassA collector_ns1ClassA; typedef std::set*> Collector_ns1ClassB; @@ -75,108 +29,13 @@ static Collector_ClassD collector_ClassD; typedef std::set*> Collector_gtsamValues; static Collector_gtsamValues collector_gtsamValues; + void _deleteAllObjects() { mstream mout; std::streambuf *outbuf = std::cout.rdbuf(&mout); bool anyDeleted = false; - { for(Collector_FunRange::iterator iter = collector_FunRange.begin(); - iter != collector_FunRange.end(); ) { - delete *iter; - collector_FunRange.erase(iter++); - anyDeleted = true; - } } - { for(Collector_FunDouble::iterator iter = collector_FunDouble.begin(); - iter != collector_FunDouble.end(); ) { - delete *iter; - collector_FunDouble.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_PrimitiveRefDouble::iterator iter = collector_PrimitiveRefDouble.begin(); - iter != collector_PrimitiveRefDouble.end(); ) { - delete *iter; - collector_PrimitiveRefDouble.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MyVector3::iterator iter = collector_MyVector3.begin(); - iter != collector_MyVector3.end(); ) { - delete *iter; - collector_MyVector3.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MyVector12::iterator iter = collector_MyVector12.begin(); - iter != collector_MyVector12.end(); ) { - delete *iter; - collector_MyVector12.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MultipleTemplatesIntDouble::iterator iter = collector_MultipleTemplatesIntDouble.begin(); - iter != collector_MultipleTemplatesIntDouble.end(); ) { - delete *iter; - collector_MultipleTemplatesIntDouble.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MultipleTemplatesIntFloat::iterator iter = collector_MultipleTemplatesIntFloat.begin(); - iter != collector_MultipleTemplatesIntFloat.end(); ) { - delete *iter; - collector_MultipleTemplatesIntFloat.erase(iter++); - anyDeleted = true; - } } - { for(Collector_ForwardKinematics::iterator iter = collector_ForwardKinematics.begin(); - iter != collector_ForwardKinematics.end(); ) { - delete *iter; - collector_ForwardKinematics.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MyFactorPosePoint2::iterator iter = collector_MyFactorPosePoint2.begin(); - iter != collector_MyFactorPosePoint2.end(); ) { - delete *iter; - collector_MyFactorPosePoint2.erase(iter++); - anyDeleted = true; - } } - { 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_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_MyTemplateMatrix::iterator iter = collector_MyTemplateMatrix.begin(); - iter != collector_MyTemplateMatrix.end(); ) { - delete *iter; - collector_MyTemplateMatrix.erase(iter++); - anyDeleted = true; - } } - { for(Collector_ForwardKinematicsFactor::iterator iter = collector_ForwardKinematicsFactor.begin(); - iter != collector_ForwardKinematicsFactor.end(); ) { - delete *iter; - collector_ForwardKinematicsFactor.erase(iter++); - anyDeleted = true; - } } { for(Collector_ns1ClassA::iterator iter = collector_ns1ClassA.begin(); iter != collector_ns1ClassA.end(); ) { delete *iter; @@ -219,6 +78,7 @@ void _deleteAllObjects() collector_gtsamValues.erase(iter++); anyDeleted = true; } } + if(anyDeleted) cout << "WARNING: Wrap modules with variables in the workspace have been reloaded due to\n" @@ -231,10 +91,8 @@ void _namespaces_RTTIRegister() { const mxArray *alreadyCreated = mexGetVariablePtr("global", "gtsam_namespaces_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(MyTemplateMatrix).name(), "MyTemplateMatrix")); - types.insert(std::make_pair(typeid(ForwardKinematicsFactor).name(), "ForwardKinematicsFactor")); + + mxArray *registry = mexGetVariable("global", "gtsamwrap_rttiRegistry"); if(!registry) @@ -242,18 +100,21 @@ void _namespaces_RTTIRegister() { typedef std::pair StringPair; for(const StringPair& rtti_matlab: types) { int fieldId = mxAddField(registry, rtti_matlab.first.c_str()); - if(fieldId < 0) + 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) + 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) + if(mexPutVariable("global", "gtsam_geometry_rttiRegistry_created", newAlreadyCreated) != 0) { mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); + } mxDestroyArray(newAlreadyCreated); } } diff --git a/tests/expected/matlab/special_cases_wrapper.cpp b/tests/expected/matlab/special_cases_wrapper.cpp index 056ce8097..69abbf73b 100644 --- a/tests/expected/matlab/special_cases_wrapper.cpp +++ b/tests/expected/matlab/special_cases_wrapper.cpp @@ -5,78 +5,11 @@ #include #include -#include #include -#include -#include -#include -#include -#include -#include -#include -#include -typedef Fun FunDouble; -typedef PrimitiveRef PrimitiveRefDouble; -typedef MyVector<3> MyVector3; -typedef MyVector<12> MyVector12; -typedef MultipleTemplates MultipleTemplatesIntDouble; -typedef MultipleTemplates MultipleTemplatesIntFloat; -typedef MyFactor MyFactorPosePoint2; -typedef MyTemplate MyTemplatePoint2; -typedef MyTemplate MyTemplateMatrix; typedef gtsam::PinholeCamera PinholeCameraCal3Bundler; typedef gtsam::GeneralSFMFactor, gtsam::Point3> GeneralSFMFactorCal3Bundler; -BOOST_CLASS_EXPORT_GUID(gtsam::Point2, "gtsamPoint2"); -BOOST_CLASS_EXPORT_GUID(gtsam::Point3, "gtsamPoint3"); - -typedef std::set*> Collector_FunRange; -static Collector_FunRange collector_FunRange; -typedef std::set*> Collector_FunDouble; -static Collector_FunDouble collector_FunDouble; -typedef std::set*> Collector_Test; -static Collector_Test collector_Test; -typedef std::set*> Collector_PrimitiveRefDouble; -static Collector_PrimitiveRefDouble collector_PrimitiveRefDouble; -typedef std::set*> Collector_MyVector3; -static Collector_MyVector3 collector_MyVector3; -typedef std::set*> Collector_MyVector12; -static Collector_MyVector12 collector_MyVector12; -typedef std::set*> Collector_MultipleTemplatesIntDouble; -static Collector_MultipleTemplatesIntDouble collector_MultipleTemplatesIntDouble; -typedef std::set*> Collector_MultipleTemplatesIntFloat; -static Collector_MultipleTemplatesIntFloat collector_MultipleTemplatesIntFloat; -typedef std::set*> Collector_ForwardKinematics; -static Collector_ForwardKinematics collector_ForwardKinematics; -typedef std::set*> Collector_MyFactorPosePoint2; -static Collector_MyFactorPosePoint2 collector_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_MyBase; -static Collector_MyBase collector_MyBase; -typedef std::set*> Collector_MyTemplatePoint2; -static Collector_MyTemplatePoint2 collector_MyTemplatePoint2; -typedef std::set*> Collector_MyTemplateMatrix; -static Collector_MyTemplateMatrix collector_MyTemplateMatrix; -typedef std::set*> Collector_ForwardKinematicsFactor; -static Collector_ForwardKinematicsFactor collector_ForwardKinematicsFactor; -typedef std::set*> Collector_ns1ClassA; -static Collector_ns1ClassA collector_ns1ClassA; -typedef std::set*> Collector_ns1ClassB; -static Collector_ns1ClassB collector_ns1ClassB; -typedef std::set*> Collector_ns2ClassA; -static Collector_ns2ClassA collector_ns2ClassA; -typedef std::set*> Collector_ns2ns3ClassB; -static Collector_ns2ns3ClassB collector_ns2ns3ClassB; -typedef std::set*> Collector_ns2ClassC; -static Collector_ns2ClassC collector_ns2ClassC; -typedef std::set*> Collector_ClassD; -static Collector_ClassD collector_ClassD; -typedef std::set*> Collector_gtsamValues; -static Collector_gtsamValues collector_gtsamValues; typedef std::set*> Collector_gtsamNonlinearFactorGraph; static Collector_gtsamNonlinearFactorGraph collector_gtsamNonlinearFactorGraph; typedef std::set*> Collector_gtsamSfmTrack; @@ -86,150 +19,13 @@ static Collector_gtsamPinholeCameraCal3Bundler collector_gtsamPinholeCameraCal3B typedef std::set*> Collector_gtsamGeneralSFMFactorCal3Bundler; static Collector_gtsamGeneralSFMFactorCal3Bundler collector_gtsamGeneralSFMFactorCal3Bundler; + void _deleteAllObjects() { mstream mout; std::streambuf *outbuf = std::cout.rdbuf(&mout); bool anyDeleted = false; - { for(Collector_FunRange::iterator iter = collector_FunRange.begin(); - iter != collector_FunRange.end(); ) { - delete *iter; - collector_FunRange.erase(iter++); - anyDeleted = true; - } } - { for(Collector_FunDouble::iterator iter = collector_FunDouble.begin(); - iter != collector_FunDouble.end(); ) { - delete *iter; - collector_FunDouble.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_PrimitiveRefDouble::iterator iter = collector_PrimitiveRefDouble.begin(); - iter != collector_PrimitiveRefDouble.end(); ) { - delete *iter; - collector_PrimitiveRefDouble.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MyVector3::iterator iter = collector_MyVector3.begin(); - iter != collector_MyVector3.end(); ) { - delete *iter; - collector_MyVector3.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MyVector12::iterator iter = collector_MyVector12.begin(); - iter != collector_MyVector12.end(); ) { - delete *iter; - collector_MyVector12.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MultipleTemplatesIntDouble::iterator iter = collector_MultipleTemplatesIntDouble.begin(); - iter != collector_MultipleTemplatesIntDouble.end(); ) { - delete *iter; - collector_MultipleTemplatesIntDouble.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MultipleTemplatesIntFloat::iterator iter = collector_MultipleTemplatesIntFloat.begin(); - iter != collector_MultipleTemplatesIntFloat.end(); ) { - delete *iter; - collector_MultipleTemplatesIntFloat.erase(iter++); - anyDeleted = true; - } } - { for(Collector_ForwardKinematics::iterator iter = collector_ForwardKinematics.begin(); - iter != collector_ForwardKinematics.end(); ) { - delete *iter; - collector_ForwardKinematics.erase(iter++); - anyDeleted = true; - } } - { for(Collector_MyFactorPosePoint2::iterator iter = collector_MyFactorPosePoint2.begin(); - iter != collector_MyFactorPosePoint2.end(); ) { - delete *iter; - collector_MyFactorPosePoint2.erase(iter++); - anyDeleted = true; - } } - { 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_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_MyTemplateMatrix::iterator iter = collector_MyTemplateMatrix.begin(); - iter != collector_MyTemplateMatrix.end(); ) { - delete *iter; - collector_MyTemplateMatrix.erase(iter++); - anyDeleted = true; - } } - { for(Collector_ForwardKinematicsFactor::iterator iter = collector_ForwardKinematicsFactor.begin(); - iter != collector_ForwardKinematicsFactor.end(); ) { - delete *iter; - collector_ForwardKinematicsFactor.erase(iter++); - anyDeleted = true; - } } - { for(Collector_ns1ClassA::iterator iter = collector_ns1ClassA.begin(); - iter != collector_ns1ClassA.end(); ) { - delete *iter; - collector_ns1ClassA.erase(iter++); - anyDeleted = true; - } } - { for(Collector_ns1ClassB::iterator iter = collector_ns1ClassB.begin(); - iter != collector_ns1ClassB.end(); ) { - delete *iter; - collector_ns1ClassB.erase(iter++); - anyDeleted = true; - } } - { for(Collector_ns2ClassA::iterator iter = collector_ns2ClassA.begin(); - iter != collector_ns2ClassA.end(); ) { - delete *iter; - collector_ns2ClassA.erase(iter++); - anyDeleted = true; - } } - { for(Collector_ns2ns3ClassB::iterator iter = collector_ns2ns3ClassB.begin(); - iter != collector_ns2ns3ClassB.end(); ) { - delete *iter; - collector_ns2ns3ClassB.erase(iter++); - anyDeleted = true; - } } - { for(Collector_ns2ClassC::iterator iter = collector_ns2ClassC.begin(); - iter != collector_ns2ClassC.end(); ) { - delete *iter; - collector_ns2ClassC.erase(iter++); - anyDeleted = true; - } } - { for(Collector_ClassD::iterator iter = collector_ClassD.begin(); - iter != collector_ClassD.end(); ) { - delete *iter; - collector_ClassD.erase(iter++); - anyDeleted = true; - } } - { for(Collector_gtsamValues::iterator iter = collector_gtsamValues.begin(); - iter != collector_gtsamValues.end(); ) { - delete *iter; - collector_gtsamValues.erase(iter++); - anyDeleted = true; - } } { for(Collector_gtsamNonlinearFactorGraph::iterator iter = collector_gtsamNonlinearFactorGraph.begin(); iter != collector_gtsamNonlinearFactorGraph.end(); ) { delete *iter; @@ -254,6 +50,7 @@ void _deleteAllObjects() collector_gtsamGeneralSFMFactorCal3Bundler.erase(iter++); anyDeleted = true; } } + if(anyDeleted) cout << "WARNING: Wrap modules with variables in the workspace have been reloaded due to\n" @@ -266,10 +63,8 @@ void _special_cases_RTTIRegister() { const mxArray *alreadyCreated = mexGetVariablePtr("global", "gtsam_special_cases_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(MyTemplateMatrix).name(), "MyTemplateMatrix")); - types.insert(std::make_pair(typeid(ForwardKinematicsFactor).name(), "ForwardKinematicsFactor")); + + mxArray *registry = mexGetVariable("global", "gtsamwrap_rttiRegistry"); if(!registry) @@ -277,18 +72,21 @@ void _special_cases_RTTIRegister() { typedef std::pair StringPair; for(const StringPair& rtti_matlab: types) { int fieldId = mxAddField(registry, rtti_matlab.first.c_str()); - if(fieldId < 0) + 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) + 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) + if(mexPutVariable("global", "gtsam_geometry_rttiRegistry_created", newAlreadyCreated) != 0) { mexErrMsgTxt("gtsam wrap: Error indexing RTTI types, inheritance will not work correctly"); + } mxDestroyArray(newAlreadyCreated); } } diff --git a/tests/fixtures/part1.i b/tests/fixtures/part1.i new file mode 100644 index 000000000..b69850baf --- /dev/null +++ b/tests/fixtures/part1.i @@ -0,0 +1,11 @@ +// First file to test for multi-file support. + +namespace gtsam { +class Class1 { + Class1(); +}; + +class Class2 { + Class2(); +}; +} // namespace gtsam \ No newline at end of file diff --git a/tests/fixtures/part2.i b/tests/fixtures/part2.i new file mode 100644 index 000000000..29ad86a7f --- /dev/null +++ b/tests/fixtures/part2.i @@ -0,0 +1,7 @@ +// Second file to test for multi-file support. + +namespace gtsam { +class ClassA { + ClassA(); +}; +} // namespace gtsam \ No newline at end of file diff --git a/tests/test_matlab_wrapper.py b/tests/test_matlab_wrapper.py index b321c4e15..fad4de16a 100644 --- a/tests/test_matlab_wrapper.py +++ b/tests/test_matlab_wrapper.py @@ -22,73 +22,31 @@ class TestWrap(unittest.TestCase): """ Test the Matlab wrapper """ - TEST_DIR = osp.dirname(osp.realpath(__file__)) - INTERFACE_DIR = osp.join(TEST_DIR, "fixtures") - MATLAB_TEST_DIR = osp.join(TEST_DIR, "expected", "matlab") - MATLAB_ACTUAL_DIR = osp.join(TEST_DIR, "actual", "matlab") + def setUp(self) -> None: + super().setUp() - # Create the `actual/matlab` directory - os.makedirs(MATLAB_ACTUAL_DIR, exist_ok=True) + # Set up all the directories + self.TEST_DIR = osp.dirname(osp.realpath(__file__)) + self.INTERFACE_DIR = osp.join(self.TEST_DIR, "fixtures") + self.MATLAB_TEST_DIR = osp.join(self.TEST_DIR, "expected", "matlab") + self.MATLAB_ACTUAL_DIR = osp.join(self.TEST_DIR, "actual", "matlab") - # set the log level to INFO by default - logger.remove() # remove the default sink - logger.add(sys.stderr, format="{time} {level} {message}", level="INFO") + if not osp.exists(self.MATLAB_ACTUAL_DIR): + os.mkdir(self.MATLAB_ACTUAL_DIR) - def generate_content(self, cc_content, path=MATLAB_ACTUAL_DIR): - """Generate files and folders from matlab wrapper content. + # Generate the matlab.h file if it does not exist + template_file = osp.join(self.TEST_DIR, "..", "gtwrap", + "matlab_wrapper", "matlab_wrapper.tpl") + if not osp.exists(template_file): + with open(template_file, 'w') as tpl: + tpl.write("#include \n#include \n") - Keyword arguments: - cc_content -- the content to generate formatted as - (file_name, file_content) or - (folder_name, [(file_name, file_content)]) - path -- the path to the files parent folder within the main folder - """ - for c in cc_content: - if isinstance(c, list): - if len(c) == 0: - continue - logger.debug("c object: {}".format(c[0][0])) - path_to_folder = osp.join(path, c[0][0]) + # Create the `actual/matlab` directory + os.makedirs(self.MATLAB_ACTUAL_DIR, exist_ok=True) - if not osp.isdir(path_to_folder): - try: - os.makedirs(path_to_folder, exist_ok=True) - except OSError: - pass - - for sub_content in c: - logger.debug("sub object: {}".format(sub_content[1][0][0])) - self.generate_content(sub_content[1], path_to_folder) - - elif isinstance(c[1], list): - path_to_folder = osp.join(path, c[0]) - - logger.debug( - "[generate_content_global]: {}".format(path_to_folder)) - if not osp.isdir(path_to_folder): - try: - os.makedirs(path_to_folder, exist_ok=True) - except OSError: - pass - for sub_content in c[1]: - path_to_file = osp.join(path_to_folder, sub_content[0]) - logger.debug( - "[generate_global_method]: {}".format(path_to_file)) - with open(path_to_file, 'w') as f: - f.write(sub_content[1]) - - else: - path_to_file = osp.join(path, c[0]) - - logger.debug("[generate_content]: {}".format(path_to_file)) - if not osp.isdir(path_to_file): - try: - os.mkdir(path) - except OSError: - pass - - with open(path_to_file, 'w') as f: - f.write(c[1]) + # set the log level to INFO by default + logger.remove() # remove the default sink + logger.add(sys.stderr, format="{time} {level} {message}", level="INFO") def compare_and_diff(self, file): """ @@ -109,11 +67,7 @@ class TestWrap(unittest.TestCase): python3 wrap/matlab_wrapper.py --src wrap/tests/geometry.h --module_name geometry --out wrap/tests/actual-matlab """ - with open(osp.join(self.INTERFACE_DIR, 'geometry.i'), 'r') as f: - content = f.read() - - if not osp.exists(self.MATLAB_ACTUAL_DIR): - os.mkdir(self.MATLAB_ACTUAL_DIR) + file = osp.join(self.INTERFACE_DIR, 'geometry.i') # Create MATLAB wrapper instance wrapper = MatlabWrapper( @@ -122,24 +76,18 @@ class TestWrap(unittest.TestCase): ignore_classes=[''], ) - cc_content = wrapper.wrap(content) - - self.generate_content(cc_content) - - self.assertTrue(osp.isdir(osp.join(self.MATLAB_ACTUAL_DIR, '+gtsam'))) + wrapper.wrap([file], path=self.MATLAB_ACTUAL_DIR) files = ['+gtsam/Point2.m', '+gtsam/Point3.m', 'geometry_wrapper.cpp'] + self.assertTrue(osp.isdir(osp.join(self.MATLAB_ACTUAL_DIR, '+gtsam'))) + for file in files: self.compare_and_diff(file) def test_functions(self): """Test interface file with function info.""" - with open(osp.join(self.INTERFACE_DIR, 'functions.i'), 'r') as f: - content = f.read() - - if not osp.exists(self.MATLAB_ACTUAL_DIR): - os.mkdir(self.MATLAB_ACTUAL_DIR) + file = osp.join(self.INTERFACE_DIR, 'functions.i') wrapper = MatlabWrapper( module_name='functions', @@ -147,9 +95,7 @@ class TestWrap(unittest.TestCase): ignore_classes=[''], ) - cc_content = wrapper.wrap(content) - - self.generate_content(cc_content) + wrapper.wrap([file], path=self.MATLAB_ACTUAL_DIR) files = [ 'functions_wrapper.cpp', 'aGlobalFunction.m', 'load2D.m', @@ -163,11 +109,7 @@ class TestWrap(unittest.TestCase): def test_class(self): """Test interface file with only class info.""" - with open(osp.join(self.INTERFACE_DIR, 'class.i'), 'r') as f: - content = f.read() - - if not osp.exists(self.MATLAB_ACTUAL_DIR): - os.mkdir(self.MATLAB_ACTUAL_DIR) + file = osp.join(self.INTERFACE_DIR, 'class.i') wrapper = MatlabWrapper( module_name='class', @@ -175,9 +117,7 @@ class TestWrap(unittest.TestCase): ignore_classes=[''], ) - cc_content = wrapper.wrap(content) - - self.generate_content(cc_content) + wrapper.wrap([file], path=self.MATLAB_ACTUAL_DIR) files = [ 'class_wrapper.cpp', 'FunDouble.m', 'FunRange.m', @@ -191,21 +131,14 @@ class TestWrap(unittest.TestCase): def test_inheritance(self): """Test interface file with class inheritance definitions.""" - with open(osp.join(self.INTERFACE_DIR, 'inheritance.i'), 'r') as f: - content = f.read() - - if not osp.exists(self.MATLAB_ACTUAL_DIR): - os.mkdir(self.MATLAB_ACTUAL_DIR) + file = osp.join(self.INTERFACE_DIR, 'inheritance.i') wrapper = MatlabWrapper( module_name='inheritance', top_module_namespace=['gtsam'], ignore_classes=[''], ) - - cc_content = wrapper.wrap(content) - - self.generate_content(cc_content) + wrapper.wrap([file], path=self.MATLAB_ACTUAL_DIR) files = [ 'inheritance_wrapper.cpp', 'MyBase.m', 'MyTemplateMatrix.m', @@ -219,11 +152,7 @@ class TestWrap(unittest.TestCase): """ Test interface file with full namespace definition. """ - with open(osp.join(self.INTERFACE_DIR, 'namespaces.i'), 'r') as f: - content = f.read() - - if not osp.exists(self.MATLAB_ACTUAL_DIR): - os.mkdir(self.MATLAB_ACTUAL_DIR) + file = osp.join(self.INTERFACE_DIR, 'namespaces.i') wrapper = MatlabWrapper( module_name='namespaces', @@ -231,9 +160,7 @@ class TestWrap(unittest.TestCase): ignore_classes=[''], ) - cc_content = wrapper.wrap(content) - - self.generate_content(cc_content) + wrapper.wrap([file], path=self.MATLAB_ACTUAL_DIR) files = [ 'namespaces_wrapper.cpp', '+ns1/aGlobalFunction.m', @@ -249,21 +176,14 @@ class TestWrap(unittest.TestCase): """ Tests for some unique, non-trivial features. """ - with open(osp.join(self.INTERFACE_DIR, 'special_cases.i'), 'r') as f: - content = f.read() - - if not osp.exists(self.MATLAB_ACTUAL_DIR): - os.mkdir(self.MATLAB_ACTUAL_DIR) + file = osp.join(self.INTERFACE_DIR, 'special_cases.i') wrapper = MatlabWrapper( module_name='special_cases', top_module_namespace=['gtsam'], ignore_classes=[''], ) - - cc_content = wrapper.wrap(content) - - self.generate_content(cc_content) + wrapper.wrap([file], path=self.MATLAB_ACTUAL_DIR) files = [ 'special_cases_wrapper.cpp', @@ -274,6 +194,31 @@ class TestWrap(unittest.TestCase): for file in files: self.compare_and_diff(file) + def test_multiple_files(self): + """ + Test for when multiple interface files are specified. + """ + file1 = osp.join(self.INTERFACE_DIR, 'part1.i') + file2 = osp.join(self.INTERFACE_DIR, 'part2.i') + + wrapper = MatlabWrapper( + module_name='multiple_files', + top_module_namespace=['gtsam'], + ignore_classes=[''], + ) + + wrapper.wrap([file1, file2], path=self.MATLAB_ACTUAL_DIR) + + files = [ + 'multiple_files_wrapper.cpp', + '+gtsam/Class1.m', + '+gtsam/Class2.m', + '+gtsam/ClassA.m', + ] + + for file in files: + self.compare_and_diff(file) + if __name__ == '__main__': unittest.main() diff --git a/tests/test_pybind_wrapper.py b/tests/test_pybind_wrapper.py index 77c884b62..67c637d14 100644 --- a/tests/test_pybind_wrapper.py +++ b/tests/test_pybind_wrapper.py @@ -31,9 +31,9 @@ class TestWrap(unittest.TestCase): # Create the `actual/python` directory os.makedirs(PYTHON_ACTUAL_DIR, exist_ok=True) - def wrap_content(self, content, module_name, output_dir): + def wrap_content(self, sources, module_name, output_dir): """ - Common function to wrap content. + Common function to wrap content in `sources`. """ with open(osp.join(self.TEST_DIR, "pybind_wrapper.tpl")) as template_file: @@ -46,15 +46,12 @@ class TestWrap(unittest.TestCase): ignore_classes=[''], module_template=module_template) - cc_content = wrapper.wrap(content) - output = osp.join(self.TEST_DIR, output_dir, module_name + ".cpp") if not osp.exists(osp.join(self.TEST_DIR, output_dir)): os.mkdir(osp.join(self.TEST_DIR, output_dir)) - with open(output, 'w') as f: - f.write(cc_content) + wrapper.wrap(sources, output) return output @@ -76,39 +73,32 @@ class TestWrap(unittest.TestCase): python3 ../pybind_wrapper.py --src geometry.h --module_name geometry_py --out output/geometry_py.cc """ - with open(osp.join(self.INTERFACE_DIR, 'geometry.i'), 'r') as f: - content = f.read() - - output = self.wrap_content(content, 'geometry_py', + source = osp.join(self.INTERFACE_DIR, 'geometry.i') + output = self.wrap_content([source], 'geometry_py', self.PYTHON_ACTUAL_DIR) self.compare_and_diff('geometry_pybind.cpp', output) def test_functions(self): """Test interface file with function info.""" - with open(osp.join(self.INTERFACE_DIR, 'functions.i'), 'r') as f: - content = f.read() - - output = self.wrap_content(content, 'functions_py', + source = osp.join(self.INTERFACE_DIR, 'functions.i') + output = self.wrap_content([source], 'functions_py', self.PYTHON_ACTUAL_DIR) self.compare_and_diff('functions_pybind.cpp', output) def test_class(self): """Test interface file with only class info.""" - with open(osp.join(self.INTERFACE_DIR, 'class.i'), 'r') as f: - content = f.read() - - output = self.wrap_content(content, 'class_py', self.PYTHON_ACTUAL_DIR) + source = osp.join(self.INTERFACE_DIR, 'class.i') + output = self.wrap_content([source], 'class_py', + self.PYTHON_ACTUAL_DIR) self.compare_and_diff('class_pybind.cpp', output) def test_inheritance(self): """Test interface file with class inheritance definitions.""" - with open(osp.join(self.INTERFACE_DIR, 'inheritance.i'), 'r') as f: - content = f.read() - - output = self.wrap_content(content, 'inheritance_py', + source = osp.join(self.INTERFACE_DIR, 'inheritance.i') + output = self.wrap_content([source], 'inheritance_py', self.PYTHON_ACTUAL_DIR) self.compare_and_diff('inheritance_pybind.cpp', output) @@ -119,10 +109,8 @@ class TestWrap(unittest.TestCase): python3 ../pybind_wrapper.py --src namespaces.i --module_name namespaces_py --out output/namespaces_py.cpp """ - with open(osp.join(self.INTERFACE_DIR, 'namespaces.i'), 'r') as f: - content = f.read() - - output = self.wrap_content(content, 'namespaces_py', + source = osp.join(self.INTERFACE_DIR, 'namespaces.i') + output = self.wrap_content([source], 'namespaces_py', self.PYTHON_ACTUAL_DIR) self.compare_and_diff('namespaces_pybind.cpp', output) @@ -131,10 +119,8 @@ class TestWrap(unittest.TestCase): """ Tests for operator overloading. """ - with open(osp.join(self.INTERFACE_DIR, 'operator.i'), 'r') as f: - content = f.read() - - output = self.wrap_content(content, 'operator_py', + source = osp.join(self.INTERFACE_DIR, 'operator.i') + output = self.wrap_content([source], 'operator_py', self.PYTHON_ACTUAL_DIR) self.compare_and_diff('operator_pybind.cpp', output) @@ -143,10 +129,8 @@ class TestWrap(unittest.TestCase): """ Tests for some unique, non-trivial features. """ - with open(osp.join(self.INTERFACE_DIR, 'special_cases.i'), 'r') as f: - content = f.read() - - output = self.wrap_content(content, 'special_cases_py', + source = osp.join(self.INTERFACE_DIR, 'special_cases.i') + output = self.wrap_content([source], 'special_cases_py', self.PYTHON_ACTUAL_DIR) self.compare_and_diff('special_cases_pybind.cpp', output) @@ -155,10 +139,8 @@ class TestWrap(unittest.TestCase): """ Test if enum generation is correct. """ - with open(osp.join(self.INTERFACE_DIR, 'enum.i'), 'r') as f: - content = f.read() - - output = self.wrap_content(content, 'enum_py', self.PYTHON_ACTUAL_DIR) + source = osp.join(self.INTERFACE_DIR, 'enum.i') + output = self.wrap_content([source], 'enum_py', self.PYTHON_ACTUAL_DIR) self.compare_and_diff('enum_pybind.cpp', output) From 61c5e89de3bf628dccd98e95c0f955bf908ca8a9 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Sun, 11 Jul 2021 22:46:54 -0400 Subject: [PATCH 051/248] try increasing pmax to pass test --- python/gtsam/tests/test_ShonanAveraging.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/python/gtsam/tests/test_ShonanAveraging.py b/python/gtsam/tests/test_ShonanAveraging.py index 69e705545..d07d84099 100644 --- a/python/gtsam/tests/test_ShonanAveraging.py +++ b/python/gtsam/tests/test_ShonanAveraging.py @@ -193,7 +193,8 @@ class TestShonanAveraging(GtsamTestCase): obj = ShonanAveraging2(between_factors, shonan_params) initial = obj.initializeRandomly() - result_values, _ = obj.run(initial, min_p=2, max_p=10) + min + result_values, _ = obj.run(initial, min_p=2, max_p=40) for i in range(num_images): wRi = result_values.atRot2(i) From 690300124c5f7a162601298fca115c8f01cc531a Mon Sep 17 00:00:00 2001 From: John Lambert Date: Sun, 11 Jul 2021 22:47:14 -0400 Subject: [PATCH 052/248] fix typo --- python/gtsam/tests/test_ShonanAveraging.py | 1 - 1 file changed, 1 deletion(-) diff --git a/python/gtsam/tests/test_ShonanAveraging.py b/python/gtsam/tests/test_ShonanAveraging.py index d07d84099..c109ce6b0 100644 --- a/python/gtsam/tests/test_ShonanAveraging.py +++ b/python/gtsam/tests/test_ShonanAveraging.py @@ -193,7 +193,6 @@ class TestShonanAveraging(GtsamTestCase): obj = ShonanAveraging2(between_factors, shonan_params) initial = obj.initializeRandomly() - min result_values, _ = obj.run(initial, min_p=2, max_p=40) for i in range(num_images): From 36c2aa1e56a11b4a99acf855258413d0230b08aa Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 12 Jul 2021 11:46:31 -0400 Subject: [PATCH 053/248] matlab wrapper header update --- wrap/matlab.h | 1 + 1 file changed, 1 insertion(+) diff --git a/wrap/matlab.h b/wrap/matlab.h index 4f3bfe96e..bcdef3c8d 100644 --- a/wrap/matlab.h +++ b/wrap/matlab.h @@ -26,6 +26,7 @@ #include #include #include +#include using gtsam::Vector; using gtsam::Matrix; From cce952fbb3fc266e89bd2599ec58de7fb92622be Mon Sep 17 00:00:00 2001 From: John Lambert Date: Mon, 12 Jul 2021 19:35:34 -0400 Subject: [PATCH 054/248] use simple example for unit test --- python/gtsam/tests/test_ShonanAveraging.py | 84 +++++++++++----------- 1 file changed, 43 insertions(+), 41 deletions(-) diff --git a/python/gtsam/tests/test_ShonanAveraging.py b/python/gtsam/tests/test_ShonanAveraging.py index c109ce6b0..08bbe5191 100644 --- a/python/gtsam/tests/test_ShonanAveraging.py +++ b/python/gtsam/tests/test_ShonanAveraging.py @@ -22,12 +22,13 @@ from gtsam import ( ShonanAveraging2, ShonanAveragingParameters2, ShonanAveraging3, - ShonanAveragingParameters3 + ShonanAveragingParameters3, ) from gtsam.utils.test_case import GtsamTestCase DEFAULT_PARAMS = ShonanAveragingParameters3( - gtsam.LevenbergMarquardtParams.CeresDefaults()) + gtsam.LevenbergMarquardtParams.CeresDefaults() +) def fromExampleName( @@ -146,58 +147,59 @@ class TestShonanAveraging(GtsamTestCase): result, _lambdaMin = shonan.run(initial, 3, 3) self.assertAlmostEqual(0.0015, shonan.cost(result), places=3) + def test_constructorBetweenFactorPose2s(self) -> None: - """Check if ShonanAveraging2 constructor works when not initialized from g2o file.""" - num_images = 11 - # map (i1,i2) pair to theta in degrees that parameterizes Rot2 object i2Ri1 + """Check if ShonanAveraging2 constructor works when not initialized from g2o file. + + GT pose graph: + + | cam 1 = (0,4) + --o + | . + . . + . . + | | + o-- ... o-- + cam 0 cam 2 = (4,0) + (0,0) + """ + num_images = 3 + + wTi_list = [ + Pose2(Rot2.fromDegrees(0), np.array([0, 0])), + Pose2(Rot2.fromDegrees(90), np.array([0, 4])), + Pose2(Rot2.fromDegrees(0), np.array([4, 0])), + ] + + edges = [(0, 1), (1, 2), (0, 2)] i2Ri1_dict = { - (1, 2): -43.86342, - (1, 5): -135.06351, - (2, 4): 72.64527, - (3, 5): 117.75967, - (6, 7): -31.73934, - (7, 10): 177.45901, - (6, 9): -133.58713, - (7, 8): -90.58668, - (0, 3): 127.02978, - (8, 10): -92.16361, - (4, 8): 51.63781, - (4, 6): 173.96384, - (4, 10): 139.59445, - (2, 3): 151.04022, - (3, 4): -78.39495, - (1, 4): 28.78185, - (6, 8): -122.32602, - (0, 2): -24.01045, - (5, 7): -53.93014, - (4, 5): -163.84535, - (2, 5): -91.20009, - (1, 3): 107.17680, - (7, 9): -102.35615, - (0, 1): 19.85297, - (5, 8): -144.51682, - (5, 6): -22.19079, - (5, 10): -56.56016, + (i1, i2): wTi_list[i2].inverse().compose(wTi_list[i1]).rotation() + for (i1, i2) in edges } - i2Ri1_dict = {(i1,i2): Rot2.fromDegrees(theta_deg) for (i1,i2), theta_deg in i2Ri1_dict.items()} + lm_params = LevenbergMarquardtParams.CeresDefaults() shonan_params = ShonanAveragingParameters2(lm_params) shonan_params.setUseHuber(False) shonan_params.setCertifyOptimality(True) - + noise_model = gtsam.noiseModel.Unit.Create(3) between_factors = gtsam.BetweenFactorPose2s() for (i1, i2), i2Ri1 in i2Ri1_dict.items(): i2Ti1 = Pose2(i2Ri1, np.zeros(2)) - between_factors.append(BetweenFactorPose2(i2, i1, i2Ti1, noise_model)) - + between_factors.append( + BetweenFactorPose2(i2, i1, i2Ti1, noise_model) + ) + obj = ShonanAveraging2(between_factors, shonan_params) initial = obj.initializeRandomly() - result_values, _ = obj.run(initial, min_p=2, max_p=40) - - for i in range(num_images): - wRi = result_values.atRot2(i) + result_values, _ = obj.run(initial, min_p=2, max_p=100) + + wRi_list = [result_values.atRot2(i) for i in range(num_images)] + thetas_deg = np.array([wRi.degrees() for wRi in wRi_list]) + thetas_deg -= max(thetas_deg) + expected_thetas_deg = np.array([0.0, -270.0, 0.0]) + np.testing.assert_allclose(thetas_deg, expected_thetas_deg) -if __name__ == '__main__': +if __name__ == "__main__": unittest.main() From d7151ed28410e57526cf5286d820fc4ae089cf05 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Mon, 12 Jul 2021 20:52:36 -0400 Subject: [PATCH 055/248] use mod when comparing angles --- python/gtsam/tests/test_ShonanAveraging.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/python/gtsam/tests/test_ShonanAveraging.py b/python/gtsam/tests/test_ShonanAveraging.py index 08bbe5191..8c70df5df 100644 --- a/python/gtsam/tests/test_ShonanAveraging.py +++ b/python/gtsam/tests/test_ShonanAveraging.py @@ -197,7 +197,9 @@ class TestShonanAveraging(GtsamTestCase): wRi_list = [result_values.atRot2(i) for i in range(num_images)] thetas_deg = np.array([wRi.degrees() for wRi in wRi_list]) thetas_deg -= max(thetas_deg) - expected_thetas_deg = np.array([0.0, -270.0, 0.0]) + # map all angles to [0,360) + thetas_deg = thetas_deg % 360 + expected_thetas_deg = np.array([0.0, 90.0, 0.0]) np.testing.assert_allclose(thetas_deg, expected_thetas_deg) From c4a4e13196be2fdf738a173ca4e70b2a4eb43590 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Tue, 13 Jul 2021 00:16:24 -0600 Subject: [PATCH 056/248] fix assert on angles --- python/gtsam/tests/test_ShonanAveraging.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/python/gtsam/tests/test_ShonanAveraging.py b/python/gtsam/tests/test_ShonanAveraging.py index 8c70df5df..29a14b1b6 100644 --- a/python/gtsam/tests/test_ShonanAveraging.py +++ b/python/gtsam/tests/test_ShonanAveraging.py @@ -196,11 +196,13 @@ class TestShonanAveraging(GtsamTestCase): wRi_list = [result_values.atRot2(i) for i in range(num_images)] thetas_deg = np.array([wRi.degrees() for wRi in wRi_list]) - thetas_deg -= max(thetas_deg) + # map all angles to [0,360) thetas_deg = thetas_deg % 360 + thetas_deg -= max(thetas_deg) + expected_thetas_deg = np.array([0.0, 90.0, 0.0]) - np.testing.assert_allclose(thetas_deg, expected_thetas_deg) + np.testing.assert_allclose(thetas_deg, expected_thetas_deg, atol=0.1) if __name__ == "__main__": From 54d34711214a438c89f173eb9878b43036876d22 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Tue, 13 Jul 2021 08:18:45 -0600 Subject: [PATCH 057/248] update logic in angular error comparison --- python/gtsam/tests/test_ShonanAveraging.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/gtsam/tests/test_ShonanAveraging.py b/python/gtsam/tests/test_ShonanAveraging.py index 29a14b1b6..19b9f8dc1 100644 --- a/python/gtsam/tests/test_ShonanAveraging.py +++ b/python/gtsam/tests/test_ShonanAveraging.py @@ -199,7 +199,7 @@ class TestShonanAveraging(GtsamTestCase): # map all angles to [0,360) thetas_deg = thetas_deg % 360 - thetas_deg -= max(thetas_deg) + thetas_deg -= thetas_deg[0] expected_thetas_deg = np.array([0.0, 90.0, 0.0]) np.testing.assert_allclose(thetas_deg, expected_thetas_deg, atol=0.1) From 3402e46ad19cc6e2bef1c71cd777b167e09fe136 Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Tue, 13 Jul 2021 22:00:21 +0200 Subject: [PATCH 058/248] Shared data for triangulation unit tests --- python/gtsam/tests/test_Cal3Fisheye.py | 46 ++++++++++---------------- 1 file changed, 18 insertions(+), 28 deletions(-) diff --git a/python/gtsam/tests/test_Cal3Fisheye.py b/python/gtsam/tests/test_Cal3Fisheye.py index d731204ef..756113b93 100644 --- a/python/gtsam/tests/test_Cal3Fisheye.py +++ b/python/gtsam/tests/test_Cal3Fisheye.py @@ -33,6 +33,19 @@ class TestCal3Fisheye(GtsamTestCase): u, v = np.arctan2(x, z), 0.0 cls.obj_point = np.array([x, y, z]) cls.img_point = np.array([u, v]) + + p1 = [-1.0, 0.0, -1.0] + p2 = [ 1.0, 0.0, -1.0] + q1 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0) + q2 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0) + pose1 = gtsam.Pose3(q1, p1) + pose2 = gtsam.Pose3(q2, p2) + camera1 = gtsam.PinholeCameraCal3Fisheye(pose1) + camera2 = gtsam.PinholeCameraCal3Fisheye(pose2) + cls.origin = np.array([0.0, 0.0, 0.0]) + cls.poses = gtsam.Pose3Vector([pose1, pose2]) + cls.cameras = gtsam.CameraSetCal3Fisheye([camera1, camera2]) + cls.measurements = gtsam.Point2Vector([k.project(cls.origin) for k in cls.cameras]) def test_Cal3Fisheye(self): K = gtsam.Cal3Fisheye() @@ -96,40 +109,17 @@ class TestCal3Fisheye(GtsamTestCase): @unittest.skip("triangulatePoint3 currently seems to require perspective projections.") def test_triangulation_skipped(self): """Estimate spatial point from image measurements""" - p1 = [-1.0, 0.0, -1.0] - p2 = [ 1.0, 0.0, -1.0] - q1 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0) - q2 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0) - obj_point = np.array([0.0, 0.0, 0.0]) - pose1 = gtsam.Pose3(q1, p1) - pose2 = gtsam.Pose3(q2, p2) - camera1 = gtsam.PinholeCameraCal3Fisheye(pose1) - camera2 = gtsam.PinholeCameraCal3Fisheye(pose2) - cameras = gtsam.CameraSetCal3Fisheye([camera1, camera2]) - measurements = gtsam.Point2Vector([k.project(obj_point) for k in cameras]) - triangulated = gtsam.triangulatePoint3(cameras, measurements, rank_tol=1e-9, optimize=True) + triangulated = gtsam.triangulatePoint3(self.cameras, self.measurements, rank_tol=1e-9, optimize=True) self.gtsamAssertEquals(measurements[0], self.img_point) - self.gtsamAssertEquals(triangulated, obj_point) + self.gtsamAssertEquals(triangulated, self.origin) def test_triangulation_rectify(self): """Estimate spatial point from image measurements using rectification""" - p1 = [-1.0, 0.0, -1.0] - p2 = [ 1.0, 0.0, -1.0] - q1 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0) - q2 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0) - obj_point = np.array([0.0, 0.0, 0.0]) - pose1 = gtsam.Pose3(q1, p1) - pose2 = gtsam.Pose3(q2, p2) - camera1 = gtsam.PinholeCameraCal3Fisheye(pose1) - camera2 = gtsam.PinholeCameraCal3Fisheye(pose2) - cameras = gtsam.CameraSetCal3Fisheye([camera1, camera2]) - measurements = gtsam.Point2Vector([k.project(obj_point) for k in cameras]) - rectified = gtsam.Point2Vector([k.calibration().calibrate(pt) for k, pt in zip(cameras, measurements)]) + rectified = gtsam.Point2Vector([k.calibration().calibrate(pt) for k, pt in zip(self.cameras, self.measurements)]) shared_cal = gtsam.Cal3_S2() - poses = gtsam.Pose3Vector([pose1, pose2]) - triangulated = gtsam.triangulatePoint3(poses, shared_cal, rectified, rank_tol=1e-9, optimize=False) + triangulated = gtsam.triangulatePoint3(self.poses, shared_cal, rectified, rank_tol=1e-9, optimize=False) self.gtsamAssertEquals(measurements[0], self.img_point) - self.gtsamAssertEquals(triangulated, obj_point) + self.gtsamAssertEquals(triangulated, self.origin) def test_retract(self): expected = gtsam.Cal3Fisheye(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6, From 17c37de7c4325bebdbfad1531ecd270f4ca80eb2 Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Tue, 13 Jul 2021 22:07:19 +0200 Subject: [PATCH 059/248] Shared setup triangulation unit test --- python/gtsam/tests/test_Cal3Unified.py | 46 ++++++++++---------------- 1 file changed, 18 insertions(+), 28 deletions(-) diff --git a/python/gtsam/tests/test_Cal3Unified.py b/python/gtsam/tests/test_Cal3Unified.py index 0b09fc3ae..ca0959e44 100644 --- a/python/gtsam/tests/test_Cal3Unified.py +++ b/python/gtsam/tests/test_Cal3Unified.py @@ -40,6 +40,19 @@ class TestCal3Unified(GtsamTestCase): xi = 1 cls.stereographic = gtsam.Cal3Unified(fx, fy, s, u0, v0, k1, k2, p1, p2, xi) + p1 = [-1.0, 0.0, -1.0] + p2 = [ 1.0, 0.0, -1.0] + q1 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0) + q2 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0) + pose1 = gtsam.Pose3(q1, p1) + pose2 = gtsam.Pose3(q2, p2) + camera1 = gtsam.PinholeCameraCal3Unified(pose1, cls.stereographic) + camera2 = gtsam.PinholeCameraCal3Unified(pose2, cls.stereographic) + cls.origin = np.array([0.0, 0.0, 0.0]) + cls.poses = gtsam.Pose3Vector([pose1, pose2]) + cls.cameras = gtsam.CameraSetCal3Unified([camera1, camera2]) + cls.measurements = gtsam.Point2Vector([k.project(cls.origin) for k in cls.cameras]) + def test_Cal3Unified(self): K = gtsam.Cal3Unified() self.assertEqual(K.fx(), 1.) @@ -108,40 +121,17 @@ class TestCal3Unified(GtsamTestCase): @unittest.skip("triangulatePoint3 currently seems to require perspective projections.") def test_triangulation(self): """Estimate spatial point from image measurements""" - p1 = [-1.0, 0.0, -1.0] - p2 = [ 1.0, 0.0, -1.0] - q1 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0) - q2 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0) - obj_point = np.array([0.0, 0.0, 0.0]) - pose1 = gtsam.Pose3(q1, p1) - pose2 = gtsam.Pose3(q2, p2) - camera1 = gtsam.PinholeCameraCal3Unified(pose1, self.stereographic) - camera2 = gtsam.PinholeCameraCal3Unified(pose2, self.stereographic) - cameras = gtsam.CameraSetCal3Unified([camera1, camera2]) - measurements = gtsam.Point2Vector([k.project(obj_point) for k in cameras]) - triangulated = gtsam.triangulatePoint3(cameras, measurements, rank_tol=1e-9, optimize=True) + triangulated = gtsam.triangulatePoint3(self.cameras, self.measurements, rank_tol=1e-9, optimize=True) self.gtsamAssertEquals(measurements[0], self.img_point) - self.gtsamAssertEquals(triangulated, obj_point) + self.gtsamAssertEquals(triangulated, self.origin) def test_triangulation_rectify(self): """Estimate spatial point from image measurements using rectification""" - p1 = [-1.0, 0.0, -1.0] - p2 = [ 1.0, 0.0, -1.0] - q1 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0) - q2 = gtsam.Rot3(1.0, 0.0, 0.0, 0.0) - obj_point = np.array([0.0, 0.0, 0.0]) - pose1 = gtsam.Pose3(q1, p1) - pose2 = gtsam.Pose3(q2, p2) - camera1 = gtsam.PinholeCameraCal3Unified(pose1, self.stereographic) - camera2 = gtsam.PinholeCameraCal3Unified(pose2, self.stereographic) - cameras = gtsam.CameraSetCal3Unified([camera1, camera2]) - measurements = gtsam.Point2Vector([k.project(obj_point) for k in cameras]) - rectified = gtsam.Point2Vector([k.calibration().calibrate(pt) for k, pt in zip(cameras, measurements)]) + rectified = gtsam.Point2Vector([k.calibration().calibrate(pt) for k, pt in zip(self.cameras, self.measurements)]) shared_cal = gtsam.Cal3_S2() - poses = gtsam.Pose3Vector([pose1, pose2]) - triangulated = gtsam.triangulatePoint3(poses, shared_cal, rectified, rank_tol=1e-9, optimize=False) + triangulated = gtsam.triangulatePoint3(self.poses, shared_cal, rectified, rank_tol=1e-9, optimize=False) self.gtsamAssertEquals(measurements[0], self.img_point) - self.gtsamAssertEquals(triangulated, obj_point) + self.gtsamAssertEquals(triangulated, self.origin) def test_retract(self): expected = gtsam.Cal3Unified(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6, From 3e41ece75a0ba8fc923ea6f59d912fd52f3914d3 Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Tue, 13 Jul 2021 22:10:38 +0200 Subject: [PATCH 060/248] Minor fix test_Cal3Unified --- python/gtsam/tests/test_Cal3Unified.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/python/gtsam/tests/test_Cal3Unified.py b/python/gtsam/tests/test_Cal3Unified.py index ca0959e44..a402ae509 100644 --- a/python/gtsam/tests/test_Cal3Unified.py +++ b/python/gtsam/tests/test_Cal3Unified.py @@ -122,7 +122,6 @@ class TestCal3Unified(GtsamTestCase): def test_triangulation(self): """Estimate spatial point from image measurements""" triangulated = gtsam.triangulatePoint3(self.cameras, self.measurements, rank_tol=1e-9, optimize=True) - self.gtsamAssertEquals(measurements[0], self.img_point) self.gtsamAssertEquals(triangulated, self.origin) def test_triangulation_rectify(self): @@ -130,7 +129,6 @@ class TestCal3Unified(GtsamTestCase): rectified = gtsam.Point2Vector([k.calibration().calibrate(pt) for k, pt in zip(self.cameras, self.measurements)]) shared_cal = gtsam.Cal3_S2() triangulated = gtsam.triangulatePoint3(self.poses, shared_cal, rectified, rank_tol=1e-9, optimize=False) - self.gtsamAssertEquals(measurements[0], self.img_point) self.gtsamAssertEquals(triangulated, self.origin) def test_retract(self): From d130387a7db0b52080905c15ddcebfa22484d0de Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Tue, 13 Jul 2021 22:12:14 +0200 Subject: [PATCH 061/248] Minor fix test_Cal3Fisheye --- python/gtsam/tests/test_Cal3Fisheye.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/python/gtsam/tests/test_Cal3Fisheye.py b/python/gtsam/tests/test_Cal3Fisheye.py index 756113b93..646b48881 100644 --- a/python/gtsam/tests/test_Cal3Fisheye.py +++ b/python/gtsam/tests/test_Cal3Fisheye.py @@ -110,7 +110,6 @@ class TestCal3Fisheye(GtsamTestCase): def test_triangulation_skipped(self): """Estimate spatial point from image measurements""" triangulated = gtsam.triangulatePoint3(self.cameras, self.measurements, rank_tol=1e-9, optimize=True) - self.gtsamAssertEquals(measurements[0], self.img_point) self.gtsamAssertEquals(triangulated, self.origin) def test_triangulation_rectify(self): @@ -118,7 +117,6 @@ class TestCal3Fisheye(GtsamTestCase): rectified = gtsam.Point2Vector([k.calibration().calibrate(pt) for k, pt in zip(self.cameras, self.measurements)]) shared_cal = gtsam.Cal3_S2() triangulated = gtsam.triangulatePoint3(self.poses, shared_cal, rectified, rank_tol=1e-9, optimize=False) - self.gtsamAssertEquals(measurements[0], self.img_point) self.gtsamAssertEquals(triangulated, self.origin) def test_retract(self): From 16cfc7fd381a9250bb573a4613037299be770d9f Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Wed, 14 Jul 2021 11:15:10 +0200 Subject: [PATCH 062/248] Remove commented out line --- python/gtsam/tests/test_Cal3Fisheye.py | 1 - 1 file changed, 1 deletion(-) diff --git a/python/gtsam/tests/test_Cal3Fisheye.py b/python/gtsam/tests/test_Cal3Fisheye.py index 646b48881..298c6e57b 100644 --- a/python/gtsam/tests/test_Cal3Fisheye.py +++ b/python/gtsam/tests/test_Cal3Fisheye.py @@ -29,7 +29,6 @@ class TestCal3Fisheye(GtsamTestCase): image plane and theta the incident angle of the object point. """ x, y, z = 1.0, 0.0, 1.0 - # x, y, z = 0.5, 0.0, 2.0 u, v = np.arctan2(x, z), 0.0 cls.obj_point = np.array([x, y, z]) cls.img_point = np.array([u, v]) From c2bbe78e867bf1797ef33be444cc400bfe946d71 Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Wed, 14 Jul 2021 11:16:00 +0200 Subject: [PATCH 063/248] Remove comment --- python/gtsam/tests/test_Cal3Unified.py | 1 - 1 file changed, 1 deletion(-) diff --git a/python/gtsam/tests/test_Cal3Unified.py b/python/gtsam/tests/test_Cal3Unified.py index a402ae509..dab1ae446 100644 --- a/python/gtsam/tests/test_Cal3Unified.py +++ b/python/gtsam/tests/test_Cal3Unified.py @@ -31,7 +31,6 @@ class TestCal3Unified(GtsamTestCase): x, y, z = 1.0, 0.0, 1.0 r = np.linalg.norm([x, y, z]) u, v = 2*x/(z+r), 0.0 - #u, v = 2*np.tan(np.arctan2(x, z)/2), 0.0 cls.obj_point = np.array([x, y, z]) cls.img_point = np.array([u, v]) From a115788ea5d56df6cfdbc127ab24b6ebdbb9d157 Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Wed, 14 Jul 2021 11:53:31 +0200 Subject: [PATCH 064/248] Remove spaces in empty line --- gtsam/geometry/SimpleCamera.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/geometry/SimpleCamera.h b/gtsam/geometry/SimpleCamera.h index 2c34bdfa7..119e9d1a3 100644 --- a/gtsam/geometry/SimpleCamera.h +++ b/gtsam/geometry/SimpleCamera.h @@ -35,7 +35,7 @@ namespace gtsam { using PinholeCameraCal3DS2 = gtsam::PinholeCamera; using PinholeCameraCal3Unified = gtsam::PinholeCamera; using PinholeCameraCal3Fisheye = gtsam::PinholeCamera; - + #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 /** * @deprecated: SimpleCamera for backwards compatability with GTSAM 3.x From 6919ad927729385ba6583b7987099a628e708db8 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 14 Jul 2021 21:53:20 -0400 Subject: [PATCH 065/248] update interface files with latest develop --- gtsam/base/base.i | 6 +- gtsam/geometry/SimpleCamera.h | 2 + gtsam/geometry/geometry.i | 58 ++++++++++++++++++ gtsam/geometry/triangulation.h | 10 ++- gtsam/linear/Coreset.h | 81 +++++++++++++++++++++++++ gtsam/nonlinear/nonlinear.i | 57 +++++++++++++---- gtsam/sfm/sfm.i | 2 + gtsam/slam/slam.i | 15 ++++- python/CMakeLists.txt | 2 + python/gtsam/specializations/geometry.h | 4 ++ 10 files changed, 220 insertions(+), 17 deletions(-) create mode 100644 gtsam/linear/Coreset.h diff --git a/gtsam/base/base.i b/gtsam/base/base.i index 09278ff5b..c24b04088 100644 --- a/gtsam/base/base.i +++ b/gtsam/base/base.i @@ -6,6 +6,8 @@ namespace gtsam { #include #include +#include +#include #include #include #include @@ -104,8 +106,8 @@ virtual class Value { template + gtsam::Cal3Fisheye, gtsam::Cal3Unified, gtsam::EssentialMatrix, + gtsam::CalibratedCamera, gtsam::imuBias::ConstantBias}> virtual class GenericValue : gtsam::Value { void serializable() const; }; diff --git a/gtsam/geometry/SimpleCamera.h b/gtsam/geometry/SimpleCamera.h index aa00222c7..27b8bb549 100644 --- a/gtsam/geometry/SimpleCamera.h +++ b/gtsam/geometry/SimpleCamera.h @@ -21,6 +21,7 @@ #include #include #include +#include #include #include #include @@ -33,6 +34,7 @@ namespace gtsam { using PinholeCameraCal3Bundler = gtsam::PinholeCamera; using PinholeCameraCal3DS2 = gtsam::PinholeCamera; using PinholeCameraCal3Unified = gtsam::PinholeCamera; + using PinholeCameraCal3Fisheye = gtsam::PinholeCamera; #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 /** diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 6217d9e80..4e56347d3 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -684,6 +684,57 @@ virtual class Cal3Unified : gtsam::Cal3DS2_Base { gtsam::Cal3Unified retract(Vector v) const; Vector localCoordinates(const gtsam::Cal3Unified& c) const; + // Action on Point2 + // Note: the signature of this functions differ from the functions + // with equal name in the base class. + gtsam::Point2 calibrate(const gtsam::Point2& p) const; + gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; +}; + +#include +class Cal3Fisheye { + // Standard Constructors + Cal3Fisheye(); + Cal3Fisheye(double fx, double fy, double s, double u0, double v0, double k1, + double k2, double k3, double k4, double tol = 1e-5); + Cal3Fisheye(Vector v); + + // Testable + void print(string s = "Cal3Fisheye") const; + bool equals(const gtsam::Cal3Fisheye& rhs, double tol) const; + + // Manifold + static size_t Dim(); + size_t dim() const; + gtsam::Cal3Fisheye retract(Vector v) const; + Vector localCoordinates(const gtsam::Cal3Fisheye& c) const; + + // Action on Point2 + 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 skew() const; + double k1() const; + double k2() const; + double k3() const; + double k4() const; + double px() const; + double py() const; + gtsam::Point2 principalPoint() const; + Vector vector() const; + Vector k() const; + Matrix K() const; + Matrix inverse() const; + // enabling serialization functionality void serialize() const; @@ -860,6 +911,7 @@ typedef gtsam::PinholeCamera PinholeCameraCal3_S2; typedef gtsam::PinholeCamera PinholeCameraCal3DS2; typedef gtsam::PinholeCamera PinholeCameraCal3Unified; typedef gtsam::PinholeCamera PinholeCameraCal3Bundler; +typedef gtsam::PinholeCamera PinholeCameraCal3Fisheye; template class CameraSet { @@ -924,6 +976,12 @@ gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3_S2& cameras, gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Bundler& cameras, const gtsam::Point2Vector& measurements, double rank_tol, bool optimize); +gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Fisheye& cameras, + const gtsam::Point2Vector& measurements, + double rank_tol, bool optimize); +gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Unified& cameras, + const gtsam::Point2Vector& measurements, + double rank_tol, bool optimize); #include template diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 1df9efd22..6f6ade6f7 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -18,14 +18,16 @@ #pragma once -#include #include +#include +#include +#include #include #include #include -#include -#include #include +#include +#include namespace gtsam { @@ -499,6 +501,8 @@ TriangulationResult triangulateSafe(const CameraSet& cameras, // Vector of Cameras - used by the Python/MATLAB wrapper using CameraSetCal3Bundler = CameraSet>; using CameraSetCal3_S2 = CameraSet>; +using CameraSetCal3Fisheye = CameraSet>; +using CameraSetCal3Unified = CameraSet>; } // \namespace gtsam diff --git a/gtsam/linear/Coreset.h b/gtsam/linear/Coreset.h new file mode 100644 index 000000000..f622d9c81 --- /dev/null +++ b/gtsam/linear/Coreset.h @@ -0,0 +1,81 @@ +#include + +#include + +namespace gtsam { + +Vector FastCaratheodory(Matrix& P, Vector& weights, size_t coreset_size) { + size_t n = P.rows(), d = P.cols(); + size_t m = 2 * d + 2; + + if (n < d + 1) { + return weights; + } + + Vector weights = weights / weights.sum(); + + size_t chunk_size = ceil(n / m); + size_t current_m = ceil(n / chunk_size); + + size_t add_z = chunk_size - size_t(n % chunk_size); + Matrix u(weights.size(), 1); + u.col(0) = weights; + + if (add_z != chunk_size) { + Matrix zeros = Matrix::Zero(add_z, d); + Matrix P_new = Matrix(P.rows() + zeros.rows(), P.cols() + zeros.cols()); + P_new << P, zeros; + zeros = Matrix::Zero(add_z, u.cols()); + Matrix u_new(u.rows() + zeros.rows(), u.cols() + zeros.cols()); + u_new << u, zeros; + } + + Vector idxarray = Vector::LinSpaced(n, 0, n - 1); + Eigen::Tensor p_groups; + + // p_groups = P.reshape(current_m, chunk_size, P.shape[1]) + // u_groups = u.reshape(current_m, chunk_size) + // idx_group = idxarray.reshape(current_m, chunk_size) + // u_nonzero = np.count_nonzero(u) + + // if not coreset_size: + // coreset_size = d+1 + // while u_nonzero > coreset_size: + + // groups_means = np.einsum('ijk,ij->ik',p_groups, u_groups) + // group_weigts = np.ones(groups_means.shape[0], dtype = + // dtype)*1/current_m + + // Cara_u_idx = Caratheodory(groups_means , group_weigts,dtype = dtype ) + + // IDX = np.nonzero(Cara_u_idx) + + // new_P = p_groups[IDX].reshape(-1,d) + + // subset_u = (current_m * u_groups[IDX] * Cara_u_idx[IDX][:, + // np.newaxis]).reshape(-1, 1) new_idx_array = + // idx_group[IDX].reshape(-1,1) + // ##############################################################################3 + // u_nonzero = np.count_nonzero(subset_u) + // chunk_size = math.ceil(new_P.shape[0]/ m) + // current_m = math.ceil(new_P.shape[0]/ chunk_size) + + // add_z = chunk_size - int(new_P.shape[0] % chunk_size) + // if add_z != chunk_size: + // new_P = np.concatenate((new_P, np.zeros((add_z, new_P.shape[1]), + // dtype = dtype))) subset_u = np.concatenate((subset_u, + // np.zeros((add_z, subset_u.shape[1]),dtype = dtype))) + // new_idx_array = np.concatenate((new_idx_array, np.zeros((add_z, + // new_idx_array.shape[1]),dtype = dtype))) + // p_groups = new_P.reshape(current_m, chunk_size, new_P.shape[1]) + // u_groups = subset_u.reshape(current_m, chunk_size) + // idx_group = new_idx_array.reshape(current_m , chunk_size) + // ########################################################### + + // new_u = np.zeros(n) + // subset_u = subset_u[(new_idx_array < n)] + // new_idx_array = new_idx_array[(new_idx_array < + // n)].reshape(-1).astype(int) new_u[new_idx_array] = subset_u return u_sum + // * new_u +} +} // namespace gtsam \ No newline at end of file diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index e22ac5954..8071e8bc7 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -6,6 +6,8 @@ namespace gtsam { #include #include +#include +#include #include #include #include @@ -151,11 +153,25 @@ class NonlinearFactorGraph { gtsam::KeySet keys() const; gtsam::KeyVector keyVector() const; - template , + template , gtsam::PinholeCamera, + gtsam::PinholeCamera, + gtsam::PinholeCamera, gtsam::imuBias::ConstantBias}> void addPrior(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); @@ -291,10 +307,13 @@ class Values { void insert(size_t j, const gtsam::Cal3_S2& cal3_s2); void insert(size_t j, const gtsam::Cal3DS2& cal3ds2); void insert(size_t j, const gtsam::Cal3Bundler& cal3bundler); + void insert(size_t j, const gtsam::Cal3Fisheye& cal3fisheye); + void insert(size_t j, const gtsam::Cal3Unified& cal3unified); void insert(size_t j, const gtsam::EssentialMatrix& essential_matrix); - void insert(size_t j, - const gtsam::PinholeCamera& simple_camera); + void insert(size_t j, const gtsam::PinholeCamera& camera); void insert(size_t j, const gtsam::PinholeCamera& camera); + void insert(size_t j, const gtsam::PinholeCamera& camera); + void insert(size_t j, const gtsam::PinholeCamera& camera); void insert(size_t j, const gtsam::imuBias::ConstantBias& constant_bias); void insert(size_t j, const gtsam::NavState& nav_state); void insert(size_t j, double c); @@ -312,10 +331,13 @@ class Values { void update(size_t j, const gtsam::Cal3_S2& cal3_s2); void update(size_t j, const gtsam::Cal3DS2& cal3ds2); void update(size_t j, const gtsam::Cal3Bundler& cal3bundler); + void update(size_t j, const gtsam::Cal3Fisheye& cal3fisheye); + void update(size_t j, const gtsam::Cal3Unified& cal3unified); void update(size_t j, const gtsam::EssentialMatrix& essential_matrix); - void update(size_t j, - const gtsam::PinholeCamera& simple_camera); + void update(size_t j, const gtsam::PinholeCamera& camera); void update(size_t j, const gtsam::PinholeCamera& camera); + void update(size_t j, const gtsam::PinholeCamera& camera); + void update(size_t j, const gtsam::PinholeCamera& camera); void update(size_t j, const gtsam::imuBias::ConstantBias& constant_bias); void update(size_t j, const gtsam::NavState& nav_state); void update(size_t j, Vector vector); @@ -335,9 +357,13 @@ class Values { gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, + gtsam::Cal3Fisheye, + gtsam::Cal3Unified, gtsam::EssentialMatrix, gtsam::PinholeCamera, gtsam::PinholeCamera, + gtsam::PinholeCamera, + gtsam::PinholeCamera, gtsam::imuBias::ConstantBias, gtsam::NavState, Vector, @@ -681,7 +707,9 @@ class ISAM2 { gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, gtsam::EssentialMatrix, gtsam::PinholeCamera, - gtsam::PinholeCamera, Vector, Matrix}> + gtsam::PinholeCamera, + gtsam::PinholeCamera, + gtsam::PinholeCamera, Vector, Matrix}> VALUE calculateEstimate(size_t key) const; gtsam::Values calculateBestEstimate() const; Matrix marginalCovariance(size_t key) const; @@ -734,10 +762,14 @@ template , - gtsam::imuBias::ConstantBias, - gtsam::PinholeCamera}> + gtsam::PinholeCamera, + gtsam::PinholeCamera, + gtsam::PinholeCamera, + gtsam::imuBias::ConstantBias}> virtual class PriorFactor : gtsam::NoiseModelFactor { PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); @@ -755,6 +787,9 @@ template , + gtsam::PinholeCamera, + gtsam::PinholeCamera, + gtsam::PinholeCamera, gtsam::imuBias::ConstantBias}> virtual class NonlinearEquality : gtsam::NoiseModelFactor { // Constructor - forces exact evaluation diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index c86de8d88..705892e60 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -87,6 +87,8 @@ class ShonanAveraging2 { ShonanAveraging2(string g2oFile); ShonanAveraging2(string g2oFile, const gtsam::ShonanAveragingParameters2& parameters); + ShonanAveraging2(const gtsam::BetweenFactorPose2s &factors, + const gtsam::ShonanAveragingParameters2 ¶meters); // Query properties size_t nrUnknowns() const; diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i index 457e907b9..1c04fd14c 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -62,6 +62,12 @@ typedef gtsam::GenericProjectionFactor GenericProjectionFactorCal3DS2; +typedef gtsam::GenericProjectionFactor + GenericProjectionFactorCal3Fisheye; +typedef gtsam::GenericProjectionFactor + GenericProjectionFactorCal3Unified; #include template @@ -80,8 +86,15 @@ typedef gtsam::GeneralSFMFactor, typedef gtsam::GeneralSFMFactor, gtsam::Point3> GeneralSFMFactorCal3Bundler; +typedef gtsam::GeneralSFMFactor, + gtsam::Point3> + GeneralSFMFactorCal3Fisheye; +typedef gtsam::GeneralSFMFactor, + gtsam::Point3> + GeneralSFMFactorCal3Unified; -template +template virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor { GeneralSFMFactor2(const gtsam::Point2& measured, const gtsam::noiseModel::Base* model, size_t poseKey, diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 676479bd5..7b8347da5 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -116,6 +116,8 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON) gtsam::BinaryMeasurementsUnit3 gtsam::CameraSetCal3_S2 gtsam::CameraSetCal3Bundler + gtsam::CameraSetCal3Unified + gtsam::CameraSetCal3Fisheye gtsam::KeyPairDoubleMap) pybind_wrap(gtsam_unstable_py # target diff --git a/python/gtsam/specializations/geometry.h b/python/gtsam/specializations/geometry.h index 3d22581d9..e11d3cc4f 100644 --- a/python/gtsam/specializations/geometry.h +++ b/python/gtsam/specializations/geometry.h @@ -21,3 +21,7 @@ py::bind_vector>>( m_, "CameraSetCal3_S2"); py::bind_vector>>( m_, "CameraSetCal3Bundler"); +py::bind_vector>>( + m_, "CameraSetCal3Unified"); +py::bind_vector>>( + m_, "CameraSetCal3Fisheye"); From 6db646d80057555925845d7618a0f9284e77033f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 15 Jul 2021 00:25:40 -0400 Subject: [PATCH 066/248] remove extraneous file --- gtsam/linear/Coreset.h | 81 ------------------------------------------ 1 file changed, 81 deletions(-) delete mode 100644 gtsam/linear/Coreset.h diff --git a/gtsam/linear/Coreset.h b/gtsam/linear/Coreset.h deleted file mode 100644 index f622d9c81..000000000 --- a/gtsam/linear/Coreset.h +++ /dev/null @@ -1,81 +0,0 @@ -#include - -#include - -namespace gtsam { - -Vector FastCaratheodory(Matrix& P, Vector& weights, size_t coreset_size) { - size_t n = P.rows(), d = P.cols(); - size_t m = 2 * d + 2; - - if (n < d + 1) { - return weights; - } - - Vector weights = weights / weights.sum(); - - size_t chunk_size = ceil(n / m); - size_t current_m = ceil(n / chunk_size); - - size_t add_z = chunk_size - size_t(n % chunk_size); - Matrix u(weights.size(), 1); - u.col(0) = weights; - - if (add_z != chunk_size) { - Matrix zeros = Matrix::Zero(add_z, d); - Matrix P_new = Matrix(P.rows() + zeros.rows(), P.cols() + zeros.cols()); - P_new << P, zeros; - zeros = Matrix::Zero(add_z, u.cols()); - Matrix u_new(u.rows() + zeros.rows(), u.cols() + zeros.cols()); - u_new << u, zeros; - } - - Vector idxarray = Vector::LinSpaced(n, 0, n - 1); - Eigen::Tensor p_groups; - - // p_groups = P.reshape(current_m, chunk_size, P.shape[1]) - // u_groups = u.reshape(current_m, chunk_size) - // idx_group = idxarray.reshape(current_m, chunk_size) - // u_nonzero = np.count_nonzero(u) - - // if not coreset_size: - // coreset_size = d+1 - // while u_nonzero > coreset_size: - - // groups_means = np.einsum('ijk,ij->ik',p_groups, u_groups) - // group_weigts = np.ones(groups_means.shape[0], dtype = - // dtype)*1/current_m - - // Cara_u_idx = Caratheodory(groups_means , group_weigts,dtype = dtype ) - - // IDX = np.nonzero(Cara_u_idx) - - // new_P = p_groups[IDX].reshape(-1,d) - - // subset_u = (current_m * u_groups[IDX] * Cara_u_idx[IDX][:, - // np.newaxis]).reshape(-1, 1) new_idx_array = - // idx_group[IDX].reshape(-1,1) - // ##############################################################################3 - // u_nonzero = np.count_nonzero(subset_u) - // chunk_size = math.ceil(new_P.shape[0]/ m) - // current_m = math.ceil(new_P.shape[0]/ chunk_size) - - // add_z = chunk_size - int(new_P.shape[0] % chunk_size) - // if add_z != chunk_size: - // new_P = np.concatenate((new_P, np.zeros((add_z, new_P.shape[1]), - // dtype = dtype))) subset_u = np.concatenate((subset_u, - // np.zeros((add_z, subset_u.shape[1]),dtype = dtype))) - // new_idx_array = np.concatenate((new_idx_array, np.zeros((add_z, - // new_idx_array.shape[1]),dtype = dtype))) - // p_groups = new_P.reshape(current_m, chunk_size, new_P.shape[1]) - // u_groups = subset_u.reshape(current_m, chunk_size) - // idx_group = new_idx_array.reshape(current_m , chunk_size) - // ########################################################### - - // new_u = np.zeros(n) - // subset_u = subset_u[(new_idx_array < n)] - // new_idx_array = new_idx_array[(new_idx_array < - // n)].reshape(-1).astype(int) new_u[new_idx_array] = subset_u return u_sum - // * new_u -} -} // namespace gtsam \ No newline at end of file From f819b1a03f74f289f50b96125610026618601a2f Mon Sep 17 00:00:00 2001 From: Akash Patel <17132214+acxz@users.noreply.github.com> Date: Thu, 15 Jul 2021 15:01:56 -0400 Subject: [PATCH 067/248] Revert "replace deprecated tbb functionality" --- gtsam/base/treeTraversal-inst.h | 5 +- .../treeTraversal/parallelTraversalTasks.h | 70 ++++++++++++------- 2 files changed, 46 insertions(+), 29 deletions(-) diff --git a/gtsam/base/treeTraversal-inst.h b/gtsam/base/treeTraversal-inst.h index 30cec3b9a..7a88f72eb 100644 --- a/gtsam/base/treeTraversal-inst.h +++ b/gtsam/base/treeTraversal-inst.h @@ -158,8 +158,9 @@ void DepthFirstForestParallel(FOREST& forest, DATA& rootData, // Typedefs typedef typename FOREST::Node Node; - internal::CreateRootTask(forest.roots(), rootData, visitorPre, - visitorPost, problemSizeThreshold); + tbb::task::spawn_root_and_wait( + internal::CreateRootTask(forest.roots(), rootData, visitorPre, + visitorPost, problemSizeThreshold)); #else DepthFirstForest(forest, rootData, visitorPre, visitorPost); #endif diff --git a/gtsam/base/treeTraversal/parallelTraversalTasks.h b/gtsam/base/treeTraversal/parallelTraversalTasks.h index dc1b45906..87d5b0d4c 100644 --- a/gtsam/base/treeTraversal/parallelTraversalTasks.h +++ b/gtsam/base/treeTraversal/parallelTraversalTasks.h @@ -22,7 +22,7 @@ #include #ifdef GTSAM_USE_TBB -#include // tbb::task_group +#include // tbb::task, tbb::task_list #include // tbb::scalable_allocator namespace gtsam { @@ -34,7 +34,7 @@ namespace gtsam { /* ************************************************************************* */ template - class PreOrderTask + class PreOrderTask : public tbb::task { public: const boost::shared_ptr& treeNode; @@ -42,30 +42,28 @@ namespace gtsam { VISITOR_PRE& visitorPre; VISITOR_POST& visitorPost; int problemSizeThreshold; - tbb::task_group& tg; bool makeNewTasks; - // Keep track of order phase across multiple calls to the same functor - mutable bool isPostOrderPhase; + bool isPostOrderPhase; PreOrderTask(const boost::shared_ptr& treeNode, const boost::shared_ptr& myData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, int problemSizeThreshold, - tbb::task_group& tg, bool makeNewTasks = true) + bool makeNewTasks = true) : treeNode(treeNode), myData(myData), visitorPre(visitorPre), visitorPost(visitorPost), problemSizeThreshold(problemSizeThreshold), - tg(tg), makeNewTasks(makeNewTasks), isPostOrderPhase(false) {} - void operator()() const + tbb::task* execute() override { if(isPostOrderPhase) { // Run the post-order visitor since this task was recycled to run the post-order visitor (void) visitorPost(treeNode, *myData); + return nullptr; } else { @@ -73,10 +71,14 @@ namespace gtsam { { if(!treeNode->children.empty()) { + // Allocate post-order task as a continuation + isPostOrderPhase = true; + recycle_as_continuation(); + bool overThreshold = (treeNode->problemSize() >= problemSizeThreshold); - // If we have child tasks, start subtasks and wait for them to complete - tbb::task_group ctg; + tbb::task* firstChild = 0; + tbb::task_list childTasks; for(const boost::shared_ptr& child: treeNode->children) { // Process child in a subtask. Important: Run visitorPre before calling @@ -84,30 +86,37 @@ namespace gtsam { // allocated an extra child, this causes a TBB error. boost::shared_ptr childData = boost::allocate_shared( tbb::scalable_allocator(), visitorPre(child, *myData)); - ctg.run(PreOrderTask(child, childData, visitorPre, visitorPost, - problemSizeThreshold, ctg, overThreshold)); + tbb::task* childTask = + new (allocate_child()) PreOrderTask(child, childData, visitorPre, visitorPost, + problemSizeThreshold, overThreshold); + if (firstChild) + childTasks.push_back(*childTask); + else + firstChild = childTask; } - ctg.wait(); - // Allocate post-order task as a continuation - isPostOrderPhase = true; - tg.run(*this); + // If we have child tasks, start subtasks and wait for them to complete + set_ref_count((int)treeNode->children.size()); + spawn(childTasks); + return firstChild; } else { // Run the post-order visitor in this task if we have no children (void) visitorPost(treeNode, *myData); + return nullptr; } } else { // Process this node and its children in this task processNodeRecursively(treeNode, *myData); + return nullptr; } } } - void processNodeRecursively(const boost::shared_ptr& node, DATA& myData) const + void processNodeRecursively(const boost::shared_ptr& node, DATA& myData) { for(const boost::shared_ptr& child: node->children) { @@ -122,7 +131,7 @@ namespace gtsam { /* ************************************************************************* */ template - class RootTask + class RootTask : public tbb::task { public: const ROOTS& roots; @@ -130,31 +139,38 @@ namespace gtsam { VISITOR_PRE& visitorPre; VISITOR_POST& visitorPost; int problemSizeThreshold; - tbb::task_group& tg; RootTask(const ROOTS& roots, DATA& myData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, - int problemSizeThreshold, tbb::task_group& tg) : + int problemSizeThreshold) : roots(roots), myData(myData), visitorPre(visitorPre), visitorPost(visitorPost), - problemSizeThreshold(problemSizeThreshold), tg(tg) {} + problemSizeThreshold(problemSizeThreshold) {} - void operator()() const + tbb::task* execute() override { typedef PreOrderTask PreOrderTask; // Create data and tasks for our children + tbb::task_list tasks; for(const boost::shared_ptr& root: roots) { boost::shared_ptr rootData = boost::allocate_shared(tbb::scalable_allocator(), visitorPre(root, myData)); - tg.run(PreOrderTask(root, rootData, visitorPre, visitorPost, problemSizeThreshold, tg)); + tasks.push_back(*new(allocate_child()) + PreOrderTask(root, rootData, visitorPre, visitorPost, problemSizeThreshold)); } + // Set TBB ref count + set_ref_count(1 + (int) roots.size()); + // Spawn tasks + spawn_and_wait_for_all(tasks); + // Return nullptr + return nullptr; } }; template - void CreateRootTask(const ROOTS& roots, DATA& rootData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, int problemSizeThreshold) + RootTask& + CreateRootTask(const ROOTS& roots, DATA& rootData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, int problemSizeThreshold) { typedef RootTask RootTask; - tbb::task_group tg; - tg.run_and_wait(RootTask(roots, rootData, visitorPre, visitorPost, problemSizeThreshold, tg)); - } + return *new(tbb::task::allocate_root()) RootTask(roots, rootData, visitorPre, visitorPost, problemSizeThreshold); + } } From 1d75a0738a9a9cb187b61cdedfb13c4ae5e9019e Mon Sep 17 00:00:00 2001 From: Scott Date: Sat, 17 Jul 2021 18:35:58 -0700 Subject: [PATCH 068/248] Try macOS fix --- gtsam/nonlinear/tests/testSerializationNonlinear.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp index 037582654..f4bb5f4f6 100644 --- a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp +++ b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp @@ -126,7 +126,8 @@ TEST(Serialization, ISAM2) { graph.add(gtsam::PriorFactor(symbol0, pose0, noiseModel)); initialValues.insert(symbol0, pose0); - solver.update(graph, initialValues, gtsam::FastVector()); + solver.update(graph, initialValues, + gtsam::FastVector()); std::string binaryPath = "saved_solver.dat"; try { From cd1d4b4df5b214ec49d2adc4e47056f576a20a03 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Mon, 19 Jul 2021 14:38:26 -0400 Subject: [PATCH 069/248] added templates for factors --- ...martProjectionPoseFactorRollingShutter.cpp | 125 ++ .../SmartProjectionPoseFactorRollingShutter.h | 369 +++++ ...martProjectionPoseFactorRollingShutter.cpp | 1382 +++++++++++++++++ 3 files changed, 1876 insertions(+) create mode 100644 gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.cpp create mode 100644 gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h create mode 100644 gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.cpp new file mode 100644 index 000000000..7ab28e614 --- /dev/null +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.cpp @@ -0,0 +1,125 @@ +/* ---------------------------------------------------------------------------- + + * 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 SmartProjectionProjectionPoseFactorRollingShutter.h + * @brief Smart projection factor on poses and extrinsic calibration + * @author Luca Carlone + * @author Frank Dellaert + */ + +#include + +namespace gtsam { + +SmartStereoProjectionFactorPP::SmartStereoProjectionFactorPP( + const SharedNoiseModel& sharedNoiseModel, + const SmartStereoProjectionParams& params) + : Base(sharedNoiseModel, params) {} // note: no extrinsic specified! + +void SmartStereoProjectionFactorPP::add( + const StereoPoint2& measured, + const Key& w_P_body_key, const Key& body_P_cam_key, + const boost::shared_ptr& K) { + // we index by cameras.. + Base::add(measured, w_P_body_key); + // but we also store the extrinsic calibration keys in the same order + world_P_body_keys_.push_back(w_P_body_key); + body_P_cam_keys_.push_back(body_P_cam_key); + + // pose keys are assumed to be unique (1 observation per time stamp), but calibration can be shared + if(std::find(keys_.begin(), keys_.end(), body_P_cam_key) == keys_.end()) + keys_.push_back(body_P_cam_key); // add only unique keys + + K_all_.push_back(K); +} + +void SmartStereoProjectionFactorPP::add( + const std::vector& measurements, + const KeyVector& world_P_body_keys, const KeyVector& body_P_cam_keys, + const std::vector>& Ks) { + assert(world_P_body_keys.size() == measurements.size()); + assert(world_P_body_keys.size() == body_P_cam_keys.size()); + assert(world_P_body_keys.size() == Ks.size()); + for (size_t i = 0; i < measurements.size(); i++) { + Base::add(measurements[i], world_P_body_keys[i]); + // pose keys are assumed to be unique (1 observation per time stamp), but calibration can be shared + if(std::find(keys_.begin(), keys_.end(), body_P_cam_keys[i]) == keys_.end()) + keys_.push_back(body_P_cam_keys[i]); // add only unique keys + + world_P_body_keys_.push_back(world_P_body_keys[i]); + body_P_cam_keys_.push_back(body_P_cam_keys[i]); + + K_all_.push_back(Ks[i]); + } +} + +void SmartStereoProjectionFactorPP::add( + const std::vector& measurements, + const KeyVector& world_P_body_keys, const KeyVector& body_P_cam_keys, + const boost::shared_ptr& K) { + assert(world_P_body_keys.size() == measurements.size()); + assert(world_P_body_keys.size() == body_P_cam_keys.size()); + for (size_t i = 0; i < measurements.size(); i++) { + Base::add(measurements[i], world_P_body_keys[i]); + // pose keys are assumed to be unique (1 observation per time stamp), but calibration can be shared + if(std::find(keys_.begin(), keys_.end(), body_P_cam_keys[i]) == keys_.end()) + keys_.push_back(body_P_cam_keys[i]); // add only unique keys + + world_P_body_keys_.push_back(world_P_body_keys[i]); + body_P_cam_keys_.push_back(body_P_cam_keys[i]); + + K_all_.push_back(K); + } +} + +void SmartStereoProjectionFactorPP::print( + const std::string& s, const KeyFormatter& keyFormatter) const { + std::cout << s << "SmartStereoProjectionFactorPP: \n "; + for (size_t i = 0; i < K_all_.size(); i++) { + K_all_[i]->print("calibration = "); + std::cout << " extrinsic pose key: " << keyFormatter(body_P_cam_keys_[i]) << std::endl; + } + Base::print("", keyFormatter); +} + +bool SmartStereoProjectionFactorPP::equals(const NonlinearFactor& p, + double tol) const { + const SmartStereoProjectionFactorPP* e = + dynamic_cast(&p); + + return e && Base::equals(p, tol) && + body_P_cam_keys_ == e->getExtrinsicPoseKeys(); +} + +double SmartStereoProjectionFactorPP::error(const Values& values) const { + if (this->active(values)) { + return this->totalReprojectionError(cameras(values)); + } else { // else of active flag + return 0.0; + } +} + +SmartStereoProjectionFactorPP::Base::Cameras +SmartStereoProjectionFactorPP::cameras(const Values& values) const { + assert(world_P_body_keys_.size() == K_all_.size()); + assert(world_P_body_keys_.size() == body_P_cam_keys_.size()); + Base::Cameras cameras; + for (size_t i = 0; i < world_P_body_keys_.size(); i++) { + Pose3 w_P_body = values.at(world_P_body_keys_[i]); + Pose3 body_P_cam = values.at(body_P_cam_keys_[i]); + Pose3 w_P_cam = w_P_body.compose(body_P_cam); + cameras.push_back(StereoCamera(w_P_cam, K_all_[i])); + } + return cameras; +} + +} // \ namespace gtsam diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h new file mode 100644 index 000000000..40d90d614 --- /dev/null +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -0,0 +1,369 @@ +/* ---------------------------------------------------------------------------- + + * 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 SmartStereoProjectionFactorPP.h + * @brief Smart stereo factor on poses (P) and camera extrinsic pose (P) calibrations + * @author Luca Carlone + * @author Frank Dellaert + */ + +#pragma once + +#include + +namespace gtsam { +/** + * + * @addtogroup SLAM + * + * If you are using the factor, please cite: + * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, + * Eliminating conditionally independent sets in factor graphs: + * a unifying perspective based on smart factors, + * Int. Conf. on Robotics and Automation (ICRA), 2014. + */ + +/** + * This factor optimizes the pose of the body as well as the extrinsic camera calibration (pose of camera wrt body). + * Each camera may have its own extrinsic calibration or the same calibration can be shared by multiple cameras. + * This factor requires that values contain the involved poses and extrinsics (both are Pose3 variables). + * @addtogroup SLAM + */ +class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor { + protected: + /// shared pointer to calibration object (one for each camera) + std::vector> K_all_; + + /// The keys corresponding to the pose of the body (with respect to an external world frame) for each view + KeyVector world_P_body_keys_; + + /// The keys corresponding to the extrinsic pose calibration for each view (pose that transform from camera to body) + KeyVector body_P_cam_keys_; + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /// shorthand for base class type + typedef SmartStereoProjectionFactor Base; + + /// shorthand for this class + typedef SmartStereoProjectionFactorPP This; + + /// shorthand for a smart pointer to a factor + typedef boost::shared_ptr shared_ptr; + + static const int Dim = 12; ///< Camera dimension: 6 for body pose, 6 for extrinsic pose + static const int DimPose = 6; ///< Pose3 dimension + static const int ZDim = 3; ///< Measurement dimension (for a StereoPoint2 measurement) + typedef Eigen::Matrix MatrixZD; // F blocks (derivatives wrt camera) + typedef std::vector > FBlocks; // vector of F blocks + + /** + * Constructor + * @param Isotropic measurement noise + * @param params internal parameters of the smart factors + */ + SmartStereoProjectionFactorPP(const SharedNoiseModel& sharedNoiseModel, + const SmartStereoProjectionParams& params = + SmartStereoProjectionParams()); + + /** Virtual destructor */ + ~SmartStereoProjectionFactorPP() override = default; + + /** + * add a new measurement, with a pose key, and an extrinsic pose key + * @param measured is the 3-dimensional location of the projection of a + * single landmark in the a single (stereo) view (the measurement) + * @param world_P_body_key is the key corresponding to the body poses observing the same landmark + * @param body_P_cam_key is the key corresponding to the extrinsic camera-to-body pose calibration + * @param K is the (fixed) camera intrinsic calibration + */ + void add(const StereoPoint2& measured, const Key& world_P_body_key, + const Key& body_P_cam_key, + const boost::shared_ptr& K); + + /** + * Variant of the previous one in which we include a set of measurements + * @param measurements vector of the 3m dimensional location of the projection + * of a single landmark in the m (stereo) view (the measurements) + * @param w_P_body_keys are the ordered keys corresponding to the body poses observing the same landmark + * @param body_P_cam_keys are the ordered keys corresponding to the extrinsic camera-to-body poses calibration + * (note: elements of this vector do not need to be unique: 2 camera views can share the same calibration) + * @param Ks vector of intrinsic calibration objects + */ + void add(const std::vector& measurements, + const KeyVector& w_P_body_keys, const KeyVector& body_P_cam_keys, + const std::vector>& Ks); + + /** + * Variant of the previous one in which we include a set of measurements with + * the same noise and calibration + * @param measurements vector of the 3m dimensional location of the projection + * of a single landmark in the m (stereo) view (the measurements) + * @param w_P_body_keys are the ordered keys corresponding to the body poses observing the same landmark + * @param body_P_cam_keys are the ordered keys corresponding to the extrinsic camera-to-body poses calibration + * (note: elements of this vector do not need to be unique: 2 camera views can share the same calibration) + * @param K the (known) camera calibration (same for all measurements) + */ + void add(const std::vector& measurements, + const KeyVector& w_P_body_keys, const KeyVector& body_P_cam_keys, + const boost::shared_ptr& K); + + /** + * print + * @param s optional string naming the factor + * @param keyFormatter optional formatter useful for printing Symbols + */ + void print(const std::string& s = "", const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const override; + + /// equals + bool equals(const NonlinearFactor& p, double tol = 1e-9) const override; + + /// equals + const KeyVector& getExtrinsicPoseKeys() const { + return body_P_cam_keys_; + } + + /** + * error calculates the error of the factor. + */ + double error(const Values& values) const override; + + /** return the calibration object */ + inline std::vector> calibration() const { + return K_all_; + } + + /** + * Collect all cameras involved in this factor + * @param values Values structure which must contain camera poses + * corresponding + * to keys involved in this factor + * @return vector of Values + */ + Base::Cameras cameras(const Values& values) const override; + + /** + * Compute jacobian F, E and error vector at a given linearization point + * @param values Values structure which must contain camera poses + * corresponding to keys involved in this factor + * @return Return arguments are the camera jacobians Fs (including the jacobian with + * respect to both the body pose and extrinsic pose), the point Jacobian E, + * and the error vector b. Note that the jacobians are computed for a given point. + */ + void computeJacobiansAndCorrectForMissingMeasurements( + FBlocks& Fs, Matrix& E, Vector& b, const Values& values) const { + if (!result_) { + throw("computeJacobiansWithTriangulatedPoint"); + } else { // valid result: compute jacobians + size_t numViews = measured_.size(); + E = Matrix::Zero(3 * numViews, 3); // a StereoPoint2 for each view (point jacobian) + b = Vector::Zero(3 * numViews); // a StereoPoint2 for each view + Matrix dPoseCam_dPoseBody_i, dPoseCam_dPoseExt_i, dProject_dPoseCam_i, Ei; + + for (size_t i = 0; i < numViews; i++) { // for each camera/measurement + Pose3 w_P_body = values.at(world_P_body_keys_.at(i)); + Pose3 body_P_cam = values.at(body_P_cam_keys_.at(i)); + StereoCamera camera( + w_P_body.compose(body_P_cam, dPoseCam_dPoseBody_i, dPoseCam_dPoseExt_i), + K_all_[i]); + // get jacobians and error vector for current measurement + StereoPoint2 reprojectionError_i = StereoPoint2( + camera.project(*result_, dProject_dPoseCam_i, Ei) - measured_.at(i)); + Eigen::Matrix J; // 3 x 12 + J.block(0, 0) = dProject_dPoseCam_i * dPoseCam_dPoseBody_i; // (3x6) * (6x6) + J.block(0, 6) = dProject_dPoseCam_i * dPoseCam_dPoseExt_i; // (3x6) * (6x6) + // if the right pixel is invalid, fix jacobians + if (std::isnan(measured_.at(i).uR())) + { + J.block<1, 12>(1, 0) = Matrix::Zero(1, 12); + Ei.block<1, 3>(1, 0) = Matrix::Zero(1, 3); + reprojectionError_i = StereoPoint2(reprojectionError_i.uL(), 0.0, + reprojectionError_i.v()); + } + // fit into the output structures + Fs.push_back(J); + size_t row = 3 * i; + b.segment(row) = -reprojectionError_i.vector(); + E.block<3, 3>(row, 0) = Ei; + } + } + } + + /// linearize and return a Hessianfactor that is an approximation of error(p) + boost::shared_ptr > createHessianFactor( + const Values& values, const double lambda = 0.0, bool diagonalDamping = + false) const { + + // we may have multiple cameras sharing the same extrinsic cals, hence the number + // of keys may be smaller than 2 * nrMeasurements (which is the upper bound where we + // have a body key and an extrinsic calibration key for each measurement) + size_t nrUniqueKeys = keys_.size(); + size_t nrNonuniqueKeys = world_P_body_keys_.size() + + body_P_cam_keys_.size(); + + // Create structures for Hessian Factors + KeyVector js; + std::vector < Matrix > Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2); + std::vector gs(nrUniqueKeys); + + if (this->measured_.size() != cameras(values).size()) + throw std::runtime_error("SmartStereoProjectionHessianFactor: this->" + "measured_.size() inconsistent with input"); + + // triangulate 3D point at given linearization point + triangulateSafe(cameras(values)); + + if (!result_) { // failed: return "empty/zero" Hessian + for (Matrix& m : Gs) + m = Matrix::Zero(DimPose, DimPose); + for (Vector& v : gs) + v = Vector::Zero(DimPose); + return boost::make_shared < RegularHessianFactor + > (keys_, Gs, gs, 0.0); + } + + // compute Jacobian given triangulated 3D Point + FBlocks Fs; + Matrix F, E; + Vector b; + computeJacobiansAndCorrectForMissingMeasurements(Fs, E, b, values); + + // Whiten using noise model + noiseModel_->WhitenSystem(E, b); + for (size_t i = 0; i < Fs.size(); i++) + Fs[i] = noiseModel_->Whiten(Fs[i]); + + // build augmented Hessian (with last row/column being the information vector) + Matrix3 P; + Cameras::ComputePointCovariance<3>(P, E, lambda, diagonalDamping); + + // marginalize point: note - we reuse the standard SchurComplement function + SymmetricBlockMatrix augmentedHessian = + Cameras::SchurComplement<3, Dim>(Fs, E, P, b); + + // now pack into an Hessian factor + std::vector dims(nrUniqueKeys + 1); // this also includes the b term + std::fill(dims.begin(), dims.end() - 1, 6); + dims.back() = 1; + SymmetricBlockMatrix augmentedHessianUniqueKeys; + + // here we have to deal with the fact that some cameras may share the same extrinsic key + if (nrUniqueKeys == nrNonuniqueKeys) { // if there is 1 calibration key per camera + augmentedHessianUniqueKeys = SymmetricBlockMatrix( + dims, Matrix(augmentedHessian.selfadjointView())); + } else { // if multiple cameras share a calibration we have to rearrange + // the results of the Schur complement matrix + std::vector nonuniqueDims(nrNonuniqueKeys + 1); // this also includes the b term + std::fill(nonuniqueDims.begin(), nonuniqueDims.end() - 1, 6); + nonuniqueDims.back() = 1; + augmentedHessian = SymmetricBlockMatrix( + nonuniqueDims, Matrix(augmentedHessian.selfadjointView())); + + // these are the keys that correspond to the blocks in augmentedHessian (output of SchurComplement) + KeyVector nonuniqueKeys; + for (size_t i = 0; i < world_P_body_keys_.size(); i++) { + nonuniqueKeys.push_back(world_P_body_keys_.at(i)); + nonuniqueKeys.push_back(body_P_cam_keys_.at(i)); + } + + // get map from key to location in the new augmented Hessian matrix (the one including only unique keys) + std::map keyToSlotMap; + for (size_t k = 0; k < nrUniqueKeys; k++) { + keyToSlotMap[keys_[k]] = k; + } + + // initialize matrix to zero + augmentedHessianUniqueKeys = SymmetricBlockMatrix( + dims, Matrix::Zero(6 * nrUniqueKeys + 1, 6 * nrUniqueKeys + 1)); + + // add contributions for each key: note this loops over the hessian with nonUnique keys (augmentedHessian) + // and populates an Hessian that only includes the unique keys (that is what we want to return) + for (size_t i = 0; i < nrNonuniqueKeys; i++) { // rows + Key key_i = nonuniqueKeys.at(i); + + // update information vector + augmentedHessianUniqueKeys.updateOffDiagonalBlock( + keyToSlotMap[key_i], nrUniqueKeys, + augmentedHessian.aboveDiagonalBlock(i, nrNonuniqueKeys)); + + // update blocks + for (size_t j = i; j < nrNonuniqueKeys; j++) { // cols + Key key_j = nonuniqueKeys.at(j); + if (i == j) { + augmentedHessianUniqueKeys.updateDiagonalBlock( + keyToSlotMap[key_i], augmentedHessian.diagonalBlock(i)); + } else { // (i < j) + if (keyToSlotMap[key_i] != keyToSlotMap[key_j]) { + augmentedHessianUniqueKeys.updateOffDiagonalBlock( + keyToSlotMap[key_i], keyToSlotMap[key_j], + augmentedHessian.aboveDiagonalBlock(i, j)); + } else { + augmentedHessianUniqueKeys.updateDiagonalBlock( + keyToSlotMap[key_i], + augmentedHessian.aboveDiagonalBlock(i, j) + + augmentedHessian.aboveDiagonalBlock(i, j).transpose()); + } + } + } + } + // update bottom right element of the matrix + augmentedHessianUniqueKeys.updateDiagonalBlock( + nrUniqueKeys, augmentedHessian.diagonalBlock(nrNonuniqueKeys)); + } + return boost::make_shared < RegularHessianFactor + > (keys_, augmentedHessianUniqueKeys); + } + + /** + * Linearize to Gaussian Factor (possibly adding a damping factor Lambda for LM) + * @param values Values structure which must contain camera poses and extrinsic pose for this factor + * @return a Gaussian factor + */ + boost::shared_ptr linearizeDamped( + const Values& values, const double lambda = 0.0) const { + // depending on flag set on construction we may linearize to different linear factors + switch (params_.linearizationMode) { + case HESSIAN: + return createHessianFactor(values, lambda); + default: + throw std::runtime_error( + "SmartStereoProjectionFactorPP: unknown linearization mode"); + } + } + + /// linearize + boost::shared_ptr linearize(const Values& values) const + override { + return linearizeDamped(values); + } + + private: + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar & BOOST_SERIALIZATION_NVP(K_all_); + } + +}; +// end of class declaration + +/// traits +template<> +struct traits : public Testable< + SmartStereoProjectionFactorPP> { +}; + +} // namespace gtsam diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp new file mode 100644 index 000000000..c7f220c10 --- /dev/null +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -0,0 +1,1382 @@ +/* ---------------------------------------------------------------------------- + + * 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 testSmartProjectionPoseFactor.cpp + * @brief Unit tests for ProjectionFactor Class + * @author Chris Beall + * @author Luca Carlone + * @author Zsolt Kira + * @author Frank Dellaert + * @date Sept 2013 + */ + +#include "smartFactorScenarios.h" +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace boost::assign; +using namespace std::placeholders; + +static const double rankTol = 1.0; +// Create a noise model for the pixel error +static const double sigma = 0.1; +static SharedIsotropic model(noiseModel::Isotropic::Sigma(2, sigma)); + +// Convenience for named keys +using symbol_shorthand::X; +using symbol_shorthand::L; + +// tests data +static Symbol x1('X', 1); +static Symbol x2('X', 2); +static Symbol x3('X', 3); + +static Point2 measurement1(323.0, 240.0); + +LevenbergMarquardtParams lmParams; +// Make more verbose like so (in tests): +// params.verbosityLM = LevenbergMarquardtParams::SUMMARY; + +/* ************************************************************************* */ +TEST( SmartProjectionPoseFactor, Constructor) { + using namespace vanillaPose; + SmartFactor::shared_ptr factor1(new SmartFactor(model, sharedK)); +} + +/* ************************************************************************* */ +TEST( SmartProjectionPoseFactor, Constructor2) { + using namespace vanillaPose; + SmartProjectionParams params; + params.setRankTolerance(rankTol); + SmartFactor factor1(model, sharedK, params); +} + +/* ************************************************************************* */ +TEST( SmartProjectionPoseFactor, Constructor3) { + using namespace vanillaPose; + SmartFactor::shared_ptr factor1(new SmartFactor(model, sharedK)); + factor1->add(measurement1, x1); +} + +/* ************************************************************************* */ +TEST( SmartProjectionPoseFactor, Constructor4) { + using namespace vanillaPose; + SmartProjectionParams params; + params.setRankTolerance(rankTol); + SmartFactor factor1(model, sharedK, params); + factor1.add(measurement1, x1); +} + +/* ************************************************************************* */ +TEST( SmartProjectionPoseFactor, params) { + using namespace vanillaPose; + SmartProjectionParams params; + double rt = params.getRetriangulationThreshold(); + EXPECT_DOUBLES_EQUAL(1e-5, rt, 1e-7); + params.setRetriangulationThreshold(1e-3); + rt = params.getRetriangulationThreshold(); + EXPECT_DOUBLES_EQUAL(1e-3, rt, 1e-7); +} + +/* ************************************************************************* */ +TEST( SmartProjectionPoseFactor, Equals ) { + using namespace vanillaPose; + SmartFactor::shared_ptr factor1(new SmartFactor(model, sharedK)); + factor1->add(measurement1, x1); + + SmartFactor::shared_ptr factor2(new SmartFactor(model,sharedK)); + factor2->add(measurement1, x1); + + CHECK(assert_equal(*factor1, *factor2)); +} + +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, noiseless ) { + + using namespace vanillaPose; + + // Project two landmarks into two cameras + Point2 level_uv = level_camera.project(landmark1); + Point2 level_uv_right = level_camera_right.project(landmark1); + + SmartFactor factor(model, sharedK); + factor.add(level_uv, x1); + factor.add(level_uv_right, x2); + + Values values; // it's a pose factor, hence these are poses + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + + double actualError = factor.error(values); + double expectedError = 0.0; + EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); + + SmartFactor::Cameras cameras = factor.cameras(values); + double actualError2 = factor.totalReprojectionError(cameras); + EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1e-7); + + // Calculate expected derivative for point (easiest to check) + std::function f = // + std::bind(&SmartFactor::whitenedError, factor, cameras, std::placeholders::_1); + + // Calculate using computeEP + Matrix actualE; + factor.triangulateAndComputeE(actualE, values); + + // get point + boost::optional point = factor.point(); + CHECK(point); + + // calculate numerical derivative with triangulated point + Matrix expectedE = sigma * numericalDerivative11(f, *point); + EXPECT(assert_equal(expectedE, actualE, 1e-7)); + + // Calculate using reprojectionError + SmartFactor::Cameras::FBlocks F; + Matrix E; + Vector actualErrors = factor.unwhitenedError(cameras, *point, F, E); + EXPECT(assert_equal(expectedE, E, 1e-7)); + + EXPECT(assert_equal(Z_4x1, actualErrors, 1e-7)); + + // Calculate using computeJacobians + Vector b; + SmartFactor::FBlocks Fs; + factor.computeJacobians(Fs, E, b, cameras, *point); + double actualError3 = b.squaredNorm(); + EXPECT(assert_equal(expectedE, E, 1e-7)); + EXPECT_DOUBLES_EQUAL(expectedError, actualError3, 1e-6); +} + +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, noisy ) { + + using namespace vanillaPose; + + // Project two landmarks into two cameras + Point2 pixelError(0.2, 0.2); + Point2 level_uv = level_camera.project(landmark1) + pixelError; + Point2 level_uv_right = level_camera_right.project(landmark1); + + Values values; + values.insert(x1, cam1.pose()); + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), + Point3(0.5, 0.1, 0.3)); + values.insert(x2, pose_right.compose(noise_pose)); + + SmartFactor::shared_ptr factor(new SmartFactor(model, sharedK)); + factor->add(level_uv, x1); + factor->add(level_uv_right, x2); + + double actualError1 = factor->error(values); + + SmartFactor::shared_ptr factor2(new SmartFactor(model, sharedK)); + Point2Vector measurements; + measurements.push_back(level_uv); + measurements.push_back(level_uv_right); + + KeyVector views {x1, x2}; + + factor2->add(measurements, views); + double actualError2 = factor2->error(values); + DOUBLES_EQUAL(actualError1, actualError2, 1e-7); +} + +/* *************************************************************************/ +TEST(SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform) { + using namespace vanillaPose; + + // create arbitrary body_T_sensor (transforms from sensor to body) + Pose3 body_T_sensor = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(1, 1, 1)); + + // These are the poses we want to estimate, from camera measurements + const Pose3 sensor_T_body = body_T_sensor.inverse(); + Pose3 wTb1 = cam1.pose() * sensor_T_body; + Pose3 wTb2 = cam2.pose() * sensor_T_body; + Pose3 wTb3 = cam3.pose() * sensor_T_body; + + // three landmarks ~5 meters infront of camera + Point3 landmark1(5, 0.5, 1.2), landmark2(5, -0.5, 1.2), landmark3(5, 0, 3.0); + + Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; + + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + + // Create smart factors + KeyVector views {x1, x2, x3}; + + SmartProjectionParams params; + params.setRankTolerance(1.0); + params.setDegeneracyMode(IGNORE_DEGENERACY); + params.setEnableEPI(false); + + SmartFactor smartFactor1(model, sharedK, body_T_sensor, params); + smartFactor1.add(measurements_cam1, views); + + SmartFactor smartFactor2(model, sharedK, body_T_sensor, params); + smartFactor2.add(measurements_cam2, views); + + SmartFactor smartFactor3(model, sharedK, body_T_sensor, params); + smartFactor3.add(measurements_cam3, views); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + // Put all factors in factor graph, adding priors + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.addPrior(x1, wTb1, noisePrior); + graph.addPrior(x2, wTb2, noisePrior); + + // Check errors at ground truth poses + Values gtValues; + gtValues.insert(x1, wTb1); + gtValues.insert(x2, wTb2); + gtValues.insert(x3, wTb3); + double actualError = graph.error(gtValues); + double expectedError = 0.0; + DOUBLES_EQUAL(expectedError, actualError, 1e-7) + + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); + Values values; + values.insert(x1, wTb1); + values.insert(x2, wTb2); + // initialize third pose with some noise, we expect it to move back to + // original pose3 + values.insert(x3, wTb3 * noise_pose); + + LevenbergMarquardtParams lmParams; + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + EXPECT(assert_equal(wTb3, result.at(x3))); +} + +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) { + + using namespace vanillaPose2; + Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; + + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + + KeyVector views; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); + + SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedK2)); + smartFactor1->add(measurements_cam1, views); + + SmartFactor::shared_ptr smartFactor2(new SmartFactor(model, sharedK2)); + smartFactor2->add(measurements_cam2, views); + + SmartFactor::shared_ptr smartFactor3(new SmartFactor(model, sharedK2)); + smartFactor3->add(measurements_cam3, views); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.addPrior(x1, cam1.pose(), noisePrior); + graph.addPrior(x2, cam2.pose(), noisePrior); + + Values groundTruth; + groundTruth.insert(x1, cam1.pose()); + groundTruth.insert(x2, cam2.pose()); + groundTruth.insert(x3, cam3.pose()); + DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9); + + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise + Values values; + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + // initialize third pose with some noise, we expect it to move back to original pose_above + values.insert(x3, pose_above * noise_pose); + EXPECT( + assert_equal( + Pose3( + Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, + -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), + Point3(0.1, -0.1, 1.9)), values.at(x3))); + + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + EXPECT(assert_equal(pose_above, result.at(x3), 1e-6)); +} + +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, Factors ) { + + using namespace vanillaPose; + + // Default cameras for simple derivatives + Rot3 R; + static Cal3_S2::shared_ptr sharedK(new Cal3_S2(100, 100, 0, 0, 0)); + Camera cam1(Pose3(R, Point3(0, 0, 0)), sharedK), cam2( + Pose3(R, Point3(1, 0, 0)), sharedK); + + // one landmarks 1m in front of camera + Point3 landmark1(0, 0, 10); + + Point2Vector measurements_cam1; + + // Project 2 landmarks into 2 cameras + measurements_cam1.push_back(cam1.project(landmark1)); + measurements_cam1.push_back(cam2.project(landmark1)); + + // Create smart factors + KeyVector views {x1, x2}; + + SmartFactor::shared_ptr smartFactor1 = boost::make_shared(model, sharedK); + smartFactor1->add(measurements_cam1, views); + + SmartFactor::Cameras cameras; + cameras.push_back(cam1); + cameras.push_back(cam2); + + // Make sure triangulation works + CHECK(smartFactor1->triangulateSafe(cameras)); + CHECK(!smartFactor1->isDegenerate()); + CHECK(!smartFactor1->isPointBehindCamera()); + boost::optional p = smartFactor1->point(); + CHECK(p); + EXPECT(assert_equal(landmark1, *p)); + + VectorValues zeroDelta; + Vector6 delta; + delta.setZero(); + zeroDelta.insert(x1, delta); + zeroDelta.insert(x2, delta); + + VectorValues perturbedDelta; + delta.setOnes(); + perturbedDelta.insert(x1, delta); + perturbedDelta.insert(x2, delta); + double expectedError = 2500; + + // After eliminating the point, A1 and A2 contain 2-rank information on cameras: + Matrix16 A1, A2; + A1 << -10, 0, 0, 0, 1, 0; + A2 << 10, 0, 1, 0, -1, 0; + A1 *= 10. / sigma; + A2 *= 10. / sigma; + Matrix expectedInformation; // filled below + { + // createHessianFactor + Matrix66 G11 = 0.5 * A1.transpose() * A1; + Matrix66 G12 = 0.5 * A1.transpose() * A2; + Matrix66 G22 = 0.5 * A2.transpose() * A2; + + Vector6 g1; + g1.setZero(); + Vector6 g2; + g2.setZero(); + + double f = 0; + + RegularHessianFactor<6> expected(x1, x2, G11, G12, g1, G22, g2, f); + expectedInformation = expected.information(); + + boost::shared_ptr > actual = + smartFactor1->createHessianFactor(cameras, 0.0); + EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6)); + EXPECT(assert_equal(expected, *actual, 1e-6)); + EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-6); + EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-6); + } + + { + Matrix26 F1; + F1.setZero(); + F1(0, 1) = -100; + F1(0, 3) = -10; + F1(1, 0) = 100; + F1(1, 4) = -10; + Matrix26 F2; + F2.setZero(); + F2(0, 1) = -101; + F2(0, 3) = -10; + F2(0, 5) = -1; + F2(1, 0) = 100; + F2(1, 2) = 10; + F2(1, 4) = -10; + Matrix E(4, 3); + E.setZero(); + E(0, 0) = 10; + E(1, 1) = 10; + E(2, 0) = 10; + E(2, 2) = 1; + E(3, 1) = 10; + SmartFactor::FBlocks Fs = list_of(F1)(F2); + Vector b(4); + b.setZero(); + + // Create smart factors + KeyVector keys; + keys.push_back(x1); + keys.push_back(x2); + + // createJacobianQFactor + SharedIsotropic n = noiseModel::Isotropic::Sigma(4, sigma); + Matrix3 P = (E.transpose() * E).inverse(); + JacobianFactorQ<6, 2> expectedQ(keys, Fs, E, P, b, n); + EXPECT(assert_equal(expectedInformation, expectedQ.information(), 1e-6)); + + boost::shared_ptr > actualQ = + smartFactor1->createJacobianQFactor(cameras, 0.0); + CHECK(actualQ); + EXPECT(assert_equal(expectedInformation, actualQ->information(), 1e-6)); + EXPECT(assert_equal(expectedQ, *actualQ)); + EXPECT_DOUBLES_EQUAL(0, actualQ->error(zeroDelta), 1e-6); + EXPECT_DOUBLES_EQUAL(expectedError, actualQ->error(perturbedDelta), 1e-6); + + // Whiten for RegularImplicitSchurFactor (does not have noise model) + model->WhitenSystem(E, b); + Matrix3 whiteP = (E.transpose() * E).inverse(); + Fs[0] = model->Whiten(Fs[0]); + Fs[1] = model->Whiten(Fs[1]); + + // createRegularImplicitSchurFactor + RegularImplicitSchurFactor expected(keys, Fs, E, whiteP, b); + + boost::shared_ptr > actual = + smartFactor1->createRegularImplicitSchurFactor(cameras, 0.0); + CHECK(actual); + EXPECT(assert_equal(expectedInformation, expected.information(), 1e-6)); + EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6)); + EXPECT(assert_equal(expected, *actual)); + EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-6); + EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-6); + } + + { + // createJacobianSVDFactor + Vector1 b; + b.setZero(); + double s = sigma * sin(M_PI_4); + SharedIsotropic n = noiseModel::Isotropic::Sigma(4 - 3, sigma); + JacobianFactor expected(x1, s * A1, x2, s * A2, b, n); + EXPECT(assert_equal(expectedInformation, expected.information(), 1e-6)); + + boost::shared_ptr actual = + smartFactor1->createJacobianSVDFactor(cameras, 0.0); + CHECK(actual); + EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6)); + EXPECT(assert_equal(expected, *actual)); + EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-6); + EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-6); + } +} + +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) { + + using namespace vanillaPose; + + KeyVector views {x1, x2, x3}; + + Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; + + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + + SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedK)); + smartFactor1->add(measurements_cam1, views); + + SmartFactor::shared_ptr smartFactor2(new SmartFactor(model, sharedK)); + smartFactor2->add(measurements_cam2, views); + + SmartFactor::shared_ptr smartFactor3(new SmartFactor(model, sharedK)); + smartFactor3->add(measurements_cam3, views); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.addPrior(x1, cam1.pose(), noisePrior); + graph.addPrior(x2, cam2.pose(), noisePrior); + + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise + Values values; + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + // initialize third pose with some noise, we expect it to move back to original pose_above + values.insert(x3, pose_above * noise_pose); + EXPECT( + assert_equal( + Pose3( + Rot3(1.11022302e-16, -0.0314107591, 0.99950656, -0.99950656, + -0.0313952598, -0.000986635786, 0.0314107591, -0.999013364, + -0.0313952598), Point3(0.1, -0.1, 1.9)), + values.at(x3))); + + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + EXPECT(assert_equal(pose_above, result.at(x3), 1e-7)); +} + +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, jacobianSVD ) { + + using namespace vanillaPose; + + KeyVector views {x1, x2, x3}; + + Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; + + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + + SmartProjectionParams params; + params.setRankTolerance(1.0); + params.setLinearizationMode(gtsam::JACOBIAN_SVD); + params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); + params.setEnableEPI(false); + SmartFactor factor1(model, sharedK, params); + + SmartFactor::shared_ptr smartFactor1( + new SmartFactor(model, sharedK, params)); + smartFactor1->add(measurements_cam1, views); + + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(model, sharedK, params)); + smartFactor2->add(measurements_cam2, views); + + SmartFactor::shared_ptr smartFactor3( + new SmartFactor(model, sharedK, params)); + smartFactor3->add(measurements_cam3, views); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.addPrior(x1, cam1.pose(), noisePrior); + graph.addPrior(x2, cam2.pose(), noisePrior); + + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise + Values values; + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + values.insert(x3, pose_above * noise_pose); + + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + EXPECT(assert_equal(pose_above, result.at(x3), 1e-6)); +} + +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, landmarkDistance ) { + + using namespace vanillaPose; + + double excludeLandmarksFutherThanDist = 2; + + KeyVector views {x1, x2, x3}; + + Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; + + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + + SmartProjectionParams params; + params.setRankTolerance(1.0); + params.setLinearizationMode(gtsam::JACOBIAN_SVD); + params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); + params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); + params.setEnableEPI(false); + + SmartFactor::shared_ptr smartFactor1( + new SmartFactor(model, sharedK, params)); + smartFactor1->add(measurements_cam1, views); + + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(model, sharedK, params)); + smartFactor2->add(measurements_cam2, views); + + SmartFactor::shared_ptr smartFactor3( + new SmartFactor(model, sharedK, params)); + smartFactor3->add(measurements_cam3, views); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.addPrior(x1, cam1.pose(), noisePrior); + graph.addPrior(x2, cam2.pose(), noisePrior); + + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise + Values values; + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + values.insert(x3, pose_above * noise_pose); + + // All factors are disabled and pose should remain where it is + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + EXPECT(assert_equal(values.at(x3), result.at(x3))); +} + +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) { + + using namespace vanillaPose; + + double excludeLandmarksFutherThanDist = 1e10; + double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error + + KeyVector views {x1, x2, x3}; + + // add fourth landmark + Point3 landmark4(5, -0.5, 1); + + Point2Vector measurements_cam1, measurements_cam2, measurements_cam3, + measurements_cam4; + + // Project 4 landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4); + measurements_cam4.at(0) = measurements_cam4.at(0) + Point2(10, 10); // add outlier + + SmartProjectionParams params; + params.setLinearizationMode(gtsam::JACOBIAN_SVD); + params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); + params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold); + + SmartFactor::shared_ptr smartFactor1( + new SmartFactor(model, sharedK, params)); + smartFactor1->add(measurements_cam1, views); + + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(model, sharedK, params)); + smartFactor2->add(measurements_cam2, views); + + SmartFactor::shared_ptr smartFactor3( + new SmartFactor(model, sharedK, params)); + smartFactor3->add(measurements_cam3, views); + + SmartFactor::shared_ptr smartFactor4( + new SmartFactor(model, sharedK, params)); + smartFactor4->add(measurements_cam4, views); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.push_back(smartFactor4); + graph.addPrior(x1, cam1.pose(), noisePrior); + graph.addPrior(x2, cam2.pose(), noisePrior); + + Values values; + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + values.insert(x3, cam3.pose()); + + // All factors are disabled and pose should remain where it is + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + EXPECT(assert_equal(cam3.pose(), result.at(x3))); +} + +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, jacobianQ ) { + + using namespace vanillaPose; + + KeyVector views {x1, x2, x3}; + + Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; + + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + + SmartProjectionParams params; + params.setLinearizationMode(gtsam::JACOBIAN_Q); + + SmartFactor::shared_ptr smartFactor1( + new SmartFactor(model, sharedK, params)); + smartFactor1->add(measurements_cam1, views); + + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(model, sharedK, params)); + smartFactor2->add(measurements_cam2, views); + + SmartFactor::shared_ptr smartFactor3( + new SmartFactor(model, sharedK, params)); + smartFactor3->add(measurements_cam3, views); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.addPrior(x1, cam1.pose(), noisePrior); + graph.addPrior(x2, cam2.pose(), noisePrior); + + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise + Values values; + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + values.insert(x3, pose_above * noise_pose); + + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + EXPECT(assert_equal(pose_above, result.at(x3), 1e-6)); +} + +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, 3poses_projection_factor ) { + + using namespace vanillaPose2; + + KeyVector views {x1, x2, x3}; + + typedef GenericProjectionFactor ProjectionFactor; + NonlinearFactorGraph graph; + + // Project three landmarks into three cameras + graph.emplace_shared(cam1.project(landmark1), model, x1, L(1), sharedK2); + graph.emplace_shared(cam2.project(landmark1), model, x2, L(1), sharedK2); + graph.emplace_shared(cam3.project(landmark1), model, x3, L(1), sharedK2); + + graph.emplace_shared(cam1.project(landmark2), model, x1, L(2), sharedK2); + graph.emplace_shared(cam2.project(landmark2), model, x2, L(2), sharedK2); + graph.emplace_shared(cam3.project(landmark2), model, x3, L(2), sharedK2); + + graph.emplace_shared(cam1.project(landmark3), model, x1, L(3), sharedK2); + graph.emplace_shared(cam2.project(landmark3), model, x2, L(3), sharedK2); + graph.emplace_shared(cam3.project(landmark3), model, x3, L(3), sharedK2); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + graph.addPrior(x1, level_pose, noisePrior); + graph.addPrior(x2, pose_right, noisePrior); + + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), + Point3(0.5, 0.1, 0.3)); + Values values; + values.insert(x1, level_pose); + values.insert(x2, pose_right); + values.insert(x3, pose_above * noise_pose); + values.insert(L(1), landmark1); + values.insert(L(2), landmark2); + values.insert(L(3), landmark3); + + DOUBLES_EQUAL(48406055, graph.error(values), 1); + + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + Values result = optimizer.optimize(); + + DOUBLES_EQUAL(0, graph.error(result), 1e-9); + + EXPECT(assert_equal(pose_above, result.at(x3), 1e-7)); +} + +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, CheckHessian) { + + KeyVector views {x1, x2, x3}; + + using namespace vanillaPose; + + // Two slightly different cameras + Pose3 pose2 = level_pose + * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); + Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); + Camera cam2(pose2, sharedK); + Camera cam3(pose3, sharedK); + + Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; + + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + + SmartProjectionParams params; + params.setRankTolerance(10); + + SmartFactor::shared_ptr smartFactor1( + new SmartFactor(model, sharedK, params)); // HESSIAN, by default + smartFactor1->add(measurements_cam1, views); + + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(model, sharedK, params)); // HESSIAN, by default + smartFactor2->add(measurements_cam2, views); + + SmartFactor::shared_ptr smartFactor3( + new SmartFactor(model, sharedK, params)); // HESSIAN, by default + smartFactor3->add(measurements_cam3, views); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise + Values values; + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + // initialize third pose with some noise, we expect it to move back to original pose_above + values.insert(x3, pose3 * noise_pose); + EXPECT( + assert_equal( + Pose3( + Rot3(0.00563056869, -0.130848107, 0.991386438, -0.991390265, + -0.130426831, -0.0115837907, 0.130819108, -0.98278564, + -0.130455917), + Point3(0.0897734171, -0.110201006, 0.901022872)), + values.at(x3))); + + boost::shared_ptr factor1 = smartFactor1->linearize(values); + boost::shared_ptr factor2 = smartFactor2->linearize(values); + boost::shared_ptr factor3 = smartFactor3->linearize(values); + + Matrix CumulativeInformation = factor1->information() + factor2->information() + + factor3->information(); + + boost::shared_ptr GaussianGraph = graph.linearize( + values); + Matrix GraphInformation = GaussianGraph->hessian().first; + + // Check Hessian + EXPECT(assert_equal(GraphInformation, CumulativeInformation, 1e-6)); + + Matrix AugInformationMatrix = factor1->augmentedInformation() + + factor2->augmentedInformation() + factor3->augmentedInformation(); + + // Check Information vector + Vector InfoVector = AugInformationMatrix.block(0, 18, 18, 1); // 18x18 Hessian + information vector + + // Check Hessian + EXPECT(assert_equal(InfoVector, GaussianGraph->hessian().second, 1e-6)); +} + +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_factor ) { + using namespace vanillaPose2; + + KeyVector views {x1, x2, x3}; + + // Two different cameras, at the same position, but different rotations + Pose3 pose2 = level_pose * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); + Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); + Camera cam2(pose2, sharedK2); + Camera cam3(pose3, sharedK2); + + Point2Vector measurements_cam1, measurements_cam2; + + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + + SmartProjectionParams params; + params.setRankTolerance(50); + params.setDegeneracyMode(gtsam::HANDLE_INFINITY); + + SmartFactor::shared_ptr smartFactor1( + new SmartFactor(model, sharedK2, params)); + smartFactor1->add(measurements_cam1, views); + + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(model, sharedK2, params)); + smartFactor2->add(measurements_cam2, views); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, 0.10); + Point3 positionPrior = Point3(0, 0, 1); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.addPrior(x1, cam1.pose(), noisePrior); + graph.emplace_shared >(x2, positionPrior, noisePriorTranslation); + graph.emplace_shared >(x3, positionPrior, noisePriorTranslation); + + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise + Values values; + values.insert(x1, cam1.pose()); + values.insert(x2, pose2 * noise_pose); + values.insert(x3, pose3 * noise_pose); + + // params.verbosityLM = LevenbergMarquardtParams::SUMMARY; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + Values result = optimizer.optimize(); + EXPECT(assert_equal(pose3, result.at(x3))); +} + +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ) { + + // this test considers a condition in which the cheirality constraint is triggered + using namespace vanillaPose; + + KeyVector views {x1, x2, x3}; + + // Two different cameras, at the same position, but different rotations + Pose3 pose2 = level_pose + * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); + Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); + Camera cam2(pose2, sharedK); + Camera cam3(pose3, sharedK); + + Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; + + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + + SmartProjectionParams params; + params.setRankTolerance(10); + params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); + + SmartFactor::shared_ptr smartFactor1( + new SmartFactor(model, sharedK, params)); + smartFactor1->add(measurements_cam1, views); + + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(model, sharedK, params)); + smartFactor2->add(measurements_cam2, views); + + SmartFactor::shared_ptr smartFactor3( + new SmartFactor(model, sharedK, params)); + smartFactor3->add(measurements_cam3, views); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, + 0.10); + Point3 positionPrior = Point3(0, 0, 1); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.addPrior(x1, cam1.pose(), noisePrior); + graph.emplace_shared >(x2, positionPrior, noisePriorTranslation); + graph.emplace_shared >(x3, positionPrior, noisePriorTranslation); + + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise + Values values; + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + values.insert(x3, pose3 * noise_pose); + EXPECT( + assert_equal( + Pose3( + Rot3(0.00563056869, -0.130848107, 0.991386438, -0.991390265, + -0.130426831, -0.0115837907, 0.130819108, -0.98278564, + -0.130455917), + Point3(0.0897734171, -0.110201006, 0.901022872)), + values.at(x3))); + + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + + // Since we do not do anything on degenerate instances (ZERO_ON_DEGENERACY) + // rotation remains the same as the initial guess, but position is fixed by PoseTranslationPrior +#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION + EXPECT(assert_equal(Pose3(values.at(x3).rotation(), + Point3(0,0,1)), result.at(x3))); +#else + // if the check is disabled, no cheirality exception if thrown and the pose converges to the right rotation + // with modest accuracy since the configuration is essentially degenerate without the translation due to noise (noise_pose) + EXPECT(assert_equal(pose3, result.at(x3),1e-3)); +#endif +} + +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, Hessian ) { + + using namespace vanillaPose2; + + KeyVector views {x1, x2}; + + // Project three landmarks into 2 cameras + Point2 cam1_uv1 = cam1.project(landmark1); + Point2 cam2_uv1 = cam2.project(landmark1); + Point2Vector measurements_cam1; + measurements_cam1.push_back(cam1_uv1); + measurements_cam1.push_back(cam2_uv1); + + SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedK2)); + smartFactor1->add(measurements_cam1, views); + + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), + Point3(0.5, 0.1, 0.3)); + Values values; + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + + boost::shared_ptr factor = smartFactor1->linearize(values); + + // compute triangulation from linearization point + // compute reprojection errors (sum squared) + // compare with factor.info(): the bottom right element is the squared sum of the reprojection errors (normalized by the covariance) + // check that it is correctly scaled when using noiseProjection = [1/4 0; 0 1/4] +} + +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, HessianWithRotation ) { + // cout << " ************************ SmartProjectionPoseFactor: rotated Hessian **********************" << endl; + + using namespace vanillaPose; + + KeyVector views {x1, x2, x3}; + + Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; + + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + + SmartFactor::shared_ptr smartFactorInstance(new SmartFactor(model, sharedK)); + smartFactorInstance->add(measurements_cam1, views); + + Values values; + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + values.insert(x3, cam3.pose()); + + boost::shared_ptr factor = smartFactorInstance->linearize( + values); + + Pose3 poseDrift = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0)); + + Values rotValues; + rotValues.insert(x1, poseDrift.compose(level_pose)); + rotValues.insert(x2, poseDrift.compose(pose_right)); + rotValues.insert(x3, poseDrift.compose(pose_above)); + + boost::shared_ptr factorRot = smartFactorInstance->linearize( + rotValues); + + // Hessian is invariant to rotations in the nondegenerate case + EXPECT(assert_equal(factor->information(), factorRot->information(), 1e-7)); + + Pose3 poseDrift2 = Pose3(Rot3::Ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2), + Point3(10, -4, 5)); + + Values tranValues; + tranValues.insert(x1, poseDrift2.compose(level_pose)); + tranValues.insert(x2, poseDrift2.compose(pose_right)); + tranValues.insert(x3, poseDrift2.compose(pose_above)); + + boost::shared_ptr factorRotTran = + smartFactorInstance->linearize(tranValues); + + // Hessian is invariant to rotations and translations in the nondegenerate case + EXPECT(assert_equal(factor->information(), factorRotTran->information(), 1e-7)); +} + +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ) { + + using namespace vanillaPose2; + + KeyVector views {x1, x2, x3}; + + // All cameras have the same pose so will be degenerate ! + Camera cam2(level_pose, sharedK2); + Camera cam3(level_pose, sharedK2); + + Point2Vector measurements_cam1; + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + + SmartFactor::shared_ptr smartFactor(new SmartFactor(model, sharedK2)); + smartFactor->add(measurements_cam1, views); + + Values values; + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + values.insert(x3, cam3.pose()); + + boost::shared_ptr factor = smartFactor->linearize(values); + + Pose3 poseDrift = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0)); + + Values rotValues; + rotValues.insert(x1, poseDrift.compose(level_pose)); + rotValues.insert(x2, poseDrift.compose(level_pose)); + rotValues.insert(x3, poseDrift.compose(level_pose)); + + boost::shared_ptr factorRot = // + smartFactor->linearize(rotValues); + + // Hessian is invariant to rotations in the nondegenerate case + EXPECT(assert_equal(factor->information(), factorRot->information(), 1e-7)); + + Pose3 poseDrift2 = Pose3(Rot3::Ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2), + Point3(10, -4, 5)); + + Values tranValues; + tranValues.insert(x1, poseDrift2.compose(level_pose)); + tranValues.insert(x2, poseDrift2.compose(level_pose)); + tranValues.insert(x3, poseDrift2.compose(level_pose)); + + boost::shared_ptr factorRotTran = smartFactor->linearize( + tranValues); + + // Hessian is invariant to rotations and translations in the nondegenerate case + EXPECT(assert_equal(factor->information(), factorRotTran->information(), 1e-7)); +} + +/* ************************************************************************* */ +TEST( SmartProjectionPoseFactor, ConstructorWithCal3Bundler) { + using namespace bundlerPose; + SmartProjectionParams params; + params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); + SmartFactor factor(model, sharedBundlerK, params); + factor.add(measurement1, x1); +} + +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, Cal3Bundler ) { + + using namespace bundlerPose; + + // three landmarks ~5 meters in front of camera + Point3 landmark3(3, 0, 3.0); + + Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; + + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + + KeyVector views {x1, x2, x3}; + + SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedBundlerK)); + smartFactor1->add(measurements_cam1, views); + + SmartFactor::shared_ptr smartFactor2(new SmartFactor(model, sharedBundlerK)); + smartFactor2->add(measurements_cam2, views); + + SmartFactor::shared_ptr smartFactor3(new SmartFactor(model, sharedBundlerK)); + smartFactor3->add(measurements_cam3, views); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.addPrior(x1, cam1.pose(), noisePrior); + graph.addPrior(x2, cam2.pose(), noisePrior); + + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise + Values values; + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + // initialize third pose with some noise, we expect it to move back to original pose_above + values.insert(x3, pose_above * noise_pose); + EXPECT( + assert_equal( + Pose3( + Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, + -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), + Point3(0.1, -0.1, 1.9)), values.at(x3))); + + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + EXPECT(assert_equal(cam3.pose(), result.at(x3), 1e-6)); +} + +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) { + + using namespace bundlerPose; + + KeyVector views {x1, x2, x3}; + + // Two different cameras + Pose3 pose2 = level_pose + * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); + Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); + Camera cam2(pose2, sharedBundlerK); + Camera cam3(pose3, sharedBundlerK); + + // landmark3 at 3 meters now + Point3 landmark3(3, 0, 3.0); + + Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; + + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + + SmartProjectionParams params; + params.setRankTolerance(10); + params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); + + SmartFactor::shared_ptr smartFactor1( + new SmartFactor(model, sharedBundlerK, params)); + smartFactor1->add(measurements_cam1, views); + + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(model, sharedBundlerK, params)); + smartFactor2->add(measurements_cam2, views); + + SmartFactor::shared_ptr smartFactor3( + new SmartFactor(model, sharedBundlerK, params)); + smartFactor3->add(measurements_cam3, views); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, + 0.10); + Point3 positionPrior = Point3(0, 0, 1); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.addPrior(x1, cam1.pose(), noisePrior); + graph.emplace_shared >(x2, positionPrior, noisePriorTranslation); + graph.emplace_shared >(x3, positionPrior, noisePriorTranslation); + + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise + Values values; + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + // initialize third pose with some noise, we expect it to move back to original pose_above + values.insert(x3, pose3 * noise_pose); + EXPECT( + assert_equal( + Pose3( + Rot3(0.00563056869, -0.130848107, 0.991386438, -0.991390265, + -0.130426831, -0.0115837907, 0.130819108, -0.98278564, + -0.130455917), + Point3(0.0897734171, -0.110201006, 0.901022872)), + values.at(x3))); + + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + + EXPECT( + assert_equal( + Pose3( + Rot3(0.00563056869, -0.130848107, 0.991386438, -0.991390265, + -0.130426831, -0.0115837907, 0.130819108, -0.98278564, + -0.130455917), + Point3(0.0897734171, -0.110201006, 0.901022872)), + values.at(x3))); +} + +/* ************************************************************************* */ +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic"); +BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel"); +BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); + +TEST(SmartProjectionPoseFactor, serialize) { + using namespace vanillaPose; + using namespace gtsam::serializationTestHelpers; + SmartProjectionParams params; + params.setRankTolerance(rankTol); + SmartFactor factor(model, sharedK, params); + + EXPECT(equalsObj(factor)); + EXPECT(equalsXML(factor)); + EXPECT(equalsBinary(factor)); +} + +TEST(SmartProjectionPoseFactor, serialize2) { + using namespace vanillaPose; + using namespace gtsam::serializationTestHelpers; + SmartProjectionParams params; + params.setRankTolerance(rankTol); + Pose3 bts; + SmartFactor factor(model, sharedK, bts, params); + + // insert some measurments + KeyVector key_view; + Point2Vector meas_view; + key_view.push_back(Symbol('x', 1)); + meas_view.push_back(Point2(10, 10)); + factor.add(meas_view, key_view); + + EXPECT(equalsObj(factor)); + EXPECT(equalsXML(factor)); + EXPECT(equalsBinary(factor)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + From 00387b32cdad2bb9007e372a832a7435af13999d Mon Sep 17 00:00:00 2001 From: lcarlone Date: Mon, 19 Jul 2021 15:30:53 -0400 Subject: [PATCH 070/248] setting up .h and tests - compiles and tests pass. --- ...martProjectionPoseFactorRollingShutter.cpp | 187 +++---- .../SmartProjectionPoseFactorRollingShutter.h | 474 ++++++++-------- ...martProjectionPoseFactorRollingShutter.cpp | 529 +----------------- 3 files changed, 356 insertions(+), 834 deletions(-) diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.cpp index 7ab28e614..4e1cbdd46 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.cpp @@ -10,116 +10,95 @@ * -------------------------------------------------------------------------- */ /** - * @file SmartProjectionProjectionPoseFactorRollingShutter.h - * @brief Smart projection factor on poses and extrinsic calibration + * @file SmartProjectionPoseFactorRollingShutter.h + * @brief Smart projection factor on poses modeling rolling shutter effect * @author Luca Carlone - * @author Frank Dellaert */ -#include +//#include namespace gtsam { -SmartStereoProjectionFactorPP::SmartStereoProjectionFactorPP( - const SharedNoiseModel& sharedNoiseModel, - const SmartStereoProjectionParams& params) - : Base(sharedNoiseModel, params) {} // note: no extrinsic specified! -void SmartStereoProjectionFactorPP::add( - const StereoPoint2& measured, - const Key& w_P_body_key, const Key& body_P_cam_key, - const boost::shared_ptr& K) { - // we index by cameras.. - Base::add(measured, w_P_body_key); - // but we also store the extrinsic calibration keys in the same order - world_P_body_keys_.push_back(w_P_body_key); - body_P_cam_keys_.push_back(body_P_cam_key); - - // pose keys are assumed to be unique (1 observation per time stamp), but calibration can be shared - if(std::find(keys_.begin(), keys_.end(), body_P_cam_key) == keys_.end()) - keys_.push_back(body_P_cam_key); // add only unique keys - - K_all_.push_back(K); -} - -void SmartStereoProjectionFactorPP::add( - const std::vector& measurements, - const KeyVector& world_P_body_keys, const KeyVector& body_P_cam_keys, - const std::vector>& Ks) { - assert(world_P_body_keys.size() == measurements.size()); - assert(world_P_body_keys.size() == body_P_cam_keys.size()); - assert(world_P_body_keys.size() == Ks.size()); - for (size_t i = 0; i < measurements.size(); i++) { - Base::add(measurements[i], world_P_body_keys[i]); - // pose keys are assumed to be unique (1 observation per time stamp), but calibration can be shared - if(std::find(keys_.begin(), keys_.end(), body_P_cam_keys[i]) == keys_.end()) - keys_.push_back(body_P_cam_keys[i]); // add only unique keys - - world_P_body_keys_.push_back(world_P_body_keys[i]); - body_P_cam_keys_.push_back(body_P_cam_keys[i]); - - K_all_.push_back(Ks[i]); - } -} - -void SmartStereoProjectionFactorPP::add( - const std::vector& measurements, - const KeyVector& world_P_body_keys, const KeyVector& body_P_cam_keys, - const boost::shared_ptr& K) { - assert(world_P_body_keys.size() == measurements.size()); - assert(world_P_body_keys.size() == body_P_cam_keys.size()); - for (size_t i = 0; i < measurements.size(); i++) { - Base::add(measurements[i], world_P_body_keys[i]); - // pose keys are assumed to be unique (1 observation per time stamp), but calibration can be shared - if(std::find(keys_.begin(), keys_.end(), body_P_cam_keys[i]) == keys_.end()) - keys_.push_back(body_P_cam_keys[i]); // add only unique keys - - world_P_body_keys_.push_back(world_P_body_keys[i]); - body_P_cam_keys_.push_back(body_P_cam_keys[i]); - - K_all_.push_back(K); - } -} - -void SmartStereoProjectionFactorPP::print( - const std::string& s, const KeyFormatter& keyFormatter) const { - std::cout << s << "SmartStereoProjectionFactorPP: \n "; - for (size_t i = 0; i < K_all_.size(); i++) { - K_all_[i]->print("calibration = "); - std::cout << " extrinsic pose key: " << keyFormatter(body_P_cam_keys_[i]) << std::endl; - } - Base::print("", keyFormatter); -} - -bool SmartStereoProjectionFactorPP::equals(const NonlinearFactor& p, - double tol) const { - const SmartStereoProjectionFactorPP* e = - dynamic_cast(&p); - - return e && Base::equals(p, tol) && - body_P_cam_keys_ == e->getExtrinsicPoseKeys(); -} - -double SmartStereoProjectionFactorPP::error(const Values& values) const { - if (this->active(values)) { - return this->totalReprojectionError(cameras(values)); - } else { // else of active flag - return 0.0; - } -} - -SmartStereoProjectionFactorPP::Base::Cameras -SmartStereoProjectionFactorPP::cameras(const Values& values) const { - assert(world_P_body_keys_.size() == K_all_.size()); - assert(world_P_body_keys_.size() == body_P_cam_keys_.size()); - Base::Cameras cameras; - for (size_t i = 0; i < world_P_body_keys_.size(); i++) { - Pose3 w_P_body = values.at(world_P_body_keys_[i]); - Pose3 body_P_cam = values.at(body_P_cam_keys_[i]); - Pose3 w_P_cam = w_P_body.compose(body_P_cam); - cameras.push_back(StereoCamera(w_P_cam, K_all_[i])); - } - return cameras; -} +// +//void SmartProjectionPoseFactorRollingShutter::add( +// const std::vector& measurements, +// const KeyVector& world_P_body_keys, const KeyVector& body_P_cam_keys, +// const std::vector>& Ks) { +// assert(world_P_body_keys.size() == measurements.size()); +// assert(world_P_body_keys.size() == body_P_cam_keys.size()); +// assert(world_P_body_keys.size() == Ks.size()); +// for (size_t i = 0; i < measurements.size(); i++) { +// Base::add(measurements[i], world_P_body_keys[i]); +// // pose keys are assumed to be unique (1 observation per time stamp), but calibration can be shared +// if(std::find(keys_.begin(), keys_.end(), body_P_cam_keys[i]) == keys_.end()) +// keys_.push_back(body_P_cam_keys[i]); // add only unique keys +// +// world_P_body_keys_.push_back(world_P_body_keys[i]); +// body_P_cam_keys_.push_back(body_P_cam_keys[i]); +// +// K_all_.push_back(Ks[i]); +// } +//} +// +//void SmartProjectionPoseFactorRollingShutter::add( +// const std::vector& measurements, +// const KeyVector& world_P_body_keys, const KeyVector& body_P_cam_keys, +// const boost::shared_ptr& K) { +// assert(world_P_body_keys.size() == measurements.size()); +// assert(world_P_body_keys.size() == body_P_cam_keys.size()); +// for (size_t i = 0; i < measurements.size(); i++) { +// Base::add(measurements[i], world_P_body_keys[i]); +// // pose keys are assumed to be unique (1 observation per time stamp), but calibration can be shared +// if(std::find(keys_.begin(), keys_.end(), body_P_cam_keys[i]) == keys_.end()) +// keys_.push_back(body_P_cam_keys[i]); // add only unique keys +// +// world_P_body_keys_.push_back(world_P_body_keys[i]); +// body_P_cam_keys_.push_back(body_P_cam_keys[i]); +// +// K_all_.push_back(K); +// } +//} +// +//void SmartProjectionPoseFactorRollingShutter::print( +// const std::string& s, const KeyFormatter& keyFormatter) const { +// std::cout << s << "SmartProjectionPoseFactorRollingShutter: \n "; +// for (size_t i = 0; i < K_all_.size(); i++) { +// K_all_[i]->print("calibration = "); +// std::cout << " extrinsic pose key: " << keyFormatter(body_P_cam_keys_[i]) << std::endl; +// } +// Base::print("", keyFormatter); +//} +// +//bool SmartProjectionPoseFactorRollingShutter::equals(const NonlinearFactor& p, +// double tol) const { +// const SmartProjectionPoseFactorRollingShutter* e = +// dynamic_cast(&p); +// +// return e && Base::equals(p, tol) && +// body_P_cam_keys_ == e->getExtrinsicPoseKeys(); +//} +// +//double SmartProjectionPoseFactorRollingShutter::error(const Values& values) const { +// if (this->active(values)) { +// return this->totalReprojectionError(cameras(values)); +// } else { // else of active flag +// return 0.0; +// } +//} +// +//SmartProjectionPoseFactorRollingShutter::Base::Cameras +//SmartProjectionPoseFactorRollingShutter::cameras(const Values& values) const { +// assert(world_P_body_keys_.size() == K_all_.size()); +// assert(world_P_body_keys_.size() == body_P_cam_keys_.size()); +// Base::Cameras cameras; +// for (size_t i = 0; i < world_P_body_keys_.size(); i++) { +// Pose3 w_P_body = values.at(world_P_body_keys_[i]); +// Pose3 body_P_cam = values.at(body_P_cam_keys_[i]); +// Pose3 w_P_cam = w_P_body.compose(body_P_cam); +// cameras.push_back(StereoCamera(w_P_cam, K_all_[i])); +// } +// return cameras; +//} } // \ namespace gtsam diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index 40d90d614..fe5ccdf7f 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -10,15 +10,14 @@ * -------------------------------------------------------------------------- */ /** - * @file SmartStereoProjectionFactorPP.h - * @brief Smart stereo factor on poses (P) and camera extrinsic pose (P) calibrations + * @file SmartProjectionPoseFactorRollingShutter.h + * @brief Smart projection factor on poses modeling rolling shutter effect with given readout time * @author Luca Carlone - * @author Frank Dellaert */ #pragma once -#include +#include namespace gtsam { /** @@ -33,37 +32,40 @@ namespace gtsam { */ /** - * This factor optimizes the pose of the body as well as the extrinsic camera calibration (pose of camera wrt body). - * Each camera may have its own extrinsic calibration or the same calibration can be shared by multiple cameras. - * This factor requires that values contain the involved poses and extrinsics (both are Pose3 variables). + * This factor optimizes the pose of the body assuming a rolling shutter model of the camera with given readout time. + * This factor requires that values contain (for each pixel observation) consecutive camera poses + * from which the pixel observation pose can be interpolated. * @addtogroup SLAM */ -class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor { +template +class SmartProjectionPoseFactorRollingShutter: public SmartProjectionFactor< + PinholePose > { + protected: - /// shared pointer to calibration object (one for each camera) - std::vector> K_all_; + /// shared pointer to calibration object (one for each observation) + std::vector > K_all_; - /// The keys corresponding to the pose of the body (with respect to an external world frame) for each view - KeyVector world_P_body_keys_; + // The keys of the pose of the body (with respect to an external world frame): two consecutive poses for each observation + std::vector> world_P_body_key_pairs_; - /// The keys corresponding to the extrinsic pose calibration for each view (pose that transform from camera to body) - KeyVector body_P_cam_keys_; + // interpolation factor (one for each observation) to interpolate between pair of consecutive poses + std::vector gammas_; public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW + typedef PinholePose Camera; /// shorthand for base class type - typedef SmartStereoProjectionFactor Base; + typedef SmartProjectionFactor Base; /// shorthand for this class - typedef SmartStereoProjectionFactorPP This; + typedef SmartProjectionPoseFactorRollingShutter This; /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; - static const int Dim = 12; ///< Camera dimension: 6 for body pose, 6 for extrinsic pose - static const int DimPose = 6; ///< Pose3 dimension - static const int ZDim = 3; ///< Measurement dimension (for a StereoPoint2 measurement) + static const int Dim = 6; ///< Pose3 dimension + static const int ZDim = 2; ///< Measurement dimension (Point2) typedef Eigen::Matrix MatrixZD; // F blocks (derivatives wrt camera) typedef std::vector > FBlocks; // vector of F blocks @@ -72,51 +74,67 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor { * @param Isotropic measurement noise * @param params internal parameters of the smart factors */ - SmartStereoProjectionFactorPP(const SharedNoiseModel& sharedNoiseModel, - const SmartStereoProjectionParams& params = - SmartStereoProjectionParams()); + SmartProjectionPoseFactorRollingShutter( + const SharedNoiseModel& sharedNoiseModel, + const SmartProjectionParams& params = SmartProjectionParams()) + : Base(sharedNoiseModel, params) {} /** Virtual destructor */ - ~SmartStereoProjectionFactorPP() override = default; + ~SmartProjectionPoseFactorRollingShutter() override = default; /** - * add a new measurement, with a pose key, and an extrinsic pose key - * @param measured is the 3-dimensional location of the projection of a - * single landmark in the a single (stereo) view (the measurement) - * @param world_P_body_key is the key corresponding to the body poses observing the same landmark - * @param body_P_cam_key is the key corresponding to the extrinsic camera-to-body pose calibration + * add a new measurement, with 2 pose keys, camera calibration, and observed pixel. + * @param measured is the 2-dimensional location of the projection of a + * single landmark in the a single view (the measurement), interpolated from the 2 poses + * @param world_P_body_key1 is the key corresponding to the first body poses (time <= time pixel is acquired) + * @param world_P_body_key2 is the key corresponding to the second body poses (time >= time pixel is acquired) + * @param gamma in [0,1] is the interpolation factor, such that if gamma = 0 the interpolated pose is the same as world_P_body_key * @param K is the (fixed) camera intrinsic calibration */ - void add(const StereoPoint2& measured, const Key& world_P_body_key, - const Key& body_P_cam_key, - const boost::shared_ptr& K); + void add(const Point2& measured, + const Key& world_P_body_key1, + const Key& world_P_body_key2, + const double& gamma, + const boost::shared_ptr& K){ + // store measurements in base class (note: we only store the first key there) + Base::add(measured, world_P_body_key1); + // but we also store the extrinsic calibration keys in the same order + world_P_body_key_pairs_.push_back(std::make_pair(world_P_body_key1,world_P_body_key2)); + + // pose keys are assumed to be unique, so we avoid duplicates here + if(std::find( this->keys_.begin(), this->keys_.end(), world_P_body_key1) == this->keys_.end()) + this->keys_.push_back(world_P_body_key1); // add only unique keys + if(std::find( this->keys_.begin(), this->keys_.end(), world_P_body_key2) == this->keys_.end()) + this->keys_.push_back(world_P_body_key2); // add only unique keys + + // store fixed calibration + K_all_.push_back(K); + } /** - * Variant of the previous one in which we include a set of measurements - * @param measurements vector of the 3m dimensional location of the projection - * of a single landmark in the m (stereo) view (the measurements) - * @param w_P_body_keys are the ordered keys corresponding to the body poses observing the same landmark - * @param body_P_cam_keys are the ordered keys corresponding to the extrinsic camera-to-body poses calibration - * (note: elements of this vector do not need to be unique: 2 camera views can share the same calibration) + * Variant of the previous one in which we include a set of measurements + * @param measurements vector of the 2m dimensional location of the projection + * of a single landmark in the m views (the measurements) + * @param world_P_body_key_pairs vector of (1 for each view) containing the pair of poses from which each view can be interpolated * @param Ks vector of intrinsic calibration objects */ - void add(const std::vector& measurements, - const KeyVector& w_P_body_keys, const KeyVector& body_P_cam_keys, - const std::vector>& Ks); +// void add(const std::vector& measurements, +// const std::vector>& world_P_body_key_pairs, +// const std::vector& gammas, +// const std::vector>& Ks); /** * Variant of the previous one in which we include a set of measurements with - * the same noise and calibration - * @param measurements vector of the 3m dimensional location of the projection - * of a single landmark in the m (stereo) view (the measurements) - * @param w_P_body_keys are the ordered keys corresponding to the body poses observing the same landmark - * @param body_P_cam_keys are the ordered keys corresponding to the extrinsic camera-to-body poses calibration - * (note: elements of this vector do not need to be unique: 2 camera views can share the same calibration) + * the same calibration + * @param measurements vector of the 2m dimensional location of the projection + * of a single landmark in the m views (the measurements) + * @param world_P_body_key_pairs vector of (1 for each view) containing the pair of poses from which each view can be interpolated * @param K the (known) camera calibration (same for all measurements) */ - void add(const std::vector& measurements, - const KeyVector& w_P_body_keys, const KeyVector& body_P_cam_keys, - const boost::shared_ptr& K); +// void add(const std::vector& measurements, +// const std::vector>& world_P_body_key_pairs, +// const std::vector& gammas, +// const boost::shared_ptr& K); /** * print @@ -130,8 +148,8 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor { bool equals(const NonlinearFactor& p, double tol = 1e-9) const override; /// equals - const KeyVector& getExtrinsicPoseKeys() const { - return body_P_cam_keys_; + const std::vector getGammas() const { + return gammas_; } /** @@ -140,18 +158,26 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor { double error(const Values& values) const override; /** return the calibration object */ - inline std::vector> calibration() const { + inline std::vector> calibration() const { return K_all_; } /** * Collect all cameras involved in this factor * @param values Values structure which must contain camera poses - * corresponding - * to keys involved in this factor - * @return vector of Values + * corresponding to keys involved in this factor + * @return Cameras */ - Base::Cameras cameras(const Values& values) const override; + typename Base::Cameras cameras(const Values& values) const override { + typename Base::Cameras cameras; + for (const Key& k : this->keys_) { +// const Pose3 world_P_sensor_k = +// Base::body_P_sensor_ ? values.at(k) * *Base::body_P_sensor_ +// : values.at(k); +// cameras.emplace_back(world_P_sensor_k, K_); + } + return cameras; + } /** * Compute jacobian F, E and error vector at a given linearization point @@ -161,169 +187,169 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor { * respect to both the body pose and extrinsic pose), the point Jacobian E, * and the error vector b. Note that the jacobians are computed for a given point. */ - void computeJacobiansAndCorrectForMissingMeasurements( - FBlocks& Fs, Matrix& E, Vector& b, const Values& values) const { - if (!result_) { - throw("computeJacobiansWithTriangulatedPoint"); - } else { // valid result: compute jacobians - size_t numViews = measured_.size(); - E = Matrix::Zero(3 * numViews, 3); // a StereoPoint2 for each view (point jacobian) - b = Vector::Zero(3 * numViews); // a StereoPoint2 for each view - Matrix dPoseCam_dPoseBody_i, dPoseCam_dPoseExt_i, dProject_dPoseCam_i, Ei; - - for (size_t i = 0; i < numViews; i++) { // for each camera/measurement - Pose3 w_P_body = values.at(world_P_body_keys_.at(i)); - Pose3 body_P_cam = values.at(body_P_cam_keys_.at(i)); - StereoCamera camera( - w_P_body.compose(body_P_cam, dPoseCam_dPoseBody_i, dPoseCam_dPoseExt_i), - K_all_[i]); - // get jacobians and error vector for current measurement - StereoPoint2 reprojectionError_i = StereoPoint2( - camera.project(*result_, dProject_dPoseCam_i, Ei) - measured_.at(i)); - Eigen::Matrix J; // 3 x 12 - J.block(0, 0) = dProject_dPoseCam_i * dPoseCam_dPoseBody_i; // (3x6) * (6x6) - J.block(0, 6) = dProject_dPoseCam_i * dPoseCam_dPoseExt_i; // (3x6) * (6x6) - // if the right pixel is invalid, fix jacobians - if (std::isnan(measured_.at(i).uR())) - { - J.block<1, 12>(1, 0) = Matrix::Zero(1, 12); - Ei.block<1, 3>(1, 0) = Matrix::Zero(1, 3); - reprojectionError_i = StereoPoint2(reprojectionError_i.uL(), 0.0, - reprojectionError_i.v()); - } - // fit into the output structures - Fs.push_back(J); - size_t row = 3 * i; - b.segment(row) = -reprojectionError_i.vector(); - E.block<3, 3>(row, 0) = Ei; - } - } - } +// void computeJacobiansAndCorrectForMissingMeasurements( +// FBlocks& Fs, Matrix& E, Vector& b, const Values& values) const { +// if (!result_) { +// throw("computeJacobiansWithTriangulatedPoint"); +// } else { // valid result: compute jacobians +// size_t numViews = measured_.size(); +// E = Matrix::Zero(3 * numViews, 3); // a StereoPoint2 for each view (point jacobian) +// b = Vector::Zero(3 * numViews); // a StereoPoint2 for each view +// Matrix dPoseCam_dPoseBody_i, dPoseCam_dPoseExt_i, dProject_dPoseCam_i, Ei; +// +// for (size_t i = 0; i < numViews; i++) { // for each camera/measurement +// Pose3 w_P_body = values.at(world_P_body_key_pairs_.at(i)); +// Pose3 body_P_cam = values.at(body_P_cam_ this->keys_.at(i)); +// StereoCamera camera( +// w_P_body.compose(body_P_cam, dPoseCam_dPoseBody_i, dPoseCam_dPoseExt_i), +// K_all_[i]); +// // get jacobians and error vector for current measurement +// StereoPoint2 reprojectionError_i = StereoPoint2( +// camera.project(*result_, dProject_dPoseCam_i, Ei) - measured_.at(i)); +// Eigen::Matrix J; // 3 x 12 +// J.block(0, 0) = dProject_dPoseCam_i * dPoseCam_dPoseBody_i; // (3x6) * (6x6) +// J.block(0, 6) = dProject_dPoseCam_i * dPoseCam_dPoseExt_i; // (3x6) * (6x6) +// // if the right pixel is invalid, fix jacobians +// if (std::isnan(measured_.at(i).uR())) +// { +// J.block<1, 12>(1, 0) = Matrix::Zero(1, 12); +// Ei.block<1, 3>(1, 0) = Matrix::Zero(1, 3); +// reprojectionError_i = StereoPoint2(reprojectionError_i.uL(), 0.0, +// reprojectionError_i.v()); +// } +// // fit into the output structures +// Fs.push_back(J); +// size_t row = 3 * i; +// b.segment(row) = -reprojectionError_i.vector(); +// E.block<3, 3>(row, 0) = Ei; +// } +// } +// } /// linearize and return a Hessianfactor that is an approximation of error(p) - boost::shared_ptr > createHessianFactor( - const Values& values, const double lambda = 0.0, bool diagonalDamping = - false) const { - - // we may have multiple cameras sharing the same extrinsic cals, hence the number - // of keys may be smaller than 2 * nrMeasurements (which is the upper bound where we - // have a body key and an extrinsic calibration key for each measurement) - size_t nrUniqueKeys = keys_.size(); - size_t nrNonuniqueKeys = world_P_body_keys_.size() - + body_P_cam_keys_.size(); - - // Create structures for Hessian Factors - KeyVector js; - std::vector < Matrix > Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2); - std::vector gs(nrUniqueKeys); - - if (this->measured_.size() != cameras(values).size()) - throw std::runtime_error("SmartStereoProjectionHessianFactor: this->" - "measured_.size() inconsistent with input"); - - // triangulate 3D point at given linearization point - triangulateSafe(cameras(values)); - - if (!result_) { // failed: return "empty/zero" Hessian - for (Matrix& m : Gs) - m = Matrix::Zero(DimPose, DimPose); - for (Vector& v : gs) - v = Vector::Zero(DimPose); - return boost::make_shared < RegularHessianFactor - > (keys_, Gs, gs, 0.0); - } - - // compute Jacobian given triangulated 3D Point - FBlocks Fs; - Matrix F, E; - Vector b; - computeJacobiansAndCorrectForMissingMeasurements(Fs, E, b, values); - - // Whiten using noise model - noiseModel_->WhitenSystem(E, b); - for (size_t i = 0; i < Fs.size(); i++) - Fs[i] = noiseModel_->Whiten(Fs[i]); - - // build augmented Hessian (with last row/column being the information vector) - Matrix3 P; - Cameras::ComputePointCovariance<3>(P, E, lambda, diagonalDamping); - - // marginalize point: note - we reuse the standard SchurComplement function - SymmetricBlockMatrix augmentedHessian = - Cameras::SchurComplement<3, Dim>(Fs, E, P, b); - - // now pack into an Hessian factor - std::vector dims(nrUniqueKeys + 1); // this also includes the b term - std::fill(dims.begin(), dims.end() - 1, 6); - dims.back() = 1; - SymmetricBlockMatrix augmentedHessianUniqueKeys; - - // here we have to deal with the fact that some cameras may share the same extrinsic key - if (nrUniqueKeys == nrNonuniqueKeys) { // if there is 1 calibration key per camera - augmentedHessianUniqueKeys = SymmetricBlockMatrix( - dims, Matrix(augmentedHessian.selfadjointView())); - } else { // if multiple cameras share a calibration we have to rearrange - // the results of the Schur complement matrix - std::vector nonuniqueDims(nrNonuniqueKeys + 1); // this also includes the b term - std::fill(nonuniqueDims.begin(), nonuniqueDims.end() - 1, 6); - nonuniqueDims.back() = 1; - augmentedHessian = SymmetricBlockMatrix( - nonuniqueDims, Matrix(augmentedHessian.selfadjointView())); - - // these are the keys that correspond to the blocks in augmentedHessian (output of SchurComplement) - KeyVector nonuniqueKeys; - for (size_t i = 0; i < world_P_body_keys_.size(); i++) { - nonuniqueKeys.push_back(world_P_body_keys_.at(i)); - nonuniqueKeys.push_back(body_P_cam_keys_.at(i)); - } - - // get map from key to location in the new augmented Hessian matrix (the one including only unique keys) - std::map keyToSlotMap; - for (size_t k = 0; k < nrUniqueKeys; k++) { - keyToSlotMap[keys_[k]] = k; - } - - // initialize matrix to zero - augmentedHessianUniqueKeys = SymmetricBlockMatrix( - dims, Matrix::Zero(6 * nrUniqueKeys + 1, 6 * nrUniqueKeys + 1)); - - // add contributions for each key: note this loops over the hessian with nonUnique keys (augmentedHessian) - // and populates an Hessian that only includes the unique keys (that is what we want to return) - for (size_t i = 0; i < nrNonuniqueKeys; i++) { // rows - Key key_i = nonuniqueKeys.at(i); - - // update information vector - augmentedHessianUniqueKeys.updateOffDiagonalBlock( - keyToSlotMap[key_i], nrUniqueKeys, - augmentedHessian.aboveDiagonalBlock(i, nrNonuniqueKeys)); - - // update blocks - for (size_t j = i; j < nrNonuniqueKeys; j++) { // cols - Key key_j = nonuniqueKeys.at(j); - if (i == j) { - augmentedHessianUniqueKeys.updateDiagonalBlock( - keyToSlotMap[key_i], augmentedHessian.diagonalBlock(i)); - } else { // (i < j) - if (keyToSlotMap[key_i] != keyToSlotMap[key_j]) { - augmentedHessianUniqueKeys.updateOffDiagonalBlock( - keyToSlotMap[key_i], keyToSlotMap[key_j], - augmentedHessian.aboveDiagonalBlock(i, j)); - } else { - augmentedHessianUniqueKeys.updateDiagonalBlock( - keyToSlotMap[key_i], - augmentedHessian.aboveDiagonalBlock(i, j) - + augmentedHessian.aboveDiagonalBlock(i, j).transpose()); - } - } - } - } - // update bottom right element of the matrix - augmentedHessianUniqueKeys.updateDiagonalBlock( - nrUniqueKeys, augmentedHessian.diagonalBlock(nrNonuniqueKeys)); - } - return boost::make_shared < RegularHessianFactor - > (keys_, augmentedHessianUniqueKeys); - } +// boost::shared_ptr > createHessianFactor( +// const Values& values, const double lambda = 0.0, bool diagonalDamping = +// false) const { +// +// // we may have multiple cameras sharing the same extrinsic cals, hence the number +// // of keys may be smaller than 2 * nrMeasurements (which is the upper bound where we +// // have a body key and an extrinsic calibration key for each measurement) +// size_t nrUniqueKeys = this->keys_.size(); +// size_t nrNonuniqueKeys = world_P_body_key_pairs_.size() +// + body_P_cam_ this->keys_.size(); +// +// // Create structures for Hessian Factors +// KeyVector js; +// std::vector < Matrix > Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2); +// std::vector gs(nrUniqueKeys); +// +// if (this->measured_.size() != cameras(values).size()) +// throw std::runtime_error("SmartStereoProjectionHessianFactor: this->" +// "measured_.size() inconsistent with input"); +// +// // triangulate 3D point at given linearization point +// triangulateSafe(cameras(values)); +// +// if (!result_) { // failed: return "empty/zero" Hessian +// for (Matrix& m : Gs) +// m = Matrix::Zero(DimPose, DimPose); +// for (Vector& v : gs) +// v = Vector::Zero(DimPose); +// return boost::make_shared < RegularHessianFactor +// > ( this->keys_, Gs, gs, 0.0); +// } +// +// // compute Jacobian given triangulated 3D Point +// FBlocks Fs; +// Matrix F, E; +// Vector b; +// computeJacobiansAndCorrectForMissingMeasurements(Fs, E, b, values); +// +// // Whiten using noise model +// noiseModel_->WhitenSystem(E, b); +// for (size_t i = 0; i < Fs.size(); i++) +// Fs[i] = noiseModel_->Whiten(Fs[i]); +// +// // build augmented Hessian (with last row/column being the information vector) +// Matrix3 P; +// Cameras::ComputePointCovariance<3>(P, E, lambda, diagonalDamping); +// +// // marginalize point: note - we reuse the standard SchurComplement function +// SymmetricBlockMatrix augmentedHessian = +// Cameras::SchurComplement<3, Dim>(Fs, E, P, b); +// +// // now pack into an Hessian factor +// std::vector dims(nrUniqueKeys + 1); // this also includes the b term +// std::fill(dims.begin(), dims.end() - 1, 6); +// dims.back() = 1; +// SymmetricBlockMatrix augmentedHessianUniqueKeys; +// +// // here we have to deal with the fact that some cameras may share the same extrinsic key +// if (nrUniqueKeys == nrNonuniqueKeys) { // if there is 1 calibration key per camera +// augmentedHessianUniqueKeys = SymmetricBlockMatrix( +// dims, Matrix(augmentedHessian.selfadjointView())); +// } else { // if multiple cameras share a calibration we have to rearrange +// // the results of the Schur complement matrix +// std::vector nonuniqueDims(nrNonuniqueKeys + 1); // this also includes the b term +// std::fill(nonuniqueDims.begin(), nonuniqueDims.end() - 1, 6); +// nonuniqueDims.back() = 1; +// augmentedHessian = SymmetricBlockMatrix( +// nonuniqueDims, Matrix(augmentedHessian.selfadjointView())); +// +// // these are the keys that correspond to the blocks in augmentedHessian (output of SchurComplement) +// KeyVector nonuniqueKeys; +// for (size_t i = 0; i < world_P_body_key_pairs_.size(); i++) { +// nonuniqueKeys.push_back(world_P_body_key_pairs_.at(i)); +// nonuniqueKeys.push_back(body_P_cam_ this->keys_.at(i)); +// } +// +// // get map from key to location in the new augmented Hessian matrix (the one including only unique keys) +// std::map keyToSlotMap; +// for (size_t k = 0; k < nrUniqueKeys; k++) { +// keyToSlotMap[ this->keys_[k]] = k; +// } +// +// // initialize matrix to zero +// augmentedHessianUniqueKeys = SymmetricBlockMatrix( +// dims, Matrix::Zero(6 * nrUniqueKeys + 1, 6 * nrUniqueKeys + 1)); +// +// // add contributions for each key: note this loops over the hessian with nonUnique keys (augmentedHessian) +// // and populates an Hessian that only includes the unique keys (that is what we want to return) +// for (size_t i = 0; i < nrNonuniqueKeys; i++) { // rows +// Key key_i = nonuniqueKeys.at(i); +// +// // update information vector +// augmentedHessianUniqueKeys.updateOffDiagonalBlock( +// keyToSlotMap[key_i], nrUniqueKeys, +// augmentedHessian.aboveDiagonalBlock(i, nrNonuniqueKeys)); +// +// // update blocks +// for (size_t j = i; j < nrNonuniqueKeys; j++) { // cols +// Key key_j = nonuniqueKeys.at(j); +// if (i == j) { +// augmentedHessianUniqueKeys.updateDiagonalBlock( +// keyToSlotMap[key_i], augmentedHessian.diagonalBlock(i)); +// } else { // (i < j) +// if (keyToSlotMap[key_i] != keyToSlotMap[key_j]) { +// augmentedHessianUniqueKeys.updateOffDiagonalBlock( +// keyToSlotMap[key_i], keyToSlotMap[key_j], +// augmentedHessian.aboveDiagonalBlock(i, j)); +// } else { +// augmentedHessianUniqueKeys.updateDiagonalBlock( +// keyToSlotMap[key_i], +// augmentedHessian.aboveDiagonalBlock(i, j) +// + augmentedHessian.aboveDiagonalBlock(i, j).transpose()); +// } +// } +// } +// } +// // update bottom right element of the matrix +// augmentedHessianUniqueKeys.updateDiagonalBlock( +// nrUniqueKeys, augmentedHessian.diagonalBlock(nrNonuniqueKeys)); +// } +// return boost::make_shared < RegularHessianFactor +// > ( this->keys_, augmentedHessianUniqueKeys); +// } /** * Linearize to Gaussian Factor (possibly adding a damping factor Lambda for LM) @@ -333,12 +359,12 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor { boost::shared_ptr linearizeDamped( const Values& values, const double lambda = 0.0) const { // depending on flag set on construction we may linearize to different linear factors - switch (params_.linearizationMode) { - case HESSIAN: - return createHessianFactor(values, lambda); + switch (this->params_.linearizationMode) { +// case HESSIAN: +// return createHessianFactor(values, lambda); default: throw std::runtime_error( - "SmartStereoProjectionFactorPP: unknown linearization mode"); + "SmartProjectionPoseFactorRollingShutter: unknown linearization mode"); } } @@ -361,9 +387,9 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor { // end of class declaration /// traits -template<> -struct traits : public Testable< - SmartStereoProjectionFactorPP> { +template +struct traits > : public Testable< + SmartProjectionPoseFactorRollingShutter > { }; } // namespace gtsam diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index c7f220c10..d4c268b3c 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -10,19 +10,17 @@ * -------------------------------------------------------------------------- */ /** - * @file testSmartProjectionPoseFactor.cpp - * @brief Unit tests for ProjectionFactor Class - * @author Chris Beall + * @file testSmartProjectionPoseFactorRollingShutter.cpp + * @brief Unit tests for SmartProjectionPoseFactorRollingShutter Class * @author Luca Carlone - * @author Zsolt Kira - * @author Frank Dellaert - * @date Sept 2013 + * @date July 2021 */ -#include "smartFactorScenarios.h" +#include "gtsam/slam/tests/smartFactorScenarios.h" #include #include #include +#include #include #include #include @@ -52,13 +50,13 @@ LevenbergMarquardtParams lmParams; // Make more verbose like so (in tests): // params.verbosityLM = LevenbergMarquardtParams::SUMMARY; -/* ************************************************************************* */ +/* ************************************************************************* * TEST( SmartProjectionPoseFactor, Constructor) { using namespace vanillaPose; SmartFactor::shared_ptr factor1(new SmartFactor(model, sharedK)); } -/* ************************************************************************* */ +/* ************************************************************************* * TEST( SmartProjectionPoseFactor, Constructor2) { using namespace vanillaPose; SmartProjectionParams params; @@ -66,14 +64,14 @@ TEST( SmartProjectionPoseFactor, Constructor2) { SmartFactor factor1(model, sharedK, params); } -/* ************************************************************************* */ +/* ************************************************************************* * TEST( SmartProjectionPoseFactor, Constructor3) { using namespace vanillaPose; SmartFactor::shared_ptr factor1(new SmartFactor(model, sharedK)); factor1->add(measurement1, x1); } -/* ************************************************************************* */ +/* ************************************************************************* * TEST( SmartProjectionPoseFactor, Constructor4) { using namespace vanillaPose; SmartProjectionParams params; @@ -82,7 +80,7 @@ TEST( SmartProjectionPoseFactor, Constructor4) { factor1.add(measurement1, x1); } -/* ************************************************************************* */ +/* ************************************************************************* * TEST( SmartProjectionPoseFactor, params) { using namespace vanillaPose; SmartProjectionParams params; @@ -93,7 +91,7 @@ TEST( SmartProjectionPoseFactor, params) { EXPECT_DOUBLES_EQUAL(1e-3, rt, 1e-7); } -/* ************************************************************************* */ +/* ************************************************************************* * TEST( SmartProjectionPoseFactor, Equals ) { using namespace vanillaPose; SmartFactor::shared_ptr factor1(new SmartFactor(model, sharedK)); @@ -105,7 +103,7 @@ TEST( SmartProjectionPoseFactor, Equals ) { CHECK(assert_equal(*factor1, *factor2)); } -/* *************************************************************************/ +/* ************************************************************************* TEST( SmartProjectionPoseFactor, noiseless ) { using namespace vanillaPose; @@ -163,7 +161,7 @@ TEST( SmartProjectionPoseFactor, noiseless ) { EXPECT_DOUBLES_EQUAL(expectedError, actualError3, 1e-6); } -/* *************************************************************************/ +/* ************************************************************************* TEST( SmartProjectionPoseFactor, noisy ) { using namespace vanillaPose; @@ -197,7 +195,7 @@ TEST( SmartProjectionPoseFactor, noisy ) { DOUBLES_EQUAL(actualError1, actualError2, 1e-7); } -/* *************************************************************************/ +/* ************************************************************************* TEST(SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform) { using namespace vanillaPose; @@ -272,7 +270,7 @@ TEST(SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform) { EXPECT(assert_equal(wTb3, result.at(x3))); } -/* *************************************************************************/ +/* ************************************************************************* TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) { using namespace vanillaPose2; @@ -333,7 +331,7 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) { EXPECT(assert_equal(pose_above, result.at(x3), 1e-6)); } -/* *************************************************************************/ +/* ************************************************************************* TEST( SmartProjectionPoseFactor, Factors ) { using namespace vanillaPose; @@ -497,7 +495,7 @@ TEST( SmartProjectionPoseFactor, Factors ) { } } -/* *************************************************************************/ +/* ************************************************************************* TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) { using namespace vanillaPose; @@ -551,7 +549,7 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) { EXPECT(assert_equal(pose_above, result.at(x3), 1e-7)); } -/* *************************************************************************/ +/* ************************************************************************* TEST( SmartProjectionPoseFactor, jacobianSVD ) { using namespace vanillaPose; @@ -607,7 +605,7 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ) { EXPECT(assert_equal(pose_above, result.at(x3), 1e-6)); } -/* *************************************************************************/ +/* ************************************************************************* TEST( SmartProjectionPoseFactor, landmarkDistance ) { using namespace vanillaPose; @@ -666,7 +664,7 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ) { EXPECT(assert_equal(values.at(x3), result.at(x3))); } -/* *************************************************************************/ +/* ************************************************************************* TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) { using namespace vanillaPose; @@ -732,58 +730,7 @@ TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) { EXPECT(assert_equal(cam3.pose(), result.at(x3))); } -/* *************************************************************************/ -TEST( SmartProjectionPoseFactor, jacobianQ ) { - - using namespace vanillaPose; - - KeyVector views {x1, x2, x3}; - - Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; - - // Project three landmarks into three cameras - projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); - projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); - projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - - SmartProjectionParams params; - params.setLinearizationMode(gtsam::JACOBIAN_Q); - - SmartFactor::shared_ptr smartFactor1( - new SmartFactor(model, sharedK, params)); - smartFactor1->add(measurements_cam1, views); - - SmartFactor::shared_ptr smartFactor2( - new SmartFactor(model, sharedK, params)); - smartFactor2->add(measurements_cam2, views); - - SmartFactor::shared_ptr smartFactor3( - new SmartFactor(model, sharedK, params)); - smartFactor3->add(measurements_cam3, views); - - const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - - NonlinearFactorGraph graph; - graph.push_back(smartFactor1); - graph.push_back(smartFactor2); - graph.push_back(smartFactor3); - graph.addPrior(x1, cam1.pose(), noisePrior); - graph.addPrior(x2, cam2.pose(), noisePrior); - - Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), - Point3(0.1, 0.1, 0.1)); // smaller noise - Values values; - values.insert(x1, cam1.pose()); - values.insert(x2, cam2.pose()); - values.insert(x3, pose_above * noise_pose); - - Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); - result = optimizer.optimize(); - EXPECT(assert_equal(pose_above, result.at(x3), 1e-6)); -} - -/* *************************************************************************/ +/* ************************************************************************* TEST( SmartProjectionPoseFactor, 3poses_projection_factor ) { using namespace vanillaPose2; @@ -830,7 +777,7 @@ TEST( SmartProjectionPoseFactor, 3poses_projection_factor ) { EXPECT(assert_equal(pose_above, result.at(x3), 1e-7)); } -/* *************************************************************************/ +/* ************************************************************************* TEST( SmartProjectionPoseFactor, CheckHessian) { KeyVector views {x1, x2, x3}; @@ -912,144 +859,7 @@ TEST( SmartProjectionPoseFactor, CheckHessian) { EXPECT(assert_equal(InfoVector, GaussianGraph->hessian().second, 1e-6)); } -/* *************************************************************************/ -TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_factor ) { - using namespace vanillaPose2; - - KeyVector views {x1, x2, x3}; - - // Two different cameras, at the same position, but different rotations - Pose3 pose2 = level_pose * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); - Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); - Camera cam2(pose2, sharedK2); - Camera cam3(pose3, sharedK2); - - Point2Vector measurements_cam1, measurements_cam2; - - // Project three landmarks into three cameras - projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); - projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); - - SmartProjectionParams params; - params.setRankTolerance(50); - params.setDegeneracyMode(gtsam::HANDLE_INFINITY); - - SmartFactor::shared_ptr smartFactor1( - new SmartFactor(model, sharedK2, params)); - smartFactor1->add(measurements_cam1, views); - - SmartFactor::shared_ptr smartFactor2( - new SmartFactor(model, sharedK2, params)); - smartFactor2->add(measurements_cam2, views); - - const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, 0.10); - Point3 positionPrior = Point3(0, 0, 1); - - NonlinearFactorGraph graph; - graph.push_back(smartFactor1); - graph.push_back(smartFactor2); - graph.addPrior(x1, cam1.pose(), noisePrior); - graph.emplace_shared >(x2, positionPrior, noisePriorTranslation); - graph.emplace_shared >(x3, positionPrior, noisePriorTranslation); - - Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), - Point3(0.1, 0.1, 0.1)); // smaller noise - Values values; - values.insert(x1, cam1.pose()); - values.insert(x2, pose2 * noise_pose); - values.insert(x3, pose3 * noise_pose); - - // params.verbosityLM = LevenbergMarquardtParams::SUMMARY; - LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); - Values result = optimizer.optimize(); - EXPECT(assert_equal(pose3, result.at(x3))); -} - -/* *************************************************************************/ -TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ) { - - // this test considers a condition in which the cheirality constraint is triggered - using namespace vanillaPose; - - KeyVector views {x1, x2, x3}; - - // Two different cameras, at the same position, but different rotations - Pose3 pose2 = level_pose - * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); - Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); - Camera cam2(pose2, sharedK); - Camera cam3(pose3, sharedK); - - Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; - - // Project three landmarks into three cameras - projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); - projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); - projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - - SmartProjectionParams params; - params.setRankTolerance(10); - params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); - - SmartFactor::shared_ptr smartFactor1( - new SmartFactor(model, sharedK, params)); - smartFactor1->add(measurements_cam1, views); - - SmartFactor::shared_ptr smartFactor2( - new SmartFactor(model, sharedK, params)); - smartFactor2->add(measurements_cam2, views); - - SmartFactor::shared_ptr smartFactor3( - new SmartFactor(model, sharedK, params)); - smartFactor3->add(measurements_cam3, views); - - const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, - 0.10); - Point3 positionPrior = Point3(0, 0, 1); - - NonlinearFactorGraph graph; - graph.push_back(smartFactor1); - graph.push_back(smartFactor2); - graph.push_back(smartFactor3); - graph.addPrior(x1, cam1.pose(), noisePrior); - graph.emplace_shared >(x2, positionPrior, noisePriorTranslation); - graph.emplace_shared >(x3, positionPrior, noisePriorTranslation); - - // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), - Point3(0.1, 0.1, 0.1)); // smaller noise - Values values; - values.insert(x1, cam1.pose()); - values.insert(x2, cam2.pose()); - values.insert(x3, pose3 * noise_pose); - EXPECT( - assert_equal( - Pose3( - Rot3(0.00563056869, -0.130848107, 0.991386438, -0.991390265, - -0.130426831, -0.0115837907, 0.130819108, -0.98278564, - -0.130455917), - Point3(0.0897734171, -0.110201006, 0.901022872)), - values.at(x3))); - - Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); - result = optimizer.optimize(); - - // Since we do not do anything on degenerate instances (ZERO_ON_DEGENERACY) - // rotation remains the same as the initial guess, but position is fixed by PoseTranslationPrior -#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION - EXPECT(assert_equal(Pose3(values.at(x3).rotation(), - Point3(0,0,1)), result.at(x3))); -#else - // if the check is disabled, no cheirality exception if thrown and the pose converges to the right rotation - // with modest accuracy since the configuration is essentially degenerate without the translation due to noise (noise_pose) - EXPECT(assert_equal(pose3, result.at(x3),1e-3)); -#endif -} - -/* *************************************************************************/ +/* ************************************************************************* TEST( SmartProjectionPoseFactor, Hessian ) { using namespace vanillaPose2; @@ -1080,299 +890,6 @@ TEST( SmartProjectionPoseFactor, Hessian ) { // check that it is correctly scaled when using noiseProjection = [1/4 0; 0 1/4] } -/* *************************************************************************/ -TEST( SmartProjectionPoseFactor, HessianWithRotation ) { - // cout << " ************************ SmartProjectionPoseFactor: rotated Hessian **********************" << endl; - - using namespace vanillaPose; - - KeyVector views {x1, x2, x3}; - - Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; - - projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); - - SmartFactor::shared_ptr smartFactorInstance(new SmartFactor(model, sharedK)); - smartFactorInstance->add(measurements_cam1, views); - - Values values; - values.insert(x1, cam1.pose()); - values.insert(x2, cam2.pose()); - values.insert(x3, cam3.pose()); - - boost::shared_ptr factor = smartFactorInstance->linearize( - values); - - Pose3 poseDrift = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0)); - - Values rotValues; - rotValues.insert(x1, poseDrift.compose(level_pose)); - rotValues.insert(x2, poseDrift.compose(pose_right)); - rotValues.insert(x3, poseDrift.compose(pose_above)); - - boost::shared_ptr factorRot = smartFactorInstance->linearize( - rotValues); - - // Hessian is invariant to rotations in the nondegenerate case - EXPECT(assert_equal(factor->information(), factorRot->information(), 1e-7)); - - Pose3 poseDrift2 = Pose3(Rot3::Ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2), - Point3(10, -4, 5)); - - Values tranValues; - tranValues.insert(x1, poseDrift2.compose(level_pose)); - tranValues.insert(x2, poseDrift2.compose(pose_right)); - tranValues.insert(x3, poseDrift2.compose(pose_above)); - - boost::shared_ptr factorRotTran = - smartFactorInstance->linearize(tranValues); - - // Hessian is invariant to rotations and translations in the nondegenerate case - EXPECT(assert_equal(factor->information(), factorRotTran->information(), 1e-7)); -} - -/* *************************************************************************/ -TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ) { - - using namespace vanillaPose2; - - KeyVector views {x1, x2, x3}; - - // All cameras have the same pose so will be degenerate ! - Camera cam2(level_pose, sharedK2); - Camera cam3(level_pose, sharedK2); - - Point2Vector measurements_cam1; - projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); - - SmartFactor::shared_ptr smartFactor(new SmartFactor(model, sharedK2)); - smartFactor->add(measurements_cam1, views); - - Values values; - values.insert(x1, cam1.pose()); - values.insert(x2, cam2.pose()); - values.insert(x3, cam3.pose()); - - boost::shared_ptr factor = smartFactor->linearize(values); - - Pose3 poseDrift = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0)); - - Values rotValues; - rotValues.insert(x1, poseDrift.compose(level_pose)); - rotValues.insert(x2, poseDrift.compose(level_pose)); - rotValues.insert(x3, poseDrift.compose(level_pose)); - - boost::shared_ptr factorRot = // - smartFactor->linearize(rotValues); - - // Hessian is invariant to rotations in the nondegenerate case - EXPECT(assert_equal(factor->information(), factorRot->information(), 1e-7)); - - Pose3 poseDrift2 = Pose3(Rot3::Ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2), - Point3(10, -4, 5)); - - Values tranValues; - tranValues.insert(x1, poseDrift2.compose(level_pose)); - tranValues.insert(x2, poseDrift2.compose(level_pose)); - tranValues.insert(x3, poseDrift2.compose(level_pose)); - - boost::shared_ptr factorRotTran = smartFactor->linearize( - tranValues); - - // Hessian is invariant to rotations and translations in the nondegenerate case - EXPECT(assert_equal(factor->information(), factorRotTran->information(), 1e-7)); -} - -/* ************************************************************************* */ -TEST( SmartProjectionPoseFactor, ConstructorWithCal3Bundler) { - using namespace bundlerPose; - SmartProjectionParams params; - params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); - SmartFactor factor(model, sharedBundlerK, params); - factor.add(measurement1, x1); -} - -/* *************************************************************************/ -TEST( SmartProjectionPoseFactor, Cal3Bundler ) { - - using namespace bundlerPose; - - // three landmarks ~5 meters in front of camera - Point3 landmark3(3, 0, 3.0); - - Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; - - // Project three landmarks into three cameras - projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); - projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); - projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - - KeyVector views {x1, x2, x3}; - - SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedBundlerK)); - smartFactor1->add(measurements_cam1, views); - - SmartFactor::shared_ptr smartFactor2(new SmartFactor(model, sharedBundlerK)); - smartFactor2->add(measurements_cam2, views); - - SmartFactor::shared_ptr smartFactor3(new SmartFactor(model, sharedBundlerK)); - smartFactor3->add(measurements_cam3, views); - - const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - - NonlinearFactorGraph graph; - graph.push_back(smartFactor1); - graph.push_back(smartFactor2); - graph.push_back(smartFactor3); - graph.addPrior(x1, cam1.pose(), noisePrior); - graph.addPrior(x2, cam2.pose(), noisePrior); - - // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), - Point3(0.1, 0.1, 0.1)); // smaller noise - Values values; - values.insert(x1, cam1.pose()); - values.insert(x2, cam2.pose()); - // initialize third pose with some noise, we expect it to move back to original pose_above - values.insert(x3, pose_above * noise_pose); - EXPECT( - assert_equal( - Pose3( - Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, - -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), - Point3(0.1, -0.1, 1.9)), values.at(x3))); - - Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); - result = optimizer.optimize(); - EXPECT(assert_equal(cam3.pose(), result.at(x3), 1e-6)); -} - -/* *************************************************************************/ -TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) { - - using namespace bundlerPose; - - KeyVector views {x1, x2, x3}; - - // Two different cameras - Pose3 pose2 = level_pose - * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); - Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); - Camera cam2(pose2, sharedBundlerK); - Camera cam3(pose3, sharedBundlerK); - - // landmark3 at 3 meters now - Point3 landmark3(3, 0, 3.0); - - Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; - - // Project three landmarks into three cameras - projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); - projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); - projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - - SmartProjectionParams params; - params.setRankTolerance(10); - params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); - - SmartFactor::shared_ptr smartFactor1( - new SmartFactor(model, sharedBundlerK, params)); - smartFactor1->add(measurements_cam1, views); - - SmartFactor::shared_ptr smartFactor2( - new SmartFactor(model, sharedBundlerK, params)); - smartFactor2->add(measurements_cam2, views); - - SmartFactor::shared_ptr smartFactor3( - new SmartFactor(model, sharedBundlerK, params)); - smartFactor3->add(measurements_cam3, views); - - const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, - 0.10); - Point3 positionPrior = Point3(0, 0, 1); - - NonlinearFactorGraph graph; - graph.push_back(smartFactor1); - graph.push_back(smartFactor2); - graph.push_back(smartFactor3); - graph.addPrior(x1, cam1.pose(), noisePrior); - graph.emplace_shared >(x2, positionPrior, noisePriorTranslation); - graph.emplace_shared >(x3, positionPrior, noisePriorTranslation); - - // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), - Point3(0.1, 0.1, 0.1)); // smaller noise - Values values; - values.insert(x1, cam1.pose()); - values.insert(x2, cam2.pose()); - // initialize third pose with some noise, we expect it to move back to original pose_above - values.insert(x3, pose3 * noise_pose); - EXPECT( - assert_equal( - Pose3( - Rot3(0.00563056869, -0.130848107, 0.991386438, -0.991390265, - -0.130426831, -0.0115837907, 0.130819108, -0.98278564, - -0.130455917), - Point3(0.0897734171, -0.110201006, 0.901022872)), - values.at(x3))); - - Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); - result = optimizer.optimize(); - - EXPECT( - assert_equal( - Pose3( - Rot3(0.00563056869, -0.130848107, 0.991386438, -0.991390265, - -0.130426831, -0.0115837907, 0.130819108, -0.98278564, - -0.130455917), - Point3(0.0897734171, -0.110201006, 0.901022872)), - values.at(x3))); -} - -/* ************************************************************************* */ -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic"); -BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel"); -BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); - -TEST(SmartProjectionPoseFactor, serialize) { - using namespace vanillaPose; - using namespace gtsam::serializationTestHelpers; - SmartProjectionParams params; - params.setRankTolerance(rankTol); - SmartFactor factor(model, sharedK, params); - - EXPECT(equalsObj(factor)); - EXPECT(equalsXML(factor)); - EXPECT(equalsBinary(factor)); -} - -TEST(SmartProjectionPoseFactor, serialize2) { - using namespace vanillaPose; - using namespace gtsam::serializationTestHelpers; - SmartProjectionParams params; - params.setRankTolerance(rankTol); - Pose3 bts; - SmartFactor factor(model, sharedK, bts, params); - - // insert some measurments - KeyVector key_view; - Point2Vector meas_view; - key_view.push_back(Symbol('x', 1)); - meas_view.push_back(Point2(10, 10)); - factor.add(meas_view, key_view); - - EXPECT(equalsObj(factor)); - EXPECT(equalsXML(factor)); - EXPECT(equalsBinary(factor)); -} - /* ************************************************************************* */ int main() { TestResult tr; From 82844b541c1fa5d91982482f62739795db77c73e Mon Sep 17 00:00:00 2001 From: lcarlone Date: Mon, 19 Jul 2021 16:11:18 -0400 Subject: [PATCH 071/248] put in place initial functions --- ...martProjectionPoseFactorRollingShutter.cpp | 82 ---------- .../SmartProjectionPoseFactorRollingShutter.h | 146 ++++++++++++------ 2 files changed, 102 insertions(+), 126 deletions(-) diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.cpp index 4e1cbdd46..c0b22160a 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.cpp @@ -19,86 +19,4 @@ namespace gtsam { - -// -//void SmartProjectionPoseFactorRollingShutter::add( -// const std::vector& measurements, -// const KeyVector& world_P_body_keys, const KeyVector& body_P_cam_keys, -// const std::vector>& Ks) { -// assert(world_P_body_keys.size() == measurements.size()); -// assert(world_P_body_keys.size() == body_P_cam_keys.size()); -// assert(world_P_body_keys.size() == Ks.size()); -// for (size_t i = 0; i < measurements.size(); i++) { -// Base::add(measurements[i], world_P_body_keys[i]); -// // pose keys are assumed to be unique (1 observation per time stamp), but calibration can be shared -// if(std::find(keys_.begin(), keys_.end(), body_P_cam_keys[i]) == keys_.end()) -// keys_.push_back(body_P_cam_keys[i]); // add only unique keys -// -// world_P_body_keys_.push_back(world_P_body_keys[i]); -// body_P_cam_keys_.push_back(body_P_cam_keys[i]); -// -// K_all_.push_back(Ks[i]); -// } -//} -// -//void SmartProjectionPoseFactorRollingShutter::add( -// const std::vector& measurements, -// const KeyVector& world_P_body_keys, const KeyVector& body_P_cam_keys, -// const boost::shared_ptr& K) { -// assert(world_P_body_keys.size() == measurements.size()); -// assert(world_P_body_keys.size() == body_P_cam_keys.size()); -// for (size_t i = 0; i < measurements.size(); i++) { -// Base::add(measurements[i], world_P_body_keys[i]); -// // pose keys are assumed to be unique (1 observation per time stamp), but calibration can be shared -// if(std::find(keys_.begin(), keys_.end(), body_P_cam_keys[i]) == keys_.end()) -// keys_.push_back(body_P_cam_keys[i]); // add only unique keys -// -// world_P_body_keys_.push_back(world_P_body_keys[i]); -// body_P_cam_keys_.push_back(body_P_cam_keys[i]); -// -// K_all_.push_back(K); -// } -//} -// -//void SmartProjectionPoseFactorRollingShutter::print( -// const std::string& s, const KeyFormatter& keyFormatter) const { -// std::cout << s << "SmartProjectionPoseFactorRollingShutter: \n "; -// for (size_t i = 0; i < K_all_.size(); i++) { -// K_all_[i]->print("calibration = "); -// std::cout << " extrinsic pose key: " << keyFormatter(body_P_cam_keys_[i]) << std::endl; -// } -// Base::print("", keyFormatter); -//} -// -//bool SmartProjectionPoseFactorRollingShutter::equals(const NonlinearFactor& p, -// double tol) const { -// const SmartProjectionPoseFactorRollingShutter* e = -// dynamic_cast(&p); -// -// return e && Base::equals(p, tol) && -// body_P_cam_keys_ == e->getExtrinsicPoseKeys(); -//} -// -//double SmartProjectionPoseFactorRollingShutter::error(const Values& values) const { -// if (this->active(values)) { -// return this->totalReprojectionError(cameras(values)); -// } else { // else of active flag -// return 0.0; -// } -//} -// -//SmartProjectionPoseFactorRollingShutter::Base::Cameras -//SmartProjectionPoseFactorRollingShutter::cameras(const Values& values) const { -// assert(world_P_body_keys_.size() == K_all_.size()); -// assert(world_P_body_keys_.size() == body_P_cam_keys_.size()); -// Base::Cameras cameras; -// for (size_t i = 0; i < world_P_body_keys_.size(); i++) { -// Pose3 w_P_body = values.at(world_P_body_keys_[i]); -// Pose3 body_P_cam = values.at(body_P_cam_keys_[i]); -// Pose3 w_P_cam = w_P_body.compose(body_P_cam); -// cameras.push_back(StereoCamera(w_P_cam, K_all_[i])); -// } -// return cameras; -//} - } // \ namespace gtsam diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index fe5ccdf7f..840094f79 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -38,19 +38,22 @@ namespace gtsam { * @addtogroup SLAM */ template -class SmartProjectionPoseFactorRollingShutter: public SmartProjectionFactor< +class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor< PinholePose > { protected: /// shared pointer to calibration object (one for each observation) std::vector > K_all_; - // The keys of the pose of the body (with respect to an external world frame): two consecutive poses for each observation - std::vector> world_P_body_key_pairs_; + /// The keys of the pose of the body (with respect to an external world frame): two consecutive poses for each observation + std::vector> world_P_body_key_pairs_; - // interpolation factor (one for each observation) to interpolate between pair of consecutive poses + /// interpolation factor (one for each observation) to interpolate between pair of consecutive poses std::vector gammas_; + /// Pose of the camera in the body frame + std::vector body_P_sensors_; ///< Pose of the camera in the body frame + public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW @@ -77,7 +80,8 @@ class SmartProjectionPoseFactorRollingShutter: public SmartProjectionFactor< SmartProjectionPoseFactorRollingShutter( const SharedNoiseModel& sharedNoiseModel, const SmartProjectionParams& params = SmartProjectionParams()) - : Base(sharedNoiseModel, params) {} + : Base(sharedNoiseModel, params) { + } /** Virtual destructor */ ~SmartProjectionPoseFactorRollingShutter() override = default; @@ -91,24 +95,28 @@ class SmartProjectionPoseFactorRollingShutter: public SmartProjectionFactor< * @param gamma in [0,1] is the interpolation factor, such that if gamma = 0 the interpolated pose is the same as world_P_body_key * @param K is the (fixed) camera intrinsic calibration */ - void add(const Point2& measured, - const Key& world_P_body_key1, - const Key& world_P_body_key2, - const double& gamma, - const boost::shared_ptr& K){ + void add(const Point2& measured, const Key& world_P_body_key1, + const Key& world_P_body_key2, const double& gamma, + const boost::shared_ptr& K, const Pose3 body_P_sensor) { // store measurements in base class (note: we only store the first key there) Base::add(measured, world_P_body_key1); // but we also store the extrinsic calibration keys in the same order - world_P_body_key_pairs_.push_back(std::make_pair(world_P_body_key1,world_P_body_key2)); + world_P_body_key_pairs_.push_back( + std::make_pair(world_P_body_key1, world_P_body_key2)); // pose keys are assumed to be unique, so we avoid duplicates here - if(std::find( this->keys_.begin(), this->keys_.end(), world_P_body_key1) == this->keys_.end()) - this->keys_.push_back(world_P_body_key1); // add only unique keys - if(std::find( this->keys_.begin(), this->keys_.end(), world_P_body_key2) == this->keys_.end()) - this->keys_.push_back(world_P_body_key2); // add only unique keys + if (std::find(this->keys_.begin(), this->keys_.end(), world_P_body_key1) + == this->keys_.end()) + this->keys_.push_back(world_P_body_key1); // add only unique keys + if (std::find(this->keys_.begin(), this->keys_.end(), world_P_body_key2) + == this->keys_.end()) + this->keys_.push_back(world_P_body_key2); // add only unique keys // store fixed calibration K_all_.push_back(K); + + // store extrinsics of the camera + body_P_sensors_.push_back(body_P_sensor); } /** @@ -118,23 +126,55 @@ class SmartProjectionPoseFactorRollingShutter: public SmartProjectionFactor< * @param world_P_body_key_pairs vector of (1 for each view) containing the pair of poses from which each view can be interpolated * @param Ks vector of intrinsic calibration objects */ -// void add(const std::vector& measurements, -// const std::vector>& world_P_body_key_pairs, -// const std::vector& gammas, -// const std::vector>& Ks); + void add(const std::vector& measurements, + const std::vector>& world_P_body_key_pairs, + const std::vector& gammas, + const std::vector>& Ks, + const std::vector body_P_sensors) { + assert(world_P_body_key_pairs.size() == measurements.size()); + assert(world_P_body_key_pairs.size() == gammas.size()); + assert(world_P_body_key_pairs.size() == Ks.size()); + for (size_t i = 0; i < measurements.size(); i++) { + add(measurements[i], world_P_body_key_pairs[i].first, + world_P_body_key_pairs[i].second, gammas[i], Ks[i], + body_P_sensors[i]); + } + } /** * Variant of the previous one in which we include a set of measurements with - * the same calibration + * the same (intrinsic and extrinsic) calibration * @param measurements vector of the 2m dimensional location of the projection * of a single landmark in the m views (the measurements) * @param world_P_body_key_pairs vector of (1 for each view) containing the pair of poses from which each view can be interpolated * @param K the (known) camera calibration (same for all measurements) */ -// void add(const std::vector& measurements, -// const std::vector>& world_P_body_key_pairs, -// const std::vector& gammas, -// const boost::shared_ptr& K); + void add(const std::vector& measurements, + const std::vector>& world_P_body_key_pairs, + const std::vector& gammas, + const boost::shared_ptr& K, const Pose3 body_P_sensor) { + assert(world_P_body_key_pairs.size() == measurements.size()); + assert(world_P_body_key_pairs.size() == gammas.size()); + for (size_t i = 0; i < measurements.size(); i++) { + add(measurements[i], world_P_body_key_pairs[i].first, + world_P_body_key_pairs[i].second, gammas[i], K, body_P_sensor); + } + } + + /// return the calibration object + inline std::vector> calibration() const { + return K_all_; + } + + /// return the interpolation factors gammas + const std::vector getGammas() const { + return gammas_; + } + + /// return the interpolation factors gammas + const std::vector body_P_sensors() const { + return body_P_sensors_; + } /** * print @@ -142,24 +182,38 @@ class SmartProjectionPoseFactorRollingShutter: public SmartProjectionFactor< * @param keyFormatter optional formatter useful for printing Symbols */ void print(const std::string& s = "", const KeyFormatter& keyFormatter = - DefaultKeyFormatter) const override; + DefaultKeyFormatter) const override { + std::cout << s << "SmartProjectionPoseFactorRollingShutter: \n "; + for (size_t i = 0; i < K_all_.size(); i++) { + std::cout << "-- Measurement nr " << i << std::endl; + std::cout << " pose1 key: " + << keyFormatter(world_P_body_key_pairs_[i].first) << std::endl; + std::cout << " pose2 key: " + << keyFormatter(world_P_body_key_pairs_[i].second) << std::endl; + std::cout << " gamma: " << gammas_[i] << std::endl; + K_all_[i]->print("calibration = "); + } + Base::print("", keyFormatter); + } /// equals - bool equals(const NonlinearFactor& p, double tol = 1e-9) const override; + bool equals(const NonlinearFactor& p, double tol = 1e-9) const override { + const SmartProjectionPoseFactorRollingShutter* e = + dynamic_cast*>(&p); - /// equals - const std::vector getGammas() const { - return gammas_; + return e && Base::equals(p, tol) && K_all_ == e->calibration() + && gammas_ == e->getGammas() && body_P_sensors_ == e->body_P_sensors(); } /** * error calculates the error of the factor. */ - double error(const Values& values) const override; - - /** return the calibration object */ - inline std::vector> calibration() const { - return K_all_; + double error(const Values& values) const override { + if (this->active(values)) { + return this->totalReprojectionError(cameras(values)); + } else { // else of active flag + return 0.0; + } } /** @@ -169,12 +223,18 @@ class SmartProjectionPoseFactorRollingShutter: public SmartProjectionFactor< * @return Cameras */ typename Base::Cameras cameras(const Values& values) const override { + assert(world_P_body_keys_.size() == K_all_.size()); + assert(world_P_body_keys_.size() == body_P_cam_keys_.size()); typename Base::Cameras cameras; - for (const Key& k : this->keys_) { -// const Pose3 world_P_sensor_k = -// Base::body_P_sensor_ ? values.at(k) * *Base::body_P_sensor_ -// : values.at(k); -// cameras.emplace_back(world_P_sensor_k, K_); + for (size_t i = 0; i < world_P_body_key_pairs_.size(); i++) { + Pose3 w_P_body1 = values.at(world_P_body_key_pairs_[i].first); + Pose3 w_P_body2 = values.at(world_P_body_key_pairs_[i].second); + double interpolationFactor = gammas_[i]; + // get interpolated pose: + Pose3 w_P_body = w_P_body1.interpolateRt(w_P_body2, interpolationFactor); + Pose3 body_P_cam = body_P_sensors_[i]; + Pose3 w_P_cam = w_P_body.compose(body_P_cam); + cameras.emplace_back(w_P_cam, K_all_[i]); } return cameras; } @@ -225,7 +285,6 @@ class SmartProjectionPoseFactorRollingShutter: public SmartProjectionFactor< // } // } // } - /// linearize and return a Hessianfactor that is an approximation of error(p) // boost::shared_ptr > createHessianFactor( // const Values& values, const double lambda = 0.0, bool diagonalDamping = @@ -350,7 +409,6 @@ class SmartProjectionPoseFactorRollingShutter: public SmartProjectionFactor< // return boost::make_shared < RegularHessianFactor // > ( this->keys_, augmentedHessianUniqueKeys); // } - /** * Linearize to Gaussian Factor (possibly adding a damping factor Lambda for LM) * @param values Values structure which must contain camera poses and extrinsic pose for this factor @@ -379,7 +437,7 @@ class SmartProjectionPoseFactorRollingShutter: public SmartProjectionFactor< friend class boost::serialization::access; template void serialize(ARCHIVE& ar, const unsigned int /*version*/) { - ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(K_all_); } @@ -388,8 +446,8 @@ class SmartProjectionPoseFactorRollingShutter: public SmartProjectionFactor< /// traits template -struct traits > : public Testable< - SmartProjectionPoseFactorRollingShutter > { +struct traits > : + public Testable > { }; } // namespace gtsam From 1e2a1d259145928efbe8648dbcff6af244891278 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Mon, 19 Jul 2021 16:11:34 -0400 Subject: [PATCH 072/248] removed cpp --- ...martProjectionPoseFactorRollingShutter.cpp | 22 ------------------- 1 file changed, 22 deletions(-) delete mode 100644 gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.cpp diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.cpp deleted file mode 100644 index c0b22160a..000000000 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.cpp +++ /dev/null @@ -1,22 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 SmartProjectionPoseFactorRollingShutter.h - * @brief Smart projection factor on poses modeling rolling shutter effect - * @author Luca Carlone - */ - -//#include - -namespace gtsam { - -} // \ namespace gtsam From 16d624d4e1dd9f14e4e243cfb1d9abd1d0ca2432 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Mon, 19 Jul 2021 17:01:27 -0400 Subject: [PATCH 073/248] now I need to move to testing and interpolation --- .../SmartProjectionPoseFactorRollingShutter.h | 187 +++++++++--------- 1 file changed, 97 insertions(+), 90 deletions(-) diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index 840094f79..f1f0e2d71 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -67,9 +67,10 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor< /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; - static const int Dim = 6; ///< Pose3 dimension + static const int DimBlock = 12; ///< size of the variable stacking 2 poses from which the observation pose is interpolated + static const int DimPose = 6; ///< Pose3 dimension static const int ZDim = 2; ///< Measurement dimension (Point2) - typedef Eigen::Matrix MatrixZD; // F blocks (derivatives wrt camera) + typedef Eigen::Matrix MatrixZD; // F blocks (derivatives wrt camera) typedef std::vector > FBlocks; // vector of F blocks /** @@ -247,96 +248,101 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor< * respect to both the body pose and extrinsic pose), the point Jacobian E, * and the error vector b. Note that the jacobians are computed for a given point. */ -// void computeJacobiansAndCorrectForMissingMeasurements( -// FBlocks& Fs, Matrix& E, Vector& b, const Values& values) const { -// if (!result_) { -// throw("computeJacobiansWithTriangulatedPoint"); -// } else { // valid result: compute jacobians -// size_t numViews = measured_.size(); -// E = Matrix::Zero(3 * numViews, 3); // a StereoPoint2 for each view (point jacobian) -// b = Vector::Zero(3 * numViews); // a StereoPoint2 for each view -// Matrix dPoseCam_dPoseBody_i, dPoseCam_dPoseExt_i, dProject_dPoseCam_i, Ei; -// -// for (size_t i = 0; i < numViews; i++) { // for each camera/measurement -// Pose3 w_P_body = values.at(world_P_body_key_pairs_.at(i)); -// Pose3 body_P_cam = values.at(body_P_cam_ this->keys_.at(i)); -// StereoCamera camera( -// w_P_body.compose(body_P_cam, dPoseCam_dPoseBody_i, dPoseCam_dPoseExt_i), -// K_all_[i]); -// // get jacobians and error vector for current measurement -// StereoPoint2 reprojectionError_i = StereoPoint2( -// camera.project(*result_, dProject_dPoseCam_i, Ei) - measured_.at(i)); -// Eigen::Matrix J; // 3 x 12 -// J.block(0, 0) = dProject_dPoseCam_i * dPoseCam_dPoseBody_i; // (3x6) * (6x6) -// J.block(0, 6) = dProject_dPoseCam_i * dPoseCam_dPoseExt_i; // (3x6) * (6x6) -// // if the right pixel is invalid, fix jacobians -// if (std::isnan(measured_.at(i).uR())) -// { -// J.block<1, 12>(1, 0) = Matrix::Zero(1, 12); -// Ei.block<1, 3>(1, 0) = Matrix::Zero(1, 3); -// reprojectionError_i = StereoPoint2(reprojectionError_i.uL(), 0.0, -// reprojectionError_i.v()); -// } -// // fit into the output structures -// Fs.push_back(J); -// size_t row = 3 * i; -// b.segment(row) = -reprojectionError_i.vector(); -// E.block<3, 3>(row, 0) = Ei; -// } -// } -// } + void computeJacobiansWithTriangulatedPoint(FBlocks& Fs, Matrix& E, Vector& b, + const Values& values) const + override { + if (!this->result_) { + throw("computeJacobiansWithTriangulatedPoint"); + } else { // valid result: compute jacobians + size_t numViews = this->measured_.size(); + E = Matrix::Zero(2 * numViews, 3); // a Point2 for each view (point jacobian) + b = Vector::Zero(2 * numViews); // a Point2 for each view + Eigen::Matrix dProject_dPoseCam; + Eigen::Matrix dInterpPose_dPoseBody1, + dInterpPose_dPoseBody2, dPoseCam_dInterpPose; + Eigen::Matrix Ei; + + for (size_t i = 0; i < numViews; i++) { // for each camera/measurement + Pose3 w_P_body1 = values.at(world_P_body_key_pairs_[i].first); + Pose3 w_P_body2 = values.at(world_P_body_key_pairs_[i].second); + double interpolationFactor = gammas_[i]; + // get interpolated pose: + std::cout << "TODO: need to add proper interpolation and Jacobians here" << std::endl; + Pose3 w_P_body = w_P_body1.interpolateRt(w_P_body2, + interpolationFactor); /*dInterpPose_dPoseBody1, dInterpPose_dPoseBody2 */ + Pose3 body_P_cam = body_P_sensors_[i]; + Pose3 w_P_cam = w_P_body.compose(body_P_cam, dPoseCam_dInterpPose); + PinholeCamera camera(w_P_cam, K_all_[i]); + + // get jacobians and error vector for current measurement + Point2 reprojectionError_i = Point2( + camera.project(*this->result_, dProject_dPoseCam, Ei) + - this->measured_.at(i)); + Eigen::Matrix J; // 2 x 12 + J.block(0, 0) = dProject_dPoseCam * dPoseCam_dInterpPose + * dInterpPose_dPoseBody1; // (2x6) * (6x6) * (6x6) + J.block(0, 6) = dProject_dPoseCam * dPoseCam_dInterpPose + * dInterpPose_dPoseBody2; // (2x6) * (6x6) * (6x6) + + // fit into the output structures + Fs.push_back(J); + size_t row = 2 * i; + b.segment(row) = -reprojectionError_i; + E.block<3, 3>(row, 0) = Ei; + } + } + } + /// linearize and return a Hessianfactor that is an approximation of error(p) -// boost::shared_ptr > createHessianFactor( -// const Values& values, const double lambda = 0.0, bool diagonalDamping = -// false) const { -// -// // we may have multiple cameras sharing the same extrinsic cals, hence the number -// // of keys may be smaller than 2 * nrMeasurements (which is the upper bound where we -// // have a body key and an extrinsic calibration key for each measurement) -// size_t nrUniqueKeys = this->keys_.size(); -// size_t nrNonuniqueKeys = world_P_body_key_pairs_.size() -// + body_P_cam_ this->keys_.size(); -// -// // Create structures for Hessian Factors -// KeyVector js; -// std::vector < Matrix > Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2); -// std::vector gs(nrUniqueKeys); -// -// if (this->measured_.size() != cameras(values).size()) -// throw std::runtime_error("SmartStereoProjectionHessianFactor: this->" -// "measured_.size() inconsistent with input"); -// -// // triangulate 3D point at given linearization point -// triangulateSafe(cameras(values)); -// -// if (!result_) { // failed: return "empty/zero" Hessian -// for (Matrix& m : Gs) -// m = Matrix::Zero(DimPose, DimPose); -// for (Vector& v : gs) -// v = Vector::Zero(DimPose); -// return boost::make_shared < RegularHessianFactor -// > ( this->keys_, Gs, gs, 0.0); -// } -// -// // compute Jacobian given triangulated 3D Point -// FBlocks Fs; -// Matrix F, E; -// Vector b; -// computeJacobiansAndCorrectForMissingMeasurements(Fs, E, b, values); -// -// // Whiten using noise model -// noiseModel_->WhitenSystem(E, b); -// for (size_t i = 0; i < Fs.size(); i++) -// Fs[i] = noiseModel_->Whiten(Fs[i]); -// + boost::shared_ptr > createHessianFactor( + const Values& values, const double lambda = 0.0, bool diagonalDamping = + false) const override { + + // we may have multiple cameras sharing the same extrinsic cals, hence the number + // of keys may be smaller than 2 * nrMeasurements (which is the upper bound where we + // have a body key and an extrinsic calibration key for each measurement) + size_t nrUniqueKeys = this->keys_.size(); + size_t nrNonuniqueKeys = 2*world_P_body_key_pairs_.size(); + + // Create structures for Hessian Factors + KeyVector js; + std::vector < Matrix > Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2); + std::vector gs(nrUniqueKeys); + + if (this->measured_.size() != cameras(values).size()) + throw std::runtime_error("SmartProjectionPoseFactorRollingShutter: " + "measured_.size() inconsistent with input"); + + // triangulate 3D point at given linearization point + triangulateSafe(cameras(values)); + + if (!this->result_) { // failed: return "empty/zero" Hessian + for (Matrix& m : Gs) + m = Matrix::Zero(DimPose, DimPose); + for (Vector& v : gs) + v = Vector::Zero(DimPose); + return boost::make_shared < RegularHessianFactor + > ( this->keys_, Gs, gs, 0.0); + } + + // compute Jacobian given triangulated 3D Point + FBlocks Fs; + Matrix F, E; + Vector b; + computeJacobiansWithTriangulatedPoint(Fs, E, b, values); + + // Whiten using noise model + this->noiseModel_->WhitenSystem(E, b); + for (size_t i = 0; i < Fs.size(); i++) + Fs[i] = this->noiseModel_->Whiten(Fs[i]); + // // build augmented Hessian (with last row/column being the information vector) // Matrix3 P; -// Cameras::ComputePointCovariance<3>(P, E, lambda, diagonalDamping); +// This::Cameras::ComputePointCovariance<3>(P, E, lambda, diagonalDamping); // // // marginalize point: note - we reuse the standard SchurComplement function -// SymmetricBlockMatrix augmentedHessian = -// Cameras::SchurComplement<3, Dim>(Fs, E, P, b); -// +// SymmetricBlockMatrix augmentedHessian = This::Cameras::SchurComplement<2,DimBlock>(Fs, E, P, b); + // // now pack into an Hessian factor // std::vector dims(nrUniqueKeys + 1); // this also includes the b term // std::fill(dims.begin(), dims.end() - 1, 6); @@ -408,7 +414,8 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor< // } // return boost::make_shared < RegularHessianFactor // > ( this->keys_, augmentedHessianUniqueKeys); -// } + } + /** * Linearize to Gaussian Factor (possibly adding a damping factor Lambda for LM) * @param values Values structure which must contain camera poses and extrinsic pose for this factor @@ -418,8 +425,8 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor< const Values& values, const double lambda = 0.0) const { // depending on flag set on construction we may linearize to different linear factors switch (this->params_.linearizationMode) { -// case HESSIAN: -// return createHessianFactor(values, lambda); + case HESSIAN: + return createHessianFactor(values, lambda); default: throw std::runtime_error( "SmartProjectionPoseFactorRollingShutter: unknown linearization mode"); From a0ca3387fb1b1bee2d87f84f0bb7fdcb6c007b58 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Mon, 19 Jul 2021 22:30:32 -0400 Subject: [PATCH 074/248] added interpolation function from shteren1 --- gtsam/base/Lie.h | 27 ++++++++++++- gtsam/geometry/tests/testPose3.cpp | 62 ++++++++++++++++++++++++++++++ 2 files changed, 87 insertions(+), 2 deletions(-) diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index fe730c934..06f63963e 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -17,6 +17,7 @@ * @author Frank Dellaert * @author Mike Bosse * @author Duy Nguyen Ta + * @author shteren1 */ @@ -322,8 +323,30 @@ T expm(const Vector& x, int K=7) { * Linear interpolation between X and Y by coefficient t in [0, 1]. */ template -T interpolate(const T& X, const T& Y, double t) { - assert(t >= 0 && t <= 1); +T interpolate(const T& X, const T& Y, double t, + OptionalJacobian< traits::dimension, traits::dimension > Hx = boost::none, + OptionalJacobian< traits::dimension, traits::dimension > Hy = boost::none) { + assert(t >= 0.0 && t <= 1.0); + if (Hx || Hy) { + typedef Eigen::Matrix::dimension, traits::dimension> Jacobian; + typename traits::TangentVector log_Xinv_Y; + Jacobian Hx_tmp, Hy_tmp, H1, H2; + + T Xinv_Y = traits::Between(X, Y, Hx_tmp, Hy_tmp); + log_Xinv_Y = traits::Logmap(Xinv_Y, H1); + Hx_tmp = H1 * Hx_tmp; + Hy_tmp = H1 * Hy_tmp; + Xinv_Y = traits::Expmap(t * log_Xinv_Y, H1); + Hx_tmp = t * H1 * Hx_tmp; + Hy_tmp = t * H1 * Hy_tmp; + Xinv_Y = traits::Compose(X, Xinv_Y, H1, H2); + Hx_tmp = H1 + H2 * Hx_tmp; + Hy_tmp = H2 * Hy_tmp; + + if(Hx) *Hx = Hx_tmp; + if(Hy) *Hy = Hy_tmp; + return Xinv_Y; + } return traits::Compose(X, traits::Expmap(t * traits::Logmap(traits::Between(X, Y)))); } diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 11b8791d4..7c1fa81e6 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -1046,6 +1046,68 @@ TEST(Pose3, interpolate) { EXPECT(assert_equal(expected2, T2.interpolateRt(T3, t))); } +/* ************************************************************************* */ +Pose3 testing_interpolate(const Pose3& t1, const Pose3& t2, double gamma) { return interpolate(t1,t2,gamma); } + +TEST(Pose3, interpolateJacobians) { + { + Pose3 X = Pose3::identity(); + Pose3 Y(Rot3::Rz(M_PI_2), Point3(1, 0, 0)); + double t = 0.5; + Pose3 expectedPoseInterp(Rot3::Rz(M_PI_4), Point3(0.5, -0.207107, 0)); // note: different from test above: this is full Pose3 interpolation + Matrix actualJacobianX, actualJacobianY; + EXPECT(assert_equal(expectedPoseInterp, interpolate(X, Y, t, actualJacobianX, actualJacobianY), 1e-5)); + + Matrix expectedJacobianX = numericalDerivative31(testing_interpolate, X, Y, t); + EXPECT(assert_equal(expectedJacobianX,actualJacobianX,1e-6)); + + Matrix expectedJacobianY = numericalDerivative32(testing_interpolate, X, Y, t); + EXPECT(assert_equal(expectedJacobianY,actualJacobianY,1e-6)); + } + { + Pose3 X = Pose3::identity(); + Pose3 Y(Rot3::identity(), Point3(1, 0, 0)); + double t = 0.3; + Pose3 expectedPoseInterp(Rot3::identity(), Point3(0.3, 0, 0)); + Matrix actualJacobianX, actualJacobianY; + EXPECT(assert_equal(expectedPoseInterp, interpolate(X, Y, t, actualJacobianX, actualJacobianY), 1e-5)); + + Matrix expectedJacobianX = numericalDerivative31(testing_interpolate, X, Y, t); + EXPECT(assert_equal(expectedJacobianX,actualJacobianX,1e-6)); + + Matrix expectedJacobianY = numericalDerivative32(testing_interpolate, X, Y, t); + EXPECT(assert_equal(expectedJacobianY,actualJacobianY,1e-6)); + } + { + Pose3 X = Pose3::identity(); + Pose3 Y(Rot3::Rz(M_PI_2), Point3(0, 0, 0)); + double t = 0.5; + Pose3 expectedPoseInterp(Rot3::Rz(M_PI_4), Point3(0, 0, 0)); + Matrix actualJacobianX, actualJacobianY; + EXPECT(assert_equal(expectedPoseInterp, interpolate(X, Y, t, actualJacobianX, actualJacobianY), 1e-5)); + + Matrix expectedJacobianX = numericalDerivative31(testing_interpolate, X, Y, t); + EXPECT(assert_equal(expectedJacobianX,actualJacobianX,1e-6)); + + Matrix expectedJacobianY = numericalDerivative32(testing_interpolate, X, Y, t); + EXPECT(assert_equal(expectedJacobianY,actualJacobianY,1e-6)); + } + { + Pose3 X(Rot3::Ypr(0.1,0.2,0.3), Point3(10, 5, -2)); + Pose3 Y(Rot3::Ypr(1.1,-2.2,-0.3), Point3(-5, 1, 1)); + double t = 0.3; + Pose3 expectedPoseInterp(Rot3::Rz(M_PI_4), Point3(0, 0, 0)); + Matrix actualJacobianX, actualJacobianY; + interpolate(X, Y, t, actualJacobianX, actualJacobianY); + + Matrix expectedJacobianX = numericalDerivative31(testing_interpolate, X, Y, t); + EXPECT(assert_equal(expectedJacobianX,actualJacobianX,1e-6)); + + Matrix expectedJacobianY = numericalDerivative32(testing_interpolate, X, Y, t); + EXPECT(assert_equal(expectedJacobianY,actualJacobianY,1e-6)); + } +} + /* ************************************************************************* */ TEST(Pose3, Create) { Matrix63 actualH1, actualH2; From 4c997e547433a988c6a6c1fb5c06eb220f0806f3 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Tue, 20 Jul 2021 20:46:29 -0400 Subject: [PATCH 075/248] removed interp from Pose3, starting to take pass on projection factor and test --- ...ShutterProjectionFactor.h => ProjectionFactorRollingShutter.h} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename gtsam_unstable/slam/{RollingShutterProjectionFactor.h => ProjectionFactorRollingShutter.h} (100%) diff --git a/gtsam_unstable/slam/RollingShutterProjectionFactor.h b/gtsam_unstable/slam/ProjectionFactorRollingShutter.h similarity index 100% rename from gtsam_unstable/slam/RollingShutterProjectionFactor.h rename to gtsam_unstable/slam/ProjectionFactorRollingShutter.h From a204f6d5088f1cea60a70d71e789012f0de63764 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Tue, 20 Jul 2021 20:46:43 -0400 Subject: [PATCH 076/248] amended --- gtsam/geometry/Pose3.cpp | 4 - gtsam/geometry/Pose3.h | 9 - .../slam/ProjectionFactorRollingShutter.h | 22 +- .../testProjectionFactorRollingShutter.cpp | 233 ++++++++++++++++++ 4 files changed, 244 insertions(+), 24 deletions(-) create mode 100644 gtsam_unstable/slam/tests/testProjectionFactorRollingShutter.cpp diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index b2cdd0c87..c183e32ed 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -423,8 +423,4 @@ std::ostream &operator<<(std::ostream &os, const Pose3& pose) { return os; } -Pose3 pose3_interp(const Pose3& X, const Pose3& Y, double t, Matrix& Hx, Matrix& Hy) { - return X.interp(t, Y, Hx, Hy); -} - } // namespace gtsam diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 00c6fa8af..d76e1b41a 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -353,15 +353,6 @@ public: return std::make_pair(0, 2); } - /** - * @brief Spherical Linear interpolation between *this and other - * @param s a value between 0 and 1.5 - * @param other final point of iterpolation geodesic on manifold - * @param Hx jacobian of the interpolation on this - & @param Hy jacobian of the interpolation on other - */ - Pose3 interp(double t, const Pose3& other, OptionalJacobian<6, 6> Hx = boost::none, OptionalJacobian<6, 6> Hy = boost::none) const; - /// Output stream operator GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Pose3& p); diff --git a/gtsam_unstable/slam/ProjectionFactorRollingShutter.h b/gtsam_unstable/slam/ProjectionFactorRollingShutter.h index 69b7e7376..499f1cec3 100644 --- a/gtsam_unstable/slam/ProjectionFactorRollingShutter.h +++ b/gtsam_unstable/slam/ProjectionFactorRollingShutter.h @@ -10,8 +10,8 @@ * -------------------------------------------------------------------------- */ /** - * @file RollingShutterProjectionFactor.h - * @brief Basic bearing factor from 2D measurement for rolling shutter cameras + * @file ProjectionFactorRollingShutter.h + * @brief Basic projection factor for rolling shutter cameras * @author Yotam Stern */ @@ -34,7 +34,7 @@ namespace gtsam { * @addtogroup SLAM */ - class RollingShutterProjectionFactor: public NoiseModelFactor3 { + class ProjectionFactorRollingShutter: public NoiseModelFactor3 { protected: // Keep a copy of measurement and calibration for I/O @@ -53,13 +53,13 @@ namespace gtsam { typedef NoiseModelFactor3 Base; /// shorthand for this class - typedef RollingShutterProjectionFactor This; + typedef ProjectionFactorRollingShutter This; /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; /// Default constructor - RollingShutterProjectionFactor() : + ProjectionFactorRollingShutter() : measured_(0, 0), interp_param_(0), throwCheirality_(false), verboseCheirality_(false) { } @@ -74,7 +74,7 @@ namespace gtsam { * @param K shared pointer to the constant calibration * @param body_P_sensor is the transform from body to sensor frame (default identity) */ - RollingShutterProjectionFactor(const Point2& measured, double interp_param, const SharedNoiseModel& model, + ProjectionFactorRollingShutter(const Point2& measured, double interp_param, const SharedNoiseModel& model, Key poseKey_a, Key poseKey_b, Key pointKey, const boost::shared_ptr& K, boost::optional body_P_sensor = boost::none) : Base(model, poseKey_a, poseKey_b, pointKey), measured_(measured), interp_param_(interp_param), K_(K), body_P_sensor_(body_P_sensor), @@ -93,7 +93,7 @@ namespace gtsam { * @param verboseCheirality determines whether exceptions are printed for Cheirality * @param body_P_sensor is the transform from body to sensor frame (default identity) */ - RollingShutterProjectionFactor(const Point2& measured, double interp_param, const SharedNoiseModel& model, + ProjectionFactorRollingShutter(const Point2& measured, double interp_param, const SharedNoiseModel& model, Key poseKey_a, Key poseKey_b, Key pointKey, const boost::shared_ptr& K, bool throwCheirality, bool verboseCheirality, boost::optional body_P_sensor = boost::none) : @@ -101,7 +101,7 @@ namespace gtsam { throwCheirality_(throwCheirality), verboseCheirality_(verboseCheirality) {} /** Virtual destructor */ - virtual ~RollingShutterProjectionFactor() {} + virtual ~ProjectionFactorRollingShutter() {} /// @return a deep copy of this factor virtual gtsam::NonlinearFactor::shared_ptr clone() const { @@ -114,7 +114,7 @@ namespace gtsam { * @param keyFormatter optional formatter useful for printing Symbols */ void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { - std::cout << s << "RollingShutterProjectionFactor, z = "; + std::cout << s << "ProjectionFactorRollingShutter, z = "; traits::Print(measured_); std::cout << " rolling shutter interpolation param = " << interp_param_; if(this->body_P_sensor_) @@ -141,7 +141,7 @@ namespace gtsam { gtsam::Matrix Hprj; //pose = interpolate(pose_a, pose_b, interp_param_, H1, H2); - pose = pose_a.interp(interp_param_, pose_b, H1, H2); + pose = interpolate(pose_a, pose_b, interp_param_, H1, H2); try { if(body_P_sensor_) { if(H1 && H2) { @@ -211,4 +211,4 @@ namespace gtsam { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; // rolling shutter projection factor -} //namespace gtsam \ No newline at end of file +} //namespace gtsam diff --git a/gtsam_unstable/slam/tests/testProjectionFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testProjectionFactorRollingShutter.cpp new file mode 100644 index 000000000..b0fcb23b1 --- /dev/null +++ b/gtsam_unstable/slam/tests/testProjectionFactorRollingShutter.cpp @@ -0,0 +1,233 @@ +/* ---------------------------------------------------------------------------- + + * 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 testProjectionFactorRollingShutter.cpp + * @brief Unit tests for ProjectionFactorRollingShutter Class + * @author Luca Carlone + * @date July 2021 + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +using namespace std::placeholders; +using namespace std; +using namespace gtsam; + +// make a realistic calibration matrix +static double fov = 60; // degrees +static size_t w=640,h=480; +static Cal3_S2::shared_ptr K(new Cal3_S2(fov,w,h)); + +// Create a noise model for the pixel error +static SharedNoiseModel model(noiseModel::Unit::Create(2)); + +// Convenience for named keys +using symbol_shorthand::X; +using symbol_shorthand::L; +using symbol_shorthand::T; + +//typedef ProjectionFactorPPP TestProjectionFactor; + +///// traits +//namespace gtsam { +//template<> +//struct traits : public Testable { +//}; +//} +// +///* ************************************************************************* */ +//TEST( ProjectionFactorPPP, nonStandard ) { +// ProjectionFactorPPP f; +//} +// +///* ************************************************************************* */ +//TEST( ProjectionFactorPPP, Constructor) { +// Key poseKey(X(1)); +// Key transformKey(T(1)); +// Key pointKey(L(1)); +// +// Point2 measurement(323.0, 240.0); +// +// TestProjectionFactor factor(measurement, model, poseKey, transformKey, pointKey, K); +//} +// +///* ************************************************************************* */ +//TEST( ProjectionFactorPPP, ConstructorWithTransform) { +// Key poseKey(X(1)); +// Key transformKey(T(1)); +// Key pointKey(L(1)); +// +// Point2 measurement(323.0, 240.0); +// TestProjectionFactor factor(measurement, model, poseKey, transformKey, pointKey, K); +//} +// +///* ************************************************************************* */ +//TEST( ProjectionFactorPPP, Equals ) { +// // Create two identical factors and make sure they're equal +// Point2 measurement(323.0, 240.0); +// +// TestProjectionFactor factor1(measurement, model, X(1), T(1), L(1), K); +// TestProjectionFactor factor2(measurement, model, X(1), T(1), L(1), K); +// +// CHECK(assert_equal(factor1, factor2)); +//} +// +///* ************************************************************************* */ +//TEST( ProjectionFactorPPP, EqualsWithTransform ) { +// // Create two identical factors and make sure they're equal +// Point2 measurement(323.0, 240.0); +// Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); +// +// TestProjectionFactor factor1(measurement, model, X(1), T(1), L(1), K); +// TestProjectionFactor factor2(measurement, model, X(1), T(1), L(1), K); +// +// CHECK(assert_equal(factor1, factor2)); +//} +// +///* ************************************************************************* */ +//TEST( ProjectionFactorPPP, Error ) { +// // Create the factor with a measurement that is 3 pixels off in x +// Key poseKey(X(1)); +// Key transformKey(T(1)); +// Key pointKey(L(1)); +// Point2 measurement(323.0, 240.0); +// TestProjectionFactor factor(measurement, model, poseKey, transformKey, pointKey, K); +// +// // Set the linearization point +// Pose3 pose(Rot3(), Point3(0,0,-6)); +// Point3 point(0.0, 0.0, 0.0); +// +// // Use the factor to calculate the error +// Vector actualError(factor.evaluateError(pose, Pose3(), point)); +// +// // The expected error is (-3.0, 0.0) pixels / UnitCovariance +// Vector expectedError = Vector2(-3.0, 0.0); +// +// // Verify we get the expected error +// CHECK(assert_equal(expectedError, actualError, 1e-9)); +//} +// +///* ************************************************************************* */ +//TEST( ProjectionFactorPPP, ErrorWithTransform ) { +// // Create the factor with a measurement that is 3 pixels off in x +// Key poseKey(X(1)); +// Key transformKey(T(1)); +// Key pointKey(L(1)); +// Point2 measurement(323.0, 240.0); +// Pose3 transform(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); +// TestProjectionFactor factor(measurement, model, poseKey,transformKey, pointKey, K); +// +// // Set the linearization point. The vehicle pose has been selected to put the camera at (-6, 0, 0) +// Pose3 pose(Rot3(), Point3(-6.25, 0.10 , -1.0)); +// Point3 point(0.0, 0.0, 0.0); +// +// // Use the factor to calculate the error +// Vector actualError(factor.evaluateError(pose, transform, point)); +// +// // The expected error is (-3.0, 0.0) pixels / UnitCovariance +// Vector expectedError = Vector2(-3.0, 0.0); +// +// // Verify we get the expected error +// CHECK(assert_equal(expectedError, actualError, 1e-9)); +//} +// +///* ************************************************************************* */ +//TEST( ProjectionFactorPPP, Jacobian ) { +// // Create the factor with a measurement that is 3 pixels off in x +// Key poseKey(X(1)); +// Key transformKey(T(1)); +// Key pointKey(L(1)); +// Point2 measurement(323.0, 240.0); +// TestProjectionFactor factor(measurement, model, poseKey, transformKey, pointKey, K); +// +// // Set the linearization point +// Pose3 pose(Rot3(), Point3(0,0,-6)); +// Point3 point(0.0, 0.0, 0.0); +// +// // Use the factor to calculate the Jacobians +// Matrix H1Actual, H2Actual, H3Actual; +// factor.evaluateError(pose, Pose3(), point, H1Actual, H2Actual, H3Actual); +// +// // The expected Jacobians +// Matrix H1Expected = (Matrix(2, 6) << 0., -554.256, 0., -92.376, 0., 0., 554.256, 0., 0., 0., -92.376, 0.).finished(); +// Matrix H3Expected = (Matrix(2, 3) << 92.376, 0., 0., 0., 92.376, 0.).finished(); +// +// // Verify the Jacobians are correct +// CHECK(assert_equal(H1Expected, H1Actual, 1e-3)); +// CHECK(assert_equal(H3Expected, H3Actual, 1e-3)); +// +// // Verify H2 with numerical derivative +// Matrix H2Expected = numericalDerivative32( +// std::function( +// std::bind(&TestProjectionFactor::evaluateError, &factor, +// std::placeholders::_1, std::placeholders::_2, +// std::placeholders::_3, boost::none, boost::none, +// boost::none)), +// pose, Pose3(), point); +// +// CHECK(assert_equal(H2Expected, H2Actual, 1e-5)); +//} +// +///* ************************************************************************* */ +//TEST( ProjectionFactorPPP, JacobianWithTransform ) { +// // Create the factor with a measurement that is 3 pixels off in x +// Key poseKey(X(1)); +// Key transformKey(T(1)); +// Key pointKey(L(1)); +// Point2 measurement(323.0, 240.0); +// Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); +// TestProjectionFactor factor(measurement, model, poseKey, transformKey, pointKey, K); +// +// // Set the linearization point. The vehicle pose has been selected to put the camera at (-6, 0, 0) +// Pose3 pose(Rot3(), Point3(-6.25, 0.10 , -1.0)); +// Point3 point(0.0, 0.0, 0.0); +// +// // Use the factor to calculate the Jacobians +// Matrix H1Actual, H2Actual, H3Actual; +// factor.evaluateError(pose, body_P_sensor, point, H1Actual, H2Actual, H3Actual); +// +// // The expected Jacobians +// Matrix H1Expected = (Matrix(2, 6) << -92.376, 0., 577.350, 0., 92.376, 0., -9.2376, -577.350, 0., 0., 0., 92.376).finished(); +// Matrix H3Expected = (Matrix(2, 3) << 0., -92.376, 0., 0., 0., -92.376).finished(); +// +// // Verify the Jacobians are correct +// CHECK(assert_equal(H1Expected, H1Actual, 1e-3)); +// CHECK(assert_equal(H3Expected, H3Actual, 1e-3)); +// +// // Verify H2 with numerical derivative +// Matrix H2Expected = numericalDerivative32( +// std::function( +// std::bind(&TestProjectionFactor::evaluateError, &factor, +// std::placeholders::_1, std::placeholders::_2, +// std::placeholders::_3, boost::none, boost::none, +// boost::none)), +// pose, body_P_sensor, point); +// +// CHECK(assert_equal(H2Expected, H2Actual, 1e-5)); +// +// +//} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ + From 2812eeb1beed6d9ab7b6bdb33d342bc83dd3c2c3 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Tue, 20 Jul 2021 21:06:51 -0400 Subject: [PATCH 077/248] pass on projection factor, but looks great overall --- .../slam/ProjectionFactorRollingShutter.h | 356 ++++++++++-------- 1 file changed, 197 insertions(+), 159 deletions(-) diff --git a/gtsam_unstable/slam/ProjectionFactorRollingShutter.h b/gtsam_unstable/slam/ProjectionFactorRollingShutter.h index 499f1cec3..b5bead621 100644 --- a/gtsam_unstable/slam/ProjectionFactorRollingShutter.h +++ b/gtsam_unstable/slam/ProjectionFactorRollingShutter.h @@ -23,192 +23,230 @@ namespace gtsam { - /** - * Non-linear factor for a constraint derived from a 2D measurement. The calibration is known here. - * i.e. the main building block for visual SLAM. - * this version takes rolling shutter information into account like so: consider camera A (pose A) and camera B, and Point2 from camera A. - * camera A has timestamp t_A for the exposure time of its first row, and so does camera B t_B, Point2 has timestamp t_p according to the timestamp - * corresponding to the time of exposure of the row in the camera it belongs to. - * let us define the interp_param = (t_p - t_A) / (t_B - t_A), we will use the pose interpolated between A and B by the interp_param to project - * the corresponding landmark to Point2. - * @addtogroup SLAM - */ +/** + * Non-linear factor for 2D projection measurement obtained using a rolling shutter camera. The calibration is known here. + * This version takes rolling shutter information into account as follows: consider two consecutive poses A and B, + * and a Point2 measurement taken starting at time A using a rolling shutter camera. + * Pose A has timestamp t_A, and Pose B has timestamp t_B. The Point2 measurement has timestamp t_p (with t_A <= t_p <= t_B) + * corresponding to the time of exposure of the row of the image the pixel belongs to. + * Let us define the interp_param = (t_p - t_A) / (t_B - t_A), we will use the pose interpolated between A and B by + * the interp_param to project the corresponding landmark to Point2. + * @addtogroup SLAM + */ - class ProjectionFactorRollingShutter: public NoiseModelFactor3 { - protected: +class ProjectionFactorRollingShutter : public NoiseModelFactor3 { + protected: - // Keep a copy of measurement and calibration for I/O - Point2 measured_; ///< 2D measurement - double interp_param_; ///< interpolation parameter corresponding to the point2 measured - boost::shared_ptr K_; ///< shared pointer to calibration object - boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame + // Keep a copy of measurement and calibration for I/O + Point2 measured_; ///< 2D measurement + double interp_param_; ///< interpolation parameter in [0,1] corresponding to the point2 measurement + boost::shared_ptr K_; ///< shared pointer to calibration object + boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame - // verbosity handling for Cheirality Exceptions - bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false) - bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions (default: false) + // verbosity handling for Cheirality Exceptions + bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false) + bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions (default: false) - public: + public: - /// shorthand for base class type - typedef NoiseModelFactor3 Base; + /// shorthand for base class type + typedef NoiseModelFactor3 Base; - /// shorthand for this class - typedef ProjectionFactorRollingShutter This; + /// shorthand for this class + typedef ProjectionFactorRollingShutter This; - /// shorthand for a smart pointer to a factor - typedef boost::shared_ptr shared_ptr; + /// shorthand for a smart pointer to a factor + typedef boost::shared_ptr shared_ptr; - /// Default constructor - ProjectionFactorRollingShutter() : - measured_(0, 0), interp_param_(0), throwCheirality_(false), verboseCheirality_(false) { + /// Default constructor + ProjectionFactorRollingShutter() + : measured_(0, 0), + interp_param_(0), + throwCheirality_(false), + verboseCheirality_(false) { } - /** - * Constructor - * @param measured is the 2 dimensional location of point in image (the measurement) - * @param interp_param is the rolling shutter parameter for the measurement - * @param model is the standard deviation - * @param poseKey_a is the index of the first camera - * @param poseKey_b is the index of the second camera - * @param pointKey is the index of the landmark - * @param K shared pointer to the constant calibration - * @param body_P_sensor is the transform from body to sensor frame (default identity) - */ - ProjectionFactorRollingShutter(const Point2& measured, double interp_param, const SharedNoiseModel& model, - Key poseKey_a, Key poseKey_b, Key pointKey, const boost::shared_ptr& K, - boost::optional body_P_sensor = boost::none) : - Base(model, poseKey_a, poseKey_b, pointKey), measured_(measured), interp_param_(interp_param), K_(K), body_P_sensor_(body_P_sensor), - throwCheirality_(false), verboseCheirality_(false) {} + /** + * Constructor + * @param measured is the 2-dimensional pixel location of point in the image (the measurement) + * @param interp_param is the rolling shutter parameter for the measurement + * @param model is the noise model + * @param poseKey_a is the key of the first camera + * @param poseKey_b is the key of the second camera + * @param pointKey is the key of the landmark + * @param K shared pointer to the constant calibration + * @param body_P_sensor is the transform from body to sensor frame (default identity) + */ + ProjectionFactorRollingShutter(const Point2& measured, double interp_param, + const SharedNoiseModel& model, + Key poseKey_a, Key poseKey_b, Key pointKey, + const boost::shared_ptr& K, + boost::optional body_P_sensor = boost::none) + : Base(model, poseKey_a, poseKey_b, pointKey), + measured_(measured), + interp_param_(interp_param), + K_(K), + body_P_sensor_(body_P_sensor), + throwCheirality_(false), + verboseCheirality_(false) { + } - /** - * Constructor with exception-handling flags - * @param measured is the 2 dimensional location of point in image (the measurement) - * @param interp_param is the rolling shutter parameter for the measurement - * @param model is the standard deviation - * @param poseKey_a is the index of the first camera - * @param poseKey_b is the index of the second camera - * @param pointKey is the index of the landmark - * @param K shared pointer to the constant calibration - * @param throwCheirality determines whether Cheirality exceptions are rethrown - * @param verboseCheirality determines whether exceptions are printed for Cheirality - * @param body_P_sensor is the transform from body to sensor frame (default identity) - */ - ProjectionFactorRollingShutter(const Point2& measured, double interp_param, const SharedNoiseModel& model, - Key poseKey_a, Key poseKey_b, Key pointKey, const boost::shared_ptr& K, - bool throwCheirality, bool verboseCheirality, - boost::optional body_P_sensor = boost::none) : - Base(model, poseKey_a, poseKey_b, pointKey), measured_(measured), interp_param_(interp_param), K_(K), body_P_sensor_(body_P_sensor), - throwCheirality_(throwCheirality), verboseCheirality_(verboseCheirality) {} + /** + * Constructor with exception-handling flags + * @param measured is the 2-dimensional pixel location of point in the image (the measurement) + * @param interp_param is the rolling shutter parameter for the measurement + * @param model is the noise model + * @param poseKey_a is the key of the first camera + * @param poseKey_b is the key of the second camera + * @param pointKey is the key of the landmark + * @param K shared pointer to the constant calibration + * @param throwCheirality determines whether Cheirality exceptions are rethrown + * @param verboseCheirality determines whether exceptions are printed for Cheirality + * @param body_P_sensor is the transform from body to sensor frame (default identity) + */ + ProjectionFactorRollingShutter(const Point2& measured, double interp_param, + const SharedNoiseModel& model, + Key poseKey_a, Key poseKey_b, Key pointKey, + const boost::shared_ptr& K, + bool throwCheirality, bool verboseCheirality, + boost::optional body_P_sensor = boost::none) + : Base(model, poseKey_a, poseKey_b, pointKey), + measured_(measured), + interp_param_(interp_param), + K_(K), + body_P_sensor_(body_P_sensor), + throwCheirality_(throwCheirality), + verboseCheirality_(verboseCheirality) { + } - /** Virtual destructor */ - virtual ~ProjectionFactorRollingShutter() {} + /** Virtual destructor */ + virtual ~ProjectionFactorRollingShutter() { + } - /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { - return boost::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new This(*this))); } + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast < gtsam::NonlinearFactor + > (gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } - /** - * print - * @param s optional string naming the factor - * @param keyFormatter optional formatter useful for printing Symbols - */ - void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { - std::cout << s << "ProjectionFactorRollingShutter, z = "; - traits::Print(measured_); - std::cout << " rolling shutter interpolation param = " << interp_param_; - if(this->body_P_sensor_) - this->body_P_sensor_->print(" sensor pose in body frame: "); - Base::print("", keyFormatter); - } + /** + * print + * @param s optional string naming the factor + * @param keyFormatter optional formatter useful for printing Symbols + */ + void print(const std::string& s = "", const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const { + std::cout << s << "ProjectionFactorRollingShutter, z = "; + traits::Print(measured_); + std::cout << " rolling shutter interpolation param = " << interp_param_; + if (this->body_P_sensor_) + this->body_P_sensor_->print(" sensor pose in body frame: "); + Base::print("", keyFormatter); + } - /// equals - virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const { - const This *e = dynamic_cast(&p); - return e - && Base::equals(p, tol) - && (interp_param_ == e->interp_param()) - && traits::Equals(this->measured_, e->measured_, tol) - && this->K_->equals(*e->K_, tol) - && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_))); - } + /// equals + virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const { + const This *e = dynamic_cast(&p); + return e && Base::equals(p, tol) && (interp_param_ == e->interp_param()) + && traits::Equals(this->measured_, e->measured_, tol) + && this->K_->equals(*e->K_, tol) + && (this->throwCheirality_ == e->throwCheirality_) + && (this->verboseCheirality_ == e->verboseCheirality_) + && ((!body_P_sensor_ && !e->body_P_sensor_) + || (body_P_sensor_ && e->body_P_sensor_ + && body_P_sensor_->equals(*e->body_P_sensor_))); + } - /// Evaluate error h(x)-z and optionally derivatives - Vector evaluateError(const Pose3& pose_a, const Pose3& pose_b, const Point3& point, - boost::optional H1 = boost::none, boost::optional H2 = boost::none, boost::optional H3 = boost::none) const { + /// Evaluate error h(x)-z and optionally derivatives + Vector evaluateError(const Pose3& pose_a, const Pose3& pose_b, const Point3& point, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none, + boost::optional H3 = boost::none) const { - Pose3 pose; + try { + Pose3 pose = interpolate(pose_a, pose_b, interp_param_, H1, H2); gtsam::Matrix Hprj; - - //pose = interpolate(pose_a, pose_b, interp_param_, H1, H2); - pose = interpolate(pose_a, pose_b, interp_param_, H1, H2); - try { - if(body_P_sensor_) { - if(H1 && H2) { - gtsam::Matrix H0; - PinholeCamera camera(pose.compose(*body_P_sensor_, H0), *K_); - Point2 reprojectionError(camera.project(point, Hprj, H3, boost::none) - measured_); - *H1 = Hprj * H0 * (*H1); - *H2 = Hprj * H0 * (*H2); - return reprojectionError; - } else { - PinholeCamera camera(pose.compose(*body_P_sensor_), *K_); - return camera.project(point, Hprj, H3, boost::none) - measured_; - } - } else { - PinholeCamera camera(pose, *K_); + if (body_P_sensor_) { + if (H1 || H2 || H3) { + gtsam::Matrix HbodySensor; + PinholeCamera camera(pose.compose(*body_P_sensor_, HbodySensor), *K_); Point2 reprojectionError(camera.project(point, Hprj, H3, boost::none) - measured_); - if (H1) *H1 = Hprj * (*H1); - if (H2) *H2 = Hprj * (*H2); + if (H1) + *H1 = Hprj * HbodySensor * (*H1); + if (H2) + *H2 = Hprj * HbodySensor * (*H2); return reprojectionError; + } else { + PinholeCamera camera(pose.compose(*body_P_sensor_), *K_); + return camera.project(point) - measured_; } - } catch( CheiralityException& e) { - if (H1) *H1 = Matrix::Zero(2,6); - if (H2) *H2 = Matrix::Zero(2,6); - if (H3) *H3 = Matrix::Zero(2,3); - if (verboseCheirality_) - std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) << - " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl; - if (throwCheirality_) - throw CheiralityException(this->key2()); + } else { + PinholeCamera camera(pose, *K_); + Point2 reprojectionError(camera.project(point, Hprj, H3, boost::none) - measured_); + if (H1) + *H1 = Hprj * (*H1); + if (H2) + *H2 = Hprj * (*H2); + return reprojectionError; } - return Vector2::Constant(2.0 * K_->fx()); + } catch (CheiralityException& e) { + if (H1) + *H1 = Matrix::Zero(2, 6); + if (H2) + *H2 = Matrix::Zero(2, 6); + if (H3) + *H3 = Matrix::Zero(2, 3); + if (verboseCheirality_) + std::cout << e.what() << ": Landmark " + << DefaultKeyFormatter(this->key2()) << " moved behind camera " + << DefaultKeyFormatter(this->key1()) << std::endl; + if (throwCheirality_) + throw CheiralityException(this->key2()); } + return Vector2::Constant(2.0 * K_->fx()); + } - /** return the measurement */ - const Point2& measured() const { - return measured_; - } + /** return the measurement */ + const Point2& measured() const { + return measured_; + } - /** return the calibration object */ - inline const boost::shared_ptr calibration() const { - return K_; - } + /** return the calibration object */ + inline const boost::shared_ptr calibration() const { + return K_; + } - /** returns the rolling shutter interp param*/ - inline double interp_param() const {return interp_param_; } + /** returns the rolling shutter interp param*/ + inline double interp_param() const { + return interp_param_; + } - /** return verbosity */ - inline bool verboseCheirality() const { return verboseCheirality_; } + /** return verbosity */ + inline bool verboseCheirality() const { + return verboseCheirality_; + } - /** return flag for throwing cheirality exceptions */ - inline bool throwCheirality() const { return throwCheirality_; } + /** return flag for throwing cheirality exceptions */ + inline bool throwCheirality() const { + return throwCheirality_; + } - private: + private: - /// Serialization function - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - ar & BOOST_SERIALIZATION_NVP(measured_); - ar & BOOST_SERIALIZATION_NVP(interp_param_); - ar & BOOST_SERIALIZATION_NVP(K_); - ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); - ar & BOOST_SERIALIZATION_NVP(throwCheirality_); - ar & BOOST_SERIALIZATION_NVP(verboseCheirality_); - } - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - }; // rolling shutter projection factor -} //namespace gtsam + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar & BOOST_SERIALIZATION_NVP(measured_); + ar & BOOST_SERIALIZATION_NVP(interp_param_); + ar & BOOST_SERIALIZATION_NVP(K_); + ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); + ar & BOOST_SERIALIZATION_NVP(throwCheirality_); + ar & BOOST_SERIALIZATION_NVP(verboseCheirality_); + } + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; +// rolling shutter projection factor +}//namespace gtsam From 0d1c3f16efb44a07073107a8b9d11e74c1b8145b Mon Sep 17 00:00:00 2001 From: lcarlone Date: Tue, 20 Jul 2021 21:29:56 -0400 Subject: [PATCH 078/248] everything working out so far with the tests --- .../slam/ProjectionFactorRollingShutter.h | 5 +- .../testProjectionFactorRollingShutter.cpp | 141 +++++++++--------- 2 files changed, 77 insertions(+), 69 deletions(-) diff --git a/gtsam_unstable/slam/ProjectionFactorRollingShutter.h b/gtsam_unstable/slam/ProjectionFactorRollingShutter.h index b5bead621..1d06e01a7 100644 --- a/gtsam_unstable/slam/ProjectionFactorRollingShutter.h +++ b/gtsam_unstable/slam/ProjectionFactorRollingShutter.h @@ -248,5 +248,8 @@ class ProjectionFactorRollingShutter : public NoiseModelFactor3 struct traits : public Testable {}; + }//namespace gtsam diff --git a/gtsam_unstable/slam/tests/testProjectionFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testProjectionFactorRollingShutter.cpp index b0fcb23b1..97e9dd2d1 100644 --- a/gtsam_unstable/slam/tests/testProjectionFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testProjectionFactorRollingShutter.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file testProjectionFactorRollingShutter.cpp + * @file ProjectionFactorRollingShutterRollingShutter.cpp * @brief Unit tests for ProjectionFactorRollingShutter Class * @author Luca Carlone * @date July 2021 @@ -45,72 +45,77 @@ using symbol_shorthand::X; using symbol_shorthand::L; using symbol_shorthand::T; -//typedef ProjectionFactorPPP TestProjectionFactor; +// Convenience to define common variables across many tests +static Key poseKey1(X(1)); +static Key poseKey2(X(2)); +static Key pointKey(L(1)); +static double interp_params = 0.5; +static Point2 measurement(323.0, 240.0); +static Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); + +/* ************************************************************************* */ +TEST( ProjectionFactorRollingShutter, Constructor) { + ProjectionFactorRollingShutter factor(measurement, interp_params, model, poseKey1, poseKey2, pointKey, K); +} + +/* ************************************************************************* */ +TEST( ProjectionFactorRollingShutter, ConstructorWithTransform) { + ProjectionFactorRollingShutter factor(measurement, interp_params, model, + poseKey1, poseKey2, pointKey, K, body_P_sensor); +} + +/* ************************************************************************* */ +TEST( ProjectionFactorRollingShutter, Equals ) { + { // factors are equal + ProjectionFactorRollingShutter factor1(measurement, interp_params, + model, poseKey1, poseKey2, pointKey, K); + ProjectionFactorRollingShutter factor2(measurement, interp_params, + model, poseKey1, poseKey2, pointKey, K); + CHECK(assert_equal(factor1, factor2)); + } + { // factors are NOT equal (keys are different) + ProjectionFactorRollingShutter factor1(measurement, interp_params, + model, poseKey1, poseKey2, pointKey, K); + ProjectionFactorRollingShutter factor2(measurement, interp_params, + model, poseKey1, poseKey1, pointKey, K); + CHECK(!assert_equal(factor1, factor2)); // not equal + } + { // factors are NOT equal (different interpolation) + ProjectionFactorRollingShutter factor1(measurement, 0.1, + model, poseKey1, poseKey1, pointKey, K); + ProjectionFactorRollingShutter factor2(measurement, 0.5, + model, poseKey1, poseKey2, pointKey, K); + CHECK(!assert_equal(factor1, factor2)); // not equal + } +} + +/* ************************************************************************* */ +TEST( ProjectionFactorRollingShutter, EqualsWithTransform ) { + { // factors are equal + ProjectionFactorRollingShutter factor1(measurement, interp_params, model, + poseKey1, poseKey2, pointKey, K, body_P_sensor); + ProjectionFactorRollingShutter factor2(measurement, interp_params, model, + poseKey1, poseKey2, pointKey, K, body_P_sensor); + CHECK(assert_equal(factor1, factor2)); + } + { // factors are NOT equal + ProjectionFactorRollingShutter factor1(measurement, interp_params, model, + poseKey1, poseKey2, pointKey, K, body_P_sensor); + Pose3 body_P_sensor2(Rot3::RzRyRx(0.0, 0.0, 0.0), Point3(0.25, -0.10, 1.0)); // rotation different from body_P_sensor + ProjectionFactorRollingShutter factor2(measurement, interp_params, model, + poseKey1, poseKey2, pointKey, K, body_P_sensor2); + CHECK(!assert_equal(factor1, factor2)); + } +} -///// traits -//namespace gtsam { -//template<> -//struct traits : public Testable { -//}; -//} -// ///* ************************************************************************* */ -//TEST( ProjectionFactorPPP, nonStandard ) { -// ProjectionFactorPPP f; -//} -// -///* ************************************************************************* */ -//TEST( ProjectionFactorPPP, Constructor) { -// Key poseKey(X(1)); -// Key transformKey(T(1)); -// Key pointKey(L(1)); -// -// Point2 measurement(323.0, 240.0); -// -// TestProjectionFactor factor(measurement, model, poseKey, transformKey, pointKey, K); -//} -// -///* ************************************************************************* */ -//TEST( ProjectionFactorPPP, ConstructorWithTransform) { -// Key poseKey(X(1)); -// Key transformKey(T(1)); -// Key pointKey(L(1)); -// -// Point2 measurement(323.0, 240.0); -// TestProjectionFactor factor(measurement, model, poseKey, transformKey, pointKey, K); -//} -// -///* ************************************************************************* */ -//TEST( ProjectionFactorPPP, Equals ) { -// // Create two identical factors and make sure they're equal -// Point2 measurement(323.0, 240.0); -// -// TestProjectionFactor factor1(measurement, model, X(1), T(1), L(1), K); -// TestProjectionFactor factor2(measurement, model, X(1), T(1), L(1), K); -// -// CHECK(assert_equal(factor1, factor2)); -//} -// -///* ************************************************************************* */ -//TEST( ProjectionFactorPPP, EqualsWithTransform ) { -// // Create two identical factors and make sure they're equal -// Point2 measurement(323.0, 240.0); -// Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); -// -// TestProjectionFactor factor1(measurement, model, X(1), T(1), L(1), K); -// TestProjectionFactor factor2(measurement, model, X(1), T(1), L(1), K); -// -// CHECK(assert_equal(factor1, factor2)); -//} -// -///* ************************************************************************* */ -//TEST( ProjectionFactorPPP, Error ) { +//TEST( ProjectionFactorRollingShutter, Error ) { // // Create the factor with a measurement that is 3 pixels off in x // Key poseKey(X(1)); // Key transformKey(T(1)); // Key pointKey(L(1)); // Point2 measurement(323.0, 240.0); -// TestProjectionFactor factor(measurement, model, poseKey, transformKey, pointKey, K); +// ProjectionFactorRollingShutter factor(measurement, model, poseKey, transformKey, pointKey, K); // // // Set the linearization point // Pose3 pose(Rot3(), Point3(0,0,-6)); @@ -127,14 +132,14 @@ using symbol_shorthand::T; //} // ///* ************************************************************************* */ -//TEST( ProjectionFactorPPP, ErrorWithTransform ) { +//TEST( ProjectionFactorRollingShutter, ErrorWithTransform ) { // // Create the factor with a measurement that is 3 pixels off in x // Key poseKey(X(1)); // Key transformKey(T(1)); // Key pointKey(L(1)); // Point2 measurement(323.0, 240.0); // Pose3 transform(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); -// TestProjectionFactor factor(measurement, model, poseKey,transformKey, pointKey, K); +// ProjectionFactorRollingShutter factor(measurement, model, poseKey,transformKey, pointKey, K); // // // Set the linearization point. The vehicle pose has been selected to put the camera at (-6, 0, 0) // Pose3 pose(Rot3(), Point3(-6.25, 0.10 , -1.0)); @@ -151,13 +156,13 @@ using symbol_shorthand::T; //} // ///* ************************************************************************* */ -//TEST( ProjectionFactorPPP, Jacobian ) { +//TEST( ProjectionFactorRollingShutter, Jacobian ) { // // Create the factor with a measurement that is 3 pixels off in x // Key poseKey(X(1)); // Key transformKey(T(1)); // Key pointKey(L(1)); // Point2 measurement(323.0, 240.0); -// TestProjectionFactor factor(measurement, model, poseKey, transformKey, pointKey, K); +// ProjectionFactorRollingShutter factor(measurement, model, poseKey, transformKey, pointKey, K); // // // Set the linearization point // Pose3 pose(Rot3(), Point3(0,0,-6)); @@ -178,7 +183,7 @@ using symbol_shorthand::T; // // Verify H2 with numerical derivative // Matrix H2Expected = numericalDerivative32( // std::function( -// std::bind(&TestProjectionFactor::evaluateError, &factor, +// std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, // std::placeholders::_1, std::placeholders::_2, // std::placeholders::_3, boost::none, boost::none, // boost::none)), @@ -188,14 +193,14 @@ using symbol_shorthand::T; //} // ///* ************************************************************************* */ -//TEST( ProjectionFactorPPP, JacobianWithTransform ) { +//TEST( ProjectionFactorRollingShutter, JacobianWithTransform ) { // // Create the factor with a measurement that is 3 pixels off in x // Key poseKey(X(1)); // Key transformKey(T(1)); // Key pointKey(L(1)); // Point2 measurement(323.0, 240.0); // Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); -// TestProjectionFactor factor(measurement, model, poseKey, transformKey, pointKey, K); +// ProjectionFactorRollingShutter factor(measurement, model, poseKey, transformKey, pointKey, K); // // // Set the linearization point. The vehicle pose has been selected to put the camera at (-6, 0, 0) // Pose3 pose(Rot3(), Point3(-6.25, 0.10 , -1.0)); @@ -216,7 +221,7 @@ using symbol_shorthand::T; // // Verify H2 with numerical derivative // Matrix H2Expected = numericalDerivative32( // std::function( -// std::bind(&TestProjectionFactor::evaluateError, &factor, +// std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, // std::placeholders::_1, std::placeholders::_2, // std::placeholders::_3, boost::none, boost::none, // boost::none)), From a480b2dcfc195731c91923d88f5768dcad89b7c7 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Tue, 20 Jul 2021 22:05:36 -0400 Subject: [PATCH 079/248] all tests are passing! --- .../testProjectionFactorRollingShutter.cpp | 312 +++++++++++------- 1 file changed, 185 insertions(+), 127 deletions(-) diff --git a/gtsam_unstable/slam/tests/testProjectionFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testProjectionFactorRollingShutter.cpp index 97e9dd2d1..66ed1cf9c 100644 --- a/gtsam_unstable/slam/tests/testProjectionFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testProjectionFactorRollingShutter.cpp @@ -93,144 +93,202 @@ TEST( ProjectionFactorRollingShutter, Equals ) { TEST( ProjectionFactorRollingShutter, EqualsWithTransform ) { { // factors are equal ProjectionFactorRollingShutter factor1(measurement, interp_params, model, - poseKey1, poseKey2, pointKey, K, body_P_sensor); + poseKey1, poseKey2, pointKey, K, body_P_sensor); ProjectionFactorRollingShutter factor2(measurement, interp_params, model, - poseKey1, poseKey2, pointKey, K, body_P_sensor); + poseKey1, poseKey2, pointKey, K, body_P_sensor); CHECK(assert_equal(factor1, factor2)); } { // factors are NOT equal ProjectionFactorRollingShutter factor1(measurement, interp_params, model, - poseKey1, poseKey2, pointKey, K, body_P_sensor); + poseKey1, poseKey2, pointKey, K, body_P_sensor); Pose3 body_P_sensor2(Rot3::RzRyRx(0.0, 0.0, 0.0), Point3(0.25, -0.10, 1.0)); // rotation different from body_P_sensor ProjectionFactorRollingShutter factor2(measurement, interp_params, model, - poseKey1, poseKey2, pointKey, K, body_P_sensor2); + poseKey1, poseKey2, pointKey, K, body_P_sensor2); CHECK(!assert_equal(factor1, factor2)); } } -///* ************************************************************************* */ -//TEST( ProjectionFactorRollingShutter, Error ) { -// // Create the factor with a measurement that is 3 pixels off in x -// Key poseKey(X(1)); -// Key transformKey(T(1)); -// Key pointKey(L(1)); -// Point2 measurement(323.0, 240.0); -// ProjectionFactorRollingShutter factor(measurement, model, poseKey, transformKey, pointKey, K); -// -// // Set the linearization point -// Pose3 pose(Rot3(), Point3(0,0,-6)); -// Point3 point(0.0, 0.0, 0.0); -// -// // Use the factor to calculate the error -// Vector actualError(factor.evaluateError(pose, Pose3(), point)); -// -// // The expected error is (-3.0, 0.0) pixels / UnitCovariance -// Vector expectedError = Vector2(-3.0, 0.0); -// -// // Verify we get the expected error -// CHECK(assert_equal(expectedError, actualError, 1e-9)); -//} -// -///* ************************************************************************* */ -//TEST( ProjectionFactorRollingShutter, ErrorWithTransform ) { -// // Create the factor with a measurement that is 3 pixels off in x -// Key poseKey(X(1)); -// Key transformKey(T(1)); -// Key pointKey(L(1)); -// Point2 measurement(323.0, 240.0); -// Pose3 transform(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); -// ProjectionFactorRollingShutter factor(measurement, model, poseKey,transformKey, pointKey, K); -// -// // Set the linearization point. The vehicle pose has been selected to put the camera at (-6, 0, 0) -// Pose3 pose(Rot3(), Point3(-6.25, 0.10 , -1.0)); -// Point3 point(0.0, 0.0, 0.0); -// -// // Use the factor to calculate the error -// Vector actualError(factor.evaluateError(pose, transform, point)); -// -// // The expected error is (-3.0, 0.0) pixels / UnitCovariance -// Vector expectedError = Vector2(-3.0, 0.0); -// -// // Verify we get the expected error -// CHECK(assert_equal(expectedError, actualError, 1e-9)); -//} -// -///* ************************************************************************* */ -//TEST( ProjectionFactorRollingShutter, Jacobian ) { -// // Create the factor with a measurement that is 3 pixels off in x -// Key poseKey(X(1)); -// Key transformKey(T(1)); -// Key pointKey(L(1)); -// Point2 measurement(323.0, 240.0); -// ProjectionFactorRollingShutter factor(measurement, model, poseKey, transformKey, pointKey, K); -// -// // Set the linearization point -// Pose3 pose(Rot3(), Point3(0,0,-6)); -// Point3 point(0.0, 0.0, 0.0); -// -// // Use the factor to calculate the Jacobians -// Matrix H1Actual, H2Actual, H3Actual; -// factor.evaluateError(pose, Pose3(), point, H1Actual, H2Actual, H3Actual); -// -// // The expected Jacobians -// Matrix H1Expected = (Matrix(2, 6) << 0., -554.256, 0., -92.376, 0., 0., 554.256, 0., 0., 0., -92.376, 0.).finished(); -// Matrix H3Expected = (Matrix(2, 3) << 92.376, 0., 0., 0., 92.376, 0.).finished(); -// -// // Verify the Jacobians are correct -// CHECK(assert_equal(H1Expected, H1Actual, 1e-3)); -// CHECK(assert_equal(H3Expected, H3Actual, 1e-3)); -// -// // Verify H2 with numerical derivative -// Matrix H2Expected = numericalDerivative32( -// std::function( -// std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, -// std::placeholders::_1, std::placeholders::_2, -// std::placeholders::_3, boost::none, boost::none, -// boost::none)), -// pose, Pose3(), point); -// -// CHECK(assert_equal(H2Expected, H2Actual, 1e-5)); -//} -// -///* ************************************************************************* */ -//TEST( ProjectionFactorRollingShutter, JacobianWithTransform ) { -// // Create the factor with a measurement that is 3 pixels off in x -// Key poseKey(X(1)); -// Key transformKey(T(1)); -// Key pointKey(L(1)); -// Point2 measurement(323.0, 240.0); -// Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); -// ProjectionFactorRollingShutter factor(measurement, model, poseKey, transformKey, pointKey, K); -// -// // Set the linearization point. The vehicle pose has been selected to put the camera at (-6, 0, 0) -// Pose3 pose(Rot3(), Point3(-6.25, 0.10 , -1.0)); -// Point3 point(0.0, 0.0, 0.0); -// -// // Use the factor to calculate the Jacobians -// Matrix H1Actual, H2Actual, H3Actual; -// factor.evaluateError(pose, body_P_sensor, point, H1Actual, H2Actual, H3Actual); -// -// // The expected Jacobians -// Matrix H1Expected = (Matrix(2, 6) << -92.376, 0., 577.350, 0., 92.376, 0., -9.2376, -577.350, 0., 0., 0., 92.376).finished(); -// Matrix H3Expected = (Matrix(2, 3) << 0., -92.376, 0., 0., 0., -92.376).finished(); -// -// // Verify the Jacobians are correct -// CHECK(assert_equal(H1Expected, H1Actual, 1e-3)); -// CHECK(assert_equal(H3Expected, H3Actual, 1e-3)); -// -// // Verify H2 with numerical derivative -// Matrix H2Expected = numericalDerivative32( -// std::function( -// std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, -// std::placeholders::_1, std::placeholders::_2, -// std::placeholders::_3, boost::none, boost::none, -// boost::none)), -// pose, body_P_sensor, point); -// -// CHECK(assert_equal(H2Expected, H2Actual, 1e-5)); -// -// -//} +/* ************************************************************************* */ +TEST( ProjectionFactorRollingShutter, Error ) { + { + // Create the factor with a measurement that is 3 pixels off in x + // Camera pose corresponds to the first camera + double t = 0.0; + ProjectionFactorRollingShutter factor(measurement, t, model, poseKey1, poseKey2, pointKey, K); + + // Set the linearization point + Pose3 pose1(Rot3(), Point3(0,0,-6)); + Pose3 pose2(Rot3(), Point3(0,0,-4)); + Point3 point(0.0, 0.0, 0.0); + + // Use the factor to calculate the error + Vector actualError(factor.evaluateError(pose1, pose2, point)); + + // The expected error is (-3.0, 0.0) pixels / UnitCovariance + Vector expectedError = Vector2(-3.0, 0.0); + + // Verify we get the expected error + CHECK(assert_equal(expectedError, actualError, 1e-9)); + } + { + // Create the factor with a measurement that is 3 pixels off in x + // Camera pose is actually interpolated now + double t = 0.5; + ProjectionFactorRollingShutter factor(measurement, t, model, poseKey1, poseKey2, pointKey, K); + + // Set the linearization point + Pose3 pose1(Rot3(), Point3(0,0,-8)); + Pose3 pose2(Rot3(), Point3(0,0,-4)); + Point3 point(0.0, 0.0, 0.0); + + // Use the factor to calculate the error + Vector actualError(factor.evaluateError(pose1, pose2, point)); + + // The expected error is (-3.0, 0.0) pixels / UnitCovariance + Vector expectedError = Vector2(-3.0, 0.0); + + // Verify we get the expected error + CHECK(assert_equal(expectedError, actualError, 1e-9)); + } + { + // Create measurement by projecting 3D landmark + double t = 0.3; + Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0,0,0)); + Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0,0,1)); + Pose3 poseInterp = interpolate(pose1, pose2, t); + PinholeCamera camera(poseInterp, *K); + Point3 point(0.0, 0.0, 5.0); // 5 meters in front of the camera + Point2 measured = camera.project(point); + + // create factor + ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2, pointKey, K); + + // Use the factor to calculate the error + Vector actualError(factor.evaluateError(pose1, pose2, point)); + + // The expected error is zero + Vector expectedError = Vector2(0.0, 0.0); + + // Verify we get the expected error + CHECK(assert_equal(expectedError, actualError, 1e-9)); + } +} + +/* ************************************************************************* */ +TEST( ProjectionFactorRollingShutter, ErrorWithTransform ) { + // Create measurement by projecting 3D landmark + double t = 0.3; + Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0,0,0)); + Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0,0,1)); + Pose3 poseInterp = interpolate(pose1, pose2, t); + Pose3 body_P_sensor3(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0,0.2,0.1)); + PinholeCamera camera(poseInterp*body_P_sensor3, *K); + Point3 point(0.0, 0.0, 5.0); // 5 meters in front of the camera + Point2 measured = camera.project(point); + + // create factor + ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2, pointKey, K, body_P_sensor3); + + // Use the factor to calculate the error + Vector actualError(factor.evaluateError(pose1, pose2, point)); + + // The expected error is zero + Vector expectedError = Vector2(0.0, 0.0); + + // Verify we get the expected error + CHECK(assert_equal(expectedError, actualError, 1e-9)); +} + +/* ************************************************************************* */ +TEST( ProjectionFactorRollingShutter, Jacobian ) { + // Create measurement by projecting 3D landmark + double t = 0.3; + Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0,0,0)); + Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0,0,1)); + Pose3 poseInterp = interpolate(pose1, pose2, t); + PinholeCamera camera(poseInterp, *K); + Point3 point(0.0, 0.0, 5.0); // 5 meters in front of the camera + Point2 measured = camera.project(point); + + // create factor + ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2, pointKey, K); + + // Use the factor to calculate the Jacobians + Matrix H1Actual, H2Actual, H3Actual; + factor.evaluateError(pose1, pose2, point, H1Actual, H2Actual, H3Actual); + + // Expected Jacobians via numerical derivatives + Matrix H1Expected = numericalDerivative31( + std::function( + std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, + std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, boost::none, boost::none, boost::none)), + pose1, pose2, point); + + Matrix H2Expected = numericalDerivative32( + std::function( + std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, + std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, boost::none, boost::none, boost::none)), + pose1, pose2, point); + + Matrix H3Expected = numericalDerivative33( + std::function( + std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, + std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, boost::none, boost::none, boost::none)), + pose1, pose2, point); + + CHECK(assert_equal(H1Expected, H1Actual, 1e-5)); + CHECK(assert_equal(H2Expected, H2Actual, 1e-5)); + CHECK(assert_equal(H3Expected, H3Actual, 1e-5)); +} + +/* ************************************************************************* */ +TEST( ProjectionFactorRollingShutter, JacobianWithTransform ) { + // Create measurement by projecting 3D landmark + double t = 0.6; + Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0,0,0)); + Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0,0,1)); + Pose3 poseInterp = interpolate(pose1, pose2, t); + Pose3 body_P_sensor3(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0,0.2,0.1)); + PinholeCamera camera(poseInterp*body_P_sensor3, *K); + Point3 point(0.0, 0.0, 5.0); // 5 meters in front of the camera + Point2 measured = camera.project(point); + + // create factor + ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2, pointKey, K, body_P_sensor3); + + // Use the factor to calculate the Jacobians + Matrix H1Actual, H2Actual, H3Actual; + factor.evaluateError(pose1, pose2, point, H1Actual, H2Actual, H3Actual); + + // Expected Jacobians via numerical derivatives + Matrix H1Expected = numericalDerivative31( + std::function( + std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, + std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, boost::none, boost::none, boost::none)), + pose1, pose2, point); + + Matrix H2Expected = numericalDerivative32( + std::function( + std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, + std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, boost::none, boost::none, boost::none)), + pose1, pose2, point); + + Matrix H3Expected = numericalDerivative33( + std::function( + std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, + std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, boost::none, boost::none, boost::none)), + pose1, pose2, point); + + CHECK(assert_equal(H1Expected, H1Actual, 1e-5)); + CHECK(assert_equal(H2Expected, H2Actual, 1e-5)); + CHECK(assert_equal(H3Expected, H3Actual, 1e-5)); +} /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } From 02d2d97a8ef404d67207d14b69697a62b627bd0e Mon Sep 17 00:00:00 2001 From: lcarlone Date: Tue, 20 Jul 2021 23:04:52 -0400 Subject: [PATCH 080/248] added nice test on cheirality exception - done with projectionFactorRollingShutter --- .../slam/ProjectionFactorRollingShutter.h | 6 +- .../testProjectionFactorRollingShutter.cpp | 79 +++++++++++++++++++ 2 files changed, 83 insertions(+), 2 deletions(-) diff --git a/gtsam_unstable/slam/ProjectionFactorRollingShutter.h b/gtsam_unstable/slam/ProjectionFactorRollingShutter.h index 1d06e01a7..fc7c939a8 100644 --- a/gtsam_unstable/slam/ProjectionFactorRollingShutter.h +++ b/gtsam_unstable/slam/ProjectionFactorRollingShutter.h @@ -18,7 +18,9 @@ #pragma once #include -#include +#include +#include +#include #include namespace gtsam { @@ -189,7 +191,7 @@ class ProjectionFactorRollingShutter : public NoiseModelFactor3(pose1, pose2, t); + PinholeCamera camera(poseInterp, *K); + Point3 point(0.0, 0.0, -5.0); // 5 meters behind the camera + +#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION + Point2 measured = Point2(0.0,0.0); // project would throw an exception + { // check that exception is thrown if we set throwCheirality = true + bool throwCheirality = true; + bool verboseCheirality = true; + ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2, pointKey, K, throwCheirality, verboseCheirality); + CHECK_EXCEPTION(factor.evaluateError(pose1, pose2, point), + CheiralityException); + } + { // check that exception is NOT thrown if we set throwCheirality = false, and outputs are correct + bool throwCheirality = false; // default + bool verboseCheirality = false; // default + ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2, pointKey, K, throwCheirality, verboseCheirality); + + // Use the factor to calculate the error + Matrix H1Actual, H2Actual, H3Actual; + Vector actualError(factor.evaluateError(pose1, pose2, point, H1Actual, H2Actual, H3Actual)); + + // The expected error is zero + Vector expectedError = Vector2::Constant(2.0 * K->fx()); // this is what we return when point is behind camera + + // Verify we get the expected error + CHECK(assert_equal(expectedError, actualError, 1e-9)); + CHECK(assert_equal(Matrix::Zero(2,6), H1Actual, 1e-5)); + CHECK(assert_equal(Matrix::Zero(2,6), H2Actual, 1e-5)); + CHECK(assert_equal(Matrix::Zero(2,3), H3Actual, 1e-5)); + } +#else + { + // everything is well defined, hence this matches the test "Jacobian" above: + Point2 measured = camera.project(point); + + // create factor + ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2, pointKey, K); + + // Use the factor to calculate the Jacobians + Matrix H1Actual, H2Actual, H3Actual; + factor.evaluateError(pose1, pose2, point, H1Actual, H2Actual, H3Actual); + + // Expected Jacobians via numerical derivatives + Matrix H1Expected = numericalDerivative31( + std::function( + std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, + std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, boost::none, boost::none, boost::none)), + pose1, pose2, point); + + Matrix H2Expected = numericalDerivative32( + std::function( + std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, + std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, boost::none, boost::none, boost::none)), + pose1, pose2, point); + + Matrix H3Expected = numericalDerivative33( + std::function( + std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, + std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, boost::none, boost::none, boost::none)), + pose1, pose2, point); + + CHECK(assert_equal(H1Expected, H1Actual, 1e-5)); + CHECK(assert_equal(H2Expected, H2Actual, 1e-5)); + CHECK(assert_equal(H3Expected, H3Actual, 1e-5)); + } +#endif +} + + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ From 15c29cacd2c5d796b62b530bdfb9c8b4d6c73e6d Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Wed, 21 Jul 2021 05:14:58 +0000 Subject: [PATCH 081/248] wrapping triangulate nonlinear --- gtsam/gtsam.i | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 94d10953b..155275702 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -1145,7 +1145,20 @@ gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3_S2& cameras, gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Bundler& cameras, const gtsam::Point2Vector& measurements, double rank_tol, bool optimize); - +gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses, + gtsam::Cal3_S2* sharedCal, const gtsam::Point2Vector& measurements, + const Point3& initialEstimate); +gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses, + gtsam::Cal3DS2* sharedCal, const gtsam::Point2Vector& measurements, + const Point3& initialEstimate); +gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses, + gtsam::Cal3Bundler* sharedCal, const gtsam::Point2Vector& measurements, + const Point3& initialEstimate); +gtsam::Point3 triangulateNonlinear(const CameraSetCal3_S2& cameras, + const gtsam::Point2Vector& measurements, const Point3& initialEstimate); +gtsam::Point3 triangulateNonlinear(const CameraSetCal3Bundler& cameras, + const gtsam::Point2Vector& measurements, const Point3& initialEstimate); + //************************************************************************* // Symbolic //************************************************************************* From 5fee983ff102bda87833af53c10ba7e972eca583 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Wed, 21 Jul 2021 10:04:05 -0600 Subject: [PATCH 082/248] use upper 3x3 sub-block of covariance matrix for converting BetweenFactor to BinaryMeasurement, and use Isotropic in ShonanAveraging2 --- gtsam/sfm/ShonanAveraging.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp index 76fd1bfc7..a5b6a8d9a 100644 --- a/gtsam/sfm/ShonanAveraging.cpp +++ b/gtsam/sfm/ShonanAveraging.cpp @@ -955,7 +955,7 @@ static BinaryMeasurement convertPose2ToBinaryMeasurementRot2( "parseMeasurements can only convert Pose2 measurements " "with Gaussian noise models."); const Matrix3 M = gaussian->covariance(); - auto model = noiseModel::Gaussian::Covariance(M.block<1, 1>(2, 2)); + auto model = noiseModel::Isotropic::Variance(1, M(2, 2)); return BinaryMeasurement(f->key1(), f->key2(), f->measured().rotation(), model); } @@ -1001,7 +1001,7 @@ static BinaryMeasurement convert( "parseMeasurements can only convert Pose3 measurements " "with Gaussian noise models."); const Matrix6 M = gaussian->covariance(); - auto model = noiseModel::Gaussian::Covariance(M.block<3, 3>(3, 3)); + auto model = noiseModel::Gaussian::Covariance(M.block<3, 3>(0, 0)); return BinaryMeasurement(f->key1(), f->key2(), f->measured().rotation(), model); } From 28ecc3331bda0bf49f94fb52630efe77926a5024 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Wed, 21 Jul 2021 10:27:31 -0600 Subject: [PATCH 083/248] add comments about tangent space and covariance matrix ordering --- gtsam/sfm/ShonanAveraging.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp index a5b6a8d9a..58e98ebfa 100644 --- a/gtsam/sfm/ShonanAveraging.cpp +++ b/gtsam/sfm/ShonanAveraging.cpp @@ -955,6 +955,8 @@ static BinaryMeasurement convertPose2ToBinaryMeasurementRot2( "parseMeasurements can only convert Pose2 measurements " "with Gaussian noise models."); const Matrix3 M = gaussian->covariance(); + // the (2,2) entry of Pose2's covariance corresponds to Rot2's covariance + // because the tangent space of Pose2 is ordered as (vx, vy, w) auto model = noiseModel::Isotropic::Variance(1, M(2, 2)); return BinaryMeasurement(f->key1(), f->key2(), f->measured().rotation(), model); @@ -1001,6 +1003,8 @@ static BinaryMeasurement convert( "parseMeasurements can only convert Pose3 measurements " "with Gaussian noise models."); const Matrix6 M = gaussian->covariance(); + // the upper-left 3x3 sub-block of Pose3's covariance corresponds to Rot3's covariance + // because the tangent space of Pose3 is ordered as (w,T) where w and T are both Vector3's auto model = noiseModel::Gaussian::Covariance(M.block<3, 3>(0, 0)); return BinaryMeasurement(f->key1(), f->key2(), f->measured().rotation(), model); From 30f304e7332222c8396e0deb6823978c07d7d1dd Mon Sep 17 00:00:00 2001 From: lcarlone Date: Wed, 21 Jul 2021 13:58:47 -0400 Subject: [PATCH 084/248] started serious testing: all tests pass for now --- gtsam/base/Lie.h | 2 +- .../tests/testSmartProjectionPoseFactor.cpp | 2 +- .../SmartProjectionPoseFactorRollingShutter.h | 85 ++++++++++++------- ...martProjectionPoseFactorRollingShutter.cpp | 53 ++++++------ 4 files changed, 84 insertions(+), 58 deletions(-) diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index 250ee6bcf..c93456a28 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -17,7 +17,7 @@ * @author Frank Dellaert * @author Mike Bosse * @author Duy Nguyen Ta - * @author shteren1 + * @author Yotam Stern */ diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index c7f220c10..997c33846 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -50,7 +50,7 @@ static Point2 measurement1(323.0, 240.0); LevenbergMarquardtParams lmParams; // Make more verbose like so (in tests): -// params.verbosityLM = LevenbergMarquardtParams::SUMMARY; +// lmParams.verbosityLM = LevenbergMarquardtParams::SUMMARY; /* ************************************************************************* */ TEST( SmartProjectionPoseFactor, Constructor) { diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index f1f0e2d71..13ba4e85e 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -167,12 +167,17 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor< return K_all_; } + /// return (for each observation) the key of the pair of poses from which we interpolate + const std::vector> world_P_body_key_pairs() const { + return world_P_body_key_pairs_; + } + /// return the interpolation factors gammas const std::vector getGammas() const { return gammas_; } - /// return the interpolation factors gammas + /// return the extrinsic camera calibration body_P_sensors const std::vector body_P_sensors() const { return body_P_sensors_; } @@ -192,7 +197,8 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor< std::cout << " pose2 key: " << keyFormatter(world_P_body_key_pairs_[i].second) << std::endl; std::cout << " gamma: " << gammas_[i] << std::endl; - K_all_[i]->print("calibration = "); + body_P_sensors_[i].print("extrinsic calibration:\n"); + K_all_[i]->print("intrinsic calibration = "); } Base::print("", keyFormatter); } @@ -202,8 +208,30 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor< const SmartProjectionPoseFactorRollingShutter* e = dynamic_cast*>(&p); + double keyPairsEqual = true; + if(this->world_P_body_key_pairs_.size() == e->world_P_body_key_pairs().size()){ + for(size_t k=0; k< this->world_P_body_key_pairs_.size(); k++){ + const Key key1own = world_P_body_key_pairs_[k].first; + const Key key1e = e->world_P_body_key_pairs()[k].first; + const Key key2own = world_P_body_key_pairs_[k].second; + const Key key2e = e->world_P_body_key_pairs()[k].second; + if ( !(key1own == key1e) || !(key2own == key2e) ){ + keyPairsEqual = false; break; + } + } + }else{ keyPairsEqual = false; } + + double extrinsicCalibrationEqual = true; + if(this->body_P_sensors_.size() == e->body_P_sensors().size()){ + for(size_t i=0; i< this->body_P_sensors_.size(); i++){ + if (!body_P_sensors_[i].equals(e->body_P_sensors()[i])){ + extrinsicCalibrationEqual = false; break; + } + } + }else{ extrinsicCalibrationEqual = false; } + return e && Base::equals(p, tol) && K_all_ == e->calibration() - && gammas_ == e->getGammas() && body_P_sensors_ == e->body_P_sensors(); + && gammas_ == e->getGammas() && keyPairsEqual && extrinsicCalibrationEqual; } /** @@ -249,8 +277,7 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor< * and the error vector b. Note that the jacobians are computed for a given point. */ void computeJacobiansWithTriangulatedPoint(FBlocks& Fs, Matrix& E, Vector& b, - const Values& values) const - override { + const Values& values) const { if (!this->result_) { throw("computeJacobiansWithTriangulatedPoint"); } else { // valid result: compute jacobians @@ -296,7 +323,7 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor< /// linearize and return a Hessianfactor that is an approximation of error(p) boost::shared_ptr > createHessianFactor( const Values& values, const double lambda = 0.0, bool diagonalDamping = - false) const override { + false) const { // we may have multiple cameras sharing the same extrinsic cals, hence the number // of keys may be smaller than 2 * nrMeasurements (which is the upper bound where we @@ -313,29 +340,29 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor< throw std::runtime_error("SmartProjectionPoseFactorRollingShutter: " "measured_.size() inconsistent with input"); - // triangulate 3D point at given linearization point - triangulateSafe(cameras(values)); - - if (!this->result_) { // failed: return "empty/zero" Hessian - for (Matrix& m : Gs) - m = Matrix::Zero(DimPose, DimPose); - for (Vector& v : gs) - v = Vector::Zero(DimPose); - return boost::make_shared < RegularHessianFactor - > ( this->keys_, Gs, gs, 0.0); - } - - // compute Jacobian given triangulated 3D Point - FBlocks Fs; - Matrix F, E; - Vector b; - computeJacobiansWithTriangulatedPoint(Fs, E, b, values); - - // Whiten using noise model - this->noiseModel_->WhitenSystem(E, b); - for (size_t i = 0; i < Fs.size(); i++) - Fs[i] = this->noiseModel_->Whiten(Fs[i]); - +// // triangulate 3D point at given linearization point +// triangulateSafe(cameras(values)); +// +// if (!this->result_) { // failed: return "empty/zero" Hessian +// for (Matrix& m : Gs) +// m = Matrix::Zero(DimPose, DimPose); +// for (Vector& v : gs) +// v = Vector::Zero(DimPose); +// return boost::make_shared < RegularHessianFactor +// > ( this->keys_, Gs, gs, 0.0); +// } +// +// // compute Jacobian given triangulated 3D Point +// FBlocks Fs; +// Matrix F, E; +// Vector b; +// computeJacobiansWithTriangulatedPoint(Fs, E, b, values); +// +// // Whiten using noise model +// this->noiseModel_->WhitenSystem(E, b); +// for (size_t i = 0; i < Fs.size(); i++) +// Fs[i] = this->noiseModel_->Whiten(Fs[i]); +// // // build augmented Hessian (with last row/column being the information vector) // Matrix3 P; // This::Cameras::ComputePointCovariance<3>(P, E, lambda, diagonalDamping); diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index d4c268b3c..fe662932c 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -10,13 +10,13 @@ * -------------------------------------------------------------------------- */ /** - * @file testSmartProjectionPoseFactorRollingShutter.cpp - * @brief Unit tests for SmartProjectionPoseFactorRollingShutter Class + * @file testSmartProjectionPoseFactorRollingShutterRollingShutter.cpp + * @brief Unit tests for SmartProjectionPoseFactorRollingShutterRollingShutter Class * @author Luca Carlone * @date July 2021 */ -#include "gtsam/slam/tests/smartFactorScenarios.h" +//#include "gtsam/slam/tests/smartFactorScenarios.h" #include #include #include @@ -27,6 +27,7 @@ #include #include +using namespace gtsam; using namespace boost::assign; using namespace std::placeholders; @@ -47,17 +48,15 @@ static Symbol x3('X', 3); static Point2 measurement1(323.0, 240.0); LevenbergMarquardtParams lmParams; -// Make more verbose like so (in tests): -// params.verbosityLM = LevenbergMarquardtParams::SUMMARY; +typedef SmartProjectionPoseFactorRollingShutter SmartFactorRS; -/* ************************************************************************* * -TEST( SmartProjectionPoseFactor, Constructor) { - using namespace vanillaPose; - SmartFactor::shared_ptr factor1(new SmartFactor(model, sharedK)); +/* ************************************************************************* */ +TEST( SmartProjectionPoseFactorRollingShutter, Constructor) { + SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); } /* ************************************************************************* * -TEST( SmartProjectionPoseFactor, Constructor2) { +TEST( SmartProjectionPoseFactorRollingShutter, Constructor2) { using namespace vanillaPose; SmartProjectionParams params; params.setRankTolerance(rankTol); @@ -65,14 +64,14 @@ TEST( SmartProjectionPoseFactor, Constructor2) { } /* ************************************************************************* * -TEST( SmartProjectionPoseFactor, Constructor3) { +TEST( SmartProjectionPoseFactorRollingShutter, Constructor3) { using namespace vanillaPose; SmartFactor::shared_ptr factor1(new SmartFactor(model, sharedK)); factor1->add(measurement1, x1); } /* ************************************************************************* * -TEST( SmartProjectionPoseFactor, Constructor4) { +TEST( SmartProjectionPoseFactorRollingShutter, Constructor4) { using namespace vanillaPose; SmartProjectionParams params; params.setRankTolerance(rankTol); @@ -81,7 +80,7 @@ TEST( SmartProjectionPoseFactor, Constructor4) { } /* ************************************************************************* * -TEST( SmartProjectionPoseFactor, params) { +TEST( SmartProjectionPoseFactorRollingShutter, params) { using namespace vanillaPose; SmartProjectionParams params; double rt = params.getRetriangulationThreshold(); @@ -92,7 +91,7 @@ TEST( SmartProjectionPoseFactor, params) { } /* ************************************************************************* * -TEST( SmartProjectionPoseFactor, Equals ) { +TEST( SmartProjectionPoseFactorRollingShutter, Equals ) { using namespace vanillaPose; SmartFactor::shared_ptr factor1(new SmartFactor(model, sharedK)); factor1->add(measurement1, x1); @@ -104,7 +103,7 @@ TEST( SmartProjectionPoseFactor, Equals ) { } /* ************************************************************************* -TEST( SmartProjectionPoseFactor, noiseless ) { +TEST( SmartProjectionPoseFactorRollingShutter, noiseless ) { using namespace vanillaPose; @@ -162,7 +161,7 @@ TEST( SmartProjectionPoseFactor, noiseless ) { } /* ************************************************************************* -TEST( SmartProjectionPoseFactor, noisy ) { +TEST( SmartProjectionPoseFactorRollingShutter, noisy ) { using namespace vanillaPose; @@ -196,7 +195,7 @@ TEST( SmartProjectionPoseFactor, noisy ) { } /* ************************************************************************* -TEST(SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform) { +TEST(SmartProjectionPoseFactorRollingShutter, smartFactorWithSensorBodyTransform) { using namespace vanillaPose; // create arbitrary body_T_sensor (transforms from sensor to body) @@ -271,7 +270,7 @@ TEST(SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform) { } /* ************************************************************************* -TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) { +TEST( SmartProjectionPoseFactorRollingShutter, 3poses_smart_projection_factor ) { using namespace vanillaPose2; Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; @@ -332,7 +331,7 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) { } /* ************************************************************************* -TEST( SmartProjectionPoseFactor, Factors ) { +TEST( SmartProjectionPoseFactorRollingShutter, Factors ) { using namespace vanillaPose; @@ -496,7 +495,7 @@ TEST( SmartProjectionPoseFactor, Factors ) { } /* ************************************************************************* -TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) { +TEST( SmartProjectionPoseFactorRollingShutter, 3poses_iterative_smart_projection_factor ) { using namespace vanillaPose; @@ -550,7 +549,7 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) { } /* ************************************************************************* -TEST( SmartProjectionPoseFactor, jacobianSVD ) { +TEST( SmartProjectionPoseFactorRollingShutter, jacobianSVD ) { using namespace vanillaPose; @@ -606,7 +605,7 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ) { } /* ************************************************************************* -TEST( SmartProjectionPoseFactor, landmarkDistance ) { +TEST( SmartProjectionPoseFactorRollingShutter, landmarkDistance ) { using namespace vanillaPose; @@ -665,7 +664,7 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ) { } /* ************************************************************************* -TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) { +TEST( SmartProjectionPoseFactorRollingShutter, dynamicOutlierRejection ) { using namespace vanillaPose; @@ -731,7 +730,7 @@ TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) { } /* ************************************************************************* -TEST( SmartProjectionPoseFactor, 3poses_projection_factor ) { +TEST( SmartProjectionPoseFactorRollingShutter, 3poses_projection_factor ) { using namespace vanillaPose2; @@ -778,7 +777,7 @@ TEST( SmartProjectionPoseFactor, 3poses_projection_factor ) { } /* ************************************************************************* -TEST( SmartProjectionPoseFactor, CheckHessian) { +TEST( SmartProjectionPoseFactorRollingShutter, CheckHessian) { KeyVector views {x1, x2, x3}; @@ -786,7 +785,7 @@ TEST( SmartProjectionPoseFactor, CheckHessian) { // Two slightly different cameras Pose3 pose2 = level_pose - * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); + * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); Camera cam2(pose2, sharedK); Camera cam3(pose3, sharedK); @@ -860,7 +859,7 @@ TEST( SmartProjectionPoseFactor, CheckHessian) { } /* ************************************************************************* -TEST( SmartProjectionPoseFactor, Hessian ) { +TEST( SmartProjectionPoseFactorRollingShutter, Hessian ) { using namespace vanillaPose2; From 306393a18c1cbbcc9a53378f6552c2effc35d23c Mon Sep 17 00:00:00 2001 From: lcarlone Date: Wed, 21 Jul 2021 14:30:55 -0400 Subject: [PATCH 085/248] solidified add and equal --- .../SmartProjectionPoseFactorRollingShutter.h | 8 +- ...martProjectionPoseFactorRollingShutter.cpp | 1656 ++++++++--------- 2 files changed, 832 insertions(+), 832 deletions(-) diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index 13ba4e85e..e5730dcd6 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -99,8 +99,9 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor< void add(const Point2& measured, const Key& world_P_body_key1, const Key& world_P_body_key2, const double& gamma, const boost::shared_ptr& K, const Pose3 body_P_sensor) { - // store measurements in base class (note: we only store the first key there) - Base::add(measured, world_P_body_key1); + // store measurements in base class (note: we manyally add keys below to make sure they are unique + this->measured_.push_back(measured); + // but we also store the extrinsic calibration keys in the same order world_P_body_key_pairs_.push_back( std::make_pair(world_P_body_key1, world_P_body_key2)); @@ -113,6 +114,9 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor< == this->keys_.end()) this->keys_.push_back(world_P_body_key2); // add only unique keys + // store interpolation factors + gammas_.push_back(gamma); + // store fixed calibration K_all_.push_back(K); diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index fe662932c..87bcbee81 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -16,7 +16,7 @@ * @date July 2021 */ -//#include "gtsam/slam/tests/smartFactorScenarios.h" +#include "gtsam/slam/tests/smartFactorScenarios.h" #include #include #include @@ -44,8 +44,17 @@ using symbol_shorthand::L; static Symbol x1('X', 1); static Symbol x2('X', 2); static Symbol x3('X', 3); +static Symbol x4('X', 4); +static Pose3 body_P_sensor = Pose3(Rot3::Ypr(-0.1, 0.2, -0.2), + Point3(0.1, 0.0, 0.0)); static Point2 measurement1(323.0, 240.0); +static Point2 measurement2(200.0, 220.0); +static Point2 measurement3(320.0, 10.0); +static double interp_factor = 0.5; +static double interp_factor1 = 0.3; +static double interp_factor2 = 0.4; +static double interp_factor3 = 0.5; LevenbergMarquardtParams lmParams; typedef SmartProjectionPoseFactorRollingShutter SmartFactorRS; @@ -55,841 +64,828 @@ TEST( SmartProjectionPoseFactorRollingShutter, Constructor) { SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); } -/* ************************************************************************* * +/* ************************************************************************* */ TEST( SmartProjectionPoseFactorRollingShutter, Constructor2) { - using namespace vanillaPose; SmartProjectionParams params; params.setRankTolerance(rankTol); - SmartFactor factor1(model, sharedK, params); -} - -/* ************************************************************************* * -TEST( SmartProjectionPoseFactorRollingShutter, Constructor3) { - using namespace vanillaPose; - SmartFactor::shared_ptr factor1(new SmartFactor(model, sharedK)); - factor1->add(measurement1, x1); -} - -/* ************************************************************************* * -TEST( SmartProjectionPoseFactorRollingShutter, Constructor4) { - using namespace vanillaPose; - SmartProjectionParams params; - params.setRankTolerance(rankTol); - SmartFactor factor1(model, sharedK, params); - factor1.add(measurement1, x1); -} - -/* ************************************************************************* * -TEST( SmartProjectionPoseFactorRollingShutter, params) { - using namespace vanillaPose; - SmartProjectionParams params; - double rt = params.getRetriangulationThreshold(); - EXPECT_DOUBLES_EQUAL(1e-5, rt, 1e-7); - params.setRetriangulationThreshold(1e-3); - rt = params.getRetriangulationThreshold(); - EXPECT_DOUBLES_EQUAL(1e-3, rt, 1e-7); -} - -/* ************************************************************************* * -TEST( SmartProjectionPoseFactorRollingShutter, Equals ) { - using namespace vanillaPose; - SmartFactor::shared_ptr factor1(new SmartFactor(model, sharedK)); - factor1->add(measurement1, x1); - - SmartFactor::shared_ptr factor2(new SmartFactor(model,sharedK)); - factor2->add(measurement1, x1); - - CHECK(assert_equal(*factor1, *factor2)); -} - -/* ************************************************************************* -TEST( SmartProjectionPoseFactorRollingShutter, noiseless ) { - - using namespace vanillaPose; - - // Project two landmarks into two cameras - Point2 level_uv = level_camera.project(landmark1); - Point2 level_uv_right = level_camera_right.project(landmark1); - - SmartFactor factor(model, sharedK); - factor.add(level_uv, x1); - factor.add(level_uv_right, x2); - - Values values; // it's a pose factor, hence these are poses - values.insert(x1, cam1.pose()); - values.insert(x2, cam2.pose()); - - double actualError = factor.error(values); - double expectedError = 0.0; - EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); - - SmartFactor::Cameras cameras = factor.cameras(values); - double actualError2 = factor.totalReprojectionError(cameras); - EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1e-7); - - // Calculate expected derivative for point (easiest to check) - std::function f = // - std::bind(&SmartFactor::whitenedError, factor, cameras, std::placeholders::_1); - - // Calculate using computeEP - Matrix actualE; - factor.triangulateAndComputeE(actualE, values); - - // get point - boost::optional point = factor.point(); - CHECK(point); - - // calculate numerical derivative with triangulated point - Matrix expectedE = sigma * numericalDerivative11(f, *point); - EXPECT(assert_equal(expectedE, actualE, 1e-7)); - - // Calculate using reprojectionError - SmartFactor::Cameras::FBlocks F; - Matrix E; - Vector actualErrors = factor.unwhitenedError(cameras, *point, F, E); - EXPECT(assert_equal(expectedE, E, 1e-7)); - - EXPECT(assert_equal(Z_4x1, actualErrors, 1e-7)); - - // Calculate using computeJacobians - Vector b; - SmartFactor::FBlocks Fs; - factor.computeJacobians(Fs, E, b, cameras, *point); - double actualError3 = b.squaredNorm(); - EXPECT(assert_equal(expectedE, E, 1e-7)); - EXPECT_DOUBLES_EQUAL(expectedError, actualError3, 1e-6); -} - -/* ************************************************************************* -TEST( SmartProjectionPoseFactorRollingShutter, noisy ) { - - using namespace vanillaPose; - - // Project two landmarks into two cameras - Point2 pixelError(0.2, 0.2); - Point2 level_uv = level_camera.project(landmark1) + pixelError; - Point2 level_uv_right = level_camera_right.project(landmark1); - - Values values; - values.insert(x1, cam1.pose()); - Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), - Point3(0.5, 0.1, 0.3)); - values.insert(x2, pose_right.compose(noise_pose)); - - SmartFactor::shared_ptr factor(new SmartFactor(model, sharedK)); - factor->add(level_uv, x1); - factor->add(level_uv_right, x2); - - double actualError1 = factor->error(values); - - SmartFactor::shared_ptr factor2(new SmartFactor(model, sharedK)); - Point2Vector measurements; - measurements.push_back(level_uv); - measurements.push_back(level_uv_right); - - KeyVector views {x1, x2}; - - factor2->add(measurements, views); - double actualError2 = factor2->error(values); - DOUBLES_EQUAL(actualError1, actualError2, 1e-7); -} - -/* ************************************************************************* -TEST(SmartProjectionPoseFactorRollingShutter, smartFactorWithSensorBodyTransform) { - using namespace vanillaPose; - - // create arbitrary body_T_sensor (transforms from sensor to body) - Pose3 body_T_sensor = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(1, 1, 1)); - - // These are the poses we want to estimate, from camera measurements - const Pose3 sensor_T_body = body_T_sensor.inverse(); - Pose3 wTb1 = cam1.pose() * sensor_T_body; - Pose3 wTb2 = cam2.pose() * sensor_T_body; - Pose3 wTb3 = cam3.pose() * sensor_T_body; - - // three landmarks ~5 meters infront of camera - Point3 landmark1(5, 0.5, 1.2), landmark2(5, -0.5, 1.2), landmark3(5, 0, 3.0); - - Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; - - // Project three landmarks into three cameras - projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); - projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); - projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - - // Create smart factors - KeyVector views {x1, x2, x3}; - - SmartProjectionParams params; - params.setRankTolerance(1.0); - params.setDegeneracyMode(IGNORE_DEGENERACY); - params.setEnableEPI(false); - - SmartFactor smartFactor1(model, sharedK, body_T_sensor, params); - smartFactor1.add(measurements_cam1, views); - - SmartFactor smartFactor2(model, sharedK, body_T_sensor, params); - smartFactor2.add(measurements_cam2, views); - - SmartFactor smartFactor3(model, sharedK, body_T_sensor, params); - smartFactor3.add(measurements_cam3, views); - - const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - - // Put all factors in factor graph, adding priors - NonlinearFactorGraph graph; - graph.push_back(smartFactor1); - graph.push_back(smartFactor2); - graph.push_back(smartFactor3); - graph.addPrior(x1, wTb1, noisePrior); - graph.addPrior(x2, wTb2, noisePrior); - - // Check errors at ground truth poses - Values gtValues; - gtValues.insert(x1, wTb1); - gtValues.insert(x2, wTb2); - gtValues.insert(x3, wTb3); - double actualError = graph.error(gtValues); - double expectedError = 0.0; - DOUBLES_EQUAL(expectedError, actualError, 1e-7) - - Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), - Point3(0.1, 0.1, 0.1)); - Values values; - values.insert(x1, wTb1); - values.insert(x2, wTb2); - // initialize third pose with some noise, we expect it to move back to - // original pose3 - values.insert(x3, wTb3 * noise_pose); - - LevenbergMarquardtParams lmParams; - Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); - result = optimizer.optimize(); - EXPECT(assert_equal(wTb3, result.at(x3))); -} - -/* ************************************************************************* -TEST( SmartProjectionPoseFactorRollingShutter, 3poses_smart_projection_factor ) { - - using namespace vanillaPose2; - Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; - - // Project three landmarks into three cameras - projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); - projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); - projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - - KeyVector views; - views.push_back(x1); - views.push_back(x2); - views.push_back(x3); - - SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedK2)); - smartFactor1->add(measurements_cam1, views); - - SmartFactor::shared_ptr smartFactor2(new SmartFactor(model, sharedK2)); - smartFactor2->add(measurements_cam2, views); - - SmartFactor::shared_ptr smartFactor3(new SmartFactor(model, sharedK2)); - smartFactor3->add(measurements_cam3, views); - - const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - - NonlinearFactorGraph graph; - graph.push_back(smartFactor1); - graph.push_back(smartFactor2); - graph.push_back(smartFactor3); - graph.addPrior(x1, cam1.pose(), noisePrior); - graph.addPrior(x2, cam2.pose(), noisePrior); - - Values groundTruth; - groundTruth.insert(x1, cam1.pose()); - groundTruth.insert(x2, cam2.pose()); - groundTruth.insert(x3, cam3.pose()); - DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9); - - // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), - Point3(0.1, 0.1, 0.1)); // smaller noise - Values values; - values.insert(x1, cam1.pose()); - values.insert(x2, cam2.pose()); - // initialize third pose with some noise, we expect it to move back to original pose_above - values.insert(x3, pose_above * noise_pose); - EXPECT( - assert_equal( - Pose3( - Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, - -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), - Point3(0.1, -0.1, 1.9)), values.at(x3))); - - Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); - result = optimizer.optimize(); - EXPECT(assert_equal(pose_above, result.at(x3), 1e-6)); -} - -/* ************************************************************************* -TEST( SmartProjectionPoseFactorRollingShutter, Factors ) { - - using namespace vanillaPose; - - // Default cameras for simple derivatives - Rot3 R; - static Cal3_S2::shared_ptr sharedK(new Cal3_S2(100, 100, 0, 0, 0)); - Camera cam1(Pose3(R, Point3(0, 0, 0)), sharedK), cam2( - Pose3(R, Point3(1, 0, 0)), sharedK); - - // one landmarks 1m in front of camera - Point3 landmark1(0, 0, 10); - - Point2Vector measurements_cam1; - - // Project 2 landmarks into 2 cameras - measurements_cam1.push_back(cam1.project(landmark1)); - measurements_cam1.push_back(cam2.project(landmark1)); - - // Create smart factors - KeyVector views {x1, x2}; - - SmartFactor::shared_ptr smartFactor1 = boost::make_shared(model, sharedK); - smartFactor1->add(measurements_cam1, views); - - SmartFactor::Cameras cameras; - cameras.push_back(cam1); - cameras.push_back(cam2); - - // Make sure triangulation works - CHECK(smartFactor1->triangulateSafe(cameras)); - CHECK(!smartFactor1->isDegenerate()); - CHECK(!smartFactor1->isPointBehindCamera()); - boost::optional p = smartFactor1->point(); - CHECK(p); - EXPECT(assert_equal(landmark1, *p)); - - VectorValues zeroDelta; - Vector6 delta; - delta.setZero(); - zeroDelta.insert(x1, delta); - zeroDelta.insert(x2, delta); - - VectorValues perturbedDelta; - delta.setOnes(); - perturbedDelta.insert(x1, delta); - perturbedDelta.insert(x2, delta); - double expectedError = 2500; - - // After eliminating the point, A1 and A2 contain 2-rank information on cameras: - Matrix16 A1, A2; - A1 << -10, 0, 0, 0, 1, 0; - A2 << 10, 0, 1, 0, -1, 0; - A1 *= 10. / sigma; - A2 *= 10. / sigma; - Matrix expectedInformation; // filled below - { - // createHessianFactor - Matrix66 G11 = 0.5 * A1.transpose() * A1; - Matrix66 G12 = 0.5 * A1.transpose() * A2; - Matrix66 G22 = 0.5 * A2.transpose() * A2; - - Vector6 g1; - g1.setZero(); - Vector6 g2; - g2.setZero(); - - double f = 0; - - RegularHessianFactor<6> expected(x1, x2, G11, G12, g1, G22, g2, f); - expectedInformation = expected.information(); - - boost::shared_ptr > actual = - smartFactor1->createHessianFactor(cameras, 0.0); - EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6)); - EXPECT(assert_equal(expected, *actual, 1e-6)); - EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-6); - EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-6); - } - - { - Matrix26 F1; - F1.setZero(); - F1(0, 1) = -100; - F1(0, 3) = -10; - F1(1, 0) = 100; - F1(1, 4) = -10; - Matrix26 F2; - F2.setZero(); - F2(0, 1) = -101; - F2(0, 3) = -10; - F2(0, 5) = -1; - F2(1, 0) = 100; - F2(1, 2) = 10; - F2(1, 4) = -10; - Matrix E(4, 3); - E.setZero(); - E(0, 0) = 10; - E(1, 1) = 10; - E(2, 0) = 10; - E(2, 2) = 1; - E(3, 1) = 10; - SmartFactor::FBlocks Fs = list_of(F1)(F2); - Vector b(4); - b.setZero(); - - // Create smart factors - KeyVector keys; - keys.push_back(x1); - keys.push_back(x2); - - // createJacobianQFactor - SharedIsotropic n = noiseModel::Isotropic::Sigma(4, sigma); - Matrix3 P = (E.transpose() * E).inverse(); - JacobianFactorQ<6, 2> expectedQ(keys, Fs, E, P, b, n); - EXPECT(assert_equal(expectedInformation, expectedQ.information(), 1e-6)); - - boost::shared_ptr > actualQ = - smartFactor1->createJacobianQFactor(cameras, 0.0); - CHECK(actualQ); - EXPECT(assert_equal(expectedInformation, actualQ->information(), 1e-6)); - EXPECT(assert_equal(expectedQ, *actualQ)); - EXPECT_DOUBLES_EQUAL(0, actualQ->error(zeroDelta), 1e-6); - EXPECT_DOUBLES_EQUAL(expectedError, actualQ->error(perturbedDelta), 1e-6); - - // Whiten for RegularImplicitSchurFactor (does not have noise model) - model->WhitenSystem(E, b); - Matrix3 whiteP = (E.transpose() * E).inverse(); - Fs[0] = model->Whiten(Fs[0]); - Fs[1] = model->Whiten(Fs[1]); - - // createRegularImplicitSchurFactor - RegularImplicitSchurFactor expected(keys, Fs, E, whiteP, b); - - boost::shared_ptr > actual = - smartFactor1->createRegularImplicitSchurFactor(cameras, 0.0); - CHECK(actual); - EXPECT(assert_equal(expectedInformation, expected.information(), 1e-6)); - EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6)); - EXPECT(assert_equal(expected, *actual)); - EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-6); - EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-6); - } - - { - // createJacobianSVDFactor - Vector1 b; - b.setZero(); - double s = sigma * sin(M_PI_4); - SharedIsotropic n = noiseModel::Isotropic::Sigma(4 - 3, sigma); - JacobianFactor expected(x1, s * A1, x2, s * A2, b, n); - EXPECT(assert_equal(expectedInformation, expected.information(), 1e-6)); - - boost::shared_ptr actual = - smartFactor1->createJacobianSVDFactor(cameras, 0.0); - CHECK(actual); - EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6)); - EXPECT(assert_equal(expected, *actual)); - EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-6); - EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-6); - } -} - -/* ************************************************************************* -TEST( SmartProjectionPoseFactorRollingShutter, 3poses_iterative_smart_projection_factor ) { - - using namespace vanillaPose; - - KeyVector views {x1, x2, x3}; - - Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; - - // Project three landmarks into three cameras - projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); - projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); - projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - - SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedK)); - smartFactor1->add(measurements_cam1, views); - - SmartFactor::shared_ptr smartFactor2(new SmartFactor(model, sharedK)); - smartFactor2->add(measurements_cam2, views); - - SmartFactor::shared_ptr smartFactor3(new SmartFactor(model, sharedK)); - smartFactor3->add(measurements_cam3, views); - - const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - - NonlinearFactorGraph graph; - graph.push_back(smartFactor1); - graph.push_back(smartFactor2); - graph.push_back(smartFactor3); - graph.addPrior(x1, cam1.pose(), noisePrior); - graph.addPrior(x2, cam2.pose(), noisePrior); - - // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), - Point3(0.1, 0.1, 0.1)); // smaller noise - Values values; - values.insert(x1, cam1.pose()); - values.insert(x2, cam2.pose()); - // initialize third pose with some noise, we expect it to move back to original pose_above - values.insert(x3, pose_above * noise_pose); - EXPECT( - assert_equal( - Pose3( - Rot3(1.11022302e-16, -0.0314107591, 0.99950656, -0.99950656, - -0.0313952598, -0.000986635786, 0.0314107591, -0.999013364, - -0.0313952598), Point3(0.1, -0.1, 1.9)), - values.at(x3))); - - Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); - result = optimizer.optimize(); - EXPECT(assert_equal(pose_above, result.at(x3), 1e-7)); -} - -/* ************************************************************************* -TEST( SmartProjectionPoseFactorRollingShutter, jacobianSVD ) { - - using namespace vanillaPose; - - KeyVector views {x1, x2, x3}; - - Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; - - // Project three landmarks into three cameras - projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); - projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); - projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - - SmartProjectionParams params; - params.setRankTolerance(1.0); - params.setLinearizationMode(gtsam::JACOBIAN_SVD); - params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); - params.setEnableEPI(false); - SmartFactor factor1(model, sharedK, params); - - SmartFactor::shared_ptr smartFactor1( - new SmartFactor(model, sharedK, params)); - smartFactor1->add(measurements_cam1, views); - - SmartFactor::shared_ptr smartFactor2( - new SmartFactor(model, sharedK, params)); - smartFactor2->add(measurements_cam2, views); - - SmartFactor::shared_ptr smartFactor3( - new SmartFactor(model, sharedK, params)); - smartFactor3->add(measurements_cam3, views); - - const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - - NonlinearFactorGraph graph; - graph.push_back(smartFactor1); - graph.push_back(smartFactor2); - graph.push_back(smartFactor3); - graph.addPrior(x1, cam1.pose(), noisePrior); - graph.addPrior(x2, cam2.pose(), noisePrior); - - // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), - Point3(0.1, 0.1, 0.1)); // smaller noise - Values values; - values.insert(x1, cam1.pose()); - values.insert(x2, cam2.pose()); - values.insert(x3, pose_above * noise_pose); - - Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); - result = optimizer.optimize(); - EXPECT(assert_equal(pose_above, result.at(x3), 1e-6)); -} - -/* ************************************************************************* -TEST( SmartProjectionPoseFactorRollingShutter, landmarkDistance ) { - - using namespace vanillaPose; - - double excludeLandmarksFutherThanDist = 2; - - KeyVector views {x1, x2, x3}; - - Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; - - // Project three landmarks into three cameras - projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); - projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); - projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - - SmartProjectionParams params; - params.setRankTolerance(1.0); - params.setLinearizationMode(gtsam::JACOBIAN_SVD); - params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); - params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); - params.setEnableEPI(false); - - SmartFactor::shared_ptr smartFactor1( - new SmartFactor(model, sharedK, params)); - smartFactor1->add(measurements_cam1, views); - - SmartFactor::shared_ptr smartFactor2( - new SmartFactor(model, sharedK, params)); - smartFactor2->add(measurements_cam2, views); - - SmartFactor::shared_ptr smartFactor3( - new SmartFactor(model, sharedK, params)); - smartFactor3->add(measurements_cam3, views); - - const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - - NonlinearFactorGraph graph; - graph.push_back(smartFactor1); - graph.push_back(smartFactor2); - graph.push_back(smartFactor3); - graph.addPrior(x1, cam1.pose(), noisePrior); - graph.addPrior(x2, cam2.pose(), noisePrior); - - // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), - Point3(0.1, 0.1, 0.1)); // smaller noise - Values values; - values.insert(x1, cam1.pose()); - values.insert(x2, cam2.pose()); - values.insert(x3, pose_above * noise_pose); - - // All factors are disabled and pose should remain where it is - Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); - result = optimizer.optimize(); - EXPECT(assert_equal(values.at(x3), result.at(x3))); -} - -/* ************************************************************************* -TEST( SmartProjectionPoseFactorRollingShutter, dynamicOutlierRejection ) { - - using namespace vanillaPose; - - double excludeLandmarksFutherThanDist = 1e10; - double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error - - KeyVector views {x1, x2, x3}; - - // add fourth landmark - Point3 landmark4(5, -0.5, 1); - - Point2Vector measurements_cam1, measurements_cam2, measurements_cam3, - measurements_cam4; - - // Project 4 landmarks into three cameras - projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); - projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); - projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4); - measurements_cam4.at(0) = measurements_cam4.at(0) + Point2(10, 10); // add outlier - - SmartProjectionParams params; - params.setLinearizationMode(gtsam::JACOBIAN_SVD); - params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); - params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold); - - SmartFactor::shared_ptr smartFactor1( - new SmartFactor(model, sharedK, params)); - smartFactor1->add(measurements_cam1, views); - - SmartFactor::shared_ptr smartFactor2( - new SmartFactor(model, sharedK, params)); - smartFactor2->add(measurements_cam2, views); - - SmartFactor::shared_ptr smartFactor3( - new SmartFactor(model, sharedK, params)); - smartFactor3->add(measurements_cam3, views); - - SmartFactor::shared_ptr smartFactor4( - new SmartFactor(model, sharedK, params)); - smartFactor4->add(measurements_cam4, views); - - const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - - NonlinearFactorGraph graph; - graph.push_back(smartFactor1); - graph.push_back(smartFactor2); - graph.push_back(smartFactor3); - graph.push_back(smartFactor4); - graph.addPrior(x1, cam1.pose(), noisePrior); - graph.addPrior(x2, cam2.pose(), noisePrior); - - Values values; - values.insert(x1, cam1.pose()); - values.insert(x2, cam2.pose()); - values.insert(x3, cam3.pose()); - - // All factors are disabled and pose should remain where it is - Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); - result = optimizer.optimize(); - EXPECT(assert_equal(cam3.pose(), result.at(x3))); -} - -/* ************************************************************************* -TEST( SmartProjectionPoseFactorRollingShutter, 3poses_projection_factor ) { - - using namespace vanillaPose2; - - KeyVector views {x1, x2, x3}; - - typedef GenericProjectionFactor ProjectionFactor; - NonlinearFactorGraph graph; - - // Project three landmarks into three cameras - graph.emplace_shared(cam1.project(landmark1), model, x1, L(1), sharedK2); - graph.emplace_shared(cam2.project(landmark1), model, x2, L(1), sharedK2); - graph.emplace_shared(cam3.project(landmark1), model, x3, L(1), sharedK2); - - graph.emplace_shared(cam1.project(landmark2), model, x1, L(2), sharedK2); - graph.emplace_shared(cam2.project(landmark2), model, x2, L(2), sharedK2); - graph.emplace_shared(cam3.project(landmark2), model, x3, L(2), sharedK2); - - graph.emplace_shared(cam1.project(landmark3), model, x1, L(3), sharedK2); - graph.emplace_shared(cam2.project(landmark3), model, x2, L(3), sharedK2); - graph.emplace_shared(cam3.project(landmark3), model, x3, L(3), sharedK2); - - const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - graph.addPrior(x1, level_pose, noisePrior); - graph.addPrior(x2, pose_right, noisePrior); - - Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), - Point3(0.5, 0.1, 0.3)); - Values values; - values.insert(x1, level_pose); - values.insert(x2, pose_right); - values.insert(x3, pose_above * noise_pose); - values.insert(L(1), landmark1); - values.insert(L(2), landmark2); - values.insert(L(3), landmark3); - - DOUBLES_EQUAL(48406055, graph.error(values), 1); - - LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); - Values result = optimizer.optimize(); - - DOUBLES_EQUAL(0, graph.error(result), 1e-9); - - EXPECT(assert_equal(pose_above, result.at(x3), 1e-7)); -} - -/* ************************************************************************* -TEST( SmartProjectionPoseFactorRollingShutter, CheckHessian) { - - KeyVector views {x1, x2, x3}; - - using namespace vanillaPose; - - // Two slightly different cameras - Pose3 pose2 = level_pose - * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); - Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); - Camera cam2(pose2, sharedK); - Camera cam3(pose3, sharedK); - - Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; - - // Project three landmarks into three cameras - projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); - projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); - projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - - SmartProjectionParams params; - params.setRankTolerance(10); - - SmartFactor::shared_ptr smartFactor1( - new SmartFactor(model, sharedK, params)); // HESSIAN, by default - smartFactor1->add(measurements_cam1, views); - - SmartFactor::shared_ptr smartFactor2( - new SmartFactor(model, sharedK, params)); // HESSIAN, by default - smartFactor2->add(measurements_cam2, views); - - SmartFactor::shared_ptr smartFactor3( - new SmartFactor(model, sharedK, params)); // HESSIAN, by default - smartFactor3->add(measurements_cam3, views); - - NonlinearFactorGraph graph; - graph.push_back(smartFactor1); - graph.push_back(smartFactor2); - graph.push_back(smartFactor3); - - // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), - Point3(0.1, 0.1, 0.1)); // smaller noise - Values values; - values.insert(x1, cam1.pose()); - values.insert(x2, cam2.pose()); - // initialize third pose with some noise, we expect it to move back to original pose_above - values.insert(x3, pose3 * noise_pose); - EXPECT( - assert_equal( - Pose3( - Rot3(0.00563056869, -0.130848107, 0.991386438, -0.991390265, - -0.130426831, -0.0115837907, 0.130819108, -0.98278564, - -0.130455917), - Point3(0.0897734171, -0.110201006, 0.901022872)), - values.at(x3))); - - boost::shared_ptr factor1 = smartFactor1->linearize(values); - boost::shared_ptr factor2 = smartFactor2->linearize(values); - boost::shared_ptr factor3 = smartFactor3->linearize(values); - - Matrix CumulativeInformation = factor1->information() + factor2->information() - + factor3->information(); - - boost::shared_ptr GaussianGraph = graph.linearize( - values); - Matrix GraphInformation = GaussianGraph->hessian().first; - - // Check Hessian - EXPECT(assert_equal(GraphInformation, CumulativeInformation, 1e-6)); - - Matrix AugInformationMatrix = factor1->augmentedInformation() - + factor2->augmentedInformation() + factor3->augmentedInformation(); - - // Check Information vector - Vector InfoVector = AugInformationMatrix.block(0, 18, 18, 1); // 18x18 Hessian + information vector - - // Check Hessian - EXPECT(assert_equal(InfoVector, GaussianGraph->hessian().second, 1e-6)); -} - -/* ************************************************************************* -TEST( SmartProjectionPoseFactorRollingShutter, Hessian ) { - - using namespace vanillaPose2; - - KeyVector views {x1, x2}; - - // Project three landmarks into 2 cameras - Point2 cam1_uv1 = cam1.project(landmark1); - Point2 cam2_uv1 = cam2.project(landmark1); - Point2Vector measurements_cam1; - measurements_cam1.push_back(cam1_uv1); - measurements_cam1.push_back(cam2_uv1); - - SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedK2)); - smartFactor1->add(measurements_cam1, views); - - Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), - Point3(0.5, 0.1, 0.3)); - Values values; - values.insert(x1, cam1.pose()); - values.insert(x2, cam2.pose()); - - boost::shared_ptr factor = smartFactor1->linearize(values); - - // compute triangulation from linearization point - // compute reprojection errors (sum squared) - // compare with factor.info(): the bottom right element is the squared sum of the reprojection errors (normalized by the covariance) - // check that it is correctly scaled when using noiseProjection = [1/4 0; 0 1/4] + SmartFactorRS factor1(model, params); } /* ************************************************************************* */ +TEST( SmartProjectionPoseFactorRollingShutter, add) { + using namespace vanillaPose; + SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); + factor1->add(measurement1, x1, x2, interp_factor, sharedK, body_P_sensor); +} + +/* ************************************************************************* */ +TEST( SmartProjectionPoseFactorRollingShutter, Equals ) { + using namespace vanillaPose; + + // create fake measurements + std::vector measurements; + measurements.push_back(measurement1); + measurements.push_back(measurement2); + measurements.push_back(measurement3); + + std::vector> key_pairs; + key_pairs.push_back(std::make_pair(x1,x2)); + key_pairs.push_back(std::make_pair(x2,x3)); + key_pairs.push_back(std::make_pair(x3,x4)); + + std::vector> intrinsicCalibrations; + intrinsicCalibrations.push_back(sharedK); + intrinsicCalibrations.push_back(sharedK); + intrinsicCalibrations.push_back(sharedK); + + std::vector extrinsicCalibrations; + extrinsicCalibrations.push_back(body_P_sensor); + extrinsicCalibrations.push_back(body_P_sensor); + extrinsicCalibrations.push_back(body_P_sensor); + + std::vector interp_factors; + interp_factors.push_back(interp_factor1); + interp_factors.push_back(interp_factor2); + interp_factors.push_back(interp_factor3); + + // create by adding a batch of measurements with a bunch of calibrations + SmartFactorRS::shared_ptr factor2(new SmartFactorRS(model)); + factor2->add(measurements, key_pairs, interp_factors, intrinsicCalibrations, extrinsicCalibrations); + + // create by adding a batch of measurements with a single calibrations + SmartFactorRS::shared_ptr factor3(new SmartFactorRS(model)); + factor3->add(measurements, key_pairs, interp_factors, sharedK, body_P_sensor); + + { // create equal factors and show equal returns true + SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); + factor1->add(measurement1, x1, x2, interp_factor1, sharedK, body_P_sensor); + factor1->add(measurement2, x2, x3, interp_factor2, sharedK, body_P_sensor); + factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor); + + CHECK(assert_equal(*factor1, *factor2)); + CHECK(assert_equal(*factor1, *factor3)); + } + { // create slightly different factors (different keys) and show equal returns false + SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); + factor1->add(measurement1, x1, x2, interp_factor1, sharedK, body_P_sensor); + factor1->add(measurement2, x2, x2, interp_factor2, sharedK, body_P_sensor); // different! + factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor); + + CHECK(!assert_equal(*factor1, *factor2)); + CHECK(!assert_equal(*factor1, *factor3)); + } + { // create slightly different factors (different extrinsics) and show equal returns false + SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); + factor1->add(measurement1, x1, x2, interp_factor1, sharedK, body_P_sensor); + factor1->add(measurement2, x2, x3, interp_factor2, sharedK, body_P_sensor*body_P_sensor); // different! + factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor); + + CHECK(!assert_equal(*factor1, *factor2)); + CHECK(!assert_equal(*factor1, *factor3)); + } + { // create slightly different factors (different interp factors) and show equal returns false + SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); + factor1->add(measurement1, x1, x2, interp_factor1, sharedK, body_P_sensor); + factor1->add(measurement2, x2, x3, interp_factor1, sharedK, body_P_sensor); // different! + factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor); + + CHECK(!assert_equal(*factor1, *factor2)); + CHECK(!assert_equal(*factor1, *factor3)); + } +} + +/* ************************************************************************* + TEST( SmartProjectionPoseFactorRollingShutter, noiseless ) { + + using namespace vanillaPose; + + // Project two landmarks into two cameras + Point2 level_uv = level_camera.project(landmark1); + Point2 level_uv_right = level_camera_right.project(landmark1); + + SmartFactor factor(model, sharedK); + factor.add(level_uv, x1); + factor.add(level_uv_right, x2); + + Values values; // it's a pose factor, hence these are poses + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + + double actualError = factor.error(values); + double expectedError = 0.0; + EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); + + SmartFactor::Cameras cameras = factor.cameras(values); + double actualError2 = factor.totalReprojectionError(cameras); + EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1e-7); + + // Calculate expected derivative for point (easiest to check) + std::function f = // + std::bind(&SmartFactor::whitenedError, factor, cameras, std::placeholders::_1); + + // Calculate using computeEP + Matrix actualE; + factor.triangulateAndComputeE(actualE, values); + + // get point + boost::optional point = factor.point(); + CHECK(point); + + // calculate numerical derivative with triangulated point + Matrix expectedE = sigma * numericalDerivative11(f, *point); + EXPECT(assert_equal(expectedE, actualE, 1e-7)); + + // Calculate using reprojectionError + SmartFactor::Cameras::FBlocks F; + Matrix E; + Vector actualErrors = factor.unwhitenedError(cameras, *point, F, E); + EXPECT(assert_equal(expectedE, E, 1e-7)); + + EXPECT(assert_equal(Z_4x1, actualErrors, 1e-7)); + + // Calculate using computeJacobians + Vector b; + SmartFactor::FBlocks Fs; + factor.computeJacobians(Fs, E, b, cameras, *point); + double actualError3 = b.squaredNorm(); + EXPECT(assert_equal(expectedE, E, 1e-7)); + EXPECT_DOUBLES_EQUAL(expectedError, actualError3, 1e-6); + } + + /* ************************************************************************* + TEST( SmartProjectionPoseFactorRollingShutter, noisy ) { + + using namespace vanillaPose; + + // Project two landmarks into two cameras + Point2 pixelError(0.2, 0.2); + Point2 level_uv = level_camera.project(landmark1) + pixelError; + Point2 level_uv_right = level_camera_right.project(landmark1); + + Values values; + values.insert(x1, cam1.pose()); + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), + Point3(0.5, 0.1, 0.3)); + values.insert(x2, pose_right.compose(noise_pose)); + + SmartFactor::shared_ptr factor(new SmartFactor(model, sharedK)); + factor->add(level_uv, x1); + factor->add(level_uv_right, x2); + + double actualError1 = factor->error(values); + + SmartFactor::shared_ptr factor2(new SmartFactor(model, sharedK)); + Point2Vector measurements; + measurements.push_back(level_uv); + measurements.push_back(level_uv_right); + + KeyVector views {x1, x2}; + + factor2->add(measurements, views); + double actualError2 = factor2->error(values); + DOUBLES_EQUAL(actualError1, actualError2, 1e-7); + } + + /* ************************************************************************* + TEST(SmartProjectionPoseFactorRollingShutter, smartFactorWithSensorBodyTransform) { + using namespace vanillaPose; + + // create arbitrary body_T_sensor (transforms from sensor to body) + Pose3 body_T_sensor = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(1, 1, 1)); + + // These are the poses we want to estimate, from camera measurements + const Pose3 sensor_T_body = body_T_sensor.inverse(); + Pose3 wTb1 = cam1.pose() * sensor_T_body; + Pose3 wTb2 = cam2.pose() * sensor_T_body; + Pose3 wTb3 = cam3.pose() * sensor_T_body; + + // three landmarks ~5 meters infront of camera + Point3 landmark1(5, 0.5, 1.2), landmark2(5, -0.5, 1.2), landmark3(5, 0, 3.0); + + Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; + + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + + // Create smart factors + KeyVector views {x1, x2, x3}; + + SmartProjectionParams params; + params.setRankTolerance(1.0); + params.setDegeneracyMode(IGNORE_DEGENERACY); + params.setEnableEPI(false); + + SmartFactor smartFactor1(model, sharedK, body_T_sensor, params); + smartFactor1.add(measurements_cam1, views); + + SmartFactor smartFactor2(model, sharedK, body_T_sensor, params); + smartFactor2.add(measurements_cam2, views); + + SmartFactor smartFactor3(model, sharedK, body_T_sensor, params); + smartFactor3.add(measurements_cam3, views); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + // Put all factors in factor graph, adding priors + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.addPrior(x1, wTb1, noisePrior); + graph.addPrior(x2, wTb2, noisePrior); + + // Check errors at ground truth poses + Values gtValues; + gtValues.insert(x1, wTb1); + gtValues.insert(x2, wTb2); + gtValues.insert(x3, wTb3); + double actualError = graph.error(gtValues); + double expectedError = 0.0; + DOUBLES_EQUAL(expectedError, actualError, 1e-7) + + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); + Values values; + values.insert(x1, wTb1); + values.insert(x2, wTb2); + // initialize third pose with some noise, we expect it to move back to + // original pose3 + values.insert(x3, wTb3 * noise_pose); + + LevenbergMarquardtParams lmParams; + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + EXPECT(assert_equal(wTb3, result.at(x3))); + } + + /* ************************************************************************* + TEST( SmartProjectionPoseFactorRollingShutter, 3poses_smart_projection_factor ) { + + using namespace vanillaPose2; + Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; + + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + + KeyVector views; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); + + SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedK2)); + smartFactor1->add(measurements_cam1, views); + + SmartFactor::shared_ptr smartFactor2(new SmartFactor(model, sharedK2)); + smartFactor2->add(measurements_cam2, views); + + SmartFactor::shared_ptr smartFactor3(new SmartFactor(model, sharedK2)); + smartFactor3->add(measurements_cam3, views); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.addPrior(x1, cam1.pose(), noisePrior); + graph.addPrior(x2, cam2.pose(), noisePrior); + + Values groundTruth; + groundTruth.insert(x1, cam1.pose()); + groundTruth.insert(x2, cam2.pose()); + groundTruth.insert(x3, cam3.pose()); + DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9); + + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise + Values values; + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + // initialize third pose with some noise, we expect it to move back to original pose_above + values.insert(x3, pose_above * noise_pose); + EXPECT( + assert_equal( + Pose3( + Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, + -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), + Point3(0.1, -0.1, 1.9)), values.at(x3))); + + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + EXPECT(assert_equal(pose_above, result.at(x3), 1e-6)); + } + + /* ************************************************************************* + TEST( SmartProjectionPoseFactorRollingShutter, Factors ) { + + using namespace vanillaPose; + + // Default cameras for simple derivatives + Rot3 R; + static Cal3_S2::shared_ptr sharedK(new Cal3_S2(100, 100, 0, 0, 0)); + Camera cam1(Pose3(R, Point3(0, 0, 0)), sharedK), cam2( + Pose3(R, Point3(1, 0, 0)), sharedK); + + // one landmarks 1m in front of camera + Point3 landmark1(0, 0, 10); + + Point2Vector measurements_cam1; + + // Project 2 landmarks into 2 cameras + measurements_cam1.push_back(cam1.project(landmark1)); + measurements_cam1.push_back(cam2.project(landmark1)); + + // Create smart factors + KeyVector views {x1, x2}; + + SmartFactor::shared_ptr smartFactor1 = boost::make_shared(model, sharedK); + smartFactor1->add(measurements_cam1, views); + + SmartFactor::Cameras cameras; + cameras.push_back(cam1); + cameras.push_back(cam2); + + // Make sure triangulation works + CHECK(smartFactor1->triangulateSafe(cameras)); + CHECK(!smartFactor1->isDegenerate()); + CHECK(!smartFactor1->isPointBehindCamera()); + boost::optional p = smartFactor1->point(); + CHECK(p); + EXPECT(assert_equal(landmark1, *p)); + + VectorValues zeroDelta; + Vector6 delta; + delta.setZero(); + zeroDelta.insert(x1, delta); + zeroDelta.insert(x2, delta); + + VectorValues perturbedDelta; + delta.setOnes(); + perturbedDelta.insert(x1, delta); + perturbedDelta.insert(x2, delta); + double expectedError = 2500; + + // After eliminating the point, A1 and A2 contain 2-rank information on cameras: + Matrix16 A1, A2; + A1 << -10, 0, 0, 0, 1, 0; + A2 << 10, 0, 1, 0, -1, 0; + A1 *= 10. / sigma; + A2 *= 10. / sigma; + Matrix expectedInformation; // filled below + { + // createHessianFactor + Matrix66 G11 = 0.5 * A1.transpose() * A1; + Matrix66 G12 = 0.5 * A1.transpose() * A2; + Matrix66 G22 = 0.5 * A2.transpose() * A2; + + Vector6 g1; + g1.setZero(); + Vector6 g2; + g2.setZero(); + + double f = 0; + + RegularHessianFactor<6> expected(x1, x2, G11, G12, g1, G22, g2, f); + expectedInformation = expected.information(); + + boost::shared_ptr > actual = + smartFactor1->createHessianFactor(cameras, 0.0); + EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6)); + EXPECT(assert_equal(expected, *actual, 1e-6)); + EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-6); + EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-6); + } + + { + Matrix26 F1; + F1.setZero(); + F1(0, 1) = -100; + F1(0, 3) = -10; + F1(1, 0) = 100; + F1(1, 4) = -10; + Matrix26 F2; + F2.setZero(); + F2(0, 1) = -101; + F2(0, 3) = -10; + F2(0, 5) = -1; + F2(1, 0) = 100; + F2(1, 2) = 10; + F2(1, 4) = -10; + Matrix E(4, 3); + E.setZero(); + E(0, 0) = 10; + E(1, 1) = 10; + E(2, 0) = 10; + E(2, 2) = 1; + E(3, 1) = 10; + SmartFactor::FBlocks Fs = list_of(F1)(F2); + Vector b(4); + b.setZero(); + + // Create smart factors + KeyVector keys; + keys.push_back(x1); + keys.push_back(x2); + + // createJacobianQFactor + SharedIsotropic n = noiseModel::Isotropic::Sigma(4, sigma); + Matrix3 P = (E.transpose() * E).inverse(); + JacobianFactorQ<6, 2> expectedQ(keys, Fs, E, P, b, n); + EXPECT(assert_equal(expectedInformation, expectedQ.information(), 1e-6)); + + boost::shared_ptr > actualQ = + smartFactor1->createJacobianQFactor(cameras, 0.0); + CHECK(actualQ); + EXPECT(assert_equal(expectedInformation, actualQ->information(), 1e-6)); + EXPECT(assert_equal(expectedQ, *actualQ)); + EXPECT_DOUBLES_EQUAL(0, actualQ->error(zeroDelta), 1e-6); + EXPECT_DOUBLES_EQUAL(expectedError, actualQ->error(perturbedDelta), 1e-6); + + // Whiten for RegularImplicitSchurFactor (does not have noise model) + model->WhitenSystem(E, b); + Matrix3 whiteP = (E.transpose() * E).inverse(); + Fs[0] = model->Whiten(Fs[0]); + Fs[1] = model->Whiten(Fs[1]); + + // createRegularImplicitSchurFactor + RegularImplicitSchurFactor expected(keys, Fs, E, whiteP, b); + + boost::shared_ptr > actual = + smartFactor1->createRegularImplicitSchurFactor(cameras, 0.0); + CHECK(actual); + EXPECT(assert_equal(expectedInformation, expected.information(), 1e-6)); + EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6)); + EXPECT(assert_equal(expected, *actual)); + EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-6); + EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-6); + } + + { + // createJacobianSVDFactor + Vector1 b; + b.setZero(); + double s = sigma * sin(M_PI_4); + SharedIsotropic n = noiseModel::Isotropic::Sigma(4 - 3, sigma); + JacobianFactor expected(x1, s * A1, x2, s * A2, b, n); + EXPECT(assert_equal(expectedInformation, expected.information(), 1e-6)); + + boost::shared_ptr actual = + smartFactor1->createJacobianSVDFactor(cameras, 0.0); + CHECK(actual); + EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6)); + EXPECT(assert_equal(expected, *actual)); + EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-6); + EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-6); + } + } + + /* ************************************************************************* + TEST( SmartProjectionPoseFactorRollingShutter, 3poses_iterative_smart_projection_factor ) { + + using namespace vanillaPose; + + KeyVector views {x1, x2, x3}; + + Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; + + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + + SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedK)); + smartFactor1->add(measurements_cam1, views); + + SmartFactor::shared_ptr smartFactor2(new SmartFactor(model, sharedK)); + smartFactor2->add(measurements_cam2, views); + + SmartFactor::shared_ptr smartFactor3(new SmartFactor(model, sharedK)); + smartFactor3->add(measurements_cam3, views); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.addPrior(x1, cam1.pose(), noisePrior); + graph.addPrior(x2, cam2.pose(), noisePrior); + + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise + Values values; + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + // initialize third pose with some noise, we expect it to move back to original pose_above + values.insert(x3, pose_above * noise_pose); + EXPECT( + assert_equal( + Pose3( + Rot3(1.11022302e-16, -0.0314107591, 0.99950656, -0.99950656, + -0.0313952598, -0.000986635786, 0.0314107591, -0.999013364, + -0.0313952598), Point3(0.1, -0.1, 1.9)), + values.at(x3))); + + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + EXPECT(assert_equal(pose_above, result.at(x3), 1e-7)); + } + + /* ************************************************************************* + TEST( SmartProjectionPoseFactorRollingShutter, landmarkDistance ) { + + using namespace vanillaPose; + + double excludeLandmarksFutherThanDist = 2; + + KeyVector views {x1, x2, x3}; + + Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; + + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + + SmartProjectionParams params; + params.setRankTolerance(1.0); + params.setLinearizationMode(gtsam::JACOBIAN_SVD); + params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); + params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); + params.setEnableEPI(false); + + SmartFactor::shared_ptr smartFactor1( + new SmartFactor(model, sharedK, params)); + smartFactor1->add(measurements_cam1, views); + + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(model, sharedK, params)); + smartFactor2->add(measurements_cam2, views); + + SmartFactor::shared_ptr smartFactor3( + new SmartFactor(model, sharedK, params)); + smartFactor3->add(measurements_cam3, views); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.addPrior(x1, cam1.pose(), noisePrior); + graph.addPrior(x2, cam2.pose(), noisePrior); + + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise + Values values; + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + values.insert(x3, pose_above * noise_pose); + + // All factors are disabled and pose should remain where it is + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + EXPECT(assert_equal(values.at(x3), result.at(x3))); + } + + /* ************************************************************************* + TEST( SmartProjectionPoseFactorRollingShutter, dynamicOutlierRejection ) { + + using namespace vanillaPose; + + double excludeLandmarksFutherThanDist = 1e10; + double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error + + KeyVector views {x1, x2, x3}; + + // add fourth landmark + Point3 landmark4(5, -0.5, 1); + + Point2Vector measurements_cam1, measurements_cam2, measurements_cam3, + measurements_cam4; + + // Project 4 landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4); + measurements_cam4.at(0) = measurements_cam4.at(0) + Point2(10, 10); // add outlier + + SmartProjectionParams params; + params.setLinearizationMode(gtsam::JACOBIAN_SVD); + params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); + params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold); + + SmartFactor::shared_ptr smartFactor1( + new SmartFactor(model, sharedK, params)); + smartFactor1->add(measurements_cam1, views); + + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(model, sharedK, params)); + smartFactor2->add(measurements_cam2, views); + + SmartFactor::shared_ptr smartFactor3( + new SmartFactor(model, sharedK, params)); + smartFactor3->add(measurements_cam3, views); + + SmartFactor::shared_ptr smartFactor4( + new SmartFactor(model, sharedK, params)); + smartFactor4->add(measurements_cam4, views); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.push_back(smartFactor4); + graph.addPrior(x1, cam1.pose(), noisePrior); + graph.addPrior(x2, cam2.pose(), noisePrior); + + Values values; + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + values.insert(x3, cam3.pose()); + + // All factors are disabled and pose should remain where it is + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + EXPECT(assert_equal(cam3.pose(), result.at(x3))); + } + + /* ************************************************************************* + TEST( SmartProjectionPoseFactorRollingShutter, 3poses_projection_factor ) { + + using namespace vanillaPose2; + + KeyVector views {x1, x2, x3}; + + typedef GenericProjectionFactor ProjectionFactor; + NonlinearFactorGraph graph; + + // Project three landmarks into three cameras + graph.emplace_shared(cam1.project(landmark1), model, x1, L(1), sharedK2); + graph.emplace_shared(cam2.project(landmark1), model, x2, L(1), sharedK2); + graph.emplace_shared(cam3.project(landmark1), model, x3, L(1), sharedK2); + + graph.emplace_shared(cam1.project(landmark2), model, x1, L(2), sharedK2); + graph.emplace_shared(cam2.project(landmark2), model, x2, L(2), sharedK2); + graph.emplace_shared(cam3.project(landmark2), model, x3, L(2), sharedK2); + + graph.emplace_shared(cam1.project(landmark3), model, x1, L(3), sharedK2); + graph.emplace_shared(cam2.project(landmark3), model, x2, L(3), sharedK2); + graph.emplace_shared(cam3.project(landmark3), model, x3, L(3), sharedK2); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + graph.addPrior(x1, level_pose, noisePrior); + graph.addPrior(x2, pose_right, noisePrior); + + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), + Point3(0.5, 0.1, 0.3)); + Values values; + values.insert(x1, level_pose); + values.insert(x2, pose_right); + values.insert(x3, pose_above * noise_pose); + values.insert(L(1), landmark1); + values.insert(L(2), landmark2); + values.insert(L(3), landmark3); + + DOUBLES_EQUAL(48406055, graph.error(values), 1); + + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + Values result = optimizer.optimize(); + + DOUBLES_EQUAL(0, graph.error(result), 1e-9); + + EXPECT(assert_equal(pose_above, result.at(x3), 1e-7)); + } + + /* ************************************************************************* + TEST( SmartProjectionPoseFactorRollingShutter, CheckHessian) { + + KeyVector views {x1, x2, x3}; + + using namespace vanillaPose; + + // Two slightly different cameras + Pose3 pose2 = level_pose + * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); + Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); + Camera cam2(pose2, sharedK); + Camera cam3(pose3, sharedK); + + Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; + + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + + SmartProjectionParams params; + params.setRankTolerance(10); + + SmartFactor::shared_ptr smartFactor1( + new SmartFactor(model, sharedK, params)); // HESSIAN, by default + smartFactor1->add(measurements_cam1, views); + + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(model, sharedK, params)); // HESSIAN, by default + smartFactor2->add(measurements_cam2, views); + + SmartFactor::shared_ptr smartFactor3( + new SmartFactor(model, sharedK, params)); // HESSIAN, by default + smartFactor3->add(measurements_cam3, views); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise + Values values; + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + // initialize third pose with some noise, we expect it to move back to original pose_above + values.insert(x3, pose3 * noise_pose); + EXPECT( + assert_equal( + Pose3( + Rot3(0.00563056869, -0.130848107, 0.991386438, -0.991390265, + -0.130426831, -0.0115837907, 0.130819108, -0.98278564, + -0.130455917), + Point3(0.0897734171, -0.110201006, 0.901022872)), + values.at(x3))); + + boost::shared_ptr factor1 = smartFactor1->linearize(values); + boost::shared_ptr factor2 = smartFactor2->linearize(values); + boost::shared_ptr factor3 = smartFactor3->linearize(values); + + Matrix CumulativeInformation = factor1->information() + factor2->information() + + factor3->information(); + + boost::shared_ptr GaussianGraph = graph.linearize( + values); + Matrix GraphInformation = GaussianGraph->hessian().first; + + // Check Hessian + EXPECT(assert_equal(GraphInformation, CumulativeInformation, 1e-6)); + + Matrix AugInformationMatrix = factor1->augmentedInformation() + + factor2->augmentedInformation() + factor3->augmentedInformation(); + + // Check Information vector + Vector InfoVector = AugInformationMatrix.block(0, 18, 18, 1); // 18x18 Hessian + information vector + + // Check Hessian + EXPECT(assert_equal(InfoVector, GaussianGraph->hessian().second, 1e-6)); + } + + /* ************************************************************************* + TEST( SmartProjectionPoseFactorRollingShutter, Hessian ) { + + using namespace vanillaPose2; + + KeyVector views {x1, x2}; + + // Project three landmarks into 2 cameras + Point2 cam1_uv1 = cam1.project(landmark1); + Point2 cam2_uv1 = cam2.project(landmark1); + Point2Vector measurements_cam1; + measurements_cam1.push_back(cam1_uv1); + measurements_cam1.push_back(cam2_uv1); + + SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedK2)); + smartFactor1->add(measurements_cam1, views); + + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), + Point3(0.5, 0.1, 0.3)); + Values values; + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + + boost::shared_ptr factor = smartFactor1->linearize(values); + + // compute triangulation from linearization point + // compute reprojection errors (sum squared) + // compare with factor.info(): the bottom right element is the squared sum of the reprojection errors (normalized by the covariance) + // check that it is correctly scaled when using noiseProjection = [1/4 0; 0 1/4] + } + + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); From 6f8d639ab8c85742563ccccc8637b66232914997 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Wed, 21 Jul 2021 14:46:52 -0400 Subject: [PATCH 086/248] finding best way to test RS errors --- ...martProjectionPoseFactorRollingShutter.cpp | 134 ++++++++++++------ 1 file changed, 90 insertions(+), 44 deletions(-) diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index 87bcbee81..b5bbf9c8b 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -56,6 +56,20 @@ static double interp_factor1 = 0.3; static double interp_factor2 = 0.4; static double interp_factor3 = 0.5; +/* ************************************************************************* */ +// default Cal3_S2 poses with rolling shutter effect +namespace vanillaPoseRS { +typedef PinholePose Camera; +typedef SmartProjectionPoseFactor SmartFactor; +static Cal3_S2::shared_ptr sharedK(new Cal3_S2(fov, w, h)); +Pose3 interp_pose1 = interpolate(level_pose,pose_right,interp_factor1); +Pose3 interp_pose2 = interpolate(pose_right,pose_above,interp_factor2); +Pose3 interp_pose3 = interpolate(pose_above,level_pose,interp_factor3); +Camera cam1(interp_pose1, sharedK); +Camera cam2(interp_pose2, sharedK); +Camera cam3(interp_pose3, sharedK); +} + LevenbergMarquardtParams lmParams; typedef SmartProjectionPoseFactorRollingShutter SmartFactorRS; @@ -154,63 +168,95 @@ TEST( SmartProjectionPoseFactorRollingShutter, Equals ) { } } -/* ************************************************************************* - TEST( SmartProjectionPoseFactorRollingShutter, noiseless ) { +/* *************************************************************************/ +TEST( SmartProjectionPoseFactorRollingShutter, noiselessError ) { - using namespace vanillaPose; + using namespace vanillaPoseRS; - // Project two landmarks into two cameras - Point2 level_uv = level_camera.project(landmark1); - Point2 level_uv_right = level_camera_right.project(landmark1); +// // 2 poses such that level_pose_1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); +// // can be interpolated with interp_factor1 = 0.2: +// Pose3 level_pose1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 5)); +// Pose3 level_pose2 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0)); +// // 2 poses such that pose_right (Second camera 1 meter to the right of first camera) +// // can be interpolated with interp_factor1 = 0.4: +// Pose3 pose_right1 = level_pose * Pose3(Rot3(), Point3(1, 0, 0)); +// Pose3 pose_right2 = level_pose * Pose3(Rot3(), Point3(1, 0, 0)); +// // 2 poses such that pose_above (Third camera 1 meter above the first camera) +// // can be interpolated with interp_factor1 = 0.5: +// Pose3 pose_above1 = level_pose * Pose3(Rot3(), Point3(0, -1, 0)); +// Pose3 pose_above1 = level_pose * Pose3(Rot3(), Point3(0, -1, 0)); +// +// // Project two landmarks into two cameras +// Point2 level_uv = level_camera.project(landmark1); +// Point2 level_uv_right = level_camera_right.project(landmark1); +// Pose3 body_P_sensorId = Pose3::identity(); +// +// SmartFactor factor(model); +// factor.add(level_uv, x1, x2, interp_factor1, sharedK, body_P_sensorId); +// factor.add(level_uv_right, x2, x3, interp_factor2, sharedK, body_P_sensorId); +// +// Values values; // it's a pose factor, hence these are poses +// values.insert(x1, cam1.pose()); +// values.insert(x2, cam2.pose()); +// +// double actualError = factor.error(values); +// double expectedError = 0.0; +// EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); +} - SmartFactor factor(model, sharedK); - factor.add(level_uv, x1); - factor.add(level_uv_right, x2); + /* ************************************************************************* + TEST( SmartProjectionPoseFactorRollingShutter, Jacobians ) { - Values values; // it's a pose factor, hence these are poses - values.insert(x1, cam1.pose()); - values.insert(x2, cam2.pose()); + using namespace vanillaPose; - double actualError = factor.error(values); - double expectedError = 0.0; - EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); + // Project two landmarks into two cameras + Point2 level_uv = level_camera.project(landmark1); + Point2 level_uv_right = level_camera_right.project(landmark1); - SmartFactor::Cameras cameras = factor.cameras(values); - double actualError2 = factor.totalReprojectionError(cameras); - EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1e-7); + SmartFactor factor(model, sharedK); + factor.add(level_uv, x1); + factor.add(level_uv_right, x2); - // Calculate expected derivative for point (easiest to check) - std::function f = // - std::bind(&SmartFactor::whitenedError, factor, cameras, std::placeholders::_1); + Values values; // it's a pose factor, hence these are poses + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); - // Calculate using computeEP - Matrix actualE; - factor.triangulateAndComputeE(actualE, values); + double actualError = factor.error(values); + double expectedError = 0.0; + EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); - // get point - boost::optional point = factor.point(); - CHECK(point); + // Calculate expected derivative for point (easiest to check) + std::function f = // + std::bind(&SmartFactor::whitenedError, factor, cameras, std::placeholders::_1); - // calculate numerical derivative with triangulated point - Matrix expectedE = sigma * numericalDerivative11(f, *point); - EXPECT(assert_equal(expectedE, actualE, 1e-7)); + // Calculate using computeEP + Matrix actualE; + factor.triangulateAndComputeE(actualE, values); - // Calculate using reprojectionError - SmartFactor::Cameras::FBlocks F; - Matrix E; - Vector actualErrors = factor.unwhitenedError(cameras, *point, F, E); - EXPECT(assert_equal(expectedE, E, 1e-7)); + // get point + boost::optional point = factor.point(); + CHECK(point); - EXPECT(assert_equal(Z_4x1, actualErrors, 1e-7)); + // calculate numerical derivative with triangulated point + Matrix expectedE = sigma * numericalDerivative11(f, *point); + EXPECT(assert_equal(expectedE, actualE, 1e-7)); - // Calculate using computeJacobians - Vector b; - SmartFactor::FBlocks Fs; - factor.computeJacobians(Fs, E, b, cameras, *point); - double actualError3 = b.squaredNorm(); - EXPECT(assert_equal(expectedE, E, 1e-7)); - EXPECT_DOUBLES_EQUAL(expectedError, actualError3, 1e-6); - } + // Calculate using reprojectionError + SmartFactor::Cameras::FBlocks F; + Matrix E; + Vector actualErrors = factor.unwhitenedError(cameras, *point, F, E); + EXPECT(assert_equal(expectedE, E, 1e-7)); + + EXPECT(assert_equal(Z_4x1, actualErrors, 1e-7)); + + // Calculate using computeJacobians + Vector b; + SmartFactor::FBlocks Fs; + factor.computeJacobians(Fs, E, b, cameras, *point); + double actualError3 = b.squaredNorm(); + EXPECT(assert_equal(expectedE, E, 1e-7)); + EXPECT_DOUBLES_EQUAL(expectedError, actualError3, 1e-6); + } /* ************************************************************************* TEST( SmartProjectionPoseFactorRollingShutter, noisy ) { From 5d55e153f019eb0f968df0a6fa5d9d2257f14eaa Mon Sep 17 00:00:00 2001 From: lcarlone Date: Wed, 21 Jul 2021 15:10:10 -0400 Subject: [PATCH 087/248] yay! error test passes! --- .../SmartProjectionPoseFactorRollingShutter.h | 291 +++++++++--------- ...martProjectionPoseFactorRollingShutter.cpp | 49 ++- 2 files changed, 165 insertions(+), 175 deletions(-) diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index e5730dcd6..d62d18712 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -39,7 +39,7 @@ namespace gtsam { */ template class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor< - PinholePose > { +PinholePose > { protected: /// shared pointer to calibration object (one for each observation) @@ -81,7 +81,7 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor< SmartProjectionPoseFactorRollingShutter( const SharedNoiseModel& sharedNoiseModel, const SmartProjectionParams& params = SmartProjectionParams()) - : Base(sharedNoiseModel, params) { + : Base(sharedNoiseModel, params) { } /** Virtual destructor */ @@ -108,10 +108,10 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor< // pose keys are assumed to be unique, so we avoid duplicates here if (std::find(this->keys_.begin(), this->keys_.end(), world_P_body_key1) - == this->keys_.end()) + == this->keys_.end()) this->keys_.push_back(world_P_body_key1); // add only unique keys if (std::find(this->keys_.begin(), this->keys_.end(), world_P_body_key2) - == this->keys_.end()) + == this->keys_.end()) this->keys_.push_back(world_P_body_key2); // add only unique keys // store interpolation factors @@ -192,7 +192,7 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor< * @param keyFormatter optional formatter useful for printing Symbols */ void print(const std::string& s = "", const KeyFormatter& keyFormatter = - DefaultKeyFormatter) const override { + DefaultKeyFormatter) const override { std::cout << s << "SmartProjectionPoseFactorRollingShutter: \n "; for (size_t i = 0; i < K_all_.size(); i++) { std::cout << "-- Measurement nr " << i << std::endl; @@ -238,40 +238,6 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor< && gammas_ == e->getGammas() && keyPairsEqual && extrinsicCalibrationEqual; } - /** - * error calculates the error of the factor. - */ - double error(const Values& values) const override { - if (this->active(values)) { - return this->totalReprojectionError(cameras(values)); - } else { // else of active flag - return 0.0; - } - } - - /** - * Collect all cameras involved in this factor - * @param values Values structure which must contain camera poses - * corresponding to keys involved in this factor - * @return Cameras - */ - typename Base::Cameras cameras(const Values& values) const override { - assert(world_P_body_keys_.size() == K_all_.size()); - assert(world_P_body_keys_.size() == body_P_cam_keys_.size()); - typename Base::Cameras cameras; - for (size_t i = 0; i < world_P_body_key_pairs_.size(); i++) { - Pose3 w_P_body1 = values.at(world_P_body_key_pairs_[i].first); - Pose3 w_P_body2 = values.at(world_P_body_key_pairs_[i].second); - double interpolationFactor = gammas_[i]; - // get interpolated pose: - Pose3 w_P_body = w_P_body1.interpolateRt(w_P_body2, interpolationFactor); - Pose3 body_P_cam = body_P_sensors_[i]; - Pose3 w_P_cam = w_P_body.compose(body_P_cam); - cameras.emplace_back(w_P_cam, K_all_[i]); - } - return cameras; - } - /** * Compute jacobian F, E and error vector at a given linearization point * @param values Values structure which must contain camera poses @@ -290,7 +256,7 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor< b = Vector::Zero(2 * numViews); // a Point2 for each view Eigen::Matrix dProject_dPoseCam; Eigen::Matrix dInterpPose_dPoseBody1, - dInterpPose_dPoseBody2, dPoseCam_dInterpPose; + dInterpPose_dPoseBody2, dPoseCam_dInterpPose; Eigen::Matrix Ei; for (size_t i = 0; i < numViews; i++) { // for each camera/measurement @@ -308,7 +274,7 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor< // get jacobians and error vector for current measurement Point2 reprojectionError_i = Point2( camera.project(*this->result_, dProject_dPoseCam, Ei) - - this->measured_.at(i)); + - this->measured_.at(i)); Eigen::Matrix J; // 2 x 12 J.block(0, 0) = dProject_dPoseCam * dPoseCam_dInterpPose * dInterpPose_dPoseBody1; // (2x6) * (6x6) * (6x6) @@ -342,109 +308,146 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor< if (this->measured_.size() != cameras(values).size()) throw std::runtime_error("SmartProjectionPoseFactorRollingShutter: " - "measured_.size() inconsistent with input"); + "measured_.size() inconsistent with input"); -// // triangulate 3D point at given linearization point -// triangulateSafe(cameras(values)); -// -// if (!this->result_) { // failed: return "empty/zero" Hessian -// for (Matrix& m : Gs) -// m = Matrix::Zero(DimPose, DimPose); -// for (Vector& v : gs) -// v = Vector::Zero(DimPose); -// return boost::make_shared < RegularHessianFactor -// > ( this->keys_, Gs, gs, 0.0); -// } -// -// // compute Jacobian given triangulated 3D Point -// FBlocks Fs; -// Matrix F, E; -// Vector b; -// computeJacobiansWithTriangulatedPoint(Fs, E, b, values); -// -// // Whiten using noise model -// this->noiseModel_->WhitenSystem(E, b); -// for (size_t i = 0; i < Fs.size(); i++) -// Fs[i] = this->noiseModel_->Whiten(Fs[i]); -// -// // build augmented Hessian (with last row/column being the information vector) -// Matrix3 P; -// This::Cameras::ComputePointCovariance<3>(P, E, lambda, diagonalDamping); -// -// // marginalize point: note - we reuse the standard SchurComplement function -// SymmetricBlockMatrix augmentedHessian = This::Cameras::SchurComplement<2,DimBlock>(Fs, E, P, b); + // // triangulate 3D point at given linearization point + // triangulateSafe(cameras(values)); + // + // if (!this->result_) { // failed: return "empty/zero" Hessian + // for (Matrix& m : Gs) + // m = Matrix::Zero(DimPose, DimPose); + // for (Vector& v : gs) + // v = Vector::Zero(DimPose); + // return boost::make_shared < RegularHessianFactor + // > ( this->keys_, Gs, gs, 0.0); + // } + // + // // compute Jacobian given triangulated 3D Point + // FBlocks Fs; + // Matrix F, E; + // Vector b; + // computeJacobiansWithTriangulatedPoint(Fs, E, b, values); + // + // // Whiten using noise model + // this->noiseModel_->WhitenSystem(E, b); + // for (size_t i = 0; i < Fs.size(); i++) + // Fs[i] = this->noiseModel_->Whiten(Fs[i]); + // + // // build augmented Hessian (with last row/column being the information vector) + // Matrix3 P; + // This::Cameras::ComputePointCovariance<3>(P, E, lambda, diagonalDamping); + // + // // marginalize point: note - we reuse the standard SchurComplement function + // SymmetricBlockMatrix augmentedHessian = This::Cameras::SchurComplement<2,DimBlock>(Fs, E, P, b); -// // now pack into an Hessian factor -// std::vector dims(nrUniqueKeys + 1); // this also includes the b term -// std::fill(dims.begin(), dims.end() - 1, 6); -// dims.back() = 1; -// SymmetricBlockMatrix augmentedHessianUniqueKeys; -// -// // here we have to deal with the fact that some cameras may share the same extrinsic key -// if (nrUniqueKeys == nrNonuniqueKeys) { // if there is 1 calibration key per camera -// augmentedHessianUniqueKeys = SymmetricBlockMatrix( -// dims, Matrix(augmentedHessian.selfadjointView())); -// } else { // if multiple cameras share a calibration we have to rearrange -// // the results of the Schur complement matrix -// std::vector nonuniqueDims(nrNonuniqueKeys + 1); // this also includes the b term -// std::fill(nonuniqueDims.begin(), nonuniqueDims.end() - 1, 6); -// nonuniqueDims.back() = 1; -// augmentedHessian = SymmetricBlockMatrix( -// nonuniqueDims, Matrix(augmentedHessian.selfadjointView())); -// -// // these are the keys that correspond to the blocks in augmentedHessian (output of SchurComplement) -// KeyVector nonuniqueKeys; -// for (size_t i = 0; i < world_P_body_key_pairs_.size(); i++) { -// nonuniqueKeys.push_back(world_P_body_key_pairs_.at(i)); -// nonuniqueKeys.push_back(body_P_cam_ this->keys_.at(i)); -// } -// -// // get map from key to location in the new augmented Hessian matrix (the one including only unique keys) -// std::map keyToSlotMap; -// for (size_t k = 0; k < nrUniqueKeys; k++) { -// keyToSlotMap[ this->keys_[k]] = k; -// } -// -// // initialize matrix to zero -// augmentedHessianUniqueKeys = SymmetricBlockMatrix( -// dims, Matrix::Zero(6 * nrUniqueKeys + 1, 6 * nrUniqueKeys + 1)); -// -// // add contributions for each key: note this loops over the hessian with nonUnique keys (augmentedHessian) -// // and populates an Hessian that only includes the unique keys (that is what we want to return) -// for (size_t i = 0; i < nrNonuniqueKeys; i++) { // rows -// Key key_i = nonuniqueKeys.at(i); -// -// // update information vector -// augmentedHessianUniqueKeys.updateOffDiagonalBlock( -// keyToSlotMap[key_i], nrUniqueKeys, -// augmentedHessian.aboveDiagonalBlock(i, nrNonuniqueKeys)); -// -// // update blocks -// for (size_t j = i; j < nrNonuniqueKeys; j++) { // cols -// Key key_j = nonuniqueKeys.at(j); -// if (i == j) { -// augmentedHessianUniqueKeys.updateDiagonalBlock( -// keyToSlotMap[key_i], augmentedHessian.diagonalBlock(i)); -// } else { // (i < j) -// if (keyToSlotMap[key_i] != keyToSlotMap[key_j]) { -// augmentedHessianUniqueKeys.updateOffDiagonalBlock( -// keyToSlotMap[key_i], keyToSlotMap[key_j], -// augmentedHessian.aboveDiagonalBlock(i, j)); -// } else { -// augmentedHessianUniqueKeys.updateDiagonalBlock( -// keyToSlotMap[key_i], -// augmentedHessian.aboveDiagonalBlock(i, j) -// + augmentedHessian.aboveDiagonalBlock(i, j).transpose()); -// } -// } -// } -// } -// // update bottom right element of the matrix -// augmentedHessianUniqueKeys.updateDiagonalBlock( -// nrUniqueKeys, augmentedHessian.diagonalBlock(nrNonuniqueKeys)); -// } -// return boost::make_shared < RegularHessianFactor -// > ( this->keys_, augmentedHessianUniqueKeys); + // // now pack into an Hessian factor + // std::vector dims(nrUniqueKeys + 1); // this also includes the b term + // std::fill(dims.begin(), dims.end() - 1, 6); + // dims.back() = 1; + // SymmetricBlockMatrix augmentedHessianUniqueKeys; + // + // // here we have to deal with the fact that some cameras may share the same extrinsic key + // if (nrUniqueKeys == nrNonuniqueKeys) { // if there is 1 calibration key per camera + // augmentedHessianUniqueKeys = SymmetricBlockMatrix( + // dims, Matrix(augmentedHessian.selfadjointView())); + // } else { // if multiple cameras share a calibration we have to rearrange + // // the results of the Schur complement matrix + // std::vector nonuniqueDims(nrNonuniqueKeys + 1); // this also includes the b term + // std::fill(nonuniqueDims.begin(), nonuniqueDims.end() - 1, 6); + // nonuniqueDims.back() = 1; + // augmentedHessian = SymmetricBlockMatrix( + // nonuniqueDims, Matrix(augmentedHessian.selfadjointView())); + // + // // these are the keys that correspond to the blocks in augmentedHessian (output of SchurComplement) + // KeyVector nonuniqueKeys; + // for (size_t i = 0; i < world_P_body_key_pairs_.size(); i++) { + // nonuniqueKeys.push_back(world_P_body_key_pairs_.at(i)); + // nonuniqueKeys.push_back(body_P_cam_ this->keys_.at(i)); + // } + // + // // get map from key to location in the new augmented Hessian matrix (the one including only unique keys) + // std::map keyToSlotMap; + // for (size_t k = 0; k < nrUniqueKeys; k++) { + // keyToSlotMap[ this->keys_[k]] = k; + // } + // + // // initialize matrix to zero + // augmentedHessianUniqueKeys = SymmetricBlockMatrix( + // dims, Matrix::Zero(6 * nrUniqueKeys + 1, 6 * nrUniqueKeys + 1)); + // + // // add contributions for each key: note this loops over the hessian with nonUnique keys (augmentedHessian) + // // and populates an Hessian that only includes the unique keys (that is what we want to return) + // for (size_t i = 0; i < nrNonuniqueKeys; i++) { // rows + // Key key_i = nonuniqueKeys.at(i); + // + // // update information vector + // augmentedHessianUniqueKeys.updateOffDiagonalBlock( + // keyToSlotMap[key_i], nrUniqueKeys, + // augmentedHessian.aboveDiagonalBlock(i, nrNonuniqueKeys)); + // + // // update blocks + // for (size_t j = i; j < nrNonuniqueKeys; j++) { // cols + // Key key_j = nonuniqueKeys.at(j); + // if (i == j) { + // augmentedHessianUniqueKeys.updateDiagonalBlock( + // keyToSlotMap[key_i], augmentedHessian.diagonalBlock(i)); + // } else { // (i < j) + // if (keyToSlotMap[key_i] != keyToSlotMap[key_j]) { + // augmentedHessianUniqueKeys.updateOffDiagonalBlock( + // keyToSlotMap[key_i], keyToSlotMap[key_j], + // augmentedHessian.aboveDiagonalBlock(i, j)); + // } else { + // augmentedHessianUniqueKeys.updateDiagonalBlock( + // keyToSlotMap[key_i], + // augmentedHessian.aboveDiagonalBlock(i, j) + // + augmentedHessian.aboveDiagonalBlock(i, j).transpose()); + // } + // } + // } + // } + // // update bottom right element of the matrix + // augmentedHessianUniqueKeys.updateDiagonalBlock( + // nrUniqueKeys, augmentedHessian.diagonalBlock(nrNonuniqueKeys)); + // } + // return boost::make_shared < RegularHessianFactor + // > ( this->keys_, augmentedHessianUniqueKeys); + } + + /** + * error calculates the error of the factor. + */ + double error(const Values& values) const override { + if (this->active(values)) { + return this->totalReprojectionError(this->cameras(values)); + } else { // else of active flag + return 0.0; + } + } + + /** + * Collect all cameras involved in this factor + * @param values Values structure which must contain camera poses + * corresponding to keys involved in this factor + * @return Cameras + */ + typename Base::Cameras cameras(const Values& values) const override { + size_t numViews = this->measured_.size(); + assert(world_P_body_keys_.size() == K_all_.size()); + assert(world_P_body_keys_.size() == body_P_cam_keys_.size()); + + typename Base::Cameras cameras; + for (size_t i = 0; i < numViews; i++) { // for each measurement + Pose3 w_P_body1 = values.at(world_P_body_key_pairs_[i].first); + Pose3 w_P_body2 = values.at(world_P_body_key_pairs_[i].second); + double interpolationFactor = gammas_[i]; + Pose3 w_P_body = interpolate(w_P_body1, w_P_body2, interpolationFactor); + Pose3 body_P_cam = body_P_sensors_[i]; + Pose3 w_P_cam = w_P_body.compose(body_P_cam); + std::cout << "id : " << i << std::endl; + w_P_cam.print("w_P_cam\n"); + cameras.emplace_back(w_P_cam, K_all_[i]); + } + return cameras; } /** @@ -466,7 +469,7 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor< /// linearize boost::shared_ptr linearize(const Values& values) const - override { + override { return linearizeDamped(values); } @@ -485,7 +488,7 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor< /// traits template struct traits > : - public Testable > { +public Testable > { }; } // namespace gtsam diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index b5bbf9c8b..850ea98fd 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -60,7 +60,6 @@ static double interp_factor3 = 0.5; // default Cal3_S2 poses with rolling shutter effect namespace vanillaPoseRS { typedef PinholePose Camera; -typedef SmartProjectionPoseFactor SmartFactor; static Cal3_S2::shared_ptr sharedK(new Cal3_S2(fov, w, h)); Pose3 interp_pose1 = interpolate(level_pose,pose_right,interp_factor1); Pose3 interp_pose2 = interpolate(pose_right,pose_above,interp_factor2); @@ -170,38 +169,26 @@ TEST( SmartProjectionPoseFactorRollingShutter, Equals ) { /* *************************************************************************/ TEST( SmartProjectionPoseFactorRollingShutter, noiselessError ) { - + std::cout << "============================== " << std::endl; using namespace vanillaPoseRS; -// // 2 poses such that level_pose_1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); -// // can be interpolated with interp_factor1 = 0.2: -// Pose3 level_pose1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 5)); -// Pose3 level_pose2 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0)); -// // 2 poses such that pose_right (Second camera 1 meter to the right of first camera) -// // can be interpolated with interp_factor1 = 0.4: -// Pose3 pose_right1 = level_pose * Pose3(Rot3(), Point3(1, 0, 0)); -// Pose3 pose_right2 = level_pose * Pose3(Rot3(), Point3(1, 0, 0)); -// // 2 poses such that pose_above (Third camera 1 meter above the first camera) -// // can be interpolated with interp_factor1 = 0.5: -// Pose3 pose_above1 = level_pose * Pose3(Rot3(), Point3(0, -1, 0)); -// Pose3 pose_above1 = level_pose * Pose3(Rot3(), Point3(0, -1, 0)); -// -// // Project two landmarks into two cameras -// Point2 level_uv = level_camera.project(landmark1); -// Point2 level_uv_right = level_camera_right.project(landmark1); -// Pose3 body_P_sensorId = Pose3::identity(); -// -// SmartFactor factor(model); -// factor.add(level_uv, x1, x2, interp_factor1, sharedK, body_P_sensorId); -// factor.add(level_uv_right, x2, x3, interp_factor2, sharedK, body_P_sensorId); -// -// Values values; // it's a pose factor, hence these are poses -// values.insert(x1, cam1.pose()); -// values.insert(x2, cam2.pose()); -// -// double actualError = factor.error(values); -// double expectedError = 0.0; -// EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); + // Project two landmarks into two cameras + Point2 level_uv = cam1.project(landmark1); + Point2 level_uv_right = cam2.project(landmark1); + Pose3 body_P_sensorId = Pose3::identity(); + + SmartFactorRS factor(model); + factor.add(level_uv, x1, x2, interp_factor1, sharedK, body_P_sensorId); + factor.add(level_uv_right, x2, x3, interp_factor2, sharedK, body_P_sensorId); + + Values values; // it's a pose factor, hence these are poses + values.insert(x1, level_pose); + values.insert(x2, pose_right); + values.insert(x3, pose_above); + + double actualError = factor.error(values); + double expectedError = 0.0; + EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); } /* ************************************************************************* From 4669213618802e07ef1eebb8e2c87a02bec53c5c Mon Sep 17 00:00:00 2001 From: lcarlone Date: Wed, 21 Jul 2021 16:19:44 -0400 Subject: [PATCH 088/248] jacobians are good to go! --- .../SmartProjectionPoseFactorRollingShutter.h | 12 +-- ...martProjectionPoseFactorRollingShutter.cpp | 100 ++++++++---------- 2 files changed, 48 insertions(+), 64 deletions(-) diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index d62d18712..18bb0e7a3 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -264,28 +264,26 @@ PinholePose > { Pose3 w_P_body2 = values.at(world_P_body_key_pairs_[i].second); double interpolationFactor = gammas_[i]; // get interpolated pose: - std::cout << "TODO: need to add proper interpolation and Jacobians here" << std::endl; - Pose3 w_P_body = w_P_body1.interpolateRt(w_P_body2, - interpolationFactor); /*dInterpPose_dPoseBody1, dInterpPose_dPoseBody2 */ + Pose3 w_P_body = interpolate(w_P_body1, w_P_body2,interpolationFactor, dInterpPose_dPoseBody1, dInterpPose_dPoseBody2); Pose3 body_P_cam = body_P_sensors_[i]; Pose3 w_P_cam = w_P_body.compose(body_P_cam, dPoseCam_dInterpPose); - PinholeCamera camera(w_P_cam, K_all_[i]); + PinholeCamera camera(w_P_cam, *K_all_[i]); // get jacobians and error vector for current measurement Point2 reprojectionError_i = Point2( camera.project(*this->result_, dProject_dPoseCam, Ei) - this->measured_.at(i)); Eigen::Matrix J; // 2 x 12 - J.block(0, 0) = dProject_dPoseCam * dPoseCam_dInterpPose + J.block(0, 0, ZDim, 6) = dProject_dPoseCam * dPoseCam_dInterpPose * dInterpPose_dPoseBody1; // (2x6) * (6x6) * (6x6) - J.block(0, 6) = dProject_dPoseCam * dPoseCam_dInterpPose + J.block(0, 6, ZDim, 6) = dProject_dPoseCam * dPoseCam_dInterpPose * dInterpPose_dPoseBody2; // (2x6) * (6x6) * (6x6) // fit into the output structures Fs.push_back(J); size_t row = 2 * i; b.segment(row) = -reprojectionError_i; - E.block<3, 3>(row, 0) = Ei; + E.block(row, 0) = Ei; } } } diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index 850ea98fd..1ff62d7c4 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -21,6 +21,7 @@ #include #include #include +#include #include #include #include @@ -45,6 +46,7 @@ static Symbol x1('X', 1); static Symbol x2('X', 2); static Symbol x3('X', 3); static Symbol x4('X', 4); +static Symbol l0('L', 0); static Pose3 body_P_sensor = Pose3(Rot3::Ypr(-0.1, 0.2, -0.2), Point3(0.1, 0.0, 0.0)); @@ -167,9 +169,14 @@ TEST( SmartProjectionPoseFactorRollingShutter, Equals ) { } } +static const int DimBlock = 12; ///< size of the variable stacking 2 poses from which the observation pose is interpolated +static const int DimPose = 6; ///< Pose3 dimension +static const int ZDim = 2; ///< Measurement dimension (Point2) +typedef Eigen::Matrix MatrixZD; // F blocks (derivatives wrt camera) +typedef std::vector > FBlocks; // vector of F blocks + /* *************************************************************************/ -TEST( SmartProjectionPoseFactorRollingShutter, noiselessError ) { - std::cout << "============================== " << std::endl; +TEST( SmartProjectionPoseFactorRollingShutter, noiselessErrorAndJacobians ) { using namespace vanillaPoseRS; // Project two landmarks into two cameras @@ -189,63 +196,42 @@ TEST( SmartProjectionPoseFactorRollingShutter, noiselessError ) { double actualError = factor.error(values); double expectedError = 0.0; EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); + + // Check triangulation + factor.triangulateSafe(factor.cameras(values)); + TriangulationResult point = factor.point(); + CHECK(point.valid()); // check triangulated point is valid + CHECK(assert_equal(landmark1, *point)); // check triangulation result matches expected 3D landmark + + // Check Jacobians + // -- actual Jacobians + FBlocks actualFs; + Matrix actualE; + Vector actualb; + factor.computeJacobiansWithTriangulatedPoint(actualFs, actualE, actualb, values); + CHECK(actualE.rows() == 4); CHECK(actualE.cols() == 3); + CHECK(actualb.rows() == 4); CHECK(actualb.cols() == 1); + CHECK(actualFs.size() == 2); + + // -- expected Jacobians from ProjectionFactorsRollingShutter + ProjectionFactorRollingShutter factor1(level_uv, interp_factor1, model, x1, x2, l0, sharedK, body_P_sensorId); + Matrix expectedF11, expectedF12, expectedE1; + Vector expectedb1 = factor1.evaluateError(level_pose, pose_right, landmark1, expectedF11, expectedF12, expectedE1); + CHECK(assert_equal( expectedF11, Matrix(actualFs[0].block(0,0,2,6)), 1e-5)); + CHECK(assert_equal( expectedF12, Matrix(actualFs[0].block(0,6,2,6)), 1e-5)); + CHECK(assert_equal( expectedE1, Matrix(actualE.block(0,0,2,3)), 1e-5)); + CHECK(assert_equal( expectedb1, Vector(actualb.segment<2>(0)), 1e-5)); + + ProjectionFactorRollingShutter factor2(level_uv_right, interp_factor2, model, x2, x3, l0, sharedK, body_P_sensorId); + Matrix expectedF21, expectedF22, expectedE2; + Vector expectedb2 = factor2.evaluateError(pose_right, pose_above, landmark1, expectedF21, expectedF22, expectedE2); + CHECK(assert_equal( expectedF21, Matrix(actualFs[1].block(0,0,2,6)), 1e-5)); + CHECK(assert_equal( expectedF22, Matrix(actualFs[1].block(0,6,2,6)), 1e-5)); + CHECK(assert_equal( expectedE2, Matrix(actualE.block(2,0,2,3)), 1e-5)); + CHECK(assert_equal( expectedb2, Vector(actualb.segment<2>(2)), 1e-5)); } - /* ************************************************************************* - TEST( SmartProjectionPoseFactorRollingShutter, Jacobians ) { - - using namespace vanillaPose; - - // Project two landmarks into two cameras - Point2 level_uv = level_camera.project(landmark1); - Point2 level_uv_right = level_camera_right.project(landmark1); - - SmartFactor factor(model, sharedK); - factor.add(level_uv, x1); - factor.add(level_uv_right, x2); - - Values values; // it's a pose factor, hence these are poses - values.insert(x1, cam1.pose()); - values.insert(x2, cam2.pose()); - - double actualError = factor.error(values); - double expectedError = 0.0; - EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); - - // Calculate expected derivative for point (easiest to check) - std::function f = // - std::bind(&SmartFactor::whitenedError, factor, cameras, std::placeholders::_1); - - // Calculate using computeEP - Matrix actualE; - factor.triangulateAndComputeE(actualE, values); - - // get point - boost::optional point = factor.point(); - CHECK(point); - - // calculate numerical derivative with triangulated point - Matrix expectedE = sigma * numericalDerivative11(f, *point); - EXPECT(assert_equal(expectedE, actualE, 1e-7)); - - // Calculate using reprojectionError - SmartFactor::Cameras::FBlocks F; - Matrix E; - Vector actualErrors = factor.unwhitenedError(cameras, *point, F, E); - EXPECT(assert_equal(expectedE, E, 1e-7)); - - EXPECT(assert_equal(Z_4x1, actualErrors, 1e-7)); - - // Calculate using computeJacobians - Vector b; - SmartFactor::FBlocks Fs; - factor.computeJacobians(Fs, E, b, cameras, *point); - double actualError3 = b.squaredNorm(); - EXPECT(assert_equal(expectedE, E, 1e-7)); - EXPECT_DOUBLES_EQUAL(expectedError, actualError3, 1e-6); - } - - /* ************************************************************************* +/* ************************************************************************* TEST( SmartProjectionPoseFactorRollingShutter, noisy ) { using namespace vanillaPose; From e6ff03f73e36daf988781357aded93894367a9d5 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Wed, 21 Jul 2021 16:31:45 -0400 Subject: [PATCH 089/248] jacobians and errors are well tested now --- ...martProjectionPoseFactorRollingShutter.cpp | 162 +++++++----------- 1 file changed, 60 insertions(+), 102 deletions(-) diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index 1ff62d7c4..43049ea61 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -220,7 +220,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, noiselessErrorAndJacobians ) { CHECK(assert_equal( expectedF11, Matrix(actualFs[0].block(0,0,2,6)), 1e-5)); CHECK(assert_equal( expectedF12, Matrix(actualFs[0].block(0,6,2,6)), 1e-5)); CHECK(assert_equal( expectedE1, Matrix(actualE.block(0,0,2,3)), 1e-5)); - CHECK(assert_equal( expectedb1, Vector(actualb.segment<2>(0)), 1e-5)); + // by definition computeJacobiansWithTriangulatedPoint returns minus reprojectionError + CHECK(assert_equal( expectedb1, -Vector(actualb.segment<2>(0)), 1e-5)); ProjectionFactorRollingShutter factor2(level_uv_right, interp_factor2, model, x2, x3, l0, sharedK, body_P_sensorId); Matrix expectedF21, expectedF22, expectedE2; @@ -228,117 +229,74 @@ TEST( SmartProjectionPoseFactorRollingShutter, noiselessErrorAndJacobians ) { CHECK(assert_equal( expectedF21, Matrix(actualFs[1].block(0,0,2,6)), 1e-5)); CHECK(assert_equal( expectedF22, Matrix(actualFs[1].block(0,6,2,6)), 1e-5)); CHECK(assert_equal( expectedE2, Matrix(actualE.block(2,0,2,3)), 1e-5)); - CHECK(assert_equal( expectedb2, Vector(actualb.segment<2>(2)), 1e-5)); + // by definition computeJacobiansWithTriangulatedPoint returns minus reprojectionError + CHECK(assert_equal( expectedb2, -Vector(actualb.segment<2>(2)), 1e-5)); } -/* ************************************************************************* - TEST( SmartProjectionPoseFactorRollingShutter, noisy ) { +/* *************************************************************************/ +TEST( SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians ) { + // also includes non-identical extrinsic calibration + using namespace vanillaPoseRS; - using namespace vanillaPose; + // Project two landmarks into two cameras + Point2 pixelError(0.5, 1.0); + Point2 level_uv = cam1.project(landmark1) + pixelError; + Point2 level_uv_right = cam2.project(landmark1); + Pose3 body_P_sensorNonId = body_P_sensor; - // Project two landmarks into two cameras - Point2 pixelError(0.2, 0.2); - Point2 level_uv = level_camera.project(landmark1) + pixelError; - Point2 level_uv_right = level_camera_right.project(landmark1); + SmartFactorRS factor(model); + factor.add(level_uv, x1, x2, interp_factor1, sharedK, body_P_sensorNonId); + factor.add(level_uv_right, x2, x3, interp_factor2, sharedK, body_P_sensorNonId); - Values values; - values.insert(x1, cam1.pose()); - Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), - Point3(0.5, 0.1, 0.3)); - values.insert(x2, pose_right.compose(noise_pose)); + Values values; // it's a pose factor, hence these are poses + values.insert(x1, level_pose); + values.insert(x2, pose_right); + values.insert(x3, pose_above); - SmartFactor::shared_ptr factor(new SmartFactor(model, sharedK)); - factor->add(level_uv, x1); - factor->add(level_uv_right, x2); + // Perform triangulation + factor.triangulateSafe(factor.cameras(values)); + TriangulationResult point = factor.point(); + CHECK(point.valid()); // check triangulated point is valid + Point3 landmarkNoisy = *point; - double actualError1 = factor->error(values); + // Check Jacobians + // -- actual Jacobians + FBlocks actualFs; + Matrix actualE; + Vector actualb; + factor.computeJacobiansWithTriangulatedPoint(actualFs, actualE, actualb, values); + CHECK(actualE.rows() == 4); CHECK(actualE.cols() == 3); + CHECK(actualb.rows() == 4); CHECK(actualb.cols() == 1); + CHECK(actualFs.size() == 2); - SmartFactor::shared_ptr factor2(new SmartFactor(model, sharedK)); - Point2Vector measurements; - measurements.push_back(level_uv); - measurements.push_back(level_uv_right); + // -- expected Jacobians from ProjectionFactorsRollingShutter + ProjectionFactorRollingShutter factor1(level_uv, interp_factor1, model, x1, x2, l0, sharedK, body_P_sensorNonId); + Matrix expectedF11, expectedF12, expectedE1; + Vector expectedb1 = factor1.evaluateError(level_pose, pose_right, landmarkNoisy, expectedF11, expectedF12, expectedE1); + CHECK(assert_equal( expectedF11, Matrix(actualFs[0].block(0,0,2,6)), 1e-5)); + CHECK(assert_equal( expectedF12, Matrix(actualFs[0].block(0,6,2,6)), 1e-5)); + CHECK(assert_equal( expectedE1, Matrix(actualE.block(0,0,2,3)), 1e-5)); + // by definition computeJacobiansWithTriangulatedPoint returns minus reprojectionError + CHECK(assert_equal( expectedb1, -Vector(actualb.segment<2>(0)), 1e-5)); - KeyVector views {x1, x2}; + ProjectionFactorRollingShutter factor2(level_uv_right, interp_factor2, model, x2, x3, l0, sharedK, body_P_sensorNonId); + Matrix expectedF21, expectedF22, expectedE2; + Vector expectedb2 = factor2.evaluateError(pose_right, pose_above, landmarkNoisy, expectedF21, expectedF22, expectedE2); + CHECK(assert_equal( expectedF21, Matrix(actualFs[1].block(0,0,2,6)), 1e-5)); + CHECK(assert_equal( expectedF22, Matrix(actualFs[1].block(0,6,2,6)), 1e-5)); + CHECK(assert_equal( expectedE2, Matrix(actualE.block(2,0,2,3)), 1e-5)); + // by definition computeJacobiansWithTriangulatedPoint returns minus reprojectionError + CHECK(assert_equal( expectedb2, -Vector(actualb.segment<2>(2)), 1e-5)); - factor2->add(measurements, views); - double actualError2 = factor2->error(values); - DOUBLES_EQUAL(actualError1, actualError2, 1e-7); - } - - /* ************************************************************************* - TEST(SmartProjectionPoseFactorRollingShutter, smartFactorWithSensorBodyTransform) { - using namespace vanillaPose; - - // create arbitrary body_T_sensor (transforms from sensor to body) - Pose3 body_T_sensor = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(1, 1, 1)); - - // These are the poses we want to estimate, from camera measurements - const Pose3 sensor_T_body = body_T_sensor.inverse(); - Pose3 wTb1 = cam1.pose() * sensor_T_body; - Pose3 wTb2 = cam2.pose() * sensor_T_body; - Pose3 wTb3 = cam3.pose() * sensor_T_body; - - // three landmarks ~5 meters infront of camera - Point3 landmark1(5, 0.5, 1.2), landmark2(5, -0.5, 1.2), landmark3(5, 0, 3.0); - - Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; - - // Project three landmarks into three cameras - projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); - projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); - projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - - // Create smart factors - KeyVector views {x1, x2, x3}; - - SmartProjectionParams params; - params.setRankTolerance(1.0); - params.setDegeneracyMode(IGNORE_DEGENERACY); - params.setEnableEPI(false); - - SmartFactor smartFactor1(model, sharedK, body_T_sensor, params); - smartFactor1.add(measurements_cam1, views); - - SmartFactor smartFactor2(model, sharedK, body_T_sensor, params); - smartFactor2.add(measurements_cam2, views); - - SmartFactor smartFactor3(model, sharedK, body_T_sensor, params); - smartFactor3.add(measurements_cam3, views); - - const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - - // Put all factors in factor graph, adding priors - NonlinearFactorGraph graph; - graph.push_back(smartFactor1); - graph.push_back(smartFactor2); - graph.push_back(smartFactor3); - graph.addPrior(x1, wTb1, noisePrior); - graph.addPrior(x2, wTb2, noisePrior); - - // Check errors at ground truth poses - Values gtValues; - gtValues.insert(x1, wTb1); - gtValues.insert(x2, wTb2); - gtValues.insert(x3, wTb3); - double actualError = graph.error(gtValues); - double expectedError = 0.0; - DOUBLES_EQUAL(expectedError, actualError, 1e-7) - - Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), - Point3(0.1, 0.1, 0.1)); - Values values; - values.insert(x1, wTb1); - values.insert(x2, wTb2); - // initialize third pose with some noise, we expect it to move back to - // original pose3 - values.insert(x3, wTb3 * noise_pose); - - LevenbergMarquardtParams lmParams; - Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); - result = optimizer.optimize(); - EXPECT(assert_equal(wTb3, result.at(x3))); - } + // Check errors + double actualError = factor.error(values); // from smart factor + NonlinearFactorGraph nfg; + nfg.add(factor1); + nfg.add(factor2); + values.insert(l0, landmarkNoisy); + double expectedError = nfg.error(values); + EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); +} /* ************************************************************************* TEST( SmartProjectionPoseFactorRollingShutter, 3poses_smart_projection_factor ) { From d4b88ba59ad63bc51ce0147d8478dc5bdcabab30 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Wed, 21 Jul 2021 22:46:42 -0400 Subject: [PATCH 090/248] got to the final monster. Now I need to implement createHessian --- .../SmartProjectionPoseFactorRollingShutter.h | 8 +- ...martProjectionPoseFactorRollingShutter.cpp | 104 +++++++++--------- 2 files changed, 56 insertions(+), 56 deletions(-) diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index 18bb0e7a3..8bc923e20 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -98,7 +98,7 @@ PinholePose > { */ void add(const Point2& measured, const Key& world_P_body_key1, const Key& world_P_body_key2, const double& gamma, - const boost::shared_ptr& K, const Pose3 body_P_sensor) { + const boost::shared_ptr& K, const Pose3 body_P_sensor = Pose3::identity()) { // store measurements in base class (note: we manyally add keys below to make sure they are unique this->measured_.push_back(measured); @@ -131,7 +131,7 @@ PinholePose > { * @param world_P_body_key_pairs vector of (1 for each view) containing the pair of poses from which each view can be interpolated * @param Ks vector of intrinsic calibration objects */ - void add(const std::vector& measurements, + void add(const Point2Vector& measurements, const std::vector>& world_P_body_key_pairs, const std::vector& gammas, const std::vector>& Ks, @@ -154,10 +154,10 @@ PinholePose > { * @param world_P_body_key_pairs vector of (1 for each view) containing the pair of poses from which each view can be interpolated * @param K the (known) camera calibration (same for all measurements) */ - void add(const std::vector& measurements, + void add(const Point2Vector& measurements, const std::vector>& world_P_body_key_pairs, const std::vector& gammas, - const boost::shared_ptr& K, const Pose3 body_P_sensor) { + const boost::shared_ptr& K, const Pose3 body_P_sensor = Pose3::identity()) { assert(world_P_body_key_pairs.size() == measurements.size()); assert(world_P_body_key_pairs.size() == gammas.size()); for (size_t i = 0; i < measurements.size(); i++) { diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index 43049ea61..d04921424 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -98,7 +98,7 @@ TEST( SmartProjectionPoseFactorRollingShutter, Equals ) { using namespace vanillaPose; // create fake measurements - std::vector measurements; + Point2Vector measurements; measurements.push_back(measurement1); measurements.push_back(measurement2); measurements.push_back(measurement3); @@ -170,7 +170,6 @@ TEST( SmartProjectionPoseFactorRollingShutter, Equals ) { } static const int DimBlock = 12; ///< size of the variable stacking 2 poses from which the observation pose is interpolated -static const int DimPose = 6; ///< Pose3 dimension static const int ZDim = 2; ///< Measurement dimension (Point2) typedef Eigen::Matrix MatrixZD; // F blocks (derivatives wrt camera) typedef std::vector > FBlocks; // vector of F blocks @@ -298,68 +297,69 @@ TEST( SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians ) { EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); } - /* ************************************************************************* - TEST( SmartProjectionPoseFactorRollingShutter, 3poses_smart_projection_factor ) { +/* *************************************************************************/ +TEST( SmartProjectionPoseFactorRollingShutter, 3poses_smart_projection_factor ) { + std::cout << "===================" << std::endl; - using namespace vanillaPose2; - Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; + using namespace vanillaPoseRS; + Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; - // Project three landmarks into three cameras - projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); - projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); - projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - KeyVector views; - views.push_back(x1); - views.push_back(x2); - views.push_back(x3); + // create inputs + std::vector> key_pairs; + key_pairs.push_back(std::make_pair(x1,x2)); + key_pairs.push_back(std::make_pair(x2,x3)); + key_pairs.push_back(std::make_pair(x3,x1)); - SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedK2)); - smartFactor1->add(measurements_cam1, views); + std::vector interp_factors; + interp_factors.push_back(interp_factor1); + interp_factors.push_back(interp_factor2); + interp_factors.push_back(interp_factor3); - SmartFactor::shared_ptr smartFactor2(new SmartFactor(model, sharedK2)); - smartFactor2->add(measurements_cam2, views); + SmartFactorRS smartFactor1(model); + smartFactor1.add(measurements_cam1, key_pairs, interp_factors, sharedK); - SmartFactor::shared_ptr smartFactor3(new SmartFactor(model, sharedK2)); - smartFactor3->add(measurements_cam3, views); + SmartFactorRS smartFactor2(model); + smartFactor2.add(measurements_cam2, key_pairs, interp_factors, sharedK); - const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + SmartFactorRS smartFactor3(model); + smartFactor3.add(measurements_cam3, key_pairs, interp_factors, sharedK); - NonlinearFactorGraph graph; - graph.push_back(smartFactor1); - graph.push_back(smartFactor2); - graph.push_back(smartFactor3); - graph.addPrior(x1, cam1.pose(), noisePrior); - graph.addPrior(x2, cam2.pose(), noisePrior); + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - Values groundTruth; - groundTruth.insert(x1, cam1.pose()); - groundTruth.insert(x2, cam2.pose()); - groundTruth.insert(x3, cam3.pose()); - DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9); + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.addPrior(x1, level_pose, noisePrior); + graph.addPrior(x2, pose_right, noisePrior); - // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), - Point3(0.1, 0.1, 0.1)); // smaller noise - Values values; - values.insert(x1, cam1.pose()); - values.insert(x2, cam2.pose()); - // initialize third pose with some noise, we expect it to move back to original pose_above - values.insert(x3, pose_above * noise_pose); - EXPECT( - assert_equal( - Pose3( - Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, - -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), - Point3(0.1, -0.1, 1.9)), values.at(x3))); + Values groundTruth; + groundTruth.insert(x1, level_pose); + groundTruth.insert(x2, pose_right); + groundTruth.insert(x3, pose_above); + DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9); - Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); - result = optimizer.optimize(); - EXPECT(assert_equal(pose_above, result.at(x3), 1e-6)); - } + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise + Values values; + values.insert(x1, level_pose); + values.insert(x2, pose_right); + // initialize third pose with some noise, we expect it to move back to original pose_above + values.insert(x3, pose_above * noise_pose); - /* ************************************************************************* + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + EXPECT(assert_equal(pose_above, result.at(x3), 1e-6)); +} + +/* ************************************************************************* TEST( SmartProjectionPoseFactorRollingShutter, Factors ) { using namespace vanillaPose; From a439cf0f0fe9858852d6a5c519db4da8eccb1d9a Mon Sep 17 00:00:00 2001 From: lcarlone Date: Wed, 21 Jul 2021 23:33:42 -0400 Subject: [PATCH 091/248] stuck on compile issue --- .../SmartProjectionPoseFactorRollingShutter.h | 76 +++++++++++-------- ...martProjectionPoseFactorRollingShutter.cpp | 6 ++ 2 files changed, 50 insertions(+), 32 deletions(-) diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index 8bc923e20..a9f6d6bdf 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -18,6 +18,9 @@ #pragma once #include +#include +#include +#include namespace gtsam { /** @@ -58,6 +61,8 @@ PinholePose > { EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef PinholePose Camera; + // typedef CameraSet Cameras; + /// shorthand for base class type typedef SmartProjectionFactor Base; @@ -289,13 +294,12 @@ PinholePose > { } /// linearize and return a Hessianfactor that is an approximation of error(p) - boost::shared_ptr > createHessianFactor( + boost::shared_ptr > createHessianFactor( const Values& values, const double lambda = 0.0, bool diagonalDamping = false) const { - // we may have multiple cameras sharing the same extrinsic cals, hence the number - // of keys may be smaller than 2 * nrMeasurements (which is the upper bound where we - // have a body key and an extrinsic calibration key for each measurement) + // we may have multiple observation sharing the same keys (due to the rolling shutter interpolation), + // hence the number of unique keys may be smaller than 2 * nrMeasurements size_t nrUniqueKeys = this->keys_.size(); size_t nrNonuniqueKeys = 2*world_P_body_key_pairs_.size(); @@ -304,37 +308,38 @@ PinholePose > { std::vector < Matrix > Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2); std::vector gs(nrUniqueKeys); - if (this->measured_.size() != cameras(values).size()) + if (this->measured_.size() != this->cameras(values).size()) throw std::runtime_error("SmartProjectionPoseFactorRollingShutter: " "measured_.size() inconsistent with input"); - // // triangulate 3D point at given linearization point - // triangulateSafe(cameras(values)); - // - // if (!this->result_) { // failed: return "empty/zero" Hessian - // for (Matrix& m : Gs) - // m = Matrix::Zero(DimPose, DimPose); - // for (Vector& v : gs) - // v = Vector::Zero(DimPose); - // return boost::make_shared < RegularHessianFactor - // > ( this->keys_, Gs, gs, 0.0); - // } - // - // // compute Jacobian given triangulated 3D Point - // FBlocks Fs; - // Matrix F, E; - // Vector b; - // computeJacobiansWithTriangulatedPoint(Fs, E, b, values); - // - // // Whiten using noise model - // this->noiseModel_->WhitenSystem(E, b); - // for (size_t i = 0; i < Fs.size(); i++) - // Fs[i] = this->noiseModel_->Whiten(Fs[i]); - // - // // build augmented Hessian (with last row/column being the information vector) - // Matrix3 P; - // This::Cameras::ComputePointCovariance<3>(P, E, lambda, diagonalDamping); - // + std::cout << "got this far.." << std::endl; + + // triangulate 3D point at given linearization point + this->triangulateSafe(this->cameras(values)); + + if (!this->result_) { // failed: return "empty/zero" Hessian + for (Matrix& m : Gs) + m = Matrix::Zero(DimPose, DimPose); + for (Vector& v : gs) + v = Vector::Zero(DimPose); + return boost::make_shared < RegularHessianFactor > ( this->keys_, Gs, gs, 0.0); + } + + // compute Jacobian given triangulated 3D Point + FBlocks Fs; + Matrix F, E; + Vector b; + this->computeJacobiansWithTriangulatedPoint(Fs, E, b, values); + + // Whiten using noise model + this->noiseModel_->WhitenSystem(E, b); + for (size_t i = 0; i < Fs.size(); i++) + Fs[i] = this->noiseModel_->Whiten(Fs[i]); + + // build augmented Hessian (with last row/column being the information vector) + Matrix3 P; + Cameras::ComputePointCovariance<3>(P, E, lambda, diagonalDamping); + // // marginalize point: note - we reuse the standard SchurComplement function // SymmetricBlockMatrix augmentedHessian = This::Cameras::SchurComplement<2,DimBlock>(Fs, E, P, b); @@ -409,6 +414,13 @@ PinholePose > { // } // return boost::make_shared < RegularHessianFactor // > ( this->keys_, augmentedHessianUniqueKeys); + + // TO REMOVE: + for (Matrix& m : Gs) + m = Matrix::Zero(DimPose, DimPose); + for (Vector& v : gs) + v = Vector::Zero(DimPose); + return boost::make_shared < RegularHessianFactor > ( this->keys_, Gs, gs, 0.0); } /** diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index d04921424..602ca155e 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -352,6 +352,12 @@ TEST( SmartProjectionPoseFactorRollingShutter, 3poses_smart_projection_factor ) values.insert(x2, pose_right); // initialize third pose with some noise, we expect it to move back to original pose_above values.insert(x3, pose_above * noise_pose); + EXPECT( // check that the pose is actually noisy + assert_equal( + Pose3( + Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, + -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), + Point3(0.1, -0.1, 1.9)), values.at(x3))); Values result; LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); From 3a5e71584874d6cc86a17a106ba806a90f50e3a5 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 22 Jul 2021 11:51:22 -0400 Subject: [PATCH 092/248] Squashed 'wrap/' changes from d9ae5ce03..571c23952 571c23952 Merge pull request #119 from borglab/feature/remove-loguru 0e5178251 remove loguru as a dependency 5b76595cf fix type info and do some refactoring git-subtree-dir: wrap git-subtree-split: 571c2395242e33dfd0596a240fbcb87775b9ba0c --- gtwrap/interface_parser/classes.py | 14 ++-- gtwrap/interface_parser/function.py | 2 +- gtwrap/interface_parser/type.py | 6 +- gtwrap/matlab_wrapper/mixins.py | 109 +++++++++++++--------------- gtwrap/matlab_wrapper/wrapper.py | 7 +- gtwrap/template_instantiator.py | 11 ++- requirements.txt | 1 - tests/test_matlab_wrapper.py | 6 -- 8 files changed, 68 insertions(+), 88 deletions(-) diff --git a/gtwrap/interface_parser/classes.py b/gtwrap/interface_parser/classes.py index 3e6a0fc3c..11317962d 100644 --- a/gtwrap/interface_parser/classes.py +++ b/gtwrap/interface_parser/classes.py @@ -10,7 +10,7 @@ Parser classes and rules for parsing C++ classes. Author: Duy Nguyen Ta, Fan Jiang, Matthew Sklar, Varun Agrawal, and Frank Dellaert """ -from typing import Iterable, List, Union +from typing import Any, Iterable, List, Union from pyparsing import Literal, Optional, ZeroOrMore # type: ignore @@ -48,12 +48,12 @@ class Method: args_list, t.is_const)) def __init__(self, - template: str, + template: Union[Template, Any], name: str, return_type: ReturnType, args: ArgumentList, is_const: str, - parent: Union[str, "Class"] = ''): + parent: Union["Class", Any] = ''): self.template = template self.name = name self.return_type = return_type @@ -98,7 +98,7 @@ class StaticMethod: name: str, return_type: ReturnType, args: ArgumentList, - parent: Union[str, "Class"] = ''): + parent: Union["Class", Any] = ''): self.name = name self.return_type = return_type self.args = args @@ -129,7 +129,7 @@ class Constructor: def __init__(self, name: str, args: ArgumentList, - parent: Union["Class", str] = ''): + parent: Union["Class", Any] = ''): self.name = name self.args = args @@ -167,7 +167,7 @@ class Operator: return_type: ReturnType, args: ArgumentList, is_const: str, - parent: Union[str, "Class"] = ''): + parent: Union["Class", Any] = ''): self.name = name self.operator = operator self.return_type = return_type @@ -284,7 +284,7 @@ class Class: properties: List[Variable], operators: List[Operator], enums: List[Enum], - parent: str = '', + parent: Any = '', ): self.template = template self.is_virtual = is_virtual diff --git a/gtwrap/interface_parser/function.py b/gtwrap/interface_parser/function.py index 995aba10e..9fe1f56f0 100644 --- a/gtwrap/interface_parser/function.py +++ b/gtwrap/interface_parser/function.py @@ -169,7 +169,7 @@ class GlobalFunction: return_type: ReturnType, args_list: ArgumentList, template: Template, - parent: str = ''): + parent: Any = ''): self.name = name self.return_type = return_type self.args = args_list diff --git a/gtwrap/interface_parser/type.py b/gtwrap/interface_parser/type.py index 0b9be6501..49315cc56 100644 --- a/gtwrap/interface_parser/type.py +++ b/gtwrap/interface_parser/type.py @@ -12,7 +12,7 @@ Author: Duy Nguyen Ta, Fan Jiang, Matthew Sklar, Varun Agrawal, and Frank Dellae # pylint: disable=unnecessary-lambda, expression-not-assigned -from typing import Iterable, List, Union +from typing import List, Sequence, Union from pyparsing import (Forward, Optional, Or, ParseResults, # type: ignore delimitedList) @@ -49,12 +49,12 @@ class Typename: def __init__(self, t: ParseResults, - instantiations: Iterable[ParseResults] = ()): + instantiations: Sequence[ParseResults] = ()): self.name = t[-1] # the name is the last element in this list self.namespaces = t[:-1] if instantiations: - if isinstance(instantiations, Iterable): + if isinstance(instantiations, Sequence): self.instantiations = instantiations # type: ignore else: self.instantiations = instantiations.asList() diff --git a/gtwrap/matlab_wrapper/mixins.py b/gtwrap/matlab_wrapper/mixins.py index 061cea283..217801ff3 100644 --- a/gtwrap/matlab_wrapper/mixins.py +++ b/gtwrap/matlab_wrapper/mixins.py @@ -1,5 +1,7 @@ """Mixins for reducing the amount of boilerplate in the main wrapper class.""" +from typing import Any, Tuple, Union + import gtwrap.interface_parser as parser import gtwrap.template_instantiator as instantiator @@ -7,13 +9,14 @@ import gtwrap.template_instantiator as instantiator class CheckMixin: """Mixin to provide various checks.""" # Data types that are primitive types - not_ptr_type = ['int', 'double', 'bool', 'char', 'unsigned char', 'size_t'] + not_ptr_type: Tuple = ('int', 'double', 'bool', 'char', 'unsigned char', + 'size_t') # Ignore the namespace for these datatypes - ignore_namespace = ['Matrix', 'Vector', 'Point2', 'Point3'] + ignore_namespace: Tuple = ('Matrix', 'Vector', 'Point2', 'Point3') # Methods that should be ignored - ignore_methods = ['pickle'] + ignore_methods: Tuple = ('pickle', ) # Methods that should not be wrapped directly - whitelist = ['serializable', 'serialize'] + whitelist: Tuple = ('serializable', 'serialize') # Datatypes that do not need to be checked in methods not_check_type: list = [] @@ -23,7 +26,7 @@ class CheckMixin: return True return False - def is_shared_ptr(self, arg_type): + def is_shared_ptr(self, arg_type: parser.Type): """ Determine if the `interface_parser.Type` should be treated as a shared pointer in the wrapper. @@ -33,7 +36,7 @@ class CheckMixin: and arg_type.typename.name not in self.ignore_namespace and arg_type.typename.name != 'string') - def is_ptr(self, arg_type): + def is_ptr(self, arg_type: parser.Type): """ Determine if the `interface_parser.Type` should be treated as a raw pointer in the wrapper. @@ -43,7 +46,7 @@ class CheckMixin: and arg_type.typename.name not in self.ignore_namespace and arg_type.typename.name != 'string') - def is_ref(self, arg_type): + def is_ref(self, arg_type: parser.Type): """ Determine if the `interface_parser.Type` should be treated as a reference in the wrapper. @@ -55,7 +58,14 @@ class CheckMixin: class FormatMixin: """Mixin to provide formatting utilities.""" - def _clean_class_name(self, instantiated_class): + + ignore_namespace: tuple + data_type: Any + data_type_param: Any + _return_count: Any + + def _clean_class_name(self, + instantiated_class: instantiator.InstantiatedClass): """Reformatted the C++ class name to fit Matlab defined naming standards """ @@ -65,23 +75,23 @@ class FormatMixin: return instantiated_class.name def _format_type_name(self, - type_name, - separator='::', - include_namespace=True, - constructor=False, - method=False): + type_name: parser.Typename, + separator: str = '::', + include_namespace: bool = True, + is_constructor: bool = False, + is_method: bool = False): """ Args: type_name: an interface_parser.Typename to reformat separator: the statement to add between namespaces and typename include_namespace: whether to include namespaces when reformatting - constructor: if the typename will be in a constructor - method: if the typename will be in a method + is_constructor: if the typename will be in a constructor + is_method: if the typename will be in a method Raises: constructor and method cannot both be true """ - if constructor and method: + if is_constructor and is_method: raise ValueError( 'Constructor and method parameters cannot both be True') @@ -93,9 +103,9 @@ class FormatMixin: if name not in self.ignore_namespace and namespace != '': formatted_type_name += namespace + separator - if constructor: + if is_constructor: formatted_type_name += self.data_type.get(name) or name - elif method: + elif is_method: formatted_type_name += self.data_type_param.get(name) or name else: formatted_type_name += name @@ -106,8 +116,8 @@ class FormatMixin: template = '{}'.format( self._format_type_name(type_name.instantiations[idx], include_namespace=include_namespace, - constructor=constructor, - method=method)) + is_constructor=is_constructor, + is_method=is_method)) templates.append(template) if len(templates) > 0: # If there are no templates @@ -119,15 +129,15 @@ class FormatMixin: self._format_type_name(type_name.instantiations[idx], separator=separator, include_namespace=False, - constructor=constructor, - method=method)) + is_constructor=is_constructor, + is_method=is_method)) return formatted_type_name def _format_return_type(self, - return_type, - include_namespace=False, - separator="::"): + return_type: parser.function.ReturnType, + include_namespace: bool = False, + separator: str = "::"): """Format return_type. Args: @@ -154,18 +164,15 @@ class FormatMixin: return return_wrap - def _format_class_name(self, instantiated_class, separator=''): + def _format_class_name(self, + instantiated_class: instantiator.InstantiatedClass, + separator: str = ''): """Format a template_instantiator.InstantiatedClass name.""" if instantiated_class.parent == '': parent_full_ns = [''] else: parent_full_ns = instantiated_class.parent.full_namespaces() - # class_name = instantiated_class.parent.name - # - # if class_name != '': - # class_name += separator - # - # class_name += instantiated_class.name + parentname = "".join([separator + x for x in parent_full_ns]) + separator @@ -175,10 +182,12 @@ class FormatMixin: return class_name - def _format_static_method(self, static_method, separator=''): - """Example: - - gtsamPoint3.staticFunction + def _format_static_method(self, + static_method: parser.StaticMethod, + separator: str = ''): + """ + Example: + gtsam.Point3.staticFunction() """ method = '' @@ -188,35 +197,17 @@ class FormatMixin: return method[2 * len(separator):] - def _format_instance_method(self, instance_method, separator=''): + def _format_global_function(self, + function: Union[parser.GlobalFunction, Any], + separator: str = ''): """Example: gtsamPoint3.staticFunction """ method = '' - if isinstance(instance_method, instantiator.InstantiatedMethod): - method_list = [ - separator + x - for x in instance_method.parent.parent.full_namespaces() - ] - method += "".join(method_list) + separator - - method += instance_method.parent.name + separator - method += instance_method.original.name - method += "<" + instance_method.instantiations.to_cpp() + ">" - - return method[2 * len(separator):] - - def _format_global_method(self, static_method, separator=''): - """Example: - - gtsamPoint3.staticFunction - """ - method = '' - - if isinstance(static_method, parser.GlobalFunction): - method += "".join([separator + x for x in static_method.parent.full_namespaces()]) + \ + if isinstance(function, parser.GlobalFunction): + method += "".join([separator + x for x in function.parent.full_namespaces()]) + \ separator return method[2 * len(separator):] diff --git a/gtwrap/matlab_wrapper/wrapper.py b/gtwrap/matlab_wrapper/wrapper.py index b040d2731..97945f73a 100755 --- a/gtwrap/matlab_wrapper/wrapper.py +++ b/gtwrap/matlab_wrapper/wrapper.py @@ -11,8 +11,6 @@ import textwrap from functools import partial, reduce from typing import Dict, Iterable, List, Union -from loguru import logger - import gtwrap.interface_parser as parser import gtwrap.template_instantiator as instantiator from gtwrap.matlab_wrapper.mixins import CheckMixin, FormatMixin @@ -200,7 +198,7 @@ class MatlabWrapper(CheckMixin, FormatMixin): check_type = self._format_type_name( arg.ctype.typename, separator='.', - constructor=not wrap_datatypes) + is_constructor=not wrap_datatypes) var_arg_wrap += " && isa(varargin{{{num}}},'{data_type}')".format( num=i, data_type=check_type) @@ -1090,11 +1088,10 @@ class MatlabWrapper(CheckMixin, FormatMixin): if method.instantiations: # method_name += '<{}>'.format( # self._format_type_name(method.instantiations)) - # method_name = self._format_instance_method(method, '::') method = method.to_cpp() elif isinstance(method, parser.GlobalFunction): - method_name = self._format_global_method(method, '::') + method_name = self._format_global_function(method, '::') method_name += method.name else: diff --git a/gtwrap/template_instantiator.py b/gtwrap/template_instantiator.py index 87729cfa6..0cda93d5d 100644 --- a/gtwrap/template_instantiator.py +++ b/gtwrap/template_instantiator.py @@ -4,7 +4,7 @@ import itertools from copy import deepcopy -from typing import Iterable, List +from typing import Any, Iterable, List, Sequence import gtwrap.interface_parser as parser @@ -214,17 +214,17 @@ class InstantiatedMethod(parser.Method): } """ def __init__(self, - original, + original: parser.Method, instantiations: Iterable[parser.Typename] = ()): self.original = original self.instantiations = instantiations - self.template = '' + self.template: Any = '' self.is_const = original.is_const self.parent = original.parent # Check for typenames if templated. # This way, we can gracefully handle both templated and non-templated methods. - typenames = self.original.template.typenames if self.original.template else [] + typenames: Sequence = self.original.template.typenames if self.original.template else [] self.name = instantiate_name(original.name, self.instantiations) self.return_type = instantiate_return_type( original.return_type, @@ -348,13 +348,12 @@ class InstantiatedClass(parser.Class): return "{virtual}Class {cpp_class} : {parent_class}\n"\ "{ctors}\n{static_methods}\n{methods}\n{operators}".format( virtual="virtual " if self.is_virtual else '', - name=self.name, cpp_class=self.to_cpp(), parent_class=self.parent, ctors="\n".join([repr(ctor) for ctor in self.ctors]), - methods="\n".join([repr(m) for m in self.methods]), static_methods="\n".join([repr(m) for m in self.static_methods]), + methods="\n".join([repr(m) for m in self.methods]), operators="\n".join([repr(op) for op in self.operators]) ) diff --git a/requirements.txt b/requirements.txt index dd24ea4ed..a7181b271 100644 --- a/requirements.txt +++ b/requirements.txt @@ -1,3 +1,2 @@ pyparsing pytest -loguru \ No newline at end of file diff --git a/tests/test_matlab_wrapper.py b/tests/test_matlab_wrapper.py index fad4de16a..112750721 100644 --- a/tests/test_matlab_wrapper.py +++ b/tests/test_matlab_wrapper.py @@ -11,8 +11,6 @@ import os.path as osp import sys import unittest -from loguru import logger - sys.path.append(osp.dirname(osp.dirname(osp.abspath(__file__)))) from gtwrap.matlab_wrapper import MatlabWrapper @@ -44,10 +42,6 @@ class TestWrap(unittest.TestCase): # Create the `actual/matlab` directory os.makedirs(self.MATLAB_ACTUAL_DIR, exist_ok=True) - # set the log level to INFO by default - logger.remove() # remove the default sink - logger.add(sys.stderr, format="{time} {level} {message}", level="INFO") - def compare_and_diff(self, file): """ Compute the comparison between the expected and actual file, From d7e8912d6a7c37c3986661033d294a2d2dee8863 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Thu, 22 Jul 2021 21:45:26 -0400 Subject: [PATCH 093/248] all pass! --- gtsam/geometry/CameraSet.h | 63 +++++- .../SmartProjectionPoseFactorRollingShutter.h | 185 +++++++++--------- .../slam/SmartStereoProjectionFactorPP.h | 2 +- 3 files changed, 152 insertions(+), 98 deletions(-) diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h index 7d2f818fa..fcbff020c 100644 --- a/gtsam/geometry/CameraSet.h +++ b/gtsam/geometry/CameraSet.h @@ -142,14 +142,67 @@ public: return ErrorVector(project2(point, Fs, E), measured); } + static SymmetricBlockMatrix mySchurComplement( + const std::vector< Eigen::Matrix, + Eigen::aligned_allocator< Eigen::Matrix > >& Fs, + const Matrix& E, const Eigen::Matrix& P, const Vector& b) { + return mySchurComplement<2,3,12>(Fs, E, P, b); + } + + template // N = 2 or 3 (point dimension), ND is the camera dimension + static SymmetricBlockMatrix mySchurComplement( + const std::vector< Eigen::Matrix, + Eigen::aligned_allocator< Eigen::Matrix > >& Fs, + const Matrix& E, const Eigen::Matrix& P, const Vector& b) { + + // a single point is observed in m cameras + size_t m = Fs.size(); + + // Create a SymmetricBlockMatrix (augmented hessian, with extra row/column with info vector) + size_t M1 = ND * m + 1; + std::vector dims(m + 1); // this also includes the b term + std::fill(dims.begin(), dims.end() - 1, ND); + dims.back() = 1; + SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(M1, M1)); + + // Blockwise Schur complement + for (size_t i = 0; i < m; i++) { // for each camera + + const Eigen::Matrix& Fi = Fs[i]; + const auto FiT = Fi.transpose(); + const Eigen::Matrix Ei_P = // + E.block(myZDim * i, 0, myZDim, N) * P; + + // D = (Dx2) * ZDim + augmentedHessian.setOffDiagonalBlock(i, m, FiT * b.segment(myZDim * i) // F' * b + - FiT * (Ei_P * (E.transpose() * b))); // D = (DxZDim) * (ZDimx3) * (N*ZDimm) * (ZDimm x 1) + + // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) ) + augmentedHessian.setDiagonalBlock(i, FiT + * (Fi - Ei_P * E.block(myZDim * i, 0, myZDim, N).transpose() * Fi)); + + // upper triangular part of the hessian + for (size_t j = i + 1; j < m; j++) { // for each camera + const Eigen::Matrix& Fj = Fs[j]; + + // (DxD) = (Dx2) * ( (2x2) * (2xD) ) + augmentedHessian.setOffDiagonalBlock(i, j, -FiT + * (Ei_P * E.block(myZDim * j, 0, myZDim, N).transpose() * Fj)); + } + } // end of for over cameras + + augmentedHessian.diagonalBlock(m)(0, 0) += b.squaredNorm(); + return augmentedHessian; + } + /** * Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix * G = F' * F - F' * E * P * E' * F * g = F' * (b - E * P * E' * b) * Fixed size version */ - template // N = 2 or 3, ND is the camera dimension - static SymmetricBlockMatrix SchurComplement( + template // N = 2 or 3 (point dimension), ND is the camera dimension + static SymmetricBlockMatrix SchurComplementWithCustomBlocks( const std::vector< Eigen::Matrix, Eigen::aligned_allocator< Eigen::Matrix > >& Fs, const Matrix& E, const Eigen::Matrix& P, const Vector& b) { @@ -202,11 +255,11 @@ public: template // N = 2 or 3 static SymmetricBlockMatrix SchurComplement(const FBlocks& Fs, const Matrix& E, const Eigen::Matrix& P, const Vector& b) { - return SchurComplement(Fs, E, P, b); + return SchurComplementWithCustomBlocks(Fs, E, P, b); } /// Computes Point Covariance P, with lambda parameter - template // N = 2 or 3 + template // N = 2 or 3 (point dimension) static void ComputePointCovariance(Eigen::Matrix& P, const Matrix& E, double lambda, bool diagonalDamping = false) { @@ -258,7 +311,7 @@ public: * Applies Schur complement (exploiting block structure) to get a smart factor on cameras, * and adds the contribution of the smart factor to a pre-allocated augmented Hessian. */ - template // N = 2 or 3 + template // N = 2 or 3 (point dimension) static void UpdateSchurComplement(const FBlocks& Fs, const Matrix& E, const Eigen::Matrix& P, const Vector& b, const KeyVector& allKeys, const KeyVector& keys, diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index a9f6d6bdf..472b6348a 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -17,10 +17,10 @@ #pragma once -#include -#include -#include #include +#include +#include +#include namespace gtsam { /** @@ -60,11 +60,10 @@ PinholePose > { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW - typedef PinholePose Camera; - // typedef CameraSet Cameras; - /// shorthand for base class type - typedef SmartProjectionFactor Base; + typedef SmartProjectionFactor > Base; +// typedef PinholePose Camera; +// typedef CameraSet Cameras; /// shorthand for this class typedef SmartProjectionPoseFactorRollingShutter This; @@ -312,8 +311,6 @@ PinholePose > { throw std::runtime_error("SmartProjectionPoseFactorRollingShutter: " "measured_.size() inconsistent with input"); - std::cout << "got this far.." << std::endl; - // triangulate 3D point at given linearization point this->triangulateSafe(this->cameras(values)); @@ -336,91 +333,95 @@ PinholePose > { for (size_t i = 0; i < Fs.size(); i++) Fs[i] = this->noiseModel_->Whiten(Fs[i]); + Matrix3 P = Base::Cameras::PointCov(E, lambda, diagonalDamping); + // build augmented Hessian (with last row/column being the information vector) - Matrix3 P; - Cameras::ComputePointCovariance<3>(P, E, lambda, diagonalDamping); + // and marginalize point: note - we reuse the standard SchurComplement function + // does not work, since assumes convensional 6-dim blocks + // SymmetricBlockMatrix augmentedHessian = + // Base::Cameras::SchurComplement(Fs, E, b, lambda,diagonalDamping); - // // marginalize point: note - we reuse the standard SchurComplement function - // SymmetricBlockMatrix augmentedHessian = This::Cameras::SchurComplement<2,DimBlock>(Fs, E, P, b); + SymmetricBlockMatrix augmentedHessian = + Base::Cameras::mySchurComplement(Fs, E, P, b); - // // now pack into an Hessian factor - // std::vector dims(nrUniqueKeys + 1); // this also includes the b term - // std::fill(dims.begin(), dims.end() - 1, 6); - // dims.back() = 1; - // SymmetricBlockMatrix augmentedHessianUniqueKeys; - // - // // here we have to deal with the fact that some cameras may share the same extrinsic key - // if (nrUniqueKeys == nrNonuniqueKeys) { // if there is 1 calibration key per camera - // augmentedHessianUniqueKeys = SymmetricBlockMatrix( - // dims, Matrix(augmentedHessian.selfadjointView())); - // } else { // if multiple cameras share a calibration we have to rearrange - // // the results of the Schur complement matrix - // std::vector nonuniqueDims(nrNonuniqueKeys + 1); // this also includes the b term - // std::fill(nonuniqueDims.begin(), nonuniqueDims.end() - 1, 6); - // nonuniqueDims.back() = 1; - // augmentedHessian = SymmetricBlockMatrix( - // nonuniqueDims, Matrix(augmentedHessian.selfadjointView())); - // - // // these are the keys that correspond to the blocks in augmentedHessian (output of SchurComplement) - // KeyVector nonuniqueKeys; - // for (size_t i = 0; i < world_P_body_key_pairs_.size(); i++) { - // nonuniqueKeys.push_back(world_P_body_key_pairs_.at(i)); - // nonuniqueKeys.push_back(body_P_cam_ this->keys_.at(i)); - // } - // - // // get map from key to location in the new augmented Hessian matrix (the one including only unique keys) - // std::map keyToSlotMap; - // for (size_t k = 0; k < nrUniqueKeys; k++) { - // keyToSlotMap[ this->keys_[k]] = k; - // } - // - // // initialize matrix to zero - // augmentedHessianUniqueKeys = SymmetricBlockMatrix( - // dims, Matrix::Zero(6 * nrUniqueKeys + 1, 6 * nrUniqueKeys + 1)); - // - // // add contributions for each key: note this loops over the hessian with nonUnique keys (augmentedHessian) - // // and populates an Hessian that only includes the unique keys (that is what we want to return) - // for (size_t i = 0; i < nrNonuniqueKeys; i++) { // rows - // Key key_i = nonuniqueKeys.at(i); - // - // // update information vector - // augmentedHessianUniqueKeys.updateOffDiagonalBlock( - // keyToSlotMap[key_i], nrUniqueKeys, - // augmentedHessian.aboveDiagonalBlock(i, nrNonuniqueKeys)); - // - // // update blocks - // for (size_t j = i; j < nrNonuniqueKeys; j++) { // cols - // Key key_j = nonuniqueKeys.at(j); - // if (i == j) { - // augmentedHessianUniqueKeys.updateDiagonalBlock( - // keyToSlotMap[key_i], augmentedHessian.diagonalBlock(i)); - // } else { // (i < j) - // if (keyToSlotMap[key_i] != keyToSlotMap[key_j]) { - // augmentedHessianUniqueKeys.updateOffDiagonalBlock( - // keyToSlotMap[key_i], keyToSlotMap[key_j], - // augmentedHessian.aboveDiagonalBlock(i, j)); - // } else { - // augmentedHessianUniqueKeys.updateDiagonalBlock( - // keyToSlotMap[key_i], - // augmentedHessian.aboveDiagonalBlock(i, j) - // + augmentedHessian.aboveDiagonalBlock(i, j).transpose()); - // } - // } - // } - // } - // // update bottom right element of the matrix - // augmentedHessianUniqueKeys.updateDiagonalBlock( - // nrUniqueKeys, augmentedHessian.diagonalBlock(nrNonuniqueKeys)); - // } - // return boost::make_shared < RegularHessianFactor - // > ( this->keys_, augmentedHessianUniqueKeys); + // now pack into an Hessian factor + std::vector dims(nrUniqueKeys + 1); // this also includes the b term + std::fill(dims.begin(), dims.end() - 1, 6); + dims.back() = 1; + SymmetricBlockMatrix augmentedHessianUniqueKeys; - // TO REMOVE: - for (Matrix& m : Gs) - m = Matrix::Zero(DimPose, DimPose); - for (Vector& v : gs) - v = Vector::Zero(DimPose); - return boost::make_shared < RegularHessianFactor > ( this->keys_, Gs, gs, 0.0); + // here we have to deal with the fact that some cameras may share the same extrinsic key + if (nrUniqueKeys == nrNonuniqueKeys) { // if there is 1 calibration key per camera + augmentedHessianUniqueKeys = SymmetricBlockMatrix( + dims, Matrix(augmentedHessian.selfadjointView())); + } else { // if multiple cameras share a calibration we have to rearrange + // the results of the Schur complement matrix + std::vector nonuniqueDims(nrNonuniqueKeys + 1); // this also includes the b term + std::fill(nonuniqueDims.begin(), nonuniqueDims.end() - 1, 6); // all are poses .. + nonuniqueDims.back() = 1; // except b is a scalar + augmentedHessian = SymmetricBlockMatrix( + nonuniqueDims, Matrix(augmentedHessian.selfadjointView())); + + // these are the keys that correspond to the blocks in augmentedHessian (output of SchurComplement) + KeyVector nonuniqueKeys; + for (size_t i = 0; i < world_P_body_key_pairs_.size(); i++) { + nonuniqueKeys.push_back(world_P_body_key_pairs_.at(i).first); + nonuniqueKeys.push_back(world_P_body_key_pairs_.at(i).second); + } + + // get map from key to location in the new augmented Hessian matrix (the one including only unique keys) + std::map keyToSlotMap; + for (size_t k = 0; k < nrUniqueKeys; k++) { + keyToSlotMap[ this->keys_[k] ] = k; + } + + // initialize matrix to zero + augmentedHessianUniqueKeys = SymmetricBlockMatrix( + dims, Matrix::Zero(6 * nrUniqueKeys + 1, 6 * nrUniqueKeys + 1)); + + // add contributions for each key: note this loops over the hessian with nonUnique keys (augmentedHessian) + // and populates an Hessian that only includes the unique keys (that is what we want to return) + for (size_t i = 0; i < nrNonuniqueKeys; i++) { // rows + Key key_i = nonuniqueKeys.at(i); + + // update information vector + augmentedHessianUniqueKeys.updateOffDiagonalBlock( + keyToSlotMap[key_i], nrUniqueKeys, + augmentedHessian.aboveDiagonalBlock(i, nrNonuniqueKeys)); + + // update blocks + for (size_t j = i; j < nrNonuniqueKeys; j++) { // cols + Key key_j = nonuniqueKeys.at(j); + if (i == j) { + augmentedHessianUniqueKeys.updateDiagonalBlock( + keyToSlotMap[key_i], augmentedHessian.diagonalBlock(i)); + } else { // (i < j) + if (keyToSlotMap[key_i] != keyToSlotMap[key_j]) { + augmentedHessianUniqueKeys.updateOffDiagonalBlock( + keyToSlotMap[key_i], keyToSlotMap[key_j], + augmentedHessian.aboveDiagonalBlock(i, j)); + } else { + augmentedHessianUniqueKeys.updateDiagonalBlock( + keyToSlotMap[key_i], + augmentedHessian.aboveDiagonalBlock(i, j) + + augmentedHessian.aboveDiagonalBlock(i, j).transpose()); + } + } + } + } + // update bottom right element of the matrix + augmentedHessianUniqueKeys.updateDiagonalBlock( + nrUniqueKeys, augmentedHessian.diagonalBlock(nrNonuniqueKeys)); + } + return boost::make_shared < RegularHessianFactor + > ( this->keys_, augmentedHessianUniqueKeys); + +// // TO REMOVE: +// for (Matrix& m : Gs) +// m = Matrix::Zero(DimPose, DimPose); +// for (Vector& v : gs) +// v = Vector::Zero(DimPose); +// return boost::make_shared < RegularHessianFactor > ( this->keys_, Gs, gs, 0.0); } /** @@ -470,7 +471,7 @@ PinholePose > { // depending on flag set on construction we may linearize to different linear factors switch (this->params_.linearizationMode) { case HESSIAN: - return createHessianFactor(values, lambda); + return this->createHessianFactor(values, lambda); default: throw std::runtime_error( "SmartProjectionPoseFactorRollingShutter: unknown linearization mode"); @@ -480,7 +481,7 @@ PinholePose > { /// linearize boost::shared_ptr linearize(const Values& values) const override { - return linearizeDamped(values); + return this->linearizeDamped(values); } private: diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h index 40d90d614..e4d714b0f 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h @@ -250,7 +250,7 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor { // marginalize point: note - we reuse the standard SchurComplement function SymmetricBlockMatrix augmentedHessian = - Cameras::SchurComplement<3, Dim>(Fs, E, P, b); + Cameras::SchurComplementWithCustomBlocks<3, Dim>(Fs, E, P, b); // now pack into an Hessian factor std::vector dims(nrUniqueKeys + 1); // this also includes the b term From 477dd5b247d0246c6efc609f62580a1d1db8b412 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Thu, 22 Jul 2021 21:49:40 -0400 Subject: [PATCH 094/248] all pass! --- gtsam/geometry/CameraSet.h | 58 ++----------------- .../SmartProjectionPoseFactorRollingShutter.h | 9 +-- .../slam/SmartStereoProjectionFactorPP.h | 2 +- 3 files changed, 8 insertions(+), 61 deletions(-) diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h index fcbff020c..2b656bd35 100644 --- a/gtsam/geometry/CameraSet.h +++ b/gtsam/geometry/CameraSet.h @@ -142,57 +142,11 @@ public: return ErrorVector(project2(point, Fs, E), measured); } - static SymmetricBlockMatrix mySchurComplement( - const std::vector< Eigen::Matrix, - Eigen::aligned_allocator< Eigen::Matrix > >& Fs, + static SymmetricBlockMatrix SchurComplement312( + const std::vector< Eigen::Matrix, + Eigen::aligned_allocator< Eigen::Matrix > >& Fs, const Matrix& E, const Eigen::Matrix& P, const Vector& b) { - return mySchurComplement<2,3,12>(Fs, E, P, b); - } - - template // N = 2 or 3 (point dimension), ND is the camera dimension - static SymmetricBlockMatrix mySchurComplement( - const std::vector< Eigen::Matrix, - Eigen::aligned_allocator< Eigen::Matrix > >& Fs, - const Matrix& E, const Eigen::Matrix& P, const Vector& b) { - - // a single point is observed in m cameras - size_t m = Fs.size(); - - // Create a SymmetricBlockMatrix (augmented hessian, with extra row/column with info vector) - size_t M1 = ND * m + 1; - std::vector dims(m + 1); // this also includes the b term - std::fill(dims.begin(), dims.end() - 1, ND); - dims.back() = 1; - SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(M1, M1)); - - // Blockwise Schur complement - for (size_t i = 0; i < m; i++) { // for each camera - - const Eigen::Matrix& Fi = Fs[i]; - const auto FiT = Fi.transpose(); - const Eigen::Matrix Ei_P = // - E.block(myZDim * i, 0, myZDim, N) * P; - - // D = (Dx2) * ZDim - augmentedHessian.setOffDiagonalBlock(i, m, FiT * b.segment(myZDim * i) // F' * b - - FiT * (Ei_P * (E.transpose() * b))); // D = (DxZDim) * (ZDimx3) * (N*ZDimm) * (ZDimm x 1) - - // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) ) - augmentedHessian.setDiagonalBlock(i, FiT - * (Fi - Ei_P * E.block(myZDim * i, 0, myZDim, N).transpose() * Fi)); - - // upper triangular part of the hessian - for (size_t j = i + 1; j < m; j++) { // for each camera - const Eigen::Matrix& Fj = Fs[j]; - - // (DxD) = (Dx2) * ( (2x2) * (2xD) ) - augmentedHessian.setOffDiagonalBlock(i, j, -FiT - * (Ei_P * E.block(myZDim * j, 0, myZDim, N).transpose() * Fj)); - } - } // end of for over cameras - - augmentedHessian.diagonalBlock(m)(0, 0) += b.squaredNorm(); - return augmentedHessian; + return SchurComplement<3,12>(Fs, E, P, b); } /** @@ -202,7 +156,7 @@ public: * Fixed size version */ template // N = 2 or 3 (point dimension), ND is the camera dimension - static SymmetricBlockMatrix SchurComplementWithCustomBlocks( + static SymmetricBlockMatrix SchurComplement( const std::vector< Eigen::Matrix, Eigen::aligned_allocator< Eigen::Matrix > >& Fs, const Matrix& E, const Eigen::Matrix& P, const Vector& b) { @@ -255,7 +209,7 @@ public: template // N = 2 or 3 static SymmetricBlockMatrix SchurComplement(const FBlocks& Fs, const Matrix& E, const Eigen::Matrix& P, const Vector& b) { - return SchurComplementWithCustomBlocks(Fs, E, P, b); + return SchurComplement(Fs, E, P, b); } /// Computes Point Covariance P, with lambda parameter diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index 472b6348a..3393f7ca4 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -342,7 +342,7 @@ PinholePose > { // Base::Cameras::SchurComplement(Fs, E, b, lambda,diagonalDamping); SymmetricBlockMatrix augmentedHessian = - Base::Cameras::mySchurComplement(Fs, E, P, b); + Base::Cameras::SchurComplement312(Fs, E, P, b); // now pack into an Hessian factor std::vector dims(nrUniqueKeys + 1); // this also includes the b term @@ -415,13 +415,6 @@ PinholePose > { } return boost::make_shared < RegularHessianFactor > ( this->keys_, augmentedHessianUniqueKeys); - -// // TO REMOVE: -// for (Matrix& m : Gs) -// m = Matrix::Zero(DimPose, DimPose); -// for (Vector& v : gs) -// v = Vector::Zero(DimPose); -// return boost::make_shared < RegularHessianFactor > ( this->keys_, Gs, gs, 0.0); } /** diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h index e4d714b0f..40d90d614 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h @@ -250,7 +250,7 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor { // marginalize point: note - we reuse the standard SchurComplement function SymmetricBlockMatrix augmentedHessian = - Cameras::SchurComplementWithCustomBlocks<3, Dim>(Fs, E, P, b); + Cameras::SchurComplement<3, Dim>(Fs, E, P, b); // now pack into an Hessian factor std::vector dims(nrUniqueKeys + 1); // this also includes the b term From 91a6613d84e786d2cf76f63d1c52bb0025730f31 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Thu, 22 Jul 2021 22:31:33 -0400 Subject: [PATCH 095/248] moved common function to cameraSet. commented issues with templated calls to functions in cameraSet --- gtsam/geometry/CameraSet.h | 99 +++++++++++++++ .../SmartProjectionPoseFactorRollingShutter.h | 114 +++++------------- .../slam/SmartStereoProjectionFactorPP.h | 92 +++----------- 3 files changed, 141 insertions(+), 164 deletions(-) diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h index 2b656bd35..19ab7ab31 100644 --- a/gtsam/geometry/CameraSet.h +++ b/gtsam/geometry/CameraSet.h @@ -200,6 +200,105 @@ public: return augmentedHessian; } + /** + * Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix + * G = F' * F - F' * E * P * E' * F + * g = F' * (b - E * P * E' * b) + * In this version, we allow for the case where the keys in the Jacobian are organized + * differents from the keys in the output SymmetricBlockMatrix + * In particular: each block of the Jacobian captures 2 poses (useful for rolling shutter and extrinsic calibration) + */ + template // N = 2 or 3 (point dimension), ND is the Jacobian block dimension, NDD is the Hessian block dimension + static SymmetricBlockMatrix SchurComplementAndRearrangeBlocks( + const std::vector, + Eigen::aligned_allocator > >& Fs, + const Matrix& E, const Eigen::Matrix& P, const Vector& b, + const KeyVector jacobianKeys, const KeyVector hessianKeys) { + + size_t nrNonuniqueKeys = jacobianKeys.size(); + size_t nrUniqueKeys = hessianKeys.size(); + + // marginalize point: note - we reuse the standard SchurComplement function + SymmetricBlockMatrix augmentedHessian = SchurComplement(Fs,E,P,b); + + // now pack into an Hessian factor + std::vector dims(nrUniqueKeys + 1); // this also includes the b term + std::fill(dims.begin(), dims.end() - 1, NDD); + dims.back() = 1; + SymmetricBlockMatrix augmentedHessianUniqueKeys; + + // here we have to deal with the fact that some blocks may share the same keys + if (nrUniqueKeys == nrNonuniqueKeys) { // if there is 1 calibration key per camera + augmentedHessianUniqueKeys = SymmetricBlockMatrix( + dims, Matrix(augmentedHessian.selfadjointView())); + } else { // if multiple cameras share a calibration we have to rearrange + // the results of the Schur complement matrix + std::vector nonuniqueDims(nrNonuniqueKeys + 1); // this also includes the b term + std::fill(nonuniqueDims.begin(), nonuniqueDims.end() - 1, NDD); + nonuniqueDims.back() = 1; + augmentedHessian = SymmetricBlockMatrix( + nonuniqueDims, Matrix(augmentedHessian.selfadjointView())); + + // get map from key to location in the new augmented Hessian matrix (the one including only unique keys) + std::map keyToSlotMap; + for (size_t k = 0; k < nrUniqueKeys; k++) { + keyToSlotMap[hessianKeys[k]] = k; + } + + // initialize matrix to zero + augmentedHessianUniqueKeys = SymmetricBlockMatrix( + dims, Matrix::Zero(NDD * nrUniqueKeys + 1, NDD * nrUniqueKeys + 1)); + + // add contributions for each key: note this loops over the hessian with nonUnique keys (augmentedHessian) + // and populates an Hessian that only includes the unique keys (that is what we want to return) + for (size_t i = 0; i < nrNonuniqueKeys; i++) { // rows + Key key_i = jacobianKeys.at(i); + + // update information vector + augmentedHessianUniqueKeys.updateOffDiagonalBlock( + keyToSlotMap[key_i], nrUniqueKeys, + augmentedHessian.aboveDiagonalBlock(i, nrNonuniqueKeys)); + + // update blocks + for (size_t j = i; j < nrNonuniqueKeys; j++) { // cols + Key key_j = jacobianKeys.at(j); + if (i == j) { + augmentedHessianUniqueKeys.updateDiagonalBlock( + keyToSlotMap[key_i], augmentedHessian.diagonalBlock(i)); + } else { // (i < j) + if (keyToSlotMap[key_i] != keyToSlotMap[key_j]) { + augmentedHessianUniqueKeys.updateOffDiagonalBlock( + keyToSlotMap[key_i], keyToSlotMap[key_j], + augmentedHessian.aboveDiagonalBlock(i, j)); + } else { + augmentedHessianUniqueKeys.updateDiagonalBlock( + keyToSlotMap[key_i], + augmentedHessian.aboveDiagonalBlock(i, j) + + augmentedHessian.aboveDiagonalBlock(i, j).transpose()); + } + } + } + } + // update bottom right element of the matrix + augmentedHessianUniqueKeys.updateDiagonalBlock( + nrUniqueKeys, augmentedHessian.diagonalBlock(nrNonuniqueKeys)); + } + return augmentedHessianUniqueKeys; + } + + /** + * non-templated version of function above + */ + static SymmetricBlockMatrix SchurComplementAndRearrangeBlocks_3_12_6( + const std::vector, + Eigen::aligned_allocator > >& Fs, + const Matrix& E, const Eigen::Matrix& P, const Vector& b, + const KeyVector jacobianKeys, const KeyVector hessianKeys) { + return SchurComplementAndRearrangeBlocks<3,12,6>(Fs, E, P, b, + jacobianKeys, + hessianKeys); + } + /** * Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix * G = F' * F - F' * E * P * E' * F diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index 3393f7ca4..c35beb0e2 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -62,8 +62,6 @@ PinholePose > { /// shorthand for base class type typedef SmartProjectionFactor > Base; -// typedef PinholePose Camera; -// typedef CameraSet Cameras; /// shorthand for this class typedef SmartProjectionPoseFactorRollingShutter This; @@ -299,27 +297,27 @@ PinholePose > { // we may have multiple observation sharing the same keys (due to the rolling shutter interpolation), // hence the number of unique keys may be smaller than 2 * nrMeasurements - size_t nrUniqueKeys = this->keys_.size(); - size_t nrNonuniqueKeys = 2*world_P_body_key_pairs_.size(); + size_t nrUniqueKeys = this->keys_.size(); // Create structures for Hessian Factors KeyVector js; std::vector < Matrix > Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2); - std::vector gs(nrUniqueKeys); + std::vector < Vector > gs(nrUniqueKeys); if (this->measured_.size() != this->cameras(values).size()) throw std::runtime_error("SmartProjectionPoseFactorRollingShutter: " - "measured_.size() inconsistent with input"); + "measured_.size() inconsistent with input"); // triangulate 3D point at given linearization point this->triangulateSafe(this->cameras(values)); - if (!this->result_) { // failed: return "empty/zero" Hessian + if (!this->result_) { // failed: return "empty/zero" Hessian for (Matrix& m : Gs) m = Matrix::Zero(DimPose, DimPose); for (Vector& v : gs) v = Vector::Zero(DimPose); - return boost::make_shared < RegularHessianFactor > ( this->keys_, Gs, gs, 0.0); + return boost::make_shared < RegularHessianFactor + > (this->keys_, Gs, gs, 0.0); } // compute Jacobian given triangulated 3D Point @@ -333,88 +331,32 @@ PinholePose > { for (size_t i = 0; i < Fs.size(); i++) Fs[i] = this->noiseModel_->Whiten(Fs[i]); - Matrix3 P = Base::Cameras::PointCov(E, lambda, diagonalDamping); + Matrix3 P = Base::Cameras::PointCov(E, lambda, diagonalDamping); + // the following unfortunately does not seem to work and causes + // an "reference to overloaded function could not be resolved; did you mean to call it?" error + // Matrix3 P; + // Base::Cameras::ComputePointCovariance<3>(P, E, lambda, diagonalDamping); // build augmented Hessian (with last row/column being the information vector) - // and marginalize point: note - we reuse the standard SchurComplement function - // does not work, since assumes convensional 6-dim blocks - // SymmetricBlockMatrix augmentedHessian = - // Base::Cameras::SchurComplement(Fs, E, b, lambda,diagonalDamping); - - SymmetricBlockMatrix augmentedHessian = - Base::Cameras::SchurComplement312(Fs, E, P, b); - - // now pack into an Hessian factor - std::vector dims(nrUniqueKeys + 1); // this also includes the b term - std::fill(dims.begin(), dims.end() - 1, 6); - dims.back() = 1; - SymmetricBlockMatrix augmentedHessianUniqueKeys; - - // here we have to deal with the fact that some cameras may share the same extrinsic key - if (nrUniqueKeys == nrNonuniqueKeys) { // if there is 1 calibration key per camera - augmentedHessianUniqueKeys = SymmetricBlockMatrix( - dims, Matrix(augmentedHessian.selfadjointView())); - } else { // if multiple cameras share a calibration we have to rearrange - // the results of the Schur complement matrix - std::vector nonuniqueDims(nrNonuniqueKeys + 1); // this also includes the b term - std::fill(nonuniqueDims.begin(), nonuniqueDims.end() - 1, 6); // all are poses .. - nonuniqueDims.back() = 1; // except b is a scalar - augmentedHessian = SymmetricBlockMatrix( - nonuniqueDims, Matrix(augmentedHessian.selfadjointView())); - - // these are the keys that correspond to the blocks in augmentedHessian (output of SchurComplement) - KeyVector nonuniqueKeys; - for (size_t i = 0; i < world_P_body_key_pairs_.size(); i++) { - nonuniqueKeys.push_back(world_P_body_key_pairs_.at(i).first); - nonuniqueKeys.push_back(world_P_body_key_pairs_.at(i).second); - } - - // get map from key to location in the new augmented Hessian matrix (the one including only unique keys) - std::map keyToSlotMap; - for (size_t k = 0; k < nrUniqueKeys; k++) { - keyToSlotMap[ this->keys_[k] ] = k; - } - - // initialize matrix to zero - augmentedHessianUniqueKeys = SymmetricBlockMatrix( - dims, Matrix::Zero(6 * nrUniqueKeys + 1, 6 * nrUniqueKeys + 1)); - - // add contributions for each key: note this loops over the hessian with nonUnique keys (augmentedHessian) - // and populates an Hessian that only includes the unique keys (that is what we want to return) - for (size_t i = 0; i < nrNonuniqueKeys; i++) { // rows - Key key_i = nonuniqueKeys.at(i); - - // update information vector - augmentedHessianUniqueKeys.updateOffDiagonalBlock( - keyToSlotMap[key_i], nrUniqueKeys, - augmentedHessian.aboveDiagonalBlock(i, nrNonuniqueKeys)); - - // update blocks - for (size_t j = i; j < nrNonuniqueKeys; j++) { // cols - Key key_j = nonuniqueKeys.at(j); - if (i == j) { - augmentedHessianUniqueKeys.updateDiagonalBlock( - keyToSlotMap[key_i], augmentedHessian.diagonalBlock(i)); - } else { // (i < j) - if (keyToSlotMap[key_i] != keyToSlotMap[key_j]) { - augmentedHessianUniqueKeys.updateOffDiagonalBlock( - keyToSlotMap[key_i], keyToSlotMap[key_j], - augmentedHessian.aboveDiagonalBlock(i, j)); - } else { - augmentedHessianUniqueKeys.updateDiagonalBlock( - keyToSlotMap[key_i], - augmentedHessian.aboveDiagonalBlock(i, j) - + augmentedHessian.aboveDiagonalBlock(i, j).transpose()); - } - } - } - } - // update bottom right element of the matrix - augmentedHessianUniqueKeys.updateDiagonalBlock( - nrUniqueKeys, augmentedHessian.diagonalBlock(nrNonuniqueKeys)); + // these are the keys that correspond to the blocks in augmentedHessian (output of SchurComplement) + KeyVector nonuniqueKeys; + for (size_t i = 0; i < world_P_body_key_pairs_.size(); i++) { + nonuniqueKeys.push_back(world_P_body_key_pairs_.at(i).first); + nonuniqueKeys.push_back(world_P_body_key_pairs_.at(i).second); } + // but we need to get the augumented hessian wrt the unique keys in key_ + SymmetricBlockMatrix augmentedHessianUniqueKeys = + Base::Cameras::SchurComplementAndRearrangeBlocks_3_12_6( + Fs, E, P, b, nonuniqueKeys, this->keys_); + + // the following unfortunately does not seem to work and causes + // an "reference to overloaded function could not be resolved; did you mean to call it?" error + // SymmetricBlockMatrix augmentedHessianUniqueKeys = + // Base::Cameras::SchurComplementAndRearrangeBlocks<3, DimBlock, DimPose>( + // Fs, E, P, b, nonuniqueKeys, keys_); + return boost::make_shared < RegularHessianFactor - > ( this->keys_, augmentedHessianUniqueKeys); + > (this->keys_, augmentedHessianUniqueKeys); } /** diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h index 40d90d614..25be48b0f 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h @@ -61,10 +61,10 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor { /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; - static const int Dim = 12; ///< Camera dimension: 6 for body pose, 6 for extrinsic pose + static const int DimBlock = 12; ///< Camera dimension: 6 for body pose, 6 for extrinsic pose static const int DimPose = 6; ///< Pose3 dimension static const int ZDim = 3; ///< Measurement dimension (for a StereoPoint2 measurement) - typedef Eigen::Matrix MatrixZD; // F blocks (derivatives wrt camera) + typedef Eigen::Matrix MatrixZD; // F blocks (derivatives wrt camera) typedef std::vector > FBlocks; // vector of F blocks /** @@ -180,7 +180,7 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor { // get jacobians and error vector for current measurement StereoPoint2 reprojectionError_i = StereoPoint2( camera.project(*result_, dProject_dPoseCam_i, Ei) - measured_.at(i)); - Eigen::Matrix J; // 3 x 12 + Eigen::Matrix J; // 3 x 12 J.block(0, 0) = dProject_dPoseCam_i * dPoseCam_dPoseBody_i; // (3x6) * (6x6) J.block(0, 6) = dProject_dPoseCam_i * dPoseCam_dPoseExt_i; // (3x6) * (6x6) // if the right pixel is invalid, fix jacobians @@ -209,8 +209,6 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor { // of keys may be smaller than 2 * nrMeasurements (which is the upper bound where we // have a body key and an extrinsic calibration key for each measurement) size_t nrUniqueKeys = keys_.size(); - size_t nrNonuniqueKeys = world_P_body_keys_.size() - + body_P_cam_keys_.size(); // Create structures for Hessian Factors KeyVector js; @@ -246,81 +244,19 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor { // build augmented Hessian (with last row/column being the information vector) Matrix3 P; - Cameras::ComputePointCovariance<3>(P, E, lambda, diagonalDamping); + Cameras::ComputePointCovariance <3> (P, E, lambda, diagonalDamping); - // marginalize point: note - we reuse the standard SchurComplement function - SymmetricBlockMatrix augmentedHessian = - Cameras::SchurComplement<3, Dim>(Fs, E, P, b); - - // now pack into an Hessian factor - std::vector dims(nrUniqueKeys + 1); // this also includes the b term - std::fill(dims.begin(), dims.end() - 1, 6); - dims.back() = 1; - SymmetricBlockMatrix augmentedHessianUniqueKeys; - - // here we have to deal with the fact that some cameras may share the same extrinsic key - if (nrUniqueKeys == nrNonuniqueKeys) { // if there is 1 calibration key per camera - augmentedHessianUniqueKeys = SymmetricBlockMatrix( - dims, Matrix(augmentedHessian.selfadjointView())); - } else { // if multiple cameras share a calibration we have to rearrange - // the results of the Schur complement matrix - std::vector nonuniqueDims(nrNonuniqueKeys + 1); // this also includes the b term - std::fill(nonuniqueDims.begin(), nonuniqueDims.end() - 1, 6); - nonuniqueDims.back() = 1; - augmentedHessian = SymmetricBlockMatrix( - nonuniqueDims, Matrix(augmentedHessian.selfadjointView())); - - // these are the keys that correspond to the blocks in augmentedHessian (output of SchurComplement) - KeyVector nonuniqueKeys; - for (size_t i = 0; i < world_P_body_keys_.size(); i++) { - nonuniqueKeys.push_back(world_P_body_keys_.at(i)); - nonuniqueKeys.push_back(body_P_cam_keys_.at(i)); - } - - // get map from key to location in the new augmented Hessian matrix (the one including only unique keys) - std::map keyToSlotMap; - for (size_t k = 0; k < nrUniqueKeys; k++) { - keyToSlotMap[keys_[k]] = k; - } - - // initialize matrix to zero - augmentedHessianUniqueKeys = SymmetricBlockMatrix( - dims, Matrix::Zero(6 * nrUniqueKeys + 1, 6 * nrUniqueKeys + 1)); - - // add contributions for each key: note this loops over the hessian with nonUnique keys (augmentedHessian) - // and populates an Hessian that only includes the unique keys (that is what we want to return) - for (size_t i = 0; i < nrNonuniqueKeys; i++) { // rows - Key key_i = nonuniqueKeys.at(i); - - // update information vector - augmentedHessianUniqueKeys.updateOffDiagonalBlock( - keyToSlotMap[key_i], nrUniqueKeys, - augmentedHessian.aboveDiagonalBlock(i, nrNonuniqueKeys)); - - // update blocks - for (size_t j = i; j < nrNonuniqueKeys; j++) { // cols - Key key_j = nonuniqueKeys.at(j); - if (i == j) { - augmentedHessianUniqueKeys.updateDiagonalBlock( - keyToSlotMap[key_i], augmentedHessian.diagonalBlock(i)); - } else { // (i < j) - if (keyToSlotMap[key_i] != keyToSlotMap[key_j]) { - augmentedHessianUniqueKeys.updateOffDiagonalBlock( - keyToSlotMap[key_i], keyToSlotMap[key_j], - augmentedHessian.aboveDiagonalBlock(i, j)); - } else { - augmentedHessianUniqueKeys.updateDiagonalBlock( - keyToSlotMap[key_i], - augmentedHessian.aboveDiagonalBlock(i, j) - + augmentedHessian.aboveDiagonalBlock(i, j).transpose()); - } - } - } - } - // update bottom right element of the matrix - augmentedHessianUniqueKeys.updateDiagonalBlock( - nrUniqueKeys, augmentedHessian.diagonalBlock(nrNonuniqueKeys)); + // these are the keys that correspond to the blocks in augmentedHessian (output of SchurComplement) + KeyVector nonuniqueKeys; + for (size_t i = 0; i < world_P_body_keys_.size(); i++) { + nonuniqueKeys.push_back(world_P_body_keys_.at(i)); + nonuniqueKeys.push_back(body_P_cam_keys_.at(i)); } + // but we need to get the augumented hessian wrt the unique keys in key_ + SymmetricBlockMatrix augmentedHessianUniqueKeys = + Cameras::SchurComplementAndRearrangeBlocks<3,DimBlock,DimPose>(Fs,E,P,b, + nonuniqueKeys, keys_); + return boost::make_shared < RegularHessianFactor > (keys_, augmentedHessianUniqueKeys); } From 48a7afa46369ef81dd430022869d328b2976f729 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Thu, 22 Jul 2021 22:35:21 -0400 Subject: [PATCH 096/248] removed comments. Code is complete now. Need few more unit tests and we are good to go --- .../slam/SmartProjectionPoseFactorRollingShutter.h | 10 ---------- 1 file changed, 10 deletions(-) diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index c35beb0e2..c5b63250b 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -332,10 +332,6 @@ PinholePose > { Fs[i] = this->noiseModel_->Whiten(Fs[i]); Matrix3 P = Base::Cameras::PointCov(E, lambda, diagonalDamping); - // the following unfortunately does not seem to work and causes - // an "reference to overloaded function could not be resolved; did you mean to call it?" error - // Matrix3 P; - // Base::Cameras::ComputePointCovariance<3>(P, E, lambda, diagonalDamping); // build augmented Hessian (with last row/column being the information vector) // these are the keys that correspond to the blocks in augmentedHessian (output of SchurComplement) @@ -349,12 +345,6 @@ PinholePose > { Base::Cameras::SchurComplementAndRearrangeBlocks_3_12_6( Fs, E, P, b, nonuniqueKeys, this->keys_); - // the following unfortunately does not seem to work and causes - // an "reference to overloaded function could not be resolved; did you mean to call it?" error - // SymmetricBlockMatrix augmentedHessianUniqueKeys = - // Base::Cameras::SchurComplementAndRearrangeBlocks<3, DimBlock, DimPose>( - // Fs, E, P, b, nonuniqueKeys, keys_); - return boost::make_shared < RegularHessianFactor > (this->keys_, augmentedHessianUniqueKeys); } From 1c3ff0580be37ec609e8aa9230bc5b2e02ab963b Mon Sep 17 00:00:00 2001 From: lcarlone Date: Fri, 23 Jul 2021 00:03:41 -0400 Subject: [PATCH 097/248] removed printout, solved CI issue --- .../slam/SmartProjectionPoseFactorRollingShutter.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index c5b63250b..b9b43bc18 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -368,8 +368,10 @@ PinholePose > { */ typename Base::Cameras cameras(const Values& values) const override { size_t numViews = this->measured_.size(); - assert(world_P_body_keys_.size() == K_all_.size()); - assert(world_P_body_keys_.size() == body_P_cam_keys_.size()); + assert(numViews == K_all_.size()); + assert(numViews == gammas_.size()); + assert(numViews == body_P_sensors_.size()); + assert(numViews == world_P_body_key_pairs_.size()); typename Base::Cameras cameras; for (size_t i = 0; i < numViews; i++) { // for each measurement @@ -379,8 +381,6 @@ PinholePose > { Pose3 w_P_body = interpolate(w_P_body1, w_P_body2, interpolationFactor); Pose3 body_P_cam = body_P_sensors_[i]; Pose3 w_P_cam = w_P_body.compose(body_P_cam); - std::cout << "id : " << i << std::endl; - w_P_cam.print("w_P_cam\n"); cameras.emplace_back(w_P_cam, K_all_[i]); } return cameras; From d4951f025d3c9c487cc6ddbea56cd975a034d538 Mon Sep 17 00:00:00 2001 From: akrishnan86 Date: Fri, 23 Jul 2021 00:09:47 -0700 Subject: [PATCH 098/248] adding gtsam scope --- gtsam/geometry/geometry.i | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index b1c34ba42..0e303cbcd 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -985,21 +985,21 @@ gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Unified& cameras, gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses, gtsam::Cal3_S2* sharedCal, const gtsam::Point2Vector& measurements, - const Point3& initialEstimate); + const gtsam::Point3& initialEstimate); gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses, gtsam::Cal3DS2* sharedCal, const gtsam::Point2Vector& measurements, - const Point3& initialEstimate); + const gtsam::Point3& initialEstimate); gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses, gtsam::Cal3Bundler* sharedCal, const gtsam::Point2Vector& measurements, - const Point3& initialEstimate); -gtsam::Point3 triangulateNonlinear(const CameraSetCal3_S2& cameras, + const gtsam::Point3& initialEstimate); +gtsam::Point3 triangulateNonlinear(const gtsam::CameraSetCal3_S2& cameras, const gtsam::Point2Vector& measurements, - const Point3& initialEstimate); -gtsam::Point3 triangulateNonlinear(const CameraSetCal3Bundler& cameras, + const gtsam::Point3& initialEstimate); +gtsam::Point3 triangulateNonlinear(const gtsam::CameraSetCal3Bundler& cameras, const gtsam::Point2Vector& measurements, - const Point3& initialEstimate); + const gtsam::Point3& initialEstimate); #include template From 934413522de7e260938de735d7d8eaa904b1f85d Mon Sep 17 00:00:00 2001 From: lcarlone Date: Fri, 23 Jul 2021 15:39:13 -0400 Subject: [PATCH 099/248] fixed another test, few more to go --- .../SmartProjectionPoseFactorRollingShutter.h | 18 +- ...martProjectionPoseFactorRollingShutter.cpp | 227 ++++++------------ 2 files changed, 86 insertions(+), 159 deletions(-) diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index b9b43bc18..6fb3f5ce1 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -52,7 +52,7 @@ PinholePose > { std::vector> world_P_body_key_pairs_; /// interpolation factor (one for each observation) to interpolate between pair of consecutive poses - std::vector gammas_; + std::vector interp_param_; /// Pose of the camera in the body frame std::vector body_P_sensors_; ///< Pose of the camera in the body frame @@ -117,7 +117,7 @@ PinholePose > { this->keys_.push_back(world_P_body_key2); // add only unique keys // store interpolation factors - gammas_.push_back(gamma); + interp_param_.push_back(gamma); // store fixed calibration K_all_.push_back(K); @@ -180,7 +180,7 @@ PinholePose > { /// return the interpolation factors gammas const std::vector getGammas() const { - return gammas_; + return interp_param_; } /// return the extrinsic camera calibration body_P_sensors @@ -202,7 +202,7 @@ PinholePose > { << keyFormatter(world_P_body_key_pairs_[i].first) << std::endl; std::cout << " pose2 key: " << keyFormatter(world_P_body_key_pairs_[i].second) << std::endl; - std::cout << " gamma: " << gammas_[i] << std::endl; + std::cout << " gamma: " << interp_param_[i] << std::endl; body_P_sensors_[i].print("extrinsic calibration:\n"); K_all_[i]->print("intrinsic calibration = "); } @@ -237,7 +237,7 @@ PinholePose > { }else{ extrinsicCalibrationEqual = false; } return e && Base::equals(p, tol) && K_all_ == e->calibration() - && gammas_ == e->getGammas() && keyPairsEqual && extrinsicCalibrationEqual; + && interp_param_ == e->getGammas() && keyPairsEqual && extrinsicCalibrationEqual; } /** @@ -264,7 +264,7 @@ PinholePose > { for (size_t i = 0; i < numViews; i++) { // for each camera/measurement Pose3 w_P_body1 = values.at(world_P_body_key_pairs_[i].first); Pose3 w_P_body2 = values.at(world_P_body_key_pairs_[i].second); - double interpolationFactor = gammas_[i]; + double interpolationFactor = interp_param_[i]; // get interpolated pose: Pose3 w_P_body = interpolate(w_P_body1, w_P_body2,interpolationFactor, dInterpPose_dPoseBody1, dInterpPose_dPoseBody2); Pose3 body_P_cam = body_P_sensors_[i]; @@ -322,7 +322,7 @@ PinholePose > { // compute Jacobian given triangulated 3D Point FBlocks Fs; - Matrix F, E; + Matrix E; Vector b; this->computeJacobiansWithTriangulatedPoint(Fs, E, b, values); @@ -369,7 +369,7 @@ PinholePose > { typename Base::Cameras cameras(const Values& values) const override { size_t numViews = this->measured_.size(); assert(numViews == K_all_.size()); - assert(numViews == gammas_.size()); + assert(numViews == interp_param_.size()); assert(numViews == body_P_sensors_.size()); assert(numViews == world_P_body_key_pairs_.size()); @@ -377,7 +377,7 @@ PinholePose > { for (size_t i = 0; i < numViews; i++) { // for each measurement Pose3 w_P_body1 = values.at(world_P_body_key_pairs_[i].first); Pose3 w_P_body2 = values.at(world_P_body_key_pairs_[i].second); - double interpolationFactor = gammas_[i]; + double interpolationFactor = interp_param_[i]; Pose3 w_P_body = interpolate(w_P_body1, w_P_body2, interpolationFactor); Pose3 body_P_cam = body_P_sensors_[i]; Pose3 w_P_cam = w_P_body.compose(body_P_cam); diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index 602ca155e..a841b753b 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -298,8 +298,7 @@ TEST( SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians ) { } /* *************************************************************************/ -TEST( SmartProjectionPoseFactorRollingShutter, 3poses_smart_projection_factor ) { - std::cout << "===================" << std::endl; +TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses ) { using namespace vanillaPoseRS; Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; @@ -365,173 +364,101 @@ TEST( SmartProjectionPoseFactorRollingShutter, 3poses_smart_projection_factor ) EXPECT(assert_equal(pose_above, result.at(x3), 1e-6)); } -/* ************************************************************************* - TEST( SmartProjectionPoseFactorRollingShutter, Factors ) { +/* *************************************************************************/ +TEST( SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses ) { + // here we replicate a test in SmartProjectionPoseFactor by setting interpolation + // factors to 0 and 1 (such that the rollingShutter measurements falls back to standard pixel measurements) + // Note: this is a quite extreme test since in typical camera you would not have more than + // 1 measurement per landmark at each interpolated pose + using namespace vanillaPose; - using namespace vanillaPose; + // Default cameras for simple derivatives + static Cal3_S2::shared_ptr sharedKSimple(new Cal3_S2(100, 100, 0, 0, 0)); - // Default cameras for simple derivatives - Rot3 R; - static Cal3_S2::shared_ptr sharedK(new Cal3_S2(100, 100, 0, 0, 0)); - Camera cam1(Pose3(R, Point3(0, 0, 0)), sharedK), cam2( - Pose3(R, Point3(1, 0, 0)), sharedK); + Rot3 R = Rot3::identity(); + Pose3 pose1 = Pose3(R, Point3(0, 0, 0)); + Pose3 pose2 = Pose3(R, Point3(1, 0, 0)); + Camera cam1(pose1, sharedKSimple), cam2(pose2, sharedKSimple); + Pose3 body_P_sensorId = Pose3::identity(); - // one landmarks 1m in front of camera - Point3 landmark1(0, 0, 10); + // one landmarks 1m in front of camera + Point3 landmark1(0, 0, 10); - Point2Vector measurements_cam1; + Point2Vector measurements_cam1; - // Project 2 landmarks into 2 cameras - measurements_cam1.push_back(cam1.project(landmark1)); - measurements_cam1.push_back(cam2.project(landmark1)); + // Project 2 landmarks into 2 cameras + measurements_cam1.push_back(cam1.project(landmark1)); + measurements_cam1.push_back(cam2.project(landmark1)); - // Create smart factors - KeyVector views {x1, x2}; + SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); + double interp_factor = 0; // equivalent to measurement taken at pose 1 + smartFactor1->add(measurements_cam1[0], x1, x2, interp_factor, sharedKSimple, + body_P_sensorId); + interp_factor = 1; // equivalent to measurement taken at pose 2 + smartFactor1->add(measurements_cam1[1], x1, x2, interp_factor, sharedKSimple, + body_P_sensorId); - SmartFactor::shared_ptr smartFactor1 = boost::make_shared(model, sharedK); - smartFactor1->add(measurements_cam1, views); + SmartFactor::Cameras cameras; + cameras.push_back(cam1); + cameras.push_back(cam2); - SmartFactor::Cameras cameras; - cameras.push_back(cam1); - cameras.push_back(cam2); + // Make sure triangulation works + CHECK(smartFactor1->triangulateSafe(cameras)); + CHECK(!smartFactor1->isDegenerate()); + CHECK(!smartFactor1->isPointBehindCamera()); + boost::optional p = smartFactor1->point(); + CHECK(p); + EXPECT(assert_equal(landmark1, *p)); - // Make sure triangulation works - CHECK(smartFactor1->triangulateSafe(cameras)); - CHECK(!smartFactor1->isDegenerate()); - CHECK(!smartFactor1->isPointBehindCamera()); - boost::optional p = smartFactor1->point(); - CHECK(p); - EXPECT(assert_equal(landmark1, *p)); + VectorValues zeroDelta; + Vector6 delta; + delta.setZero(); + zeroDelta.insert(x1, delta); + zeroDelta.insert(x2, delta); - VectorValues zeroDelta; - Vector6 delta; - delta.setZero(); - zeroDelta.insert(x1, delta); - zeroDelta.insert(x2, delta); + VectorValues perturbedDelta; + delta.setOnes(); + perturbedDelta.insert(x1, delta); + perturbedDelta.insert(x2, delta); + double expectedError = 2500; - VectorValues perturbedDelta; - delta.setOnes(); - perturbedDelta.insert(x1, delta); - perturbedDelta.insert(x2, delta); - double expectedError = 2500; + // After eliminating the point, A1 and A2 contain 2-rank information on cameras: + Matrix16 A1, A2; + A1 << -10, 0, 0, 0, 1, 0; + A2 << 10, 0, 1, 0, -1, 0; + A1 *= 10. / sigma; + A2 *= 10. / sigma; + Matrix expectedInformation; // filled below - // After eliminating the point, A1 and A2 contain 2-rank information on cameras: - Matrix16 A1, A2; - A1 << -10, 0, 0, 0, 1, 0; - A2 << 10, 0, 1, 0, -1, 0; - A1 *= 10. / sigma; - A2 *= 10. / sigma; - Matrix expectedInformation; // filled below - { - // createHessianFactor - Matrix66 G11 = 0.5 * A1.transpose() * A1; - Matrix66 G12 = 0.5 * A1.transpose() * A2; - Matrix66 G22 = 0.5 * A2.transpose() * A2; + // createHessianFactor + Matrix66 G11 = 0.5 * A1.transpose() * A1; + Matrix66 G12 = 0.5 * A1.transpose() * A2; + Matrix66 G22 = 0.5 * A2.transpose() * A2; - Vector6 g1; - g1.setZero(); - Vector6 g2; - g2.setZero(); + Vector6 g1; + g1.setZero(); + Vector6 g2; + g2.setZero(); - double f = 0; + double f = 0; - RegularHessianFactor<6> expected(x1, x2, G11, G12, g1, G22, g2, f); - expectedInformation = expected.information(); + RegularHessianFactor<6> expected(x1, x2, G11, G12, g1, G22, g2, f); + expectedInformation = expected.information(); - boost::shared_ptr > actual = - smartFactor1->createHessianFactor(cameras, 0.0); - EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6)); - EXPECT(assert_equal(expected, *actual, 1e-6)); - EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-6); - EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-6); - } - - { - Matrix26 F1; - F1.setZero(); - F1(0, 1) = -100; - F1(0, 3) = -10; - F1(1, 0) = 100; - F1(1, 4) = -10; - Matrix26 F2; - F2.setZero(); - F2(0, 1) = -101; - F2(0, 3) = -10; - F2(0, 5) = -1; - F2(1, 0) = 100; - F2(1, 2) = 10; - F2(1, 4) = -10; - Matrix E(4, 3); - E.setZero(); - E(0, 0) = 10; - E(1, 1) = 10; - E(2, 0) = 10; - E(2, 2) = 1; - E(3, 1) = 10; - SmartFactor::FBlocks Fs = list_of(F1)(F2); - Vector b(4); - b.setZero(); - - // Create smart factors - KeyVector keys; - keys.push_back(x1); - keys.push_back(x2); - - // createJacobianQFactor - SharedIsotropic n = noiseModel::Isotropic::Sigma(4, sigma); - Matrix3 P = (E.transpose() * E).inverse(); - JacobianFactorQ<6, 2> expectedQ(keys, Fs, E, P, b, n); - EXPECT(assert_equal(expectedInformation, expectedQ.information(), 1e-6)); - - boost::shared_ptr > actualQ = - smartFactor1->createJacobianQFactor(cameras, 0.0); - CHECK(actualQ); - EXPECT(assert_equal(expectedInformation, actualQ->information(), 1e-6)); - EXPECT(assert_equal(expectedQ, *actualQ)); - EXPECT_DOUBLES_EQUAL(0, actualQ->error(zeroDelta), 1e-6); - EXPECT_DOUBLES_EQUAL(expectedError, actualQ->error(perturbedDelta), 1e-6); - - // Whiten for RegularImplicitSchurFactor (does not have noise model) - model->WhitenSystem(E, b); - Matrix3 whiteP = (E.transpose() * E).inverse(); - Fs[0] = model->Whiten(Fs[0]); - Fs[1] = model->Whiten(Fs[1]); - - // createRegularImplicitSchurFactor - RegularImplicitSchurFactor expected(keys, Fs, E, whiteP, b); - - boost::shared_ptr > actual = - smartFactor1->createRegularImplicitSchurFactor(cameras, 0.0); - CHECK(actual); - EXPECT(assert_equal(expectedInformation, expected.information(), 1e-6)); - EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6)); - EXPECT(assert_equal(expected, *actual)); - EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-6); - EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-6); - } - - { - // createJacobianSVDFactor - Vector1 b; - b.setZero(); - double s = sigma * sin(M_PI_4); - SharedIsotropic n = noiseModel::Isotropic::Sigma(4 - 3, sigma); - JacobianFactor expected(x1, s * A1, x2, s * A2, b, n); - EXPECT(assert_equal(expectedInformation, expected.information(), 1e-6)); - - boost::shared_ptr actual = - smartFactor1->createJacobianSVDFactor(cameras, 0.0); - CHECK(actual); - EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6)); - EXPECT(assert_equal(expected, *actual)); - EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-6); - EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-6); - } - } + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + boost::shared_ptr < RegularHessianFactor<6> > actual = smartFactor1 + ->createHessianFactor(values); + EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6)); + EXPECT(assert_equal(expected, *actual, 1e-6)); + EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-6); + EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-6); +} /* ************************************************************************* TEST( SmartProjectionPoseFactorRollingShutter, 3poses_iterative_smart_projection_factor ) { - + std::cout << "===================" << std::endl; using namespace vanillaPose; KeyVector views {x1, x2, x3}; From aeb1d35dd6407350d2d997c670befddf618de368 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Fri, 23 Jul 2021 15:47:07 -0400 Subject: [PATCH 100/248] fixed test with lmk distance --- ...martProjectionPoseFactorRollingShutter.cpp | 152 ++++++------------ 1 file changed, 51 insertions(+), 101 deletions(-) diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index a841b753b..c2ebd49fb 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -456,121 +456,71 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses ) { EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-6); } - /* ************************************************************************* - TEST( SmartProjectionPoseFactorRollingShutter, 3poses_iterative_smart_projection_factor ) { - std::cout << "===================" << std::endl; - using namespace vanillaPose; +/* *************************************************************************/ +TEST( SmartProjectionPoseFactorRollingShutter, landmarkDistance ) { + using namespace vanillaPoseRS; + Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; - KeyVector views {x1, x2, x3}; + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; + // create inputs + std::vector> key_pairs; + key_pairs.push_back(std::make_pair(x1, x2)); + key_pairs.push_back(std::make_pair(x2, x3)); + key_pairs.push_back(std::make_pair(x3, x1)); - // Project three landmarks into three cameras - projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); - projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); - projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + std::vector interp_factors; + interp_factors.push_back(interp_factor1); + interp_factors.push_back(interp_factor2); + interp_factors.push_back(interp_factor3); - SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedK)); - smartFactor1->add(measurements_cam1, views); + double excludeLandmarksFutherThanDist = 2; + SmartProjectionParams params; + params.setRankTolerance(1.0); + params.setLinearizationMode(gtsam::HESSIAN); + params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); + params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); + params.setEnableEPI(false); - SmartFactor::shared_ptr smartFactor2(new SmartFactor(model, sharedK)); - smartFactor2->add(measurements_cam2, views); + SmartFactorRS smartFactor1(model,params); + smartFactor1.add(measurements_cam1, key_pairs, interp_factors, sharedK); - SmartFactor::shared_ptr smartFactor3(new SmartFactor(model, sharedK)); - smartFactor3->add(measurements_cam3, views); + SmartFactorRS smartFactor2(model,params); + smartFactor2.add(measurements_cam2, key_pairs, interp_factors, sharedK); - const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + SmartFactorRS smartFactor3(model,params); + smartFactor3.add(measurements_cam3, key_pairs, interp_factors, sharedK); - NonlinearFactorGraph graph; - graph.push_back(smartFactor1); - graph.push_back(smartFactor2); - graph.push_back(smartFactor3); - graph.addPrior(x1, cam1.pose(), noisePrior); - graph.addPrior(x2, cam2.pose(), noisePrior); + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), - Point3(0.1, 0.1, 0.1)); // smaller noise - Values values; - values.insert(x1, cam1.pose()); - values.insert(x2, cam2.pose()); - // initialize third pose with some noise, we expect it to move back to original pose_above - values.insert(x3, pose_above * noise_pose); - EXPECT( - assert_equal( - Pose3( - Rot3(1.11022302e-16, -0.0314107591, 0.99950656, -0.99950656, - -0.0313952598, -0.000986635786, 0.0314107591, -0.999013364, - -0.0313952598), Point3(0.1, -0.1, 1.9)), - values.at(x3))); + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.addPrior(x1, level_pose, noisePrior); + graph.addPrior(x2, pose_right, noisePrior); - Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); - result = optimizer.optimize(); - EXPECT(assert_equal(pose_above, result.at(x3), 1e-7)); - } + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise + Values values; + values.insert(x1, level_pose); + values.insert(x2, pose_right); + // initialize third pose with some noise, we expect it to move back to original pose_above + values.insert(x3, pose_above * noise_pose); - /* ************************************************************************* - TEST( SmartProjectionPoseFactorRollingShutter, landmarkDistance ) { - - using namespace vanillaPose; - - double excludeLandmarksFutherThanDist = 2; - - KeyVector views {x1, x2, x3}; - - Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; - - // Project three landmarks into three cameras - projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); - projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); - projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - - SmartProjectionParams params; - params.setRankTolerance(1.0); - params.setLinearizationMode(gtsam::JACOBIAN_SVD); - params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); - params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); - params.setEnableEPI(false); - - SmartFactor::shared_ptr smartFactor1( - new SmartFactor(model, sharedK, params)); - smartFactor1->add(measurements_cam1, views); - - SmartFactor::shared_ptr smartFactor2( - new SmartFactor(model, sharedK, params)); - smartFactor2->add(measurements_cam2, views); - - SmartFactor::shared_ptr smartFactor3( - new SmartFactor(model, sharedK, params)); - smartFactor3->add(measurements_cam3, views); - - const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - - NonlinearFactorGraph graph; - graph.push_back(smartFactor1); - graph.push_back(smartFactor2); - graph.push_back(smartFactor3); - graph.addPrior(x1, cam1.pose(), noisePrior); - graph.addPrior(x2, cam2.pose(), noisePrior); - - // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), - Point3(0.1, 0.1, 0.1)); // smaller noise - Values values; - values.insert(x1, cam1.pose()); - values.insert(x2, cam2.pose()); - values.insert(x3, pose_above * noise_pose); - - // All factors are disabled and pose should remain where it is - Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); - result = optimizer.optimize(); - EXPECT(assert_equal(values.at(x3), result.at(x3))); - } + // All factors are disabled and pose should remain where it is + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + EXPECT(assert_equal(values.at(x3), result.at(x3))); +} /* ************************************************************************* TEST( SmartProjectionPoseFactorRollingShutter, dynamicOutlierRejection ) { + std::cout << "===================" << std::endl; using namespace vanillaPose; From a7b7770310acaa84a7ef047cebd7e183290560d1 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Fri, 23 Jul 2021 15:50:16 -0400 Subject: [PATCH 101/248] test with EPI fixed --- ...martProjectionPoseFactorRollingShutter.cpp | 64 ++++++++++++++++++- 1 file changed, 63 insertions(+), 1 deletion(-) diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index c2ebd49fb..e501b9d49 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -456,6 +456,68 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses ) { EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-6); } +/* *************************************************************************/ +TEST( SmartProjectionPoseFactorRollingShutter, EPI ) { + using namespace vanillaPoseRS; + Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; + + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + + // create inputs + std::vector> key_pairs; + key_pairs.push_back(std::make_pair(x1, x2)); + key_pairs.push_back(std::make_pair(x2, x3)); + key_pairs.push_back(std::make_pair(x3, x1)); + + std::vector interp_factors; + interp_factors.push_back(interp_factor1); + interp_factors.push_back(interp_factor2); + interp_factors.push_back(interp_factor3); + + double excludeLandmarksFutherThanDist = 1e10; //very large + SmartProjectionParams params; + params.setRankTolerance(1.0); + params.setLinearizationMode(gtsam::HESSIAN); + params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); + params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); + params.setEnableEPI(true); + + SmartFactorRS smartFactor1(model,params); + smartFactor1.add(measurements_cam1, key_pairs, interp_factors, sharedK); + + SmartFactorRS smartFactor2(model,params); + smartFactor2.add(measurements_cam2, key_pairs, interp_factors, sharedK); + + SmartFactorRS smartFactor3(model,params); + smartFactor3.add(measurements_cam3, key_pairs, interp_factors, sharedK); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.addPrior(x1, level_pose, noisePrior); + graph.addPrior(x2, pose_right, noisePrior); + + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise + Values values; + values.insert(x1, level_pose); + values.insert(x2, pose_right); + // initialize third pose with some noise, we expect it to move back to original pose_above + values.insert(x3, pose_above * noise_pose); + + // Optimization should correct 3rd pose + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + EXPECT(assert_equal(pose_above, result.at(x3), 1e-6)); +} + /* *************************************************************************/ TEST( SmartProjectionPoseFactorRollingShutter, landmarkDistance ) { using namespace vanillaPoseRS; @@ -511,7 +573,7 @@ TEST( SmartProjectionPoseFactorRollingShutter, landmarkDistance ) { // initialize third pose with some noise, we expect it to move back to original pose_above values.insert(x3, pose_above * noise_pose); - // All factors are disabled and pose should remain where it is + // All factors are disabled (due to the distance threshold) and pose should remain where it is Values result; LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); result = optimizer.optimize(); From 9c288d90cec950b95e0e882e2e7986170249b244 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Fri, 23 Jul 2021 17:48:08 -0400 Subject: [PATCH 102/248] working on testing + cosmetic improvements to print for smart factors --- gtsam/slam/SmartFactorBase.h | 2 +- gtsam/slam/SmartProjectionFactor.h | 2 +- ...martProjectionPoseFactorRollingShutter.cpp | 128 ++++++++++-------- 3 files changed, 72 insertions(+), 60 deletions(-) diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 0b0308c5c..380283141 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -178,7 +178,7 @@ protected: DefaultKeyFormatter) const override { std::cout << s << "SmartFactorBase, z = \n"; for (size_t k = 0; k < measured_.size(); ++k) { - std::cout << "measurement, p = " << measured_[k] << "\t"; + std::cout << "measurement " << k<<", px = \n" << measured_[k] << "\n"; noiseModel_->print("noise model = "); } if(body_P_sensor_) diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index 475a9e829..f67ca0740 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -101,7 +101,7 @@ public: void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { std::cout << s << "SmartProjectionFactor\n"; - std::cout << "linearizationMode:\n" << params_.linearizationMode + std::cout << "linearizationMode: " << params_.linearizationMode << std::endl; std::cout << "triangulationParameters:\n" << params_.triangulation << std::endl; diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index e501b9d49..802ddbf43 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -319,14 +319,14 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses ) { interp_factors.push_back(interp_factor2); interp_factors.push_back(interp_factor3); - SmartFactorRS smartFactor1(model); - smartFactor1.add(measurements_cam1, key_pairs, interp_factors, sharedK); + SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); + smartFactor1->add(measurements_cam1, key_pairs, interp_factors, sharedK); - SmartFactorRS smartFactor2(model); - smartFactor2.add(measurements_cam2, key_pairs, interp_factors, sharedK); + SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model)); + smartFactor2->add(measurements_cam2, key_pairs, interp_factors, sharedK); - SmartFactorRS smartFactor3(model); - smartFactor3.add(measurements_cam3, key_pairs, interp_factors, sharedK); + SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model)); + smartFactor3->add(measurements_cam3, key_pairs, interp_factors, sharedK); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -457,7 +457,7 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses ) { } /* *************************************************************************/ -TEST( SmartProjectionPoseFactorRollingShutter, EPI ) { +TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI ) { using namespace vanillaPoseRS; Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; @@ -519,7 +519,7 @@ TEST( SmartProjectionPoseFactorRollingShutter, EPI ) { } /* *************************************************************************/ -TEST( SmartProjectionPoseFactorRollingShutter, landmarkDistance ) { +TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_landmarkDistance ) { using namespace vanillaPoseRS; Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; @@ -581,70 +581,82 @@ TEST( SmartProjectionPoseFactorRollingShutter, landmarkDistance ) { } /* ************************************************************************* - TEST( SmartProjectionPoseFactorRollingShutter, dynamicOutlierRejection ) { - std::cout << "===================" << std::endl; +TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_dynamicOutlierRejection ) { + std::cout << "===================" << std::endl; + using namespace vanillaPoseRS; + // add fourth landmark + Point3 landmark4(5, -0.5, 1); - using namespace vanillaPose; + Point2Vector measurements_cam1, measurements_cam2, measurements_cam3, measurements_cam4; + // Project 4 landmarks into cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4); + measurements_cam4.at(0) = measurements_cam4.at(0) + Point2(10, 10); // add outlier - double excludeLandmarksFutherThanDist = 1e10; - double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error + // create inputs + std::vector> key_pairs; + key_pairs.push_back(std::make_pair(x1, x2)); + key_pairs.push_back(std::make_pair(x2, x3)); + key_pairs.push_back(std::make_pair(x3, x1)); - KeyVector views {x1, x2, x3}; + std::vector interp_factors; + interp_factors.push_back(interp_factor1); + interp_factors.push_back(interp_factor2); + interp_factors.push_back(interp_factor3); - // add fourth landmark - Point3 landmark4(5, -0.5, 1); + double excludeLandmarksFutherThanDist = 1e10; + double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error - Point2Vector measurements_cam1, measurements_cam2, measurements_cam3, - measurements_cam4; + SmartProjectionParams params; + params.setRankTolerance(1.0); + params.setLinearizationMode(gtsam::HESSIAN); + params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); + params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); + params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold); + params.setEnableEPI(false); - // Project 4 landmarks into three cameras - projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); - projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); - projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4); - measurements_cam4.at(0) = measurements_cam4.at(0) + Point2(10, 10); // add outlier + SmartFactorRS smartFactor1(model, params); + smartFactor1.add(measurements_cam1, key_pairs, interp_factors, sharedK); - SmartProjectionParams params; - params.setLinearizationMode(gtsam::JACOBIAN_SVD); - params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); - params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold); + SmartFactorRS smartFactor2(model, params); + smartFactor2.add(measurements_cam2, key_pairs, interp_factors, sharedK); - SmartFactor::shared_ptr smartFactor1( - new SmartFactor(model, sharedK, params)); - smartFactor1->add(measurements_cam1, views); + SmartFactorRS smartFactor3(model, params); + smartFactor3.add(measurements_cam3, key_pairs, interp_factors, sharedK); - SmartFactor::shared_ptr smartFactor2( - new SmartFactor(model, sharedK, params)); - smartFactor2->add(measurements_cam2, views); + SmartFactorRS smartFactor4(model, params); + smartFactor4.add(measurements_cam4, key_pairs, interp_factors, sharedK); - SmartFactor::shared_ptr smartFactor3( - new SmartFactor(model, sharedK, params)); - smartFactor3->add(measurements_cam3, views); + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - SmartFactor::shared_ptr smartFactor4( - new SmartFactor(model, sharedK, params)); - smartFactor4->add(measurements_cam4, views); + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.push_back(smartFactor4); + graph.addPrior(x1, level_pose, noisePrior); + graph.addPrior(x2, pose_right, noisePrior); - const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + Pose3 noise_pose = Pose3::identity(); // Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), +// Point3(0.1, 0.1, 0.1)); // smaller noise + Values values; + values.insert(x1, level_pose); + values.insert(x2, pose_right); + // initialize third pose with some noise, we expect it to move back to original pose_above + values.insert(x3, pose_above * noise_pose); - NonlinearFactorGraph graph; - graph.push_back(smartFactor1); - graph.push_back(smartFactor2); - graph.push_back(smartFactor3); - graph.push_back(smartFactor4); - graph.addPrior(x1, cam1.pose(), noisePrior); - graph.addPrior(x2, cam2.pose(), noisePrior); + // Optimization should correct 3rd pose + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + EXPECT(assert_equal(pose_above, result.at(x3), 1e-6)); - Values values; - values.insert(x1, cam1.pose()); - values.insert(x2, cam2.pose()); - values.insert(x3, cam3.pose()); - - // All factors are disabled and pose should remain where it is - Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); - result = optimizer.optimize(); - EXPECT(assert_equal(cam3.pose(), result.at(x3))); + smartFactor1.print("smartFactor1"); + smartFactor2.print("smartFactor2"); + smartFactor3.print("smartFactor3"); + smartFactor4.print("smartFactor4"); } /* ************************************************************************* From 81526e8917b5cd256b0021c473d16de7d70c0741 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Fri, 23 Jul 2021 17:57:24 -0400 Subject: [PATCH 103/248] fixed another test. --- .../SmartProjectionPoseFactorRollingShutter.h | 1 - ...martProjectionPoseFactorRollingShutter.cpp | 31 ++++++++----------- 2 files changed, 13 insertions(+), 19 deletions(-) diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index 6fb3f5ce1..03c44dbe9 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -319,7 +319,6 @@ PinholePose > { return boost::make_shared < RegularHessianFactor > (this->keys_, Gs, gs, 0.0); } - // compute Jacobian given triangulated 3D Point FBlocks Fs; Matrix E; diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index 802ddbf43..8b2bc26dd 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -580,9 +580,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_landmarkDista EXPECT(assert_equal(values.at(x3), result.at(x3))); } - /* ************************************************************************* + /* *************************************************************************/ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_dynamicOutlierRejection ) { - std::cout << "===================" << std::endl; using namespace vanillaPoseRS; // add fourth landmark Point3 landmark4(5, -0.5, 1); @@ -607,7 +606,7 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_dynamicOutlie interp_factors.push_back(interp_factor3); double excludeLandmarksFutherThanDist = 1e10; - double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error + double dynamicOutlierRejectionThreshold = 3; // max 3 pixel of average reprojection error SmartProjectionParams params; params.setRankTolerance(1.0); @@ -617,17 +616,17 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_dynamicOutlie params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold); params.setEnableEPI(false); - SmartFactorRS smartFactor1(model, params); - smartFactor1.add(measurements_cam1, key_pairs, interp_factors, sharedK); + SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model,params)); + smartFactor1->add(measurements_cam1, key_pairs, interp_factors, sharedK); - SmartFactorRS smartFactor2(model, params); - smartFactor2.add(measurements_cam2, key_pairs, interp_factors, sharedK); + SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model,params)); + smartFactor2->add(measurements_cam2, key_pairs, interp_factors, sharedK); - SmartFactorRS smartFactor3(model, params); - smartFactor3.add(measurements_cam3, key_pairs, interp_factors, sharedK); + SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model,params)); + smartFactor3->add(measurements_cam3, key_pairs, interp_factors, sharedK); - SmartFactorRS smartFactor4(model, params); - smartFactor4.add(measurements_cam4, key_pairs, interp_factors, sharedK); + SmartFactorRS::shared_ptr smartFactor4(new SmartFactorRS(model,params)); + smartFactor4->add(measurements_cam4, key_pairs, interp_factors, sharedK); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -639,8 +638,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_dynamicOutlie graph.addPrior(x1, level_pose, noisePrior); graph.addPrior(x2, pose_right, noisePrior); - Pose3 noise_pose = Pose3::identity(); // Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), -// Point3(0.1, 0.1, 0.1)); // smaller noise + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.01, 0.01, 0.01)); // smaller noise, otherwise outlier rejection will kick in Values values; values.insert(x1, level_pose); values.insert(x2, pose_right); @@ -652,15 +651,11 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_dynamicOutlie LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); result = optimizer.optimize(); EXPECT(assert_equal(pose_above, result.at(x3), 1e-6)); - - smartFactor1.print("smartFactor1"); - smartFactor2.print("smartFactor2"); - smartFactor3.print("smartFactor3"); - smartFactor4.print("smartFactor4"); } /* ************************************************************************* TEST( SmartProjectionPoseFactorRollingShutter, 3poses_projection_factor ) { + std::cout << "===================" << std::endl; using namespace vanillaPose2; From 5350e3463ef8168653f2f16f241443eae851ce1d Mon Sep 17 00:00:00 2001 From: lcarlone Date: Fri, 23 Jul 2021 18:58:43 -0400 Subject: [PATCH 104/248] done with tests, now I only have to rename gamma to keep consistency with the projection factors RS --- ...martProjectionPoseFactorRollingShutter.cpp | 327 ++++++++---------- 1 file changed, 141 insertions(+), 186 deletions(-) diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index 8b2bc26dd..1cfe44d84 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -10,8 +10,8 @@ * -------------------------------------------------------------------------- */ /** - * @file testSmartProjectionPoseFactorRollingShutterRollingShutter.cpp - * @brief Unit tests for SmartProjectionPoseFactorRollingShutterRollingShutter Class + * @file testSmartProjectionPoseFactorRollingShutter.cpp + * @brief Unit tests for SmartProjectionPoseFactorRollingShutter Class * @author Luca Carlone * @date July 2021 */ @@ -301,12 +301,12 @@ TEST( SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians ) { TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses ) { using namespace vanillaPoseRS; - Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; + Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; // Project three landmarks into three cameras - projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); - projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); - projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); // create inputs std::vector> key_pairs; @@ -320,13 +320,13 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses ) { interp_factors.push_back(interp_factor3); SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); - smartFactor1->add(measurements_cam1, key_pairs, interp_factors, sharedK); + smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, sharedK); SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model)); - smartFactor2->add(measurements_cam2, key_pairs, interp_factors, sharedK); + smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, sharedK); SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model)); - smartFactor3->add(measurements_cam3, key_pairs, interp_factors, sharedK); + smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, sharedK); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -384,18 +384,18 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses ) { // one landmarks 1m in front of camera Point3 landmark1(0, 0, 10); - Point2Vector measurements_cam1; + Point2Vector measurements_lmk1; // Project 2 landmarks into 2 cameras - measurements_cam1.push_back(cam1.project(landmark1)); - measurements_cam1.push_back(cam2.project(landmark1)); + measurements_lmk1.push_back(cam1.project(landmark1)); + measurements_lmk1.push_back(cam2.project(landmark1)); SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); double interp_factor = 0; // equivalent to measurement taken at pose 1 - smartFactor1->add(measurements_cam1[0], x1, x2, interp_factor, sharedKSimple, + smartFactor1->add(measurements_lmk1[0], x1, x2, interp_factor, sharedKSimple, body_P_sensorId); interp_factor = 1; // equivalent to measurement taken at pose 2 - smartFactor1->add(measurements_cam1[1], x1, x2, interp_factor, sharedKSimple, + smartFactor1->add(measurements_lmk1[1], x1, x2, interp_factor, sharedKSimple, body_P_sensorId); SmartFactor::Cameras cameras; @@ -459,12 +459,12 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses ) { /* *************************************************************************/ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI ) { using namespace vanillaPoseRS; - Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; + Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; // Project three landmarks into three cameras - projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); - projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); - projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); // create inputs std::vector> key_pairs; @@ -486,13 +486,13 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI ) { params.setEnableEPI(true); SmartFactorRS smartFactor1(model,params); - smartFactor1.add(measurements_cam1, key_pairs, interp_factors, sharedK); + smartFactor1.add(measurements_lmk1, key_pairs, interp_factors, sharedK); SmartFactorRS smartFactor2(model,params); - smartFactor2.add(measurements_cam2, key_pairs, interp_factors, sharedK); + smartFactor2.add(measurements_lmk2, key_pairs, interp_factors, sharedK); SmartFactorRS smartFactor3(model,params); - smartFactor3.add(measurements_cam3, key_pairs, interp_factors, sharedK); + smartFactor3.add(measurements_lmk3, key_pairs, interp_factors, sharedK); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -521,12 +521,12 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI ) { /* *************************************************************************/ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_landmarkDistance ) { using namespace vanillaPoseRS; - Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; + Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; // Project three landmarks into three cameras - projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); - projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); - projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); // create inputs std::vector> key_pairs; @@ -548,13 +548,13 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_landmarkDista params.setEnableEPI(false); SmartFactorRS smartFactor1(model,params); - smartFactor1.add(measurements_cam1, key_pairs, interp_factors, sharedK); + smartFactor1.add(measurements_lmk1, key_pairs, interp_factors, sharedK); SmartFactorRS smartFactor2(model,params); - smartFactor2.add(measurements_cam2, key_pairs, interp_factors, sharedK); + smartFactor2.add(measurements_lmk2, key_pairs, interp_factors, sharedK); SmartFactorRS smartFactor3(model,params); - smartFactor3.add(measurements_cam3, key_pairs, interp_factors, sharedK); + smartFactor3.add(measurements_lmk3, key_pairs, interp_factors, sharedK); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -580,19 +580,20 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_landmarkDista EXPECT(assert_equal(values.at(x3), result.at(x3))); } - /* *************************************************************************/ +/* *************************************************************************/ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_dynamicOutlierRejection ) { using namespace vanillaPoseRS; // add fourth landmark Point3 landmark4(5, -0.5, 1); - Point2Vector measurements_cam1, measurements_cam2, measurements_cam3, measurements_cam4; + Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3, + measurements_lmk4; // Project 4 landmarks into cameras - projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); - projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); - projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4); - measurements_cam4.at(0) = measurements_cam4.at(0) + Point2(10, 10); // add outlier + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); + projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_lmk4); + measurements_lmk4.at(0) = measurements_lmk4.at(0) + Point2(10, 10); // add outlier // create inputs std::vector> key_pairs; @@ -616,17 +617,17 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_dynamicOutlie params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold); params.setEnableEPI(false); - SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model,params)); - smartFactor1->add(measurements_cam1, key_pairs, interp_factors, sharedK); + SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model, params)); + smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, sharedK); - SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model,params)); - smartFactor2->add(measurements_cam2, key_pairs, interp_factors, sharedK); + SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model, params)); + smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, sharedK); - SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model,params)); - smartFactor3->add(measurements_cam3, key_pairs, interp_factors, sharedK); + SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model, params)); + smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, sharedK); - SmartFactorRS::shared_ptr smartFactor4(new SmartFactorRS(model,params)); - smartFactor4->add(measurements_cam4, key_pairs, interp_factors, sharedK); + SmartFactorRS::shared_ptr smartFactor4(new SmartFactorRS(model, params)); + smartFactor4->add(measurements_lmk4, key_pairs, interp_factors, sharedK); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -651,170 +652,124 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_dynamicOutlie LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); result = optimizer.optimize(); EXPECT(assert_equal(pose_above, result.at(x3), 1e-6)); - } +} - /* ************************************************************************* - TEST( SmartProjectionPoseFactorRollingShutter, 3poses_projection_factor ) { - std::cout << "===================" << std::endl; +/* *************************************************************************/ +TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRollingShutter) { - using namespace vanillaPose2; + using namespace vanillaPoseRS; + Point2Vector measurements_lmk1; - KeyVector views {x1, x2, x3}; + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); - typedef GenericProjectionFactor ProjectionFactor; - NonlinearFactorGraph graph; + // create inputs + std::vector> key_pairs; + key_pairs.push_back(std::make_pair(x1, x2)); + key_pairs.push_back(std::make_pair(x2, x3)); + key_pairs.push_back(std::make_pair(x3, x1)); - // Project three landmarks into three cameras - graph.emplace_shared(cam1.project(landmark1), model, x1, L(1), sharedK2); - graph.emplace_shared(cam2.project(landmark1), model, x2, L(1), sharedK2); - graph.emplace_shared(cam3.project(landmark1), model, x3, L(1), sharedK2); + std::vector interp_factors; + interp_factors.push_back(interp_factor1); + interp_factors.push_back(interp_factor2); + interp_factors.push_back(interp_factor3); - graph.emplace_shared(cam1.project(landmark2), model, x1, L(2), sharedK2); - graph.emplace_shared(cam2.project(landmark2), model, x2, L(2), sharedK2); - graph.emplace_shared(cam3.project(landmark2), model, x3, L(2), sharedK2); + SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); + smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, sharedK); - graph.emplace_shared(cam1.project(landmark3), model, x1, L(3), sharedK2); - graph.emplace_shared(cam2.project(landmark3), model, x2, L(3), sharedK2); - graph.emplace_shared(cam3.project(landmark3), model, x3, L(3), sharedK2); + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise + Values values; + values.insert(x1, level_pose); + values.insert(x2, pose_right); + // initialize third pose with some noise to get a nontrivial linearization point + values.insert(x3, pose_above * noise_pose); + EXPECT( // check that the pose is actually noisy + assert_equal( Pose3( Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), Point3(0.1, -0.1, 1.9)), values.at(x3))); - const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - graph.addPrior(x1, level_pose, noisePrior); - graph.addPrior(x2, pose_right, noisePrior); + // linearization point for the poses + Pose3 pose1 = level_pose; + Pose3 pose2 = pose_right; + Pose3 pose3 = pose_above * noise_pose; - Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), - Point3(0.5, 0.1, 0.3)); - Values values; - values.insert(x1, level_pose); - values.insert(x2, pose_right); - values.insert(x3, pose_above * noise_pose); - values.insert(L(1), landmark1); - values.insert(L(2), landmark2); - values.insert(L(3), landmark3); + // ==== check Hessian of smartFactor1 ===== + // -- compute actual Hessian + boost::shared_ptr linearfactor1 = smartFactor1->linearize( + values); + Matrix actualHessian = linearfactor1->information(); - DOUBLES_EQUAL(48406055, graph.error(values), 1); + // -- compute expected Hessian from manual Schur complement from Jacobians + // linearization point for the 3D point + smartFactor1->triangulateSafe(smartFactor1->cameras(values)); + TriangulationResult point = smartFactor1->point(); + CHECK(point.valid()); // check triangulated point is valid - LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); - Values result = optimizer.optimize(); + // Use the factor to calculate the Jacobians + Matrix F = Matrix::Zero(2 * 3, 6 * 3); + Matrix E = Matrix::Zero(2 * 3, 3); + Vector b = Vector::Zero(6); - DOUBLES_EQUAL(0, graph.error(result), 1e-9); + // create projection factors rolling shutter + ProjectionFactorRollingShutter factor11(measurements_lmk1[0], interp_factor1, + model, x1, x2, l0, sharedK); + Matrix H1Actual, H2Actual, H3Actual; + // note: b is minus the reprojection error, cf the smart factor jacobian computation + b.segment<2>(0) = -factor11.evaluateError(pose1, pose2, *point, H1Actual, H2Actual, H3Actual); + F.block<2, 6>(0, 0) = H1Actual; + F.block<2, 6>(0, 6) = H2Actual; + E.block<2, 3>(0, 0) = H3Actual; - EXPECT(assert_equal(pose_above, result.at(x3), 1e-7)); - } + ProjectionFactorRollingShutter factor12(measurements_lmk1[1], interp_factor2, + model, x2, x3, l0, sharedK); + b.segment<2>(2) = -factor12.evaluateError(pose2, pose3, *point, H1Actual, H2Actual, H3Actual); + F.block<2, 6>(2, 6) = H1Actual; + F.block<2, 6>(2, 12) = H2Actual; + E.block<2, 3>(2, 0) = H3Actual; - /* ************************************************************************* - TEST( SmartProjectionPoseFactorRollingShutter, CheckHessian) { + ProjectionFactorRollingShutter factor13(measurements_lmk1[2], interp_factor3, + model, x3, x1, l0, sharedK); + b.segment<2>(4) = -factor13.evaluateError(pose3, pose1, *point, H1Actual, H2Actual, H3Actual); + F.block<2, 6>(4, 12) = H1Actual; + F.block<2, 6>(4, 0) = H2Actual; + E.block<2, 3>(4, 0) = H3Actual; - KeyVector views {x1, x2, x3}; + // whiten + F = (1/sigma) * F; + E = (1/sigma) * E; + b = (1/sigma) * b; + //* G = F' * F - F' * E * P * E' * F + Matrix P = (E.transpose() * E).inverse(); + Matrix expectedHessian = F.transpose() * F + - (F.transpose() * E * P * E.transpose() * F); + EXPECT(assert_equal(expectedHessian, actualHessian, 1e-6)); - using namespace vanillaPose; + // ==== check Information vector of smartFactor1 ===== + GaussianFactorGraph gfg; + gfg.add(linearfactor1); + Matrix actualHessian_v2 = gfg.hessian().first; + EXPECT(assert_equal(actualHessian_v2, actualHessian, 1e-6)); // sanity check on hessian - // Two slightly different cameras - Pose3 pose2 = level_pose - * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); - Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); - Camera cam2(pose2, sharedK); - Camera cam3(pose3, sharedK); + // -- compute actual information vector + Vector actualInfoVector = gfg.hessian().second; - Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; + // -- compute expected information vector from manual Schur complement from Jacobians + //* g = F' * (b - E * P * E' * b) + Vector expectedInfoVector = F.transpose() * (b - E * P * E.transpose() * b); + EXPECT(assert_equal(expectedInfoVector, actualInfoVector, 1e-6)); - // Project three landmarks into three cameras - projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); - projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); - projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + // ==== check error of smartFactor1 (again) ===== + NonlinearFactorGraph nfg_projFactorsRS; + nfg_projFactorsRS.add(factor11); + nfg_projFactorsRS.add(factor12); + nfg_projFactorsRS.add(factor13); + values.insert(l0, *point); - SmartProjectionParams params; - params.setRankTolerance(10); + double actualError = smartFactor1->error(values); + double expectedError = nfg_projFactorsRS.error(values); + EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); +} - SmartFactor::shared_ptr smartFactor1( - new SmartFactor(model, sharedK, params)); // HESSIAN, by default - smartFactor1->add(measurements_cam1, views); - - SmartFactor::shared_ptr smartFactor2( - new SmartFactor(model, sharedK, params)); // HESSIAN, by default - smartFactor2->add(measurements_cam2, views); - - SmartFactor::shared_ptr smartFactor3( - new SmartFactor(model, sharedK, params)); // HESSIAN, by default - smartFactor3->add(measurements_cam3, views); - - NonlinearFactorGraph graph; - graph.push_back(smartFactor1); - graph.push_back(smartFactor2); - graph.push_back(smartFactor3); - - // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), - Point3(0.1, 0.1, 0.1)); // smaller noise - Values values; - values.insert(x1, cam1.pose()); - values.insert(x2, cam2.pose()); - // initialize third pose with some noise, we expect it to move back to original pose_above - values.insert(x3, pose3 * noise_pose); - EXPECT( - assert_equal( - Pose3( - Rot3(0.00563056869, -0.130848107, 0.991386438, -0.991390265, - -0.130426831, -0.0115837907, 0.130819108, -0.98278564, - -0.130455917), - Point3(0.0897734171, -0.110201006, 0.901022872)), - values.at(x3))); - - boost::shared_ptr factor1 = smartFactor1->linearize(values); - boost::shared_ptr factor2 = smartFactor2->linearize(values); - boost::shared_ptr factor3 = smartFactor3->linearize(values); - - Matrix CumulativeInformation = factor1->information() + factor2->information() - + factor3->information(); - - boost::shared_ptr GaussianGraph = graph.linearize( - values); - Matrix GraphInformation = GaussianGraph->hessian().first; - - // Check Hessian - EXPECT(assert_equal(GraphInformation, CumulativeInformation, 1e-6)); - - Matrix AugInformationMatrix = factor1->augmentedInformation() - + factor2->augmentedInformation() + factor3->augmentedInformation(); - - // Check Information vector - Vector InfoVector = AugInformationMatrix.block(0, 18, 18, 1); // 18x18 Hessian + information vector - - // Check Hessian - EXPECT(assert_equal(InfoVector, GaussianGraph->hessian().second, 1e-6)); - } - - /* ************************************************************************* - TEST( SmartProjectionPoseFactorRollingShutter, Hessian ) { - - using namespace vanillaPose2; - - KeyVector views {x1, x2}; - - // Project three landmarks into 2 cameras - Point2 cam1_uv1 = cam1.project(landmark1); - Point2 cam2_uv1 = cam2.project(landmark1); - Point2Vector measurements_cam1; - measurements_cam1.push_back(cam1_uv1); - measurements_cam1.push_back(cam2_uv1); - - SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedK2)); - smartFactor1->add(measurements_cam1, views); - - Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), - Point3(0.5, 0.1, 0.3)); - Values values; - values.insert(x1, cam1.pose()); - values.insert(x2, cam2.pose()); - - boost::shared_ptr factor = smartFactor1->linearize(values); - - // compute triangulation from linearization point - // compute reprojection errors (sum squared) - // compare with factor.info(): the bottom right element is the squared sum of the reprojection errors (normalized by the covariance) - // check that it is correctly scaled when using noiseProjection = [1/4 0; 0 1/4] - } - - /* ************************************************************************* */ +/* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); From 1f07142b5bfdbb22e12f1ff559d666aae9d6e897 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Fri, 23 Jul 2021 19:03:23 -0400 Subject: [PATCH 105/248] renamed params. need one last test --- .../SmartProjectionPoseFactorRollingShutter.h | 36 +++++++++---------- 1 file changed, 18 insertions(+), 18 deletions(-) diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index 03c44dbe9..4d0d6d9e6 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -52,7 +52,7 @@ PinholePose > { std::vector> world_P_body_key_pairs_; /// interpolation factor (one for each observation) to interpolate between pair of consecutive poses - std::vector interp_param_; + std::vector interp_params_; /// Pose of the camera in the body frame std::vector body_P_sensors_; ///< Pose of the camera in the body frame @@ -95,11 +95,11 @@ PinholePose > { * single landmark in the a single view (the measurement), interpolated from the 2 poses * @param world_P_body_key1 is the key corresponding to the first body poses (time <= time pixel is acquired) * @param world_P_body_key2 is the key corresponding to the second body poses (time >= time pixel is acquired) - * @param gamma in [0,1] is the interpolation factor, such that if gamma = 0 the interpolated pose is the same as world_P_body_key + * @param interp_param in [0,1] is the interpolation factor, such that if interp_param = 0 the interpolated pose is the same as world_P_body_key * @param K is the (fixed) camera intrinsic calibration */ void add(const Point2& measured, const Key& world_P_body_key1, - const Key& world_P_body_key2, const double& gamma, + const Key& world_P_body_key2, const double& interp_param, const boost::shared_ptr& K, const Pose3 body_P_sensor = Pose3::identity()) { // store measurements in base class (note: we manyally add keys below to make sure they are unique this->measured_.push_back(measured); @@ -117,7 +117,7 @@ PinholePose > { this->keys_.push_back(world_P_body_key2); // add only unique keys // store interpolation factors - interp_param_.push_back(gamma); + interp_params_.push_back(interp_param); // store fixed calibration K_all_.push_back(K); @@ -135,15 +135,15 @@ PinholePose > { */ void add(const Point2Vector& measurements, const std::vector>& world_P_body_key_pairs, - const std::vector& gammas, + const std::vector& interp_params, const std::vector>& Ks, const std::vector body_P_sensors) { assert(world_P_body_key_pairs.size() == measurements.size()); - assert(world_P_body_key_pairs.size() == gammas.size()); + assert(world_P_body_key_pairs.size() == interp_params.size()); assert(world_P_body_key_pairs.size() == Ks.size()); for (size_t i = 0; i < measurements.size(); i++) { add(measurements[i], world_P_body_key_pairs[i].first, - world_P_body_key_pairs[i].second, gammas[i], Ks[i], + world_P_body_key_pairs[i].second, interp_params[i], Ks[i], body_P_sensors[i]); } } @@ -158,13 +158,13 @@ PinholePose > { */ void add(const Point2Vector& measurements, const std::vector>& world_P_body_key_pairs, - const std::vector& gammas, + const std::vector& interp_params, const boost::shared_ptr& K, const Pose3 body_P_sensor = Pose3::identity()) { assert(world_P_body_key_pairs.size() == measurements.size()); - assert(world_P_body_key_pairs.size() == gammas.size()); + assert(world_P_body_key_pairs.size() == interp_params.size()); for (size_t i = 0; i < measurements.size(); i++) { add(measurements[i], world_P_body_key_pairs[i].first, - world_P_body_key_pairs[i].second, gammas[i], K, body_P_sensor); + world_P_body_key_pairs[i].second, interp_params[i], K, body_P_sensor); } } @@ -178,9 +178,9 @@ PinholePose > { return world_P_body_key_pairs_; } - /// return the interpolation factors gammas - const std::vector getGammas() const { - return interp_param_; + /// return the interpolation factors interp_params + const std::vector interp_params() const { + return interp_params_; } /// return the extrinsic camera calibration body_P_sensors @@ -202,7 +202,7 @@ PinholePose > { << keyFormatter(world_P_body_key_pairs_[i].first) << std::endl; std::cout << " pose2 key: " << keyFormatter(world_P_body_key_pairs_[i].second) << std::endl; - std::cout << " gamma: " << interp_param_[i] << std::endl; + std::cout << " interp_param: " << interp_params_[i] << std::endl; body_P_sensors_[i].print("extrinsic calibration:\n"); K_all_[i]->print("intrinsic calibration = "); } @@ -237,7 +237,7 @@ PinholePose > { }else{ extrinsicCalibrationEqual = false; } return e && Base::equals(p, tol) && K_all_ == e->calibration() - && interp_param_ == e->getGammas() && keyPairsEqual && extrinsicCalibrationEqual; + && interp_params_ == e->interp_params() && keyPairsEqual && extrinsicCalibrationEqual; } /** @@ -264,7 +264,7 @@ PinholePose > { for (size_t i = 0; i < numViews; i++) { // for each camera/measurement Pose3 w_P_body1 = values.at(world_P_body_key_pairs_[i].first); Pose3 w_P_body2 = values.at(world_P_body_key_pairs_[i].second); - double interpolationFactor = interp_param_[i]; + double interpolationFactor = interp_params_[i]; // get interpolated pose: Pose3 w_P_body = interpolate(w_P_body1, w_P_body2,interpolationFactor, dInterpPose_dPoseBody1, dInterpPose_dPoseBody2); Pose3 body_P_cam = body_P_sensors_[i]; @@ -368,7 +368,7 @@ PinholePose > { typename Base::Cameras cameras(const Values& values) const override { size_t numViews = this->measured_.size(); assert(numViews == K_all_.size()); - assert(numViews == interp_param_.size()); + assert(numViews == interp_params_.size()); assert(numViews == body_P_sensors_.size()); assert(numViews == world_P_body_key_pairs_.size()); @@ -376,7 +376,7 @@ PinholePose > { for (size_t i = 0; i < numViews; i++) { // for each measurement Pose3 w_P_body1 = values.at(world_P_body_key_pairs_[i].first); Pose3 w_P_body2 = values.at(world_P_body_key_pairs_[i].second); - double interpolationFactor = interp_param_[i]; + double interpolationFactor = interp_params_[i]; Pose3 w_P_body = interpolate(w_P_body1, w_P_body2, interpolationFactor); Pose3 body_P_cam = body_P_sensors_[i]; Pose3 w_P_cam = w_P_body.compose(body_P_cam); From a10d495611e6bb875971b88185175f2aa3fcd4fe Mon Sep 17 00:00:00 2001 From: lcarlone Date: Fri, 23 Jul 2021 22:23:01 -0400 Subject: [PATCH 106/248] extra cleanup --- .../SmartProjectionPoseFactorRollingShutter.h | 117 ++++++++++-------- ...martProjectionPoseFactorRollingShutter.cpp | 4 +- 2 files changed, 65 insertions(+), 56 deletions(-) diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index 4d0d6d9e6..5c7f1a54e 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -17,9 +17,6 @@ #pragma once -#include -#include -#include #include namespace gtsam { @@ -35,8 +32,8 @@ namespace gtsam { */ /** - * This factor optimizes the pose of the body assuming a rolling shutter model of the camera with given readout time. - * This factor requires that values contain (for each pixel observation) consecutive camera poses + * This factor optimizes two consecutive poses of the body assuming a rolling shutter model of the camera with given readout time. + * The factor requires that values contain (for each pixel observation) two consecutive camera poses * from which the pixel observation pose can be interpolated. * @addtogroup SLAM */ @@ -55,7 +52,7 @@ PinholePose > { std::vector interp_params_; /// Pose of the camera in the body frame - std::vector body_P_sensors_; ///< Pose of the camera in the body frame + std::vector body_P_sensors_; public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW @@ -72,7 +69,7 @@ PinholePose > { static const int DimBlock = 12; ///< size of the variable stacking 2 poses from which the observation pose is interpolated static const int DimPose = 6; ///< Pose3 dimension static const int ZDim = 2; ///< Measurement dimension (Point2) - typedef Eigen::Matrix MatrixZD; // F blocks (derivatives wrt camera) + typedef Eigen::Matrix MatrixZD; // F blocks (derivatives wrt block of 2 poses) typedef std::vector > FBlocks; // vector of F blocks /** @@ -90,48 +87,50 @@ PinholePose > { ~SmartProjectionPoseFactorRollingShutter() override = default; /** - * add a new measurement, with 2 pose keys, camera calibration, and observed pixel. - * @param measured is the 2-dimensional location of the projection of a - * single landmark in the a single view (the measurement), interpolated from the 2 poses - * @param world_P_body_key1 is the key corresponding to the first body poses (time <= time pixel is acquired) - * @param world_P_body_key2 is the key corresponding to the second body poses (time >= time pixel is acquired) - * @param interp_param in [0,1] is the interpolation factor, such that if interp_param = 0 the interpolated pose is the same as world_P_body_key - * @param K is the (fixed) camera intrinsic calibration + * add a new measurement, with 2 pose keys, interpolation factor, camera (intrinsic and extrinsic) calibration, and observed pixel. + * @param measured 2-dimensional location of the projection of a + * single landmark in a single view (the measurement), interpolated from the 2 poses + * @param world_P_body_key1 key corresponding to the first body poses (time <= time pixel is acquired) + * @param world_P_body_key2 key corresponding to the second body poses (time >= time pixel is acquired) + * @param interp_param interpolation factor in [0,1], such that if interp_param = 0 the interpolated pose is the same as world_P_body_key1 + * @param K (fixed) camera intrinsic calibration + * @param body_P_sensor (fixed) camera extrinsic calibration */ void add(const Point2& measured, const Key& world_P_body_key1, const Key& world_P_body_key2, const double& interp_param, const boost::shared_ptr& K, const Pose3 body_P_sensor = Pose3::identity()) { - // store measurements in base class (note: we manyally add keys below to make sure they are unique + // store measurements in base class this->measured_.push_back(measured); - // but we also store the extrinsic calibration keys in the same order + // store the pair of keys for each measurement, in the same order world_P_body_key_pairs_.push_back( std::make_pair(world_P_body_key1, world_P_body_key2)); - // pose keys are assumed to be unique, so we avoid duplicates here - if (std::find(this->keys_.begin(), this->keys_.end(), world_P_body_key1) - == this->keys_.end()) + // also store keys in the keys_ vector: these keys are assumed to be unique, so we avoid duplicates here + if (std::find(this->keys_.begin(), this->keys_.end(), world_P_body_key1) == this->keys_.end()) this->keys_.push_back(world_P_body_key1); // add only unique keys - if (std::find(this->keys_.begin(), this->keys_.end(), world_P_body_key2) - == this->keys_.end()) + if (std::find(this->keys_.begin(), this->keys_.end(), world_P_body_key2) == this->keys_.end()) this->keys_.push_back(world_P_body_key2); // add only unique keys - // store interpolation factors + // store interpolation factor interp_params_.push_back(interp_param); - // store fixed calibration + // store fixed intrinsic calibration K_all_.push_back(K); - // store extrinsics of the camera + // store fixed extrinsics of the camera body_P_sensors_.push_back(body_P_sensor); } /** - * Variant of the previous one in which we include a set of measurements + * Variant of the previous "add" function in which we include multiple measurements * @param measurements vector of the 2m dimensional location of the projection * of a single landmark in the m views (the measurements) - * @param world_P_body_key_pairs vector of (1 for each view) containing the pair of poses from which each view can be interpolated - * @param Ks vector of intrinsic calibration objects + * @param world_P_body_key_pairs vector where the i-th element contains a pair of keys corresponding + * to the pair of poses from which the observation pose for the i0-th measurement can be interpolated + * @param interp_params vector of interpolation params, one for each measurement (in the same order) + * @param Ks vector of (fixed) intrinsic calibration objects + * @param body_P_sensors vector of (fixed) extrinsic calibration objects */ void add(const Point2Vector& measurements, const std::vector>& world_P_body_key_pairs, @@ -149,12 +148,15 @@ PinholePose > { } /** - * Variant of the previous one in which we include a set of measurements with - * the same (intrinsic and extrinsic) calibration + * Variant of the previous "add" function in which we include multiple measurements + * with the same (intrinsic and extrinsic) calibration * @param measurements vector of the 2m dimensional location of the projection * of a single landmark in the m views (the measurements) - * @param world_P_body_key_pairs vector of (1 for each view) containing the pair of poses from which each view can be interpolated - * @param K the (known) camera calibration (same for all measurements) + * @param world_P_body_key_pairs vector where the i-th element contains a pair of keys corresponding + * to the pair of poses from which the observation pose for the i0-th measurement can be interpolated + * @param interp_params vector of interpolation params, one for each measurement (in the same order) + * @param K (fixed) camera intrinsic calibration (same for all measurements) + * @param body_P_sensor (fixed) camera extrinsic calibration (same for all measurements) */ void add(const Point2Vector& measurements, const std::vector>& world_P_body_key_pairs, @@ -173,7 +175,7 @@ PinholePose > { return K_all_; } - /// return (for each observation) the key of the pair of poses from which we interpolate + /// return (for each observation) the keys of the pair of poses from which we interpolate const std::vector> world_P_body_key_pairs() const { return world_P_body_key_pairs_; } @@ -245,7 +247,7 @@ PinholePose > { * @param values Values structure which must contain camera poses * corresponding to keys involved in this factor * @return Return arguments are the camera jacobians Fs (including the jacobian with - * respect to both the body pose and extrinsic pose), the point Jacobian E, + * respect to both body poses we interpolate from), the point Jacobian E, * and the error vector b. Note that the jacobians are computed for a given point. */ void computeJacobiansWithTriangulatedPoint(FBlocks& Fs, Matrix& E, Vector& b, @@ -256,19 +258,20 @@ PinholePose > { size_t numViews = this->measured_.size(); E = Matrix::Zero(2 * numViews, 3); // a Point2 for each view (point jacobian) b = Vector::Zero(2 * numViews); // a Point2 for each view + // intermediate Jacobians Eigen::Matrix dProject_dPoseCam; Eigen::Matrix dInterpPose_dPoseBody1, dInterpPose_dPoseBody2, dPoseCam_dInterpPose; Eigen::Matrix Ei; for (size_t i = 0; i < numViews; i++) { // for each camera/measurement - Pose3 w_P_body1 = values.at(world_P_body_key_pairs_[i].first); - Pose3 w_P_body2 = values.at(world_P_body_key_pairs_[i].second); + const Pose3& w_P_body1 = values.at(world_P_body_key_pairs_[i].first); + const Pose3& w_P_body2 = values.at(world_P_body_key_pairs_[i].second); double interpolationFactor = interp_params_[i]; // get interpolated pose: - Pose3 w_P_body = interpolate(w_P_body1, w_P_body2,interpolationFactor, dInterpPose_dPoseBody1, dInterpPose_dPoseBody2); - Pose3 body_P_cam = body_P_sensors_[i]; - Pose3 w_P_cam = w_P_body.compose(body_P_cam, dPoseCam_dInterpPose); + const Pose3& w_P_body = interpolate(w_P_body1, w_P_body2,interpolationFactor, dInterpPose_dPoseBody1, dInterpPose_dPoseBody2); + const Pose3& body_P_cam = body_P_sensors_[i]; + const Pose3& w_P_cam = w_P_body.compose(body_P_cam, dPoseCam_dInterpPose); PinholeCamera camera(w_P_cam, *K_all_[i]); // get jacobians and error vector for current measurement @@ -297,14 +300,14 @@ PinholePose > { // we may have multiple observation sharing the same keys (due to the rolling shutter interpolation), // hence the number of unique keys may be smaller than 2 * nrMeasurements - size_t nrUniqueKeys = this->keys_.size(); + size_t nrUniqueKeys = this->keys_.size(); // note: by construction, keys_ only contains unique keys // Create structures for Hessian Factors KeyVector js; std::vector < Matrix > Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2); std::vector < Vector > gs(nrUniqueKeys); - if (this->measured_.size() != this->cameras(values).size()) + if (this->measured_.size() != this->cameras(values).size()) // 1 observation per interpolated camera throw std::runtime_error("SmartProjectionPoseFactorRollingShutter: " "measured_.size() inconsistent with input"); @@ -312,12 +315,17 @@ PinholePose > { this->triangulateSafe(this->cameras(values)); if (!this->result_) { // failed: return "empty/zero" Hessian - for (Matrix& m : Gs) - m = Matrix::Zero(DimPose, DimPose); - for (Vector& v : gs) - v = Vector::Zero(DimPose); - return boost::make_shared < RegularHessianFactor - > (this->keys_, Gs, gs, 0.0); + if (this->params_.degeneracyMode == ZERO_ON_DEGENERACY) { + for (Matrix& m : Gs) + m = Matrix::Zero(DimPose, DimPose); + for (Vector& v : gs) + v = Vector::Zero(DimPose); + return boost::make_shared < RegularHessianFactor + > (this->keys_, Gs, gs, 0.0); + } else { + throw std::runtime_error("SmartProjectionPoseFactorRollingShutter: " + "only supported degeneracy mode is ZERO_ON_DEGENERACY"); + } } // compute Jacobian given triangulated 3D Point FBlocks Fs; @@ -332,14 +340,15 @@ PinholePose > { Matrix3 P = Base::Cameras::PointCov(E, lambda, diagonalDamping); - // build augmented Hessian (with last row/column being the information vector) - // these are the keys that correspond to the blocks in augmentedHessian (output of SchurComplement) + // Collect all the key pairs: these are the keys that correspond to the blocks in Fs (on which we apply the Schur Complement) KeyVector nonuniqueKeys; for (size_t i = 0; i < world_P_body_key_pairs_.size(); i++) { nonuniqueKeys.push_back(world_P_body_key_pairs_.at(i).first); nonuniqueKeys.push_back(world_P_body_key_pairs_.at(i).second); } - // but we need to get the augumented hessian wrt the unique keys in key_ + + // Build augmented Hessian (with last row/column being the information vector) + // Note: we need to get the augumented hessian wrt the unique keys in key_ SymmetricBlockMatrix augmentedHessianUniqueKeys = Base::Cameras::SchurComplementAndRearrangeBlocks_3_12_6( Fs, E, P, b, nonuniqueKeys, this->keys_); @@ -374,12 +383,12 @@ PinholePose > { typename Base::Cameras cameras; for (size_t i = 0; i < numViews; i++) { // for each measurement - Pose3 w_P_body1 = values.at(world_P_body_key_pairs_[i].first); - Pose3 w_P_body2 = values.at(world_P_body_key_pairs_[i].second); + const Pose3& w_P_body1 = values.at(world_P_body_key_pairs_[i].first); + const Pose3& w_P_body2 = values.at(world_P_body_key_pairs_[i].second); double interpolationFactor = interp_params_[i]; - Pose3 w_P_body = interpolate(w_P_body1, w_P_body2, interpolationFactor); - Pose3 body_P_cam = body_P_sensors_[i]; - Pose3 w_P_cam = w_P_body.compose(body_P_cam); + const Pose3& w_P_body = interpolate(w_P_body1, w_P_body2, interpolationFactor); + const Pose3& body_P_cam = body_P_sensors_[i]; + const Pose3& w_P_cam = w_P_body.compose(body_P_cam); cameras.emplace_back(w_P_cam, K_all_[i]); } return cameras; diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index 1cfe44d84..a89a490e5 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -481,7 +481,7 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI ) { SmartProjectionParams params; params.setRankTolerance(1.0); params.setLinearizationMode(gtsam::HESSIAN); - params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); + params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); params.setEnableEPI(true); @@ -612,7 +612,7 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_dynamicOutlie SmartProjectionParams params; params.setRankTolerance(1.0); params.setLinearizationMode(gtsam::HESSIAN); - params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); + params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold); params.setEnableEPI(false); From e838d011a6223fd0f693ea2dba28ac9f53053e1d Mon Sep 17 00:00:00 2001 From: lcarlone Date: Fri, 23 Jul 2021 22:48:16 -0400 Subject: [PATCH 107/248] added timing test --- ...martProjectionPoseFactorRollingShutter.cpp | 64 +++++++++++++++++++ 1 file changed, 64 insertions(+) diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index a89a490e5..f75e2fea2 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -27,6 +27,7 @@ #include #include #include +#define DISABLE_TIMING using namespace gtsam; using namespace boost::assign; @@ -769,6 +770,69 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRolli EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); } +#ifndef DISABLE_TIMING +#include +// -Total: 0 CPU (0 times, 0 wall, 0.04 children, min: 0 max: 0) +//| -SF RS LINEARIZE: 0.02 CPU (1000 times, 0.017244 wall, 0.02 children, min: 0 max: 0) +//| -RS LINEARIZE: 0.02 CPU (1000 times, 0.009035 wall, 0.02 children, min: 0 max: 0) +/* *************************************************************************/ +TEST( SmartProjectionPoseFactorRollingShutter, timing ) { + + using namespace vanillaPose; + + // Default cameras for simple derivatives + static Cal3_S2::shared_ptr sharedKSimple(new Cal3_S2(100, 100, 0, 0, 0)); + + Rot3 R = Rot3::identity(); + Pose3 pose1 = Pose3(R, Point3(0, 0, 0)); + Pose3 pose2 = Pose3(R, Point3(1, 0, 0)); + Camera cam1(pose1, sharedKSimple), cam2(pose2, sharedKSimple); + Pose3 body_P_sensorId = Pose3::identity(); + + // one landmarks 1m in front of camera + Point3 landmark1(0, 0, 10); + + Point2Vector measurements_lmk1; + + // Project 2 landmarks into 2 cameras + measurements_lmk1.push_back(cam1.project(landmark1)); + measurements_lmk1.push_back(cam2.project(landmark1)); + + size_t nrTests = 1000; + + for(size_t i = 0; iadd(measurements_lmk1[0], x1, x2, interp_factor, sharedKSimple, + body_P_sensorId); + interp_factor = 1; // equivalent to measurement taken at pose 2 + smartFactorRS->add(measurements_lmk1[1], x1, x2, interp_factor, sharedKSimple, + body_P_sensorId); + + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + gttic_(SF_RS_LINEARIZE); + smartFactorRS->linearize(values); + gttoc_(SF_RS_LINEARIZE); + } + + for(size_t i = 0; iadd(measurements_lmk1[0], x1); + smartFactor->add(measurements_lmk1[1], x2); + + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + gttic_(RS_LINEARIZE); + smartFactor->linearize(values); + gttoc_(RS_LINEARIZE); + } + tictoc_print_(); +} +#endif + /* ************************************************************************* */ int main() { TestResult tr; From e4f1bb1bd0df65f8ed333cdb4c331a39aacae211 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Fri, 23 Jul 2021 22:56:37 -0400 Subject: [PATCH 108/248] CHECK -> EXPECT --- ...martProjectionPoseFactorRollingShutter.cpp | 76 +++++++++---------- 1 file changed, 38 insertions(+), 38 deletions(-) diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index f75e2fea2..75aaf4ac1 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -138,8 +138,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, Equals ) { factor1->add(measurement2, x2, x3, interp_factor2, sharedK, body_P_sensor); factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor); - CHECK(assert_equal(*factor1, *factor2)); - CHECK(assert_equal(*factor1, *factor3)); + EXPECT(assert_equal(*factor1, *factor2)); + EXPECT(assert_equal(*factor1, *factor3)); } { // create slightly different factors (different keys) and show equal returns false SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); @@ -147,8 +147,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, Equals ) { factor1->add(measurement2, x2, x2, interp_factor2, sharedK, body_P_sensor); // different! factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor); - CHECK(!assert_equal(*factor1, *factor2)); - CHECK(!assert_equal(*factor1, *factor3)); + EXPECT(!assert_equal(*factor1, *factor2)); + EXPECT(!assert_equal(*factor1, *factor3)); } { // create slightly different factors (different extrinsics) and show equal returns false SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); @@ -156,8 +156,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, Equals ) { factor1->add(measurement2, x2, x3, interp_factor2, sharedK, body_P_sensor*body_P_sensor); // different! factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor); - CHECK(!assert_equal(*factor1, *factor2)); - CHECK(!assert_equal(*factor1, *factor3)); + EXPECT(!assert_equal(*factor1, *factor2)); + EXPECT(!assert_equal(*factor1, *factor3)); } { // create slightly different factors (different interp factors) and show equal returns false SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); @@ -165,8 +165,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, Equals ) { factor1->add(measurement2, x2, x3, interp_factor1, sharedK, body_P_sensor); // different! factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor); - CHECK(!assert_equal(*factor1, *factor2)); - CHECK(!assert_equal(*factor1, *factor3)); + EXPECT(!assert_equal(*factor1, *factor2)); + EXPECT(!assert_equal(*factor1, *factor3)); } } @@ -200,8 +200,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, noiselessErrorAndJacobians ) { // Check triangulation factor.triangulateSafe(factor.cameras(values)); TriangulationResult point = factor.point(); - CHECK(point.valid()); // check triangulated point is valid - CHECK(assert_equal(landmark1, *point)); // check triangulation result matches expected 3D landmark + EXPECT(point.valid()); // check triangulated point is valid + EXPECT(assert_equal(landmark1, *point)); // check triangulation result matches expected 3D landmark // Check Jacobians // -- actual Jacobians @@ -209,28 +209,28 @@ TEST( SmartProjectionPoseFactorRollingShutter, noiselessErrorAndJacobians ) { Matrix actualE; Vector actualb; factor.computeJacobiansWithTriangulatedPoint(actualFs, actualE, actualb, values); - CHECK(actualE.rows() == 4); CHECK(actualE.cols() == 3); - CHECK(actualb.rows() == 4); CHECK(actualb.cols() == 1); - CHECK(actualFs.size() == 2); + EXPECT(actualE.rows() == 4); EXPECT(actualE.cols() == 3); + EXPECT(actualb.rows() == 4); EXPECT(actualb.cols() == 1); + EXPECT(actualFs.size() == 2); // -- expected Jacobians from ProjectionFactorsRollingShutter ProjectionFactorRollingShutter factor1(level_uv, interp_factor1, model, x1, x2, l0, sharedK, body_P_sensorId); Matrix expectedF11, expectedF12, expectedE1; Vector expectedb1 = factor1.evaluateError(level_pose, pose_right, landmark1, expectedF11, expectedF12, expectedE1); - CHECK(assert_equal( expectedF11, Matrix(actualFs[0].block(0,0,2,6)), 1e-5)); - CHECK(assert_equal( expectedF12, Matrix(actualFs[0].block(0,6,2,6)), 1e-5)); - CHECK(assert_equal( expectedE1, Matrix(actualE.block(0,0,2,3)), 1e-5)); + EXPECT(assert_equal( expectedF11, Matrix(actualFs[0].block(0,0,2,6)), 1e-5)); + EXPECT(assert_equal( expectedF12, Matrix(actualFs[0].block(0,6,2,6)), 1e-5)); + EXPECT(assert_equal( expectedE1, Matrix(actualE.block(0,0,2,3)), 1e-5)); // by definition computeJacobiansWithTriangulatedPoint returns minus reprojectionError - CHECK(assert_equal( expectedb1, -Vector(actualb.segment<2>(0)), 1e-5)); + EXPECT(assert_equal( expectedb1, -Vector(actualb.segment<2>(0)), 1e-5)); ProjectionFactorRollingShutter factor2(level_uv_right, interp_factor2, model, x2, x3, l0, sharedK, body_P_sensorId); Matrix expectedF21, expectedF22, expectedE2; Vector expectedb2 = factor2.evaluateError(pose_right, pose_above, landmark1, expectedF21, expectedF22, expectedE2); - CHECK(assert_equal( expectedF21, Matrix(actualFs[1].block(0,0,2,6)), 1e-5)); - CHECK(assert_equal( expectedF22, Matrix(actualFs[1].block(0,6,2,6)), 1e-5)); - CHECK(assert_equal( expectedE2, Matrix(actualE.block(2,0,2,3)), 1e-5)); + EXPECT(assert_equal( expectedF21, Matrix(actualFs[1].block(0,0,2,6)), 1e-5)); + EXPECT(assert_equal( expectedF22, Matrix(actualFs[1].block(0,6,2,6)), 1e-5)); + EXPECT(assert_equal( expectedE2, Matrix(actualE.block(2,0,2,3)), 1e-5)); // by definition computeJacobiansWithTriangulatedPoint returns minus reprojectionError - CHECK(assert_equal( expectedb2, -Vector(actualb.segment<2>(2)), 1e-5)); + EXPECT(assert_equal( expectedb2, -Vector(actualb.segment<2>(2)), 1e-5)); } /* *************************************************************************/ @@ -256,7 +256,7 @@ TEST( SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians ) { // Perform triangulation factor.triangulateSafe(factor.cameras(values)); TriangulationResult point = factor.point(); - CHECK(point.valid()); // check triangulated point is valid + EXPECT(point.valid()); // check triangulated point is valid Point3 landmarkNoisy = *point; // Check Jacobians @@ -265,28 +265,28 @@ TEST( SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians ) { Matrix actualE; Vector actualb; factor.computeJacobiansWithTriangulatedPoint(actualFs, actualE, actualb, values); - CHECK(actualE.rows() == 4); CHECK(actualE.cols() == 3); - CHECK(actualb.rows() == 4); CHECK(actualb.cols() == 1); - CHECK(actualFs.size() == 2); + EXPECT(actualE.rows() == 4); EXPECT(actualE.cols() == 3); + EXPECT(actualb.rows() == 4); EXPECT(actualb.cols() == 1); + EXPECT(actualFs.size() == 2); // -- expected Jacobians from ProjectionFactorsRollingShutter ProjectionFactorRollingShutter factor1(level_uv, interp_factor1, model, x1, x2, l0, sharedK, body_P_sensorNonId); Matrix expectedF11, expectedF12, expectedE1; Vector expectedb1 = factor1.evaluateError(level_pose, pose_right, landmarkNoisy, expectedF11, expectedF12, expectedE1); - CHECK(assert_equal( expectedF11, Matrix(actualFs[0].block(0,0,2,6)), 1e-5)); - CHECK(assert_equal( expectedF12, Matrix(actualFs[0].block(0,6,2,6)), 1e-5)); - CHECK(assert_equal( expectedE1, Matrix(actualE.block(0,0,2,3)), 1e-5)); + EXPECT(assert_equal( expectedF11, Matrix(actualFs[0].block(0,0,2,6)), 1e-5)); + EXPECT(assert_equal( expectedF12, Matrix(actualFs[0].block(0,6,2,6)), 1e-5)); + EXPECT(assert_equal( expectedE1, Matrix(actualE.block(0,0,2,3)), 1e-5)); // by definition computeJacobiansWithTriangulatedPoint returns minus reprojectionError - CHECK(assert_equal( expectedb1, -Vector(actualb.segment<2>(0)), 1e-5)); + EXPECT(assert_equal( expectedb1, -Vector(actualb.segment<2>(0)), 1e-5)); ProjectionFactorRollingShutter factor2(level_uv_right, interp_factor2, model, x2, x3, l0, sharedK, body_P_sensorNonId); Matrix expectedF21, expectedF22, expectedE2; Vector expectedb2 = factor2.evaluateError(pose_right, pose_above, landmarkNoisy, expectedF21, expectedF22, expectedE2); - CHECK(assert_equal( expectedF21, Matrix(actualFs[1].block(0,0,2,6)), 1e-5)); - CHECK(assert_equal( expectedF22, Matrix(actualFs[1].block(0,6,2,6)), 1e-5)); - CHECK(assert_equal( expectedE2, Matrix(actualE.block(2,0,2,3)), 1e-5)); + EXPECT(assert_equal( expectedF21, Matrix(actualFs[1].block(0,0,2,6)), 1e-5)); + EXPECT(assert_equal( expectedF22, Matrix(actualFs[1].block(0,6,2,6)), 1e-5)); + EXPECT(assert_equal( expectedE2, Matrix(actualE.block(2,0,2,3)), 1e-5)); // by definition computeJacobiansWithTriangulatedPoint returns minus reprojectionError - CHECK(assert_equal( expectedb2, -Vector(actualb.segment<2>(2)), 1e-5)); + EXPECT(assert_equal( expectedb2, -Vector(actualb.segment<2>(2)), 1e-5)); // Check errors double actualError = factor.error(values); // from smart factor @@ -404,11 +404,11 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses ) { cameras.push_back(cam2); // Make sure triangulation works - CHECK(smartFactor1->triangulateSafe(cameras)); - CHECK(!smartFactor1->isDegenerate()); - CHECK(!smartFactor1->isPointBehindCamera()); + EXPECT(smartFactor1->triangulateSafe(cameras)); + EXPECT(!smartFactor1->isDegenerate()); + EXPECT(!smartFactor1->isPointBehindCamera()); boost::optional p = smartFactor1->point(); - CHECK(p); + EXPECT(p); EXPECT(assert_equal(landmark1, *p)); VectorValues zeroDelta; @@ -703,7 +703,7 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRolli // linearization point for the 3D point smartFactor1->triangulateSafe(smartFactor1->cameras(values)); TriangulationResult point = smartFactor1->point(); - CHECK(point.valid()); // check triangulated point is valid + EXPECT(point.valid()); // check triangulated point is valid // Use the factor to calculate the Jacobians Matrix F = Matrix::Zero(2 * 3, 6 * 3); From 98340420407e4896fac02a8d97c0c20dd9a62876 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sat, 24 Jul 2021 00:15:33 -0400 Subject: [PATCH 109/248] test still in progress; removed a tmp function --- gtsam/geometry/CameraSet.h | 7 --- gtsam/geometry/tests/testCameraSet.cpp | 87 ++++++++++++++++++++++++++ 2 files changed, 87 insertions(+), 7 deletions(-) diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h index 19ab7ab31..f0bfffb58 100644 --- a/gtsam/geometry/CameraSet.h +++ b/gtsam/geometry/CameraSet.h @@ -142,13 +142,6 @@ public: return ErrorVector(project2(point, Fs, E), measured); } - static SymmetricBlockMatrix SchurComplement312( - const std::vector< Eigen::Matrix, - Eigen::aligned_allocator< Eigen::Matrix > >& Fs, - const Matrix& E, const Eigen::Matrix& P, const Vector& b) { - return SchurComplement<3,12>(Fs, E, P, b); - } - /** * Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix * G = F' * F - F' * E * P * E' * F diff --git a/gtsam/geometry/tests/testCameraSet.cpp b/gtsam/geometry/tests/testCameraSet.cpp index 761ef3a8c..e706b326b 100644 --- a/gtsam/geometry/tests/testCameraSet.cpp +++ b/gtsam/geometry/tests/testCameraSet.cpp @@ -125,6 +125,93 @@ TEST(CameraSet, Pinhole) { EXPECT(assert_equal(actualE, E)); } +/* ************************************************************************* */ +TEST(CameraSet, SchurComplementAndRearrangeBlocks) { + typedef PinholePose Camera; + typedef CameraSet Set; + typedef Point2Vector ZZ; + + KeyVector nonuniqueKeys; + nonuniqueKeys.push_back(0); + nonuniqueKeys.push_back(1); + nonuniqueKeys.push_back(1); + nonuniqueKeys.push_back(2); + nonuniqueKeys.push_back(2); + nonuniqueKeys.push_back(0); + + KeyVector uniqueKeys; + uniqueKeys.push_back(0); + uniqueKeys.push_back(1); + uniqueKeys.push_back(2); + + // this is the (block) Jacobian with respect to the nonuniqueKeys + std::vector, + Eigen::aligned_allocator > > Fs; + Fs.push_back(1 * Matrix::Ones(2,12)); // corresponding to key pair (0,1) + Fs.push_back(2 * Matrix::Ones(2,12)); // corresponding to key pair (1,2) + Fs.push_back(3 * Matrix::Ones(2,12)); // corresponding to key pair (2,0) + Matrix E = Matrix::Identity(6,3) + Matrix::Ones(6,3); + Matrix34 Et = E.transpose(); + Matrix P = (Et * E).inverse(); + Vector b = 5*Vector::Ones(6); + +// { // SchurComplement +// // Actual +// SymmetricBlockMatrix augmentedHessianBM = Set::SchurComplement<3,12>(Fs,E,P,b); +// Matrix actualAugmentedHessian = augmentedHessianBM.selfadjointView(); +// +// // Expected +// Matrix F = Matrix::Zero(6,3*12); +// F.block<2,12>(0,0) = Fs[0]; +// F.block<2,12>(2,12) = Fs[1]; +// F.block<2,12>(4,24) = Fs[2]; +// +// std::cout << "E \n" << E << std::endl; +// std::cout << "P \n" << P << std::endl; +// std::cout << "F \n" << F << std::endl; +// +// Matrix Ft = F.transpose(); +// Matrix H = Ft * F - Ft * E * P * Et * F; +// Vector v = Ft * (b - E * P * Et * b); +// Matrix expectedAugmentedHessian = Matrix::Zero(3*12+1, 3*12+1); +// expectedAugmentedHessian.block<36,36>(0,0) = H; +// expectedAugmentedHessian.block<36,1>(0,36) = v; +// expectedAugmentedHessian.block<1,36>(36,0) = v.transpose(); +// expectedAugmentedHessian(36,36) = b.squaredNorm(); +// +// EXPECT(assert_equal(expectedAugmentedHessian, actualAugmentedHessian)); +// } + +// { // SchurComplementAndRearrangeBlocks +// // Actual +// SymmetricBlockMatrix augmentedHessianBM = Set::SchurComplementAndRearrangeBlocks<3,12,6>( +// Fs,E,P,b,nonuniqueKeys,uniqueKeys); +// Matrix actualAugmentedHessian = augmentedHessianBM.selfadjointView(); +// +// // Expected +// // we first need to build the Jacobian F according to unique keys +// Matrix F = Matrix::Zero(6,18); +// F.block<2,6>(0,0) = Fs[0].block<2,6>(0,0); +// F.block<2,6>(0,6) = Fs[0].block<2,6>(0,6); +// F.block<2,6>(2,6) = Fs[1].block<2,6>(0,0); +// F.block<2,6>(2,12) = Fs[1].block<2,6>(0,6); +// F.block<2,6>(4,12) = Fs[2].block<2,6>(0,0); +// F.block<2,6>(4,0) = Fs[2].block<2,6>(0,6); +// +// std::cout << "P \n" << P << std::endl; +// std::cout << "F \n" << F << std::endl; +// +// Matrix Ft = F.transpose(); +// Matrix34 Et = E.transpose(); +// Vector v = Ft * (b - E * P * Et * b); +// Matrix H = Ft * F - Ft * E * P * Et * F; +// Matrix expectedAugmentedHessian(19, 19); +// expectedAugmentedHessian << H, v, v.transpose(), b.squaredNorm(); +// +// EXPECT(assert_equal(expectedAugmentedHessian, actualAugmentedHessian)); +// } +} + /* ************************************************************************* */ #include TEST(CameraSet, Stereo) { From 2f03e588fce0727e5895634dd500a66286fd684c Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sat, 24 Jul 2021 00:47:17 -0400 Subject: [PATCH 110/248] fixed last test - this is good to go! --- gtsam/geometry/tests/testCameraSet.cpp | 152 ++++++++++++------------- 1 file changed, 75 insertions(+), 77 deletions(-) diff --git a/gtsam/geometry/tests/testCameraSet.cpp b/gtsam/geometry/tests/testCameraSet.cpp index e706b326b..9118c8899 100644 --- a/gtsam/geometry/tests/testCameraSet.cpp +++ b/gtsam/geometry/tests/testCameraSet.cpp @@ -17,6 +17,7 @@ */ #include +#include #include #include #include @@ -127,89 +128,86 @@ TEST(CameraSet, Pinhole) { /* ************************************************************************* */ TEST(CameraSet, SchurComplementAndRearrangeBlocks) { - typedef PinholePose Camera; + typedef PinholePose Camera; typedef CameraSet Set; - typedef Point2Vector ZZ; - - KeyVector nonuniqueKeys; - nonuniqueKeys.push_back(0); - nonuniqueKeys.push_back(1); - nonuniqueKeys.push_back(1); - nonuniqueKeys.push_back(2); - nonuniqueKeys.push_back(2); - nonuniqueKeys.push_back(0); - - KeyVector uniqueKeys; - uniqueKeys.push_back(0); - uniqueKeys.push_back(1); - uniqueKeys.push_back(2); // this is the (block) Jacobian with respect to the nonuniqueKeys - std::vector, - Eigen::aligned_allocator > > Fs; - Fs.push_back(1 * Matrix::Ones(2,12)); // corresponding to key pair (0,1) - Fs.push_back(2 * Matrix::Ones(2,12)); // corresponding to key pair (1,2) - Fs.push_back(3 * Matrix::Ones(2,12)); // corresponding to key pair (2,0) - Matrix E = Matrix::Identity(6,3) + Matrix::Ones(6,3); - Matrix34 Et = E.transpose(); + std::vector, + Eigen::aligned_allocator > > Fs; + Fs.push_back(1 * Matrix::Ones(2, 12)); // corresponding to key pair (0,1) + Fs.push_back(2 * Matrix::Ones(2, 12)); // corresponding to key pair (1,2) + Fs.push_back(3 * Matrix::Ones(2, 12)); // corresponding to key pair (2,0) + Matrix E = 4 * Matrix::Identity(6, 3) + Matrix::Ones(6, 3); + E(0, 0) = 3; + E(1, 1) = 2; + E(2, 2) = 5; + Matrix Et = E.transpose(); Matrix P = (Et * E).inverse(); - Vector b = 5*Vector::Ones(6); + Vector b = 5 * Vector::Ones(6); -// { // SchurComplement -// // Actual -// SymmetricBlockMatrix augmentedHessianBM = Set::SchurComplement<3,12>(Fs,E,P,b); -// Matrix actualAugmentedHessian = augmentedHessianBM.selfadjointView(); -// -// // Expected -// Matrix F = Matrix::Zero(6,3*12); -// F.block<2,12>(0,0) = Fs[0]; -// F.block<2,12>(2,12) = Fs[1]; -// F.block<2,12>(4,24) = Fs[2]; -// -// std::cout << "E \n" << E << std::endl; -// std::cout << "P \n" << P << std::endl; -// std::cout << "F \n" << F << std::endl; -// -// Matrix Ft = F.transpose(); -// Matrix H = Ft * F - Ft * E * P * Et * F; -// Vector v = Ft * (b - E * P * Et * b); -// Matrix expectedAugmentedHessian = Matrix::Zero(3*12+1, 3*12+1); -// expectedAugmentedHessian.block<36,36>(0,0) = H; -// expectedAugmentedHessian.block<36,1>(0,36) = v; -// expectedAugmentedHessian.block<1,36>(36,0) = v.transpose(); -// expectedAugmentedHessian(36,36) = b.squaredNorm(); -// -// EXPECT(assert_equal(expectedAugmentedHessian, actualAugmentedHessian)); -// } + { // SchurComplement + // Actual + SymmetricBlockMatrix augmentedHessianBM = Set::SchurComplement<3, 12>(Fs, E, + P, b); + Matrix actualAugmentedHessian = augmentedHessianBM.selfadjointView(); -// { // SchurComplementAndRearrangeBlocks -// // Actual -// SymmetricBlockMatrix augmentedHessianBM = Set::SchurComplementAndRearrangeBlocks<3,12,6>( -// Fs,E,P,b,nonuniqueKeys,uniqueKeys); -// Matrix actualAugmentedHessian = augmentedHessianBM.selfadjointView(); -// -// // Expected -// // we first need to build the Jacobian F according to unique keys -// Matrix F = Matrix::Zero(6,18); -// F.block<2,6>(0,0) = Fs[0].block<2,6>(0,0); -// F.block<2,6>(0,6) = Fs[0].block<2,6>(0,6); -// F.block<2,6>(2,6) = Fs[1].block<2,6>(0,0); -// F.block<2,6>(2,12) = Fs[1].block<2,6>(0,6); -// F.block<2,6>(4,12) = Fs[2].block<2,6>(0,0); -// F.block<2,6>(4,0) = Fs[2].block<2,6>(0,6); -// -// std::cout << "P \n" << P << std::endl; -// std::cout << "F \n" << F << std::endl; -// -// Matrix Ft = F.transpose(); -// Matrix34 Et = E.transpose(); -// Vector v = Ft * (b - E * P * Et * b); -// Matrix H = Ft * F - Ft * E * P * Et * F; -// Matrix expectedAugmentedHessian(19, 19); -// expectedAugmentedHessian << H, v, v.transpose(), b.squaredNorm(); -// -// EXPECT(assert_equal(expectedAugmentedHessian, actualAugmentedHessian)); -// } + // Expected + Matrix F = Matrix::Zero(6, 3 * 12); + F.block<2, 12>(0, 0) = 1 * Matrix::Ones(2, 12); + F.block<2, 12>(2, 12) = 2 * Matrix::Ones(2, 12); + F.block<2, 12>(4, 24) = 3 * Matrix::Ones(2, 12); + + Matrix Ft = F.transpose(); + Matrix H = Ft * F - Ft * E * P * Et * F; + Vector v = Ft * (b - E * P * Et * b); + Matrix expectedAugmentedHessian = Matrix::Zero(3 * 12 + 1, 3 * 12 + 1); + expectedAugmentedHessian.block<36, 36>(0, 0) = H; + expectedAugmentedHessian.block<36, 1>(0, 36) = v; + expectedAugmentedHessian.block<1, 36>(36, 0) = v.transpose(); + expectedAugmentedHessian(36, 36) = b.squaredNorm(); + + EXPECT(assert_equal(expectedAugmentedHessian, actualAugmentedHessian)); + } + + { // SchurComplementAndRearrangeBlocks + KeyVector nonuniqueKeys; + nonuniqueKeys.push_back(0); + nonuniqueKeys.push_back(1); + nonuniqueKeys.push_back(1); + nonuniqueKeys.push_back(2); + nonuniqueKeys.push_back(2); + nonuniqueKeys.push_back(0); + + KeyVector uniqueKeys; + uniqueKeys.push_back(0); + uniqueKeys.push_back(1); + uniqueKeys.push_back(2); + + // Actual + SymmetricBlockMatrix augmentedHessianBM = + Set::SchurComplementAndRearrangeBlocks<3, 12, 6>(Fs, E, P, b, + nonuniqueKeys, + uniqueKeys); + Matrix actualAugmentedHessian = augmentedHessianBM.selfadjointView(); + + // Expected + // we first need to build the Jacobian F according to unique keys + Matrix F = Matrix::Zero(6, 18); + F.block<2, 6>(0, 0) = Fs[0].block<2, 6>(0, 0); + F.block<2, 6>(0, 6) = Fs[0].block<2, 6>(0, 6); + F.block<2, 6>(2, 6) = Fs[1].block<2, 6>(0, 0); + F.block<2, 6>(2, 12) = Fs[1].block<2, 6>(0, 6); + F.block<2, 6>(4, 12) = Fs[2].block<2, 6>(0, 0); + F.block<2, 6>(4, 0) = Fs[2].block<2, 6>(0, 6); + + Matrix Ft = F.transpose(); + Vector v = Ft * (b - E * P * Et * b); + Matrix H = Ft * F - Ft * E * P * Et * F; + Matrix expectedAugmentedHessian(19, 19); + expectedAugmentedHessian << H, v, v.transpose(), b.squaredNorm(); + + EXPECT(assert_equal(expectedAugmentedHessian, actualAugmentedHessian)); + } } /* ************************************************************************* */ From 5e0a6b45c4e4bbb684e3e5a078f0289042e83fcb Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sat, 24 Jul 2021 10:11:54 -0400 Subject: [PATCH 111/248] fixing ci issue --- gtsam/geometry/tests/testCameraSet.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/tests/testCameraSet.cpp b/gtsam/geometry/tests/testCameraSet.cpp index 9118c8899..e583c0e80 100644 --- a/gtsam/geometry/tests/testCameraSet.cpp +++ b/gtsam/geometry/tests/testCameraSet.cpp @@ -128,7 +128,7 @@ TEST(CameraSet, Pinhole) { /* ************************************************************************* */ TEST(CameraSet, SchurComplementAndRearrangeBlocks) { - typedef PinholePose Camera; + typedef PinholePose Camera; typedef CameraSet Set; // this is the (block) Jacobian with respect to the nonuniqueKeys @@ -185,7 +185,7 @@ TEST(CameraSet, SchurComplementAndRearrangeBlocks) { // Actual SymmetricBlockMatrix augmentedHessianBM = - Set::SchurComplementAndRearrangeBlocks<3, 12, 6>(Fs, E, P, b, + Set::SchurComplementAndRearrangeBlocks_3_12_6(Fs, E, P, b, nonuniqueKeys, uniqueKeys); Matrix actualAugmentedHessian = augmentedHessianBM.selfadjointView(); From 95bf0b16802410bfec2c164cdd73141b272308b9 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 31 Jul 2021 20:58:48 -0700 Subject: [PATCH 112/248] add Windows export symbols for PinholeCamera --- gtsam/geometry/PinholeCamera.h | 2 +- gtsam/geometry/PinholePose.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index 8ac67a5c3..c1f0b6b3f 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -30,7 +30,7 @@ namespace gtsam { * \nosubgrouping */ template -class PinholeCamera: public PinholeBaseK { +class GTSAM_EXPORT PinholeCamera: public PinholeBaseK { public: diff --git a/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h index 949caaa27..cc6435a58 100644 --- a/gtsam/geometry/PinholePose.h +++ b/gtsam/geometry/PinholePose.h @@ -31,7 +31,7 @@ namespace gtsam { * \nosubgrouping */ template -class PinholeBaseK: public PinholeBase { +class GTSAM_EXPORT PinholeBaseK: public PinholeBase { private: From f6a432961a845dd89fbff337a8da8ad0c441162e Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 1 Aug 2021 05:25:56 -0700 Subject: [PATCH 113/248] first pass at IMUKittiExampleGPS.py --- python/gtsam/examples/IMUKittiExampleGPS.py | 227 ++++++++++++++++++++ 1 file changed, 227 insertions(+) create mode 100644 python/gtsam/examples/IMUKittiExampleGPS.py diff --git a/python/gtsam/examples/IMUKittiExampleGPS.py b/python/gtsam/examples/IMUKittiExampleGPS.py new file mode 100644 index 000000000..33e032614 --- /dev/null +++ b/python/gtsam/examples/IMUKittiExampleGPS.py @@ -0,0 +1,227 @@ +""" +Example of application of ISAM2 for GPS-aided navigation on the KITTI VISION BENCHMARK SUITE +""" +import argparse +from typing import List + +import gtsam +import numpy as np +from gtsam import Pose3, Rot3, noiseModel +from gtsam.symbol_shorthand import B, V, X + + +class KittiCalibration: + def __init__(self, bodyTimu: gtsam.Pose3): + self.bodyTimu = bodyTimu + + +class ImuMeasurement: + def __init__(self, time, dt, accelerometer, gyroscope): + pass + + +class GpsMeasurement: + def __init__(self, time, position: gtsam.Point3): + self.time = time + self.position = position + + +def lodKittiData(): + pass + + +def parse_args(): + parser = argparse.ArgumentParser() + return parser.parse_args() + + +def getImuParams(kitti_calibration): + GRAVITY = 9.8 + w_coriolis = np.zeros(3) + + # Set IMU preintegration parameters + measured_acc_cov = np.eye(3) * np.power( + kitti_calibration.accelerometer_sigma, 2) + measured_omega_cov = np.eye(3) * np.powwe( + kitti_calibration.gyroscope_sigma, 2) + # error committed in integrating position from velocities + integration_error_cov = np.eye(3) * np.power( + kitti_calibration.integration_sigma, 2) + + imu_params = gtsam.PreintegrationParams.MakeSharedU(GRAVITY) + imu_params.accelerometerCovariance = measured_acc_cov # acc white noise in continuous + imu_params.integrationCovariance = integration_error_cov # integration uncertainty continuous + imu_params.gyroscopeCovariance = measured_omega_cov # gyro white noise in continuous + imu_params.omegaCoriolis = w_coriolis + + return imu_params + + +def main(): + args = parse_args() + kitti_calibration, imu_measurements, gps_measurements = lodKittiData() + + if kitti_calibration.bodyTimu != gtsam.Pose3: + raise ValueError( + "Currently only support IMUinBody is identity, i.e. IMU and body frame are the same" + ) + + # Configure different variables + first_gps_pose = 1 + gps_skip = 10 + + # Configure noise models + noise_model_gps = noiseModel.Diagonal.Precisions( + np.asarray([0, 0, 0, 1.0 / 0.07, 1.0 / 0.07, 1.0 / 0.07])) + + # Set initial conditions for the estimated trajectory + # initial pose is the reference frame (navigation frame) + current_pose_global = Pose3(Rot3(), + gps_measurements[first_gps_pose].position) + # the vehicle is stationary at the beginning at position 0,0,0 + current_velocity_global = np.zeros(3) + current_bias = gtsam.imuBias.ConstantBias() # init with zero bias + + sigma_init_x = noiseModel.Diagonal.Precisions( + np.asarray([0, 0, 0, 1, 1, 1])) + sigma_init_v = noiseModel.Diagonal.Sigmas(np.ones(3) * 1000.0) + sigma_init_b = noiseModel.Diagonal.Sigmas( + np.asarray([0.1, 0.1, 0.1, 5.00e-05, 5.00e-05, 5.00e-05])) + + imu_params = getImuParams() + + # Set ISAM2 parameters and create ISAM2 solver object + isam_params = gtsam.ISAM2Params() + isam_params.setFactorization("CHOLESKY") + isam_params.setRelinearizeSkip(10) + + isam = gtsam.ISAM2(isam_params) + + # Create the factor graph and values object that will store new factors and + # values to add to the incremental graph + new_factors = gtsam.NonlinearFactorGraph() + new_values = gtsam.Values( + ) # values storing the initial estimates of new nodes in the factor graph + + # Main loop: + # (1) we read the measurements + # (2) we create the corresponding factors in the graph + # (3) we solve the graph to obtain and optimal estimate of robot trajectory + print( + "-- Starting main loop: inference is performed at each time step, but we plot trajectory every 10 steps\n" + ) + j = 0 + for i in range(first_gps_pose, len(gps_measurements) - 1): + # At each non=IMU measurement we initialize a new node in the graph + current_pose_key = X(i) + current_vel_key = V(i) + current_bias_key = B(i) + t = gps_measurements[i].time + + if i == first_gps_pose: + # Create initial estimate and prior on initial pose, velocity, and biases + new_values.insert(current_pose_key, current_pose_global) + new_values.insert(current_vel_key, current_velocity_global) + new_values.insert(current_bias_key, current_bias) + + new_factors.addPriorPose3(current_pose_key, current_pose_global, + sigma_init_x) + new_factors.addPriorVector(current_vel_key, + current_velocity_global, sigma_init_v) + new_factors.addPriorConstantBias(current_bias_key, current_bias, + sigma_init_b) + else: + t_previous = gps_measurements[i - 1].time + + # Summarize IMU data between the previous GPS measurement and now + current_summarized_measurement = gtsam.PreintegratedImuMeasurements( + imu_params, current_bias) + + included_imu_measurement_count = 0 + while (j < imu_measurements.size() + and imu_measurements[j].time <= t): + if imu_measurements[j].time >= t_previous: + current_summarized_measurement.integrateMeasurement( + imu_measurements[j].accelerometer, + imu_measurements[j].gyroscope, imu_measurements[j].dt) + included_imu_measurement_count += 1 + j += 1 + + # Create IMU factor + previous_pose_key = X(i - 1) + previous_vel_key = V(i - 1) + previous_bias_key = B(i - 1) + + new_factors.push_back( + gtsam.ImuFactor > (previous_pose_key, previous_vel_key, + current_pose_key, current_vel_key, + previous_bias_key, + current_summarized_measurement)) + + # Bias evolution as given in the IMU metadata + sigma_between_b = noiseModel.Diagonal.Sigmas( + np.asarray([ + np.sqrt(included_imu_measurement_count) * + kitti_calibration.accelerometer_bias_sigma + ] * 3 + [ + np.sqrt(included_imu_measurement_count) * + kitti_calibration.gyroscope_bias_sigma + ] * 3)) + + new_factors.push_back( + gtsam.BetweenFactorConstantBias(previous_bias_key, + current_bias_key, + gtsam.imuBias.ConstantBias(), + sigma_between_b)) + + # Create GPS factor + gps_pose = Pose3(current_pose_global.rotation(), + gps_measurements[i].position) + if (i % gps_skip) == 0: + new_factors.addPriorPose3(current_pose_key, gps_pose, + noise_model_gps) + new_values.insert(current_pose_key, gps_pose) + + print( + "################ POSE INCLUDED AT TIME %lf ################\n", + t) + print(gps_pose.translation(), "\n") + else: + new_values.insert(current_pose_key, current_pose_global) + + # Add initial values for velocity and bias based on the previous + # estimates + new_values.insert(current_vel_key, current_velocity_global) + new_values.insert(current_bias_key, current_bias) + + # Update solver + # ======================================================================= + # We accumulate 2*GPSskip GPS measurements before updating the solver at + # first so that the heading becomes observable. + if i > (first_gps_pose + 2 * gps_skip): + print( + "################ NEW FACTORS AT TIME %lf ################\n", + t) + new_factors.print() + + isam.update(new_factors, new_values) + + # Reset the newFactors and newValues list + new_factors.resize(0) + new_values.clear() + + # Extract the result/current estimates + result = isam.calculateEstimate() + + current_pose_global = result.atPose3(current_pose_key) + current_velocity_global = result.atVector(current_vel_key) + current_bias = result.atConstantBias(current_bias_key) + + print("\n################ POSE AT TIME %lf ################\n", + t) + current_pose_global.print() + print("\n\n") + + +if __name__ == "__main__": + main() From f23f29d1ef256cb860469351a2be9b847fd87933 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 5 Aug 2021 11:22:27 -0400 Subject: [PATCH 114/248] speed up boost install --- .github/scripts/boost.sh | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/scripts/boost.sh b/.github/scripts/boost.sh index 647a84628..3c7e01274 100644 --- a/.github/scripts/boost.sh +++ b/.github/scripts/boost.sh @@ -9,10 +9,10 @@ tar -zxf ${BOOST_FOLDER}.tar.gz # Bootstrap cd ${BOOST_FOLDER}/ -./bootstrap.sh +./bootstrap.sh --with-libraries=serialization,filesystem,thread,system,atomic,date_time,timer,chrono,program_options,regex # Build and install -sudo ./b2 install +sudo ./b2 -j$(nproc) install # Rebuild ld cache sudo ldconfig From f9b8de3876bf5128091f970f1563fa6fc49a7d28 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 6 Aug 2021 15:53:40 -0400 Subject: [PATCH 115/248] improvements to CI files --- .github/workflows/build-linux.yml | 11 ++++------- .github/workflows/build-macos.yml | 9 +++------ .github/workflows/build-python.yml | 2 +- .github/workflows/build-special.yml | 2 +- .github/workflows/build-windows.yml | 20 +++++++++----------- 5 files changed, 18 insertions(+), 26 deletions(-) diff --git a/.github/workflows/build-linux.yml b/.github/workflows/build-linux.yml index 5483c32cf..f52e5eec3 100644 --- a/.github/workflows/build-linux.yml +++ b/.github/workflows/build-linux.yml @@ -47,8 +47,7 @@ jobs: - name: Checkout uses: actions/checkout@v2 - - name: Install (Linux) - if: runner.os == 'Linux' + - name: Install Dependencies run: | # LLVM (clang) 9 is not in Bionic's repositories so we add the official LLVM repository. if [ "${{ matrix.compiler }}" = "clang" ] && [ "${{ matrix.version }}" = "9" ]; then @@ -63,7 +62,7 @@ jobs: fi sudo apt-get -y update - sudo apt-get install cmake build-essential pkg-config libpython-dev python-numpy libicu-dev + sudo apt-get -y install cmake build-essential pkg-config libpython-dev python-numpy libicu-dev if [ "${{ matrix.compiler }}" = "gcc" ]; then sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib @@ -79,7 +78,5 @@ jobs: run: | bash .github/scripts/boost.sh - - name: Build and Test (Linux) - if: runner.os == 'Linux' - run: | - bash .github/scripts/unix.sh -t + - name: Build and Test + run: bash .github/scripts/unix.sh -t diff --git a/.github/workflows/build-macos.yml b/.github/workflows/build-macos.yml index ed8f8563b..462723222 100644 --- a/.github/workflows/build-macos.yml +++ b/.github/workflows/build-macos.yml @@ -34,8 +34,7 @@ jobs: - name: Checkout uses: actions/checkout@v2 - - name: Install (macOS) - if: runner.os == 'macOS' + - name: Install Dependencies run: | brew install cmake ninja brew install boost @@ -48,7 +47,5 @@ jobs: echo "CC=clang" >> $GITHUB_ENV echo "CXX=clang++" >> $GITHUB_ENV fi - - name: Build and Test (macOS) - if: runner.os == 'macOS' - run: | - bash .github/scripts/unix.sh -t + - name: Build and Test + run: bash .github/scripts/unix.sh -t diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index addde8c64..3fc2d662f 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -81,7 +81,7 @@ jobs: fi sudo apt-get -y update - sudo apt install cmake build-essential pkg-config libpython-dev python-numpy libboost-all-dev + sudo apt-get -y install cmake build-essential pkg-config libpython-dev python-numpy libboost-all-dev if [ "${{ matrix.compiler }}" = "gcc" ]; then sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib diff --git a/.github/workflows/build-special.yml b/.github/workflows/build-special.yml index 6427e13bc..3bb3fa298 100644 --- a/.github/workflows/build-special.yml +++ b/.github/workflows/build-special.yml @@ -70,7 +70,7 @@ jobs: fi sudo apt-get -y update - sudo apt install cmake build-essential pkg-config libpython-dev python-numpy libicu-dev + sudo apt-get -y install cmake build-essential pkg-config libpython-dev python-numpy libicu-dev if [ "${{ matrix.compiler }}" = "gcc" ]; then sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml index a564dade9..8af23d76b 100644 --- a/.github/workflows/build-windows.yml +++ b/.github/workflows/build-windows.yml @@ -18,15 +18,14 @@ jobs: # Github Actions requires a single row to be added to the build matrix. # See https://help.github.com/en/articles/workflow-syntax-for-github-actions. name: [ - #TODO This build keeps timing out, need to understand why. - # windows-2016-cl, - windows-2019-cl, - ] + #TODO This build keeps timing out, need to understand why. + # windows-2016-cl, + windows-2019-cl, + ] build_type: [Debug, Release] build_unstable: [ON] include: - #TODO This build keeps timing out, need to understand why. # - name: windows-2016-cl # os: windows-2016 @@ -37,10 +36,7 @@ jobs: compiler: cl steps: - - name: Checkout - uses: actions/checkout@v2 - - name: Install (Windows) - if: runner.os == 'Windows' + - name: Install shell: powershell run: | Invoke-Expression (New-Object System.Net.WebClient).DownloadString('https://get.scoop.sh') @@ -72,8 +68,10 @@ jobs: boost_version: 1.72.0 toolset: msvc14.2 - - name: Build (Windows) - if: runner.os == 'Windows' + - name: Checkout + uses: actions/checkout@v2 + + - name: Build env: BOOST_ROOT: ${{ steps.install-boost.outputs.BOOST_ROOT }} run: | From 19f13f3b72544eb43bfa8a803c5d93c8408be3b3 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 6 Aug 2021 16:04:40 -0400 Subject: [PATCH 116/248] Install boost directly from binary --- .github/workflows/build-windows.yml | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml index 8af23d76b..d03a95100 100644 --- a/.github/workflows/build-windows.yml +++ b/.github/workflows/build-windows.yml @@ -12,6 +12,8 @@ jobs: CTEST_PARALLEL_LEVEL: 2 CMAKE_BUILD_TYPE: ${{ matrix.build_type }} GTSAM_BUILD_UNSTABLE: ${{ matrix.build_unstable }} + BOOST_ROOT: C:\hostedtoolcache\windows\Boost\1.72.0\x86_64 + strategy: fail-fast: false matrix: @@ -61,19 +63,18 @@ jobs: # Scoop modifies the PATH so we make the modified PATH global. echo "$env:PATH" >> $GITHUB_PATH - - name: Download and install Boost - uses: MarkusJx/install-boost@v1.0.1 - id: install-boost - with: - boost_version: 1.72.0 - toolset: msvc14.2 + - name: Install Boost + run: | + # Snippet from: https://github.com/actions/virtual-environments/issues/2667 + # Use the boost_1_72_0-msvc-14.1-64.exe for Windows 2016 + $Url = "https://sourceforge.net/projects/boost/files/boost-binaries/1.72.0/boost_1_72_0-msvc-14.2-64.exe" + (New-Object System.Net.WebClient).DownloadFile($Url, "$env:TEMP\boost.exe") + Start-Process -Wait -FilePath "$env:TEMP\boost.exe" "/SILENT","/SP-","/SUPPRESSMSGBOXES","/DIR=C:\hostedtoolcache\windows\Boost\1.72.0\x86_64" - name: Checkout uses: actions/checkout@v2 - name: Build - env: - BOOST_ROOT: ${{ steps.install-boost.outputs.BOOST_ROOT }} run: | cmake -E remove_directory build cmake -B build -S . -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DBOOST_ROOT="${env:BOOST_ROOT}" -DBOOST_INCLUDEDIR="${env:BOOST_ROOT}\boost\include" -DBOOST_LIBRARYDIR="${env:BOOST_ROOT}\lib" From 2d590656fc92a0e6ed292b8909697e093af7a49f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 6 Aug 2021 20:16:33 -0400 Subject: [PATCH 117/248] use Boost 1.67.0 for Windows --- .github/workflows/build-windows.yml | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml index d03a95100..20408b0f9 100644 --- a/.github/workflows/build-windows.yml +++ b/.github/workflows/build-windows.yml @@ -12,7 +12,7 @@ jobs: CTEST_PARALLEL_LEVEL: 2 CMAKE_BUILD_TYPE: ${{ matrix.build_type }} GTSAM_BUILD_UNSTABLE: ${{ matrix.build_unstable }} - BOOST_ROOT: C:\hostedtoolcache\windows\Boost\1.72.0\x86_64 + BOOST_ROOT: C:\hostedtoolcache\windows\Boost\1.67.0\x86_64 strategy: fail-fast: false @@ -66,10 +66,10 @@ jobs: - name: Install Boost run: | # Snippet from: https://github.com/actions/virtual-environments/issues/2667 - # Use the boost_1_72_0-msvc-14.1-64.exe for Windows 2016 - $Url = "https://sourceforge.net/projects/boost/files/boost-binaries/1.72.0/boost_1_72_0-msvc-14.2-64.exe" + # Use the boost_1_67_0-msvc-14.1-64.exe for Windows 2016 + $Url = "https://sourceforge.net/projects/boost/files/boost-binaries/1.67.0/boost_1_67_0-msvc-14.1-64.exe" (New-Object System.Net.WebClient).DownloadFile($Url, "$env:TEMP\boost.exe") - Start-Process -Wait -FilePath "$env:TEMP\boost.exe" "/SILENT","/SP-","/SUPPRESSMSGBOXES","/DIR=C:\hostedtoolcache\windows\Boost\1.72.0\x86_64" + Start-Process -Wait -FilePath "$env:TEMP\boost.exe" "/SILENT","/SP-","/SUPPRESSMSGBOXES","/DIR=C:\hostedtoolcache\windows\Boost\1.67.0\x86_64" - name: Checkout uses: actions/checkout@v2 From d34555f45d148cb57c6f14bb231bcd68c484a4ed Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 8 Aug 2021 14:14:01 -0400 Subject: [PATCH 118/248] use env variables and fix setting of GITHUB_ENV --- .github/workflows/build-windows.yml | 25 ++++++++++++++++--------- 1 file changed, 16 insertions(+), 9 deletions(-) diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml index 20408b0f9..0a4774744 100644 --- a/.github/workflows/build-windows.yml +++ b/.github/workflows/build-windows.yml @@ -12,7 +12,8 @@ jobs: CTEST_PARALLEL_LEVEL: 2 CMAKE_BUILD_TYPE: ${{ matrix.build_type }} GTSAM_BUILD_UNSTABLE: ${{ matrix.build_unstable }} - BOOST_ROOT: C:\hostedtoolcache\windows\Boost\1.67.0\x86_64 + BOOST_VERSION: 1.67.0 + BOOST_EXE: boost_1_67_0-msvc-14.1-64.exe strategy: fail-fast: false @@ -38,7 +39,7 @@ jobs: compiler: cl steps: - - name: Install + - name: Install Dependencies shell: powershell run: | Invoke-Expression (New-Object System.Net.WebClient).DownloadString('https://get.scoop.sh') @@ -57,19 +58,25 @@ jobs: echo "CC=clang" >> $GITHUB_ENV echo "CXX=clang++" >> $GITHUB_ENV } else { - echo "CC=${{ matrix.compiler }}" >> $GITHUB_ENV - echo "CXX=${{ matrix.compiler }}" >> $GITHUB_ENV + echo "CC=${{ matrix.compiler }}" >> $env:GITHUB_ENV + echo "CXX=${{ matrix.compiler }}" >> $env:GITHUB_ENV } # Scoop modifies the PATH so we make the modified PATH global. - echo "$env:PATH" >> $GITHUB_PATH + echo "$env:PATH" >> $env:GITHUB_PATH - name: Install Boost + shell: powershell run: | # Snippet from: https://github.com/actions/virtual-environments/issues/2667 - # Use the boost_1_67_0-msvc-14.1-64.exe for Windows 2016 - $Url = "https://sourceforge.net/projects/boost/files/boost-binaries/1.67.0/boost_1_67_0-msvc-14.1-64.exe" - (New-Object System.Net.WebClient).DownloadFile($Url, "$env:TEMP\boost.exe") - Start-Process -Wait -FilePath "$env:TEMP\boost.exe" "/SILENT","/SP-","/SUPPRESSMSGBOXES","/DIR=C:\hostedtoolcache\windows\Boost\1.67.0\x86_64" + $BOOST_PATH = "C:\hostedtoolcache\windows\Boost\$env:BOOST_VERSION\x86_64" + + # Use the prebuilt binary for Windows + $Url = "https://sourceforge.net/projects/boost/files/boost-binaries/$env:BOOST_VERSION/$env:BOOST_EXE" + (New-Object System.Net.WebClient).DownloadFile($Url, "$env:TEMP\boost.exe") + Start-Process -Wait -FilePath "$env:TEMP\boost.exe" "/SILENT","/SP-","/SUPPRESSMSGBOXES","/DIR=$BOOST_PATH" + + # Set the BOOST_ROOT variable + echo "BOOST_ROOT=$BOOST_PATH" >> $env:GITHUB_ENV - name: Checkout uses: actions/checkout@v2 From 517ff4391abcf62f8c20cf8423ae979c5d5939b4 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 9 Aug 2021 19:50:40 -0400 Subject: [PATCH 119/248] use more cores for make on unix systems --- .github/scripts/unix.sh | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/.github/scripts/unix.sh b/.github/scripts/unix.sh index 55a8ac372..6abbb5596 100644 --- a/.github/scripts/unix.sh +++ b/.github/scripts/unix.sh @@ -92,7 +92,11 @@ function build () configure - make -j2 + if [ "$(uname)" == "Linux" ]; then + make -j$(nproc) + elif [ "$(uname)" == "Darwin" ]; then + make -j$(sysctl -n hw.physicalcpu) + fi finish } @@ -105,8 +109,12 @@ function test () configure - # Actual build: - make -j2 check + # Actual testing + if [ "$(uname)" == "Linux" ]; then + make -j$(nproc) + elif [ "$(uname)" == "Darwin" ]; then + make -j$(sysctl -n hw.physicalcpu) + fi finish } From 47c0f717ae1bf2512aeebb66c510f2beae6c86a5 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 9 Aug 2021 19:50:56 -0400 Subject: [PATCH 120/248] improvements to windows builds --- .github/workflows/build-windows.yml | 17 +++++++++++++---- 1 file changed, 13 insertions(+), 4 deletions(-) diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml index 0a4774744..ab15d64bb 100644 --- a/.github/workflows/build-windows.yml +++ b/.github/workflows/build-windows.yml @@ -13,7 +13,7 @@ jobs: CMAKE_BUILD_TYPE: ${{ matrix.build_type }} GTSAM_BUILD_UNSTABLE: ${{ matrix.build_unstable }} BOOST_VERSION: 1.67.0 - BOOST_EXE: boost_1_67_0-msvc-14.1-64.exe + BOOST_EXE: boost_1_67_0-msvc-14.1 strategy: fail-fast: false @@ -21,7 +21,7 @@ jobs: # Github Actions requires a single row to be added to the build matrix. # See https://help.github.com/en/articles/workflow-syntax-for-github-actions. name: [ - #TODO This build keeps timing out, need to understand why. + #TODO This build fails, need to understand why. # windows-2016-cl, windows-2019-cl, ] @@ -29,24 +29,29 @@ jobs: build_type: [Debug, Release] build_unstable: [ON] include: - #TODO This build keeps timing out, need to understand why. + #TODO This build fails, need to understand why. # - name: windows-2016-cl # os: windows-2016 # compiler: cl + # platform: 32 - name: windows-2019-cl os: windows-2019 compiler: cl + platform: 64 steps: - name: Install Dependencies shell: powershell run: | Invoke-Expression (New-Object System.Net.WebClient).DownloadString('https://get.scoop.sh') + scoop install cmake --global # So we don't get issues with CMP0074 policy scoop install ninja --global + if ("${{ matrix.compiler }}".StartsWith("clang")) { scoop install llvm --global } + if ("${{ matrix.compiler }}" -eq "gcc") { # Chocolatey GCC is broken on the windows-2019 image. # See: https://github.com/DaanDeMeyer/doctest/runs/231595515 @@ -54,13 +59,17 @@ jobs: scoop install gcc --global echo "CC=gcc" >> $GITHUB_ENV echo "CXX=g++" >> $GITHUB_ENV + } elseif ("${{ matrix.compiler }}" -eq "clang") { echo "CC=clang" >> $GITHUB_ENV echo "CXX=clang++" >> $GITHUB_ENV + } else { echo "CC=${{ matrix.compiler }}" >> $env:GITHUB_ENV echo "CXX=${{ matrix.compiler }}" >> $env:GITHUB_ENV + } + # Scoop modifies the PATH so we make the modified PATH global. echo "$env:PATH" >> $env:GITHUB_PATH @@ -71,7 +80,7 @@ jobs: $BOOST_PATH = "C:\hostedtoolcache\windows\Boost\$env:BOOST_VERSION\x86_64" # Use the prebuilt binary for Windows - $Url = "https://sourceforge.net/projects/boost/files/boost-binaries/$env:BOOST_VERSION/$env:BOOST_EXE" + $Url = "https://sourceforge.net/projects/boost/files/boost-binaries/$env:BOOST_VERSION/$env:BOOST_EXE-${{matrix.platform}}.exe" (New-Object System.Net.WebClient).DownloadFile($Url, "$env:TEMP\boost.exe") Start-Process -Wait -FilePath "$env:TEMP\boost.exe" "/SILENT","/SP-","/SUPPRESSMSGBOXES","/DIR=$BOOST_PATH" From 103b1985694abf33b5e3a861f37a8b7f443577fc Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 10 Aug 2021 13:04:22 -0400 Subject: [PATCH 121/248] Boost 1.72.0 for Windows --- .github/workflows/build-windows.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml index ab15d64bb..5dfdcd013 100644 --- a/.github/workflows/build-windows.yml +++ b/.github/workflows/build-windows.yml @@ -12,8 +12,8 @@ jobs: CTEST_PARALLEL_LEVEL: 2 CMAKE_BUILD_TYPE: ${{ matrix.build_type }} GTSAM_BUILD_UNSTABLE: ${{ matrix.build_unstable }} - BOOST_VERSION: 1.67.0 - BOOST_EXE: boost_1_67_0-msvc-14.1 + BOOST_VERSION: 1.72.0 + BOOST_EXE: boost_1_72_0-msvc-14.2 strategy: fail-fast: false From 570a44b7b2e5262624c7bba05d7c6cc3be3b15d6 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco Claraco Date: Wed, 11 Aug 2021 01:27:14 +0200 Subject: [PATCH 122/248] Add missing getter --- gtsam/slam/ProjectionFactor.h | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h index 67100a0ac..ada304f27 100644 --- a/gtsam/slam/ProjectionFactor.h +++ b/gtsam/slam/ProjectionFactor.h @@ -164,10 +164,15 @@ namespace gtsam { } /** return the calibration object */ - inline const boost::shared_ptr calibration() const { + const boost::shared_ptr calibration() const { return K_; } + /** return the (optional) sensor pose with respect to the vehicle frame */ + const boost::optional& body_P_sensor() const { + return body_P_sensor_; + } + /** return verbosity */ inline bool verboseCheirality() const { return verboseCheirality_; } From d0c31d3eb0450e398ec945540fbc7a5223e7082b Mon Sep 17 00:00:00 2001 From: John Lambert Date: Wed, 11 Aug 2021 04:46:32 -0400 Subject: [PATCH 123/248] start wrapping GNC --- gtsam/nonlinear/nonlinear.i | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index 8071e8bc7..dceb0c308 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -522,6 +522,14 @@ virtual class DoglegParams : gtsam::NonlinearOptimizerParams { void setVerbosityDL(string verbosityDL) const; }; +#include +template +class GncParams { + GncParams(const BaseOptimizerParameters& baseOptimizerParams); + GncParams(); + void print(const std::string& str) const; +}; + #include virtual class NonlinearOptimizer { gtsam::Values optimize(); @@ -551,6 +559,15 @@ virtual class DoglegOptimizer : gtsam::NonlinearOptimizer { const gtsam::DoglegParams& params); double getDelta() const; }; + +#include +template +class GncOptimizer { + GncOptimizer(const gtsam::NonlinearFactorGraph& graph, + const gtsam::Values& initialValues, + const gtsam::GncParameters& params); + gtsam::Values optimize(); +}; #include virtual class LevenbergMarquardtOptimizer : gtsam::NonlinearOptimizer { From 6bd6651ea29f61f5c46b73274b098e380fd1c8c1 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Wed, 11 Aug 2021 22:17:08 +0900 Subject: [PATCH 124/248] Remove std prefix in .i fn signatures --- gtsam/nonlinear/nonlinear.i | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index dceb0c308..9f19141e1 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -527,7 +527,7 @@ template class GncParams { GncParams(const BaseOptimizerParameters& baseOptimizerParams); GncParams(); - void print(const std::string& str) const; + void print(const string& str) const; }; #include From d0f25ec7b98f3e0f194b3482a99848cac7270768 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Thu, 12 Aug 2021 05:07:33 +0900 Subject: [PATCH 125/248] =?UTF-8?q?Remove=20=E2=80=98class=E2=80=99=20from?= =?UTF-8?q?=20template=20specifications?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- gtsam/nonlinear/nonlinear.i | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index 9f19141e1..704d1d257 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -523,7 +523,7 @@ virtual class DoglegParams : gtsam::NonlinearOptimizerParams { }; #include -template +template class GncParams { GncParams(const BaseOptimizerParameters& baseOptimizerParams); GncParams(); @@ -561,7 +561,7 @@ virtual class DoglegOptimizer : gtsam::NonlinearOptimizer { }; #include -template +template class GncOptimizer { GncOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues, From 651815724b793cbb006f38e900339492e9baae57 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Wed, 11 Aug 2021 21:33:10 -0400 Subject: [PATCH 126/248] try imports --- python/gtsam/tests/test_NonlinearOptimizer.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/python/gtsam/tests/test_NonlinearOptimizer.py b/python/gtsam/tests/test_NonlinearOptimizer.py index e9234a43b..5df7fcc0a 100644 --- a/python/gtsam/tests/test_NonlinearOptimizer.py +++ b/python/gtsam/tests/test_NonlinearOptimizer.py @@ -17,8 +17,9 @@ import unittest import gtsam from gtsam import (DoglegOptimizer, DoglegParams, DummyPreconditionerParameters, GaussNewtonOptimizer, - GaussNewtonParams, LevenbergMarquardtOptimizer, - LevenbergMarquardtParams, NonlinearFactorGraph, Ordering, + GaussNewtonParams, GncParams, GncOptimizer, + LevenbergMarquardtOptimizer, LevenbergMarquardtParams, + NonlinearFactorGraph, Ordering, PCGSolverParameters, Point2, PriorFactorPoint2, Values) from gtsam.utils.test_case import GtsamTestCase From 6364e34013985942db1ed9f77e839cc0449ade79 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Thu, 12 Aug 2021 03:15:52 -0400 Subject: [PATCH 127/248] provide template list of typedef --- gtsam/nonlinear/nonlinear.i | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index 704d1d257..97b8d2c23 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -523,9 +523,9 @@ virtual class DoglegParams : gtsam::NonlinearOptimizerParams { }; #include -template +template class GncParams { - GncParams(const BaseOptimizerParameters& baseOptimizerParams); + GncParams(const PARAMS& baseOptimizerParams); GncParams(); void print(const string& str) const; }; @@ -561,11 +561,11 @@ virtual class DoglegOptimizer : gtsam::NonlinearOptimizer { }; #include -template +template class GncOptimizer { GncOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues, - const gtsam::GncParameters& params); + const PARAMS& params); gtsam::Values optimize(); }; From c9bcb1430cfb350e3db3b28a8f2fca0f9999d28a Mon Sep 17 00:00:00 2001 From: John Lambert Date: Thu, 12 Aug 2021 03:26:53 -0400 Subject: [PATCH 128/248] test GNC along with other non-linear optimizers in python unit tests --- python/gtsam/tests/test_NonlinearOptimizer.py | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/python/gtsam/tests/test_NonlinearOptimizer.py b/python/gtsam/tests/test_NonlinearOptimizer.py index 5df7fcc0a..66339b41b 100644 --- a/python/gtsam/tests/test_NonlinearOptimizer.py +++ b/python/gtsam/tests/test_NonlinearOptimizer.py @@ -78,6 +78,12 @@ class TestScenario(GtsamTestCase): dlParams.setOrdering(ordering) actual3 = DoglegOptimizer(fg, initial_values, dlParams).optimize() self.assertAlmostEqual(0, fg.error(actual3)) + + # Graduated Non-Convexity (GNC) + gncParams = GncParams() + actual4 = GncOptimizer(fg, initial_values, gncParams).optimize() + self.assertAlmostEqual(0, fg.error(actual4)) + if __name__ == "__main__": From 9b05390ccba3f17c12c9f738bd8766b07e92ed28 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Thu, 12 Aug 2021 06:44:01 -0400 Subject: [PATCH 129/248] remove Dogleg from GNC-supported base-optimizers, and use nested templates --- gtsam/nonlinear/nonlinear.i | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index 97b8d2c23..7c81f04f2 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -523,7 +523,7 @@ virtual class DoglegParams : gtsam::NonlinearOptimizerParams { }; #include -template +template class GncParams { GncParams(const PARAMS& baseOptimizerParams); GncParams(); @@ -561,7 +561,8 @@ virtual class DoglegOptimizer : gtsam::NonlinearOptimizer { }; #include -template +template, + gtsam::GncParams}> class GncOptimizer { GncOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues, From e9465128fda2cfdcdac0d1fffaab443c555b2d87 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Thu, 12 Aug 2021 07:19:58 -0400 Subject: [PATCH 130/248] add virtual to all classes in the .i file --- gtsam/nonlinear/nonlinear.i | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index 7c81f04f2..a22b31023 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -524,7 +524,7 @@ virtual class DoglegParams : gtsam::NonlinearOptimizerParams { #include template -class GncParams { +virtual class GncParams { GncParams(const PARAMS& baseOptimizerParams); GncParams(); void print(const string& str) const; @@ -563,7 +563,7 @@ virtual class DoglegOptimizer : gtsam::NonlinearOptimizer { #include template, gtsam::GncParams}> -class GncOptimizer { +virtual class GncOptimizer { GncOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues, const PARAMS& params); From 7bf640e1f5135fa0d6bd157f0f872725063abd8c Mon Sep 17 00:00:00 2001 From: John Lambert Date: Thu, 12 Aug 2021 07:22:10 -0400 Subject: [PATCH 131/248] add GTSAM_EXPORT to GncParams.h --- gtsam/nonlinear/GncParams.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/nonlinear/GncParams.h b/gtsam/nonlinear/GncParams.h index c1bf7a035..086f08acc 100644 --- a/gtsam/nonlinear/GncParams.h +++ b/gtsam/nonlinear/GncParams.h @@ -39,7 +39,7 @@ enum GncLossType { }; template -class GncParams { +class GTSAM_EXPORT GncParams { public: /// For each parameter, specify the corresponding optimizer: e.g., GaussNewtonParams -> GaussNewtonOptimizer. typedef typename BaseOptimizerParameters::OptimizerType OptimizerType; From 6e788faf34a1fa63e10c57a1b6d2cb3c783d69f2 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Thu, 12 Aug 2021 07:22:37 -0400 Subject: [PATCH 132/248] add GTSAM_EXPORT to GncOptimizer.h --- gtsam/nonlinear/GncOptimizer.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h index eb353c53f..3ddaf4820 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -42,7 +42,7 @@ static double Chi2inv(const double alpha, const size_t dofs) { /* ************************************************************************* */ template -class GncOptimizer { +class GTSAM_EXPORT GncOptimizer { public: /// For each parameter, specify the corresponding optimizer: e.g., GaussNewtonParams -> GaussNewtonOptimizer. typedef typename GncParameters::OptimizerType BaseOptimizer; From 85e58a78bb9ab4e55315ce17704fc528ba73053c Mon Sep 17 00:00:00 2001 From: John Lambert Date: Thu, 12 Aug 2021 07:41:45 -0400 Subject: [PATCH 133/248] clean up test_Pose3SLAMExample.py --- python/gtsam/tests/test_Pose3SLAMExample.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/python/gtsam/tests/test_Pose3SLAMExample.py b/python/gtsam/tests/test_Pose3SLAMExample.py index fce171b55..cb5e3b226 100644 --- a/python/gtsam/tests/test_Pose3SLAMExample.py +++ b/python/gtsam/tests/test_Pose3SLAMExample.py @@ -15,14 +15,14 @@ import numpy as np import gtsam from gtsam.utils.test_case import GtsamTestCase -from gtsam.utils.circlePose3 import * +from gtsam.utils.circlePose3 import circlePose3 class TestPose3SLAMExample(GtsamTestCase): - def test_Pose3SLAMExample(self): + def test_Pose3SLAMExample(self) -> None: # Create a hexagon of poses - hexagon = circlePose3(6, 1.0) + hexagon = circlePose3(numPoses=6, radius=1.0) p0 = hexagon.atPose3(0) p1 = hexagon.atPose3(1) @@ -31,7 +31,7 @@ class TestPose3SLAMExample(GtsamTestCase): fg.add(gtsam.NonlinearEqualityPose3(0, p0)) delta = p0.between(p1) covariance = gtsam.noiseModel.Diagonal.Sigmas( - np.array([0.05, 0.05, 0.05, 5. * pi / 180, 5. * pi / 180, 5. * pi / 180])) + np.array([0.05, 0.05, 0.05, np.deg2rad(5.), np.deg2rad(5.), np.deg2rad(5.)])) fg.add(gtsam.BetweenFactorPose3(0, 1, delta, covariance)) fg.add(gtsam.BetweenFactorPose3(1, 2, delta, covariance)) fg.add(gtsam.BetweenFactorPose3(2, 3, delta, covariance)) From c20fcc5a7cd2e6f09e305bd4c1590e55982f14c7 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Thu, 12 Aug 2021 07:44:56 -0400 Subject: [PATCH 134/248] add type hints, use numpy instead of math module --- python/gtsam/utils/circlePose3.py | 25 ++++++++++++++++--------- 1 file changed, 16 insertions(+), 9 deletions(-) diff --git a/python/gtsam/utils/circlePose3.py b/python/gtsam/utils/circlePose3.py index e1def9427..2c08c8749 100644 --- a/python/gtsam/utils/circlePose3.py +++ b/python/gtsam/utils/circlePose3.py @@ -1,10 +1,10 @@ -import gtsam -import math + import numpy as np -from math import pi +import gtsam +from gtsam import Values -def circlePose3(numPoses=8, radius=1.0, symbolChar='\0'): +def circlePose3(numPoses: int = 8, radius: float = 1.0, symbolChar='\0') -> Values: """ circlePose3 generates a set of poses in a circle. This function returns those poses inside a gtsam.Values object, with sequential @@ -21,14 +21,21 @@ def circlePose3(numPoses=8, radius=1.0, symbolChar='\0'): values = gtsam.Values() theta = 0.0 - dtheta = 2 * pi / numPoses + dtheta = 2 * np.pi / numPoses gRo = gtsam.Rot3( - np.array([[0., 1., 0.], [1., 0., 0.], [0., 0., -1.]], order='F')) + np.array( + [ + [0., 1., 0.], + [1., 0., 0.], + [0., 0., -1.] + ], order='F' + ) + ) for i in range(numPoses): key = gtsam.symbol(symbolChar, i) - gti = gtsam.Point3(radius * math.cos(theta), radius * math.sin(theta), 0) - oRi = gtsam.Rot3.Yaw( - -theta) # negative yaw goes counterclockwise, with Z down ! + gti = gtsam.Point3(radius * np.cos(theta), radius * np.sin(theta), 0) + # negative yaw goes counterclockwise, with Z down ! + oRi = gtsam.Rot3.Yaw(-theta) gTi = gtsam.Pose3(gRo.compose(oRi), gti) values.insert(key, gTi) theta = theta + dtheta From 678d1c7270593994028331aa655c16dc62b0a5b2 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Thu, 12 Aug 2021 07:48:23 -0400 Subject: [PATCH 135/248] add type hints to visual_data_generator.py --- python/gtsam/utils/visual_data_generator.py | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/python/gtsam/utils/visual_data_generator.py b/python/gtsam/utils/visual_data_generator.py index 32ccbc8fa..51852760a 100644 --- a/python/gtsam/utils/visual_data_generator.py +++ b/python/gtsam/utils/visual_data_generator.py @@ -1,8 +1,10 @@ from __future__ import print_function +from typing import Tuple -import numpy as np import math +import numpy as np from math import pi + import gtsam from gtsam import Point3, Pose3, PinholeCameraCal3_S2, Cal3_S2 @@ -12,7 +14,7 @@ class Options: Options to generate test scenario """ - def __init__(self, triangle=False, nrCameras=3, K=Cal3_S2()): + def __init__(self, triangle: bool = False, nrCameras: int = 3, K=Cal3_S2()) -> None: """ Options to generate test scenario @param triangle: generate a triangle scene with 3 points if True, otherwise @@ -29,12 +31,12 @@ class GroundTruth: Object holding generated ground-truth data """ - def __init__(self, K=Cal3_S2(), nrCameras=3, nrPoints=4): + def __init__(self, K=Cal3_S2(), nrCameras: int = 3, nrPoints: int = 4) -> None: self.K = K self.cameras = [Pose3()] * nrCameras self.points = [Point3(0, 0, 0)] * nrPoints - def print_(self, s=""): + def print_(self, s="") -> None: print(s) print("K = ", self.K) print("Cameras: ", len(self.cameras)) @@ -54,7 +56,7 @@ class Data: class NoiseModels: pass - def __init__(self, K=Cal3_S2(), nrCameras=3, nrPoints=4): + def __init__(self, K=Cal3_S2(), nrCameras: int = 3, nrPoints: int = 4) -> None: self.K = K self.Z = [x[:] for x in [[gtsam.Point2()] * nrPoints] * nrCameras] self.J = [x[:] for x in [[0] * nrPoints] * nrCameras] @@ -72,7 +74,7 @@ class Data: self.noiseModels.measurement = gtsam.noiseModel.Isotropic.Sigma(2, 1.0) -def generate_data(options): +def generate_data(options) -> Tuple[Data, GroundTruth]: """ Generate ground-truth and measurement data. """ K = Cal3_S2(500, 500, 0, 640. / 2., 480. / 2.) From 68794468f22298c3e07058078feca0a194ecb25b Mon Sep 17 00:00:00 2001 From: John Lambert Date: Thu, 12 Aug 2021 08:06:12 -0400 Subject: [PATCH 136/248] clean up plot.py with modern type hints --- python/gtsam/utils/plot.py | 115 ++++++++++++++++++++++--------------- 1 file changed, 70 insertions(+), 45 deletions(-) diff --git a/python/gtsam/utils/plot.py b/python/gtsam/utils/plot.py index 7f48d03a3..9e74cf38e 100644 --- a/python/gtsam/utils/plot.py +++ b/python/gtsam/utils/plot.py @@ -2,22 +2,25 @@ # pylint: disable=no-member, invalid-name +from typing import Iterable, Tuple + import matplotlib.pyplot as plt import numpy as np from matplotlib import patches from mpl_toolkits.mplot3d import Axes3D # pylint: disable=unused-import import gtsam +from gtsam import Marginals, Point3, Pose2, Values -def set_axes_equal(fignum): +def set_axes_equal(fignum: int) -> None: """ Make axes of 3D plot have equal scale so that spheres appear as spheres, cubes as cubes, etc.. This is one possible solution to Matplotlib's ax.set_aspect('equal') and ax.axis('equal') not working for 3D. Args: - fignum (int): An integer representing the figure number for Matplotlib. + fignum: An integer representing the figure number for Matplotlib. """ fig = plt.figure(fignum) ax = fig.gca(projection='3d') @@ -36,18 +39,20 @@ def set_axes_equal(fignum): ax.set_zlim3d([origin[2] - radius, origin[2] + radius]) -def ellipsoid(rx, ry, rz, n): +def ellipsoid( + rx: float, ry: float, rz: float, n: int +) -> Tuple[np.ndarray, np.ndarray, np.ndarray]: """ Numpy equivalent of Matlab's ellipsoid function. Args: - rx (double): Radius of ellipsoid in X-axis. - ry (double): Radius of ellipsoid in Y-axis. - rz (double): Radius of ellipsoid in Z-axis. - n (int): The granularity of the ellipsoid plotted. + rx: Radius of ellipsoid in X-axis. + ry: Radius of ellipsoid in Y-axis. + rz: Radius of ellipsoid in Z-axis. + n: The granularity of the ellipsoid plotted. Returns: - tuple[numpy.ndarray]: The points in the x, y and z axes to use for the surface plot. + The points in the x, y and z axes to use for the surface plot. """ u = np.linspace(0, 2*np.pi, n+1) v = np.linspace(0, np.pi, n+1) @@ -58,7 +63,9 @@ def ellipsoid(rx, ry, rz, n): return x, y, z -def plot_covariance_ellipse_3d(axes, origin, P, scale=1, n=8, alpha=0.5): +def plot_covariance_ellipse_3d( + axes, origin: Point3, P, scale: float = 1, n: int = 8, alpha: float = 0.5 +) -> None: """ Plots a Gaussian as an uncertainty ellipse @@ -68,12 +75,12 @@ def plot_covariance_ellipse_3d(axes, origin, P, scale=1, n=8, alpha=0.5): Args: axes (matplotlib.axes.Axes): Matplotlib axes. - origin (gtsam.Point3): The origin in the world frame. + origin: The origin in the world frame. P (numpy.ndarray): The marginal covariance matrix of the 3D point which will be represented as an ellipse. - scale (float): Scaling factor of the radii of the covariance ellipse. - n (int): Defines the granularity of the ellipse. Higher values indicate finer ellipses. - alpha (float): Transparency value for the plotted surface in the range [0, 1]. + scale: Scaling factor of the radii of the covariance ellipse. + n: Defines the granularity of the ellipse. Higher values indicate finer ellipses. + alpha: Transparency value for the plotted surface in the range [0, 1]. """ k = 11.82 U, S, _ = np.linalg.svd(P) @@ -96,14 +103,16 @@ def plot_covariance_ellipse_3d(axes, origin, P, scale=1, n=8, alpha=0.5): axes.plot_surface(x, y, z, alpha=alpha, cmap='hot') -def plot_pose2_on_axes(axes, pose, axis_length=0.1, covariance=None): +def plot_pose2_on_axes( + axes, pose: Pose2, axis_length: float = 0.1, covariance: np.ndarray = None +) -> None: """ Plot a 2D pose on given axis `axes` with given `axis_length`. Args: axes (matplotlib.axes.Axes): Matplotlib axes. - pose (gtsam.Pose2): The pose to be plotted. - axis_length (float): The length of the camera axes. + pose: The pose to be plotted. + axis_length: The length of the camera axes. covariance (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation. """ @@ -136,16 +145,21 @@ def plot_pose2_on_axes(axes, pose, axis_length=0.1, covariance=None): axes.add_patch(e1) -def plot_pose2(fignum, pose, axis_length=0.1, covariance=None, - axis_labels=('X axis', 'Y axis', 'Z axis')): +def plot_pose2( + fignum: int, + pose: Pose2, + axis_length: float = 0.1, + covariance: np.ndarray = None, + axis_labels=("X axis", "Y axis", "Z axis"), +) -> plt.Figure: """ Plot a 2D pose on given figure with given `axis_length`. Args: - fignum (int): Integer representing the figure number to use for plotting. - pose (gtsam.Pose2): The pose to be plotted. - axis_length (float): The length of the camera axes. - covariance (numpy.ndarray): Marginal covariance matrix to plot + fignum: Integer representing the figure number to use for plotting. + pose: The pose to be plotted. + axis_length: The length of the camera axes. + covariance: Marginal covariance matrix to plot the uncertainty of the estimation. axis_labels (iterable[string]): List of axis labels to set. """ @@ -176,17 +190,17 @@ def plot_point3_on_axes(axes, point, linespec, P=None): plot_covariance_ellipse_3d(axes, point, P) -def plot_point3(fignum, point, linespec, P=None, - axis_labels=('X axis', 'Y axis', 'Z axis')): +def plot_point3(fignum: int, point: Point3, linespec: str, P: np.ndarray = None, + axis_labels: Iterable[str] = ('X axis', 'Y axis', 'Z axis')) -> plt.Figure: """ Plot a 3D point on given figure with given `linespec`. Args: - fignum (int): Integer representing the figure number to use for plotting. - point (gtsam.Point3): The point to be plotted. - linespec (string): String representing formatting options for Matplotlib. - P (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation. - axis_labels (iterable[string]): List of axis labels to set. + fignum: Integer representing the figure number to use for plotting. + point: The point to be plotted. + linespec: String representing formatting options for Matplotlib. + P: Marginal covariance matrix to plot the uncertainty of the estimation. + axis_labels: List of axis labels to set. Returns: fig: The matplotlib figure. @@ -308,18 +322,24 @@ def plot_pose3(fignum, pose, axis_length=0.1, P=None, return fig -def plot_trajectory(fignum, values, scale=1, marginals=None, - title="Plot Trajectory", axis_labels=('X axis', 'Y axis', 'Z axis')): +def plot_trajectory( + fignum: int, + values: Values, + scale: float = 1, + marginals: Marginals = None, + title: str = "Plot Trajectory", + axis_labels: Iterable[str] = ("X axis", "Y axis", "Z axis"), +) -> None: """ Plot a complete 2D/3D trajectory using poses in `values`. Args: - fignum (int): Integer representing the figure number to use for plotting. - values (gtsam.Values): Values containing some Pose2 and/or Pose3 values. - scale (float): Value to scale the poses by. - marginals (gtsam.Marginals): Marginalized probability values of the estimation. + fignum: Integer representing the figure number to use for plotting. + values: Values containing some Pose2 and/or Pose3 values. + scale: Value to scale the poses by. + marginals: Marginalized probability values of the estimation. Used to plot uncertainty bounds. - title (string): The title of the plot. + title: The title of the plot. axis_labels (iterable[string]): List of axis labels to set. """ fig = plt.figure(fignum) @@ -357,20 +377,25 @@ def plot_trajectory(fignum, values, scale=1, marginals=None, fig.canvas.set_window_title(title.lower()) -def plot_incremental_trajectory(fignum, values, start=0, - scale=1, marginals=None, - time_interval=0.0): +def plot_incremental_trajectory( + fignum: int, + values: Values, + start: int = 0, + scale: float = 1, + marginals: Marginals = None, + time_interval: float = 0.0 +) -> None: """ Incrementally plot a complete 3D trajectory using poses in `values`. Args: - fignum (int): Integer representing the figure number to use for plotting. - values (gtsam.Values): Values dict containing the poses. - start (int): Starting index to start plotting from. - scale (float): Value to scale the poses by. - marginals (gtsam.Marginals): Marginalized probability values of the estimation. + fignum: Integer representing the figure number to use for plotting. + values: Values dict containing the poses. + start: Starting index to start plotting from. + scale: Value to scale the poses by. + marginals: Marginalized probability values of the estimation. Used to plot uncertainty bounds. - time_interval (float): Time in seconds to pause between each rendering. + time_interval: Time in seconds to pause between each rendering. Used to create animation effect. """ fig = plt.figure(fignum) From c0ae0ccd68b7ddd6551b66bcfa0394bde3a2cc3c Mon Sep 17 00:00:00 2001 From: John Lambert Date: Thu, 12 Aug 2021 08:11:34 -0400 Subject: [PATCH 137/248] add more missing type hints --- python/gtsam/utils/plot.py | 30 ++++++++++++++++++++---------- 1 file changed, 20 insertions(+), 10 deletions(-) diff --git a/python/gtsam/utils/plot.py b/python/gtsam/utils/plot.py index 9e74cf38e..a56face0c 100644 --- a/python/gtsam/utils/plot.py +++ b/python/gtsam/utils/plot.py @@ -64,7 +64,7 @@ def ellipsoid( def plot_covariance_ellipse_3d( - axes, origin: Point3, P, scale: float = 1, n: int = 8, alpha: float = 0.5 + axes, origin: Point3, P: np.ndarray, scale: float = 1, n: int = 8, alpha: float = 0.5 ) -> None: """ Plots a Gaussian as an uncertainty ellipse @@ -76,7 +76,7 @@ def plot_covariance_ellipse_3d( Args: axes (matplotlib.axes.Axes): Matplotlib axes. origin: The origin in the world frame. - P (numpy.ndarray): The marginal covariance matrix of the 3D point + P: The marginal covariance matrix of the 3D point which will be represented as an ellipse. scale: Scaling factor of the radii of the covariance ellipse. n: Defines the granularity of the ellipse. Higher values indicate finer ellipses. @@ -190,8 +190,13 @@ def plot_point3_on_axes(axes, point, linespec, P=None): plot_covariance_ellipse_3d(axes, point, P) -def plot_point3(fignum: int, point: Point3, linespec: str, P: np.ndarray = None, - axis_labels: Iterable[str] = ('X axis', 'Y axis', 'Z axis')) -> plt.Figure: +def plot_point3( + fignum: int, + point: Point3, + linespec: str, + P: np.ndarray = None, + axis_labels: Iterable[str] = ("X axis", "Y axis", "Z axis"), +) -> plt.Figure: """ Plot a 3D point on given figure with given `linespec`. @@ -294,17 +299,22 @@ def plot_pose3_on_axes(axes, pose, axis_length=0.1, P=None, scale=1): plot_covariance_ellipse_3d(axes, origin, gPp) -def plot_pose3(fignum, pose, axis_length=0.1, P=None, - axis_labels=('X axis', 'Y axis', 'Z axis')): +def plot_pose3( + fignum: int, + pose: Pose3, + axis_length: float = 0.1, + P: np.ndarray = None, + axis_labels: Iterable[str] = ("X axis", "Y axis", "Z axis"), +) -> plt.Figure: """ Plot a 3D pose on given figure with given `axis_length`. Args: - fignum (int): Integer representing the figure number to use for plotting. + fignum: Integer representing the figure number to use for plotting. pose (gtsam.Pose3): 3D pose to be plotted. - linespec (string): String representing formatting options for Matplotlib. - P (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation. - axis_labels (iterable[string]): List of axis labels to set. + axis_length: + P: Marginal covariance matrix to plot the uncertainty of the estimation. + axis_labels: List of axis labels to set. Returns: fig: The matplotlib figure. From 1684cb1bf4f6366022772d5de4ab2a9bedf64aef Mon Sep 17 00:00:00 2001 From: John Lambert Date: Thu, 12 Aug 2021 09:09:37 -0400 Subject: [PATCH 138/248] add missing type hint --- python/gtsam/utils/circlePose3.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/gtsam/utils/circlePose3.py b/python/gtsam/utils/circlePose3.py index 2c08c8749..5cd3a07ce 100644 --- a/python/gtsam/utils/circlePose3.py +++ b/python/gtsam/utils/circlePose3.py @@ -4,7 +4,7 @@ import numpy as np import gtsam from gtsam import Values -def circlePose3(numPoses: int = 8, radius: float = 1.0, symbolChar='\0') -> Values: +def circlePose3(numPoses: int = 8, radius: float = 1.0, symbolChar: str = '\0') -> Values: """ circlePose3 generates a set of poses in a circle. This function returns those poses inside a gtsam.Values object, with sequential From 8a97f7ddeb575c1a0aada2eea13731721bf8bd53 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Thu, 12 Aug 2021 09:15:33 -0400 Subject: [PATCH 139/248] add missing docstring for an input arg --- python/gtsam/utils/plot.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/gtsam/utils/plot.py b/python/gtsam/utils/plot.py index a56face0c..2eaf889bb 100644 --- a/python/gtsam/utils/plot.py +++ b/python/gtsam/utils/plot.py @@ -312,7 +312,7 @@ def plot_pose3( Args: fignum: Integer representing the figure number to use for plotting. pose (gtsam.Pose3): 3D pose to be plotted. - axis_length: + axis_length: The length of the camera axes. P: Marginal covariance matrix to plot the uncertainty of the estimation. axis_labels: List of axis labels to set. From 30d028ef74d86dbdfa9f3e18118edf5ee24562a0 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Thu, 12 Aug 2021 17:30:20 -0400 Subject: [PATCH 140/248] use custom typedefs for GNC + GaussNewton and GNC + LM --- gtsam/nonlinear/nonlinear.i | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index a22b31023..d2047b798 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -523,12 +523,15 @@ virtual class DoglegParams : gtsam::NonlinearOptimizerParams { }; #include -template +template virtual class GncParams { GncParams(const PARAMS& baseOptimizerParams); GncParams(); void print(const string& str) const; }; + +typedef GncParams GncGaussNewtonParams; +typedef GncParams GncLMParams; #include virtual class NonlinearOptimizer { @@ -561,8 +564,7 @@ virtual class DoglegOptimizer : gtsam::NonlinearOptimizer { }; #include -template, - gtsam::GncParams}> +template virtual class GncOptimizer { GncOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues, @@ -570,6 +572,9 @@ virtual class GncOptimizer { gtsam::Values optimize(); }; +typedef gtsam::GncOptimizer > GncGaussNewtonOptimizer; +typedef gtsam::GncOptimizer > GncLMOptimizer; + #include virtual class LevenbergMarquardtOptimizer : gtsam::NonlinearOptimizer { LevenbergMarquardtOptimizer(const gtsam::NonlinearFactorGraph& graph, From db9b98030b8ffc0d485bdae0a21453bf476dc898 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Thu, 12 Aug 2021 18:31:24 -0400 Subject: [PATCH 141/248] import GncLMParams, GncLMOptimizer to prevent pybind's automatic long names from name concat --- python/gtsam/tests/test_NonlinearOptimizer.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/python/gtsam/tests/test_NonlinearOptimizer.py b/python/gtsam/tests/test_NonlinearOptimizer.py index 66339b41b..e2561ae52 100644 --- a/python/gtsam/tests/test_NonlinearOptimizer.py +++ b/python/gtsam/tests/test_NonlinearOptimizer.py @@ -17,7 +17,7 @@ import unittest import gtsam from gtsam import (DoglegOptimizer, DoglegParams, DummyPreconditionerParameters, GaussNewtonOptimizer, - GaussNewtonParams, GncParams, GncOptimizer, + GaussNewtonParams, GncLMParams, GncLMOptimizer, LevenbergMarquardtOptimizer, LevenbergMarquardtParams, NonlinearFactorGraph, Ordering, PCGSolverParameters, Point2, PriorFactorPoint2, Values) @@ -80,8 +80,8 @@ class TestScenario(GtsamTestCase): self.assertAlmostEqual(0, fg.error(actual3)) # Graduated Non-Convexity (GNC) - gncParams = GncParams() - actual4 = GncOptimizer(fg, initial_values, gncParams).optimize() + gncParams = GncLMParams() + actual4 = GncLMOptimizer(fg, initial_values, gncParams).optimize() self.assertAlmostEqual(0, fg.error(actual4)) From b7eccdab7ba31e9f81da575a0456afd56d6e871c Mon Sep 17 00:00:00 2001 From: John Lambert Date: Thu, 12 Aug 2021 18:35:39 -0400 Subject: [PATCH 142/248] add missing gtsam prefix --- gtsam/nonlinear/nonlinear.i | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index d2047b798..73eaef125 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -530,8 +530,8 @@ virtual class GncParams { void print(const string& str) const; }; -typedef GncParams GncGaussNewtonParams; -typedef GncParams GncLMParams; +typedef gtsam::GncParams GncGaussNewtonParams; +typedef gtsam::GncParams GncLMParams; #include virtual class NonlinearOptimizer { From 939c3047e77bf3137ac68d74f9966ab55910153d Mon Sep 17 00:00:00 2001 From: John Lambert Date: Thu, 12 Aug 2021 20:01:03 -0400 Subject: [PATCH 143/248] add Optional type annotations where needed --- python/gtsam/utils/plot.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/python/gtsam/utils/plot.py b/python/gtsam/utils/plot.py index 2eaf889bb..a0c6f09b4 100644 --- a/python/gtsam/utils/plot.py +++ b/python/gtsam/utils/plot.py @@ -2,7 +2,7 @@ # pylint: disable=no-member, invalid-name -from typing import Iterable, Tuple +from typing import Iterable, Optional, Tuple import matplotlib.pyplot as plt import numpy as np @@ -175,15 +175,15 @@ def plot_pose2( return fig -def plot_point3_on_axes(axes, point, linespec, P=None): +def plot_point3_on_axes(axes, point: Point3, linespec: str, P: Optional[np.ndarray] = None) -> None: """ Plot a 3D point on given axis `axes` with given `linespec`. Args: axes (matplotlib.axes.Axes): Matplotlib axes. - point (gtsam.Point3): The point to be plotted. - linespec (string): String representing formatting options for Matplotlib. - P (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation. + point: The point to be plotted. + linespec: String representing formatting options for Matplotlib. + P: Marginal covariance matrix to plot the uncertainty of the estimation. """ axes.plot([point[0]], [point[1]], [point[2]], linespec) if P is not None: @@ -392,7 +392,7 @@ def plot_incremental_trajectory( values: Values, start: int = 0, scale: float = 1, - marginals: Marginals = None, + marginals: Optional[Marginals] = None, time_interval: float = 0.0 ) -> None: """ From cbd6a2a01c1ba857b2570bf69936884dd9e5ded5 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Fri, 13 Aug 2021 19:36:15 -0400 Subject: [PATCH 144/248] now using MakeJacobian --- gtsam/base/Lie.h | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index c93456a28..1e19bfc0e 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -324,13 +324,12 @@ T expm(const Vector& x, int K=7) { */ template T interpolate(const T& X, const T& Y, double t, - OptionalJacobian< traits::dimension, traits::dimension > Hx = boost::none, - OptionalJacobian< traits::dimension, traits::dimension > Hy = boost::none) { + typename MakeOptionalJacobian::type Hx = boost::none, + typename MakeOptionalJacobian::type Hy = boost::none) { assert(t >= 0.0 && t <= 1.5); if (Hx || Hy) { - typedef Eigen::Matrix::dimension, traits::dimension> Jacobian; typename traits::TangentVector log_Xinv_Y; - Jacobian Hx_tmp, Hy_tmp, H1, H2; + typename MakeJacobian::type Hx_tmp, Hy_tmp, H1, H2; T Xinv_Y = traits::Between(X, Y, Hx_tmp, Hy_tmp); log_Xinv_Y = traits::Logmap(Xinv_Y, H1); From 9f19077217e31187282c718e6a57226f5fd396c7 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Fri, 13 Aug 2021 19:54:23 -0400 Subject: [PATCH 145/248] simplified jacobian computation, improved comments --- gtsam/base/Lie.h | 20 +++++++------------- gtsam/geometry/CameraSet.h | 5 +++-- 2 files changed, 10 insertions(+), 15 deletions(-) diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index 1e19bfc0e..48d7bf531 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -329,21 +329,15 @@ T interpolate(const T& X, const T& Y, double t, assert(t >= 0.0 && t <= 1.5); if (Hx || Hy) { typename traits::TangentVector log_Xinv_Y; - typename MakeJacobian::type Hx_tmp, Hy_tmp, H1, H2; + typename MakeJacobian::type between_H_x, log_H, exp_H, compose_H_x; - T Xinv_Y = traits::Between(X, Y, Hx_tmp, Hy_tmp); - log_Xinv_Y = traits::Logmap(Xinv_Y, H1); - Hx_tmp = H1 * Hx_tmp; - Hy_tmp = H1 * Hy_tmp; - Xinv_Y = traits::Expmap(t * log_Xinv_Y, H1); - Hx_tmp = t * H1 * Hx_tmp; - Hy_tmp = t * H1 * Hy_tmp; - Xinv_Y = traits::Compose(X, Xinv_Y, H1, H2); - Hx_tmp = H1 + H2 * Hx_tmp; - Hy_tmp = H2 * Hy_tmp; + T Xinv_Y = traits::Between(X, Y, between_H_x); // between_H_y = identity + log_Xinv_Y = traits::Logmap(Xinv_Y, log_H); + Xinv_Y = traits::Expmap(t * log_Xinv_Y, exp_H); + Xinv_Y = traits::Compose(X, Xinv_Y, compose_H_x); // compose_H_xinv_y = identity - if(Hx) *Hx = Hx_tmp; - if(Hy) *Hy = Hy_tmp; + if(Hx) *Hx = compose_H_x + t * exp_H * log_H * between_H_x; + if(Hy) *Hy = t * exp_H * log_H; return Xinv_Y; } return traits::Compose(X, traits::Expmap(t * traits::Logmap(traits::Between(X, Y)))); diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h index f0bfffb58..3fc77bb2e 100644 --- a/gtsam/geometry/CameraSet.h +++ b/gtsam/geometry/CameraSet.h @@ -198,8 +198,9 @@ public: * G = F' * F - F' * E * P * E' * F * g = F' * (b - E * P * E' * b) * In this version, we allow for the case where the keys in the Jacobian are organized - * differents from the keys in the output SymmetricBlockMatrix - * In particular: each block of the Jacobian captures 2 poses (useful for rolling shutter and extrinsic calibration) + * differently from the keys in the output SymmetricBlockMatrix + * In particular: each diagonal block of the Jacobian F captures 2 poses (useful for rolling shutter and extrinsic calibration) + * such that F keeps the block structure that makes the Schur complement trick fast. */ template // N = 2 or 3 (point dimension), ND is the Jacobian block dimension, NDD is the Hessian block dimension static SymmetricBlockMatrix SchurComplementAndRearrangeBlocks( From a3ee695b85f92eb7487e568aa59f45f55b2080ca Mon Sep 17 00:00:00 2001 From: lcarlone Date: Fri, 13 Aug 2021 20:05:19 -0400 Subject: [PATCH 146/248] reformatted using google style --- .../slam/ProjectionFactorRollingShutter.h | 39 ++++++++++++------- 1 file changed, 24 insertions(+), 15 deletions(-) diff --git a/gtsam_unstable/slam/ProjectionFactorRollingShutter.h b/gtsam_unstable/slam/ProjectionFactorRollingShutter.h index fc7c939a8..6ab16f4a0 100644 --- a/gtsam_unstable/slam/ProjectionFactorRollingShutter.h +++ b/gtsam_unstable/slam/ProjectionFactorRollingShutter.h @@ -36,7 +36,8 @@ namespace gtsam { * @addtogroup SLAM */ -class ProjectionFactorRollingShutter : public NoiseModelFactor3 { +class ProjectionFactorRollingShutter : public NoiseModelFactor3 { protected: // Keep a copy of measurement and calibration for I/O @@ -80,10 +81,11 @@ class ProjectionFactorRollingShutter : public NoiseModelFactor3& K, - boost::optional body_P_sensor = boost::none) + boost::optional body_P_sensor = + boost::none) : Base(model, poseKey_a, poseKey_b, pointKey), measured_(measured), interp_param_(interp_param), @@ -107,11 +109,12 @@ class ProjectionFactorRollingShutter : public NoiseModelFactor3& K, bool throwCheirality, bool verboseCheirality, - boost::optional body_P_sensor = boost::none) + boost::optional body_P_sensor = + boost::none) : Base(model, poseKey_a, poseKey_b, pointKey), measured_(measured), interp_param_(interp_param), @@ -160,8 +163,9 @@ class ProjectionFactorRollingShutter : public NoiseModelFactor3 H1 = boost::none, + Vector evaluateError(const Pose3& pose_a, const Pose3& pose_b, + const Point3& point, boost::optional H1 = + boost::none, boost::optional H2 = boost::none, boost::optional H3 = boost::none) const { @@ -171,8 +175,10 @@ class ProjectionFactorRollingShutter : public NoiseModelFactor3 camera(pose.compose(*body_P_sensor_, HbodySensor), *K_); - Point2 reprojectionError(camera.project(point, Hprj, H3, boost::none) - measured_); + PinholeCamera camera( + pose.compose(*body_P_sensor_, HbodySensor), *K_); + Point2 reprojectionError( + camera.project(point, Hprj, H3, boost::none) - measured_); if (H1) *H1 = Hprj * HbodySensor * (*H1); if (H2) @@ -184,14 +190,15 @@ class ProjectionFactorRollingShutter : public NoiseModelFactor3 camera(pose, *K_); - Point2 reprojectionError(camera.project(point, Hprj, H3, boost::none) - measured_); + Point2 reprojectionError( + camera.project(point, Hprj, H3, boost::none) - measured_); if (H1) *H1 = Hprj * (*H1); if (H2) *H2 = Hprj * (*H2); return reprojectionError; } - } catch( CheiralityException& e) { + } catch (CheiralityException& e) { if (H1) *H1 = Matrix::Zero(2, 6); if (H2) @@ -252,6 +259,8 @@ class ProjectionFactorRollingShutter : public NoiseModelFactor3 struct traits : public Testable {}; +template<> struct traits : public Testable< + ProjectionFactorRollingShutter> { +}; -}//namespace gtsam +} //namespace gtsam From 0a8ebdf8cd027e93eef7e38ab1b3c8d2f62ce6d7 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Fri, 13 Aug 2021 21:42:09 -0400 Subject: [PATCH 147/248] renamed interp param to alpha, improved comments, added cpp --- gtsam/base/Lie.h | 5 +- .../slam/ProjectionFactorRollingShutter.cpp | 73 +++++++++++++++++ .../slam/ProjectionFactorRollingShutter.h | 78 ++++--------------- .../SmartProjectionPoseFactorRollingShutter.h | 40 +++++----- 4 files changed, 112 insertions(+), 84 deletions(-) create mode 100644 gtsam_unstable/slam/ProjectionFactorRollingShutter.cpp diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index 48d7bf531..34422edd7 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -320,13 +320,14 @@ T expm(const Vector& x, int K=7) { } /** - * Linear interpolation between X and Y by coefficient t in [0, 1.5] (t>1 implies extrapolation), with optional jacobians. + * Linear interpolation between X and Y by coefficient t (typically t \in [0,1], + * but can also be used to extrapolate before pose X or after pose Y), with optional jacobians. */ template T interpolate(const T& X, const T& Y, double t, typename MakeOptionalJacobian::type Hx = boost::none, typename MakeOptionalJacobian::type Hy = boost::none) { - assert(t >= 0.0 && t <= 1.5); + if (Hx || Hy) { typename traits::TangentVector log_Xinv_Y; typename MakeJacobian::type between_H_x, log_H, exp_H, compose_H_x; diff --git a/gtsam_unstable/slam/ProjectionFactorRollingShutter.cpp b/gtsam_unstable/slam/ProjectionFactorRollingShutter.cpp new file mode 100644 index 000000000..5fc1c05eb --- /dev/null +++ b/gtsam_unstable/slam/ProjectionFactorRollingShutter.cpp @@ -0,0 +1,73 @@ +/* ---------------------------------------------------------------------------- + + * 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 ProjectionFactorRollingShutter.cpp + * @brief Basic projection factor for rolling shutter cameras + * @author Yotam Stern + */ + +#include + +namespace gtsam { + +Vector ProjectionFactorRollingShutter::evaluateError( + const Pose3& pose_a, const Pose3& pose_b, const Point3& point, + boost::optional H1, boost::optional H2, + boost::optional H3) const { + + try { + Pose3 pose = interpolate(pose_a, pose_b, alpha_, H1, H2); + gtsam::Matrix Hprj; + if (body_P_sensor_) { + if (H1 || H2 || H3) { + gtsam::Matrix HbodySensor; + PinholeCamera camera( + pose.compose(*body_P_sensor_, HbodySensor), *K_); + Point2 reprojectionError( + camera.project(point, Hprj, H3, boost::none) - measured_); + if (H1) + *H1 = Hprj * HbodySensor * (*H1); + if (H2) + *H2 = Hprj * HbodySensor * (*H2); + return reprojectionError; + } else { + PinholeCamera camera(pose.compose(*body_P_sensor_), *K_); + return camera.project(point) - measured_; + } + } else { + PinholeCamera camera(pose, *K_); + Point2 reprojectionError( + camera.project(point, Hprj, H3, boost::none) - measured_); + if (H1) + *H1 = Hprj * (*H1); + if (H2) + *H2 = Hprj * (*H2); + return reprojectionError; + } + } catch (CheiralityException& e) { + if (H1) + *H1 = Matrix::Zero(2, 6); + if (H2) + *H2 = Matrix::Zero(2, 6); + if (H3) + *H3 = Matrix::Zero(2, 3); + if (verboseCheirality_) + std::cout << e.what() << ": Landmark " + << DefaultKeyFormatter(this->key2()) << " moved behind camera " + << DefaultKeyFormatter(this->key1()) << std::endl; + if (throwCheirality_) + throw CheiralityException(this->key2()); + } + return Vector2::Constant(2.0 * K_->fx()); +} + +} //namespace gtsam diff --git a/gtsam_unstable/slam/ProjectionFactorRollingShutter.h b/gtsam_unstable/slam/ProjectionFactorRollingShutter.h index 6ab16f4a0..5827a538c 100644 --- a/gtsam_unstable/slam/ProjectionFactorRollingShutter.h +++ b/gtsam_unstable/slam/ProjectionFactorRollingShutter.h @@ -31,8 +31,8 @@ namespace gtsam { * and a Point2 measurement taken starting at time A using a rolling shutter camera. * Pose A has timestamp t_A, and Pose B has timestamp t_B. The Point2 measurement has timestamp t_p (with t_A <= t_p <= t_B) * corresponding to the time of exposure of the row of the image the pixel belongs to. - * Let us define the interp_param = (t_p - t_A) / (t_B - t_A), we will use the pose interpolated between A and B by - * the interp_param to project the corresponding landmark to Point2. + * Let us define the alpha = (t_p - t_A) / (t_B - t_A), we will use the pose interpolated between A and B by + * the alpha to project the corresponding landmark to Point2. * @addtogroup SLAM */ @@ -42,7 +42,7 @@ class ProjectionFactorRollingShutter : public NoiseModelFactor3 K_; ///< shared pointer to calibration object boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame @@ -64,7 +64,7 @@ class ProjectionFactorRollingShutter : public NoiseModelFactor3& K, @@ -88,7 +88,7 @@ class ProjectionFactorRollingShutter : public NoiseModelFactor3& K, @@ -117,7 +117,7 @@ class ProjectionFactorRollingShutter : public NoiseModelFactor3(&p); - return e && Base::equals(p, tol) && (interp_param_ == e->interp_param()) + return e && Base::equals(p, tol) && (alpha_ == e->alpha()) && traits::Equals(this->measured_, e->measured_, tol) && this->K_->equals(*e->K_, tol) && (this->throwCheirality_ == e->throwCheirality_) @@ -167,53 +167,7 @@ class ProjectionFactorRollingShutter : public NoiseModelFactor3 H1 = boost::none, boost::optional H2 = boost::none, - boost::optional H3 = boost::none) const { - - try { - Pose3 pose = interpolate(pose_a, pose_b, interp_param_, H1, H2); - gtsam::Matrix Hprj; - if (body_P_sensor_) { - if (H1 || H2 || H3) { - gtsam::Matrix HbodySensor; - PinholeCamera camera( - pose.compose(*body_P_sensor_, HbodySensor), *K_); - Point2 reprojectionError( - camera.project(point, Hprj, H3, boost::none) - measured_); - if (H1) - *H1 = Hprj * HbodySensor * (*H1); - if (H2) - *H2 = Hprj * HbodySensor * (*H2); - return reprojectionError; - } else { - PinholeCamera camera(pose.compose(*body_P_sensor_), *K_); - return camera.project(point) - measured_; - } - } else { - PinholeCamera camera(pose, *K_); - Point2 reprojectionError( - camera.project(point, Hprj, H3, boost::none) - measured_); - if (H1) - *H1 = Hprj * (*H1); - if (H2) - *H2 = Hprj * (*H2); - return reprojectionError; - } - } catch (CheiralityException& e) { - if (H1) - *H1 = Matrix::Zero(2, 6); - if (H2) - *H2 = Matrix::Zero(2, 6); - if (H3) - *H3 = Matrix::Zero(2, 3); - if (verboseCheirality_) - std::cout << e.what() << ": Landmark " - << DefaultKeyFormatter(this->key2()) << " moved behind camera " - << DefaultKeyFormatter(this->key1()) << std::endl; - if (throwCheirality_) - throw CheiralityException(this->key2()); - } - return Vector2::Constant(2.0 * K_->fx()); - } + boost::optional H3 = boost::none) const; /** return the measurement */ const Point2& measured() const { @@ -226,8 +180,8 @@ class ProjectionFactorRollingShutter : public NoiseModelFactor3 > { std::vector> world_P_body_key_pairs_; /// interpolation factor (one for each observation) to interpolate between pair of consecutive poses - std::vector interp_params_; + std::vector alphas_; /// Pose of the camera in the body frame std::vector body_P_sensors_; @@ -92,12 +92,12 @@ PinholePose > { * single landmark in a single view (the measurement), interpolated from the 2 poses * @param world_P_body_key1 key corresponding to the first body poses (time <= time pixel is acquired) * @param world_P_body_key2 key corresponding to the second body poses (time >= time pixel is acquired) - * @param interp_param interpolation factor in [0,1], such that if interp_param = 0 the interpolated pose is the same as world_P_body_key1 + * @param alpha interpolation factor in [0,1], such that if alpha = 0 the interpolated pose is the same as world_P_body_key1 * @param K (fixed) camera intrinsic calibration * @param body_P_sensor (fixed) camera extrinsic calibration */ void add(const Point2& measured, const Key& world_P_body_key1, - const Key& world_P_body_key2, const double& interp_param, + const Key& world_P_body_key2, const double& alpha, const boost::shared_ptr& K, const Pose3 body_P_sensor = Pose3::identity()) { // store measurements in base class this->measured_.push_back(measured); @@ -113,7 +113,7 @@ PinholePose > { this->keys_.push_back(world_P_body_key2); // add only unique keys // store interpolation factor - interp_params_.push_back(interp_param); + alphas_.push_back(alpha); // store fixed intrinsic calibration K_all_.push_back(K); @@ -128,21 +128,21 @@ PinholePose > { * of a single landmark in the m views (the measurements) * @param world_P_body_key_pairs vector where the i-th element contains a pair of keys corresponding * to the pair of poses from which the observation pose for the i0-th measurement can be interpolated - * @param interp_params vector of interpolation params, one for each measurement (in the same order) + * @param alphas vector of interpolation params (in [0,1]), one for each measurement (in the same order) * @param Ks vector of (fixed) intrinsic calibration objects * @param body_P_sensors vector of (fixed) extrinsic calibration objects */ void add(const Point2Vector& measurements, const std::vector>& world_P_body_key_pairs, - const std::vector& interp_params, + const std::vector& alphas, const std::vector>& Ks, const std::vector body_P_sensors) { assert(world_P_body_key_pairs.size() == measurements.size()); - assert(world_P_body_key_pairs.size() == interp_params.size()); + assert(world_P_body_key_pairs.size() == alphas.size()); assert(world_P_body_key_pairs.size() == Ks.size()); for (size_t i = 0; i < measurements.size(); i++) { add(measurements[i], world_P_body_key_pairs[i].first, - world_P_body_key_pairs[i].second, interp_params[i], Ks[i], + world_P_body_key_pairs[i].second, alphas[i], Ks[i], body_P_sensors[i]); } } @@ -154,19 +154,19 @@ PinholePose > { * of a single landmark in the m views (the measurements) * @param world_P_body_key_pairs vector where the i-th element contains a pair of keys corresponding * to the pair of poses from which the observation pose for the i0-th measurement can be interpolated - * @param interp_params vector of interpolation params, one for each measurement (in the same order) + * @param alphas vector of interpolation params (in [0,1]), one for each measurement (in the same order) * @param K (fixed) camera intrinsic calibration (same for all measurements) * @param body_P_sensor (fixed) camera extrinsic calibration (same for all measurements) */ void add(const Point2Vector& measurements, const std::vector>& world_P_body_key_pairs, - const std::vector& interp_params, + const std::vector& alphas, const boost::shared_ptr& K, const Pose3 body_P_sensor = Pose3::identity()) { assert(world_P_body_key_pairs.size() == measurements.size()); - assert(world_P_body_key_pairs.size() == interp_params.size()); + assert(world_P_body_key_pairs.size() == alphas.size()); for (size_t i = 0; i < measurements.size(); i++) { add(measurements[i], world_P_body_key_pairs[i].first, - world_P_body_key_pairs[i].second, interp_params[i], K, body_P_sensor); + world_P_body_key_pairs[i].second, alphas[i], K, body_P_sensor); } } @@ -180,9 +180,9 @@ PinholePose > { return world_P_body_key_pairs_; } - /// return the interpolation factors interp_params - const std::vector interp_params() const { - return interp_params_; + /// return the interpolation factors alphas + const std::vector alphas() const { + return alphas_; } /// return the extrinsic camera calibration body_P_sensors @@ -204,7 +204,7 @@ PinholePose > { << keyFormatter(world_P_body_key_pairs_[i].first) << std::endl; std::cout << " pose2 key: " << keyFormatter(world_P_body_key_pairs_[i].second) << std::endl; - std::cout << " interp_param: " << interp_params_[i] << std::endl; + std::cout << " alpha: " << alphas_[i] << std::endl; body_P_sensors_[i].print("extrinsic calibration:\n"); K_all_[i]->print("intrinsic calibration = "); } @@ -239,7 +239,7 @@ PinholePose > { }else{ extrinsicCalibrationEqual = false; } return e && Base::equals(p, tol) && K_all_ == e->calibration() - && interp_params_ == e->interp_params() && keyPairsEqual && extrinsicCalibrationEqual; + && alphas_ == e->alphas() && keyPairsEqual && extrinsicCalibrationEqual; } /** @@ -267,7 +267,7 @@ PinholePose > { for (size_t i = 0; i < numViews; i++) { // for each camera/measurement const Pose3& w_P_body1 = values.at(world_P_body_key_pairs_[i].first); const Pose3& w_P_body2 = values.at(world_P_body_key_pairs_[i].second); - double interpolationFactor = interp_params_[i]; + double interpolationFactor = alphas_[i]; // get interpolated pose: const Pose3& w_P_body = interpolate(w_P_body1, w_P_body2,interpolationFactor, dInterpPose_dPoseBody1, dInterpPose_dPoseBody2); const Pose3& body_P_cam = body_P_sensors_[i]; @@ -377,7 +377,7 @@ PinholePose > { typename Base::Cameras cameras(const Values& values) const override { size_t numViews = this->measured_.size(); assert(numViews == K_all_.size()); - assert(numViews == interp_params_.size()); + assert(numViews == alphas_.size()); assert(numViews == body_P_sensors_.size()); assert(numViews == world_P_body_key_pairs_.size()); @@ -385,7 +385,7 @@ PinholePose > { for (size_t i = 0; i < numViews; i++) { // for each measurement const Pose3& w_P_body1 = values.at(world_P_body_key_pairs_[i].first); const Pose3& w_P_body2 = values.at(world_P_body_key_pairs_[i].second); - double interpolationFactor = interp_params_[i]; + double interpolationFactor = alphas_[i]; const Pose3& w_P_body = interpolate(w_P_body1, w_P_body2, interpolationFactor); const Pose3& body_P_cam = body_P_sensors_[i]; const Pose3& w_P_cam = w_P_body.compose(body_P_cam); From 5dff04648867eeae2a8067f475e714d1f644d0e4 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Sat, 14 Aug 2021 01:14:23 -0400 Subject: [PATCH 148/248] Start wrapping the verbosity options for GNC --- gtsam/nonlinear/nonlinear.i | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index 73eaef125..c3f17c02e 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -527,8 +527,13 @@ template virtual class GncParams { GncParams(const PARAMS& baseOptimizerParams); GncParams(); + void setVerbosityGNC(const gtsam::GncParams::Verbosity value); void print(const string& str) const; }; + +typedef gtsam::GncParams::Verbosity::SILENT GncVerbositySilent; +typedef gtsam::GncParams::Verbosity::SUMMARY GncVerbositySummary; +typedef gtsam::GncParams::Verbosity::VALUES GncVerbosityValues; typedef gtsam::GncParams GncGaussNewtonParams; typedef gtsam::GncParams GncLMParams; From 0e231be536f64bb92264d5b53d64fd4d6063fa0e Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 19 Aug 2021 19:33:30 -0400 Subject: [PATCH 149/248] modernize NonlinearEquality.h --- gtsam/nonlinear/NonlinearEquality.h | 18 +++++++----------- 1 file changed, 7 insertions(+), 11 deletions(-) diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index 6b9972156..243611433 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -66,12 +66,9 @@ private: public: - /** - * Function that compares two values - */ - typedef std::function CompareFunction; + /// Function that compares two values. + using CompareFunction = std::function; CompareFunction compare_; -// bool (*compare_)(const T& a, const T& b); /// Default constructor - only for serialization NonlinearEquality() { @@ -198,9 +195,8 @@ private: }; // \class NonlinearEquality -template -struct traits > : Testable > { -}; +template +struct traits> : Testable> {}; /* ************************************************************************* */ /** @@ -285,9 +281,9 @@ private: }; // \NonlinearEquality1 -template -struct traits > : Testable > { -}; +template +struct traits > + : Testable > {}; /* ************************************************************************* */ /** From d90dca7fab5014478a356f101d505523ca8375b3 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 19 Aug 2021 19:33:49 -0400 Subject: [PATCH 150/248] update docs to reflect min Boost version --- INSTALL.md | 2 +- README.md | 2 +- cmake/HandleBoost.cmake | 4 ++-- cmake/HandleCPack.cmake | 2 +- 4 files changed, 5 insertions(+), 5 deletions(-) diff --git a/INSTALL.md b/INSTALL.md index 64857774a..074ce6438 100644 --- a/INSTALL.md +++ b/INSTALL.md @@ -13,7 +13,7 @@ $ make install ## Important Installation Notes 1. GTSAM requires the following libraries to be installed on your system: - - BOOST version 1.58 or greater (install through Linux repositories or MacPorts) + - BOOST version 1.67 or greater (install through Linux repositories or MacPorts) - Cmake version 3.0 or higher - Support for XCode 4.3 command line tools on Mac requires CMake 2.8.8 or higher diff --git a/README.md b/README.md index 60eff197a..82a17a8fa 100644 --- a/README.md +++ b/README.md @@ -40,7 +40,7 @@ $ make install Prerequisites: -- [Boost](http://www.boost.org/users/download/) >= 1.58 (Ubuntu: `sudo apt-get install libboost-all-dev`) +- [Boost](http://www.boost.org/users/download/) >= 1.67 (Ubuntu: `sudo apt-get install libboost-all-dev`) - [CMake](http://www.cmake.org/cmake/resources/software.html) >= 3.0 (Ubuntu: `sudo apt-get install cmake`) - A modern compiler, i.e., at least gcc 4.7.3 on Linux. diff --git a/cmake/HandleBoost.cmake b/cmake/HandleBoost.cmake index e73c2237d..a02afba57 100644 --- a/cmake/HandleBoost.cmake +++ b/cmake/HandleBoost.cmake @@ -22,7 +22,7 @@ endif() # Store these in variables so they are automatically replicated in GTSAMConfig.cmake and such. -set(BOOST_FIND_MINIMUM_VERSION 1.58) +set(BOOST_FIND_MINIMUM_VERSION 1.67) set(BOOST_FIND_MINIMUM_COMPONENTS serialization system filesystem thread program_options date_time timer chrono regex) find_package(Boost ${BOOST_FIND_MINIMUM_VERSION} COMPONENTS ${BOOST_FIND_MINIMUM_COMPONENTS}) @@ -30,7 +30,7 @@ find_package(Boost ${BOOST_FIND_MINIMUM_VERSION} COMPONENTS ${BOOST_FIND_MINIMUM # Required components if(NOT Boost_SERIALIZATION_LIBRARY OR NOT Boost_SYSTEM_LIBRARY OR NOT Boost_FILESYSTEM_LIBRARY OR NOT Boost_THREAD_LIBRARY OR NOT Boost_DATE_TIME_LIBRARY) - message(FATAL_ERROR "Missing required Boost components >= v1.58, please install/upgrade Boost or configure your search paths.") + message(FATAL_ERROR "Missing required Boost components >= v1.67, please install/upgrade Boost or configure your search paths.") endif() option(GTSAM_DISABLE_NEW_TIMERS "Disables using Boost.chrono for timing" OFF) diff --git a/cmake/HandleCPack.cmake b/cmake/HandleCPack.cmake index 1c32433a4..eff36c42d 100644 --- a/cmake/HandleCPack.cmake +++ b/cmake/HandleCPack.cmake @@ -25,4 +25,4 @@ set(CPACK_SOURCE_PACKAGE_FILE_NAME "gtsam-${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION # Deb-package specific cpack set(CPACK_DEBIAN_PACKAGE_NAME "libgtsam-dev") -set(CPACK_DEBIAN_PACKAGE_DEPENDS "libboost-dev (>= 1.58)") #Example: "libc6 (>= 2.3.1-6), libgcc1 (>= 1:3.4.2-12)") +set(CPACK_DEBIAN_PACKAGE_DEPENDS "libboost-dev (>= 1.67)") #Example: "libc6 (>= 2.3.1-6), libgcc1 (>= 1:3.4.2-12)") From 158a620eba4e4cb4eff5a3bb1d773c4c59d4ac63 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 19 Aug 2021 19:34:58 -0400 Subject: [PATCH 151/248] small wrapper updates --- gtsam/base/base.i | 21 +++++++-------------- gtsam/nonlinear/nonlinear.i | 4 ++-- 2 files changed, 9 insertions(+), 16 deletions(-) diff --git a/gtsam/base/base.i b/gtsam/base/base.i index c24b04088..d9c51fbe8 100644 --- a/gtsam/base/base.i +++ b/gtsam/base/base.i @@ -33,13 +33,13 @@ class IndexPair { size_t j() const; }; -// template -// class DSFMap { -// DSFMap(); -// KEY find(const KEY& key) const; -// void merge(const KEY& x, const KEY& y); -// std::map sets(); -// }; +template +class DSFMap { + DSFMap(); + KEY find(const KEY& key) const; + void merge(const KEY& x, const KEY& y); + std::map sets(); +}; class IndexPairSet { IndexPairSet(); @@ -81,13 +81,6 @@ class IndexPairSetMap { gtsam::IndexPairSet at(gtsam::IndexPair& key); }; -class DSFMapIndexPair { - DSFMapIndexPair(); - gtsam::IndexPair find(const gtsam::IndexPair& key) const; - void merge(const gtsam::IndexPair& x, const gtsam::IndexPair& y); - gtsam::IndexPairSetMap sets(); -}; - #include bool linear_independent(Matrix A, Matrix B, double tol); diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index 73eaef125..91525c5fd 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -572,8 +572,8 @@ virtual class GncOptimizer { gtsam::Values optimize(); }; -typedef gtsam::GncOptimizer > GncGaussNewtonOptimizer; -typedef gtsam::GncOptimizer > GncLMOptimizer; +typedef gtsam::GncOptimizer> GncGaussNewtonOptimizer; +typedef gtsam::GncOptimizer> GncLMOptimizer; #include virtual class LevenbergMarquardtOptimizer : gtsam::NonlinearOptimizer { From 910aceae74c85a81d38493515ddcbdc3cedc0c7b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 19 Aug 2021 23:32:34 -0400 Subject: [PATCH 152/248] fix extractPoint2/3 and added C++ tests --- gtsam/geometry/tests/testUtilities.cpp | 64 ++++++++++++++++++++++++++ gtsam/nonlinear/utilities.h | 32 ++++++++++--- 2 files changed, 90 insertions(+), 6 deletions(-) create mode 100644 gtsam/geometry/tests/testUtilities.cpp diff --git a/gtsam/geometry/tests/testUtilities.cpp b/gtsam/geometry/tests/testUtilities.cpp new file mode 100644 index 000000000..25ac3acc8 --- /dev/null +++ b/gtsam/geometry/tests/testUtilities.cpp @@ -0,0 +1,64 @@ +/* ---------------------------------------------------------------------------- + + * 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 testUtilities.cpp + * @date Aug 19, 2021 + * @author Varun Agrawal + * @brief Tests for the utilities. + */ + +#include +#include +#include +#include +#include +#include +#include + +using namespace gtsam; +using gtsam::symbol_shorthand::L; +using gtsam::symbol_shorthand::R; +using gtsam::symbol_shorthand::X; + +/* ************************************************************************* */ +TEST(Utilities, ExtractPoint2) { + Point2 p0(0, 0), p1(1, 0); + Values values; + values.insert(L(0), p0); + values.insert(L(1), p1); + values.insert(R(0), Rot3()); + values.insert(X(0), Pose3()); + + Matrix all_points = utilities::extractPoint2(values); + EXPECT_LONGS_EQUAL(2, all_points.rows()); +} + +/* ************************************************************************* */ +TEST(Utilities, ExtractPoint3) { + Point3 p0(0, 0, 0), p1(1, 0, 0); + Values values; + values.insert(L(0), p0); + values.insert(L(1), p1); + values.insert(R(0), Rot3()); + values.insert(X(0), Pose3()); + + Matrix all_points = utilities::extractPoint3(values); + EXPECT_LONGS_EQUAL(2, all_points.rows()); +} + +/* ************************************************************************* */ +int main() { + srand(time(nullptr)); + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/gtsam/nonlinear/utilities.h b/gtsam/nonlinear/utilities.h index 4e79e20ff..5a7b4f473 100644 --- a/gtsam/nonlinear/utilities.h +++ b/gtsam/nonlinear/utilities.h @@ -89,21 +89,41 @@ KeySet createKeySet(std::string s, const Vector& I) { /// Extract all Point2 values into a single matrix [x y] Matrix extractPoint2(const Values& values) { + Values::ConstFiltered points = values.filter(); + // Point2 is aliased as a gtsam::Vector in the wrapper + Values::ConstFiltered points2 = values.filter(); + + Matrix result(points.size() + points2.size(), 2); + size_t j = 0; - Values::ConstFiltered points = values.filter(); - Matrix result(points.size(), 2); - for(const auto& key_value: points) + for (const auto& key_value : points) { result.row(j++) = key_value.value; + } + for (const auto& key_value : points2) { + if (key_value.value.rows() == 2) { + result.row(j++) = key_value.value; + } + } return result; } /// Extract all Point3 values into a single matrix [x y z] Matrix extractPoint3(const Values& values) { - Values::ConstFiltered points = values.filter(); - Matrix result(points.size(), 3); + Values::ConstFiltered points = values.filter(); + // Point3 is aliased as a gtsam::Vector in the wrapper + Values::ConstFiltered points2 = values.filter(); + + Matrix result(points.size() + points2.size(), 3); + size_t j = 0; - for(const auto& key_value: points) + for (const auto& key_value : points) { result.row(j++) = key_value.value; + } + for (const auto& key_value : points2) { + if (key_value.value.rows() == 3) { + result.row(j++) = key_value.value; + } + } return result; } From e056a3393cbca5edce8523efa4e135dde518905c Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 19 Aug 2021 23:32:52 -0400 Subject: [PATCH 153/248] added Python tests --- python/gtsam/tests/test_Utilities.py | 108 +++++++++++++++++++++++++++ 1 file changed, 108 insertions(+) create mode 100644 python/gtsam/tests/test_Utilities.py diff --git a/python/gtsam/tests/test_Utilities.py b/python/gtsam/tests/test_Utilities.py new file mode 100644 index 000000000..ef53660bf --- /dev/null +++ b/python/gtsam/tests/test_Utilities.py @@ -0,0 +1,108 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Utilities unit tests. +Author: Varun Agrawal +""" +# pylint: disable=invalid-name, E1101, E0611 +import unittest + +import numpy as np + +import gtsam +from gtsam.utils.test_case import GtsamTestCase + + +class TestUtilites(GtsamTestCase): + """Test various GTSAM utilities.""" + def test_createKeyList(self): + """Test createKeyList.""" + I = [0, 1, 2] + kl = gtsam.utilities.createKeyList(I) + self.assertEqual(kl.size(), 3) + + kl = gtsam.utilities.createKeyList("s", I) + self.assertEqual(kl.size(), 3) + + def test_createKeyVector(self): + """Test createKeyVector.""" + I = [0, 1, 2] + kl = gtsam.utilities.createKeyVector(I) + self.assertEqual(len(kl), 3) + + kl = gtsam.utilities.createKeyVector("s", I) + self.assertEqual(len(kl), 3) + + def test_createKeySet(self): + """Test createKeySet.""" + I = [0, 1, 2] + kl = gtsam.utilities.createKeySet(I) + self.assertEqual(kl.size(), 3) + + kl = gtsam.utilities.createKeySet("s", I) + self.assertEqual(kl.size(), 3) + + def test_extractPoint2(self): + """Test extractPoint2.""" + initial = gtsam.Values() + point2 = gtsam.Point2(0.0, 0.1) + initial.insert(1, gtsam.Pose2(0.0, 0.1, 0.1)) + initial.insert(2, point2) + np.testing.assert_equal(gtsam.utilities.extractPoint2(initial), + point2.reshape(-1, 2)) + + def test_extractPoint3(self): + """Test extractPoint3.""" + initial = gtsam.Values() + point3 = gtsam.Point3(0.0, 0.1, 0.0) + initial.insert(1, gtsam.Pose2(0.0, 0.1, 0.1)) + initial.insert(2, point3) + np.testing.assert_equal(gtsam.utilities.extractPoint3(initial), + point3.reshape(-1, 3)) + + def test_allPose2s(self): + """Test allPose2s.""" + initial = gtsam.Values() + initial.insert(0, gtsam.Pose2()) + initial.insert(1, gtsam.Pose2(1, 1, 1)) + initial.insert(2, gtsam.Point2(1, 1)) + initial.insert(3, gtsam.Point3(1, 2, 3)) + result = gtsam.utilities.allPose2s(initial) + self.assertEqual(result.size(), 2) + + def test_extractPose2(self): + """Test extractPose2.""" + initial = gtsam.Values() + pose2 = np.asarray((0.0, 0.1, 0.1)) + + initial.insert(1, gtsam.Pose2(*pose2)) + initial.insert(2, gtsam.Point3(0.0, 0.1, 0.0)) + np.testing.assert_allclose(gtsam.utilities.extractPose2(initial), + pose2.reshape(-1, 3)) + + def test_allPose3s(self): + """Test allPose3s.""" + initial = gtsam.Values() + initial.insert(0, gtsam.Pose3()) + initial.insert(2, gtsam.Point2(1, 1)) + initial.insert(1, gtsam.Pose3()) + initial.insert(3, gtsam.Point3(1, 2, 3)) + result = gtsam.utilities.allPose3s(initial) + self.assertEqual(result.size(), 2) + + def test_extractPose3(self): + """Test extractPose3.""" + initial = gtsam.Values() + pose3 = np.asarray([1., 0., 0., 0., 1., 0., 0., 0., 1., 0., 0., 0.]) + initial.insert(1, gtsam.Pose2(0.0, 0.1, 0.1)) + initial.insert(2, gtsam.Pose3()) + np.testing.assert_allclose(gtsam.utilities.extractPose3(initial), + pose3.reshape(-1, 12)) + + +if __name__ == "__main__": + unittest.main() From d074dbedf50d99045ed425d1c59edf90e45d224a Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 20 Aug 2021 00:55:10 -0400 Subject: [PATCH 154/248] updated docs --- gtsam/nonlinear/utilities.h | 56 +++++++++++++++++++++++++++++-------- 1 file changed, 44 insertions(+), 12 deletions(-) diff --git a/gtsam/nonlinear/utilities.h b/gtsam/nonlinear/utilities.h index 5a7b4f473..fdc1da2c4 100644 --- a/gtsam/nonlinear/utilities.h +++ b/gtsam/nonlinear/utilities.h @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file matlab.h + * @file utilities.h * @brief Contains *generic* global functions designed particularly for the matlab interface * @author Stephen Williams */ @@ -164,11 +164,18 @@ Matrix extractPose3(const Values& values) { /// Perturb all Point2 values using normally distributed noise void perturbPoint2(Values& values, double sigma, int32_t seed = 42u) { - noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(2, - sigma); + noiseModel::Isotropic::shared_ptr model = + noiseModel::Isotropic::Sigma(2, sigma); Sampler sampler(model, seed); - for(const auto& key_value: values.filter()) { - values.update(key_value.key, key_value.value + Point2(sampler.sample())); + for (const auto& key_value : values.filter()) { + values.update(key_value.key, + key_value.value + Point2(sampler.sample())); + } + for (const auto& key_value : values.filter()) { + if (key_value.value.rows() == 2) { + values.update(key_value.key, + key_value.value + Point2(sampler.sample())); + } } } @@ -185,19 +192,34 @@ void perturbPose2(Values& values, double sigmaT, double sigmaR, int32_t seed = /// Perturb all Point3 values using normally distributed noise void perturbPoint3(Values& values, double sigma, int32_t seed = 42u) { - noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3, - sigma); + noiseModel::Isotropic::shared_ptr model = + noiseModel::Isotropic::Sigma(3, sigma); Sampler sampler(model, seed); - for(const auto& key_value: values.filter()) { - values.update(key_value.key, key_value.value + Point3(sampler.sample())); + for (const auto& key_value : values.filter()) { + values.update(key_value.key, + key_value.value + Point3(sampler.sample())); + } + for (const auto& key_value : values.filter()) { + if (key_value.value.rows() == 3) { + values.update(key_value.key, + key_value.value + Point3(sampler.sample())); + } } } -/// Insert a number of initial point values by backprojecting +/** + * @brief Insert a number of initial point values by backprojecting + * + * @param values The values dict to insert the backprojections to. + * @param camera The camera model. + * @param J Vector of key indices. + * @param Z 2*J matrix of pixel values. + * @param depth Initial depth value. + */ void insertBackprojections(Values& values, const PinholeCamera& camera, const Vector& J, const Matrix& Z, double depth) { if (Z.rows() != 2) - throw std::invalid_argument("insertBackProjections: Z must be 2*K"); + throw std::invalid_argument("insertBackProjections: Z must be 2*J"); if (Z.cols() != J.size()) throw std::invalid_argument( "insertBackProjections: J and Z must have same number of entries"); @@ -208,7 +230,17 @@ void insertBackprojections(Values& values, const PinholeCamera& camera, } } -/// Insert multiple projection factors for a single pose key +/** + * @brief Insert multiple projection factors for a single pose key + * + * @param graph The nonlinear factor graph to add the factors to. + * @param i Camera key. + * @param J Vector of key indices. + * @param Z 2*J matrix of pixel values. + * @param model Factor noise model. + * @param K Calibration matrix. + * @param body_P_sensor Pose of the camera sensor in the body frame. + */ void insertProjectionFactors(NonlinearFactorGraph& graph, Key i, const Vector& J, const Matrix& Z, const SharedNoiseModel& model, const Cal3_S2::shared_ptr K, const Pose3& body_P_sensor = Pose3()) { From 0098e76e99a714f56fedb86174761787ae46e1cf Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 20 Aug 2021 00:55:15 -0400 Subject: [PATCH 155/248] full slew of tests --- python/gtsam/tests/test_Utilities.py | 88 ++++++++++++++++++++++++++++ 1 file changed, 88 insertions(+) diff --git a/python/gtsam/tests/test_Utilities.py b/python/gtsam/tests/test_Utilities.py index ef53660bf..851684f12 100644 --- a/python/gtsam/tests/test_Utilities.py +++ b/python/gtsam/tests/test_Utilities.py @@ -103,6 +103,94 @@ class TestUtilites(GtsamTestCase): np.testing.assert_allclose(gtsam.utilities.extractPose3(initial), pose3.reshape(-1, 12)) + def test_perturbPoint2(self): + """Test perturbPoint2.""" + values = gtsam.Values() + values.insert(0, gtsam.Pose3()) + values.insert(1, gtsam.Point2(1, 1)) + gtsam.utilities.perturbPoint2(values, 1.0) + self.assertTrue( + not np.allclose(values.atPoint2(1), gtsam.Point2(1, 1))) + + def test_perturbPose2(self): + """Test perturbPose2.""" + values = gtsam.Values() + values.insert(0, gtsam.Pose2()) + values.insert(1, gtsam.Point2(1, 1)) + gtsam.utilities.perturbPose2(values, 1, 1) + self.assertTrue(values.atPose2(0) != gtsam.Pose2()) + + def test_perturbPoint3(self): + """Test perturbPoint3.""" + values = gtsam.Values() + point3 = gtsam.Point3(0, 0, 0) + values.insert(0, gtsam.Pose2()) + values.insert(1, point3) + gtsam.utilities.perturbPoint3(values, 1) + self.assertTrue(not np.allclose(values.atPoint3(1), point3)) + + def test_insertBackprojections(self): + """Test insertBackprojections.""" + values = gtsam.Values() + cam = gtsam.PinholeCameraCal3_S2() + gtsam.utilities.insertBackprojections( + values, cam, [0, 1, 2], np.asarray([[20, 30, 40], [20, 30, 40]]), + 10) + np.testing.assert_allclose(values.atPoint3(0), + gtsam.Point3(200, 200, 10)) + + def test_insertProjectionFactors(self): + """Test insertProjectionFactors.""" + graph = gtsam.NonlinearFactorGraph() + gtsam.utilities.insertProjectionFactors( + graph, 0, [0, 1], np.asarray([[20, 30], [20, 30]]), + gtsam.noiseModel.Isotropic.Sigma(2, 0.1), gtsam.Cal3_S2()) + self.assertEqual(graph.size(), 2) + + graph = gtsam.NonlinearFactorGraph() + gtsam.utilities.insertProjectionFactors( + graph, 0, [0, 1], np.asarray([[20, 30], [20, 30]]), + gtsam.noiseModel.Isotropic.Sigma(2, 0.1), gtsam.Cal3_S2(), + gtsam.Pose3(gtsam.Rot3(), gtsam.Point3(1, 0, 0))) + self.assertEqual(graph.size(), 2) + + def test_reprojectionErrors(self): + """Test reprojectionErrors.""" + pixels = np.asarray([[20, 30], [20, 30]]) + I = [1, 2] + K = gtsam.Cal3_S2() + graph = gtsam.NonlinearFactorGraph() + gtsam.utilities.insertProjectionFactors( + graph, 0, I, pixels, gtsam.noiseModel.Isotropic.Sigma(2, 0.1), K) + values = gtsam.Values() + values.insert(0, gtsam.Pose3()) + cam = gtsam.PinholeCameraCal3_S2(gtsam.Pose3(), K) + gtsam.utilities.insertBackprojections(values, cam, I, pixels, 10) + errors = gtsam.utilities.reprojectionErrors(graph, values) + np.testing.assert_allclose(errors, np.zeros((2, 2))) + + def test_localToWorld(self): + """Test localToWorld.""" + local = gtsam.Values() + local.insert(0, gtsam.Point2(10, 10)) + local.insert(1, gtsam.Pose2(6, 11, 0.0)) + base = gtsam.Pose2(1, 0, 0) + world = gtsam.utilities.localToWorld(local, base) + + expected_point2 = gtsam.Point2(11, 10) + expected_pose2 = gtsam.Pose2(7, 11, 0) + np.testing.assert_allclose(world.atPoint2(0), expected_point2) + np.testing.assert_allclose( + world.atPose2(1).matrix(), expected_pose2.matrix()) + + user_keys = [1] + world = gtsam.utilities.localToWorld(local, base, user_keys) + np.testing.assert_allclose( + world.atPose2(1).matrix(), expected_pose2.matrix()) + + # Raise error since 0 is not in user_keys + self.assertRaises(RuntimeError, world.atPoint2, 0) + if __name__ == "__main__": unittest.main() From a7ff8e06003ee05f6d5ccc5e4b6ddb0ef1f6abc1 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 20 Aug 2021 00:55:39 -0400 Subject: [PATCH 156/248] update wrapper with defaults --- gtsam/gtsam.i | 19 +++++++------------ 1 file changed, 7 insertions(+), 12 deletions(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index a55581e50..67c3278a3 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -165,22 +165,17 @@ gtsam::Values allPose2s(gtsam::Values& values); Matrix extractPose2(const gtsam::Values& values); gtsam::Values allPose3s(gtsam::Values& values); Matrix extractPose3(const gtsam::Values& values); -void perturbPoint2(gtsam::Values& values, double sigma, int seed); +void perturbPoint2(gtsam::Values& values, double sigma, int seed = 42u); void perturbPose2(gtsam::Values& values, double sigmaT, double sigmaR, - int seed); -void perturbPoint3(gtsam::Values& values, double sigma, int seed); + int seed = 42u); +void perturbPoint3(gtsam::Values& values, double sigma, int seed = 42u); void insertBackprojections(gtsam::Values& values, const gtsam::PinholeCamera& c, Vector J, Matrix Z, double depth); -void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, - Vector J, Matrix Z, - const gtsam::noiseModel::Base* model, - const gtsam::Cal3_S2* K); -void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, - Vector J, Matrix Z, - const gtsam::noiseModel::Base* model, - const gtsam::Cal3_S2* K, - const gtsam::Pose3& body_P_sensor); +void insertProjectionFactors( + gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, + const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K, + const gtsam::Pose3& body_P_sensor = gtsam::Pose3()); Matrix reprojectionErrors(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values); gtsam::Values localToWorld(const gtsam::Values& local, From 42b75253373bae499b40ac34631b8895c195a0f4 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 20 Aug 2021 11:10:13 -0400 Subject: [PATCH 157/248] set lowest common boost version --- INSTALL.md | 4 ++-- README.md | 12 ++++++------ cmake/HandleBoost.cmake | 4 ++-- cmake/HandleCPack.cmake | 2 +- 4 files changed, 11 insertions(+), 11 deletions(-) diff --git a/INSTALL.md b/INSTALL.md index 074ce6438..3a0f2896a 100644 --- a/INSTALL.md +++ b/INSTALL.md @@ -13,7 +13,7 @@ $ make install ## Important Installation Notes 1. GTSAM requires the following libraries to be installed on your system: - - BOOST version 1.67 or greater (install through Linux repositories or MacPorts) + - BOOST version 1.65 or greater (install through Linux repositories or MacPorts) - Cmake version 3.0 or higher - Support for XCode 4.3 command line tools on Mac requires CMake 2.8.8 or higher @@ -70,7 +70,7 @@ execute commands as follows for an out-of-source build: - When using `GTSAM_BUILD_WITH_MARCH_NATIVE=ON`, you may encounter issues in running tests which we are still investigating: - Use of a version of GCC < 7.5 results in an "Indeterminant Linear System" error for `testSmartProjectionFactor`. - - Use of Boost version < 1.67 with clang will give a segfault for mulitple test cases. + - Use of Boost version < 1.65 with clang will give a segfault for mulitple test cases. - MSVC 2013 is not yet supported because it cannot build the serialization module of Boost 1.55 (or earlier). # Windows Installation diff --git a/README.md b/README.md index 82a17a8fa..046132301 100644 --- a/README.md +++ b/README.md @@ -40,7 +40,7 @@ $ make install Prerequisites: -- [Boost](http://www.boost.org/users/download/) >= 1.67 (Ubuntu: `sudo apt-get install libboost-all-dev`) +- [Boost](http://www.boost.org/users/download/) >= 1.65 (Ubuntu: `sudo apt-get install libboost-all-dev`) - [CMake](http://www.cmake.org/cmake/resources/software.html) >= 3.0 (Ubuntu: `sudo apt-get install cmake`) - A modern compiler, i.e., at least gcc 4.7.3 on Linux. @@ -55,9 +55,9 @@ Optional prerequisites - used automatically if findable by CMake: GTSAM 4 introduces several new features, most notably Expressions and a Python toolbox. It also introduces traits, a C++ technique that allows optimizing with non-GTSAM types. That opens the door to retiring geometric types such as Point2 and Point3 to pure Eigen types, which we also do. A significant change which will not trigger a compile error is that zero-initializing of Point2 and Point3 is deprecated, so please be aware that this might render functions using their default constructor incorrect. -GTSAM 4 also deprecated some legacy functionality and wrongly named methods. If you are on a 4.0.X release, you can define the flag GTSAM_ALLOW_DEPRECATED_SINCE_V4 to use the deprecated methods. +GTSAM 4 also deprecated some legacy functionality and wrongly named methods. If you are on a 4.0.X release, you can define the flag `GTSAM_ALLOW_DEPRECATED_SINCE_V4` to use the deprecated methods. -GTSAM 4.1 added a new pybind wrapper, and **removed** the deprecated functionality. There is a flag GTSAM_ALLOW_DEPRECATED_SINCE_V41 for newly deprecated methods since the 4.1 release, which is on by default, allowing anyone to just pull version 4.1 and compile. +GTSAM 4.1 added a new pybind wrapper, and **removed** the deprecated functionality. There is a flag `GTSAM_ALLOW_DEPRECATED_SINCE_V41` for newly deprecated methods since the 4.1 release, which is on by default, allowing anyone to just pull version 4.1 and compile. ## Wrappers @@ -68,16 +68,16 @@ We provide support for [MATLAB](matlab/README.md) and [Python](python/README.md) GTSAM includes a state of the art IMU handling scheme based on -- Todd Lupton and Salah Sukkarieh, "Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built Environments Without Initial Conditions", TRO, 28(1):61-76, 2012. [[link]](https://ieeexplore.ieee.org/document/6092505) +- Todd Lupton and Salah Sukkarieh, _"Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built Environments Without Initial Conditions"_, TRO, 28(1):61-76, 2012. [[link]](https://ieeexplore.ieee.org/document/6092505) Our implementation improves on this using integration on the manifold, as detailed in -- Luca Carlone, Zsolt Kira, Chris Beall, Vadim Indelman, and Frank Dellaert, "Eliminating conditionally independent sets in factor graphs: a unifying perspective based on smart factors", Int. Conf. on Robotics and Automation (ICRA), 2014. [[link]](https://ieeexplore.ieee.org/abstract/document/6907483) +- Luca Carlone, Zsolt Kira, Chris Beall, Vadim Indelman, and Frank Dellaert, _"Eliminating conditionally independent sets in factor graphs: a unifying perspective based on smart factors"_, Int. Conf. on Robotics and Automation (ICRA), 2014. [[link]](https://ieeexplore.ieee.org/abstract/document/6907483) - Christian Forster, Luca Carlone, Frank Dellaert, and Davide Scaramuzza, "IMU Preintegration on Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation", Robotics: Science and Systems (RSS), 2015. [[link]](http://www.roboticsproceedings.org/rss11/p06.pdf) If you are using the factor in academic work, please cite the publications above. -In GTSAM 4 a new and more efficient implementation, based on integrating on the NavState tangent space and detailed in [this document](doc/ImuFactor.pdf), is enabled by default. To switch to the RSS 2015 version, set the flag **GTSAM_TANGENT_PREINTEGRATION** to OFF. +In GTSAM 4 a new and more efficient implementation, based on integrating on the NavState tangent space and detailed in [this document](doc/ImuFactor.pdf), is enabled by default. To switch to the RSS 2015 version, set the flag `GTSAM_TANGENT_PREINTEGRATION` to OFF. ## Additional Information diff --git a/cmake/HandleBoost.cmake b/cmake/HandleBoost.cmake index a02afba57..6c742cfe5 100644 --- a/cmake/HandleBoost.cmake +++ b/cmake/HandleBoost.cmake @@ -22,7 +22,7 @@ endif() # Store these in variables so they are automatically replicated in GTSAMConfig.cmake and such. -set(BOOST_FIND_MINIMUM_VERSION 1.67) +set(BOOST_FIND_MINIMUM_VERSION 1.65) set(BOOST_FIND_MINIMUM_COMPONENTS serialization system filesystem thread program_options date_time timer chrono regex) find_package(Boost ${BOOST_FIND_MINIMUM_VERSION} COMPONENTS ${BOOST_FIND_MINIMUM_COMPONENTS}) @@ -30,7 +30,7 @@ find_package(Boost ${BOOST_FIND_MINIMUM_VERSION} COMPONENTS ${BOOST_FIND_MINIMUM # Required components if(NOT Boost_SERIALIZATION_LIBRARY OR NOT Boost_SYSTEM_LIBRARY OR NOT Boost_FILESYSTEM_LIBRARY OR NOT Boost_THREAD_LIBRARY OR NOT Boost_DATE_TIME_LIBRARY) - message(FATAL_ERROR "Missing required Boost components >= v1.67, please install/upgrade Boost or configure your search paths.") + message(FATAL_ERROR "Missing required Boost components >= v1.65, please install/upgrade Boost or configure your search paths.") endif() option(GTSAM_DISABLE_NEW_TIMERS "Disables using Boost.chrono for timing" OFF) diff --git a/cmake/HandleCPack.cmake b/cmake/HandleCPack.cmake index eff36c42d..0f8d1680c 100644 --- a/cmake/HandleCPack.cmake +++ b/cmake/HandleCPack.cmake @@ -25,4 +25,4 @@ set(CPACK_SOURCE_PACKAGE_FILE_NAME "gtsam-${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION # Deb-package specific cpack set(CPACK_DEBIAN_PACKAGE_NAME "libgtsam-dev") -set(CPACK_DEBIAN_PACKAGE_DEPENDS "libboost-dev (>= 1.67)") #Example: "libc6 (>= 2.3.1-6), libgcc1 (>= 1:3.4.2-12)") +set(CPACK_DEBIAN_PACKAGE_DEPENDS "libboost-dev (>= 1.65)") #Example: "libc6 (>= 2.3.1-6), libgcc1 (>= 1:3.4.2-12)") From 4c9ce4ca95f97c8d0fe47b56fae58f444511bbb4 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 20 Aug 2021 11:11:28 -0400 Subject: [PATCH 158/248] Link to GTSAM-EXPORT doc --- DEVELOP.md | 2 ++ Using-GTSAM-EXPORT.md | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/DEVELOP.md b/DEVELOP.md index 133f3ea11..8604afe0f 100644 --- a/DEVELOP.md +++ b/DEVELOP.md @@ -17,3 +17,5 @@ class GTSAM_EXPORT MyClass { ... }; GTSAM_EXPORT myFunction(); ``` + +More details [here](Using-GTSAM-EXPORT.md). diff --git a/Using-GTSAM-EXPORT.md b/Using-GTSAM-EXPORT.md index 41eccc178..cae1d499c 100644 --- a/Using-GTSAM-EXPORT.md +++ b/Using-GTSAM-EXPORT.md @@ -10,7 +10,7 @@ To create a DLL in windows, the `GTSAM_EXPORT` keyword has been created and need 3. If you have defined a class using `GTSAM_EXPORT`, do not use `GTSAM_EXPORT` in any of its individual function declarations. (Note that you _can_ put `GTSAM_EXPORT` in the definition of individual functions within a class as long as you don't put `GTSAM_EXPORT` in the class definition.) ## When is GTSAM_EXPORT being used incorrectly -Unfortunately, using `GTSAM_EXPORT` incorrectly often does not cause a compiler or linker error in the library that is being compiled, but only when you try to use that DLL in a different library. For example, an error in gtsam/base will often show up when compiling the check_base_program or the MATLAB wrapper, but not when compiling/linking gtsam itself. The most common errors will say something like: +Unfortunately, using `GTSAM_EXPORT` incorrectly often does not cause a compiler or linker error in the library that is being compiled, but only when you try to use that DLL in a different library. For example, an error in `gtsam/base` will often show up when compiling the `check_base_program` or the MATLAB wrapper, but not when compiling/linking gtsam itself. The most common errors will say something like: ``` Error LNK2019 unresolved external symbol "public: void __cdecl gtsam::SO3::print(class std::basic_string,class std::allocator > const &)const " (?print@SO3@gtsam@@QEBAXAEBV?$basic_string@DU?$char_traits@D@std@@V?$allocator@D@2@@std@@@Z) referenced in function "public: static void __cdecl gtsam::Testable::Print(class gtsam::SO3 const &,class std::basic_string,class std::allocator > const &)" (?Print@?$Testable@VSO3@gtsam@@@gtsam@@SAXAEBVSO3@2@AEBV?$basic_string@DU?$char_traits@D@std@@V?$allocator@D@2@@std@@@Z) check_geometry_program C:\AFIT\lib\gtsam\build\gtsam\geometry\tests\testSO3.obj From a4a58cf8030a7a0a72c0ce1d83fdddc3e0392e47 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 20 Aug 2021 13:33:57 -0400 Subject: [PATCH 159/248] only format c++ file (no code changes) --- examples/IMUKittiExampleGPS.cpp | 571 +++++++++++++++++--------------- 1 file changed, 297 insertions(+), 274 deletions(-) diff --git a/examples/IMUKittiExampleGPS.cpp b/examples/IMUKittiExampleGPS.cpp index e2ca49647..a2c82575f 100644 --- a/examples/IMUKittiExampleGPS.cpp +++ b/examples/IMUKittiExampleGPS.cpp @@ -11,21 +11,23 @@ /** * @file IMUKittiExampleGPS - * @brief Example of application of ISAM2 for GPS-aided navigation on the KITTI VISION BENCHMARK SUITE - * @author Ported by Thomas Jespersen (thomasj@tkjelectronics.dk), TKJ Electronics + * @brief Example of application of ISAM2 for GPS-aided navigation on the KITTI + * VISION BENCHMARK SUITE + * @author Ported by Thomas Jespersen (thomasj@tkjelectronics.dk), TKJ + * Electronics */ // GTSAM related includes. +#include #include #include #include -#include -#include -#include #include #include #include -#include +#include +#include +#include #include #include @@ -34,35 +36,35 @@ using namespace std; using namespace gtsam; -using symbol_shorthand::X; // Pose3 (x,y,z,r,p,y) -using symbol_shorthand::V; // Vel (xdot,ydot,zdot) using symbol_shorthand::B; // Bias (ax,ay,az,gx,gy,gz) +using symbol_shorthand::V; // Vel (xdot,ydot,zdot) +using symbol_shorthand::X; // Pose3 (x,y,z,r,p,y) struct KittiCalibration { - double body_ptx; - double body_pty; - double body_ptz; - double body_prx; - double body_pry; - double body_prz; - double accelerometer_sigma; - double gyroscope_sigma; - double integration_sigma; - double accelerometer_bias_sigma; - double gyroscope_bias_sigma; - double average_delta_t; + double body_ptx; + double body_pty; + double body_ptz; + double body_prx; + double body_pry; + double body_prz; + double accelerometer_sigma; + double gyroscope_sigma; + double integration_sigma; + double accelerometer_bias_sigma; + double gyroscope_bias_sigma; + double average_delta_t; }; struct ImuMeasurement { - double time; - double dt; - Vector3 accelerometer; - Vector3 gyroscope; // omega + double time; + double dt; + Vector3 accelerometer; + Vector3 gyroscope; // omega }; struct GpsMeasurement { - double time; - Vector3 position; // x,y,z + double time; + Vector3 position; // x,y,z }; const string output_filename = "IMUKittiExampleGPSResults.csv"; @@ -70,290 +72,311 @@ const string output_filename = "IMUKittiExampleGPSResults.csv"; void loadKittiData(KittiCalibration& kitti_calibration, vector& imu_measurements, vector& gps_measurements) { - string line; + string line; - // Read IMU metadata and compute relative sensor pose transforms - // BodyPtx BodyPty BodyPtz BodyPrx BodyPry BodyPrz AccelerometerSigma GyroscopeSigma IntegrationSigma - // AccelerometerBiasSigma GyroscopeBiasSigma AverageDeltaT - string imu_metadata_file = findExampleDataFile("KittiEquivBiasedImu_metadata.txt"); - ifstream imu_metadata(imu_metadata_file.c_str()); + // Read IMU metadata and compute relative sensor pose transforms + // BodyPtx BodyPty BodyPtz BodyPrx BodyPry BodyPrz AccelerometerSigma + // GyroscopeSigma IntegrationSigma AccelerometerBiasSigma GyroscopeBiasSigma + // AverageDeltaT + string imu_metadata_file = + findExampleDataFile("KittiEquivBiasedImu_metadata.txt"); + ifstream imu_metadata(imu_metadata_file.c_str()); - printf("-- Reading sensor metadata\n"); + printf("-- Reading sensor metadata\n"); - getline(imu_metadata, line, '\n'); // ignore the first line + getline(imu_metadata, line, '\n'); // ignore the first line - // Load Kitti calibration - getline(imu_metadata, line, '\n'); - sscanf(line.c_str(), "%lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf", - &kitti_calibration.body_ptx, - &kitti_calibration.body_pty, - &kitti_calibration.body_ptz, - &kitti_calibration.body_prx, - &kitti_calibration.body_pry, - &kitti_calibration.body_prz, - &kitti_calibration.accelerometer_sigma, - &kitti_calibration.gyroscope_sigma, - &kitti_calibration.integration_sigma, - &kitti_calibration.accelerometer_bias_sigma, - &kitti_calibration.gyroscope_bias_sigma, - &kitti_calibration.average_delta_t); - printf("IMU metadata: %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf\n", - kitti_calibration.body_ptx, - kitti_calibration.body_pty, - kitti_calibration.body_ptz, - kitti_calibration.body_prx, - kitti_calibration.body_pry, - kitti_calibration.body_prz, - kitti_calibration.accelerometer_sigma, - kitti_calibration.gyroscope_sigma, - kitti_calibration.integration_sigma, - kitti_calibration.accelerometer_bias_sigma, - kitti_calibration.gyroscope_bias_sigma, - kitti_calibration.average_delta_t); + // Load Kitti calibration + getline(imu_metadata, line, '\n'); + sscanf(line.c_str(), "%lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf", + &kitti_calibration.body_ptx, &kitti_calibration.body_pty, + &kitti_calibration.body_ptz, &kitti_calibration.body_prx, + &kitti_calibration.body_pry, &kitti_calibration.body_prz, + &kitti_calibration.accelerometer_sigma, + &kitti_calibration.gyroscope_sigma, + &kitti_calibration.integration_sigma, + &kitti_calibration.accelerometer_bias_sigma, + &kitti_calibration.gyroscope_bias_sigma, + &kitti_calibration.average_delta_t); + printf("IMU metadata: %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf\n", + kitti_calibration.body_ptx, kitti_calibration.body_pty, + kitti_calibration.body_ptz, kitti_calibration.body_prx, + kitti_calibration.body_pry, kitti_calibration.body_prz, + kitti_calibration.accelerometer_sigma, + kitti_calibration.gyroscope_sigma, kitti_calibration.integration_sigma, + kitti_calibration.accelerometer_bias_sigma, + kitti_calibration.gyroscope_bias_sigma, + kitti_calibration.average_delta_t); - // Read IMU data - // Time dt accelX accelY accelZ omegaX omegaY omegaZ - string imu_data_file = findExampleDataFile("KittiEquivBiasedImu.txt"); - printf("-- Reading IMU measurements from file\n"); - { - ifstream imu_data(imu_data_file.c_str()); - getline(imu_data, line, '\n'); // ignore the first line + // Read IMU data + // Time dt accelX accelY accelZ omegaX omegaY omegaZ + string imu_data_file = findExampleDataFile("KittiEquivBiasedImu.txt"); + printf("-- Reading IMU measurements from file\n"); + { + ifstream imu_data(imu_data_file.c_str()); + getline(imu_data, line, '\n'); // ignore the first line - double time = 0, dt = 0, acc_x = 0, acc_y = 0, acc_z = 0, gyro_x = 0, gyro_y = 0, gyro_z = 0; - while (!imu_data.eof()) { - getline(imu_data, line, '\n'); - sscanf(line.c_str(), "%lf %lf %lf %lf %lf %lf %lf %lf", - &time, &dt, - &acc_x, &acc_y, &acc_z, - &gyro_x, &gyro_y, &gyro_z); + double time = 0, dt = 0, acc_x = 0, acc_y = 0, acc_z = 0, gyro_x = 0, + gyro_y = 0, gyro_z = 0; + while (!imu_data.eof()) { + getline(imu_data, line, '\n'); + sscanf(line.c_str(), "%lf %lf %lf %lf %lf %lf %lf %lf", &time, &dt, + &acc_x, &acc_y, &acc_z, &gyro_x, &gyro_y, &gyro_z); - ImuMeasurement measurement; - measurement.time = time; - measurement.dt = dt; - measurement.accelerometer = Vector3(acc_x, acc_y, acc_z); - measurement.gyroscope = Vector3(gyro_x, gyro_y, gyro_z); - imu_measurements.push_back(measurement); - } + ImuMeasurement measurement; + measurement.time = time; + measurement.dt = dt; + measurement.accelerometer = Vector3(acc_x, acc_y, acc_z); + measurement.gyroscope = Vector3(gyro_x, gyro_y, gyro_z); + imu_measurements.push_back(measurement); } + } - // Read GPS data - // Time,X,Y,Z - string gps_data_file = findExampleDataFile("KittiGps_converted.txt"); - printf("-- Reading GPS measurements from file\n"); - { - ifstream gps_data(gps_data_file.c_str()); - getline(gps_data, line, '\n'); // ignore the first line + // Read GPS data + // Time,X,Y,Z + string gps_data_file = findExampleDataFile("KittiGps_converted.txt"); + printf("-- Reading GPS measurements from file\n"); + { + ifstream gps_data(gps_data_file.c_str()); + getline(gps_data, line, '\n'); // ignore the first line - double time = 0, gps_x = 0, gps_y = 0, gps_z = 0; - while (!gps_data.eof()) { - getline(gps_data, line, '\n'); - sscanf(line.c_str(), "%lf,%lf,%lf,%lf", &time, &gps_x, &gps_y, &gps_z); + double time = 0, gps_x = 0, gps_y = 0, gps_z = 0; + while (!gps_data.eof()) { + getline(gps_data, line, '\n'); + sscanf(line.c_str(), "%lf,%lf,%lf,%lf", &time, &gps_x, &gps_y, &gps_z); - GpsMeasurement measurement; - measurement.time = time; - measurement.position = Vector3(gps_x, gps_y, gps_z); - gps_measurements.push_back(measurement); - } + GpsMeasurement measurement; + measurement.time = time; + measurement.position = Vector3(gps_x, gps_y, gps_z); + gps_measurements.push_back(measurement); } + } } int main(int argc, char* argv[]) { - KittiCalibration kitti_calibration; - vector imu_measurements; - vector gps_measurements; - loadKittiData(kitti_calibration, imu_measurements, gps_measurements); + KittiCalibration kitti_calibration; + vector imu_measurements; + vector gps_measurements; + loadKittiData(kitti_calibration, imu_measurements, gps_measurements); - Vector6 BodyP = (Vector6() << kitti_calibration.body_ptx, kitti_calibration.body_pty, kitti_calibration.body_ptz, - kitti_calibration.body_prx, kitti_calibration.body_pry, kitti_calibration.body_prz) - .finished(); - auto body_T_imu = Pose3::Expmap(BodyP); - if (!body_T_imu.equals(Pose3(), 1e-5)) { - printf("Currently only support IMUinBody is identity, i.e. IMU and body frame are the same"); - exit(-1); - } + Vector6 BodyP = + (Vector6() << kitti_calibration.body_ptx, kitti_calibration.body_pty, + kitti_calibration.body_ptz, kitti_calibration.body_prx, + kitti_calibration.body_pry, kitti_calibration.body_prz) + .finished(); + auto body_T_imu = Pose3::Expmap(BodyP); + if (!body_T_imu.equals(Pose3(), 1e-5)) { + printf( + "Currently only support IMUinBody is identity, i.e. IMU and body frame " + "are the same"); + exit(-1); + } - // Configure different variables - // double t_offset = gps_measurements[0].time; - size_t first_gps_pose = 1; - size_t gps_skip = 10; // Skip this many GPS measurements each time - double g = 9.8; - auto w_coriolis = Vector3::Zero(); // zero vector + // Configure different variables + // double t_offset = gps_measurements[0].time; + size_t first_gps_pose = 1; + size_t gps_skip = 10; // Skip this many GPS measurements each time + double g = 9.8; + auto w_coriolis = Vector3::Zero(); // zero vector - // Configure noise models - auto noise_model_gps = noiseModel::Diagonal::Precisions((Vector6() << Vector3::Constant(0), - Vector3::Constant(1.0/0.07)) - .finished()); + // Configure noise models + auto noise_model_gps = noiseModel::Diagonal::Precisions( + (Vector6() << Vector3::Constant(0), Vector3::Constant(1.0 / 0.07)) + .finished()); - // Set initial conditions for the estimated trajectory - // initial pose is the reference frame (navigation frame) - auto current_pose_global = Pose3(Rot3(), gps_measurements[first_gps_pose].position); - // the vehicle is stationary at the beginning at position 0,0,0 - Vector3 current_velocity_global = Vector3::Zero(); - auto current_bias = imuBias::ConstantBias(); // init with zero bias + // Set initial conditions for the estimated trajectory + // initial pose is the reference frame (navigation frame) + auto current_pose_global = + Pose3(Rot3(), gps_measurements[first_gps_pose].position); + // the vehicle is stationary at the beginning at position 0,0,0 + Vector3 current_velocity_global = Vector3::Zero(); + auto current_bias = imuBias::ConstantBias(); // init with zero bias - auto sigma_init_x = noiseModel::Diagonal::Precisions((Vector6() << Vector3::Constant(0), - Vector3::Constant(1.0)) - .finished()); - auto sigma_init_v = noiseModel::Diagonal::Sigmas(Vector3::Constant(1000.0)); - auto sigma_init_b = noiseModel::Diagonal::Sigmas((Vector6() << Vector3::Constant(0.100), - Vector3::Constant(5.00e-05)) - .finished()); + auto sigma_init_x = noiseModel::Diagonal::Precisions( + (Vector6() << Vector3::Constant(0), Vector3::Constant(1.0)).finished()); + auto sigma_init_v = noiseModel::Diagonal::Sigmas(Vector3::Constant(1000.0)); + auto sigma_init_b = noiseModel::Diagonal::Sigmas( + (Vector6() << Vector3::Constant(0.100), Vector3::Constant(5.00e-05)) + .finished()); - // Set IMU preintegration parameters - Matrix33 measured_acc_cov = I_3x3 * pow(kitti_calibration.accelerometer_sigma, 2); - Matrix33 measured_omega_cov = I_3x3 * pow(kitti_calibration.gyroscope_sigma, 2); - // error committed in integrating position from velocities - Matrix33 integration_error_cov = I_3x3 * pow(kitti_calibration.integration_sigma, 2); + // Set IMU preintegration parameters + Matrix33 measured_acc_cov = + I_3x3 * pow(kitti_calibration.accelerometer_sigma, 2); + Matrix33 measured_omega_cov = + I_3x3 * pow(kitti_calibration.gyroscope_sigma, 2); + // error committed in integrating position from velocities + Matrix33 integration_error_cov = + I_3x3 * pow(kitti_calibration.integration_sigma, 2); - auto imu_params = PreintegratedImuMeasurements::Params::MakeSharedU(g); - imu_params->accelerometerCovariance = measured_acc_cov; // acc white noise in continuous - imu_params->integrationCovariance = integration_error_cov; // integration uncertainty continuous - imu_params->gyroscopeCovariance = measured_omega_cov; // gyro white noise in continuous - imu_params->omegaCoriolis = w_coriolis; + auto imu_params = PreintegratedImuMeasurements::Params::MakeSharedU(g); + imu_params->accelerometerCovariance = + measured_acc_cov; // acc white noise in continuous + imu_params->integrationCovariance = + integration_error_cov; // integration uncertainty continuous + imu_params->gyroscopeCovariance = + measured_omega_cov; // gyro white noise in continuous + imu_params->omegaCoriolis = w_coriolis; - std::shared_ptr current_summarized_measurement = nullptr; + std::shared_ptr current_summarized_measurement = + nullptr; - // Set ISAM2 parameters and create ISAM2 solver object - ISAM2Params isam_params; - isam_params.factorization = ISAM2Params::CHOLESKY; - isam_params.relinearizeSkip = 10; + // Set ISAM2 parameters and create ISAM2 solver object + ISAM2Params isam_params; + isam_params.factorization = ISAM2Params::CHOLESKY; + isam_params.relinearizeSkip = 10; - ISAM2 isam(isam_params); + ISAM2 isam(isam_params); - // Create the factor graph and values object that will store new factors and values to add to the incremental graph - NonlinearFactorGraph new_factors; - Values new_values; // values storing the initial estimates of new nodes in the factor graph + // Create the factor graph and values object that will store new factors and + // values to add to the incremental graph + NonlinearFactorGraph new_factors; + Values new_values; // values storing the initial estimates of new nodes in + // the factor graph - /// Main loop: - /// (1) we read the measurements - /// (2) we create the corresponding factors in the graph - /// (3) we solve the graph to obtain and optimal estimate of robot trajectory - printf("-- Starting main loop: inference is performed at each time step, but we plot trajectory every 10 steps\n"); - size_t j = 0; - for (size_t i = first_gps_pose; i < gps_measurements.size() - 1; i++) { - // At each non=IMU measurement we initialize a new node in the graph - auto current_pose_key = X(i); - auto current_vel_key = V(i); - auto current_bias_key = B(i); - double t = gps_measurements[i].time; + /// Main loop: + /// (1) we read the measurements + /// (2) we create the corresponding factors in the graph + /// (3) we solve the graph to obtain and optimal estimate of robot trajectory + printf( + "-- Starting main loop: inference is performed at each time step, but we " + "plot trajectory every 10 steps\n"); + size_t j = 0; + for (size_t i = first_gps_pose; i < gps_measurements.size() - 1; i++) { + // At each non=IMU measurement we initialize a new node in the graph + auto current_pose_key = X(i); + auto current_vel_key = V(i); + auto current_bias_key = B(i); + double t = gps_measurements[i].time; - if (i == first_gps_pose) { - // Create initial estimate and prior on initial pose, velocity, and biases - new_values.insert(current_pose_key, current_pose_global); - new_values.insert(current_vel_key, current_velocity_global); - new_values.insert(current_bias_key, current_bias); - new_factors.emplace_shared>(current_pose_key, current_pose_global, sigma_init_x); - new_factors.emplace_shared>(current_vel_key, current_velocity_global, sigma_init_v); - new_factors.emplace_shared>(current_bias_key, current_bias, sigma_init_b); - } else { - double t_previous = gps_measurements[i-1].time; + if (i == first_gps_pose) { + // Create initial estimate and prior on initial pose, velocity, and biases + new_values.insert(current_pose_key, current_pose_global); + new_values.insert(current_vel_key, current_velocity_global); + new_values.insert(current_bias_key, current_bias); + new_factors.emplace_shared>( + current_pose_key, current_pose_global, sigma_init_x); + new_factors.emplace_shared>( + current_vel_key, current_velocity_global, sigma_init_v); + new_factors.emplace_shared>( + current_bias_key, current_bias, sigma_init_b); + } else { + double t_previous = gps_measurements[i - 1].time; - // Summarize IMU data between the previous GPS measurement and now - current_summarized_measurement = std::make_shared(imu_params, current_bias); - static size_t included_imu_measurement_count = 0; - while (j < imu_measurements.size() && imu_measurements[j].time <= t) { - if (imu_measurements[j].time >= t_previous) { - current_summarized_measurement->integrateMeasurement(imu_measurements[j].accelerometer, - imu_measurements[j].gyroscope, - imu_measurements[j].dt); - included_imu_measurement_count++; - } - j++; - } - - // Create IMU factor - auto previous_pose_key = X(i-1); - auto previous_vel_key = V(i-1); - auto previous_bias_key = B(i-1); - - new_factors.emplace_shared(previous_pose_key, previous_vel_key, - current_pose_key, current_vel_key, - previous_bias_key, *current_summarized_measurement); - - // Bias evolution as given in the IMU metadata - auto sigma_between_b = noiseModel::Diagonal::Sigmas((Vector6() << - Vector3::Constant(sqrt(included_imu_measurement_count) * kitti_calibration.accelerometer_bias_sigma), - Vector3::Constant(sqrt(included_imu_measurement_count) * kitti_calibration.gyroscope_bias_sigma)) - .finished()); - new_factors.emplace_shared>(previous_bias_key, - current_bias_key, - imuBias::ConstantBias(), - sigma_between_b); - - // Create GPS factor - auto gps_pose = Pose3(current_pose_global.rotation(), gps_measurements[i].position); - if ((i % gps_skip) == 0) { - new_factors.emplace_shared>(current_pose_key, gps_pose, noise_model_gps); - new_values.insert(current_pose_key, gps_pose); - - printf("################ POSE INCLUDED AT TIME %lf ################\n", t); - cout << gps_pose.translation(); - printf("\n\n"); - } else { - new_values.insert(current_pose_key, current_pose_global); - } - - // Add initial values for velocity and bias based on the previous estimates - new_values.insert(current_vel_key, current_velocity_global); - new_values.insert(current_bias_key, current_bias); - - // Update solver - // ======================================================================= - // We accumulate 2*GPSskip GPS measurements before updating the solver at - // first so that the heading becomes observable. - if (i > (first_gps_pose + 2*gps_skip)) { - printf("################ NEW FACTORS AT TIME %lf ################\n", t); - new_factors.print(); - - isam.update(new_factors, new_values); - - // Reset the newFactors and newValues list - new_factors.resize(0); - new_values.clear(); - - // Extract the result/current estimates - Values result = isam.calculateEstimate(); - - current_pose_global = result.at(current_pose_key); - current_velocity_global = result.at(current_vel_key); - current_bias = result.at(current_bias_key); - - printf("\n################ POSE AT TIME %lf ################\n", t); - current_pose_global.print(); - printf("\n\n"); - } + // Summarize IMU data between the previous GPS measurement and now + current_summarized_measurement = + std::make_shared(imu_params, + current_bias); + static size_t included_imu_measurement_count = 0; + while (j < imu_measurements.size() && imu_measurements[j].time <= t) { + if (imu_measurements[j].time >= t_previous) { + current_summarized_measurement->integrateMeasurement( + imu_measurements[j].accelerometer, imu_measurements[j].gyroscope, + imu_measurements[j].dt); + included_imu_measurement_count++; } + j++; + } + + // Create IMU factor + auto previous_pose_key = X(i - 1); + auto previous_vel_key = V(i - 1); + auto previous_bias_key = B(i - 1); + + new_factors.emplace_shared( + previous_pose_key, previous_vel_key, current_pose_key, + current_vel_key, previous_bias_key, *current_summarized_measurement); + + // Bias evolution as given in the IMU metadata + auto sigma_between_b = noiseModel::Diagonal::Sigmas( + (Vector6() << Vector3::Constant( + sqrt(included_imu_measurement_count) * + kitti_calibration.accelerometer_bias_sigma), + Vector3::Constant(sqrt(included_imu_measurement_count) * + kitti_calibration.gyroscope_bias_sigma)) + .finished()); + new_factors.emplace_shared>( + previous_bias_key, current_bias_key, imuBias::ConstantBias(), + sigma_between_b); + + // Create GPS factor + auto gps_pose = + Pose3(current_pose_global.rotation(), gps_measurements[i].position); + if ((i % gps_skip) == 0) { + new_factors.emplace_shared>( + current_pose_key, gps_pose, noise_model_gps); + new_values.insert(current_pose_key, gps_pose); + + printf("################ POSE INCLUDED AT TIME %lf ################\n", + t); + cout << gps_pose.translation(); + printf("\n\n"); + } else { + new_values.insert(current_pose_key, current_pose_global); + } + + // Add initial values for velocity and bias based on the previous + // estimates + new_values.insert(current_vel_key, current_velocity_global); + new_values.insert(current_bias_key, current_bias); + + // Update solver + // ======================================================================= + // We accumulate 2*GPSskip GPS measurements before updating the solver at + // first so that the heading becomes observable. + if (i > (first_gps_pose + 2 * gps_skip)) { + printf("################ NEW FACTORS AT TIME %lf ################\n", + t); + new_factors.print(); + + isam.update(new_factors, new_values); + + // Reset the newFactors and newValues list + new_factors.resize(0); + new_values.clear(); + + // Extract the result/current estimates + Values result = isam.calculateEstimate(); + + current_pose_global = result.at(current_pose_key); + current_velocity_global = result.at(current_vel_key); + current_bias = result.at(current_bias_key); + + printf("\n################ POSE AT TIME %lf ################\n", t); + current_pose_global.print(); + printf("\n\n"); + } } + } - // Save results to file - printf("\nWriting results to file...\n"); - FILE* fp_out = fopen(output_filename.c_str(), "w+"); - fprintf(fp_out, "#time(s),x(m),y(m),z(m),qx,qy,qz,qw,gt_x(m),gt_y(m),gt_z(m)\n"); + // Save results to file + printf("\nWriting results to file...\n"); + FILE* fp_out = fopen(output_filename.c_str(), "w+"); + fprintf(fp_out, + "#time(s),x(m),y(m),z(m),qx,qy,qz,qw,gt_x(m),gt_y(m),gt_z(m)\n"); - Values result = isam.calculateEstimate(); - for (size_t i = first_gps_pose; i < gps_measurements.size() - 1; i++) { - auto pose_key = X(i); - auto vel_key = V(i); - auto bias_key = B(i); + Values result = isam.calculateEstimate(); + for (size_t i = first_gps_pose; i < gps_measurements.size() - 1; i++) { + auto pose_key = X(i); + auto vel_key = V(i); + auto bias_key = B(i); - auto pose = result.at(pose_key); - auto velocity = result.at(vel_key); - auto bias = result.at(bias_key); + auto pose = result.at(pose_key); + auto velocity = result.at(vel_key); + auto bias = result.at(bias_key); - auto pose_quat = pose.rotation().toQuaternion(); - auto gps = gps_measurements[i].position; + auto pose_quat = pose.rotation().toQuaternion(); + auto gps = gps_measurements[i].position; - cout << "State at #" << i << endl; - cout << "Pose:" << endl << pose << endl; - cout << "Velocity:" << endl << velocity << endl; - cout << "Bias:" << endl << bias << endl; + cout << "State at #" << i << endl; + cout << "Pose:" << endl << pose << endl; + cout << "Velocity:" << endl << velocity << endl; + cout << "Bias:" << endl << bias << endl; - fprintf(fp_out, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n", - gps_measurements[i].time, - pose.x(), pose.y(), pose.z(), - pose_quat.x(), pose_quat.y(), pose_quat.z(), pose_quat.w(), - gps(0), gps(1), gps(2)); - } + fprintf(fp_out, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n", + gps_measurements[i].time, pose.x(), pose.y(), pose.z(), + pose_quat.x(), pose_quat.y(), pose_quat.z(), pose_quat.w(), gps(0), + gps(1), gps(2)); + } - fclose(fp_out); + fclose(fp_out); } From 23858f31e9298d4dd9bd6a6f9304c9434e691821 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 20 Aug 2021 13:34:26 -0400 Subject: [PATCH 160/248] working implementation --- python/gtsam/examples/IMUKittiExampleGPS.py | 195 +++++++++++++++----- 1 file changed, 152 insertions(+), 43 deletions(-) diff --git a/python/gtsam/examples/IMUKittiExampleGPS.py b/python/gtsam/examples/IMUKittiExampleGPS.py index 33e032614..a23f98186 100644 --- a/python/gtsam/examples/IMUKittiExampleGPS.py +++ b/python/gtsam/examples/IMUKittiExampleGPS.py @@ -1,67 +1,173 @@ """ Example of application of ISAM2 for GPS-aided navigation on the KITTI VISION BENCHMARK SUITE + +Author: Varun Agrawal """ import argparse -from typing import List + +import numpy as np import gtsam -import numpy as np -from gtsam import Pose3, Rot3, noiseModel +from gtsam import Pose3, noiseModel from gtsam.symbol_shorthand import B, V, X class KittiCalibration: - def __init__(self, bodyTimu: gtsam.Pose3): - self.bodyTimu = bodyTimu + """Class to hold KITTI calibration info.""" + def __init__(self, body_ptx: float, body_pty: float, body_ptz: float, + body_prx: float, body_pry: float, body_prz: float, + accelerometer_sigma: float, gyroscope_sigma: float, + integration_sigma: float, accelerometer_bias_sigma: float, + gyroscope_bias_sigma: float, average_delta_t: float): + self.bodyTimu = Pose3(gtsam.Rot3.RzRyRx(body_prx, body_pry, body_prz), + gtsam.Point3(body_ptx, body_pty, body_ptz)) + self.accelerometer_sigma = accelerometer_sigma + self.gyroscope_sigma = gyroscope_sigma + self.integration_sigma = integration_sigma + self.accelerometer_bias_sigma = accelerometer_bias_sigma + self.gyroscope_bias_sigma = gyroscope_bias_sigma + self.average_delta_t = average_delta_t class ImuMeasurement: + """An instance of an IMU measurement.""" def __init__(self, time, dt, accelerometer, gyroscope): - pass + self.time = time + self.dt = dt + self.accelerometer = accelerometer + self.gyroscope = gyroscope class GpsMeasurement: + """An instance of a GPS measurement.""" def __init__(self, time, position: gtsam.Point3): self.time = time self.position = position -def lodKittiData(): - pass +def loadKittiData(imu_data_file="KittiEquivBiasedImu.txt", + gps_data_file="KittiGps_converted.txt", + imu_metadata_file="KittiEquivBiasedImu_metadata.txt"): + """ + Load the KITTI Dataset. + """ + # Read IMU metadata and compute relative sensor pose transforms + # BodyPtx BodyPty BodyPtz BodyPrx BodyPry BodyPrz AccelerometerSigma + # GyroscopeSigma IntegrationSigma AccelerometerBiasSigma GyroscopeBiasSigma + # AverageDeltaT + imu_metadata_file = gtsam.findExampleDataFile(imu_metadata_file) + with open(imu_metadata_file) as imu_metadata: + print("-- Reading sensor metadata") + line = imu_metadata.readline() # Ignore the first line + line = imu_metadata.readline().strip() + data = list(map(float, line.split(' '))) + kitti_calibration = KittiCalibration(*data) + print("IMU metadata:", data) + # Read IMU data + # Time dt accelX accelY accelZ omegaX omegaY omegaZ + imu_data_file = gtsam.findExampleDataFile(imu_data_file) + imu_measurements = [] -def parse_args(): - parser = argparse.ArgumentParser() - return parser.parse_args() + print("-- Reading IMU measurements from file") + with open(imu_data_file) as imu_data: + data = imu_data.readlines() + for i in range(1, len(data)): # ignore the first line + time, dt, acc_x, acc_y, acc_z, gyro_x, gyro_y, gyro_z = map( + float, data[i].split(' ')) + imu_measurement = ImuMeasurement( + time, dt, np.asarray([acc_x, acc_y, acc_z]), + np.asarray([gyro_x, gyro_y, gyro_z])) + imu_measurements.append(imu_measurement) + + # Read GPS data + # Time,X,Y,Z + gps_data_file = gtsam.findExampleDataFile(gps_data_file) + gps_measurements = [] + + print("-- Reading GPS measurements from file") + with open(gps_data_file) as gps_data: + data = gps_data.readlines() + for i in range(1, len(data)): + time, x, y, z = map(float, data[i].split(',')) + gps_measurement = GpsMeasurement(time, np.asarray([x, y, z])) + gps_measurements.append(gps_measurement) + + return kitti_calibration, imu_measurements, gps_measurements def getImuParams(kitti_calibration): + """Get the IMU parameters from the KITTI calibration data.""" GRAVITY = 9.8 w_coriolis = np.zeros(3) # Set IMU preintegration parameters measured_acc_cov = np.eye(3) * np.power( kitti_calibration.accelerometer_sigma, 2) - measured_omega_cov = np.eye(3) * np.powwe( + measured_omega_cov = np.eye(3) * np.power( kitti_calibration.gyroscope_sigma, 2) # error committed in integrating position from velocities integration_error_cov = np.eye(3) * np.power( kitti_calibration.integration_sigma, 2) imu_params = gtsam.PreintegrationParams.MakeSharedU(GRAVITY) - imu_params.accelerometerCovariance = measured_acc_cov # acc white noise in continuous - imu_params.integrationCovariance = integration_error_cov # integration uncertainty continuous - imu_params.gyroscopeCovariance = measured_omega_cov # gyro white noise in continuous - imu_params.omegaCoriolis = w_coriolis + # acc white noise in continuous + imu_params.setAccelerometerCovariance(measured_acc_cov) + # integration uncertainty continuous + imu_params.setIntegrationCovariance(integration_error_cov) + # gyro white noise in continuous + imu_params.setGyroscopeCovariance(measured_omega_cov) + imu_params.setOmegaCoriolis(w_coriolis) return imu_params -def main(): - args = parse_args() - kitti_calibration, imu_measurements, gps_measurements = lodKittiData() +def save_results(isam, output_filename, first_gps_pose, gps_measurements): + """Write the results from `isam` to `output_filename`.""" + # Save results to file + print("Writing results to file...") + with open(output_filename, 'w') as fp_out: + fp_out.write( + "#time(s),x(m),y(m),z(m),qx,qy,qz,qw,gt_x(m),gt_y(m),gt_z(m)\n") - if kitti_calibration.bodyTimu != gtsam.Pose3: + result = isam.calculateEstimate() + for i in range(first_gps_pose, len(gps_measurements)): + pose_key = X(i) + vel_key = V(i) + bias_key = B(i) + + pose = result.atPose3(pose_key) + velocity = result.atVector(vel_key) + bias = result.atConstantBias(bias_key) + + pose_quat = pose.rotation().toQuaternion() + gps = gps_measurements[i].position + + print("State at #{}".format(i)) + print("Pose:\n", pose) + print("Velocity:\n", velocity) + print("Bias:\n", bias) + + fp_out.write("{},{},{},{},{},{},{},{},{},{},{}\n".format( + gps_measurements[i].time, pose.x(), pose.y(), pose.z(), + pose_quat.x(), pose_quat.y(), pose_quat.z(), pose_quat.w(), + gps[0], gps[1], gps[2])) + + +def parse_args(): + """Parse command line arguments.""" + parser = argparse.ArgumentParser() + parser.add_argument("--output_filename", + default="IMUKittiExampleGPSResults.csv") + return parser.parse_args() + + +def main(): + """Main runner.""" + args = parse_args() + kitti_calibration, imu_measurements, gps_measurements = loadKittiData() + + if not kitti_calibration.bodyTimu.equals(Pose3(), 1e-8): raise ValueError( "Currently only support IMUinBody is identity, i.e. IMU and body frame are the same" ) @@ -72,12 +178,13 @@ def main(): # Configure noise models noise_model_gps = noiseModel.Diagonal.Precisions( - np.asarray([0, 0, 0, 1.0 / 0.07, 1.0 / 0.07, 1.0 / 0.07])) + np.asarray([0, 0, 0] + [1.0 / 0.07] * 3)) # Set initial conditions for the estimated trajectory # initial pose is the reference frame (navigation frame) - current_pose_global = Pose3(Rot3(), + current_pose_global = Pose3(gtsam.Rot3(), gps_measurements[first_gps_pose].position) + # the vehicle is stationary at the beginning at position 0,0,0 current_velocity_global = np.zeros(3) current_bias = gtsam.imuBias.ConstantBias() # init with zero bias @@ -86,9 +193,9 @@ def main(): np.asarray([0, 0, 0, 1, 1, 1])) sigma_init_v = noiseModel.Diagonal.Sigmas(np.ones(3) * 1000.0) sigma_init_b = noiseModel.Diagonal.Sigmas( - np.asarray([0.1, 0.1, 0.1, 5.00e-05, 5.00e-05, 5.00e-05])) + np.asarray([0.1] * 3 + [5.00e-05] * 3)) - imu_params = getImuParams() + imu_params = getImuParams(kitti_calibration) # Set ISAM2 parameters and create ISAM2 solver object isam_params = gtsam.ISAM2Params() @@ -100,18 +207,17 @@ def main(): # Create the factor graph and values object that will store new factors and # values to add to the incremental graph new_factors = gtsam.NonlinearFactorGraph() - new_values = gtsam.Values( - ) # values storing the initial estimates of new nodes in the factor graph + # values storing the initial estimates of new nodes in the factor graph + new_values = gtsam.Values() # Main loop: # (1) we read the measurements # (2) we create the corresponding factors in the graph # (3) we solve the graph to obtain and optimal estimate of robot trajectory - print( - "-- Starting main loop: inference is performed at each time step, but we plot trajectory every 10 steps\n" - ) + print("-- Starting main loop: inference is performed at each time step, " + "but we plot trajectory every 10 steps") j = 0 - for i in range(first_gps_pose, len(gps_measurements) - 1): + for i in range(first_gps_pose, len(gps_measurements)): # At each non=IMU measurement we initialize a new node in the graph current_pose_key = X(i) current_vel_key = V(i) @@ -138,7 +244,7 @@ def main(): imu_params, current_bias) included_imu_measurement_count = 0 - while (j < imu_measurements.size() + while (j < len(imu_measurements) and imu_measurements[j].time <= t): if imu_measurements[j].time >= t_previous: current_summarized_measurement.integrateMeasurement( @@ -153,13 +259,13 @@ def main(): previous_bias_key = B(i - 1) new_factors.push_back( - gtsam.ImuFactor > (previous_pose_key, previous_vel_key, - current_pose_key, current_vel_key, - previous_bias_key, - current_summarized_measurement)) + gtsam.ImuFactor(previous_pose_key, previous_vel_key, + current_pose_key, current_vel_key, + previous_bias_key, + current_summarized_measurement)) # Bias evolution as given in the IMU metadata - sigma_between_b = noiseModel.Diagonal.Sigmas( + sigma_between_b = gtsam.noiseModel.Diagonal.Sigmas( np.asarray([ np.sqrt(included_imu_measurement_count) * kitti_calibration.accelerometer_bias_sigma @@ -183,8 +289,8 @@ def main(): new_values.insert(current_pose_key, gps_pose) print( - "################ POSE INCLUDED AT TIME %lf ################\n", - t) + "################ POSE INCLUDED AT TIME {} ################" + .format(t)) print(gps_pose.translation(), "\n") else: new_values.insert(current_pose_key, current_pose_global) @@ -200,8 +306,8 @@ def main(): # first so that the heading becomes observable. if i > (first_gps_pose + 2 * gps_skip): print( - "################ NEW FACTORS AT TIME %lf ################\n", - t) + "################ NEW FACTORS AT TIME {:.6f} ################" + .format(t)) new_factors.print() isam.update(new_factors, new_values) @@ -217,10 +323,13 @@ def main(): current_velocity_global = result.atVector(current_vel_key) current_bias = result.atConstantBias(current_bias_key) - print("\n################ POSE AT TIME %lf ################\n", - t) + print( + "################ POSE AT TIME {} ################".format( + t)) current_pose_global.print() - print("\n\n") + print("\n") + + save_results(isam, args.output_filename, first_gps_pose, gps_measurements) if __name__ == "__main__": From 1046bf481af08ff5690f17591b10af07a87fb0b9 Mon Sep 17 00:00:00 2001 From: Eric Date: Sat, 21 Aug 2021 17:04:33 -0400 Subject: [PATCH 161/248] Small clarification and md cleanup --- examples/README.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/examples/README.md b/examples/README.md index 9d58b5200..5a72736e0 100644 --- a/examples/README.md +++ b/examples/README.md @@ -51,13 +51,13 @@ The directory **vSLAMexample** includes 2 simple examples using GTSAM: See the separate README file there. -##Undirected Graphical Models (UGM) +## Undirected Graphical Models (UGM) The best representation for a Markov Random Field is a factor graph :-) This is illustrated with some discrete examples from the UGM MATLAB toolbox, which can be found at -##Building and Running -To build, cd into the directory and do: +## Building and Running +To build, cd into the top-level gtsam directory and do: ``` mkdir build From 068e558d34469aee0011aead6cfb399c45e8f9fc Mon Sep 17 00:00:00 2001 From: John Lambert Date: Sat, 21 Aug 2021 20:16:39 -0600 Subject: [PATCH 162/248] Expand DSF map unit tests --- python/gtsam/tests/test_dsf_map.py | 43 +++++++++++++++++++++--------- 1 file changed, 31 insertions(+), 12 deletions(-) diff --git a/python/gtsam/tests/test_dsf_map.py b/python/gtsam/tests/test_dsf_map.py index e36657178..0ab8b678f 100644 --- a/python/gtsam/tests/test_dsf_map.py +++ b/python/gtsam/tests/test_dsf_map.py @@ -6,49 +6,68 @@ All Rights Reserved See LICENSE for the license information Unit tests for Disjoint Set Forest. -Author: Frank Dellaert & Varun Agrawal +Author: Frank Dellaert & Varun Agrawal & John Lambert """ # pylint: disable=invalid-name, no-name-in-module, no-member from __future__ import print_function import unittest +from typing import Tuple import gtsam +from gtsam import IndexPair from gtsam.utils.test_case import GtsamTestCase class TestDSFMap(GtsamTestCase): """Tests for DSFMap.""" - def test_all(self): + def test_all(self) -> None: """Test everything in DFSMap.""" - def key(index_pair): + + def key(index_pair) -> Tuple[int, int]: return index_pair.i(), index_pair.j() dsf = gtsam.DSFMapIndexPair() pair1 = gtsam.IndexPair(1, 18) self.assertEqual(key(dsf.find(pair1)), key(pair1)) pair2 = gtsam.IndexPair(2, 2) - + # testing the merge feature of dsf dsf.merge(pair1, pair2) self.assertEqual(key(dsf.find(pair1)), key(dsf.find(pair2))) - def test_sets(self): - from gtsam import IndexPair + def test_sets(self) -> None: + """Ensure that unique keys are merged correctly during Union-Find. + + An IndexPair (i,k) representing a unique key might represent the + k'th detected keypoint in image i. For the data below, merging such + measurements into feature tracks across frames should create 2 distinct sets. + """ dsf = gtsam.DSFMapIndexPair() - dsf.merge(IndexPair(0, 1), IndexPair(1,2)) - dsf.merge(IndexPair(0, 1), IndexPair(3,4)) - dsf.merge(IndexPair(4,5), IndexPair(6,8)) + dsf.merge(IndexPair(0, 1), IndexPair(1, 2)) + dsf.merge(IndexPair(0, 1), IndexPair(3, 4)) + dsf.merge(IndexPair(4, 5), IndexPair(6, 8)) sets = dsf.sets() + merged_sets = set() + for i in sets: + set_keys = [] s = sets[i] for val in gtsam.IndexPairSetAsArray(s): - val.i() - val.j() + set_keys.append((val.i(), val.j())) + merged_sets.add(tuple(set_keys)) + + # fmt: off + expected_sets = { + ((0, 1), (1, 2), (3, 4)), # set 1 + ((4, 5), (6, 8)) # set 2 + } + # fmt: on + assert expected_sets == merged_sets -if __name__ == '__main__': +if __name__ == "__main__": unittest.main() From 36421243aaaaf401f108b6696276525e095cd184 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Sat, 21 Aug 2021 20:22:53 -0600 Subject: [PATCH 163/248] improve docstring --- python/gtsam/tests/test_dsf_map.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/gtsam/tests/test_dsf_map.py b/python/gtsam/tests/test_dsf_map.py index 0ab8b678f..6cae98ff5 100644 --- a/python/gtsam/tests/test_dsf_map.py +++ b/python/gtsam/tests/test_dsf_map.py @@ -39,7 +39,7 @@ class TestDSFMap(GtsamTestCase): self.assertEqual(key(dsf.find(pair1)), key(dsf.find(pair2))) def test_sets(self) -> None: - """Ensure that unique keys are merged correctly during Union-Find. + """Ensure that pairs are merged correctly during Union-Find. An IndexPair (i,k) representing a unique key might represent the k'th detected keypoint in image i. For the data below, merging such From 67403b0e96226a1a1ab24bd635e8fe5b3d04acbb Mon Sep 17 00:00:00 2001 From: John Lambert Date: Sat, 21 Aug 2021 21:08:37 -0600 Subject: [PATCH 164/248] clean up SFMdata --- python/gtsam/examples/SFMdata.py | 33 +++++++++++++++++--------------- 1 file changed, 18 insertions(+), 15 deletions(-) diff --git a/python/gtsam/examples/SFMdata.py b/python/gtsam/examples/SFMdata.py index 6ac9c5726..ad414a256 100644 --- a/python/gtsam/examples/SFMdata.py +++ b/python/gtsam/examples/SFMdata.py @@ -5,36 +5,39 @@ A structure-from-motion example with landmarks """ # pylint: disable=invalid-name, E1101 +from typing import List + import numpy as np import gtsam +from gtsam import Cal3_S2, Point3, Pose3 -def createPoints(): +def createPoints() -> List[Point3]: # Create the set of ground-truth landmarks - points = [gtsam.Point3(10.0, 10.0, 10.0), - gtsam.Point3(-10.0, 10.0, 10.0), - gtsam.Point3(-10.0, -10.0, 10.0), - gtsam.Point3(10.0, -10.0, 10.0), - gtsam.Point3(10.0, 10.0, -10.0), - gtsam.Point3(-10.0, 10.0, -10.0), - gtsam.Point3(-10.0, -10.0, -10.0), - gtsam.Point3(10.0, -10.0, -10.0)] + points = [ + Point3(10.0, 10.0, 10.0), + Point3(-10.0, 10.0, 10.0), + Point3(-10.0, -10.0, 10.0), + Point3(10.0, -10.0, 10.0), + Point3(10.0, 10.0, -10.0), + Point3(-10.0, 10.0, -10.0), + Point3(-10.0, -10.0, -10.0), + Point3(10.0, -10.0, -10.0), + ] return points -def createPoses(K): - # Create the set of ground-truth poses +def createPoses(K: Cal3_S2) -> List[Pose3]: + """Generate a set of ground-truth camera poses arranged in a circle about the origin.""" radius = 40.0 height = 10.0 - angles = np.linspace(0, 2*np.pi, 8, endpoint=False) + angles = np.linspace(0, 2 * np.pi, 8, endpoint=False) up = gtsam.Point3(0, 0, 1) target = gtsam.Point3(0, 0, 0) poses = [] for theta in angles: - position = gtsam.Point3(radius*np.cos(theta), - radius*np.sin(theta), - height) + position = gtsam.Point3(radius * np.cos(theta), radius * np.sin(theta), height) camera = gtsam.PinholeCameraCal3_S2.Lookat(position, target, up, K) poses.append(camera.pose()) return poses From d8fe2b283937965deb2fc20d85c8532ee30420c6 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 23 Aug 2021 01:44:39 -0400 Subject: [PATCH 165/248] format and modernize NonlinearEquality2 --- gtsam/nonlinear/NonlinearEquality.h | 62 +++++++++++++---------------- 1 file changed, 27 insertions(+), 35 deletions(-) diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index 243611433..47083d5d7 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -290,24 +290,19 @@ struct traits > * Simple binary equality constraint - this constraint forces two variables to * be the same. */ -template -class NonlinearEquality2: public NoiseModelFactor2 { -public: - typedef VALUE X; +template +class NonlinearEquality2 : public NoiseModelFactor2 { + protected: + using Base = NoiseModelFactor2; + using This = NonlinearEquality2; -protected: - typedef NoiseModelFactor2 Base; - typedef NonlinearEquality2 This; - - GTSAM_CONCEPT_MANIFOLD_TYPE(X) + GTSAM_CONCEPT_MANIFOLD_TYPE(T) /// Default constructor to allow for serialization - NonlinearEquality2() { - } + NonlinearEquality2() {} -public: - - typedef boost::shared_ptr > shared_ptr; + public: + typedef boost::shared_ptr> shared_ptr; /** * Constructor @@ -315,11 +310,10 @@ public: * @param key2 the key for the second unknown variable to be constrained * @param mu a parameter which really turns this into a strong prior */ - NonlinearEquality2(Key key1, Key key2, double mu = 1000.0) : - Base(noiseModel::Constrained::All(traits::dimension, std::abs(mu)), key1, key2) { - } - ~NonlinearEquality2() override { - } + NonlinearEquality2(Key key1, Key key2, double mu = 1e4) + : Base(noiseModel::Constrained::All(traits::dimension, std::abs(mu)), + key1, key2) {} + ~NonlinearEquality2() override {} /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { @@ -328,32 +322,30 @@ public: } /// g(x) with optional derivative2 - Vector evaluateError(const X& x1, const X& x2, boost::optional H1 = - boost::none, boost::optional H2 = boost::none) const override { - static const size_t p = traits::dimension; - if (H1) *H1 = -Matrix::Identity(p,p); - if (H2) *H2 = Matrix::Identity(p,p); - return traits::Local(x1,x2); + Vector evaluateError( + const T& x1, const T& x2, boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const override { + static const size_t p = traits::dimension; + if (H1) *H1 = -Matrix::Identity(p, p); + if (H2) *H2 = Matrix::Identity(p, p); + return traits::Local(x1, x2); } GTSAM_MAKE_ALIGNED_OPERATOR_NEW -private: - + private: /// Serialization function friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar - & boost::serialization::make_nvp("NoiseModelFactor2", - boost::serialization::base_object(*this)); + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp( + "NoiseModelFactor2", boost::serialization::base_object(*this)); } }; // \NonlinearEquality2 -template -struct traits > : Testable > { +template +struct traits> : Testable> { }; - }// namespace gtsam From 366ad847733168e50a1fdd5f6cac351a7f93b5b0 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 23 Aug 2021 02:13:24 -0400 Subject: [PATCH 166/248] wrap NonlinearEquality2 + python unit test --- gtsam/nonlinear/nonlinear.i | 13 ++++++++++++ python/gtsam/tests/test_Factors.py | 34 ++++++++++++++++++++++++++++++ 2 files changed, 47 insertions(+) create mode 100644 python/gtsam/tests/test_Factors.py diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index 91525c5fd..61f164b43 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -824,4 +824,17 @@ virtual class NonlinearEquality : gtsam::NoiseModelFactor { void serialize() const; }; +template , + gtsam::PinholeCamera, + gtsam::PinholeCamera, + gtsam::PinholeCamera, + gtsam::imuBias::ConstantBias}> +virtual class NonlinearEquality2 : gtsam::NoiseModelFactor { + NonlinearEquality2(Key key1, Key key2, double mu = 1e4); + gtsam::Vector evaluateError(const T& x1, const T& x2); +}; + } // namespace gtsam diff --git a/python/gtsam/tests/test_Factors.py b/python/gtsam/tests/test_Factors.py new file mode 100644 index 000000000..3ec8648a4 --- /dev/null +++ b/python/gtsam/tests/test_Factors.py @@ -0,0 +1,34 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Unit tests for various factors. + +Author: Varun Agrawal +""" +import unittest + +import numpy as np + +import gtsam +from gtsam.utils.test_case import GtsamTestCase + + +class TestNonlinearEquality2Factor(GtsamTestCase): + """ + Test various instantiations of NonlinearEquality2. + """ + def test_point3(self): + """Test for Point3 version.""" + factor = gtsam.NonlinearEquality2Point3(0, 1) + error = factor.evaluateError(gtsam.Point3(0, 0, 0), + gtsam.Point3(0, 0, 0)) + + np.testing.assert_allclose(error, np.zeros(3)) + + +if __name__ == "__main__": + unittest.main() From 5baf0ce85a01630881567fb054ff20e0daf99e3b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 23 Aug 2021 02:14:04 -0400 Subject: [PATCH 167/248] Update `make python-test` so that it works even if GTSAM is already installed --- python/CMakeLists.txt | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 7b8347da5..bdda15acb 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -165,6 +165,5 @@ add_custom_target( COMMAND ${CMAKE_COMMAND} -E env # add package to python path so no need to install "PYTHONPATH=${GTSAM_PYTHON_BUILD_DIRECTORY}/$ENV{PYTHONPATH}" - ${PYTHON_EXECUTABLE} -m unittest discover - DEPENDS ${GTSAM_PYTHON_DEPENDENCIES} - WORKING_DIRECTORY ${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam/tests) + ${PYTHON_EXECUTABLE} -m unittest discover -s "${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam/tests" + DEPENDS ${GTSAM_PYTHON_DEPENDENCIES}) From bc1f64e94331d703c70809edfe8e274644f4804e Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 23 Aug 2021 13:05:26 -0400 Subject: [PATCH 168/248] Add section about Boost version requirement --- INSTALL.md | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/INSTALL.md b/INSTALL.md index 3a0f2896a..965246304 100644 --- a/INSTALL.md +++ b/INSTALL.md @@ -13,7 +13,8 @@ $ make install ## Important Installation Notes 1. GTSAM requires the following libraries to be installed on your system: - - BOOST version 1.65 or greater (install through Linux repositories or MacPorts) + - BOOST version 1.65 or greater (install through Linux repositories or MacPorts). Please see [Boost Notes](#boost-notes). + - Cmake version 3.0 or higher - Support for XCode 4.3 command line tools on Mac requires CMake 2.8.8 or higher @@ -66,11 +67,15 @@ execute commands as follows for an out-of-source build: This will build the library and unit tests, run all of the unit tests, and then install the library itself. +## Boost Notes + +Versions of Boost prior to 1.65 have a known bug that prevents proper "deep" serialization of objects, which means that objects encapsulated inside other objects don't get serialized. +This is particularly seen when using `clang` as the C++ compiler. + +For this reason we require Boost>=1.65, and recommend installing it through alternative channels when it is not available through your operating system's primary package manager. + ## Known Issues -- When using `GTSAM_BUILD_WITH_MARCH_NATIVE=ON`, you may encounter issues in running tests which we are still investigating: - - Use of a version of GCC < 7.5 results in an "Indeterminant Linear System" error for `testSmartProjectionFactor`. - - Use of Boost version < 1.65 with clang will give a segfault for mulitple test cases. - MSVC 2013 is not yet supported because it cannot build the serialization module of Boost 1.55 (or earlier). # Windows Installation From d4202a23ec2204a83356dc4eeba19c30bd0205db Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 23 Aug 2021 18:45:37 -0400 Subject: [PATCH 169/248] add missing import of Pose3 --- python/gtsam/utils/plot.py | 106 +++++++++++++++++++++---------------- 1 file changed, 61 insertions(+), 45 deletions(-) diff --git a/python/gtsam/utils/plot.py b/python/gtsam/utils/plot.py index a0c6f09b4..f324da63a 100644 --- a/python/gtsam/utils/plot.py +++ b/python/gtsam/utils/plot.py @@ -10,7 +10,7 @@ from matplotlib import patches from mpl_toolkits.mplot3d import Axes3D # pylint: disable=unused-import import gtsam -from gtsam import Marginals, Point3, Pose2, Values +from gtsam import Marginals, Point3, Pose2, Pose3, Values def set_axes_equal(fignum: int) -> None: @@ -39,9 +39,8 @@ def set_axes_equal(fignum: int) -> None: ax.set_zlim3d([origin[2] - radius, origin[2] + radius]) -def ellipsoid( - rx: float, ry: float, rz: float, n: int -) -> Tuple[np.ndarray, np.ndarray, np.ndarray]: +def ellipsoid(rx: float, ry: float, rz: float, + n: int) -> Tuple[np.ndarray, np.ndarray, np.ndarray]: """ Numpy equivalent of Matlab's ellipsoid function. @@ -54,8 +53,8 @@ def ellipsoid( Returns: The points in the x, y and z axes to use for the surface plot. """ - u = np.linspace(0, 2*np.pi, n+1) - v = np.linspace(0, np.pi, n+1) + u = np.linspace(0, 2 * np.pi, n + 1) + v = np.linspace(0, np.pi, n + 1) x = -rx * np.outer(np.cos(u), np.sin(v)).T y = -ry * np.outer(np.sin(u), np.sin(v)).T z = -rz * np.outer(np.ones_like(u), np.cos(v)).T @@ -63,9 +62,12 @@ def ellipsoid( return x, y, z -def plot_covariance_ellipse_3d( - axes, origin: Point3, P: np.ndarray, scale: float = 1, n: int = 8, alpha: float = 0.5 -) -> None: +def plot_covariance_ellipse_3d(axes, + origin: Point3, + P: np.ndarray, + scale: float = 1, + n: int = 8, + alpha: float = 0.5) -> None: """ Plots a Gaussian as an uncertainty ellipse @@ -97,15 +99,16 @@ def plot_covariance_ellipse_3d( np.kron(U[:, 2:3], zc) n = data.shape[1] x = data[0:n, :] + origin[0] - y = data[n:2*n, :] + origin[1] - z = data[2*n:, :] + origin[2] + y = data[n:2 * n, :] + origin[1] + z = data[2 * n:, :] + origin[2] axes.plot_surface(x, y, z, alpha=alpha, cmap='hot') -def plot_pose2_on_axes( - axes, pose: Pose2, axis_length: float = 0.1, covariance: np.ndarray = None -) -> None: +def plot_pose2_on_axes(axes, + pose: Pose2, + axis_length: float = 0.1, + covariance: np.ndarray = None) -> None: """ Plot a 2D pose on given axis `axes` with given `axis_length`. @@ -140,17 +143,20 @@ def plot_pose2_on_axes( k = 5.0 angle = np.arctan2(v[1, 0], v[0, 0]) - e1 = patches.Ellipse(origin, np.sqrt(w[0]*k), np.sqrt(w[1]*k), - np.rad2deg(angle), fill=False) + e1 = patches.Ellipse(origin, + np.sqrt(w[0] * k), + np.sqrt(w[1] * k), + np.rad2deg(angle), + fill=False) axes.add_patch(e1) def plot_pose2( - fignum: int, - pose: Pose2, - axis_length: float = 0.1, - covariance: np.ndarray = None, - axis_labels=("X axis", "Y axis", "Z axis"), + fignum: int, + pose: Pose2, + axis_length: float = 0.1, + covariance: np.ndarray = None, + axis_labels=("X axis", "Y axis", "Z axis"), ) -> plt.Figure: """ Plot a 2D pose on given figure with given `axis_length`. @@ -166,7 +172,9 @@ def plot_pose2( # get figure object fig = plt.figure(fignum) axes = fig.gca() - plot_pose2_on_axes(axes, pose, axis_length=axis_length, + plot_pose2_on_axes(axes, + pose, + axis_length=axis_length, covariance=covariance) axes.set_xlabel(axis_labels[0]) @@ -175,7 +183,10 @@ def plot_pose2( return fig -def plot_point3_on_axes(axes, point: Point3, linespec: str, P: Optional[np.ndarray] = None) -> None: +def plot_point3_on_axes(axes, + point: Point3, + linespec: str, + P: Optional[np.ndarray] = None) -> None: """ Plot a 3D point on given axis `axes` with given `linespec`. @@ -222,8 +233,12 @@ def plot_point3( return fig -def plot_3d_points(fignum, values, linespec="g*", marginals=None, - title="3D Points", axis_labels=('X axis', 'Y axis', 'Z axis')): +def plot_3d_points(fignum, + values, + linespec="g*", + marginals=None, + title="3D Points", + axis_labels=('X axis', 'Y axis', 'Z axis')): """ Plots the Point3s in `values`, with optional covariances. Finds all the Point3 objects in the given Values object and plots them. @@ -251,7 +266,10 @@ def plot_3d_points(fignum, values, linespec="g*", marginals=None, else: covariance = None - fig = plot_point3(fignum, point, linespec, covariance, + fig = plot_point3(fignum, + point, + linespec, + covariance, axis_labels=axis_labels) except RuntimeError: @@ -322,8 +340,7 @@ def plot_pose3( # get figure object fig = plt.figure(fignum) axes = fig.gca(projection='3d') - plot_pose3_on_axes(axes, pose, P=P, - axis_length=axis_length) + plot_pose3_on_axes(axes, pose, P=P, axis_length=axis_length) axes.set_xlabel(axis_labels[0]) axes.set_ylabel(axis_labels[1]) @@ -333,12 +350,12 @@ def plot_pose3( def plot_trajectory( - fignum: int, - values: Values, - scale: float = 1, - marginals: Marginals = None, - title: str = "Plot Trajectory", - axis_labels: Iterable[str] = ("X axis", "Y axis", "Z axis"), + fignum: int, + values: Values, + scale: float = 1, + marginals: Marginals = None, + title: str = "Plot Trajectory", + axis_labels: Iterable[str] = ("X axis", "Y axis", "Z axis"), ) -> None: """ Plot a complete 2D/3D trajectory using poses in `values`. @@ -368,7 +385,9 @@ def plot_trajectory( else: covariance = None - plot_pose2_on_axes(axes, pose, covariance=covariance, + plot_pose2_on_axes(axes, + pose, + covariance=covariance, axis_length=scale) # Then 3D poses, if any @@ -380,21 +399,18 @@ def plot_trajectory( else: covariance = None - plot_pose3_on_axes(axes, pose, P=covariance, - axis_length=scale) + plot_pose3_on_axes(axes, pose, P=covariance, axis_length=scale) fig.suptitle(title) fig.canvas.set_window_title(title.lower()) -def plot_incremental_trajectory( - fignum: int, - values: Values, - start: int = 0, - scale: float = 1, - marginals: Optional[Marginals] = None, - time_interval: float = 0.0 -) -> None: +def plot_incremental_trajectory(fignum: int, + values: Values, + start: int = 0, + scale: float = 1, + marginals: Optional[Marginals] = None, + time_interval: float = 0.0) -> None: """ Incrementally plot a complete 3D trajectory using poses in `values`. From 4f33cb8835ec800d3721592a593b37fac4803172 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 26 Aug 2021 02:22:28 -0400 Subject: [PATCH 170/248] add guards for system Metis --- gtsam/config.h.in | 3 +++ gtsam_unstable/partition/FindSeparator-inl.h | 4 ++++ gtsam_unstable/partition/tests/testFindSeparator.cpp | 4 ++++ 3 files changed, 11 insertions(+) diff --git a/gtsam/config.h.in b/gtsam/config.h.in index 9d1bd4ebd..9f7106187 100644 --- a/gtsam/config.h.in +++ b/gtsam/config.h.in @@ -77,3 +77,6 @@ // Support Metis-based nested dissection #cmakedefine GTSAM_TANGENT_PREINTEGRATION + +// Whether to use the system installed Metis instead of the provided one +#cmakedefine GTSAM_USE_SYSTEM_METIS diff --git a/gtsam_unstable/partition/FindSeparator-inl.h b/gtsam_unstable/partition/FindSeparator-inl.h index ce657e7a0..2e48b0d45 100644 --- a/gtsam_unstable/partition/FindSeparator-inl.h +++ b/gtsam_unstable/partition/FindSeparator-inl.h @@ -20,6 +20,8 @@ #include "FindSeparator.h" +#ifndef GTSAM_USE_SYSTEM_METIS + extern "C" { #include #include "metislib.h" @@ -564,3 +566,5 @@ namespace gtsam { namespace partition { } }} //namespace + +#endif diff --git a/gtsam_unstable/partition/tests/testFindSeparator.cpp b/gtsam_unstable/partition/tests/testFindSeparator.cpp index fe49de928..63acc8f18 100644 --- a/gtsam_unstable/partition/tests/testFindSeparator.cpp +++ b/gtsam_unstable/partition/tests/testFindSeparator.cpp @@ -20,6 +20,8 @@ using namespace std; using namespace gtsam; using namespace gtsam::partition; +#ifndef GTSAM_USE_SYSTEM_METIS + /* ************************************************************************* */ // x0 - x1 - x2 // l3 l4 @@ -227,6 +229,8 @@ TEST ( Partition, findSeparator3_with_reduced_camera ) LONGS_EQUAL(2, partitionTable[28]); } +#endif + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */ From 94dc709f9ba91c98643390806a79931c05641a64 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 26 Aug 2021 09:04:43 -0400 Subject: [PATCH 171/248] CI special case for using system version of 3rd party libraries --- .github/scripts/unix.sh | 2 ++ .github/workflows/build-special.yml | 12 ++++++++++++ 2 files changed, 14 insertions(+) diff --git a/.github/scripts/unix.sh b/.github/scripts/unix.sh index 6abbb5596..7fb925593 100644 --- a/.github/scripts/unix.sh +++ b/.github/scripts/unix.sh @@ -68,6 +68,8 @@ function configure() -DGTSAM_USE_QUATERNIONS=${GTSAM_USE_QUATERNIONS:-OFF} \ -DGTSAM_ROT3_EXPMAP=${GTSAM_ROT3_EXPMAP:-ON} \ -DGTSAM_POSE3_EXPMAP=${GTSAM_POSE3_EXPMAP:-ON} \ + -DGTSAM_USE_SYSTEM_EIGEN=${GTSAM_USE_SYSTEM_EIGEN:-OFF} \ + -DGTSAM_USE_SYSTEM_METIS=${GTSAM_USE_SYSTEM_METIS:-OFF} \ -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF \ -DBOOST_ROOT=$BOOST_ROOT \ -DBoost_NO_SYSTEM_PATHS=ON \ diff --git a/.github/workflows/build-special.yml b/.github/workflows/build-special.yml index 3bb3fa298..b7eb24c67 100644 --- a/.github/workflows/build-special.yml +++ b/.github/workflows/build-special.yml @@ -55,6 +55,12 @@ jobs: version: "9" flag: cayley + - name: ubuntu-system-libs + os: ubuntu-18.04 + compiler: gcc + version: "9" + flag: system-libs + steps: - name: Checkout uses: actions/checkout@v2 @@ -126,6 +132,12 @@ jobs: echo "GTSAM_ROT3_EXPMAP=OFF" >> $GITHUB_ENV echo "GTSAM Uses Cayley map for Rot3" + -name: Use system versions of 3rd party libraries + if: matrix.flag == 'system' + run: | + echo "GTSAM_USE_SYSTEM_EIGEN=ON" >> $GITHUB_ENV + echo "GTSAM_USE_SYSTEM_METIS=ON" >> $GITHUB_ENV + - name: Build & Test run: | bash .github/scripts/unix.sh -t From d83e9d0fd83bd0d175a31a3dd207f0f358781e8c Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 26 Aug 2021 09:05:55 -0400 Subject: [PATCH 172/248] formatting --- .github/workflows/build-special.yml | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/.github/workflows/build-special.yml b/.github/workflows/build-special.yml index b7eb24c67..647b9c0f1 100644 --- a/.github/workflows/build-special.yml +++ b/.github/workflows/build-special.yml @@ -132,11 +132,11 @@ jobs: echo "GTSAM_ROT3_EXPMAP=OFF" >> $GITHUB_ENV echo "GTSAM Uses Cayley map for Rot3" - -name: Use system versions of 3rd party libraries - if: matrix.flag == 'system' - run: | - echo "GTSAM_USE_SYSTEM_EIGEN=ON" >> $GITHUB_ENV - echo "GTSAM_USE_SYSTEM_METIS=ON" >> $GITHUB_ENV + - name: Use system versions of 3rd party libraries + if: matrix.flag == 'system' + run: | + echo "GTSAM_USE_SYSTEM_EIGEN=ON" >> $GITHUB_ENV + echo "GTSAM_USE_SYSTEM_METIS=ON" >> $GITHUB_ENV - name: Build & Test run: | From 4e295b91f22c1235dc7c9f85585e07a4e02ac043 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 26 Aug 2021 11:18:58 -0400 Subject: [PATCH 173/248] add appropriate guards for metis --- gtsam/inference/Ordering.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/gtsam/inference/Ordering.cpp b/gtsam/inference/Ordering.cpp index 266c54ca6..440d2b828 100644 --- a/gtsam/inference/Ordering.cpp +++ b/gtsam/inference/Ordering.cpp @@ -25,8 +25,12 @@ #include #ifdef GTSAM_SUPPORT_NESTED_DISSECTION +#ifdef GTSAM_USE_SYSTEM_METIS +#include +#else #include #endif +#endif using namespace std; From 37b001307e14bf859b8dcb2b9902f8d700243bcb Mon Sep 17 00:00:00 2001 From: lcarlone Date: Thu, 26 Aug 2021 11:29:34 -0400 Subject: [PATCH 174/248] plot twist: templating new factor on CAMERA --- .../SmartProjectionPoseFactorRollingShutter.h | 18 +- ...martProjectionPoseFactorRollingShutter.cpp | 209 +++++++++++++++++- 2 files changed, 218 insertions(+), 9 deletions(-) diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index dbe734a2c..524943a3f 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -37,9 +37,11 @@ namespace gtsam { * from which the pixel observation pose can be interpolated. * @addtogroup SLAM */ -template -class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor< -PinholePose > { +template +class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor { + + public: + typedef typename CAMERA::CalibrationType CALIBRATION; protected: /// shared pointer to calibration object (one for each observation) @@ -213,8 +215,8 @@ PinholePose > { /// equals bool equals(const NonlinearFactor& p, double tol = 1e-9) const override { - const SmartProjectionPoseFactorRollingShutter* e = - dynamic_cast*>(&p); + const SmartProjectionPoseFactorRollingShutter* e = + dynamic_cast*>(&p); double keyPairsEqual = true; if(this->world_P_body_key_pairs_.size() == e->world_P_body_key_pairs().size()){ @@ -430,9 +432,9 @@ PinholePose > { // end of class declaration /// traits -template -struct traits > : -public Testable > { +template +struct traits > : +public Testable > { }; } // namespace gtsam diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index 75aaf4ac1..bf8a8c4ab 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -73,7 +73,7 @@ Camera cam3(interp_pose3, sharedK); } LevenbergMarquardtParams lmParams; -typedef SmartProjectionPoseFactorRollingShutter SmartFactorRS; +typedef SmartProjectionPoseFactorRollingShutter< PinholePose > SmartFactorRS; /* ************************************************************************* */ TEST( SmartProjectionPoseFactorRollingShutter, Constructor) { @@ -770,6 +770,213 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRolli EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); } +/* *************************************************************************/ +TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRollingShutter_measurementsFromSamePose) { + // in this test we make sure the fact works even if we have multiple pixel measurements of the same landmark + // at a single pose, a setup that occurs in multi-camera systems + + using namespace vanillaPoseRS; + Point2Vector measurements_lmk1; + + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); + + // create redundant measurements: + Camera::MeasurementVector measurements_lmk1_redundant = measurements_lmk1; + measurements_lmk1_redundant.push_back(measurements_lmk1.at(0)); // we readd the first measurement + + // create inputs + std::vector> key_pairs; + key_pairs.push_back(std::make_pair(x1, x2)); + key_pairs.push_back(std::make_pair(x2, x3)); + key_pairs.push_back(std::make_pair(x3, x1)); + key_pairs.push_back(std::make_pair(x1, x2)); + + std::vector interp_factors; + interp_factors.push_back(interp_factor1); + interp_factors.push_back(interp_factor2); + interp_factors.push_back(interp_factor3); + interp_factors.push_back(interp_factor1); + + SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); + smartFactor1->add(measurements_lmk1_redundant, key_pairs, interp_factors, sharedK); + + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise + Values values; + values.insert(x1, level_pose); + values.insert(x2, pose_right); + // initialize third pose with some noise to get a nontrivial linearization point + values.insert(x3, pose_above * noise_pose); + EXPECT( // check that the pose is actually noisy + assert_equal( Pose3( Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), Point3(0.1, -0.1, 1.9)), values.at(x3))); + + // linearization point for the poses + Pose3 pose1 = level_pose; + Pose3 pose2 = pose_right; + Pose3 pose3 = pose_above * noise_pose; + + // ==== check Hessian of smartFactor1 ===== + // -- compute actual Hessian + boost::shared_ptr linearfactor1 = smartFactor1->linearize( + values); + Matrix actualHessian = linearfactor1->information(); + + // -- compute expected Hessian from manual Schur complement from Jacobians + // linearization point for the 3D point + smartFactor1->triangulateSafe(smartFactor1->cameras(values)); + TriangulationResult point = smartFactor1->point(); + EXPECT(point.valid()); // check triangulated point is valid + + // Use standard ProjectionFactorRollingShutter factor to calculate the Jacobians + Matrix F = Matrix::Zero(2 * 4, 6 * 3); + Matrix E = Matrix::Zero(2 * 4, 3); + Vector b = Vector::Zero(8); + + // create projection factors rolling shutter + ProjectionFactorRollingShutter factor11(measurements_lmk1_redundant[0], interp_factor1, + model, x1, x2, l0, sharedK); + Matrix H1Actual, H2Actual, H3Actual; + // note: b is minus the reprojection error, cf the smart factor jacobian computation + b.segment<2>(0) = -factor11.evaluateError(pose1, pose2, *point, H1Actual, H2Actual, H3Actual); + F.block<2, 6>(0, 0) = H1Actual; + F.block<2, 6>(0, 6) = H2Actual; + E.block<2, 3>(0, 0) = H3Actual; + + ProjectionFactorRollingShutter factor12(measurements_lmk1_redundant[1], interp_factor2, + model, x2, x3, l0, sharedK); + b.segment<2>(2) = -factor12.evaluateError(pose2, pose3, *point, H1Actual, H2Actual, H3Actual); + F.block<2, 6>(2, 6) = H1Actual; + F.block<2, 6>(2, 12) = H2Actual; + E.block<2, 3>(2, 0) = H3Actual; + + ProjectionFactorRollingShutter factor13(measurements_lmk1_redundant[2], interp_factor3, + model, x3, x1, l0, sharedK); + b.segment<2>(4) = -factor13.evaluateError(pose3, pose1, *point, H1Actual, H2Actual, H3Actual); + F.block<2, 6>(4, 12) = H1Actual; + F.block<2, 6>(4, 0) = H2Actual; + E.block<2, 3>(4, 0) = H3Actual; + + ProjectionFactorRollingShutter factor14(measurements_lmk1_redundant[3], interp_factor1, + model, x1, x2, l0, sharedK); + b.segment<2>(6) = -factor11.evaluateError(pose1, pose2, *point, H1Actual, H2Actual, H3Actual); + F.block<2, 6>(6, 0) = H1Actual; + F.block<2, 6>(6, 6) = H2Actual; + E.block<2, 3>(6, 0) = H3Actual; + + // whiten + F = (1/sigma) * F; + E = (1/sigma) * E; + b = (1/sigma) * b; + //* G = F' * F - F' * E * P * E' * F + Matrix P = (E.transpose() * E).inverse(); + Matrix expectedHessian = F.transpose() * F + - (F.transpose() * E * P * E.transpose() * F); + EXPECT(assert_equal(expectedHessian, actualHessian, 1e-6)); + + // ==== check Information vector of smartFactor1 ===== + GaussianFactorGraph gfg; + gfg.add(linearfactor1); + Matrix actualHessian_v2 = gfg.hessian().first; + EXPECT(assert_equal(actualHessian_v2, actualHessian, 1e-6)); // sanity check on hessian + + // -- compute actual information vector + Vector actualInfoVector = gfg.hessian().second; + + // -- compute expected information vector from manual Schur complement from Jacobians + //* g = F' * (b - E * P * E' * b) + Vector expectedInfoVector = F.transpose() * (b - E * P * E.transpose() * b); + EXPECT(assert_equal(expectedInfoVector, actualInfoVector, 1e-6)); + + // ==== check error of smartFactor1 (again) ===== + NonlinearFactorGraph nfg_projFactorsRS; + nfg_projFactorsRS.add(factor11); + nfg_projFactorsRS.add(factor12); + nfg_projFactorsRS.add(factor13); + nfg_projFactorsRS.add(factor14); + values.insert(l0, *point); + + double actualError = smartFactor1->error(values); + double expectedError = nfg_projFactorsRS.error(values); + EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); +} + +/* *************************************************************************/ +TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_measurementsFromSamePose ) { + + using namespace vanillaPoseRS; + Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; + + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); + + // create inputs + std::vector> key_pairs; + key_pairs.push_back(std::make_pair(x1,x2)); + key_pairs.push_back(std::make_pair(x2,x3)); + key_pairs.push_back(std::make_pair(x3,x1)); + + std::vector interp_factors; + interp_factors.push_back(interp_factor1); + interp_factors.push_back(interp_factor2); + interp_factors.push_back(interp_factor3); + + // For first factor, we create redundant measurement (taken by the same keys as factor 1, to + // make sure the redundancy in the keys does not create problems) + Camera::MeasurementVector& measurements_lmk1_redundant = measurements_lmk1; + measurements_lmk1_redundant.push_back(measurements_lmk1.at(0)); // we readd the first measurement + std::vector> key_pairs_redundant = key_pairs; + key_pairs_redundant.push_back(key_pairs.at(0)); // we readd the first pair of keys + std::vector interp_factors_redundant = interp_factors; + interp_factors_redundant.push_back(interp_factors.at(0));// we readd the first interp factor + + SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); + smartFactor1->add(measurements_lmk1_redundant, key_pairs_redundant, interp_factors_redundant, sharedK); + + SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model)); + smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, sharedK); + + SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model)); + smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, sharedK); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.addPrior(x1, level_pose, noisePrior); + graph.addPrior(x2, pose_right, noisePrior); + + Values groundTruth; + groundTruth.insert(x1, level_pose); + groundTruth.insert(x2, pose_right); + groundTruth.insert(x3, pose_above); + DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9); + + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise + Values values; + values.insert(x1, level_pose); + values.insert(x2, pose_right); + // initialize third pose with some noise, we expect it to move back to original pose_above + values.insert(x3, pose_above * noise_pose); + EXPECT( // check that the pose is actually noisy + assert_equal( + Pose3( + Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, + -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), + Point3(0.1, -0.1, 1.9)), values.at(x3))); + + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + EXPECT(assert_equal(pose_above, result.at(x3), 1e-5)); +} + #ifndef DISABLE_TIMING #include // -Total: 0 CPU (0 times, 0 wall, 0.04 children, min: 0 max: 0) From ff7ddf48bd8bb2f993437f01cd32a4727d6f023b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 27 Aug 2021 08:10:31 -0400 Subject: [PATCH 175/248] Basis functions (#403) --- doc/CMakeLists.txt | 25 +- gtsam/CMakeLists.txt | 1 + gtsam/basis/Basis.h | 507 ++++++++++++++++++ gtsam/basis/BasisFactors.h | 424 +++++++++++++++ gtsam/basis/CMakeLists.txt | 6 + gtsam/basis/Chebyshev.cpp | 98 ++++ gtsam/basis/Chebyshev.h | 109 ++++ gtsam/basis/Chebyshev2.cpp | 205 +++++++ gtsam/basis/Chebyshev2.h | 148 +++++ gtsam/basis/FitBasis.h | 99 ++++ gtsam/basis/Fourier.h | 108 ++++ gtsam/basis/ParameterMatrix.h | 215 ++++++++ gtsam/basis/basis.i | 146 +++++ gtsam/basis/tests/CMakeLists.txt | 1 + gtsam/basis/tests/testChebyshev.cpp | 236 ++++++++ gtsam/basis/tests/testChebyshev2.cpp | 435 +++++++++++++++ gtsam/basis/tests/testFourier.cpp | 254 +++++++++ gtsam/basis/tests/testParameterMatrix.cpp | 145 +++++ gtsam/nonlinear/FunctorizedFactor.h | 2 +- .../nonlinear/tests/testFunctorizedFactor.cpp | 140 ++++- python/CMakeLists.txt | 1 + python/gtsam/preamble/basis.h | 12 + python/gtsam/specializations/basis.h | 12 + 23 files changed, 3313 insertions(+), 16 deletions(-) create mode 100644 gtsam/basis/Basis.h create mode 100644 gtsam/basis/BasisFactors.h create mode 100644 gtsam/basis/CMakeLists.txt create mode 100644 gtsam/basis/Chebyshev.cpp create mode 100644 gtsam/basis/Chebyshev.h create mode 100644 gtsam/basis/Chebyshev2.cpp create mode 100644 gtsam/basis/Chebyshev2.h create mode 100644 gtsam/basis/FitBasis.h create mode 100644 gtsam/basis/Fourier.h create mode 100644 gtsam/basis/ParameterMatrix.h create mode 100644 gtsam/basis/basis.i create mode 100644 gtsam/basis/tests/CMakeLists.txt create mode 100644 gtsam/basis/tests/testChebyshev.cpp create mode 100644 gtsam/basis/tests/testChebyshev2.cpp create mode 100644 gtsam/basis/tests/testFourier.cpp create mode 100644 gtsam/basis/tests/testParameterMatrix.cpp create mode 100644 python/gtsam/preamble/basis.h create mode 100644 python/gtsam/specializations/basis.h diff --git a/doc/CMakeLists.txt b/doc/CMakeLists.txt index 7c43a8989..2218addcf 100644 --- a/doc/CMakeLists.txt +++ b/doc/CMakeLists.txt @@ -22,18 +22,19 @@ if (GTSAM_BUILD_DOCS) # GTSAM core subfolders set(gtsam_doc_subdirs - gtsam/base - gtsam/discrete - gtsam/geometry - gtsam/inference - gtsam/linear - gtsam/navigation - gtsam/nonlinear - gtsam/sam - gtsam/sfm - gtsam/slam - gtsam/smart - gtsam/symbolic + gtsam/base + gtsam/basis + gtsam/discrete + gtsam/geometry + gtsam/inference + gtsam/linear + gtsam/navigation + gtsam/nonlinear + gtsam/sam + gtsam/sfm + gtsam/slam + gtsam/smart + gtsam/symbolic gtsam ) diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index 71daf0653..e2f2ad828 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -5,6 +5,7 @@ project(gtsam LANGUAGES CXX) # The following variable is the master list of subdirs to add set (gtsam_subdirs base + basis geometry inference symbolic diff --git a/gtsam/basis/Basis.h b/gtsam/basis/Basis.h new file mode 100644 index 000000000..d8bd28c1a --- /dev/null +++ b/gtsam/basis/Basis.h @@ -0,0 +1,507 @@ +/* ---------------------------------------------------------------------------- + + * 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 Basis.h + * @brief Compute an interpolating basis + * @author Varun Agrawal, Jing Dong, Frank Dellaert + * @date July 4, 2020 + */ + +#pragma once + +#include +#include +#include + +#include + +/** + * This file supports creating continuous functions `f(x;p)` as a linear + * combination of `basis functions` such as the Fourier basis on SO(2) or a set + * of Chebyshev polynomials on [-1,1]. + * + * In the expression `f(x;p)` the variable `x` is + * the continuous argument at which the function is evaluated, and `p` are + * the parameters which are coefficients of the different basis functions, + * e.g. p = [4; 3; 2] => 4 + 3x + 2x^2 for a polynomial. + * However, different parameterizations are also possible. + + * The `Basis` class below defines a number of functors that can be used to + * evaluate `f(x;p)` at a given `x`, and these functors also calculate + * the Jacobian of `f(x;p)` with respect to the parameters `p`. + * This is actually the most important calculation, as it will allow GTSAM + * to optimize over the parameters `p`. + + * This functionality is implemented using the `CRTP` or "Curiously recurring + * template pattern" C++ idiom, which is a meta-programming technique in which + * the derived class is passed as a template argument to `Basis`. + * The DERIVED class is assumed to satisfy a C++ concept, + * i.e., we expect it to define the following types and methods: + + - type `Parameters`: the parameters `p` in f(x;p) + - `CalculateWeights(size_t N, double x, double a=default, double b=default)` + - `DerivativeWeights(size_t N, double x, double a=default, double b=default)` + + where `Weights` is an N*1 row vector which defines the basis values for the + polynomial at the specified point `x`. + + E.g. A Fourier series would give the following: + - `CalculateWeights` -> For N=5, the values for the bases: + [1, cos(x), sin(x), cos(2x), sin(2x)] + - `DerivativeWeights` -> For N=5, these are: + [0, -sin(x), cos(x), -2sin(2x), 2cos(x)] + + Note that for a pseudo-spectral basis (as in Chebyshev2), the weights are + instead the values for the Barycentric interpolation formula, since the values + at the polynomial points (e.g. Chebyshev points) define the bases. + */ + +namespace gtsam { + +using Weights = Eigen::Matrix; /* 1xN vector */ + +/** + * @brief Function for computing the kronecker product of the 1*N Weight vector + * `w` with the MxM identity matrix `I` efficiently. + * The main reason for this is so we don't need to use Eigen's Unsupported + * library. + * + * @tparam M Size of the identity matrix. + * @param w The weights of the polynomial. + * @return Mx(M*N) kronecker product [w(0)*I, w(1)*I, ..., w(N-1)*I] + */ +template +Matrix kroneckerProductIdentity(const Weights& w) { + Matrix result(M, w.cols() * M); + result.setZero(); + + for (int i = 0; i < w.cols(); i++) { + result.block(0, i * M, M, M).diagonal().array() = w(i); + } + return result; +} + +/// CRTP Base class for function bases +template +class GTSAM_EXPORT Basis { + public: + /** + * Calculate weights for all x in vector X. + * Returns M*N matrix where M is the size of the vector X, + * and N is the number of basis functions. + */ + static Matrix WeightMatrix(size_t N, const Vector& X) { + Matrix W(X.size(), N); + for (int i = 0; i < X.size(); i++) + W.row(i) = DERIVED::CalculateWeights(N, X(i)); + return W; + } + + /** + * @brief Calculate weights for all x in vector X, with interval [a,b]. + * + * @param N The number of basis functions. + * @param X The vector for which to compute the weights. + * @param a The lower bound for the interval range. + * @param b The upper bound for the interval range. + * @return Returns M*N matrix where M is the size of the vector X. + */ + static Matrix WeightMatrix(size_t N, const Vector& X, double a, double b) { + Matrix W(X.size(), N); + for (int i = 0; i < X.size(); i++) + W.row(i) = DERIVED::CalculateWeights(N, X(i), a, b); + return W; + } + + /** + * An instance of an EvaluationFunctor calculates f(x;p) at a given `x`, + * applied to Parameters `p`. + * This functor is used to evaluate a parameterized function at a given scalar + * value x. When given a specific N*1 vector of Parameters, returns the scalar + * value of the function at x, possibly with Jacobians wrpt the parameters. + */ + class EvaluationFunctor { + protected: + Weights weights_; + + public: + /// For serialization + EvaluationFunctor() {} + + /// Constructor with interval [a,b] + EvaluationFunctor(size_t N, double x) + : weights_(DERIVED::CalculateWeights(N, x)) {} + + /// Constructor with interval [a,b] + EvaluationFunctor(size_t N, double x, double a, double b) + : weights_(DERIVED::CalculateWeights(N, x, a, b)) {} + + /// Regular 1D evaluation + double apply(const typename DERIVED::Parameters& p, + OptionalJacobian<-1, -1> H = boost::none) const { + if (H) *H = weights_; + return (weights_ * p)(0); + } + + /// c++ sugar + double operator()(const typename DERIVED::Parameters& p, + OptionalJacobian<-1, -1> H = boost::none) const { + return apply(p, H); // might call apply in derived + } + + void print(const std::string& s = "") const { + std::cout << s << (s != "" ? " " : "") << weights_ << std::endl; + } + }; + + /** + * VectorEvaluationFunctor at a given x, applied to ParameterMatrix. + * This functor is used to evaluate a parameterized function at a given scalar + * value x. When given a specific M*N parameters, returns an M-vector the M + * corresponding functions at x, possibly with Jacobians wrpt the parameters. + */ + template + class VectorEvaluationFunctor : protected EvaluationFunctor { + protected: + using VectorM = Eigen::Matrix; + using Jacobian = Eigen::Matrix; + Jacobian H_; + + /** + * Calculate the `M*(M*N)` Jacobian of this functor with respect to + * the M*N parameter matrix `P`. + * We flatten assuming column-major order, e.g., if N=3 and M=2, we have + * H =[ w(0) 0 w(1) 0 w(2) 0 + * 0 w(0) 0 w(1) 0 w(2) ] + * i.e., the Kronecker product of weights_ with the MxM identity matrix. + */ + void calculateJacobian() { + H_ = kroneckerProductIdentity(this->weights_); + } + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /// For serialization + VectorEvaluationFunctor() {} + + /// Default Constructor + VectorEvaluationFunctor(size_t N, double x) : EvaluationFunctor(N, x) { + calculateJacobian(); + } + + /// Constructor, with interval [a,b] + VectorEvaluationFunctor(size_t N, double x, double a, double b) + : EvaluationFunctor(N, x, a, b) { + calculateJacobian(); + } + + /// M-dimensional evaluation + VectorM apply(const ParameterMatrix& P, + OptionalJacobian H = boost::none) const { + if (H) *H = H_; + return P.matrix() * this->weights_.transpose(); + } + + /// c++ sugar + VectorM operator()(const ParameterMatrix& P, + OptionalJacobian H = boost::none) const { + return apply(P, H); + } + }; + + /** + * Given a M*N Matrix of M-vectors at N polynomial points, an instance of + * VectorComponentFunctor computes the N-vector value for a specific row + * component of the M-vectors at all the polynomial points. + * + * This component is specified by the row index i, with 0 + class VectorComponentFunctor : public EvaluationFunctor { + protected: + using Jacobian = Eigen::Matrix; + size_t rowIndex_; + Jacobian H_; + + /* + * Calculate the `1*(M*N)` Jacobian of this functor with respect to + * the M*N parameter matrix `P`. + * We flatten assuming column-major order, e.g., if N=3 and M=2, we have + * H=[w(0) 0 w(1) 0 w(2) 0] for rowIndex==0 + * H=[0 w(0) 0 w(1) 0 w(2)] for rowIndex==1 + * i.e., one row of the Kronecker product of weights_ with the + * MxM identity matrix. See also VectorEvaluationFunctor. + */ + void calculateJacobian(size_t N) { + H_.setZero(1, M * N); + for (int j = 0; j < EvaluationFunctor::weights_.size(); j++) + H_(0, rowIndex_ + j * M) = EvaluationFunctor::weights_(j); + } + + public: + /// For serialization + VectorComponentFunctor() {} + + /// Construct with row index + VectorComponentFunctor(size_t N, size_t i, double x) + : EvaluationFunctor(N, x), rowIndex_(i) { + calculateJacobian(N); + } + + /// Construct with row index and interval + VectorComponentFunctor(size_t N, size_t i, double x, double a, double b) + : EvaluationFunctor(N, x, a, b), rowIndex_(i) { + calculateJacobian(N); + } + + /// Calculate component of component rowIndex_ of P + double apply(const ParameterMatrix& P, + OptionalJacobian H = boost::none) const { + if (H) *H = H_; + return P.row(rowIndex_) * EvaluationFunctor::weights_.transpose(); + } + + /// c++ sugar + double operator()(const ParameterMatrix& P, + OptionalJacobian H = boost::none) const { + return apply(P, H); + } + }; + + /** + * Manifold EvaluationFunctor at a given x, applied to ParameterMatrix. + * This functor is used to evaluate a parameterized function at a given scalar + * value x. When given a specific M*N parameters, returns an M-vector the M + * corresponding functions at x, possibly with Jacobians wrpt the parameters. + * + * The difference with the VectorEvaluationFunctor is that after computing the + * M*1 vector xi=F(x;P), with x a scalar and P the M*N parameter vector, we + * also retract xi back to the T manifold. + * For example, if T==Rot3, then we first compute a 3-vector xi using x and P, + * and then map that 3-vector xi back to the Rot3 manifold, yielding a valid + * 3D rotation. + */ + template + class ManifoldEvaluationFunctor + : public VectorEvaluationFunctor::dimension> { + enum { M = traits::dimension }; + using Base = VectorEvaluationFunctor; + + public: + /// For serialization + ManifoldEvaluationFunctor() {} + + /// Default Constructor + ManifoldEvaluationFunctor(size_t N, double x) : Base(N, x) {} + + /// Constructor, with interval [a,b] + ManifoldEvaluationFunctor(size_t N, double x, double a, double b) + : Base(N, x, a, b) {} + + /// Manifold evaluation + T apply(const ParameterMatrix& P, + OptionalJacobian H = boost::none) const { + // Interpolate the M-dimensional vector to yield a vector in tangent space + Eigen::Matrix xi = Base::operator()(P, H); + + // Now call retract with this M-vector, possibly with derivatives + Eigen::Matrix D_result_xi; + T result = T::ChartAtOrigin::Retract(xi, H ? &D_result_xi : 0); + + // Finally, if derivatives are asked, apply chain rule where H is Mx(M*N) + // derivative of interpolation and D_result_xi is MxM derivative of + // retract. + if (H) *H = D_result_xi * (*H); + + // and return a T + return result; + } + + /// c++ sugar + T operator()(const ParameterMatrix& P, + OptionalJacobian H = boost::none) const { + return apply(P, H); // might call apply in derived + } + }; + + /// Base class for functors below that calculate derivative weights + class DerivativeFunctorBase { + protected: + Weights weights_; + + public: + /// For serialization + DerivativeFunctorBase() {} + + DerivativeFunctorBase(size_t N, double x) + : weights_(DERIVED::DerivativeWeights(N, x)) {} + + DerivativeFunctorBase(size_t N, double x, double a, double b) + : weights_(DERIVED::DerivativeWeights(N, x, a, b)) {} + + void print(const std::string& s = "") const { + std::cout << s << (s != "" ? " " : "") << weights_ << std::endl; + } + }; + + /** + * An instance of a DerivativeFunctor calculates f'(x;p) at a given `x`, + * applied to Parameters `p`. + * When given a scalar value x and a specific N*1 vector of Parameters, + * this functor returns the scalar derivative value of the function at x, + * possibly with Jacobians wrpt the parameters. + */ + class DerivativeFunctor : protected DerivativeFunctorBase { + public: + /// For serialization + DerivativeFunctor() {} + + DerivativeFunctor(size_t N, double x) : DerivativeFunctorBase(N, x) {} + + DerivativeFunctor(size_t N, double x, double a, double b) + : DerivativeFunctorBase(N, x, a, b) {} + + double apply(const typename DERIVED::Parameters& p, + OptionalJacobian H = boost::none) const { + if (H) *H = this->weights_; + return (this->weights_ * p)(0); + } + /// c++ sugar + double operator()(const typename DERIVED::Parameters& p, + OptionalJacobian H = boost::none) const { + return apply(p, H); // might call apply in derived + } + }; + + /** + * VectorDerivativeFunctor at a given x, applied to ParameterMatrix. + * + * This functor is used to evaluate the derivatives of a parameterized + * function at a given scalar value x. When given a specific M*N parameters, + * returns an M-vector the M corresponding function derivatives at x, possibly + * with Jacobians wrpt the parameters. + */ + template + class VectorDerivativeFunctor : protected DerivativeFunctorBase { + protected: + using VectorM = Eigen::Matrix; + using Jacobian = Eigen::Matrix; + Jacobian H_; + + /** + * Calculate the `M*(M*N)` Jacobian of this functor with respect to + * the M*N parameter matrix `P`. + * We flatten assuming column-major order, e.g., if N=3 and M=2, we have + * H =[ w(0) 0 w(1) 0 w(2) 0 + * 0 w(0) 0 w(1) 0 w(2) ] + * i.e., the Kronecker product of weights_ with the MxM identity matrix. + */ + void calculateJacobian() { + H_ = kroneckerProductIdentity(this->weights_); + } + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /// For serialization + VectorDerivativeFunctor() {} + + /// Default Constructor + VectorDerivativeFunctor(size_t N, double x) : DerivativeFunctorBase(N, x) { + calculateJacobian(); + } + + /// Constructor, with optional interval [a,b] + VectorDerivativeFunctor(size_t N, double x, double a, double b) + : DerivativeFunctorBase(N, x, a, b) { + calculateJacobian(); + } + + VectorM apply(const ParameterMatrix& P, + OptionalJacobian H = boost::none) const { + if (H) *H = H_; + return P.matrix() * this->weights_.transpose(); + } + /// c++ sugar + VectorM operator()( + const ParameterMatrix& P, + OptionalJacobian H = boost::none) const { + return apply(P, H); + } + }; + + /** + * Given a M*N Matrix of M-vectors at N polynomial points, an instance of + * ComponentDerivativeFunctor computes the N-vector derivative for a specific + * row component of the M-vectors at all the polynomial points. + * + * This component is specified by the row index i, with 0 + class ComponentDerivativeFunctor : protected DerivativeFunctorBase { + protected: + using Jacobian = Eigen::Matrix; + size_t rowIndex_; + Jacobian H_; + + /* + * Calculate the `1*(M*N)` Jacobian of this functor with respect to + * the M*N parameter matrix `P`. + * We flatten assuming column-major order, e.g., if N=3 and M=2, we have + * H=[w(0) 0 w(1) 0 w(2) 0] for rowIndex==0 + * H=[0 w(0) 0 w(1) 0 w(2)] for rowIndex==1 + * i.e., one row of the Kronecker product of weights_ with the + * MxM identity matrix. See also VectorDerivativeFunctor. + */ + void calculateJacobian(size_t N) { + H_.setZero(1, M * N); + for (int j = 0; j < this->weights_.size(); j++) + H_(0, rowIndex_ + j * M) = this->weights_(j); + } + + public: + /// For serialization + ComponentDerivativeFunctor() {} + + /// Construct with row index + ComponentDerivativeFunctor(size_t N, size_t i, double x) + : DerivativeFunctorBase(N, x), rowIndex_(i) { + calculateJacobian(N); + } + + /// Construct with row index and interval + ComponentDerivativeFunctor(size_t N, size_t i, double x, double a, double b) + : DerivativeFunctorBase(N, x, a, b), rowIndex_(i) { + calculateJacobian(N); + } + /// Calculate derivative of component rowIndex_ of F + double apply(const ParameterMatrix& P, + OptionalJacobian H = boost::none) const { + if (H) *H = H_; + return P.row(rowIndex_) * this->weights_.transpose(); + } + /// c++ sugar + double operator()(const ParameterMatrix& P, + OptionalJacobian H = boost::none) const { + return apply(P, H); + } + }; + + // Vector version for MATLAB :-( + static double Derivative(double x, const Vector& p, // + OptionalJacobian H = boost::none) { + return DerivativeFunctor(x)(p.transpose(), H); + } +}; + +} // namespace gtsam diff --git a/gtsam/basis/BasisFactors.h b/gtsam/basis/BasisFactors.h new file mode 100644 index 000000000..0b3d4c1a0 --- /dev/null +++ b/gtsam/basis/BasisFactors.h @@ -0,0 +1,424 @@ +/* ---------------------------------------------------------------------------- + + * 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 BasisFactors.h + * @brief Factor definitions for various Basis functors. + * @author Varun Agrawal + * @date July 4, 2020 + **/ + +#pragma once + +#include +#include + +namespace gtsam { + +/** + * @brief Factor for enforcing the scalar value of the polynomial BASIS + * representation at `x` is the same as the measurement `z` when using a + * pseudo-spectral parameterization. + * + * @tparam BASIS The basis class to use e.g. Chebyshev2 + */ +template +class GTSAM_EXPORT EvaluationFactor : public FunctorizedFactor { + private: + using Base = FunctorizedFactor; + + public: + EvaluationFactor() {} + + /** + * @brief Construct a new EvaluationFactor object + * + * @param key Symbol for value to optimize. + * @param z The measurement value. + * @param model Noise model + * @param N The degree of the polynomial. + * @param x The point at which to evaluate the polynomial. + */ + EvaluationFactor(Key key, const double &z, const SharedNoiseModel &model, + const size_t N, double x) + : Base(key, z, model, typename BASIS::EvaluationFunctor(N, x)) {} + + /** + * @brief Construct a new EvaluationFactor object + * + * @param key Symbol for value to optimize. + * @param z The measurement value. + * @param model Noise model + * @param N The degree of the polynomial. + * @param x The point at which to evaluate the polynomial. + * @param a Lower bound for the polynomial. + * @param b Upper bound for the polynomial. + */ + EvaluationFactor(Key key, const double &z, const SharedNoiseModel &model, + const size_t N, double x, double a, double b) + : Base(key, z, model, typename BASIS::EvaluationFunctor(N, x, a, b)) {} + + virtual ~EvaluationFactor() {} +}; + +/** + * Unary factor for enforcing BASIS polynomial evaluation on a ParameterMatrix + * of size (M, N) is equal to a vector-valued measurement at the same point, + when + * using a pseudo-spectral parameterization. + * + * This factors tries to enforce the basis function evaluation `f(x;p)` to take + * on the value `z` at location `x`, providing a gradient on the parameters p. + * In a probabilistic estimation context, `z` is known as a measurement, and the + * parameterized basis function can be seen as a + * measurement prediction function. + * + * @param BASIS: The basis class to use e.g. Chebyshev2 + * @param M: Size of the evaluated state vector. + */ +template +class GTSAM_EXPORT VectorEvaluationFactor + : public FunctorizedFactor> { + private: + using Base = FunctorizedFactor>; + + public: + VectorEvaluationFactor() {} + + /** + * @brief Construct a new VectorEvaluationFactor object. + * + * @param key The key to the ParameterMatrix object used to represent the + * polynomial. + * @param z The measurement value. + * @param model The noise model. + * @param N The degree of the polynomial. + * @param x The point at which to evaluate the basis polynomial. + */ + VectorEvaluationFactor(Key key, const Vector &z, + const SharedNoiseModel &model, const size_t N, + double x) + : Base(key, z, model, + typename BASIS::template VectorEvaluationFunctor(N, x)) {} + + /** + * @brief Construct a new VectorEvaluationFactor object. + * + * @param key The key to the ParameterMatrix object used to represent the + * polynomial. + * @param z The measurement value. + * @param model The noise model. + * @param N The degree of the polynomial. + * @param x The point at which to evaluate the basis polynomial. + * @param a Lower bound for the polynomial. + * @param b Upper bound for the polynomial. + */ + VectorEvaluationFactor(Key key, const Vector &z, + const SharedNoiseModel &model, const size_t N, + double x, double a, double b) + : Base(key, z, model, + typename BASIS::template VectorEvaluationFunctor(N, x, a, b)) {} + + virtual ~VectorEvaluationFactor() {} +}; + +/** + * Unary factor for enforcing BASIS polynomial evaluation on a ParameterMatrix + * of size (P, N) is equal to specified measurement at the same point, when + * using a pseudo-spectral parameterization. + * + * This factor is similar to `VectorEvaluationFactor` with the key difference + * being that it only enforces the constraint for a single scalar in the vector, + * indexed by `i`. + * + * @param BASIS: The basis class to use e.g. Chebyshev2 + * @param P: Size of the fixed-size vector. + * + * Example: + * VectorComponentFactor controlPrior(key, measured, model, + * N, i, t, a, b); + * where N is the degree and i is the component index. + */ +template +class GTSAM_EXPORT VectorComponentFactor + : public FunctorizedFactor> { + private: + using Base = FunctorizedFactor>; + + public: + VectorComponentFactor() {} + + /** + * @brief Construct a new VectorComponentFactor object. + * + * @param key The key to the ParameterMatrix object used to represent the + * polynomial. + * @param z The scalar value at a specified index `i` of the full measurement + * vector. + * @param model The noise model. + * @param N The degree of the polynomial. + * @param i The index for the evaluated vector to give us the desired scalar + * value. + * @param x The point at which to evaluate the basis polynomial. + */ + VectorComponentFactor(Key key, const double &z, const SharedNoiseModel &model, + const size_t N, size_t i, double x) + : Base(key, z, model, + typename BASIS::template VectorComponentFunctor

(N, i, x)) {} + + /** + * @brief Construct a new VectorComponentFactor object. + * + * @param key The key to the ParameterMatrix object used to represent the + * polynomial. + * @param z The scalar value at a specified index `i` of the full measurement + * vector. + * @param model The noise model. + * @param N The degree of the polynomial. + * @param i The index for the evaluated vector to give us the desired scalar + * value. + * @param x The point at which to evaluate 0the basis polynomial. + * @param a Lower bound for the polynomial. + * @param b Upper bound for the polynomial. + */ + VectorComponentFactor(Key key, const double &z, const SharedNoiseModel &model, + const size_t N, size_t i, double x, double a, double b) + : Base( + key, z, model, + typename BASIS::template VectorComponentFunctor

(N, i, x, a, b)) { + } + + virtual ~VectorComponentFactor() {} +}; + +/** + * For a measurement value of type T i.e. `T z = g(x)`, this unary + * factor enforces that the polynomial basis, when evaluated at `x`, gives us + * the same `z`, i.e. `T z = g(x) = f(x;p)`. + * + * This is done via computations on the tangent space of the + * manifold of T. + * + * @param BASIS: The basis class to use e.g. Chebyshev2 + * @param T: Object type which is synthesized by the provided functor. + * + * Example: + * ManifoldEvaluationFactor rotationFactor(key, measurement, + * model, N, x, a, b); + * + * where `x` is the value (e.g. timestep) at which the rotation was evaluated. + */ +template +class GTSAM_EXPORT ManifoldEvaluationFactor + : public FunctorizedFactor::dimension>> { + private: + using Base = FunctorizedFactor::dimension>>; + + public: + ManifoldEvaluationFactor() {} + + /** + * @brief Construct a new ManifoldEvaluationFactor object. + * + * @param key Key for the state matrix parameterizing the function to estimate + * via the BASIS. + * @param z The measurement value. + * @param model The noise model. + * @param N The degree of the polynomial. + * @param x The point at which the estimated function is evaluated. + */ + ManifoldEvaluationFactor(Key key, const T &z, const SharedNoiseModel &model, + const size_t N, double x) + : Base(key, z, model, + typename BASIS::template ManifoldEvaluationFunctor(N, x)) {} + + /** + * @brief Construct a new ManifoldEvaluationFactor object. + * + * @param key Key for the state matrix parameterizing the function to estimate + * via the BASIS. + * @param z The measurement value. + * @param model The noise model. + * @param N The degree of the polynomial. + * @param x The point at which the estimated function is evaluated. + * @param a Lower bound for the polynomial. + * @param b Upper bound for the polynomial. + */ + ManifoldEvaluationFactor(Key key, const T &z, const SharedNoiseModel &model, + const size_t N, double x, double a, double b) + : Base( + key, z, model, + typename BASIS::template ManifoldEvaluationFunctor(N, x, a, b)) { + } + + virtual ~ManifoldEvaluationFactor() {} +}; + +/** + * A unary factor which enforces the evaluation of the derivative of a BASIS + * polynomial at a specified point`x` is equal to the scalar measurement `z`. + * + * @param BASIS: The basis class to use e.g. Chebyshev2 + */ +template +class GTSAM_EXPORT DerivativeFactor + : public FunctorizedFactor { + private: + using Base = FunctorizedFactor; + + public: + DerivativeFactor() {} + + /** + * @brief Construct a new DerivativeFactor object. + * + * @param key The key to the ParameterMatrix which represents the basis + * polynomial. + * @param z The measurement value. + * @param model The noise model. + * @param N The degree of the polynomial. + * @param x The point at which to evaluate the basis polynomial. + */ + DerivativeFactor(Key key, const double &z, const SharedNoiseModel &model, + const size_t N, double x) + : Base(key, z, model, typename BASIS::DerivativeFunctor(N, x)) {} + + /** + * @brief Construct a new DerivativeFactor object. + * + * @param key The key to the ParameterMatrix which represents the basis + * polynomial. + * @param z The measurement value. + * @param model The noise model. + * @param N The degree of the polynomial. + * @param x The point at which to evaluate the basis polynomial. + * @param a Lower bound for the polynomial. + * @param b Upper bound for the polynomial. + */ + DerivativeFactor(Key key, const double &z, const SharedNoiseModel &model, + const size_t N, double x, double a, double b) + : Base(key, z, model, typename BASIS::DerivativeFunctor(N, x, a, b)) {} + + virtual ~DerivativeFactor() {} +}; + +/** + * A unary factor which enforces the evaluation of the derivative of a BASIS + * polynomial at a specified point `x` is equal to the vector value `z`. + * + * @param BASIS: The basis class to use e.g. Chebyshev2 + * @param M: Size of the evaluated state vector derivative. + */ +template +class GTSAM_EXPORT VectorDerivativeFactor + : public FunctorizedFactor> { + private: + using Base = FunctorizedFactor>; + using Func = typename BASIS::template VectorDerivativeFunctor; + + public: + VectorDerivativeFactor() {} + + /** + * @brief Construct a new VectorDerivativeFactor object. + * + * @param key The key to the ParameterMatrix which represents the basis + * polynomial. + * @param z The measurement value. + * @param model The noise model. + * @param N The degree of the polynomial. + * @param x The point at which to evaluate the basis polynomial. + */ + VectorDerivativeFactor(Key key, const Vector &z, + const SharedNoiseModel &model, const size_t N, + double x) + : Base(key, z, model, Func(N, x)) {} + + /** + * @brief Construct a new VectorDerivativeFactor object. + * + * @param key The key to the ParameterMatrix which represents the basis + * polynomial. + * @param z The measurement value. + * @param model The noise model. + * @param N The degree of the polynomial. + * @param x The point at which to evaluate the basis polynomial. + * @param a Lower bound for the polynomial. + * @param b Upper bound for the polynomial. + */ + VectorDerivativeFactor(Key key, const Vector &z, + const SharedNoiseModel &model, const size_t N, + double x, double a, double b) + : Base(key, z, model, Func(N, x, a, b)) {} + + virtual ~VectorDerivativeFactor() {} +}; + +/** + * A unary factor which enforces the evaluation of the derivative of a BASIS + * polynomial is equal to the scalar value at a specific index `i` of a + * vector-valued measurement `z`. + * + * @param BASIS: The basis class to use e.g. Chebyshev2 + * @param P: Size of the control component derivative. + */ +template +class GTSAM_EXPORT ComponentDerivativeFactor + : public FunctorizedFactor> { + private: + using Base = FunctorizedFactor>; + using Func = typename BASIS::template ComponentDerivativeFunctor

; + + public: + ComponentDerivativeFactor() {} + + /** + * @brief Construct a new ComponentDerivativeFactor object. + * + * @param key The key to the ParameterMatrix which represents the basis + * polynomial. + * @param z The scalar measurement value at a specific index `i` of the full + * measurement vector. + * @param model The degree of the polynomial. + * @param N The degree of the polynomial. + * @param i The index for the evaluated vector to give us the desired scalar + * value. + * @param x The point at which to evaluate the basis polynomial. + */ + ComponentDerivativeFactor(Key key, const double &z, + const SharedNoiseModel &model, const size_t N, + size_t i, double x) + : Base(key, z, model, Func(N, i, x)) {} + + /** + * @brief Construct a new ComponentDerivativeFactor object. + * + * @param key The key to the ParameterMatrix which represents the basis + * polynomial. + * @param z The scalar measurement value at a specific index `i` of the full + * measurement vector. + * @param model The degree of the polynomial. + * @param N The degree of the polynomial. + * @param i The index for the evaluated vector to give us the desired scalar + * value. + * @param x The point at which to evaluate the basis polynomial. + * @param a Lower bound for the polynomial. + * @param b Upper bound for the polynomial. + */ + ComponentDerivativeFactor(Key key, const double &z, + const SharedNoiseModel &model, const size_t N, + size_t i, double x, double a, double b) + : Base(key, z, model, Func(N, i, x, a, b)) {} + + virtual ~ComponentDerivativeFactor() {} +}; + +} // namespace gtsam diff --git a/gtsam/basis/CMakeLists.txt b/gtsam/basis/CMakeLists.txt new file mode 100644 index 000000000..203fd96a2 --- /dev/null +++ b/gtsam/basis/CMakeLists.txt @@ -0,0 +1,6 @@ +# Install headers +file(GLOB basis_headers "*.h") +install(FILES ${basis_headers} DESTINATION include/gtsam/basis) + +# Build tests +add_subdirectory(tests) diff --git a/gtsam/basis/Chebyshev.cpp b/gtsam/basis/Chebyshev.cpp new file mode 100644 index 000000000..3b5825fc3 --- /dev/null +++ b/gtsam/basis/Chebyshev.cpp @@ -0,0 +1,98 @@ +/* ---------------------------------------------------------------------------- + + * 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 Chebyshev.cpp + * @brief Chebyshev basis decompositions + * @author Varun Agrawal, Jing Dong, Frank Dellaert + * @date July 4, 2020 + */ + +#include + +namespace gtsam { + +/** + * @brief Scale x from [a, b] to [t1, t2] + * + * ((b'-a') * (x - a) / (b - a)) + a' + * + * @param x Value to scale to new range. + * @param a Original lower limit. + * @param b Original upper limit. + * @param t1 New lower limit. + * @param t2 New upper limit. + * @return double + */ +static double scale(double x, double a, double b, double t1, double t2) { + return ((t2 - t1) * (x - a) / (b - a)) + t1; +} + +Weights Chebyshev1Basis::CalculateWeights(size_t N, double x, double a, + double b) { + Weights Tx(1, N); + + x = scale(x, a, b, -1, 1); + + Tx(0) = 1; + Tx(1) = x; + for (size_t i = 2; i < N; i++) { + // instead of cos(i*acos(x)), this recurrence is much faster + Tx(i) = 2 * x * Tx(i - 1) - Tx(i - 2); + } + return Tx; +} + +Weights Chebyshev1Basis::DerivativeWeights(size_t N, double x, double a, + double b) { + Weights Ux = Chebyshev2Basis::CalculateWeights(N, x, a, b); + Weights weights = Weights::Zero(N); + for (size_t n = 1; n < N; n++) { + weights(n) = n * Ux(n - 1); + } + return weights; +} + +Weights Chebyshev2Basis::CalculateWeights(size_t N, double x, double a, + double b) { + Weights Ux(N); + + x = scale(x, a, b, -1, 1); + + Ux(0) = 1; + Ux(1) = 2 * x; + for (size_t i = 2; i < N; i++) { + // instead of cos(i*acos(x)), this recurrence is much faster + Ux(i) = 2 * x * Ux(i - 1) - Ux(i - 2); + } + return Ux; +} + +Weights Chebyshev2Basis::DerivativeWeights(size_t N, double x, double a, + double b) { + Weights Tx = Chebyshev1Basis::CalculateWeights(N + 1, x, a, b); + Weights Ux = Chebyshev2Basis::CalculateWeights(N, x, a, b); + + Weights weights(N); + + x = scale(x, a, b, -1, 1); + if (x == -1 || x == 1) { + throw std::runtime_error( + "Derivative of Chebyshev2 Basis does not exist at range limits."); + } + + for (size_t n = 0; n < N; n++) { + weights(n) = ((n + 1) * Tx(n + 1) - x * Ux(n)) / (x * x - 1); + } + return weights; +} + +} // namespace gtsam diff --git a/gtsam/basis/Chebyshev.h b/gtsam/basis/Chebyshev.h new file mode 100644 index 000000000..d16ccfaac --- /dev/null +++ b/gtsam/basis/Chebyshev.h @@ -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 Chebyshev.h + * @brief Chebyshev basis decompositions + * @author Varun Agrawal, Jing Dong, Frank Dellaert + * @date July 4, 2020 + */ + +#pragma once + +#include +#include + +#include + +namespace gtsam { + +/** + * Basis of Chebyshev polynomials of the first kind + * https://en.wikipedia.org/wiki/Chebyshev_polynomials#First_kind + * These are typically denoted with the symbol T_n, where n is the degree. + * The parameter N is the number of coefficients, i.e., N = n+1. + */ +struct Chebyshev1Basis : Basis { + using Parameters = Eigen::Matrix; + + Parameters parameters_; + + /** + * @brief Evaluate Chebyshev Weights on [-1,1] at x up to order N-1 (N values) + * + * @param N Degree of the polynomial. + * @param x Point to evaluate polynomial at. + * @param a Lower limit of polynomial (default=-1). + * @param b Upper limit of polynomial (default=1). + */ + static Weights CalculateWeights(size_t N, double x, double a = -1, + double b = 1); + + /** + * @brief Evaluate Chebyshev derivative at x. + * The derivative weights are pre-multiplied to the polynomial Parameters. + * + * From Wikipedia we have D[T_n(x),x] = n*U_{n-1}(x) + * I.e. the derivative fo a first kind cheb is just a second kind cheb + * So, we define a second kind basis here of order N-1 + * Note that it has one less weight. + * + * The Parameters pertain to 1st kind chebs up to order N-1 + * But of course the first one (order 0) is constant, so omit that weight. + * + * @param N Degree of the polynomial. + * @param x Point to evaluate polynomial at. + * @param a Lower limit of polynomial (default=-1). + * @param b Upper limit of polynomial (default=1). + * @return Weights + */ + static Weights DerivativeWeights(size_t N, double x, double a = -1, + double b = 1); +}; // Chebyshev1Basis + +/** + * Basis of Chebyshev polynomials of the second kind. + * https://en.wikipedia.org/wiki/Chebyshev_polynomials#Second_kind + * These are typically denoted with the symbol U_n, where n is the degree. + * The parameter N is the number of coefficients, i.e., N = n+1. + * In contrast to the templates in Chebyshev2, the classes below specify + * basis functions, weighted combinations of which are used to approximate + * functions. In this sense, they are like the sines and cosines of the Fourier + * basis. + */ +struct Chebyshev2Basis : Basis { + using Parameters = Eigen::Matrix; + + /** + * Evaluate Chebyshev Weights on [-1,1] at any x up to order N-1 (N values). + * + * @param N Degree of the polynomial. + * @param x Point to evaluate polynomial at. + * @param a Lower limit of polynomial (default=-1). + * @param b Upper limit of polynomial (default=1). + */ + static Weights CalculateWeights(size_t N, double x, double a = -1, + double b = 1); + + /** + * @brief Evaluate Chebyshev derivative at x. + * + * @param N Degree of the polynomial. + * @param x Point to evaluate polynomial at. + * @param a Lower limit of polynomial (default=-1). + * @param b Upper limit of polynomial (default=1). + * @return Weights + */ + static Weights DerivativeWeights(size_t N, double x, double a = -1, + double b = 1); +}; // Chebyshev2Basis + +} // namespace gtsam diff --git a/gtsam/basis/Chebyshev2.cpp b/gtsam/basis/Chebyshev2.cpp new file mode 100644 index 000000000..44876b6e9 --- /dev/null +++ b/gtsam/basis/Chebyshev2.cpp @@ -0,0 +1,205 @@ +/* ---------------------------------------------------------------------------- + + * 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 Chebyshev2.cpp + * @brief Chebyshev parameterizations on Chebyshev points of second kind + * @author Varun Agrawal, Jing Dong, Frank Dellaert + * @date July 4, 2020 + */ + +#include + +namespace gtsam { + +Weights Chebyshev2::CalculateWeights(size_t N, double x, double a, double b) { + // Allocate space for weights + Weights weights(N); + + // We start by getting distances from x to all Chebyshev points + // as well as getting smallest distance + Weights distances(N); + + for (size_t j = 0; j < N; j++) { + const double dj = + x - Point(N, j, a, b); // only thing that depends on [a,b] + + if (std::abs(dj) < 1e-10) { + // exceptional case: x coincides with a Chebyshev point + weights.setZero(); + weights(j) = 1; + return weights; + } + distances(j) = dj; + } + + // Beginning of interval, j = 0, x(0) = a + weights(0) = 0.5 / distances(0); + + // All intermediate points j=1:N-2 + double d = weights(0), s = -1; // changes sign s at every iteration + for (size_t j = 1; j < N - 1; j++, s = -s) { + weights(j) = s / distances(j); + d += weights(j); + } + + // End of interval, j = N-1, x(N-1) = b + weights(N - 1) = 0.5 * s / distances(N - 1); + d += weights(N - 1); + + // normalize + return weights / d; +} + +Weights Chebyshev2::DerivativeWeights(size_t N, double x, double a, double b) { + // Allocate space for weights + Weights weightDerivatives(N); + + // toggle variable so we don't need to use `pow` for -1 + double t = -1; + + // We start by getting distances from x to all Chebyshev points + // as well as getting smallest distance + Weights distances(N); + + for (size_t j = 0; j < N; j++) { + const double dj = + x - Point(N, j, a, b); // only thing that depends on [a,b] + if (std::abs(dj) < 1e-10) { + // exceptional case: x coincides with a Chebyshev point + weightDerivatives.setZero(); + // compute the jth row of the differentiation matrix for this point + double cj = (j == 0 || j == N - 1) ? 2. : 1.; + for (size_t k = 0; k < N; k++) { + if (j == 0 && k == 0) { + // we reverse the sign since we order the cheb points from -1 to 1 + weightDerivatives(k) = -(cj * (N - 1) * (N - 1) + 1) / 6.0; + } else if (j == N - 1 && k == N - 1) { + // we reverse the sign since we order the cheb points from -1 to 1 + weightDerivatives(k) = (cj * (N - 1) * (N - 1) + 1) / 6.0; + } else if (k == j) { + double xj = Point(N, j); + double xj2 = xj * xj; + weightDerivatives(k) = -0.5 * xj / (1 - xj2); + } else { + double xj = Point(N, j); + double xk = Point(N, k); + double ck = (k == 0 || k == N - 1) ? 2. : 1.; + t = ((j + k) % 2) == 0 ? 1 : -1; + weightDerivatives(k) = (cj / ck) * t / (xj - xk); + } + } + return 2 * weightDerivatives / (b - a); + } + distances(j) = dj; + } + + // This section of code computes the derivative of + // the Barycentric Interpolation weights formula by applying + // the chain rule on the original formula. + + // g and k are multiplier terms which represent the derivatives of + // the numerator and denominator + double g = 0, k = 0; + double w = 1; + + for (size_t j = 0; j < N; j++) { + if (j == 0 || j == N - 1) { + w = 0.5; + } else { + w = 1.0; + } + + t = (j % 2 == 0) ? 1 : -1; + + double c = t / distances(j); + g += w * c; + k += (w * c / distances(j)); + } + + double s = 1; // changes sign s at every iteration + double g2 = g * g; + + for (size_t j = 0; j < N; j++, s = -s) { + // Beginning of interval, j = 0, x0 = -1.0 and end of interval, j = N-1, + // x0 = 1.0 + if (j == 0 || j == N - 1) { + w = 0.5; + } else { + // All intermediate points j=1:N-2 + w = 1.0; + } + weightDerivatives(j) = (w * -s / (g * distances(j) * distances(j))) - + (w * -s * k / (g2 * distances(j))); + } + + return weightDerivatives; +} + +Chebyshev2::DiffMatrix Chebyshev2::DifferentiationMatrix(size_t N, double a, + double b) { + DiffMatrix D(N, N); + if (N == 1) { + D(0, 0) = 1; + return D; + } + + // toggle variable so we don't need to use `pow` for -1 + double t = -1; + + for (size_t i = 0; i < N; i++) { + double xi = Point(N, i); + double ci = (i == 0 || i == N - 1) ? 2. : 1.; + for (size_t j = 0; j < N; j++) { + if (i == 0 && j == 0) { + // we reverse the sign since we order the cheb points from -1 to 1 + D(i, j) = -(ci * (N - 1) * (N - 1) + 1) / 6.0; + } else if (i == N - 1 && j == N - 1) { + // we reverse the sign since we order the cheb points from -1 to 1 + D(i, j) = (ci * (N - 1) * (N - 1) + 1) / 6.0; + } else if (i == j) { + double xi2 = xi * xi; + D(i, j) = -xi / (2 * (1 - xi2)); + } else { + double xj = Point(N, j); + double cj = (j == 0 || j == N - 1) ? 2. : 1.; + t = ((i + j) % 2) == 0 ? 1 : -1; + D(i, j) = (ci / cj) * t / (xi - xj); + } + } + } + // scale the matrix to the range + return D / ((b - a) / 2.0); +} + +Weights Chebyshev2::IntegrationWeights(size_t N, double a, double b) { + // Allocate space for weights + Weights weights(N); + size_t K = N - 1, // number of intervals between N points + K2 = K * K; + weights(0) = 0.5 * (b - a) / (K2 + K % 2 - 1); + weights(N - 1) = weights(0); + + size_t last_k = K / 2 + K % 2 - 1; + + for (size_t i = 1; i <= N - 2; ++i) { + double theta = i * M_PI / K; + weights(i) = (K % 2 == 0) ? 1 - cos(K * theta) / (K2 - 1) : 1; + + for (size_t k = 1; k <= last_k; ++k) + weights(i) -= 2 * cos(2 * k * theta) / (4 * k * k - 1); + weights(i) *= (b - a) / K; + } + + return weights; +} + +} // namespace gtsam diff --git a/gtsam/basis/Chebyshev2.h b/gtsam/basis/Chebyshev2.h new file mode 100644 index 000000000..28590961d --- /dev/null +++ b/gtsam/basis/Chebyshev2.h @@ -0,0 +1,148 @@ +/* ---------------------------------------------------------------------------- + + * 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 Chebyshev2.h + * @brief Pseudo-spectral parameterization for Chebyshev polynomials of the + * second kind. + * + * In a pseudo-spectral case, rather than the parameters acting as + * weights for the bases polynomials (as in Chebyshev2Basis), here the + * parameters are the *values* at a specific set of points in the interval, the + * "Chebyshev points". These values uniquely determine the polynomial that + * interpolates them at the Chebyshev points. + * + * This is different from Chebyshev.h since it leverage ideas from + * pseudo-spectral optimization, i.e. we don't decompose into basis functions, + * rather estimate function parameters that enforce function nodes at Chebyshev + * points. + * + * Please refer to Agrawal21icra for more details. + * + * @author Varun Agrawal, Jing Dong, Frank Dellaert + * @date July 4, 2020 + */ + +#pragma once + +#include +#include +#include + +#include + +namespace gtsam { + +/** + * Chebyshev Interpolation on Chebyshev points of the second kind + * Note that N here, the number of points, is one less than N from + * 'Approximation Theory and Approximation Practice by L. N. Trefethen (pg.42)'. + */ +class GTSAM_EXPORT Chebyshev2 : public Basis { + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + using Base = Basis; + using Parameters = Eigen::Matrix; + using DiffMatrix = Eigen::Matrix; + + /// Specific Chebyshev point + static double Point(size_t N, int j) { + assert(j >= 0 && size_t(j) < N); + const double dtheta = M_PI / (N > 1 ? (N - 1) : 1); + // We add -PI so that we get values ordered from -1 to +1 + // sin(- M_PI_2 + dtheta*j); also works + return cos(-M_PI + dtheta * j); + } + + /// Specific Chebyshev point, within [a,b] interval + static double Point(size_t N, int j, double a, double b) { + assert(j >= 0 && size_t(j) < N); + const double dtheta = M_PI / (N - 1); + // We add -PI so that we get values ordered from -1 to +1 + return a + (b - a) * (1. + cos(-M_PI + dtheta * j)) / 2; + } + + /// All Chebyshev points + static Vector Points(size_t N) { + Vector points(N); + for (size_t j = 0; j < N; j++) points(j) = Point(N, j); + return points; + } + + /// All Chebyshev points, within [a,b] interval + static Vector Points(size_t N, double a, double b) { + Vector points = Points(N); + const double T1 = (a + b) / 2, T2 = (b - a) / 2; + points = T1 + (T2 * points).array(); + return points; + } + + /** + * Evaluate Chebyshev Weights on [-1,1] at any x up to order N-1 (N values) + * These weights implement barycentric interpolation at a specific x. + * More precisely, f(x) ~ [w0;...;wN] * [f0;...;fN], where the fj are the + * values of the function f at the Chebyshev points. As such, for a given x we + * obtain a linear map from parameter vectors f to interpolated values f(x). + * Optional [a,b] interval can be specified as well. + */ + static Weights CalculateWeights(size_t N, double x, double a = -1, + double b = 1); + + /** + * Evaluate derivative of barycentric weights. + * This is easy and efficient via the DifferentiationMatrix. + */ + static Weights DerivativeWeights(size_t N, double x, double a = -1, + double b = 1); + + /// compute D = differentiation matrix, Trefethen00book p.53 + /// when given a parameter vector f of function values at the Chebyshev + /// points, D*f are the values of f'. + /// https://people.maths.ox.ac.uk/trefethen/8all.pdf Theorem 8.4 + static DiffMatrix DifferentiationMatrix(size_t N, double a = -1, + double b = 1); + + /** + * Evaluate Clenshaw-Curtis integration weights. + * Trefethen00book, pg 128, clencurt.m + * Note that N in clencurt.m is 1 less than our N + * K = N-1; + theta = pi*(0:K)'/K; + w = zeros(1,N); ii = 2:K; v = ones(K-1, 1); + if mod(K,2) == 0 + w(1) = 1/(K^2-1); w(N) = w(1); + for k=1:K/2-1, v = v-2*cos(2*k*theta(ii))/(4*k^2-1); end + v = v - cos(K*theta(ii))/(K^2-1); + else + w(1) = 1/K^2; w(N) = w(1); + for k=1:K/2, v = v-2*cos(2*k*theta(ii))/(4*k^2-1); end + end + w(ii) = 2*v/K; + + */ + static Weights IntegrationWeights(size_t N, double a = -1, double b = 1); + + /** + * Create matrix of values at Chebyshev points given vector-valued function. + */ + template + static Matrix matrix(boost::function(double)> f, + size_t N, double a = -1, double b = 1) { + Matrix Xmat(M, N); + for (size_t j = 0; j < N; j++) { + Xmat.col(j) = f(Point(N, j, a, b)); + } + return Xmat; + } +}; // \ Chebyshev2 + +} // namespace gtsam diff --git a/gtsam/basis/FitBasis.h b/gtsam/basis/FitBasis.h new file mode 100644 index 000000000..f5cb99bd7 --- /dev/null +++ b/gtsam/basis/FitBasis.h @@ -0,0 +1,99 @@ +/* ---------------------------------------------------------------------------- + + * 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 FitBasis.h + * @date July 4, 2020 + * @author Varun Agrawal, Frank Dellaert + * @brief Fit a Basis using least-squares + */ + +/* + * Concept needed for LS. Parameters = Coefficients | Values + * - Parameters, Jacobian + * - PredictFactor(double x)(Parameters p, OptionalJacobian<1,N> H) + */ + +#pragma once + +#include +#include +#include +#include +#include + +namespace gtsam { + +/// Our sequence representation is a map of {x: y} values where y = f(x) +using Sequence = std::map; +/// A sample is a key-value pair from a sequence. +using Sample = std::pair; + +/** + * Class that does regression via least squares + * Example usage: + * size_t N = 3; + * auto fit = FitBasis(data_points, noise_model, N); + * Vector coefficients = fit.parameters(); + * + * where `data_points` are a map from `x` to `y` values indicating a function + * mapping at specific points, `noise_model` is the gaussian noise model, and + * `N` is the degree of the polynomial basis used to fit the function. + */ +template +class FitBasis { + public: + using Parameters = typename Basis::Parameters; + + private: + Parameters parameters_; + + public: + /// Create nonlinear FG from Sequence + static NonlinearFactorGraph NonlinearGraph(const Sequence& sequence, + const SharedNoiseModel& model, + size_t N) { + NonlinearFactorGraph graph; + for (const Sample sample : sequence) { + graph.emplace_shared>(0, sample.second, model, N, + sample.first); + } + return graph; + } + + /// Create linear FG from Sequence + static GaussianFactorGraph::shared_ptr LinearGraph( + const Sequence& sequence, const SharedNoiseModel& model, size_t N) { + NonlinearFactorGraph graph = NonlinearGraph(sequence, model, N); + Values values; + values.insert(0, Parameters::Zero(N)); + GaussianFactorGraph::shared_ptr gfg = graph.linearize(values); + return gfg; + } + + /** + * @brief Construct a new FitBasis object. + * + * @param sequence map of x->y values for a function, a.k.a. y = f(x). + * @param model The noise model to use. + * @param N The degree of the polynomial to fit. + */ + FitBasis(const Sequence& sequence, const SharedNoiseModel& model, size_t N) { + GaussianFactorGraph::shared_ptr gfg = LinearGraph(sequence, model, N); + VectorValues solution = gfg->optimize(); + parameters_ = solution.at(0); + } + + /// Return Fourier coefficients + Parameters parameters() const { return parameters_; } +}; + +} // namespace gtsam diff --git a/gtsam/basis/Fourier.h b/gtsam/basis/Fourier.h new file mode 100644 index 000000000..6943150d7 --- /dev/null +++ b/gtsam/basis/Fourier.h @@ -0,0 +1,108 @@ +/* ---------------------------------------------------------------------------- + + * 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 Fourier.h + * @brief Fourier decomposition, see e.g. + * http://mathworld.wolfram.com/FourierSeries.html + * @author Varun Agrawal, Frank Dellaert + * @date July 4, 2020 + */ + +#pragma once + +#include + +namespace gtsam { + +/// Fourier basis +class GTSAM_EXPORT FourierBasis : public Basis { + public: + using Parameters = Eigen::Matrix; + using DiffMatrix = Eigen::Matrix; + + /** + * @brief Evaluate Real Fourier Weights of size N in interval [a, b], + * e.g. N=5 yields bases: 1, cos(x), sin(x), cos(2*x), sin(2*x) + * + * @param N The degree of the polynomial to use. + * @param x The point at which to compute the derivaive weights. + * @return Weights + */ + static Weights CalculateWeights(size_t N, double x) { + Weights b(N); + b[0] = 1; + for (size_t i = 1, n = 1; i < N; i += 2, n++) { + b[i] = cos(n * x); + b[i + 1] = sin(n * x); + } + return b; + } + + /** + * @brief Evaluate Real Fourier Weights of size N in interval [a, b], + * e.g. N=5 yields bases: 1, cos(x), sin(x), cos(2*x), sin(2*x) + * + * @param N The degree of the polynomial to use. + * @param x The point at which to compute the weights. + * @param a Lower bound of interval. + * @param b Upper bound of interval. + * @return Weights + */ + static Weights CalculateWeights(size_t N, double x, double a, double b) { + // TODO(Varun) How do we enforce an interval for Fourier series? + return CalculateWeights(N, x); + } + + /** + * Compute D = differentiation matrix. + * Given coefficients c of a Fourier series c, D*c are the values of c'. + */ + static DiffMatrix DifferentiationMatrix(size_t N) { + DiffMatrix D = DiffMatrix::Zero(N, N); + double k = 1; + for (size_t i = 1; i < N; i += 2) { + D(i, i + 1) = k; // sin'(k*x) = k*cos(k*x) + D(i + 1, i) = -k; // cos'(k*x) = -k*sin(k*x) + k += 1; + } + + return D; + } + + /** + * @brief Get weights at a given x that calculate the derivative. + * + * @param N The degree of the polynomial to use. + * @param x The point at which to compute the derivaive weights. + * @return Weights + */ + static Weights DerivativeWeights(size_t N, double x) { + return CalculateWeights(N, x) * DifferentiationMatrix(N); + } + + /** + * @brief Get derivative weights at a given x that calculate the derivative, + in the interval [a, b]. + * + * @param N The degree of the polynomial to use. + * @param x The point at which to compute the derivaive weights. + * @param a Lower bound of interval. + * @param b Upper bound of interval. + * @return Weights + */ + static Weights DerivativeWeights(size_t N, double x, double a, double b) { + return CalculateWeights(N, x, a, b) * DifferentiationMatrix(N); + } + +}; // FourierBasis + +} // namespace gtsam diff --git a/gtsam/basis/ParameterMatrix.h b/gtsam/basis/ParameterMatrix.h new file mode 100644 index 000000000..df2d9f62e --- /dev/null +++ b/gtsam/basis/ParameterMatrix.h @@ -0,0 +1,215 @@ +/* ---------------------------------------------------------------------------- + + * 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 ParamaterMatrix.h + * @brief Define ParameterMatrix class which is used to store values at + * interpolation points. + * @author Varun Agrawal, Frank Dellaert + * @date September 21, 2020 + */ + +#pragma once + +#include +#include +#include + +#include + +namespace gtsam { + +/** + * A matrix abstraction of MxN values at the Basis points. + * This class serves as a wrapper over an Eigen matrix. + * @tparam M: The dimension of the type you wish to evaluate. + * @param N: the number of Basis points (e.g. Chebyshev points of the second + * kind). + */ +template +class ParameterMatrix { + using MatrixType = Eigen::Matrix; + + private: + MatrixType matrix_; + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + enum { dimension = Eigen::Dynamic }; + + /** + * Create ParameterMatrix using the number of basis points. + * @param N: The number of basis points (the columns). + */ + ParameterMatrix(const size_t N) : matrix_(M, N) { matrix_.setZero(); } + + /** + * Create ParameterMatrix from an MxN Eigen Matrix. + * @param matrix: An Eigen matrix used to initialze the ParameterMatrix. + */ + ParameterMatrix(const MatrixType& matrix) : matrix_(matrix) {} + + /// Get the number of rows. + size_t rows() const { return matrix_.rows(); } + + /// Get the number of columns. + size_t cols() const { return matrix_.cols(); } + + /// Get the underlying matrix. + MatrixType matrix() const { return matrix_; } + + /// Return the tranpose of the underlying matrix. + Eigen::Matrix transpose() const { return matrix_.transpose(); } + + /** + * Get the matrix row specified by `index`. + * @param index: The row index to retrieve. + */ + Eigen::Matrix row(size_t index) const { + return matrix_.row(index); + } + + /** + * Set the matrix row specified by `index`. + * @param index: The row index to set. + */ + auto row(size_t index) -> Eigen::Block { + return matrix_.row(index); + } + + /** + * Get the matrix column specified by `index`. + * @param index: The column index to retrieve. + */ + Eigen::Matrix col(size_t index) const { + return matrix_.col(index); + } + + /** + * Set the matrix column specified by `index`. + * @param index: The column index to set. + */ + auto col(size_t index) -> Eigen::Block { + return matrix_.col(index); + } + + /** + * Set all matrix coefficients to zero. + */ + void setZero() { matrix_.setZero(); } + + /** + * Add a ParameterMatrix to another. + * @param other: ParameterMatrix to add. + */ + ParameterMatrix operator+(const ParameterMatrix& other) const { + return ParameterMatrix(matrix_ + other.matrix()); + } + + /** + * Add a MxN-sized vector to the ParameterMatrix. + * @param other: Vector which is reshaped and added. + */ + ParameterMatrix operator+( + const Eigen::Matrix& other) const { + // This form avoids a deep copy and instead typecasts `other`. + Eigen::Map other_(other.data(), M, cols()); + return ParameterMatrix(matrix_ + other_); + } + + /** + * Subtract a ParameterMatrix from another. + * @param other: ParameterMatrix to subtract. + */ + ParameterMatrix operator-(const ParameterMatrix& other) const { + return ParameterMatrix(matrix_ - other.matrix()); + } + + /** + * Subtract a MxN-sized vector from the ParameterMatrix. + * @param other: Vector which is reshaped and subracted. + */ + ParameterMatrix operator-( + const Eigen::Matrix& other) const { + Eigen::Map other_(other.data(), M, cols()); + return ParameterMatrix(matrix_ - other_); + } + + /** + * Multiply ParameterMatrix with an Eigen matrix. + * @param other: Eigen matrix which should be multiplication compatible with + * the ParameterMatrix. + */ + MatrixType operator*(const Eigen::Matrix& other) const { + return matrix_ * other; + } + + /// @name Vector Space requirements, following LieMatrix + /// @{ + + /** + * Print the ParameterMatrix. + * @param s: The prepend string to add more contextual info. + */ + void print(const std::string& s = "") const { + std::cout << (s == "" ? s : s + " ") << matrix_ << std::endl; + } + + /** + * Check for equality up to absolute tolerance. + * @param other: The ParameterMatrix to check equality with. + * @param tol: The absolute tolerance threshold. + */ + bool equals(const ParameterMatrix& other, double tol = 1e-8) const { + return gtsam::equal_with_abs_tol(matrix_, other.matrix(), tol); + } + + /// Returns dimensionality of the tangent space + inline size_t dim() const { return matrix_.size(); } + + /// Convert to vector form, is done row-wise + inline Vector vector() const { + using RowMajor = Eigen::Matrix; + Vector result(matrix_.size()); + Eigen::Map(&result(0), rows(), cols()) = matrix_; + return result; + } + + /** Identity function to satisfy VectorSpace traits. + * + * NOTE: The size at compile time is unknown so this identity is zero + * length and thus not valid. + */ + inline static ParameterMatrix identity() { + // throw std::runtime_error( + // "ParameterMatrix::identity(): Don't use this function"); + return ParameterMatrix(0); + } + + /// @} +}; + +// traits for ParameterMatrix +template +struct traits> + : public internal::VectorSpace> {}; + +/* ************************************************************************* */ +// Stream operator that takes a ParameterMatrix. Used for printing. +template +inline std::ostream& operator<<(std::ostream& os, + const ParameterMatrix& parameterMatrix) { + os << parameterMatrix.matrix(); + return os; +} + +} // namespace gtsam \ No newline at end of file diff --git a/gtsam/basis/basis.i b/gtsam/basis/basis.i new file mode 100644 index 000000000..8f06fd2e1 --- /dev/null +++ b/gtsam/basis/basis.i @@ -0,0 +1,146 @@ +//************************************************************************* +// basis +//************************************************************************* + +namespace gtsam { + +// TODO(gerry): add all the Functors to the Basis interfaces, e.g. +// `EvaluationFunctor` + +#include + +class FourierBasis { + static Vector CalculateWeights(size_t N, double x); + static Matrix WeightMatrix(size_t N, Vector x); + + static Matrix DifferentiationMatrix(size_t N); + static Vector DerivativeWeights(size_t N, double x); +}; + +#include + +class Chebyshev1Basis { + static Matrix CalculateWeights(size_t N, double x); + static Matrix WeightMatrix(size_t N, Vector X); +}; + +class Chebyshev2Basis { + static Matrix CalculateWeights(size_t N, double x); + static Matrix WeightMatrix(size_t N, Vector x); +}; + +#include +class Chebyshev2 { + static double Point(size_t N, int j); + static double Point(size_t N, int j, double a, double b); + + static Vector Points(size_t N); + static Vector Points(size_t N, double a, double b); + + static Matrix WeightMatrix(size_t N, Vector X); + static Matrix WeightMatrix(size_t N, Vector X, double a, double b); + + static Matrix CalculateWeights(size_t N, double x, double a, double b); + static Matrix DerivativeWeights(size_t N, double x, double a, double b); + static Matrix IntegrationWeights(size_t N, double a, double b); + static Matrix DifferentiationMatrix(size_t N, double a, double b); + + // TODO Needs OptionalJacobian + // static double Derivative(double x, Vector f); +}; + +#include + +template +class ParameterMatrix { + ParameterMatrix(const size_t N); + ParameterMatrix(const Matrix& matrix); + + Matrix matrix() const; + + void print(const string& s = "") const; +}; + +#include + +template +virtual class EvaluationFactor : gtsam::NoiseModelFactor { + EvaluationFactor(); + EvaluationFactor(gtsam::Key key, const double z, + const gtsam::noiseModel::Base* model, const size_t N, + double x); + EvaluationFactor(gtsam::Key key, const double z, + const gtsam::noiseModel::Base* model, const size_t N, + double x, double a, double b); +}; + +template +virtual class VectorEvaluationFactor : gtsam::NoiseModelFactor { + VectorEvaluationFactor(); + VectorEvaluationFactor(gtsam::Key key, const Vector& z, + const gtsam::noiseModel::Base* model, const size_t N, + double x); + VectorEvaluationFactor(gtsam::Key key, const Vector& z, + const gtsam::noiseModel::Base* model, const size_t N, + double x, double a, double b); +}; + +// TODO(Varun) Better way to support arbitrary dimensions? +// Especially if users mainly do `pip install gtsam` for the Python wrapper. +typedef gtsam::VectorEvaluationFactor + VectorEvaluationFactorChebyshev2D3; +typedef gtsam::VectorEvaluationFactor + VectorEvaluationFactorChebyshev2D4; +typedef gtsam::VectorEvaluationFactor + VectorEvaluationFactorChebyshev2D12; + +template +virtual class VectorComponentFactor : gtsam::NoiseModelFactor { + VectorComponentFactor(); + VectorComponentFactor(gtsam::Key key, const double z, + const gtsam::noiseModel::Base* model, const size_t N, + size_t i, double x); + VectorComponentFactor(gtsam::Key key, const double z, + const gtsam::noiseModel::Base* model, const size_t N, + size_t i, double x, double a, double b); +}; + +typedef gtsam::VectorComponentFactor + VectorComponentFactorChebyshev2D3; +typedef gtsam::VectorComponentFactor + VectorComponentFactorChebyshev2D4; +typedef gtsam::VectorComponentFactor + VectorComponentFactorChebyshev2D12; + +template +virtual class ManifoldEvaluationFactor : gtsam::NoiseModelFactor { + ManifoldEvaluationFactor(); + ManifoldEvaluationFactor(gtsam::Key key, const T& z, + const gtsam::noiseModel::Base* model, const size_t N, + double x); + ManifoldEvaluationFactor(gtsam::Key key, const T& z, + const gtsam::noiseModel::Base* model, const size_t N, + double x, double a, double b); +}; + +// TODO(gerry): Add `DerivativeFactor`, `VectorDerivativeFactor`, and +// `ComponentDerivativeFactor` + +#include +template +class FitBasis { + FitBasis(const std::map& sequence, + const gtsam::noiseModel::Base* model, size_t N); + + static gtsam::NonlinearFactorGraph NonlinearGraph( + const std::map& sequence, + const gtsam::noiseModel::Base* model, size_t N); + static gtsam::GaussianFactorGraph::shared_ptr LinearGraph( + const std::map& sequence, + const gtsam::noiseModel::Base* model, size_t N); + Parameters parameters() const; +}; + +} // namespace gtsam diff --git a/gtsam/basis/tests/CMakeLists.txt b/gtsam/basis/tests/CMakeLists.txt new file mode 100644 index 000000000..63cad0be6 --- /dev/null +++ b/gtsam/basis/tests/CMakeLists.txt @@ -0,0 +1 @@ +gtsamAddTestsGlob(basis "test*.cpp" "" "gtsam") diff --git a/gtsam/basis/tests/testChebyshev.cpp b/gtsam/basis/tests/testChebyshev.cpp new file mode 100644 index 000000000..64c925886 --- /dev/null +++ b/gtsam/basis/tests/testChebyshev.cpp @@ -0,0 +1,236 @@ +/* ---------------------------------------------------------------------------- + + * 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 testChebyshev.cpp + * @date July 4, 2020 + * @author Varun Agrawal + * @brief Unit tests for Chebyshev Basis Decompositions + */ + +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +auto model = noiseModel::Unit::Create(1); + +const size_t N = 3; + +//****************************************************************************** +TEST(Chebyshev, Chebyshev1) { + using Synth = Chebyshev1Basis::EvaluationFunctor; + Vector c(N); + double x; + c << 12, 3, 1; + x = -1.0; + EXPECT_DOUBLES_EQUAL(12 + 3 * x + 2 * x * x - 1, Synth(N, x)(c), 1e-9); + x = -0.5; + EXPECT_DOUBLES_EQUAL(12 + 3 * x + 2 * x * x - 1, Synth(N, x)(c), 1e-9); + x = 0.3; + EXPECT_DOUBLES_EQUAL(12 + 3 * x + 2 * x * x - 1, Synth(N, x)(c), 1e-9); +} + +//****************************************************************************** +TEST(Chebyshev, Chebyshev2) { + using Synth = Chebyshev2Basis::EvaluationFunctor; + Vector c(N); + double x; + c << 12, 3, 1; + x = -1.0; + EXPECT_DOUBLES_EQUAL(12 + 6 * x + 4 * x * x - 1, Synth(N, x)(c), 1e-9); + x = -0.5; + EXPECT_DOUBLES_EQUAL(12 + 6 * x + 4 * x * x - 1, Synth(N, x)(c), 1e-9); + x = 0.3; + EXPECT_DOUBLES_EQUAL(12 + 6 * x + 4 * x * x - 1, Synth(N, x)(c), 1e-9); +} + +//****************************************************************************** +TEST(Chebyshev, Evaluation) { + Chebyshev1Basis::EvaluationFunctor fx(N, 0.5); + Vector c(N); + c << 3, 5, -12; + EXPECT_DOUBLES_EQUAL(11.5, fx(c), 1e-9); +} + +//****************************************************************************** +#include +#include +TEST(Chebyshev, Expression) { + // Create linear factor graph + NonlinearFactorGraph graph; + Key key(1); + + // Let's pretend we have 6 GPS measurements (we just do x coordinate) + // at times + const size_t m = 6; + Vector t(m); + t << -0.7, -0.4, 0.1, 0.3, 0.7, 0.9; + Vector x(m); + x << -0.7, -0.4, 0.1, 0.3, 0.7, 0.9; + + for (size_t i = 0; i < m; i++) { + graph.emplace_shared>(key, x(i), model, N, + t(i)); + } + + // Solve + Values initial; + initial.insert(key, Vector::Zero(N)); // initial does not matter + + // ... and optimize + GaussNewtonParams parameters; + GaussNewtonOptimizer optimizer(graph, initial, parameters); + Values result = optimizer.optimize(); + + // Check + Vector expected(N); + expected << 0, 1, 0; + Vector actual_c = result.at(key); + EXPECT(assert_equal(expected, actual_c)); + + // Calculate and print covariances + Marginals marginals(graph, result); + Matrix3 cov = marginals.marginalCovariance(key); + EXPECT_DOUBLES_EQUAL(0.626, cov(1, 1), 1e-3); + + // Predict x at time 1.0 + Chebyshev1Basis::EvaluationFunctor f(N, 1.0); + Matrix H; + double actual = f(actual_c, H); + EXPECT_DOUBLES_EQUAL(1.0, actual, 1e-9); + + // Calculate predictive variance on prediction + double actual_variance_on_prediction = (H * cov * H.transpose())(0); + EXPECT_DOUBLES_EQUAL(1.1494, actual_variance_on_prediction, 1e-4); +} + +//****************************************************************************** +TEST(Chebyshev, Decomposition) { + const size_t M = 16; + + // Create example sequence + Sequence sequence; + for (size_t i = 0; i < M; i++) { + double x = ((double)i / M); // - 0.99; + double y = x; + sequence[x] = y; + } + + // Do Chebyshev Decomposition + FitBasis actual(sequence, model, N); + + // Check + Vector expected = Vector::Zero(N); + expected(1) = 1; + EXPECT(assert_equal(expected, (Vector)actual.parameters(), 1e-4)); +} + +//****************************************************************************** +TEST(Chebyshev1, Derivative) { + Vector c(N); + c << 12, 3, 2; + + Weights D; + + double x = -1.0; + D = Chebyshev1Basis::DerivativeWeights(N, x); + // regression + EXPECT_DOUBLES_EQUAL(-5, (D * c)(0), 1e-9); + + x = -0.5; + D = Chebyshev1Basis::DerivativeWeights(N, x); + // regression + EXPECT_DOUBLES_EQUAL(-1, (D * c)(0), 1e-9); + + x = 0.3; + D = Chebyshev1Basis::DerivativeWeights(N, x); + // regression + EXPECT_DOUBLES_EQUAL(5.4, (D * c)(0), 1e-9); +} + +//****************************************************************************** +Vector3 f(-6, 1, 0.5); + +double proxy1(double x, size_t N) { + return Chebyshev1Basis::EvaluationFunctor(N, x)(Vector(f)); +} + +TEST(Chebyshev1, Derivative2) { + const double x = 0.5; + auto D = Chebyshev1Basis::DerivativeWeights(N, x); + + Matrix numeric_dTdx = + numericalDerivative21(proxy1, x, N); + // regression + EXPECT_DOUBLES_EQUAL(2, numeric_dTdx(0, 0), 1e-9); + EXPECT_DOUBLES_EQUAL(2, (D * f)(0), 1e-9); +} + +//****************************************************************************** +TEST(Chebyshev2, Derivative) { + Vector c(N); + c << 12, 6, 2; + + Weights D; + + double x = -1.0; + CHECK_EXCEPTION(Chebyshev2Basis::DerivativeWeights(N, x), std::runtime_error); + x = 1.0; + CHECK_EXCEPTION(Chebyshev2Basis::DerivativeWeights(N, x), std::runtime_error); + + x = -0.5; + D = Chebyshev2Basis::DerivativeWeights(N, x); + // regression + EXPECT_DOUBLES_EQUAL(4, (D * c)(0), 1e-9); + + x = 0.3; + D = Chebyshev2Basis::DerivativeWeights(N, x); + // regression + EXPECT_DOUBLES_EQUAL(16.8, (D * c)(0), 1e-9); + + x = 0.75; + D = Chebyshev2Basis::DerivativeWeights(N, x); + // regression + EXPECT_DOUBLES_EQUAL(24, (D * c)(0), 1e-9); + + x = 10; + D = Chebyshev2Basis::DerivativeWeights(N, x, 0, 20); + // regression + EXPECT_DOUBLES_EQUAL(12, (D * c)(0), 1e-9); +} + +//****************************************************************************** +double proxy2(double x, size_t N) { + return Chebyshev2Basis::EvaluationFunctor(N, x)(Vector(f)); +} + +TEST(Chebyshev2, Derivative2) { + const double x = 0.5; + auto D = Chebyshev2Basis::DerivativeWeights(N, x); + + Matrix numeric_dTdx = + numericalDerivative21(proxy2, x, N); + // regression + EXPECT_DOUBLES_EQUAL(4, numeric_dTdx(0, 0), 1e-9); + EXPECT_DOUBLES_EQUAL(4, (D * f)(0), 1e-9); +} + +//****************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//****************************************************************************** diff --git a/gtsam/basis/tests/testChebyshev2.cpp b/gtsam/basis/tests/testChebyshev2.cpp new file mode 100644 index 000000000..4cee70daf --- /dev/null +++ b/gtsam/basis/tests/testChebyshev2.cpp @@ -0,0 +1,435 @@ +/* ---------------------------------------------------------------------------- + + * 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 testChebyshev.cpp + * @date July 4, 2020 + * @author Varun Agrawal + * @brief Unit tests for Chebyshev Basis Decompositions via pseudo-spectral + * methods + */ + +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; +using namespace boost::placeholders; + +noiseModel::Diagonal::shared_ptr model = noiseModel::Unit::Create(1); + +const size_t N = 32; + +//****************************************************************************** +TEST(Chebyshev2, Point) { + static const int N = 5; + auto points = Chebyshev2::Points(N); + Vector expected(N); + expected << -1., -sqrt(2.) / 2., 0., sqrt(2.) / 2., 1.; + static const double tol = 1e-15; // changing this reveals errors + EXPECT_DOUBLES_EQUAL(expected(0), points(0), tol); + EXPECT_DOUBLES_EQUAL(expected(1), points(1), tol); + EXPECT_DOUBLES_EQUAL(expected(2), points(2), tol); + EXPECT_DOUBLES_EQUAL(expected(3), points(3), tol); + EXPECT_DOUBLES_EQUAL(expected(4), points(4), tol); + + // Check symmetry + EXPECT_DOUBLES_EQUAL(Chebyshev2::Point(N, 0), -Chebyshev2::Point(N, 4), tol); + EXPECT_DOUBLES_EQUAL(Chebyshev2::Point(N, 1), -Chebyshev2::Point(N, 3), tol); +} + +//****************************************************************************** +TEST(Chebyshev2, PointInInterval) { + static const int N = 5; + auto points = Chebyshev2::Points(N, 0, 20); + Vector expected(N); + expected << 0., 1. - sqrt(2.) / 2., 1., 1. + sqrt(2.) / 2., 2.; + expected *= 10.0; + static const double tol = 1e-15; // changing this reveals errors + EXPECT_DOUBLES_EQUAL(expected(0), points(0), tol); + EXPECT_DOUBLES_EQUAL(expected(1), points(1), tol); + EXPECT_DOUBLES_EQUAL(expected(2), points(2), tol); + EXPECT_DOUBLES_EQUAL(expected(3), points(3), tol); + EXPECT_DOUBLES_EQUAL(expected(4), points(4), tol); + + // all at once + Vector actual = Chebyshev2::Points(N, 0, 20); + CHECK(assert_equal(expected, actual)); +} + +//****************************************************************************** +// InterpolatingPolynomial[{{-1, 4}, {0, 2}, {1, 6}}, 0.5] +TEST(Chebyshev2, Interpolate2) { + size_t N = 3; + Chebyshev2::EvaluationFunctor fx(N, 0.5); + Vector f(N); + f << 4, 2, 6; + EXPECT_DOUBLES_EQUAL(3.25, fx(f), 1e-9); +} + +//****************************************************************************** +// InterpolatingPolynomial[{{0, 4}, {1, 2}, {2, 6}}, 1.5] +TEST(Chebyshev2, Interpolate2_Interval) { + Chebyshev2::EvaluationFunctor fx(3, 1.5, 0, 2); + Vector3 f(4, 2, 6); + EXPECT_DOUBLES_EQUAL(3.25, fx(f), 1e-9); +} + +//****************************************************************************** +// InterpolatingPolynomial[{{-1, 4}, {-Sqrt[2]/2, 2}, {0, 6}, {Sqrt[2]/2,3}, {1, +// 3}}, 0.5] +TEST(Chebyshev2, Interpolate5) { + Chebyshev2::EvaluationFunctor fx(5, 0.5); + Vector f(5); + f << 4, 2, 6, 3, 3; + EXPECT_DOUBLES_EQUAL(4.34283, fx(f), 1e-5); +} + +//****************************************************************************** +// Interpolating vectors +TEST(Chebyshev2, InterpolateVector) { + double t = 30, a = 0, b = 100; + const size_t N = 3; + // Create 2x3 matrix with Vectors at Chebyshev points + ParameterMatrix<2> X(N); + X.row(0) = Chebyshev2::Points(N, a, b); // slope 1 ramp + + // Check value + Vector expected(2); + expected << t, 0; + Eigen::Matrix actualH(2, 2 * N); + + Chebyshev2::VectorEvaluationFunctor<2> fx(N, t, a, b); + EXPECT(assert_equal(expected, fx(X, actualH), 1e-9)); + + // Check derivative + boost::function)> f = boost::bind( + &Chebyshev2::VectorEvaluationFunctor<2>::operator(), fx, _1, boost::none); + Matrix numericalH = + numericalDerivative11, 2 * N>(f, X); + EXPECT(assert_equal(numericalH, actualH, 1e-9)); +} + +//****************************************************************************** +TEST(Chebyshev2, Decomposition) { + // Create example sequence + Sequence sequence; + for (size_t i = 0; i < 16; i++) { + double x = (double)i / 16. - 0.99, y = x; + sequence[x] = y; + } + + // Do Chebyshev Decomposition + FitBasis actual(sequence, model, 3); + + // Check + Vector expected(3); + expected << -1, 0, 1; + EXPECT(assert_equal(expected, actual.parameters(), 1e-4)); +} + +//****************************************************************************** +TEST(Chebyshev2, DifferentiationMatrix3) { + // Trefethen00book, p.55 + const size_t N = 3; + Matrix expected(N, N); + // Differentiation matrix computed from Chebfun + expected << 1.5000, -2.0000, 0.5000, // + 0.5000, -0.0000, -0.5000, // + -0.5000, 2.0000, -1.5000; + // multiply by -1 since the cheb points have a phase shift wrt Trefethen + // This was verified with chebfun + expected = -expected; + + Matrix actual = Chebyshev2::DifferentiationMatrix(N); + EXPECT(assert_equal(expected, actual, 1e-4)); +} + +//****************************************************************************** +TEST(Chebyshev2, DerivativeMatrix6) { + // Trefethen00book, p.55 + const size_t N = 6; + Matrix expected(N, N); + expected << 8.5000, -10.4721, 2.8944, -1.5279, 1.1056, -0.5000, // + 2.6180, -1.1708, -2.0000, 0.8944, -0.6180, 0.2764, // + -0.7236, 2.0000, -0.1708, -1.6180, 0.8944, -0.3820, // + 0.3820, -0.8944, 1.6180, 0.1708, -2.0000, 0.7236, // + -0.2764, 0.6180, -0.8944, 2.0000, 1.1708, -2.6180, // + 0.5000, -1.1056, 1.5279, -2.8944, 10.4721, -8.5000; + // multiply by -1 since the cheb points have a phase shift wrt Trefethen + // This was verified with chebfun + expected = -expected; + + Matrix actual = Chebyshev2::DifferentiationMatrix(N); + EXPECT(assert_equal((Matrix)expected, actual, 1e-4)); +} + +// test function for CalculateWeights and DerivativeWeights +double f(double x) { + // return 3*(x**3) - 2*(x**2) + 5*x - 11 + return 3.0 * pow(x, 3) - 2.0 * pow(x, 2) + 5.0 * x - 11; +} + +// its derivative +double fprime(double x) { + // return 9*(x**2) - 4*(x) + 5 + return 9.0 * pow(x, 2) - 4.0 * x + 5.0; +} + +//****************************************************************************** +TEST(Chebyshev2, CalculateWeights) { + Eigen::Matrix fvals(N); + for (size_t i = 0; i < N; i++) { + fvals(i) = f(Chebyshev2::Point(N, i)); + } + double x1 = 0.7, x2 = -0.376; + Weights weights1 = Chebyshev2::CalculateWeights(N, x1); + Weights weights2 = Chebyshev2::CalculateWeights(N, x2); + EXPECT_DOUBLES_EQUAL(f(x1), weights1 * fvals, 1e-8); + EXPECT_DOUBLES_EQUAL(f(x2), weights2 * fvals, 1e-8); +} + +TEST(Chebyshev2, CalculateWeights2) { + double a = 0, b = 10, x1 = 7, x2 = 4.12; + + Eigen::Matrix fvals(N); + for (size_t i = 0; i < N; i++) { + fvals(i) = f(Chebyshev2::Point(N, i, a, b)); + } + + Weights weights1 = Chebyshev2::CalculateWeights(N, x1, a, b); + EXPECT_DOUBLES_EQUAL(f(x1), weights1 * fvals, 1e-8); + + Weights weights2 = Chebyshev2::CalculateWeights(N, x2, a, b); + double expected2 = f(x2); // 185.454784 + double actual2 = weights2 * fvals; + EXPECT_DOUBLES_EQUAL(expected2, actual2, 1e-8); +} + +TEST(Chebyshev2, DerivativeWeights) { + Eigen::Matrix fvals(N); + for (size_t i = 0; i < N; i++) { + fvals(i) = f(Chebyshev2::Point(N, i)); + } + double x1 = 0.7, x2 = -0.376, x3 = 0.0; + Weights dWeights1 = Chebyshev2::DerivativeWeights(N, x1); + EXPECT_DOUBLES_EQUAL(fprime(x1), dWeights1 * fvals, 1e-9); + + Weights dWeights2 = Chebyshev2::DerivativeWeights(N, x2); + EXPECT_DOUBLES_EQUAL(fprime(x2), dWeights2 * fvals, 1e-9); + + Weights dWeights3 = Chebyshev2::DerivativeWeights(N, x3); + EXPECT_DOUBLES_EQUAL(fprime(x3), dWeights3 * fvals, 1e-9); + + // test if derivative calculation and cheb point is correct + double x4 = Chebyshev2::Point(N, 3); + Weights dWeights4 = Chebyshev2::DerivativeWeights(N, x4); + EXPECT_DOUBLES_EQUAL(fprime(x4), dWeights4 * fvals, 1e-9); +} + +TEST(Chebyshev2, DerivativeWeights2) { + double x1 = 5, x2 = 4.12, a = 0, b = 10; + + Eigen::Matrix fvals(N); + for (size_t i = 0; i < N; i++) { + fvals(i) = f(Chebyshev2::Point(N, i, a, b)); + } + + Weights dWeights1 = Chebyshev2::DerivativeWeights(N, x1, a, b); + EXPECT_DOUBLES_EQUAL(fprime(x1), dWeights1 * fvals, 1e-8); + + Weights dWeights2 = Chebyshev2::DerivativeWeights(N, x2, a, b); + EXPECT_DOUBLES_EQUAL(fprime(x2), dWeights2 * fvals, 1e-8); + + // test if derivative calculation and cheb point is correct + double x3 = Chebyshev2::Point(N, 3, a, b); + Weights dWeights3 = Chebyshev2::DerivativeWeights(N, x3, a, b); + EXPECT_DOUBLES_EQUAL(fprime(x3), dWeights3 * fvals, 1e-8); +} + +//****************************************************************************** +// Check two different ways to calculate the derivative weights +TEST(Chebyshev2, DerivativeWeightsDifferentiationMatrix) { + const size_t N6 = 6; + double x1 = 0.311; + Matrix D6 = Chebyshev2::DifferentiationMatrix(N6); + Weights expected = Chebyshev2::CalculateWeights(N6, x1) * D6; + Weights actual = Chebyshev2::DerivativeWeights(N6, x1); + EXPECT(assert_equal(expected, actual, 1e-12)); + + double a = -3, b = 8, x2 = 5.05; + Matrix D6_2 = Chebyshev2::DifferentiationMatrix(N6, a, b); + Weights expected1 = Chebyshev2::CalculateWeights(N6, x2, a, b) * D6_2; + Weights actual1 = Chebyshev2::DerivativeWeights(N6, x2, a, b); + EXPECT(assert_equal(expected1, actual1, 1e-12)); +} + +//****************************************************************************** +// Check two different ways to calculate the derivative weights +TEST(Chebyshev2, DerivativeWeights6) { + const size_t N6 = 6; + Matrix D6 = Chebyshev2::DifferentiationMatrix(N6); + Chebyshev2::Parameters x = Chebyshev2::Points(N6); // ramp with slope 1 + EXPECT(assert_equal(Vector::Ones(N6), Vector(D6 * x))); +} + +//****************************************************************************** +// Check two different ways to calculate the derivative weights +TEST(Chebyshev2, DerivativeWeights7) { + const size_t N7 = 7; + Matrix D7 = Chebyshev2::DifferentiationMatrix(N7); + Chebyshev2::Parameters x = Chebyshev2::Points(N7); // ramp with slope 1 + EXPECT(assert_equal(Vector::Ones(N7), Vector(D7 * x))); +} + +//****************************************************************************** +// Check derivative in two different ways: numerical and using D on f +Vector6 f3_at_6points = (Vector6() << 4, 2, 6, 2, 4, 3).finished(); +double proxy3(double x) { + return Chebyshev2::EvaluationFunctor(6, x)(f3_at_6points); +} + +TEST(Chebyshev2, Derivative6) { + // Check Derivative evaluation at point x=0.2 + + // calculate expected values by numerical derivative of synthesis + const double x = 0.2; + Matrix numeric_dTdx = numericalDerivative11(proxy3, x); + + // Calculate derivatives at Chebyshev points using D3, interpolate + Matrix D6 = Chebyshev2::DifferentiationMatrix(6); + Vector derivative_at_points = D6 * f3_at_6points; + Chebyshev2::EvaluationFunctor fx(6, x); + EXPECT_DOUBLES_EQUAL(numeric_dTdx(0, 0), fx(derivative_at_points), 1e-8); + + // Do directly + Chebyshev2::DerivativeFunctor dfdx(6, x); + EXPECT_DOUBLES_EQUAL(numeric_dTdx(0, 0), dfdx(f3_at_6points), 1e-8); +} + +//****************************************************************************** +// Assert that derivative also works in non-standard interval [0,3] +double proxy4(double x) { + return Chebyshev2::EvaluationFunctor(6, x, 0, 3)(f3_at_6points); +} + +TEST(Chebyshev2, Derivative6_03) { + // Check Derivative evaluation at point x=0.2, in interval [0,3] + + // calculate expected values by numerical derivative of synthesis + const double x = 0.2; + Matrix numeric_dTdx = numericalDerivative11(proxy4, x); + + // Calculate derivatives at Chebyshev points using D3, interpolate + Matrix D6 = Chebyshev2::DifferentiationMatrix(6, 0, 3); + Vector derivative_at_points = D6 * f3_at_6points; + Chebyshev2::EvaluationFunctor fx(6, x, 0, 3); + EXPECT_DOUBLES_EQUAL(numeric_dTdx(0, 0), fx(derivative_at_points), 1e-8); + + // Do directly + Chebyshev2::DerivativeFunctor dfdx(6, x, 0, 3); + EXPECT_DOUBLES_EQUAL(numeric_dTdx(0, 0), dfdx(f3_at_6points), 1e-8); +} + +//****************************************************************************** +// Test VectorDerivativeFunctor +TEST(Chebyshev2, VectorDerivativeFunctor) { + const size_t N = 3, M = 2; + const double x = 0.2; + using VecD = Chebyshev2::VectorDerivativeFunctor; + VecD fx(N, x, 0, 3); + ParameterMatrix X(N); + Matrix actualH(M, M * N); + EXPECT(assert_equal(Vector::Zero(M), (Vector)fx(X, actualH), 1e-8)); + + // Test Jacobian + Matrix expectedH = numericalDerivative11, M * N>( + boost::bind(&VecD::operator(), fx, _1, boost::none), X); + EXPECT(assert_equal(expectedH, actualH, 1e-7)); +} + +//****************************************************************************** +// Test VectorDerivativeFunctor with polynomial function +TEST(Chebyshev2, VectorDerivativeFunctor2) { + const size_t N = 64, M = 1, T = 15; + using VecD = Chebyshev2::VectorDerivativeFunctor; + + const Vector points = Chebyshev2::Points(N, 0, T); + + // Assign the parameter matrix + Vector values(N); + for (size_t i = 0; i < N; ++i) { + values(i) = f(points(i)); + } + ParameterMatrix X(values); + + // Evaluate the derivative at the chebyshev points using + // VectorDerivativeFunctor. + for (size_t i = 0; i < N; ++i) { + VecD d(N, points(i), 0, T); + Vector1 Dx = d(X); + EXPECT_DOUBLES_EQUAL(fprime(points(i)), Dx(0), 1e-6); + } + + // Test Jacobian at the first chebyshev point. + Matrix actualH(M, M * N); + VecD vecd(N, points(0), 0, T); + vecd(X, actualH); + Matrix expectedH = numericalDerivative11, M * N>( + boost::bind(&VecD::operator(), vecd, _1, boost::none), X); + EXPECT(assert_equal(expectedH, actualH, 1e-6)); +} + +//****************************************************************************** +// Test ComponentDerivativeFunctor +TEST(Chebyshev2, ComponentDerivativeFunctor) { + const size_t N = 6, M = 2; + const double x = 0.2; + using CompFunc = Chebyshev2::ComponentDerivativeFunctor; + size_t row = 1; + CompFunc fx(N, row, x, 0, 3); + ParameterMatrix X(N); + Matrix actualH(1, M * N); + EXPECT_DOUBLES_EQUAL(0, fx(X, actualH), 1e-8); + + Matrix expectedH = numericalDerivative11, M * N>( + boost::bind(&CompFunc::operator(), fx, _1, boost::none), X); + EXPECT(assert_equal(expectedH, actualH, 1e-7)); +} + +//****************************************************************************** +TEST(Chebyshev2, IntegralWeights) { + const size_t N7 = 7; + Vector actual = Chebyshev2::IntegrationWeights(N7); + Vector expected = (Vector(N7) << 0.0285714285714286, 0.253968253968254, + 0.457142857142857, 0.520634920634921, 0.457142857142857, + 0.253968253968254, 0.0285714285714286) + .finished(); + EXPECT(assert_equal(expected, actual)); + + const size_t N8 = 8; + Vector actual2 = Chebyshev2::IntegrationWeights(N8); + Vector expected2 = (Vector(N8) << 0.0204081632653061, 0.190141007218208, + 0.352242423718159, 0.437208405798326, 0.437208405798326, + 0.352242423718159, 0.190141007218208, 0.0204081632653061) + .finished(); + EXPECT(assert_equal(expected2, actual2)); +} + +//****************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//****************************************************************************** diff --git a/gtsam/basis/tests/testFourier.cpp b/gtsam/basis/tests/testFourier.cpp new file mode 100644 index 000000000..7a53cfcc9 --- /dev/null +++ b/gtsam/basis/tests/testFourier.cpp @@ -0,0 +1,254 @@ +/* ---------------------------------------------------------------------------- + + * 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 testFourier.cpp + * @date July 4, 2020 + * @author Frank Dellaert, Varun Agrawal + * @brief Unit tests for Fourier Basis Decompositions with Expressions + */ + +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +auto model = noiseModel::Unit::Create(1); + +// Coefficients for testing, respectively 3 and 7 parameter Fourier basis. +// They correspond to best approximation of TestFunction(x) +const Vector k3Coefficients = (Vector3() << 1.5661, 1.2717, 1.2717).finished(); +const Vector7 k7Coefficients = + (Vector7() << 1.5661, 1.2717, 1.2717, -0.0000, 0.5887, -0.0943, 0.0943) + .finished(); + +// The test-function used below +static double TestFunction(double x) { return exp(sin(x) + cos(x)); } + +//****************************************************************************** +TEST(Basis, BasisEvaluationFunctor) { + // fx(0) takes coefficients c to calculate the value f(c;x==0) + FourierBasis::EvaluationFunctor fx(3, 0); + EXPECT_DOUBLES_EQUAL(k3Coefficients[0] + k3Coefficients[1], + fx(k3Coefficients), 1e-9); +} + +//****************************************************************************** +TEST(Basis, BasisEvaluationFunctorDerivative) { + // If we give the H argument, we get the derivative of fx(0) wrpt coefficients + // Needs to be Matrix so it can be used by OptionalJacobian. + Matrix H(1, 3); + FourierBasis::EvaluationFunctor fx(3, 0); + EXPECT_DOUBLES_EQUAL(k3Coefficients[0] + k3Coefficients[1], + fx(k3Coefficients, H), 1e-9); + + Matrix13 expectedH(1, 1, 0); + EXPECT(assert_equal(expectedH, H)); +} + +//****************************************************************************** +TEST(Basis, Manual) { + const size_t N = 3; + + // We will create a linear factor graph + GaussianFactorGraph graph; + + // We create an unknown Vector expression for the coefficients + Key key(1); + + // We will need values below to test the Jacobians + Values values; + values.insert(key, Vector::Zero(N)); // value does not really matter + + // At 16 different samples points x, check Predict_ expression + for (size_t i = 0; i < 16; i++) { + const double x = i * M_PI / 8; + const double desiredValue = TestFunction(x); + + // Manual JacobianFactor + Matrix A(1, N); + A << 1, cos(x), sin(x); + Vector b(1); + b << desiredValue; + JacobianFactor linearFactor(key, A, b); + graph.add(linearFactor); + + // Create factor to predict value at x + EvaluationFactor predictFactor(key, desiredValue, model, N, + x); + + // Check expression Jacobians + EXPECT_CORRECT_FACTOR_JACOBIANS(predictFactor, values, 1e-5, 1e-9); + + auto linearizedFactor = predictFactor.linearize(values); + auto linearizedJacobianFactor = + boost::dynamic_pointer_cast(linearizedFactor); + CHECK(linearizedJacobianFactor); // makes sure it's indeed a JacobianFactor + EXPECT(assert_equal(linearFactor, *linearizedJacobianFactor, 1e-9)); + } + + // Solve linear graph + VectorValues actual = graph.optimize(); + EXPECT(assert_equal((Vector)k3Coefficients, actual.at(key), 1e-4)); +} + +//****************************************************************************** +TEST(Basis, EvaluationFactor) { + // Check fitting a function with a 7-parameter Fourier basis + + // Create linear factor graph + NonlinearFactorGraph graph; + Key key(1); + for (size_t i = 0; i < 16; i++) { + double x = i * M_PI / 8, desiredValue = TestFunction(x); + graph.emplace_shared>(key, desiredValue, + model, 7, x); + } + + // Solve FourierFactorGraph + Values values; + values.insert(key, Vector::Zero(7)); + GaussianFactorGraph::shared_ptr lfg = graph.linearize(values); + VectorValues actual = lfg->optimize(); + + EXPECT(assert_equal((Vector)k7Coefficients, actual.at(key), 1e-4)); +} + +//****************************************************************************** +TEST(Basis, WeightMatrix) { + // The WeightMatrix creates an m*n matrix, where m is the number of sample + // points, and n is the number of parameters. + Matrix expected(2, 3); + expected.row(0) << 1, cos(1), sin(1); + expected.row(1) << 1, cos(2), sin(2); + Vector2 X(1, 2); + Matrix actual = FourierBasis::WeightMatrix(3, X); + EXPECT(assert_equal(expected, actual, 1e-9)); +} + +//****************************************************************************** +TEST(Basis, Decomposition) { + // Create example sequence + Sequence sequence; + for (size_t i = 0; i < 16; i++) { + double x = i * M_PI / 8, y = TestFunction(x); + sequence[x] = y; + } + + // Do Fourier Decomposition + FitBasis actual(sequence, model, 3); + + // Check + EXPECT(assert_equal((Vector)k3Coefficients, actual.parameters(), 1e-4)); +} + +//****************************************************************************** +// Check derivative in two different ways: numerical and using D on f +double proxy(double x) { + return FourierBasis::EvaluationFunctor(7, x)(k7Coefficients); +} + +TEST(Basis, Derivative7) { + // Check Derivative evaluation at point x=0.2 + + // Calculate expected values by numerical derivative of proxy. + const double x = 0.2; + Matrix numeric_dTdx = numericalDerivative11(proxy, x); + + // Calculate derivatives at Chebyshev points using D7, interpolate + Matrix D7 = FourierBasis::DifferentiationMatrix(7); + Vector derivativeCoefficients = D7 * k7Coefficients; + FourierBasis::EvaluationFunctor fx(7, x); + EXPECT_DOUBLES_EQUAL(numeric_dTdx(0, 0), fx(derivativeCoefficients), 1e-8); + + // Do directly + FourierBasis::DerivativeFunctor dfdx(7, x); + EXPECT_DOUBLES_EQUAL(numeric_dTdx(0, 0), dfdx(k7Coefficients), 1e-8); +} + +//****************************************************************************** +TEST(Basis, VecDerivativeFunctor) { + using DotShape = typename FourierBasis::VectorDerivativeFunctor<2>; + const size_t N = 3; + + // MATLAB example, Dec 27 2019, commit 014eded5 + double h = 2 * M_PI / 16; + Vector2 dotShape(0.5556, -0.8315); // at h/2 + DotShape dotShapeFunction(N, h / 2); + Matrix23 theta_mat = (Matrix32() << 0, 0, 0.7071, 0.7071, 0.7071, -0.7071) + .finished() + .transpose(); + ParameterMatrix<2> theta(theta_mat); + EXPECT(assert_equal(Vector(dotShape), dotShapeFunction(theta), 1e-4)); +} + +//****************************************************************************** +// Suppose we want to parameterize a periodic function with function values at +// specific times, rather than coefficients. Can we do it? This would be a +// generalization of the Fourier transform, and constitute a "pseudo-spectral" +// parameterization. One way to do this is to establish hard constraints that +// express the relationship between the new parameters and the coefficients. +// For example, below I'd like the parameters to be the function values at +// X = {0.1,0.2,0.3}, rather than a 3-vector of coefficients. +// Because the values f(X) = at these points can be written as f(X) = W(X)*c, +// we can simply express the coefficents c as c=inv(W(X))*f, which is a +// generalized Fourier transform. That also means we can create factors with the +// unknown f-values, as done manually below. +TEST(Basis, PseudoSpectral) { + // We will create a linear factor graph + GaussianFactorGraph graph; + + const size_t N = 3; + const Key key(1); + + // The correct values at X = {0.1,0.2,0.3} are simply W*c + const Vector X = (Vector3() << 0.1, 0.2, 0.3).finished(); + const Matrix W = FourierBasis::WeightMatrix(N, X); + const Vector expected = W * k3Coefficients; + + // Check those values are indeed correct values of Fourier approximation + using Eval = FourierBasis::EvaluationFunctor; + EXPECT_DOUBLES_EQUAL(Eval(N, 0.1)(k3Coefficients), expected(0), 1e-9); + EXPECT_DOUBLES_EQUAL(Eval(N, 0.2)(k3Coefficients), expected(1), 1e-9); + EXPECT_DOUBLES_EQUAL(Eval(N, 0.3)(k3Coefficients), expected(2), 1e-9); + + // Calculate "inverse Fourier transform" matrix + const Matrix invW = W.inverse(); + + // At 16 different samples points x, add a factor on fExpr + for (size_t i = 0; i < 16; i++) { + const double x = i * M_PI / 8; + const double desiredValue = TestFunction(x); + + // Manual JacobianFactor + Matrix A(1, 3); + A << 1, cos(x), sin(x); + Vector b(1); + b << desiredValue; + JacobianFactor linearFactor(key, A * invW, b); + graph.add(linearFactor); + } + + // Solve linear graph + VectorValues actual = graph.optimize(); + EXPECT(assert_equal((Vector)expected, actual.at(key), 1e-4)); +} + +//****************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//****************************************************************************** diff --git a/gtsam/basis/tests/testParameterMatrix.cpp b/gtsam/basis/tests/testParameterMatrix.cpp new file mode 100644 index 000000000..ec62e8eea --- /dev/null +++ b/gtsam/basis/tests/testParameterMatrix.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 testParameterMatrix.cpp + * @date Sep 22, 2020 + * @author Varun Agrawal, Frank Dellaert + * @brief Unit tests for ParameterMatrix.h + */ + +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +using Parameters = Chebyshev2::Parameters; + +const size_t M = 2, N = 5; + +//****************************************************************************** +TEST(ParameterMatrix, Constructor) { + ParameterMatrix<2> actual1(3); + ParameterMatrix<2> expected1(Matrix::Zero(2, 3)); + EXPECT(assert_equal(expected1, actual1)); + + ParameterMatrix<2> actual2(Matrix::Ones(2, 3)); + ParameterMatrix<2> expected2(Matrix::Ones(2, 3)); + EXPECT(assert_equal(expected2, actual2)); + EXPECT(assert_equal(expected2.matrix(), actual2.matrix())); +} + +//****************************************************************************** +TEST(ParameterMatrix, Dimensions) { + ParameterMatrix params(N); + EXPECT_LONGS_EQUAL(params.rows(), M); + EXPECT_LONGS_EQUAL(params.cols(), N); + EXPECT_LONGS_EQUAL(params.dim(), M * N); +} + +//****************************************************************************** +TEST(ParameterMatrix, Getters) { + ParameterMatrix params(N); + + Matrix expectedMatrix = Matrix::Zero(2, 5); + EXPECT(assert_equal(expectedMatrix, params.matrix())); + + Matrix expectedMatrixTranspose = Matrix::Zero(5, 2); + EXPECT(assert_equal(expectedMatrixTranspose, params.transpose())); + + ParameterMatrix p2(Matrix::Ones(M, N)); + Vector expectedRowVector = Vector::Ones(N); + for (size_t r = 0; r < M; ++r) { + EXPECT(assert_equal(p2.row(r), expectedRowVector)); + } + + ParameterMatrix p3(2 * Matrix::Ones(M, N)); + Vector expectedColVector = 2 * Vector::Ones(M); + for (size_t c = 0; c < M; ++c) { + EXPECT(assert_equal(p3.col(c), expectedColVector)); + } +} + +//****************************************************************************** +TEST(ParameterMatrix, Setters) { + ParameterMatrix params(Matrix::Zero(M, N)); + Matrix expected = Matrix::Zero(M, N); + + // row + params.row(0) = Vector::Ones(N); + expected.row(0) = Vector::Ones(N); + EXPECT(assert_equal(expected, params.matrix())); + + // col + params.col(2) = Vector::Ones(M); + expected.col(2) = Vector::Ones(M); + + EXPECT(assert_equal(expected, params.matrix())); + + // setZero + params.setZero(); + expected.setZero(); + EXPECT(assert_equal(expected, params.matrix())); +} + +//****************************************************************************** +TEST(ParameterMatrix, Addition) { + ParameterMatrix params(Matrix::Ones(M, N)); + ParameterMatrix expected(2 * Matrix::Ones(M, N)); + + // Add vector + EXPECT(assert_equal(expected, params + Vector::Ones(M * N))); + // Add another ParameterMatrix + ParameterMatrix actual = params + ParameterMatrix(Matrix::Ones(M, N)); + EXPECT(assert_equal(expected, actual)); +} + +//****************************************************************************** +TEST(ParameterMatrix, Subtraction) { + ParameterMatrix params(2 * Matrix::Ones(M, N)); + ParameterMatrix expected(Matrix::Ones(M, N)); + + // Subtract vector + EXPECT(assert_equal(expected, params - Vector::Ones(M * N))); + // Subtract another ParameterMatrix + ParameterMatrix actual = params - ParameterMatrix(Matrix::Ones(M, N)); + EXPECT(assert_equal(expected, actual)); +} + +//****************************************************************************** +TEST(ParameterMatrix, Multiplication) { + ParameterMatrix params(Matrix::Ones(M, N)); + Matrix multiplier = 2 * Matrix::Ones(N, 2); + Matrix expected = 2 * N * Matrix::Ones(M, 2); + EXPECT(assert_equal(expected, params * multiplier)); +} + +//****************************************************************************** +TEST(ParameterMatrix, VectorSpace) { + ParameterMatrix params(Matrix::Ones(M, N)); + // vector + EXPECT(assert_equal(Vector::Ones(M * N), params.vector())); + // identity + EXPECT(assert_equal(ParameterMatrix::identity(), + ParameterMatrix(Matrix::Zero(M, 0)))); +} + +//****************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//****************************************************************************** diff --git a/gtsam/nonlinear/FunctorizedFactor.h b/gtsam/nonlinear/FunctorizedFactor.h index 691ab8ac8..e1f8ece8d 100644 --- a/gtsam/nonlinear/FunctorizedFactor.h +++ b/gtsam/nonlinear/FunctorizedFactor.h @@ -110,7 +110,7 @@ class GTSAM_EXPORT FunctorizedFactor : public NoiseModelFactor1 { bool equals(const NonlinearFactor &other, double tol = 1e-9) const override { const FunctorizedFactor *e = dynamic_cast *>(&other); - return e && Base::equals(other, tol) && + return e != nullptr && Base::equals(other, tol) && traits::Equals(this->measured_, e->measured_, tol); } /// @} diff --git a/gtsam/nonlinear/tests/testFunctorizedFactor.cpp b/gtsam/nonlinear/tests/testFunctorizedFactor.cpp index b0ec5e722..14a14fc19 100644 --- a/gtsam/nonlinear/tests/testFunctorizedFactor.cpp +++ b/gtsam/nonlinear/tests/testFunctorizedFactor.cpp @@ -20,8 +20,12 @@ #include #include #include +#include +#include +#include #include #include +#include #include using namespace std; @@ -60,7 +64,7 @@ class ProjectionFunctor { if (H1) { H1->resize(x.size(), A.size()); *H1 << I_3x3, I_3x3, I_3x3; - } + } if (H2) *H2 = A; return A * x; } @@ -255,18 +259,148 @@ TEST(FunctorizedFactor, Lambda2) { if (H1) { H1->resize(x.size(), A.size()); *H1 << I_3x3, I_3x3, I_3x3; - } + } if (H2) *H2 = A; return A * x; }; // FunctorizedFactor factor(key, measurement, model, lambda); - auto factor = MakeFunctorizedFactor2(keyA, keyx, measurement, model2, lambda); + auto factor = MakeFunctorizedFactor2(keyA, keyx, measurement, + model2, lambda); Vector error = factor.evaluateError(A, x); EXPECT(assert_equal(Vector::Zero(3), error, 1e-9)); } +const size_t N = 2; + +//****************************************************************************** +TEST(FunctorizedFactor, Print2) { + const size_t M = 1; + + Vector measured = Vector::Ones(M) * 42; + + auto model = noiseModel::Isotropic::Sigma(M, 1.0); + VectorEvaluationFactor priorFactor(key, measured, model, N, 0); + + string expected = + " keys = { X0 }\n" + " noise model: unit (1) \n" + "FunctorizedFactor(X0)\n" + " measurement: [\n" + " 42\n" + "]\n" + " noise model sigmas: 1\n"; + + EXPECT(assert_print_equal(expected, priorFactor)); +} + +//****************************************************************************** +TEST(FunctorizedFactor, VectorEvaluationFactor) { + const size_t M = 4; + + Vector measured = Vector::Zero(M); + + auto model = noiseModel::Isotropic::Sigma(M, 1.0); + VectorEvaluationFactor priorFactor(key, measured, model, N, 0); + + NonlinearFactorGraph graph; + graph.add(priorFactor); + + ParameterMatrix stateMatrix(N); + + Values initial; + initial.insert>(key, stateMatrix); + + LevenbergMarquardtParams parameters; + parameters.verbosity = NonlinearOptimizerParams::SILENT; + parameters.verbosityLM = LevenbergMarquardtParams::SILENT; + parameters.setMaxIterations(20); + Values result = + LevenbergMarquardtOptimizer(graph, initial, parameters).optimize(); + + EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-9); +} + +//****************************************************************************** +TEST(FunctorizedFactor, VectorComponentFactor) { + const int P = 4; + const size_t i = 2; + const double measured = 0.0, t = 3.0, a = 2.0, b = 4.0; + auto model = noiseModel::Isotropic::Sigma(1, 1.0); + VectorComponentFactor controlPrior(key, measured, model, N, i, + t, a, b); + + NonlinearFactorGraph graph; + graph.add(controlPrior); + + ParameterMatrix

stateMatrix(N); + + Values initial; + initial.insert>(key, stateMatrix); + + LevenbergMarquardtParams parameters; + parameters.verbosity = NonlinearOptimizerParams::SILENT; + parameters.verbosityLM = LevenbergMarquardtParams::SILENT; + parameters.setMaxIterations(20); + Values result = + LevenbergMarquardtOptimizer(graph, initial, parameters).optimize(); + + EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-9); +} + +//****************************************************************************** +TEST(FunctorizedFactor, VecDerivativePrior) { + const size_t M = 4; + + Vector measured = Vector::Zero(M); + auto model = noiseModel::Isotropic::Sigma(M, 1.0); + VectorDerivativeFactor vecDPrior(key, measured, model, N, 0); + + NonlinearFactorGraph graph; + graph.add(vecDPrior); + + ParameterMatrix stateMatrix(N); + + Values initial; + initial.insert>(key, stateMatrix); + + LevenbergMarquardtParams parameters; + parameters.verbosity = NonlinearOptimizerParams::SILENT; + parameters.verbosityLM = LevenbergMarquardtParams::SILENT; + parameters.setMaxIterations(20); + Values result = + LevenbergMarquardtOptimizer(graph, initial, parameters).optimize(); + + EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-9); +} + +//****************************************************************************** +TEST(FunctorizedFactor, ComponentDerivativeFactor) { + const size_t M = 4; + + double measured = 0; + auto model = noiseModel::Isotropic::Sigma(1, 1.0); + ComponentDerivativeFactor controlDPrior(key, measured, model, + N, 0, 0); + + NonlinearFactorGraph graph; + graph.add(controlDPrior); + + Values initial; + ParameterMatrix stateMatrix(N); + initial.insert>(key, stateMatrix); + + LevenbergMarquardtParams parameters; + parameters.verbosity = NonlinearOptimizerParams::SILENT; + parameters.verbosityLM = LevenbergMarquardtParams::SILENT; + parameters.setMaxIterations(20); + Values result = + LevenbergMarquardtOptimizer(graph, initial, parameters).optimize(); + + EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-9); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index bdda15acb..a5fdc80a6 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -61,6 +61,7 @@ set(interface_headers ${PROJECT_SOURCE_DIR}/gtsam/slam/slam.i ${PROJECT_SOURCE_DIR}/gtsam/sfm/sfm.i ${PROJECT_SOURCE_DIR}/gtsam/navigation/navigation.i + ${PROJECT_SOURCE_DIR}/gtsam/basis/basis.i ) diff --git a/python/gtsam/preamble/basis.h b/python/gtsam/preamble/basis.h new file mode 100644 index 000000000..d07a75f6f --- /dev/null +++ b/python/gtsam/preamble/basis.h @@ -0,0 +1,12 @@ +/* Please refer to: + * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `PYBIND11_MAKE_OPAQUE` will mark the type as "opaque" for the pybind11 + * automatic STL binding, such that the raw objects can be accessed in Python. + * Without this they will be automatically converted to a Python object, and all + * mutations on Python side will not be reflected on C++. + */ diff --git a/python/gtsam/specializations/basis.h b/python/gtsam/specializations/basis.h new file mode 100644 index 000000000..da8842eaf --- /dev/null +++ b/python/gtsam/specializations/basis.h @@ -0,0 +1,12 @@ +/* Please refer to: + * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `py::bind_vector` and similar machinery gives the std container a Python-like + * interface, but without the `` copying mechanism. Combined + * with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python, + * and saves one copy operation. + */ From 15120ce9ab48db2860ea2704a504d799a95420b2 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Wed, 4 Aug 2021 15:30:35 -0400 Subject: [PATCH 176/248] python unit test for FitBasis --- python/gtsam/tests/test_basis.py | 38 ++++++++++++++++++++++++++++++++ 1 file changed, 38 insertions(+) create mode 100644 python/gtsam/tests/test_basis.py diff --git a/python/gtsam/tests/test_basis.py b/python/gtsam/tests/test_basis.py new file mode 100644 index 000000000..944bdd9f3 --- /dev/null +++ b/python/gtsam/tests/test_basis.py @@ -0,0 +1,38 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Basis unit tests. +Author: Frank Dellaert & Gerry Chen (Python) +""" +import unittest + +import numpy as np + +import gtsam +from gtsam.utils.test_case import GtsamTestCase +from gtsam.symbol_shorthand import B + +class TestBasis(GtsamTestCase): + def test_fit_basis(self): + f = lambda x: x # line y = x + N = 2 + datax = [0., 0.5, 0.75] + interpx = np.linspace(0., 1., 10) + noise = gtsam.noiseModel.Unit.Create(1) + def testBasis(fitter, basis, f=f): + data = {x: f(x) for x in datax} + fit = fitter(N, data, noise) + coeff = fit.parameters() + interpy = basis.WeightMatrix(N, interpx) @ coeff + np.testing.assert_almost_equal(interpy, np.array([f(x) for x in interpx]), decimal=7) + testBasis(gtsam.FitBasisFourierBasis, gtsam.FourierBasis, f=lambda x: 0.7*np.cos(x)) + testBasis(gtsam.FitBasisChebyshev1Basis, gtsam.Chebyshev1Basis) + testBasis(gtsam.FitBasisChebyshev2Basis, gtsam.Chebyshev2Basis) + testBasis(gtsam.FitBasisChebyshev2, gtsam.Chebyshev2) + +if __name__ == "__main__": + unittest.main() From b99bf4e92912f4ad0020f79d6b222a3d6593514f Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Fri, 27 Aug 2021 11:23:38 -0400 Subject: [PATCH 177/248] add and fix constructor argument order --- python/gtsam/preamble/basis.h | 2 ++ python/gtsam/tests/test_basis.py | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/python/gtsam/preamble/basis.h b/python/gtsam/preamble/basis.h index d07a75f6f..56a07cfdd 100644 --- a/python/gtsam/preamble/basis.h +++ b/python/gtsam/preamble/basis.h @@ -10,3 +10,5 @@ * Without this they will be automatically converted to a Python object, and all * mutations on Python side will not be reflected on C++. */ + +#include diff --git a/python/gtsam/tests/test_basis.py b/python/gtsam/tests/test_basis.py index 944bdd9f3..0df9a80b0 100644 --- a/python/gtsam/tests/test_basis.py +++ b/python/gtsam/tests/test_basis.py @@ -25,7 +25,7 @@ class TestBasis(GtsamTestCase): noise = gtsam.noiseModel.Unit.Create(1) def testBasis(fitter, basis, f=f): data = {x: f(x) for x in datax} - fit = fitter(N, data, noise) + fit = fitter(data, noise, N) coeff = fit.parameters() interpy = basis.WeightMatrix(N, interpx) @ coeff np.testing.assert_almost_equal(interpy, np.array([f(x) for x in interpx]), decimal=7) From 2f6b8d6314ede652df5cb2125cd9d41490da98a9 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Fri, 27 Aug 2021 12:01:06 -0400 Subject: [PATCH 178/248] docstrings and formatting --- python/gtsam/tests/test_basis.py | 22 ++++++++++++++++++++-- 1 file changed, 20 insertions(+), 2 deletions(-) diff --git a/python/gtsam/tests/test_basis.py b/python/gtsam/tests/test_basis.py index 0df9a80b0..8d4039249 100644 --- a/python/gtsam/tests/test_basis.py +++ b/python/gtsam/tests/test_basis.py @@ -16,23 +16,41 @@ import gtsam from gtsam.utils.test_case import GtsamTestCase from gtsam.symbol_shorthand import B + class TestBasis(GtsamTestCase): + """Tests Basis module python bindings + """ def test_fit_basis(self): + """Tests FitBasis python binding for FourierBasis, Chebyshev1Basis, Chebyshev2Basis, and + Chebyshev2. + It tests FitBasis by fitting to a ground-truth function that can be represented exactly in + the basis, then checking that the regression (fit result) matches the function. For the + Chebyshev bases, the line y=x is used to generate the data while for Fourier, 0.7*cos(x) is + used. + """ f = lambda x: x # line y = x N = 2 datax = [0., 0.5, 0.75] interpx = np.linspace(0., 1., 10) noise = gtsam.noiseModel.Unit.Create(1) + + def evaluate(basis, fitparams, x): + # until wrapper for Basis functors are ready, this is how to evaluate a basis function. + return basis.WeightMatrix(N, x) @ fitparams + def testBasis(fitter, basis, f=f): + # test a basis by checking that the fit result matches the function at x-values interpx. data = {x: f(x) for x in datax} fit = fitter(data, noise, N) coeff = fit.parameters() - interpy = basis.WeightMatrix(N, interpx) @ coeff + interpy = evaluate(basis, coeff, interpx) np.testing.assert_almost_equal(interpy, np.array([f(x) for x in interpx]), decimal=7) - testBasis(gtsam.FitBasisFourierBasis, gtsam.FourierBasis, f=lambda x: 0.7*np.cos(x)) + + testBasis(gtsam.FitBasisFourierBasis, gtsam.FourierBasis, f=lambda x: 0.7 * np.cos(x)) testBasis(gtsam.FitBasisChebyshev1Basis, gtsam.Chebyshev1Basis) testBasis(gtsam.FitBasisChebyshev2Basis, gtsam.Chebyshev2Basis) testBasis(gtsam.FitBasisChebyshev2, gtsam.Chebyshev2) + if __name__ == "__main__": unittest.main() From 286b2fa4b0acd8611be292e4e7a9e8b29a144a5f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 28 Aug 2021 15:37:06 -0400 Subject: [PATCH 179/248] fix python tests and make verbose so they are easy to debug --- python/CMakeLists.txt | 5 +++-- python/gtsam/tests/test_basis.py | 6 ++++-- 2 files changed, 7 insertions(+), 4 deletions(-) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index a5fdc80a6..4254a21c6 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -166,5 +166,6 @@ add_custom_target( COMMAND ${CMAKE_COMMAND} -E env # add package to python path so no need to install "PYTHONPATH=${GTSAM_PYTHON_BUILD_DIRECTORY}/$ENV{PYTHONPATH}" - ${PYTHON_EXECUTABLE} -m unittest discover -s "${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam/tests" - DEPENDS ${GTSAM_PYTHON_DEPENDENCIES}) + ${PYTHON_EXECUTABLE} -m unittest discover -v -s . + DEPENDS ${GTSAM_PYTHON_DEPENDENCIES} + WORKING_DIRECTORY "${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam/tests") diff --git a/python/gtsam/tests/test_basis.py b/python/gtsam/tests/test_basis.py index 8d4039249..3af0a87f1 100644 --- a/python/gtsam/tests/test_basis.py +++ b/python/gtsam/tests/test_basis.py @@ -18,10 +18,12 @@ from gtsam.symbol_shorthand import B class TestBasis(GtsamTestCase): - """Tests Basis module python bindings + """ + Tests Basis module python bindings. """ def test_fit_basis(self): - """Tests FitBasis python binding for FourierBasis, Chebyshev1Basis, Chebyshev2Basis, and + """ + Tests FitBasis python binding for FourierBasis, Chebyshev1Basis, Chebyshev2Basis, and Chebyshev2. It tests FitBasis by fitting to a ground-truth function that can be represented exactly in the basis, then checking that the regression (fit result) matches the function. For the From f712d6215086b8508062df29658337af62ee3d11 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 28 Aug 2021 16:30:25 -0400 Subject: [PATCH 180/248] Added override --- .../slam/ProjectionFactorRollingShutter.h | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/gtsam_unstable/slam/ProjectionFactorRollingShutter.h b/gtsam_unstable/slam/ProjectionFactorRollingShutter.h index 5827a538c..ed56ad8f3 100644 --- a/gtsam_unstable/slam/ProjectionFactorRollingShutter.h +++ b/gtsam_unstable/slam/ProjectionFactorRollingShutter.h @@ -129,7 +129,7 @@ class ProjectionFactorRollingShutter : public NoiseModelFactor3 (gtsam::NonlinearFactor::shared_ptr(new This(*this))); } @@ -140,7 +140,7 @@ class ProjectionFactorRollingShutter : public NoiseModelFactor3measured_.size() != + this->cameras(values).size()) // 1 observation per interpolated camera + throw std::runtime_error( + "SmartProjectionPoseFactorRollingShutter: " + "measured_.size() inconsistent with input"); // triangulate 3D point at given linearization point this->triangulateSafe(this->cameras(values)); if (!this->result_) { // failed: return "empty/zero" Hessian if (this->params_.degeneracyMode == ZERO_ON_DEGENERACY) { - for (Matrix& m : Gs) - m = Matrix::Zero(DimPose, DimPose); - for (Vector& v : gs) - v = Vector::Zero(DimPose); - return boost::make_shared < RegularHessianFactor - > (this->keys_, Gs, gs, 0.0); + for (Matrix& m : Gs) m = Matrix::Zero(DimPose, DimPose); + for (Vector& v : gs) v = Vector::Zero(DimPose); + return boost::make_shared>(this->keys_, + Gs, gs, 0.0); } else { - throw std::runtime_error("SmartProjectionPoseFactorRollingShutter: " - "only supported degeneracy mode is ZERO_ON_DEGENERACY"); + throw std::runtime_error( + "SmartProjectionPoseFactorRollingShutter: " + "only supported degeneracy mode is ZERO_ON_DEGENERACY"); } } // compute Jacobian given triangulated 3D Point From 2b3709ec73496f74e98b11b26024ccfc2a35fede Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 28 Aug 2021 17:19:53 -0400 Subject: [PATCH 182/248] Got rid of SchurComplementAndRearrangeBlocks_3_12_6 --- gtsam/geometry/CameraSet.h | 13 ------------- gtsam/geometry/tests/testCameraSet.cpp | 5 ++--- .../slam/SmartProjectionPoseFactorRollingShutter.h | 2 +- 3 files changed, 3 insertions(+), 17 deletions(-) diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h index b8fbb4200..58122e33e 100644 --- a/gtsam/geometry/CameraSet.h +++ b/gtsam/geometry/CameraSet.h @@ -286,19 +286,6 @@ public: return augmentedHessianUniqueKeys; } - /** - * non-templated version of function above - */ - static SymmetricBlockMatrix SchurComplementAndRearrangeBlocks_3_12_6( - const std::vector, - Eigen::aligned_allocator > >& Fs, - const Matrix& E, const Eigen::Matrix& P, const Vector& b, - const KeyVector jacobianKeys, const KeyVector hessianKeys) { - return SchurComplementAndRearrangeBlocks<3,12,6>(Fs, E, P, b, - jacobianKeys, - hessianKeys); - } - /** * Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix * G = F' * F - F' * E * P * E' * F diff --git a/gtsam/geometry/tests/testCameraSet.cpp b/gtsam/geometry/tests/testCameraSet.cpp index e583c0e80..144f28314 100644 --- a/gtsam/geometry/tests/testCameraSet.cpp +++ b/gtsam/geometry/tests/testCameraSet.cpp @@ -185,9 +185,8 @@ TEST(CameraSet, SchurComplementAndRearrangeBlocks) { // Actual SymmetricBlockMatrix augmentedHessianBM = - Set::SchurComplementAndRearrangeBlocks_3_12_6(Fs, E, P, b, - nonuniqueKeys, - uniqueKeys); + Set::SchurComplementAndRearrangeBlocks<3, 12, 6>( + Fs, E, P, b, nonuniqueKeys, uniqueKeys); Matrix actualAugmentedHessian = augmentedHessianBM.selfadjointView(); // Expected diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index 3624e3f8e..236ae6c95 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -363,7 +363,7 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor( Fs, E, P, b, nonuniqueKeys, this->keys_); return boost::make_shared < RegularHessianFactor From d0505d4bc3767b79aebfc28c7fc59258f309f90c Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 28 Aug 2021 17:20:08 -0400 Subject: [PATCH 183/248] Check equals not assert_equal --- ...stSmartProjectionPoseFactorRollingShutter.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index bf8a8c4ab..a7441e170 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -138,8 +138,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, Equals ) { factor1->add(measurement2, x2, x3, interp_factor2, sharedK, body_P_sensor); factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor); - EXPECT(assert_equal(*factor1, *factor2)); - EXPECT(assert_equal(*factor1, *factor3)); + EXPECT(factor1->equals(*factor2)); + EXPECT(factor1->equals(*factor3)); } { // create slightly different factors (different keys) and show equal returns false SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); @@ -147,8 +147,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, Equals ) { factor1->add(measurement2, x2, x2, interp_factor2, sharedK, body_P_sensor); // different! factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor); - EXPECT(!assert_equal(*factor1, *factor2)); - EXPECT(!assert_equal(*factor1, *factor3)); + EXPECT(!factor1->equals(*factor2)); + EXPECT(!factor1->equals(*factor3)); } { // create slightly different factors (different extrinsics) and show equal returns false SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); @@ -156,8 +156,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, Equals ) { factor1->add(measurement2, x2, x3, interp_factor2, sharedK, body_P_sensor*body_P_sensor); // different! factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor); - EXPECT(!assert_equal(*factor1, *factor2)); - EXPECT(!assert_equal(*factor1, *factor3)); + EXPECT(!factor1->equals(*factor2)); + EXPECT(!factor1->equals(*factor3)); } { // create slightly different factors (different interp factors) and show equal returns false SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); @@ -165,8 +165,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, Equals ) { factor1->add(measurement2, x2, x3, interp_factor1, sharedK, body_P_sensor); // different! factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor); - EXPECT(!assert_equal(*factor1, *factor2)); - EXPECT(!assert_equal(*factor1, *factor3)); + EXPECT(!factor1->equals(*factor2)); + EXPECT(!factor1->equals(*factor3)); } } From bafcde9ee195216a0b4bae1381bfa74e59f45fad Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 28 Aug 2021 17:36:14 -0400 Subject: [PATCH 184/248] Google-style formatting in new files. --- gtsam/geometry/CameraSet.h | 41 +- .../slam/ProjectionFactorRollingShutter.cpp | 39 +- .../slam/ProjectionFactorRollingShutter.h | 173 +++--- .../SmartProjectionPoseFactorRollingShutter.h | 230 ++++---- .../testProjectionFactorRollingShutter.cpp | 258 +++++---- ...martProjectionPoseFactorRollingShutter.cpp | 494 +++++++++++------- 6 files changed, 695 insertions(+), 540 deletions(-) diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h index 58122e33e..143d4bc3c 100644 --- a/gtsam/geometry/CameraSet.h +++ b/gtsam/geometry/CameraSet.h @@ -218,48 +218,52 @@ public: size_t nrNonuniqueKeys = jacobianKeys.size(); size_t nrUniqueKeys = hessianKeys.size(); - // marginalize point: note - we reuse the standard SchurComplement function - SymmetricBlockMatrix augmentedHessian = SchurComplement(Fs,E,P,b); + // Marginalize point: note - we reuse the standard SchurComplement function. + SymmetricBlockMatrix augmentedHessian = SchurComplement(Fs, E, P, b); - // now pack into an Hessian factor - std::vector dims(nrUniqueKeys + 1); // this also includes the b term + // Pack into an Hessian factor, allow space for b term. + std::vector dims(nrUniqueKeys + 1); std::fill(dims.begin(), dims.end() - 1, NDD); dims.back() = 1; SymmetricBlockMatrix augmentedHessianUniqueKeys; - // here we have to deal with the fact that some blocks may share the same keys - if (nrUniqueKeys == nrNonuniqueKeys) { // if there is 1 calibration key per camera + // Deal with the fact that some blocks may share the same keys. + if (nrUniqueKeys == nrNonuniqueKeys) { + // Case when there is 1 calibration key per camera: augmentedHessianUniqueKeys = SymmetricBlockMatrix( dims, Matrix(augmentedHessian.selfadjointView())); - } else { // if multiple cameras share a calibration we have to rearrange - // the results of the Schur complement matrix - std::vector nonuniqueDims(nrNonuniqueKeys + 1); // this also includes the b term + } else { + // When multiple cameras share a calibration we have to rearrange + // the results of the Schur complement matrix. + std::vector nonuniqueDims(nrNonuniqueKeys + 1); // includes b std::fill(nonuniqueDims.begin(), nonuniqueDims.end() - 1, NDD); nonuniqueDims.back() = 1; augmentedHessian = SymmetricBlockMatrix( nonuniqueDims, Matrix(augmentedHessian.selfadjointView())); - // get map from key to location in the new augmented Hessian matrix (the one including only unique keys) + // Get map from key to location in the new augmented Hessian matrix (the + // one including only unique keys). std::map keyToSlotMap; for (size_t k = 0; k < nrUniqueKeys; k++) { keyToSlotMap[hessianKeys[k]] = k; } - // initialize matrix to zero + // Initialize matrix to zero. augmentedHessianUniqueKeys = SymmetricBlockMatrix( dims, Matrix::Zero(NDD * nrUniqueKeys + 1, NDD * nrUniqueKeys + 1)); - // add contributions for each key: note this loops over the hessian with nonUnique keys (augmentedHessian) - // and populates an Hessian that only includes the unique keys (that is what we want to return) + // Add contributions for each key: note this loops over the hessian with + // nonUnique keys (augmentedHessian) and populates an Hessian that only + // includes the unique keys (that is what we want to return). for (size_t i = 0; i < nrNonuniqueKeys; i++) { // rows Key key_i = jacobianKeys.at(i); - // update information vector + // Update information vector. augmentedHessianUniqueKeys.updateOffDiagonalBlock( keyToSlotMap[key_i], nrUniqueKeys, augmentedHessian.aboveDiagonalBlock(i, nrNonuniqueKeys)); - // update blocks + // Update blocks. for (size_t j = i; j < nrNonuniqueKeys; j++) { // cols Key key_j = jacobianKeys.at(j); if (i == j) { @@ -273,13 +277,14 @@ public: } else { augmentedHessianUniqueKeys.updateDiagonalBlock( keyToSlotMap[key_i], - augmentedHessian.aboveDiagonalBlock(i, j) - + augmentedHessian.aboveDiagonalBlock(i, j).transpose()); + augmentedHessian.aboveDiagonalBlock(i, j) + + augmentedHessian.aboveDiagonalBlock(i, j).transpose()); } } } } - // update bottom right element of the matrix + + // Update bottom right element of the matrix. augmentedHessianUniqueKeys.updateDiagonalBlock( nrUniqueKeys, augmentedHessian.diagonalBlock(nrNonuniqueKeys)); } diff --git a/gtsam_unstable/slam/ProjectionFactorRollingShutter.cpp b/gtsam_unstable/slam/ProjectionFactorRollingShutter.cpp index 5fc1c05eb..c92a13daf 100644 --- a/gtsam_unstable/slam/ProjectionFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/ProjectionFactorRollingShutter.cpp @@ -23,7 +23,6 @@ Vector ProjectionFactorRollingShutter::evaluateError( const Pose3& pose_a, const Pose3& pose_b, const Point3& point, boost::optional H1, boost::optional H2, boost::optional H3) const { - try { Pose3 pose = interpolate(pose_a, pose_b, alpha_, H1, H2); gtsam::Matrix Hprj; @@ -32,12 +31,10 @@ Vector ProjectionFactorRollingShutter::evaluateError( gtsam::Matrix HbodySensor; PinholeCamera camera( pose.compose(*body_P_sensor_, HbodySensor), *K_); - Point2 reprojectionError( - camera.project(point, Hprj, H3, boost::none) - measured_); - if (H1) - *H1 = Hprj * HbodySensor * (*H1); - if (H2) - *H2 = Hprj * HbodySensor * (*H2); + Point2 reprojectionError(camera.project(point, Hprj, H3, boost::none) - + measured_); + if (H1) *H1 = Hprj * HbodySensor * (*H1); + if (H2) *H2 = Hprj * HbodySensor * (*H2); return reprojectionError; } else { PinholeCamera camera(pose.compose(*body_P_sensor_), *K_); @@ -45,29 +42,23 @@ Vector ProjectionFactorRollingShutter::evaluateError( } } else { PinholeCamera camera(pose, *K_); - Point2 reprojectionError( - camera.project(point, Hprj, H3, boost::none) - measured_); - if (H1) - *H1 = Hprj * (*H1); - if (H2) - *H2 = Hprj * (*H2); + Point2 reprojectionError(camera.project(point, Hprj, H3, boost::none) - + measured_); + if (H1) *H1 = Hprj * (*H1); + if (H2) *H2 = Hprj * (*H2); return reprojectionError; } } catch (CheiralityException& e) { - if (H1) - *H1 = Matrix::Zero(2, 6); - if (H2) - *H2 = Matrix::Zero(2, 6); - if (H3) - *H3 = Matrix::Zero(2, 3); + if (H1) *H1 = Matrix::Zero(2, 6); + if (H2) *H2 = Matrix::Zero(2, 6); + if (H3) *H3 = Matrix::Zero(2, 3); if (verboseCheirality_) std::cout << e.what() << ": Landmark " - << DefaultKeyFormatter(this->key2()) << " moved behind camera " - << DefaultKeyFormatter(this->key1()) << std::endl; - if (throwCheirality_) - throw CheiralityException(this->key2()); + << DefaultKeyFormatter(this->key2()) << " moved behind camera " + << DefaultKeyFormatter(this->key1()) << std::endl; + if (throwCheirality_) throw CheiralityException(this->key2()); } return Vector2::Constant(2.0 * K_->fx()); } -} //namespace gtsam +} // namespace gtsam diff --git a/gtsam_unstable/slam/ProjectionFactorRollingShutter.h b/gtsam_unstable/slam/ProjectionFactorRollingShutter.h index ed56ad8f3..c92653c13 100644 --- a/gtsam_unstable/slam/ProjectionFactorRollingShutter.h +++ b/gtsam_unstable/slam/ProjectionFactorRollingShutter.h @@ -17,41 +17,47 @@ #pragma once -#include -#include -#include #include +#include +#include +#include + #include namespace gtsam { /** - * Non-linear factor for 2D projection measurement obtained using a rolling shutter camera. The calibration is known here. - * This version takes rolling shutter information into account as follows: consider two consecutive poses A and B, - * and a Point2 measurement taken starting at time A using a rolling shutter camera. - * Pose A has timestamp t_A, and Pose B has timestamp t_B. The Point2 measurement has timestamp t_p (with t_A <= t_p <= t_B) - * corresponding to the time of exposure of the row of the image the pixel belongs to. - * Let us define the alpha = (t_p - t_A) / (t_B - t_A), we will use the pose interpolated between A and B by - * the alpha to project the corresponding landmark to Point2. + * Non-linear factor for 2D projection measurement obtained using a rolling + * shutter camera. The calibration is known here. This version takes rolling + * shutter information into account as follows: consider two consecutive poses A + * and B, and a Point2 measurement taken starting at time A using a rolling + * shutter camera. Pose A has timestamp t_A, and Pose B has timestamp t_B. The + * Point2 measurement has timestamp t_p (with t_A <= t_p <= t_B) corresponding + * to the time of exposure of the row of the image the pixel belongs to. Let us + * define the alpha = (t_p - t_A) / (t_B - t_A), we will use the pose + * interpolated between A and B by the alpha to project the corresponding + * landmark to Point2. * @addtogroup SLAM */ -class ProjectionFactorRollingShutter : public NoiseModelFactor3 { +class ProjectionFactorRollingShutter + : public NoiseModelFactor3 { protected: - // Keep a copy of measurement and calibration for I/O - Point2 measured_; ///< 2D measurement - double alpha_; ///< interpolation parameter in [0,1] corresponding to the point2 measurement + Point2 measured_; ///< 2D measurement + double alpha_; ///< interpolation parameter in [0,1] corresponding to the + ///< point2 measurement boost::shared_ptr K_; ///< shared pointer to calibration object - boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame + boost::optional + body_P_sensor_; ///< The pose of the sensor in the body frame // verbosity handling for Cheirality Exceptions - bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false) - bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions (default: false) + bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: + ///< false) + bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions + ///< (default: false) public: - /// shorthand for base class type typedef NoiseModelFactor3 Base; @@ -66,72 +72,72 @@ class ProjectionFactorRollingShutter : public NoiseModelFactor3& K, - boost::optional body_P_sensor = - boost::none) + ProjectionFactorRollingShutter( + const Point2& measured, double alpha, const SharedNoiseModel& model, + Key poseKey_a, Key poseKey_b, Key pointKey, + const boost::shared_ptr& K, + boost::optional body_P_sensor = boost::none) : Base(model, poseKey_a, poseKey_b, pointKey), measured_(measured), alpha_(alpha), K_(K), body_P_sensor_(body_P_sensor), throwCheirality_(false), - verboseCheirality_(false) { - } + verboseCheirality_(false) {} /** * Constructor with exception-handling flags - * @param measured is the 2-dimensional pixel location of point in the image (the measurement) + * @param measured is the 2-dimensional pixel location of point in the image + * (the measurement) * @param alpha in [0,1] is the rolling shutter parameter for the measurement * @param model is the noise model * @param poseKey_a is the key of the first camera * @param poseKey_b is the key of the second camera * @param pointKey is the key of the landmark * @param K shared pointer to the constant calibration - * @param throwCheirality determines whether Cheirality exceptions are rethrown - * @param verboseCheirality determines whether exceptions are printed for Cheirality - * @param body_P_sensor is the transform from body to sensor frame (default identity) + * @param throwCheirality determines whether Cheirality exceptions are + * rethrown + * @param verboseCheirality determines whether exceptions are printed for + * Cheirality + * @param body_P_sensor is the transform from body to sensor frame (default + * identity) */ - ProjectionFactorRollingShutter(const Point2& measured, double alpha, - const SharedNoiseModel& model, Key poseKey_a, - Key poseKey_b, Key pointKey, - const boost::shared_ptr& K, - bool throwCheirality, bool verboseCheirality, - boost::optional body_P_sensor = - boost::none) + ProjectionFactorRollingShutter( + const Point2& measured, double alpha, const SharedNoiseModel& model, + Key poseKey_a, Key poseKey_b, Key pointKey, + const boost::shared_ptr& K, bool throwCheirality, + bool verboseCheirality, + boost::optional body_P_sensor = boost::none) : Base(model, poseKey_a, poseKey_b, pointKey), measured_(measured), alpha_(alpha), K_(K), body_P_sensor_(body_P_sensor), throwCheirality_(throwCheirality), - verboseCheirality_(verboseCheirality) { - } + verboseCheirality_(verboseCheirality) {} /** Virtual destructor */ - virtual ~ProjectionFactorRollingShutter() { - } + virtual ~ProjectionFactorRollingShutter() {} /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast < gtsam::NonlinearFactor - > (gtsam::NonlinearFactor::shared_ptr(new This(*this))); + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); } /** @@ -139,8 +145,9 @@ class ProjectionFactorRollingShutter : public NoiseModelFactor3>& world_P_body_key_pairs, const std::vector& alphas, const std::vector>& Ks, - const std::vector body_P_sensors) { + const std::vector& body_P_sensors) { assert(world_P_body_key_pairs.size() == measurements.size()); assert(world_P_body_key_pairs.size() == alphas.size()); assert(world_P_body_key_pairs.size() == Ks.size()); @@ -151,20 +169,24 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor>& world_P_body_key_pairs, const std::vector& alphas, - const boost::shared_ptr& K, const Pose3 body_P_sensor = Pose3::identity()) { + const boost::shared_ptr& K, + const Pose3& body_P_sensor = Pose3::identity()) { assert(world_P_body_key_pairs.size() == measurements.size()); assert(world_P_body_key_pairs.size() == alphas.size()); for (size_t i = 0; i < measurements.size(); i++) { @@ -174,39 +196,37 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor> calibration() const { + const std::vector>& calibration() const { return K_all_; } - /// return (for each observation) the keys of the pair of poses from which we interpolate - const std::vector> world_P_body_key_pairs() const { + /// return (for each observation) the keys of the pair of poses from which we + /// interpolate + const std::vector>& world_P_body_key_pairs() const { return world_P_body_key_pairs_; } /// return the interpolation factors alphas - const std::vector alphas() const { - return alphas_; - } + const std::vector& alphas() const { return alphas_; } /// return the extrinsic camera calibration body_P_sensors - const std::vector body_P_sensors() const { - return body_P_sensors_; - } + const std::vector& body_P_sensors() const { return body_P_sensors_; } /** * print * @param s optional string naming the factor * @param keyFormatter optional formatter useful for printing Symbols */ - void print(const std::string& s = "", const KeyFormatter& keyFormatter = - DefaultKeyFormatter) const override { + void print( + const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { std::cout << s << "SmartProjectionPoseFactorRollingShutter: \n "; for (size_t i = 0; i < K_all_.size(); i++) { std::cout << "-- Measurement nr " << i << std::endl; std::cout << " pose1 key: " - << keyFormatter(world_P_body_key_pairs_[i].first) << std::endl; + << keyFormatter(world_P_body_key_pairs_[i].first) << std::endl; std::cout << " pose2 key: " - << keyFormatter(world_P_body_key_pairs_[i].second) << std::endl; + << keyFormatter(world_P_body_key_pairs_[i].second) << std::endl; std::cout << " alpha: " << alphas_[i] << std::endl; body_P_sensors_[i].print("extrinsic calibration:\n"); K_all_[i]->print("intrinsic calibration = "); @@ -217,17 +237,20 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor* e = - dynamic_cast*>(&p); + dynamic_cast*>( + &p); double keyPairsEqual = true; - if(this->world_P_body_key_pairs_.size() == e->world_P_body_key_pairs().size()){ - for(size_t k=0; k< this->world_P_body_key_pairs_.size(); k++){ + if (this->world_P_body_key_pairs_.size() == + e->world_P_body_key_pairs().size()) { + for (size_t k = 0; k < this->world_P_body_key_pairs_.size(); k++) { const Key key1own = world_P_body_key_pairs_[k].first; const Key key1e = e->world_P_body_key_pairs()[k].first; const Key key2own = world_P_body_key_pairs_[k].second; const Key key2e = e->world_P_body_key_pairs()[k].second; - if ( !(key1own == key1e) || !(key2own == key2e) ){ - keyPairsEqual = false; break; + if (!(key1own == key1e) || !(key2own == key2e)) { + keyPairsEqual = false; + break; } } } else { @@ -235,18 +258,19 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactorbody_P_sensors_.size() == e->body_P_sensors().size()){ - for(size_t i=0; i< this->body_P_sensors_.size(); i++){ - if (!body_P_sensors_[i].equals(e->body_P_sensors()[i])){ - extrinsicCalibrationEqual = false; break; + if (this->body_P_sensors_.size() == e->body_P_sensors().size()) { + for (size_t i = 0; i < this->body_P_sensors_.size(); i++) { + if (!body_P_sensors_[i].equals(e->body_P_sensors()[i])) { + extrinsicCalibrationEqual = false; + break; } } } else { extrinsicCalibrationEqual = false; } - return e && Base::equals(p, tol) && K_all_ == e->calibration() - && alphas_ == e->alphas() && keyPairsEqual && extrinsicCalibrationEqual; + return e && Base::equals(p, tol) && K_all_ == e->calibration() && + alphas_ == e->alphas() && keyPairsEqual && extrinsicCalibrationEqual; } /** @@ -264,12 +288,13 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactormeasured_.size(); - E = Matrix::Zero(2 * numViews, 3); // a Point2 for each view (point jacobian) + E = Matrix::Zero(2 * numViews, + 3); // a Point2 for each view (point jacobian) b = Vector::Zero(2 * numViews); // a Point2 for each view // intermediate Jacobians Eigen::Matrix dProject_dPoseCam; Eigen::Matrix dInterpPose_dPoseBody1, - dInterpPose_dPoseBody2, dPoseCam_dInterpPose; + dInterpPose_dPoseBody2, dPoseCam_dInterpPose; Eigen::Matrix Ei; for (size_t i = 0; i < numViews; i++) { // for each camera/measurement @@ -285,14 +310,16 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor camera(w_P_cam, *K_all_[i]); // get jacobians and error vector for current measurement - Point2 reprojectionError_i = Point2( - camera.project(*this->result_, dProject_dPoseCam, Ei) - - this->measured_.at(i)); + Point2 reprojectionError_i = + Point2(camera.project(*this->result_, dProject_dPoseCam, Ei) - + this->measured_.at(i)); Eigen::Matrix J; // 2 x 12 - J.block(0, 0, ZDim, 6) = dProject_dPoseCam * dPoseCam_dInterpPose - * dInterpPose_dPoseBody1; // (2x6) * (6x6) * (6x6) - J.block(0, 6, ZDim, 6) = dProject_dPoseCam * dPoseCam_dInterpPose - * dInterpPose_dPoseBody2; // (2x6) * (6x6) * (6x6) + J.block(0, 0, ZDim, 6) = + dProject_dPoseCam * dPoseCam_dInterpPose * + dInterpPose_dPoseBody1; // (2x6) * (6x6) * (6x6) + J.block(0, 6, ZDim, 6) = + dProject_dPoseCam * dPoseCam_dInterpPose * + dInterpPose_dPoseBody2; // (2x6) * (6x6) * (6x6) // fit into the output structures Fs.push_back(J); @@ -353,21 +380,23 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor( Fs, E, P, b, nonuniqueKeys, this->keys_); - return boost::make_shared < RegularHessianFactor - > (this->keys_, augmentedHessianUniqueKeys); + return boost::make_shared>( + this->keys_, augmentedHessianUniqueKeys); } /** @@ -376,7 +405,7 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactoractive(values)) { return this->totalReprojectionError(this->cameras(values)); - } else { // else of active flag + } else { // else of active flag return 0.0; } } @@ -396,10 +425,13 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor(world_P_body_key_pairs_[i].first); - const Pose3& w_P_body2 = values.at(world_P_body_key_pairs_[i].second); + const Pose3& w_P_body1 = + values.at(world_P_body_key_pairs_[i].first); + const Pose3& w_P_body2 = + values.at(world_P_body_key_pairs_[i].second); double interpolationFactor = alphas_[i]; - const Pose3& w_P_body = interpolate(w_P_body1, w_P_body2, interpolationFactor); + const Pose3& w_P_body = + interpolate(w_P_body1, w_P_body2, interpolationFactor); const Pose3& body_P_cam = body_P_sensors_[i]; const Pose3& w_P_cam = w_P_body.compose(body_P_cam); cameras.emplace_back(w_P_cam, K_all_[i]); @@ -408,44 +440,46 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor linearizeDamped( const Values& values, const double lambda = 0.0) const { - // depending on flag set on construction we may linearize to different linear factors + // depending on flag set on construction we may linearize to different + // linear factors switch (this->params_.linearizationMode) { case HESSIAN: return this->createHessianFactor(values, lambda); default: throw std::runtime_error( - "SmartProjectionPoseFactorRollingShutter: unknown linearization mode"); + "SmartProjectionPoseFactorRollingShutter: unknown linearization " + "mode"); } } /// linearize - boost::shared_ptr linearize(const Values& values) const - override { + boost::shared_ptr linearize( + const Values& values) const override { return this->linearizeDamped(values); } private: /// Serialization function friend class boost::serialization::access; - template + template void serialize(ARCHIVE& ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - ar & BOOST_SERIALIZATION_NVP(K_all_); + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar& BOOST_SERIALIZATION_NVP(K_all_); } - }; // end of class declaration /// traits -template -struct traits > : -public Testable > { -}; +template +struct traits> + : public Testable> {}; } // namespace gtsam diff --git a/gtsam_unstable/slam/tests/testProjectionFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testProjectionFactorRollingShutter.cpp index 943e350d4..161c9aa55 100644 --- a/gtsam_unstable/slam/tests/testProjectionFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testProjectionFactorRollingShutter.cpp @@ -16,34 +16,33 @@ * @date July 2021 */ -#include +#include #include -#include -#include +#include #include #include -#include -#include #include - -#include +#include +#include +#include +#include using namespace std::placeholders; using namespace std; using namespace gtsam; // make a realistic calibration matrix -static double fov = 60; // degrees -static size_t w=640,h=480; -static Cal3_S2::shared_ptr K(new Cal3_S2(fov,w,h)); +static double fov = 60; // degrees +static size_t w = 640, h = 480; +static Cal3_S2::shared_ptr K(new Cal3_S2(fov, w, h)); // Create a noise model for the pixel error static SharedNoiseModel model(noiseModel::Unit::Create(2)); // Convenience for named keys -using symbol_shorthand::X; using symbol_shorthand::L; using symbol_shorthand::T; +using symbol_shorthand::X; // Convenience to define common variables across many tests static Key poseKey1(X(1)); @@ -51,74 +50,84 @@ static Key poseKey2(X(2)); static Key pointKey(L(1)); static double interp_params = 0.5; static Point2 measurement(323.0, 240.0); -static Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); +static Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), + Point3(0.25, -0.10, 1.0)); /* ************************************************************************* */ -TEST( ProjectionFactorRollingShutter, Constructor) { - ProjectionFactorRollingShutter factor(measurement, interp_params, model, poseKey1, poseKey2, pointKey, K); -} - -/* ************************************************************************* */ -TEST( ProjectionFactorRollingShutter, ConstructorWithTransform) { +TEST(ProjectionFactorRollingShutter, Constructor) { ProjectionFactorRollingShutter factor(measurement, interp_params, model, - poseKey1, poseKey2, pointKey, K, body_P_sensor); + poseKey1, poseKey2, pointKey, K); } /* ************************************************************************* */ -TEST( ProjectionFactorRollingShutter, Equals ) { - { // factors are equal - ProjectionFactorRollingShutter factor1(measurement, interp_params, - model, poseKey1, poseKey2, pointKey, K); - ProjectionFactorRollingShutter factor2(measurement, interp_params, - model, poseKey1, poseKey2, pointKey, K); +TEST(ProjectionFactorRollingShutter, ConstructorWithTransform) { + ProjectionFactorRollingShutter factor(measurement, interp_params, model, + poseKey1, poseKey2, pointKey, K, + body_P_sensor); +} + +/* ************************************************************************* */ +TEST(ProjectionFactorRollingShutter, Equals) { + { // factors are equal + ProjectionFactorRollingShutter factor1(measurement, interp_params, model, + poseKey1, poseKey2, pointKey, K); + ProjectionFactorRollingShutter factor2(measurement, interp_params, model, + poseKey1, poseKey2, pointKey, K); CHECK(assert_equal(factor1, factor2)); } - { // factors are NOT equal (keys are different) - ProjectionFactorRollingShutter factor1(measurement, interp_params, - model, poseKey1, poseKey2, pointKey, K); - ProjectionFactorRollingShutter factor2(measurement, interp_params, - model, poseKey1, poseKey1, pointKey, K); - CHECK(!assert_equal(factor1, factor2)); // not equal + { // factors are NOT equal (keys are different) + ProjectionFactorRollingShutter factor1(measurement, interp_params, model, + poseKey1, poseKey2, pointKey, K); + ProjectionFactorRollingShutter factor2(measurement, interp_params, model, + poseKey1, poseKey1, pointKey, K); + CHECK(!assert_equal(factor1, factor2)); // not equal } - { // factors are NOT equal (different interpolation) - ProjectionFactorRollingShutter factor1(measurement, 0.1, - model, poseKey1, poseKey1, pointKey, K); - ProjectionFactorRollingShutter factor2(measurement, 0.5, - model, poseKey1, poseKey2, pointKey, K); - CHECK(!assert_equal(factor1, factor2)); // not equal + { // factors are NOT equal (different interpolation) + ProjectionFactorRollingShutter factor1(measurement, 0.1, model, poseKey1, + poseKey1, pointKey, K); + ProjectionFactorRollingShutter factor2(measurement, 0.5, model, poseKey1, + poseKey2, pointKey, K); + CHECK(!assert_equal(factor1, factor2)); // not equal } } /* ************************************************************************* */ -TEST( ProjectionFactorRollingShutter, EqualsWithTransform ) { - { // factors are equal +TEST(ProjectionFactorRollingShutter, EqualsWithTransform) { + { // factors are equal ProjectionFactorRollingShutter factor1(measurement, interp_params, model, - poseKey1, poseKey2, pointKey, K, body_P_sensor); + poseKey1, poseKey2, pointKey, K, + body_P_sensor); ProjectionFactorRollingShutter factor2(measurement, interp_params, model, - poseKey1, poseKey2, pointKey, K, body_P_sensor); + poseKey1, poseKey2, pointKey, K, + body_P_sensor); CHECK(assert_equal(factor1, factor2)); } - { // factors are NOT equal + { // factors are NOT equal ProjectionFactorRollingShutter factor1(measurement, interp_params, model, - poseKey1, poseKey2, pointKey, K, body_P_sensor); - Pose3 body_P_sensor2(Rot3::RzRyRx(0.0, 0.0, 0.0), Point3(0.25, -0.10, 1.0)); // rotation different from body_P_sensor + poseKey1, poseKey2, pointKey, K, + body_P_sensor); + Pose3 body_P_sensor2( + Rot3::RzRyRx(0.0, 0.0, 0.0), + Point3(0.25, -0.10, 1.0)); // rotation different from body_P_sensor ProjectionFactorRollingShutter factor2(measurement, interp_params, model, - poseKey1, poseKey2, pointKey, K, body_P_sensor2); + poseKey1, poseKey2, pointKey, K, + body_P_sensor2); CHECK(!assert_equal(factor1, factor2)); } } /* ************************************************************************* */ -TEST( ProjectionFactorRollingShutter, Error ) { +TEST(ProjectionFactorRollingShutter, Error) { { // Create the factor with a measurement that is 3 pixels off in x // Camera pose corresponds to the first camera double t = 0.0; - ProjectionFactorRollingShutter factor(measurement, t, model, poseKey1, poseKey2, pointKey, K); + ProjectionFactorRollingShutter factor(measurement, t, model, poseKey1, + poseKey2, pointKey, K); // Set the linearization point - Pose3 pose1(Rot3(), Point3(0,0,-6)); - Pose3 pose2(Rot3(), Point3(0,0,-4)); + Pose3 pose1(Rot3(), Point3(0, 0, -6)); + Pose3 pose2(Rot3(), Point3(0, 0, -4)); Point3 point(0.0, 0.0, 0.0); // Use the factor to calculate the error @@ -134,11 +143,12 @@ TEST( ProjectionFactorRollingShutter, Error ) { // Create the factor with a measurement that is 3 pixels off in x // Camera pose is actually interpolated now double t = 0.5; - ProjectionFactorRollingShutter factor(measurement, t, model, poseKey1, poseKey2, pointKey, K); + ProjectionFactorRollingShutter factor(measurement, t, model, poseKey1, + poseKey2, pointKey, K); // Set the linearization point - Pose3 pose1(Rot3(), Point3(0,0,-8)); - Pose3 pose2(Rot3(), Point3(0,0,-4)); + Pose3 pose1(Rot3(), Point3(0, 0, -8)); + Pose3 pose2(Rot3(), Point3(0, 0, -4)); Point3 point(0.0, 0.0, 0.0); // Use the factor to calculate the error @@ -153,15 +163,16 @@ TEST( ProjectionFactorRollingShutter, Error ) { { // Create measurement by projecting 3D landmark double t = 0.3; - Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0,0,0)); - Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0,0,1)); + Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0, 0, 0)); + Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0, 0, 1)); Pose3 poseInterp = interpolate(pose1, pose2, t); PinholeCamera camera(poseInterp, *K); - Point3 point(0.0, 0.0, 5.0); // 5 meters in front of the camera + Point3 point(0.0, 0.0, 5.0); // 5 meters in front of the camera Point2 measured = camera.project(point); // create factor - ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2, pointKey, K); + ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, + poseKey2, pointKey, K); // Use the factor to calculate the error Vector actualError(factor.evaluateError(pose1, pose2, point)); @@ -175,19 +186,20 @@ TEST( ProjectionFactorRollingShutter, Error ) { } /* ************************************************************************* */ -TEST( ProjectionFactorRollingShutter, ErrorWithTransform ) { +TEST(ProjectionFactorRollingShutter, ErrorWithTransform) { // Create measurement by projecting 3D landmark double t = 0.3; - Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0,0,0)); - Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0,0,1)); + Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0, 0, 0)); + Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0, 0, 1)); Pose3 poseInterp = interpolate(pose1, pose2, t); - Pose3 body_P_sensor3(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0,0.2,0.1)); - PinholeCamera camera(poseInterp*body_P_sensor3, *K); - Point3 point(0.0, 0.0, 5.0); // 5 meters in front of the camera + Pose3 body_P_sensor3(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0, 0.2, 0.1)); + PinholeCamera camera(poseInterp * body_P_sensor3, *K); + Point3 point(0.0, 0.0, 5.0); // 5 meters in front of the camera Point2 measured = camera.project(point); // create factor - ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2, pointKey, K, body_P_sensor3); + ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2, + pointKey, K, body_P_sensor3); // Use the factor to calculate the error Vector actualError(factor.evaluateError(pose1, pose2, point)); @@ -200,18 +212,19 @@ TEST( ProjectionFactorRollingShutter, ErrorWithTransform ) { } /* ************************************************************************* */ -TEST( ProjectionFactorRollingShutter, Jacobian ) { +TEST(ProjectionFactorRollingShutter, Jacobian) { // Create measurement by projecting 3D landmark double t = 0.3; - Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0,0,0)); - Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0,0,1)); + Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0, 0, 0)); + Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0, 0, 1)); Pose3 poseInterp = interpolate(pose1, pose2, t); PinholeCamera camera(poseInterp, *K); - Point3 point(0.0, 0.0, 5.0); // 5 meters in front of the camera + Point3 point(0.0, 0.0, 5.0); // 5 meters in front of the camera Point2 measured = camera.project(point); // create factor - ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2, pointKey, K); + ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2, + pointKey, K); // Use the factor to calculate the Jacobians Matrix H1Actual, H2Actual, H3Actual; @@ -222,22 +235,25 @@ TEST( ProjectionFactorRollingShutter, Jacobian ) { std::function( std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3, boost::none, boost::none, boost::none)), - pose1, pose2, point); + std::placeholders::_3, boost::none, boost::none, + boost::none)), + pose1, pose2, point); Matrix H2Expected = numericalDerivative32( std::function( std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3, boost::none, boost::none, boost::none)), - pose1, pose2, point); + std::placeholders::_3, boost::none, boost::none, + boost::none)), + pose1, pose2, point); Matrix H3Expected = numericalDerivative33( std::function( std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3, boost::none, boost::none, boost::none)), - pose1, pose2, point); + std::placeholders::_3, boost::none, boost::none, + boost::none)), + pose1, pose2, point); CHECK(assert_equal(H1Expected, H1Actual, 1e-5)); CHECK(assert_equal(H2Expected, H2Actual, 1e-5)); @@ -245,19 +261,20 @@ TEST( ProjectionFactorRollingShutter, Jacobian ) { } /* ************************************************************************* */ -TEST( ProjectionFactorRollingShutter, JacobianWithTransform ) { +TEST(ProjectionFactorRollingShutter, JacobianWithTransform) { // Create measurement by projecting 3D landmark double t = 0.6; - Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0,0,0)); - Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0,0,1)); + Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0, 0, 0)); + Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0, 0, 1)); Pose3 poseInterp = interpolate(pose1, pose2, t); - Pose3 body_P_sensor3(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0,0.2,0.1)); - PinholeCamera camera(poseInterp*body_P_sensor3, *K); - Point3 point(0.0, 0.0, 5.0); // 5 meters in front of the camera + Pose3 body_P_sensor3(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0, 0.2, 0.1)); + PinholeCamera camera(poseInterp * body_P_sensor3, *K); + Point3 point(0.0, 0.0, 5.0); // 5 meters in front of the camera Point2 measured = camera.project(point); // create factor - ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2, pointKey, K, body_P_sensor3); + ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2, + pointKey, K, body_P_sensor3); // Use the factor to calculate the Jacobians Matrix H1Actual, H2Actual, H3Actual; @@ -268,22 +285,25 @@ TEST( ProjectionFactorRollingShutter, JacobianWithTransform ) { std::function( std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3, boost::none, boost::none, boost::none)), - pose1, pose2, point); + std::placeholders::_3, boost::none, boost::none, + boost::none)), + pose1, pose2, point); Matrix H2Expected = numericalDerivative32( std::function( std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3, boost::none, boost::none, boost::none)), - pose1, pose2, point); + std::placeholders::_3, boost::none, boost::none, + boost::none)), + pose1, pose2, point); Matrix H3Expected = numericalDerivative33( std::function( std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3, boost::none, boost::none, boost::none)), - pose1, pose2, point); + std::placeholders::_3, boost::none, boost::none, + boost::none)), + pose1, pose2, point); CHECK(assert_equal(H1Expected, H1Actual, 1e-5)); CHECK(assert_equal(H2Expected, H2Actual, 1e-5)); @@ -291,41 +311,48 @@ TEST( ProjectionFactorRollingShutter, JacobianWithTransform ) { } /* ************************************************************************* */ -TEST( ProjectionFactorRollingShutter, cheirality ) { +TEST(ProjectionFactorRollingShutter, cheirality) { // Create measurement by projecting 3D landmark behind camera double t = 0.3; - Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0,0,0)); - Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0,0,1)); + Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0, 0, 0)); + Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0, 0, 1)); Pose3 poseInterp = interpolate(pose1, pose2, t); PinholeCamera camera(poseInterp, *K); - Point3 point(0.0, 0.0, -5.0); // 5 meters behind the camera + Point3 point(0.0, 0.0, -5.0); // 5 meters behind the camera #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION - Point2 measured = Point2(0.0,0.0); // project would throw an exception - { // check that exception is thrown if we set throwCheirality = true + Point2 measured = Point2(0.0, 0.0); // project would throw an exception + { // check that exception is thrown if we set throwCheirality = true bool throwCheirality = true; bool verboseCheirality = true; - ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2, pointKey, K, throwCheirality, verboseCheirality); + ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, + poseKey2, pointKey, K, + throwCheirality, verboseCheirality); CHECK_EXCEPTION(factor.evaluateError(pose1, pose2, point), CheiralityException); } - { // check that exception is NOT thrown if we set throwCheirality = false, and outputs are correct - bool throwCheirality = false; // default - bool verboseCheirality = false; // default - ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2, pointKey, K, throwCheirality, verboseCheirality); + { // check that exception is NOT thrown if we set throwCheirality = false, + // and outputs are correct + bool throwCheirality = false; // default + bool verboseCheirality = false; // default + ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, + poseKey2, pointKey, K, + throwCheirality, verboseCheirality); // Use the factor to calculate the error Matrix H1Actual, H2Actual, H3Actual; - Vector actualError(factor.evaluateError(pose1, pose2, point, H1Actual, H2Actual, H3Actual)); + Vector actualError(factor.evaluateError(pose1, pose2, point, H1Actual, + H2Actual, H3Actual)); // The expected error is zero - Vector expectedError = Vector2::Constant(2.0 * K->fx()); // this is what we return when point is behind camera + Vector expectedError = Vector2::Constant( + 2.0 * K->fx()); // this is what we return when point is behind camera // Verify we get the expected error CHECK(assert_equal(expectedError, actualError, 1e-9)); - CHECK(assert_equal(Matrix::Zero(2,6), H1Actual, 1e-5)); - CHECK(assert_equal(Matrix::Zero(2,6), H2Actual, 1e-5)); - CHECK(assert_equal(Matrix::Zero(2,3), H3Actual, 1e-5)); + CHECK(assert_equal(Matrix::Zero(2, 6), H1Actual, 1e-5)); + CHECK(assert_equal(Matrix::Zero(2, 6), H2Actual, 1e-5)); + CHECK(assert_equal(Matrix::Zero(2, 3), H3Actual, 1e-5)); } #else { @@ -333,7 +360,8 @@ TEST( ProjectionFactorRollingShutter, cheirality ) { Point2 measured = camera.project(point); // create factor - ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2, pointKey, K); + ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, + poseKey2, pointKey, K); // Use the factor to calculate the Jacobians Matrix H1Actual, H2Actual, H3Actual; @@ -344,22 +372,25 @@ TEST( ProjectionFactorRollingShutter, cheirality ) { std::function( std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3, boost::none, boost::none, boost::none)), - pose1, pose2, point); + std::placeholders::_3, boost::none, boost::none, + boost::none)), + pose1, pose2, point); Matrix H2Expected = numericalDerivative32( std::function( std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3, boost::none, boost::none, boost::none)), - pose1, pose2, point); + std::placeholders::_3, boost::none, boost::none, + boost::none)), + pose1, pose2, point); Matrix H3Expected = numericalDerivative33( std::function( std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3, boost::none, boost::none, boost::none)), - pose1, pose2, point); + std::placeholders::_3, boost::none, boost::none, + boost::none)), + pose1, pose2, point); CHECK(assert_equal(H1Expected, H1Actual, 1e-5)); CHECK(assert_equal(H2Expected, H2Actual, 1e-5)); @@ -368,8 +399,9 @@ TEST( ProjectionFactorRollingShutter, cheirality ) { #endif } - /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ - diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index a7441e170..0b94d2c3f 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -16,17 +16,19 @@ * @date July 2021 */ -#include "gtsam/slam/tests/smartFactorScenarios.h" -#include -#include -#include -#include -#include +#include #include #include -#include +#include +#include +#include +#include +#include + #include #include + +#include "gtsam/slam/tests/smartFactorScenarios.h" #define DISABLE_TIMING using namespace gtsam; @@ -39,8 +41,8 @@ static const double sigma = 0.1; static SharedIsotropic model(noiseModel::Isotropic::Sigma(2, sigma)); // Convenience for named keys -using symbol_shorthand::X; using symbol_shorthand::L; +using symbol_shorthand::X; // tests data static Symbol x1('X', 1); @@ -48,8 +50,8 @@ static Symbol x2('X', 2); static Symbol x3('X', 3); static Symbol x4('X', 4); static Symbol l0('L', 0); -static Pose3 body_P_sensor = Pose3(Rot3::Ypr(-0.1, 0.2, -0.2), - Point3(0.1, 0.0, 0.0)); +static Pose3 body_P_sensor = + Pose3(Rot3::Ypr(-0.1, 0.2, -0.2), Point3(0.1, 0.0, 0.0)); static Point2 measurement1(323.0, 240.0); static Point2 measurement2(200.0, 220.0); @@ -64,38 +66,39 @@ static double interp_factor3 = 0.5; namespace vanillaPoseRS { typedef PinholePose Camera; static Cal3_S2::shared_ptr sharedK(new Cal3_S2(fov, w, h)); -Pose3 interp_pose1 = interpolate(level_pose,pose_right,interp_factor1); -Pose3 interp_pose2 = interpolate(pose_right,pose_above,interp_factor2); -Pose3 interp_pose3 = interpolate(pose_above,level_pose,interp_factor3); +Pose3 interp_pose1 = interpolate(level_pose, pose_right, interp_factor1); +Pose3 interp_pose2 = interpolate(pose_right, pose_above, interp_factor2); +Pose3 interp_pose3 = interpolate(pose_above, level_pose, interp_factor3); Camera cam1(interp_pose1, sharedK); Camera cam2(interp_pose2, sharedK); Camera cam3(interp_pose3, sharedK); -} +} // namespace vanillaPoseRS LevenbergMarquardtParams lmParams; -typedef SmartProjectionPoseFactorRollingShutter< PinholePose > SmartFactorRS; +typedef SmartProjectionPoseFactorRollingShutter> + SmartFactorRS; /* ************************************************************************* */ -TEST( SmartProjectionPoseFactorRollingShutter, Constructor) { +TEST(SmartProjectionPoseFactorRollingShutter, Constructor) { SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); } /* ************************************************************************* */ -TEST( SmartProjectionPoseFactorRollingShutter, Constructor2) { +TEST(SmartProjectionPoseFactorRollingShutter, Constructor2) { SmartProjectionParams params; params.setRankTolerance(rankTol); SmartFactorRS factor1(model, params); } /* ************************************************************************* */ -TEST( SmartProjectionPoseFactorRollingShutter, add) { +TEST(SmartProjectionPoseFactorRollingShutter, add) { using namespace vanillaPose; SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); factor1->add(measurement1, x1, x2, interp_factor, sharedK, body_P_sensor); } /* ************************************************************************* */ -TEST( SmartProjectionPoseFactorRollingShutter, Equals ) { +TEST(SmartProjectionPoseFactorRollingShutter, Equals) { using namespace vanillaPose; // create fake measurements @@ -104,10 +107,10 @@ TEST( SmartProjectionPoseFactorRollingShutter, Equals ) { measurements.push_back(measurement2); measurements.push_back(measurement3); - std::vector> key_pairs; - key_pairs.push_back(std::make_pair(x1,x2)); - key_pairs.push_back(std::make_pair(x2,x3)); - key_pairs.push_back(std::make_pair(x3,x4)); + std::vector> key_pairs; + key_pairs.push_back(std::make_pair(x1, x2)); + key_pairs.push_back(std::make_pair(x2, x3)); + key_pairs.push_back(std::make_pair(x3, x4)); std::vector> intrinsicCalibrations; intrinsicCalibrations.push_back(sharedK); @@ -126,13 +129,14 @@ TEST( SmartProjectionPoseFactorRollingShutter, Equals ) { // create by adding a batch of measurements with a bunch of calibrations SmartFactorRS::shared_ptr factor2(new SmartFactorRS(model)); - factor2->add(measurements, key_pairs, interp_factors, intrinsicCalibrations, extrinsicCalibrations); + factor2->add(measurements, key_pairs, interp_factors, intrinsicCalibrations, + extrinsicCalibrations); // create by adding a batch of measurements with a single calibrations SmartFactorRS::shared_ptr factor3(new SmartFactorRS(model)); factor3->add(measurements, key_pairs, interp_factors, sharedK, body_P_sensor); - { // create equal factors and show equal returns true + { // create equal factors and show equal returns true SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); factor1->add(measurement1, x1, x2, interp_factor1, sharedK, body_P_sensor); factor1->add(measurement2, x2, x3, interp_factor2, sharedK, body_P_sensor); @@ -141,28 +145,34 @@ TEST( SmartProjectionPoseFactorRollingShutter, Equals ) { EXPECT(factor1->equals(*factor2)); EXPECT(factor1->equals(*factor3)); } - { // create slightly different factors (different keys) and show equal returns false + { // create slightly different factors (different keys) and show equal + // returns false SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); factor1->add(measurement1, x1, x2, interp_factor1, sharedK, body_P_sensor); - factor1->add(measurement2, x2, x2, interp_factor2, sharedK, body_P_sensor); // different! + factor1->add(measurement2, x2, x2, interp_factor2, sharedK, + body_P_sensor); // different! factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor); EXPECT(!factor1->equals(*factor2)); EXPECT(!factor1->equals(*factor3)); } - { // create slightly different factors (different extrinsics) and show equal returns false + { // create slightly different factors (different extrinsics) and show equal + // returns false SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); factor1->add(measurement1, x1, x2, interp_factor1, sharedK, body_P_sensor); - factor1->add(measurement2, x2, x3, interp_factor2, sharedK, body_P_sensor*body_P_sensor); // different! + factor1->add(measurement2, x2, x3, interp_factor2, sharedK, + body_P_sensor * body_P_sensor); // different! factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor); EXPECT(!factor1->equals(*factor2)); EXPECT(!factor1->equals(*factor3)); } - { // create slightly different factors (different interp factors) and show equal returns false + { // create slightly different factors (different interp factors) and show + // equal returns false SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); factor1->add(measurement1, x1, x2, interp_factor1, sharedK, body_P_sensor); - factor1->add(measurement2, x2, x3, interp_factor1, sharedK, body_P_sensor); // different! + factor1->add(measurement2, x2, x3, interp_factor1, sharedK, + body_P_sensor); // different! factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor); EXPECT(!factor1->equals(*factor2)); @@ -170,13 +180,16 @@ TEST( SmartProjectionPoseFactorRollingShutter, Equals ) { } } -static const int DimBlock = 12; ///< size of the variable stacking 2 poses from which the observation pose is interpolated -static const int ZDim = 2; ///< Measurement dimension (Point2) -typedef Eigen::Matrix MatrixZD; // F blocks (derivatives wrt camera) -typedef std::vector > FBlocks; // vector of F blocks +static const int DimBlock = 12; ///< size of the variable stacking 2 poses from + ///< which the observation pose is interpolated +static const int ZDim = 2; ///< Measurement dimension (Point2) +typedef Eigen::Matrix + MatrixZD; // F blocks (derivatives wrt camera) +typedef std::vector> + FBlocks; // vector of F blocks /* *************************************************************************/ -TEST( SmartProjectionPoseFactorRollingShutter, noiselessErrorAndJacobians ) { +TEST(SmartProjectionPoseFactorRollingShutter, noiselessErrorAndJacobians) { using namespace vanillaPoseRS; // Project two landmarks into two cameras @@ -188,7 +201,7 @@ TEST( SmartProjectionPoseFactorRollingShutter, noiselessErrorAndJacobians ) { factor.add(level_uv, x1, x2, interp_factor1, sharedK, body_P_sensorId); factor.add(level_uv_right, x2, x3, interp_factor2, sharedK, body_P_sensorId); - Values values; // it's a pose factor, hence these are poses + Values values; // it's a pose factor, hence these are poses values.insert(x1, level_pose); values.insert(x2, pose_right); values.insert(x3, pose_above); @@ -200,41 +213,56 @@ TEST( SmartProjectionPoseFactorRollingShutter, noiselessErrorAndJacobians ) { // Check triangulation factor.triangulateSafe(factor.cameras(values)); TriangulationResult point = factor.point(); - EXPECT(point.valid()); // check triangulated point is valid - EXPECT(assert_equal(landmark1, *point)); // check triangulation result matches expected 3D landmark + EXPECT(point.valid()); // check triangulated point is valid + EXPECT(assert_equal( + landmark1, + *point)); // check triangulation result matches expected 3D landmark // Check Jacobians // -- actual Jacobians FBlocks actualFs; Matrix actualE; Vector actualb; - factor.computeJacobiansWithTriangulatedPoint(actualFs, actualE, actualb, values); - EXPECT(actualE.rows() == 4); EXPECT(actualE.cols() == 3); - EXPECT(actualb.rows() == 4); EXPECT(actualb.cols() == 1); + factor.computeJacobiansWithTriangulatedPoint(actualFs, actualE, actualb, + values); + EXPECT(actualE.rows() == 4); + EXPECT(actualE.cols() == 3); + EXPECT(actualb.rows() == 4); + EXPECT(actualb.cols() == 1); EXPECT(actualFs.size() == 2); // -- expected Jacobians from ProjectionFactorsRollingShutter - ProjectionFactorRollingShutter factor1(level_uv, interp_factor1, model, x1, x2, l0, sharedK, body_P_sensorId); + ProjectionFactorRollingShutter factor1(level_uv, interp_factor1, model, x1, + x2, l0, sharedK, body_P_sensorId); Matrix expectedF11, expectedF12, expectedE1; - Vector expectedb1 = factor1.evaluateError(level_pose, pose_right, landmark1, expectedF11, expectedF12, expectedE1); - EXPECT(assert_equal( expectedF11, Matrix(actualFs[0].block(0,0,2,6)), 1e-5)); - EXPECT(assert_equal( expectedF12, Matrix(actualFs[0].block(0,6,2,6)), 1e-5)); - EXPECT(assert_equal( expectedE1, Matrix(actualE.block(0,0,2,3)), 1e-5)); - // by definition computeJacobiansWithTriangulatedPoint returns minus reprojectionError - EXPECT(assert_equal( expectedb1, -Vector(actualb.segment<2>(0)), 1e-5)); + Vector expectedb1 = factor1.evaluateError( + level_pose, pose_right, landmark1, expectedF11, expectedF12, expectedE1); + EXPECT( + assert_equal(expectedF11, Matrix(actualFs[0].block(0, 0, 2, 6)), 1e-5)); + EXPECT( + assert_equal(expectedF12, Matrix(actualFs[0].block(0, 6, 2, 6)), 1e-5)); + EXPECT(assert_equal(expectedE1, Matrix(actualE.block(0, 0, 2, 3)), 1e-5)); + // by definition computeJacobiansWithTriangulatedPoint returns minus + // reprojectionError + EXPECT(assert_equal(expectedb1, -Vector(actualb.segment<2>(0)), 1e-5)); - ProjectionFactorRollingShutter factor2(level_uv_right, interp_factor2, model, x2, x3, l0, sharedK, body_P_sensorId); + ProjectionFactorRollingShutter factor2(level_uv_right, interp_factor2, model, + x2, x3, l0, sharedK, body_P_sensorId); Matrix expectedF21, expectedF22, expectedE2; - Vector expectedb2 = factor2.evaluateError(pose_right, pose_above, landmark1, expectedF21, expectedF22, expectedE2); - EXPECT(assert_equal( expectedF21, Matrix(actualFs[1].block(0,0,2,6)), 1e-5)); - EXPECT(assert_equal( expectedF22, Matrix(actualFs[1].block(0,6,2,6)), 1e-5)); - EXPECT(assert_equal( expectedE2, Matrix(actualE.block(2,0,2,3)), 1e-5)); - // by definition computeJacobiansWithTriangulatedPoint returns minus reprojectionError - EXPECT(assert_equal( expectedb2, -Vector(actualb.segment<2>(2)), 1e-5)); + Vector expectedb2 = factor2.evaluateError( + pose_right, pose_above, landmark1, expectedF21, expectedF22, expectedE2); + EXPECT( + assert_equal(expectedF21, Matrix(actualFs[1].block(0, 0, 2, 6)), 1e-5)); + EXPECT( + assert_equal(expectedF22, Matrix(actualFs[1].block(0, 6, 2, 6)), 1e-5)); + EXPECT(assert_equal(expectedE2, Matrix(actualE.block(2, 0, 2, 3)), 1e-5)); + // by definition computeJacobiansWithTriangulatedPoint returns minus + // reprojectionError + EXPECT(assert_equal(expectedb2, -Vector(actualb.segment<2>(2)), 1e-5)); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians ) { +TEST(SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians) { // also includes non-identical extrinsic calibration using namespace vanillaPoseRS; @@ -246,9 +274,10 @@ TEST( SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians ) { SmartFactorRS factor(model); factor.add(level_uv, x1, x2, interp_factor1, sharedK, body_P_sensorNonId); - factor.add(level_uv_right, x2, x3, interp_factor2, sharedK, body_P_sensorNonId); + factor.add(level_uv_right, x2, x3, interp_factor2, sharedK, + body_P_sensorNonId); - Values values; // it's a pose factor, hence these are poses + Values values; // it's a pose factor, hence these are poses values.insert(x1, level_pose); values.insert(x2, pose_right); values.insert(x3, pose_above); @@ -256,7 +285,7 @@ TEST( SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians ) { // Perform triangulation factor.triangulateSafe(factor.cameras(values)); TriangulationResult point = factor.point(); - EXPECT(point.valid()); // check triangulated point is valid + EXPECT(point.valid()); // check triangulated point is valid Point3 landmarkNoisy = *point; // Check Jacobians @@ -264,32 +293,48 @@ TEST( SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians ) { FBlocks actualFs; Matrix actualE; Vector actualb; - factor.computeJacobiansWithTriangulatedPoint(actualFs, actualE, actualb, values); - EXPECT(actualE.rows() == 4); EXPECT(actualE.cols() == 3); - EXPECT(actualb.rows() == 4); EXPECT(actualb.cols() == 1); + factor.computeJacobiansWithTriangulatedPoint(actualFs, actualE, actualb, + values); + EXPECT(actualE.rows() == 4); + EXPECT(actualE.cols() == 3); + EXPECT(actualb.rows() == 4); + EXPECT(actualb.cols() == 1); EXPECT(actualFs.size() == 2); // -- expected Jacobians from ProjectionFactorsRollingShutter - ProjectionFactorRollingShutter factor1(level_uv, interp_factor1, model, x1, x2, l0, sharedK, body_P_sensorNonId); + ProjectionFactorRollingShutter factor1(level_uv, interp_factor1, model, x1, + x2, l0, sharedK, body_P_sensorNonId); Matrix expectedF11, expectedF12, expectedE1; - Vector expectedb1 = factor1.evaluateError(level_pose, pose_right, landmarkNoisy, expectedF11, expectedF12, expectedE1); - EXPECT(assert_equal( expectedF11, Matrix(actualFs[0].block(0,0,2,6)), 1e-5)); - EXPECT(assert_equal( expectedF12, Matrix(actualFs[0].block(0,6,2,6)), 1e-5)); - EXPECT(assert_equal( expectedE1, Matrix(actualE.block(0,0,2,3)), 1e-5)); - // by definition computeJacobiansWithTriangulatedPoint returns minus reprojectionError - EXPECT(assert_equal( expectedb1, -Vector(actualb.segment<2>(0)), 1e-5)); + Vector expectedb1 = + factor1.evaluateError(level_pose, pose_right, landmarkNoisy, expectedF11, + expectedF12, expectedE1); + EXPECT( + assert_equal(expectedF11, Matrix(actualFs[0].block(0, 0, 2, 6)), 1e-5)); + EXPECT( + assert_equal(expectedF12, Matrix(actualFs[0].block(0, 6, 2, 6)), 1e-5)); + EXPECT(assert_equal(expectedE1, Matrix(actualE.block(0, 0, 2, 3)), 1e-5)); + // by definition computeJacobiansWithTriangulatedPoint returns minus + // reprojectionError + EXPECT(assert_equal(expectedb1, -Vector(actualb.segment<2>(0)), 1e-5)); - ProjectionFactorRollingShutter factor2(level_uv_right, interp_factor2, model, x2, x3, l0, sharedK, body_P_sensorNonId); + ProjectionFactorRollingShutter factor2(level_uv_right, interp_factor2, model, + x2, x3, l0, sharedK, + body_P_sensorNonId); Matrix expectedF21, expectedF22, expectedE2; - Vector expectedb2 = factor2.evaluateError(pose_right, pose_above, landmarkNoisy, expectedF21, expectedF22, expectedE2); - EXPECT(assert_equal( expectedF21, Matrix(actualFs[1].block(0,0,2,6)), 1e-5)); - EXPECT(assert_equal( expectedF22, Matrix(actualFs[1].block(0,6,2,6)), 1e-5)); - EXPECT(assert_equal( expectedE2, Matrix(actualE.block(2,0,2,3)), 1e-5)); - // by definition computeJacobiansWithTriangulatedPoint returns minus reprojectionError - EXPECT(assert_equal( expectedb2, -Vector(actualb.segment<2>(2)), 1e-5)); + Vector expectedb2 = + factor2.evaluateError(pose_right, pose_above, landmarkNoisy, expectedF21, + expectedF22, expectedE2); + EXPECT( + assert_equal(expectedF21, Matrix(actualFs[1].block(0, 0, 2, 6)), 1e-5)); + EXPECT( + assert_equal(expectedF22, Matrix(actualFs[1].block(0, 6, 2, 6)), 1e-5)); + EXPECT(assert_equal(expectedE2, Matrix(actualE.block(2, 0, 2, 3)), 1e-5)); + // by definition computeJacobiansWithTriangulatedPoint returns minus + // reprojectionError + EXPECT(assert_equal(expectedb2, -Vector(actualb.segment<2>(2)), 1e-5)); // Check errors - double actualError = factor.error(values); // from smart factor + double actualError = factor.error(values); // from smart factor NonlinearFactorGraph nfg; nfg.add(factor1); nfg.add(factor2); @@ -299,8 +344,7 @@ TEST( SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians ) { } /* *************************************************************************/ -TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses ) { - +TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses) { using namespace vanillaPoseRS; Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; @@ -310,10 +354,10 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses ) { projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); // create inputs - std::vector> key_pairs; - key_pairs.push_back(std::make_pair(x1,x2)); - key_pairs.push_back(std::make_pair(x2,x3)); - key_pairs.push_back(std::make_pair(x3,x1)); + std::vector> key_pairs; + key_pairs.push_back(std::make_pair(x1, x2)); + key_pairs.push_back(std::make_pair(x2, x3)); + key_pairs.push_back(std::make_pair(x3, x1)); std::vector interp_factors; interp_factors.push_back(interp_factor1); @@ -344,20 +388,22 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses ) { groundTruth.insert(x3, pose_above); DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9); - // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), + // Point3(0.5,0.1,0.3)); // noise from regular projection factor test below Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), - Point3(0.1, 0.1, 0.1)); // smaller noise + Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, level_pose); values.insert(x2, pose_right); - // initialize third pose with some noise, we expect it to move back to original pose_above + // initialize third pose with some noise, we expect it to move back to + // original pose_above values.insert(x3, pose_above * noise_pose); - EXPECT( // check that the pose is actually noisy - assert_equal( - Pose3( - Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, - -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), - Point3(0.1, -0.1, 1.9)), values.at(x3))); + EXPECT( // check that the pose is actually noisy + assert_equal(Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656, + -0.0313952598, -0.000986635786, 0.0314107591, + -0.999013364, -0.0313952598), + Point3(0.1, -0.1, 1.9)), + values.at(x3))); Values result; LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); @@ -366,11 +412,12 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses ) { } /* *************************************************************************/ -TEST( SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses ) { - // here we replicate a test in SmartProjectionPoseFactor by setting interpolation - // factors to 0 and 1 (such that the rollingShutter measurements falls back to standard pixel measurements) - // Note: this is a quite extreme test since in typical camera you would not have more than - // 1 measurement per landmark at each interpolated pose +TEST(SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses) { + // here we replicate a test in SmartProjectionPoseFactor by setting + // interpolation factors to 0 and 1 (such that the rollingShutter measurements + // falls back to standard pixel measurements) Note: this is a quite extreme + // test since in typical camera you would not have more than 1 measurement per + // landmark at each interpolated pose using namespace vanillaPose; // Default cameras for simple derivatives @@ -423,7 +470,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses ) { perturbedDelta.insert(x2, delta); double expectedError = 2500; - // After eliminating the point, A1 and A2 contain 2-rank information on cameras: + // After eliminating the point, A1 and A2 contain 2-rank information on + // cameras: Matrix16 A1, A2; A1 << -10, 0, 0, 0, 1, 0; A2 << 10, 0, 1, 0, -1, 0; @@ -449,8 +497,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses ) { Values values; values.insert(x1, pose1); values.insert(x2, pose2); - boost::shared_ptr < RegularHessianFactor<6> > actual = smartFactor1 - ->createHessianFactor(values); + boost::shared_ptr> actual = + smartFactor1->createHessianFactor(values); EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6)); EXPECT(assert_equal(expected, *actual, 1e-6)); EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-6); @@ -458,7 +506,7 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses ) { } /* *************************************************************************/ -TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI ) { +TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI) { using namespace vanillaPoseRS; Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; @@ -478,7 +526,7 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI ) { interp_factors.push_back(interp_factor2); interp_factors.push_back(interp_factor3); - double excludeLandmarksFutherThanDist = 1e10; //very large + double excludeLandmarksFutherThanDist = 1e10; // very large SmartProjectionParams params; params.setRankTolerance(1.0); params.setLinearizationMode(gtsam::HESSIAN); @@ -486,13 +534,13 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI ) { params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); params.setEnableEPI(true); - SmartFactorRS smartFactor1(model,params); + SmartFactorRS smartFactor1(model, params); smartFactor1.add(measurements_lmk1, key_pairs, interp_factors, sharedK); - SmartFactorRS smartFactor2(model,params); + SmartFactorRS smartFactor2(model, params); smartFactor2.add(measurements_lmk2, key_pairs, interp_factors, sharedK); - SmartFactorRS smartFactor3(model,params); + SmartFactorRS smartFactor3(model, params); smartFactor3.add(measurements_lmk3, key_pairs, interp_factors, sharedK); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -509,7 +557,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI ) { Values values; values.insert(x1, level_pose); values.insert(x2, pose_right); - // initialize third pose with some noise, we expect it to move back to original pose_above + // initialize third pose with some noise, we expect it to move back to + // original pose_above values.insert(x3, pose_above * noise_pose); // Optimization should correct 3rd pose @@ -520,7 +569,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI ) { } /* *************************************************************************/ -TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_landmarkDistance ) { +TEST(SmartProjectionPoseFactorRollingShutter, + optimization_3poses_landmarkDistance) { using namespace vanillaPoseRS; Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; @@ -548,13 +598,13 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_landmarkDista params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); params.setEnableEPI(false); - SmartFactorRS smartFactor1(model,params); + SmartFactorRS smartFactor1(model, params); smartFactor1.add(measurements_lmk1, key_pairs, interp_factors, sharedK); - SmartFactorRS smartFactor2(model,params); + SmartFactorRS smartFactor2(model, params); smartFactor2.add(measurements_lmk2, key_pairs, interp_factors, sharedK); - SmartFactorRS smartFactor3(model,params); + SmartFactorRS smartFactor3(model, params); smartFactor3.add(measurements_lmk3, key_pairs, interp_factors, sharedK); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -571,10 +621,12 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_landmarkDista Values values; values.insert(x1, level_pose); values.insert(x2, pose_right); - // initialize third pose with some noise, we expect it to move back to original pose_above + // initialize third pose with some noise, we expect it to move back to + // original pose_above values.insert(x3, pose_above * noise_pose); - // All factors are disabled (due to the distance threshold) and pose should remain where it is + // All factors are disabled (due to the distance threshold) and pose should + // remain where it is Values result; LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); result = optimizer.optimize(); @@ -582,7 +634,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_landmarkDista } /* *************************************************************************/ -TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_dynamicOutlierRejection ) { +TEST(SmartProjectionPoseFactorRollingShutter, + optimization_3poses_dynamicOutlierRejection) { using namespace vanillaPoseRS; // add fourth landmark Point3 landmark4(5, -0.5, 1); @@ -594,7 +647,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_dynamicOutlie projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_lmk4); - measurements_lmk4.at(0) = measurements_lmk4.at(0) + Point2(10, 10); // add outlier + measurements_lmk4.at(0) = + measurements_lmk4.at(0) + Point2(10, 10); // add outlier // create inputs std::vector> key_pairs; @@ -608,7 +662,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_dynamicOutlie interp_factors.push_back(interp_factor3); double excludeLandmarksFutherThanDist = 1e10; - double dynamicOutlierRejectionThreshold = 3; // max 3 pixel of average reprojection error + double dynamicOutlierRejectionThreshold = + 3; // max 3 pixel of average reprojection error SmartProjectionParams params; params.setRankTolerance(1.0); @@ -640,12 +695,15 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_dynamicOutlie graph.addPrior(x1, level_pose, noisePrior); graph.addPrior(x2, pose_right, noisePrior); - Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), - Point3(0.01, 0.01, 0.01)); // smaller noise, otherwise outlier rejection will kick in + Pose3 noise_pose = Pose3( + Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.01, 0.01, + 0.01)); // smaller noise, otherwise outlier rejection will kick in Values values; values.insert(x1, level_pose); values.insert(x2, pose_right); - // initialize third pose with some noise, we expect it to move back to original pose_above + // initialize third pose with some noise, we expect it to move back to + // original pose_above values.insert(x3, pose_above * noise_pose); // Optimization should correct 3rd pose @@ -656,8 +714,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_dynamicOutlie } /* *************************************************************************/ -TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRollingShutter) { - +TEST(SmartProjectionPoseFactorRollingShutter, + hessianComparedToProjFactorsRollingShutter) { using namespace vanillaPoseRS; Point2Vector measurements_lmk1; @@ -683,10 +741,15 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRolli Values values; values.insert(x1, level_pose); values.insert(x2, pose_right); - // initialize third pose with some noise to get a nontrivial linearization point + // initialize third pose with some noise to get a nontrivial linearization + // point values.insert(x3, pose_above * noise_pose); EXPECT( // check that the pose is actually noisy - assert_equal( Pose3( Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), Point3(0.1, -0.1, 1.9)), values.at(x3))); + assert_equal(Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656, + -0.0313952598, -0.000986635786, 0.0314107591, + -0.999013364, -0.0313952598), + Point3(0.1, -0.1, 1.9)), + values.at(x3))); // linearization point for the poses Pose3 pose1 = level_pose; @@ -695,8 +758,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRolli // ==== check Hessian of smartFactor1 ===== // -- compute actual Hessian - boost::shared_ptr linearfactor1 = smartFactor1->linearize( - values); + boost::shared_ptr linearfactor1 = + smartFactor1->linearize(values); Matrix actualHessian = linearfactor1->information(); // -- compute expected Hessian from manual Schur complement from Jacobians @@ -714,46 +777,52 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRolli ProjectionFactorRollingShutter factor11(measurements_lmk1[0], interp_factor1, model, x1, x2, l0, sharedK); Matrix H1Actual, H2Actual, H3Actual; - // note: b is minus the reprojection error, cf the smart factor jacobian computation - b.segment<2>(0) = -factor11.evaluateError(pose1, pose2, *point, H1Actual, H2Actual, H3Actual); + // note: b is minus the reprojection error, cf the smart factor jacobian + // computation + b.segment<2>(0) = -factor11.evaluateError(pose1, pose2, *point, H1Actual, + H2Actual, H3Actual); F.block<2, 6>(0, 0) = H1Actual; F.block<2, 6>(0, 6) = H2Actual; E.block<2, 3>(0, 0) = H3Actual; ProjectionFactorRollingShutter factor12(measurements_lmk1[1], interp_factor2, model, x2, x3, l0, sharedK); - b.segment<2>(2) = -factor12.evaluateError(pose2, pose3, *point, H1Actual, H2Actual, H3Actual); + b.segment<2>(2) = -factor12.evaluateError(pose2, pose3, *point, H1Actual, + H2Actual, H3Actual); F.block<2, 6>(2, 6) = H1Actual; F.block<2, 6>(2, 12) = H2Actual; E.block<2, 3>(2, 0) = H3Actual; ProjectionFactorRollingShutter factor13(measurements_lmk1[2], interp_factor3, model, x3, x1, l0, sharedK); - b.segment<2>(4) = -factor13.evaluateError(pose3, pose1, *point, H1Actual, H2Actual, H3Actual); + b.segment<2>(4) = -factor13.evaluateError(pose3, pose1, *point, H1Actual, + H2Actual, H3Actual); F.block<2, 6>(4, 12) = H1Actual; F.block<2, 6>(4, 0) = H2Actual; E.block<2, 3>(4, 0) = H3Actual; // whiten - F = (1/sigma) * F; - E = (1/sigma) * E; - b = (1/sigma) * b; + F = (1 / sigma) * F; + E = (1 / sigma) * E; + b = (1 / sigma) * b; //* G = F' * F - F' * E * P * E' * F Matrix P = (E.transpose() * E).inverse(); - Matrix expectedHessian = F.transpose() * F - - (F.transpose() * E * P * E.transpose() * F); + Matrix expectedHessian = + F.transpose() * F - (F.transpose() * E * P * E.transpose() * F); EXPECT(assert_equal(expectedHessian, actualHessian, 1e-6)); // ==== check Information vector of smartFactor1 ===== GaussianFactorGraph gfg; gfg.add(linearfactor1); Matrix actualHessian_v2 = gfg.hessian().first; - EXPECT(assert_equal(actualHessian_v2, actualHessian, 1e-6)); // sanity check on hessian + EXPECT(assert_equal(actualHessian_v2, actualHessian, + 1e-6)); // sanity check on hessian // -- compute actual information vector Vector actualInfoVector = gfg.hessian().second; - // -- compute expected information vector from manual Schur complement from Jacobians + // -- compute expected information vector from manual Schur complement from + // Jacobians //* g = F' * (b - E * P * E' * b) Vector expectedInfoVector = F.transpose() * (b - E * P * E.transpose() * b); EXPECT(assert_equal(expectedInfoVector, actualInfoVector, 1e-6)); @@ -771,9 +840,11 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRolli } /* *************************************************************************/ -TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRollingShutter_measurementsFromSamePose) { - // in this test we make sure the fact works even if we have multiple pixel measurements of the same landmark - // at a single pose, a setup that occurs in multi-camera systems +TEST(SmartProjectionPoseFactorRollingShutter, + hessianComparedToProjFactorsRollingShutter_measurementsFromSamePose) { + // in this test we make sure the fact works even if we have multiple pixel + // measurements of the same landmark at a single pose, a setup that occurs in + // multi-camera systems using namespace vanillaPoseRS; Point2Vector measurements_lmk1; @@ -783,7 +854,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRolli // create redundant measurements: Camera::MeasurementVector measurements_lmk1_redundant = measurements_lmk1; - measurements_lmk1_redundant.push_back(measurements_lmk1.at(0)); // we readd the first measurement + measurements_lmk1_redundant.push_back( + measurements_lmk1.at(0)); // we readd the first measurement // create inputs std::vector> key_pairs; @@ -799,17 +871,23 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRolli interp_factors.push_back(interp_factor1); SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); - smartFactor1->add(measurements_lmk1_redundant, key_pairs, interp_factors, sharedK); + smartFactor1->add(measurements_lmk1_redundant, key_pairs, interp_factors, + sharedK); Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, level_pose); values.insert(x2, pose_right); - // initialize third pose with some noise to get a nontrivial linearization point + // initialize third pose with some noise to get a nontrivial linearization + // point values.insert(x3, pose_above * noise_pose); EXPECT( // check that the pose is actually noisy - assert_equal( Pose3( Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), Point3(0.1, -0.1, 1.9)), values.at(x3))); + assert_equal(Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656, + -0.0313952598, -0.000986635786, 0.0314107591, + -0.999013364, -0.0313952598), + Point3(0.1, -0.1, 1.9)), + values.at(x3))); // linearization point for the poses Pose3 pose1 = level_pose; @@ -818,8 +896,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRolli // ==== check Hessian of smartFactor1 ===== // -- compute actual Hessian - boost::shared_ptr linearfactor1 = smartFactor1->linearize( - values); + boost::shared_ptr linearfactor1 = + smartFactor1->linearize(values); Matrix actualHessian = linearfactor1->information(); // -- compute expected Hessian from manual Schur complement from Jacobians @@ -828,62 +906,74 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRolli TriangulationResult point = smartFactor1->point(); EXPECT(point.valid()); // check triangulated point is valid - // Use standard ProjectionFactorRollingShutter factor to calculate the Jacobians + // Use standard ProjectionFactorRollingShutter factor to calculate the + // Jacobians Matrix F = Matrix::Zero(2 * 4, 6 * 3); Matrix E = Matrix::Zero(2 * 4, 3); Vector b = Vector::Zero(8); // create projection factors rolling shutter - ProjectionFactorRollingShutter factor11(measurements_lmk1_redundant[0], interp_factor1, - model, x1, x2, l0, sharedK); + ProjectionFactorRollingShutter factor11(measurements_lmk1_redundant[0], + interp_factor1, model, x1, x2, l0, + sharedK); Matrix H1Actual, H2Actual, H3Actual; - // note: b is minus the reprojection error, cf the smart factor jacobian computation - b.segment<2>(0) = -factor11.evaluateError(pose1, pose2, *point, H1Actual, H2Actual, H3Actual); + // note: b is minus the reprojection error, cf the smart factor jacobian + // computation + b.segment<2>(0) = -factor11.evaluateError(pose1, pose2, *point, H1Actual, + H2Actual, H3Actual); F.block<2, 6>(0, 0) = H1Actual; F.block<2, 6>(0, 6) = H2Actual; E.block<2, 3>(0, 0) = H3Actual; - ProjectionFactorRollingShutter factor12(measurements_lmk1_redundant[1], interp_factor2, - model, x2, x3, l0, sharedK); - b.segment<2>(2) = -factor12.evaluateError(pose2, pose3, *point, H1Actual, H2Actual, H3Actual); + ProjectionFactorRollingShutter factor12(measurements_lmk1_redundant[1], + interp_factor2, model, x2, x3, l0, + sharedK); + b.segment<2>(2) = -factor12.evaluateError(pose2, pose3, *point, H1Actual, + H2Actual, H3Actual); F.block<2, 6>(2, 6) = H1Actual; F.block<2, 6>(2, 12) = H2Actual; E.block<2, 3>(2, 0) = H3Actual; - ProjectionFactorRollingShutter factor13(measurements_lmk1_redundant[2], interp_factor3, - model, x3, x1, l0, sharedK); - b.segment<2>(4) = -factor13.evaluateError(pose3, pose1, *point, H1Actual, H2Actual, H3Actual); + ProjectionFactorRollingShutter factor13(measurements_lmk1_redundant[2], + interp_factor3, model, x3, x1, l0, + sharedK); + b.segment<2>(4) = -factor13.evaluateError(pose3, pose1, *point, H1Actual, + H2Actual, H3Actual); F.block<2, 6>(4, 12) = H1Actual; F.block<2, 6>(4, 0) = H2Actual; E.block<2, 3>(4, 0) = H3Actual; - ProjectionFactorRollingShutter factor14(measurements_lmk1_redundant[3], interp_factor1, - model, x1, x2, l0, sharedK); - b.segment<2>(6) = -factor11.evaluateError(pose1, pose2, *point, H1Actual, H2Actual, H3Actual); + ProjectionFactorRollingShutter factor14(measurements_lmk1_redundant[3], + interp_factor1, model, x1, x2, l0, + sharedK); + b.segment<2>(6) = -factor11.evaluateError(pose1, pose2, *point, H1Actual, + H2Actual, H3Actual); F.block<2, 6>(6, 0) = H1Actual; F.block<2, 6>(6, 6) = H2Actual; E.block<2, 3>(6, 0) = H3Actual; // whiten - F = (1/sigma) * F; - E = (1/sigma) * E; - b = (1/sigma) * b; + F = (1 / sigma) * F; + E = (1 / sigma) * E; + b = (1 / sigma) * b; //* G = F' * F - F' * E * P * E' * F Matrix P = (E.transpose() * E).inverse(); - Matrix expectedHessian = F.transpose() * F - - (F.transpose() * E * P * E.transpose() * F); + Matrix expectedHessian = + F.transpose() * F - (F.transpose() * E * P * E.transpose() * F); EXPECT(assert_equal(expectedHessian, actualHessian, 1e-6)); // ==== check Information vector of smartFactor1 ===== GaussianFactorGraph gfg; gfg.add(linearfactor1); Matrix actualHessian_v2 = gfg.hessian().first; - EXPECT(assert_equal(actualHessian_v2, actualHessian, 1e-6)); // sanity check on hessian + EXPECT(assert_equal(actualHessian_v2, actualHessian, + 1e-6)); // sanity check on hessian // -- compute actual information vector Vector actualInfoVector = gfg.hessian().second; - // -- compute expected information vector from manual Schur complement from Jacobians + // -- compute expected information vector from manual Schur complement from + // Jacobians //* g = F' * (b - E * P * E' * b) Vector expectedInfoVector = F.transpose() * (b - E * P * E.transpose() * b); EXPECT(assert_equal(expectedInfoVector, actualInfoVector, 1e-6)); @@ -902,8 +992,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRolli } /* *************************************************************************/ -TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_measurementsFromSamePose ) { - +TEST(SmartProjectionPoseFactorRollingShutter, + optimization_3poses_measurementsFromSamePose) { using namespace vanillaPoseRS; Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; @@ -913,27 +1003,32 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_measurementsF projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); // create inputs - std::vector> key_pairs; - key_pairs.push_back(std::make_pair(x1,x2)); - key_pairs.push_back(std::make_pair(x2,x3)); - key_pairs.push_back(std::make_pair(x3,x1)); + std::vector> key_pairs; + key_pairs.push_back(std::make_pair(x1, x2)); + key_pairs.push_back(std::make_pair(x2, x3)); + key_pairs.push_back(std::make_pair(x3, x1)); std::vector interp_factors; interp_factors.push_back(interp_factor1); interp_factors.push_back(interp_factor2); interp_factors.push_back(interp_factor3); - // For first factor, we create redundant measurement (taken by the same keys as factor 1, to - // make sure the redundancy in the keys does not create problems) + // For first factor, we create redundant measurement (taken by the same keys + // as factor 1, to make sure the redundancy in the keys does not create + // problems) Camera::MeasurementVector& measurements_lmk1_redundant = measurements_lmk1; - measurements_lmk1_redundant.push_back(measurements_lmk1.at(0)); // we readd the first measurement - std::vector> key_pairs_redundant = key_pairs; - key_pairs_redundant.push_back(key_pairs.at(0)); // we readd the first pair of keys + measurements_lmk1_redundant.push_back( + measurements_lmk1.at(0)); // we readd the first measurement + std::vector> key_pairs_redundant = key_pairs; + key_pairs_redundant.push_back( + key_pairs.at(0)); // we readd the first pair of keys std::vector interp_factors_redundant = interp_factors; - interp_factors_redundant.push_back(interp_factors.at(0));// we readd the first interp factor + interp_factors_redundant.push_back( + interp_factors.at(0)); // we readd the first interp factor SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); - smartFactor1->add(measurements_lmk1_redundant, key_pairs_redundant, interp_factors_redundant, sharedK); + smartFactor1->add(measurements_lmk1_redundant, key_pairs_redundant, + interp_factors_redundant, sharedK); SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model)); smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, sharedK); @@ -956,20 +1051,22 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_measurementsF groundTruth.insert(x3, pose_above); DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9); - // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), + // Point3(0.5,0.1,0.3)); // noise from regular projection factor test below Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), - Point3(0.1, 0.1, 0.1)); // smaller noise + Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, level_pose); values.insert(x2, pose_right); - // initialize third pose with some noise, we expect it to move back to original pose_above + // initialize third pose with some noise, we expect it to move back to + // original pose_above values.insert(x3, pose_above * noise_pose); - EXPECT( // check that the pose is actually noisy - assert_equal( - Pose3( - Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, - -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), - Point3(0.1, -0.1, 1.9)), values.at(x3))); + EXPECT( // check that the pose is actually noisy + assert_equal(Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656, + -0.0313952598, -0.000986635786, 0.0314107591, + -0.999013364, -0.0313952598), + Point3(0.1, -0.1, 1.9)), + values.at(x3))); Values result; LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); @@ -980,11 +1077,11 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_measurementsF #ifndef DISABLE_TIMING #include // -Total: 0 CPU (0 times, 0 wall, 0.04 children, min: 0 max: 0) -//| -SF RS LINEARIZE: 0.02 CPU (1000 times, 0.017244 wall, 0.02 children, min: 0 max: 0) -//| -RS LINEARIZE: 0.02 CPU (1000 times, 0.009035 wall, 0.02 children, min: 0 max: 0) +//| -SF RS LINEARIZE: 0.02 CPU (1000 times, 0.017244 wall, 0.02 children, min: +// 0 max: 0) | -RS LINEARIZE: 0.02 CPU (1000 times, 0.009035 wall, 0.02 +// children, min: 0 max: 0) /* *************************************************************************/ -TEST( SmartProjectionPoseFactorRollingShutter, timing ) { - +TEST(SmartProjectionPoseFactorRollingShutter, timing) { using namespace vanillaPose; // Default cameras for simple derivatives @@ -1007,14 +1104,14 @@ TEST( SmartProjectionPoseFactorRollingShutter, timing ) { size_t nrTests = 1000; - for(size_t i = 0; iadd(measurements_lmk1[0], x1, x2, interp_factor, sharedKSimple, - body_P_sensorId); + smartFactorRS->add(measurements_lmk1[0], x1, x2, interp_factor, + sharedKSimple, body_P_sensorId); interp_factor = 1; // equivalent to measurement taken at pose 2 - smartFactorRS->add(measurements_lmk1[1], x1, x2, interp_factor, sharedKSimple, - body_P_sensorId); + smartFactorRS->add(measurements_lmk1[1], x1, x2, interp_factor, + sharedKSimple, body_P_sensorId); Values values; values.insert(x1, pose1); @@ -1024,7 +1121,7 @@ TEST( SmartProjectionPoseFactorRollingShutter, timing ) { gttoc_(SF_RS_LINEARIZE); } - for(size_t i = 0; iadd(measurements_lmk1[0], x1); smartFactor->add(measurements_lmk1[1], x2); @@ -1046,4 +1143,3 @@ int main() { return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ - From 9798bfa8151788b31e225be9acce0ca4c4d30b10 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 28 Aug 2021 17:46:12 -0400 Subject: [PATCH 185/248] Cleaned up interpolate --- gtsam/base/Lie.h | 26 +++++++++++++------------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index 34422edd7..ac7c2a9a5 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -320,28 +320,28 @@ T expm(const Vector& x, int K=7) { } /** - * Linear interpolation between X and Y by coefficient t (typically t \in [0,1], - * but can also be used to extrapolate before pose X or after pose Y), with optional jacobians. + * Linear interpolation between X and Y by coefficient t. Typically t \in [0,1], + * but can also be used to extrapolate before pose X or after pose Y. */ template T interpolate(const T& X, const T& Y, double t, typename MakeOptionalJacobian::type Hx = boost::none, typename MakeOptionalJacobian::type Hy = boost::none) { - if (Hx || Hy) { - typename traits::TangentVector log_Xinv_Y; typename MakeJacobian::type between_H_x, log_H, exp_H, compose_H_x; + const T between = + traits::Between(X, Y, between_H_x); // between_H_y = identity + typename traits::TangentVector delta = traits::Logmap(between, log_H); + const T Delta = traits::Expmap(t * delta, exp_H); + const T result = traits::Compose( + X, Delta, compose_H_x); // compose_H_xinv_y = identity - T Xinv_Y = traits::Between(X, Y, between_H_x); // between_H_y = identity - log_Xinv_Y = traits::Logmap(Xinv_Y, log_H); - Xinv_Y = traits::Expmap(t * log_Xinv_Y, exp_H); - Xinv_Y = traits::Compose(X, Xinv_Y, compose_H_x); // compose_H_xinv_y = identity - - if(Hx) *Hx = compose_H_x + t * exp_H * log_H * between_H_x; - if(Hy) *Hy = t * exp_H * log_H; - return Xinv_Y; + if (Hx) *Hx = compose_H_x + t * exp_H * log_H * between_H_x; + if (Hy) *Hy = t * exp_H * log_H; + return result; } - return traits::Compose(X, traits::Expmap(t * traits::Logmap(traits::Between(X, Y)))); + return traits::Compose( + X, traits::Expmap(t * traits::Logmap(traits::Between(X, Y)))); } /** From 066bd0f05bb91a0f7aedfbc7a382bfef2acf0869 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 28 Aug 2021 20:03:03 -0400 Subject: [PATCH 186/248] verbose python testing --- .github/scripts/python.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/scripts/python.sh b/.github/scripts/python.sh index 22098ec08..3f5701281 100644 --- a/.github/scripts/python.sh +++ b/.github/scripts/python.sh @@ -85,4 +85,4 @@ make -j2 install cd $GITHUB_WORKSPACE/build/python $PYTHON setup.py install --user --prefix= cd $GITHUB_WORKSPACE/python/gtsam/tests -$PYTHON -m unittest discover +$PYTHON -m unittest discover -v From 65837c103010beb48ddc152bc684983c1fd8671b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 29 Aug 2021 04:21:57 -0400 Subject: [PATCH 187/248] Fix bug in FourierBasis --- gtsam/basis/Fourier.h | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/gtsam/basis/Fourier.h b/gtsam/basis/Fourier.h index 6943150d7..d264e182d 100644 --- a/gtsam/basis/Fourier.h +++ b/gtsam/basis/Fourier.h @@ -40,9 +40,13 @@ class GTSAM_EXPORT FourierBasis : public Basis { static Weights CalculateWeights(size_t N, double x) { Weights b(N); b[0] = 1; - for (size_t i = 1, n = 1; i < N; i += 2, n++) { - b[i] = cos(n * x); - b[i + 1] = sin(n * x); + for (size_t i = 1, n = 1; i < N; i++) { + if (i % 2 == 1) { + b[i] = cos(n * x); + } else { + b[i] = sin(n * x); + n++; + } } return b; } From 289cb8f35b1b68f280b220f3eb755a5d44148eae Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 29 Aug 2021 04:36:57 -0400 Subject: [PATCH 188/248] break down tests to make reporting clearer --- python/gtsam/tests/test_basis.py | 92 ++++++++++++++++++++++---------- 1 file changed, 65 insertions(+), 27 deletions(-) diff --git a/python/gtsam/tests/test_basis.py b/python/gtsam/tests/test_basis.py index 3af0a87f1..5d3c5ace3 100644 --- a/python/gtsam/tests/test_basis.py +++ b/python/gtsam/tests/test_basis.py @@ -19,39 +19,77 @@ from gtsam.symbol_shorthand import B class TestBasis(GtsamTestCase): """ - Tests Basis module python bindings. + Tests FitBasis python binding for FourierBasis, Chebyshev1Basis, Chebyshev2Basis, and Chebyshev2. + + It tests FitBasis by fitting to a ground-truth function that can be represented exactly in + the basis, then checking that the regression (fit result) matches the function. For the + Chebyshev bases, the line y=x is used to generate the data while for Fourier, 0.7*cos(x) is + used. """ - def test_fit_basis(self): + def setUp(self): + self.N = 2 + self.x = [0., 0.5, 0.75] + self.interpx = np.linspace(0., 1., 10) + self.noise = gtsam.noiseModel.Unit.Create(1) + + def evaluate(self, basis, fitparams, x): """ - Tests FitBasis python binding for FourierBasis, Chebyshev1Basis, Chebyshev2Basis, and - Chebyshev2. - It tests FitBasis by fitting to a ground-truth function that can be represented exactly in - the basis, then checking that the regression (fit result) matches the function. For the - Chebyshev bases, the line y=x is used to generate the data while for Fourier, 0.7*cos(x) is - used. + Until wrapper for Basis functors are ready, + this is how to evaluate a basis function. """ - f = lambda x: x # line y = x - N = 2 - datax = [0., 0.5, 0.75] - interpx = np.linspace(0., 1., 10) - noise = gtsam.noiseModel.Unit.Create(1) + return basis.WeightMatrix(self.N, x) @ fitparams - def evaluate(basis, fitparams, x): - # until wrapper for Basis functors are ready, this is how to evaluate a basis function. - return basis.WeightMatrix(N, x) @ fitparams + def fit_basis_helper(self, fitter, basis, f=lambda x: x): + """Helper method to fit data to a specified fitter using a specified basis.""" + data = {x: f(x) for x in self.x} + fit = fitter(data, self.noise, self.N) + coeff = fit.parameters() + interpy = self.evaluate(basis, coeff, self.interpx) + return interpy - def testBasis(fitter, basis, f=f): - # test a basis by checking that the fit result matches the function at x-values interpx. - data = {x: f(x) for x in datax} - fit = fitter(data, noise, N) - coeff = fit.parameters() - interpy = evaluate(basis, coeff, interpx) - np.testing.assert_almost_equal(interpy, np.array([f(x) for x in interpx]), decimal=7) + def test_fit_basis_fourier(self): + """Fit a Fourier basis.""" - testBasis(gtsam.FitBasisFourierBasis, gtsam.FourierBasis, f=lambda x: 0.7 * np.cos(x)) - testBasis(gtsam.FitBasisChebyshev1Basis, gtsam.Chebyshev1Basis) - testBasis(gtsam.FitBasisChebyshev2Basis, gtsam.Chebyshev2Basis) - testBasis(gtsam.FitBasisChebyshev2, gtsam.Chebyshev2) + f = lambda x: 0.7 * np.cos(x) + interpy = self.fit_basis_helper(gtsam.FitBasisFourierBasis, + gtsam.FourierBasis, f) + # test a basis by checking that the fit result matches the function at x-values interpx. + np.testing.assert_almost_equal(interpy, + np.array([f(x) for x in self.interpx]), + decimal=7) + + def test_fit_basis_chebyshev1basis(self): + """Fit a Chebyshev1 basis.""" + + f = lambda x: x + interpy = self.fit_basis_helper(gtsam.FitBasisChebyshev1Basis, + gtsam.Chebyshev1Basis, f) + # test a basis by checking that the fit result matches the function at x-values interpx. + np.testing.assert_almost_equal(interpy, + np.array([f(x) for x in self.interpx]), + decimal=7) + + def test_fit_basis_chebyshev2basis(self): + """Fit a Chebyshev2 basis.""" + + f = lambda x: x + interpy = self.fit_basis_helper(gtsam.FitBasisChebyshev2Basis, + gtsam.Chebyshev2Basis) + # test a basis by checking that the fit result matches the function at x-values interpx. + np.testing.assert_almost_equal(interpy, + np.array([f(x) for x in self.interpx]), + decimal=7) + + def test_fit_basis_chebyshev2(self): + """Fit a Chebyshev2 pseudospectral basis.""" + + f = lambda x: x + interpy = self.fit_basis_helper(gtsam.FitBasisChebyshev2, + gtsam.Chebyshev2) + # test a basis by checking that the fit result matches the function at x-values interpx. + np.testing.assert_almost_equal(interpy, + np.array([f(x) for x in self.interpx]), + decimal=7) if __name__ == "__main__": From 13c164f25d4683bd1720b564ee3dcbf67b4ffff5 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Mon, 30 Aug 2021 16:38:47 -0400 Subject: [PATCH 189/248] add Pose2.align() to wrapper --- gtsam/geometry/geometry.i | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 0e303cbcd..c495c22d9 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -31,6 +31,14 @@ class Point2 { // enable pickling in python void pickle() const; }; + +class Point2Pairs { + Point2Pairs(); + size_t size() const; + bool empty() const; + gtsam::Point2Pair at(size_t n) const; + void push_back(const gtsam::Point2Pair& point_pair); +}; // std::vector class Point2Vector { @@ -428,6 +436,8 @@ class Pose2 { // enable pickling in python void pickle() const; + + gtsam::Pose2 align(const gtsam::Point2Pairs& pairs); }; #include From ce495cb7bc0ab3ed5fac8482e083f629aafeb684 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Mon, 30 Aug 2021 16:41:27 -0400 Subject: [PATCH 190/248] add Point2Pairs typedef to Point2.h --- gtsam/geometry/Point2.h | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index 17f87f656..cdb9f4480 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -25,6 +25,12 @@ namespace gtsam { /// As of GTSAM 4, in order to make GTSAM more lean, /// it is now possible to just typedef Point2 to Vector2 typedef Vector2 Point2; + +// Convenience typedef +using Point2Pair = std::pair; +GTSAM_EXPORT std::ostream &operator<<(std::ostream &os, const gtsam::Point2Pair &p); + +using Point2Pairs = std::vector; /// Distance of the point from the origin, with Jacobian GTSAM_EXPORT double norm2(const Point2& p, OptionalJacobian<1, 2> H = boost::none); @@ -34,10 +40,6 @@ GTSAM_EXPORT double distance2(const Point2& p1, const Point2& q, OptionalJacobian<1, 2> H1 = boost::none, OptionalJacobian<1, 2> H2 = boost::none); -// Convenience typedef -typedef std::pair Point2Pair; -GTSAM_EXPORT std::ostream &operator<<(std::ostream &os, const gtsam::Point2Pair &p); - // For MATLAB wrapper typedef std::vector > Point2Vector; From 8bd2e6a976071ae4f5fa6063ec6d40c5a38de0c9 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Mon, 30 Aug 2021 16:44:11 -0400 Subject: [PATCH 191/248] add gtsam::Point2Pairs to CMakeLists.txt --- python/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 4254a21c6..c3524adad 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -43,6 +43,7 @@ set(ignore gtsam::BetweenFactorPose2s gtsam::BetweenFactorPose3s gtsam::Point2Vector + gtsam::Point2Pairs gtsam::Point3Pairs gtsam::Pose3Pairs gtsam::Pose3Vector From 55785f81809777fdc0115a0761a16f10f5ad8ab6 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Mon, 30 Aug 2021 16:44:58 -0400 Subject: [PATCH 192/248] add Point2Pairs to specializations --- python/gtsam/specializations/geometry.h | 1 + 1 file changed, 1 insertion(+) diff --git a/python/gtsam/specializations/geometry.h b/python/gtsam/specializations/geometry.h index e11d3cc4f..a492ce8eb 100644 --- a/python/gtsam/specializations/geometry.h +++ b/python/gtsam/specializations/geometry.h @@ -14,6 +14,7 @@ py::bind_vector< std::vector>>( m_, "Point2Vector"); +py::bind_vector>(m_, "Point2Pairs"); py::bind_vector>(m_, "Point3Pairs"); py::bind_vector>(m_, "Pose3Pairs"); py::bind_vector>(m_, "Pose3Vector"); From 50f3b93324a6041aad6465159b1cddc86724dca4 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Mon, 30 Aug 2021 18:01:17 -0600 Subject: [PATCH 193/248] move align as function, not as class method --- gtsam/geometry/geometry.i | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index c495c22d9..bf906d67f 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -436,9 +436,9 @@ class Pose2 { // enable pickling in python void pickle() const; - - gtsam::Pose2 align(const gtsam::Point2Pairs& pairs); }; + +gtsam::Pose2 align(const gtsam::Point2Pairs& pairs); #include class Pose3 { From cff3c5b4f43662c81a095fd9123afeae94f9970c Mon Sep 17 00:00:00 2001 From: John Lambert Date: Tue, 31 Aug 2021 06:26:52 -0600 Subject: [PATCH 194/248] start python unit test for align() --- python/gtsam/tests/test_Pose2.py | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/python/gtsam/tests/test_Pose2.py b/python/gtsam/tests/test_Pose2.py index 9652b594a..b44914a94 100644 --- a/python/gtsam/tests/test_Pose2.py +++ b/python/gtsam/tests/test_Pose2.py @@ -6,7 +6,7 @@ All Rights Reserved See LICENSE for the license information Pose2 unit tests. -Author: Frank Dellaert & Duy Nguyen Ta (Python) +Author: Frank Dellaert & Duy Nguyen Ta & John Lambert """ import unittest @@ -20,13 +20,20 @@ from gtsam.utils.test_case import GtsamTestCase class TestPose2(GtsamTestCase): """Test selected Pose2 methods.""" - def test_adjoint(self): + def test_adjoint(self) -> None: """Test adjoint method.""" xi = np.array([1, 2, 3]) expected = np.dot(Pose2.adjointMap_(xi), xi) actual = Pose2.adjoint_(xi, xi) np.testing.assert_array_equal(actual, expected) + def test_align(self) -> None: + """Ensure estimation of the Pose2 element to align two 2d point clouds succeeds. + + """ + pass + + if __name__ == "__main__": unittest.main() From 2d2ca21d1aea4d5bd4b92523075553598afe60c0 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Tue, 31 Aug 2021 09:14:59 -0400 Subject: [PATCH 195/248] add python unit test on Pose2.align() --- python/gtsam/tests/test_Pose2.py | 36 ++++++++++++++++++++++++++++++-- 1 file changed, 34 insertions(+), 2 deletions(-) diff --git a/python/gtsam/tests/test_Pose2.py b/python/gtsam/tests/test_Pose2.py index b44914a94..fc9c7acf9 100644 --- a/python/gtsam/tests/test_Pose2.py +++ b/python/gtsam/tests/test_Pose2.py @@ -30,9 +30,41 @@ class TestPose2(GtsamTestCase): def test_align(self) -> None: """Ensure estimation of the Pose2 element to align two 2d point clouds succeeds. - """ - pass + Two point clouds represent horseshoe-shapes of the same size, just rotated and translated: + | X---X + | | + | X---X + ------------------ + | + | + O | O + | | | + O---O + """ + # fmt: off + pts_a = [ + Point2(3, 1), + Point2(1, 1), + Point2(1, 3), + Point2(3, 3), + ] + pts_b = [ + Point2(1, -3), + Point2(1, -5), + Point2(-1, -5), + Point2(-1, -3) + ] + + # fmt: on + ab_pairs = Point2Pairs(list(zip(pts_a, pts_b))) + bTa = Pose2.align(ab_pairs) + aTb = bTa.inverse() + assert aTb is not None + + for pt_a, pt_b in zip(pts_a, pts_b): + pt_a_ = aTb.transformFrom(pt_b) + assert np.allclose(pt_a, pt_a_) if __name__ == "__main__": From 5c737c3cc4592a588a6bfd781b277de1feeb67a1 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Tue, 31 Aug 2021 09:42:49 -0400 Subject: [PATCH 196/248] fix missing imports --- python/gtsam/tests/test_Pose2.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/python/gtsam/tests/test_Pose2.py b/python/gtsam/tests/test_Pose2.py index fc9c7acf9..f18eab079 100644 --- a/python/gtsam/tests/test_Pose2.py +++ b/python/gtsam/tests/test_Pose2.py @@ -13,7 +13,7 @@ import unittest import numpy as np import gtsam -from gtsam import Pose2 +from gtsam import Point2, Point2Pairs, Pose2 from gtsam.utils.test_case import GtsamTestCase @@ -47,7 +47,7 @@ class TestPose2(GtsamTestCase): Point2(3, 1), Point2(1, 1), Point2(1, 3), - Point2(3, 3), + Point2(3, 3) ] pts_b = [ Point2(1, -3), From 3fc7692b4a72f62ef944532d554652f369543c6d Mon Sep 17 00:00:00 2001 From: John Lambert Date: Tue, 31 Aug 2021 08:35:43 -0600 Subject: [PATCH 197/248] import align from gtsam directly --- python/gtsam/tests/test_Pose2.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/python/gtsam/tests/test_Pose2.py b/python/gtsam/tests/test_Pose2.py index f18eab079..213de655e 100644 --- a/python/gtsam/tests/test_Pose2.py +++ b/python/gtsam/tests/test_Pose2.py @@ -13,7 +13,7 @@ import unittest import numpy as np import gtsam -from gtsam import Point2, Point2Pairs, Pose2 +from gtsam import align, Point2, Point2Pairs, Pose2 from gtsam.utils.test_case import GtsamTestCase @@ -58,7 +58,7 @@ class TestPose2(GtsamTestCase): # fmt: on ab_pairs = Point2Pairs(list(zip(pts_a, pts_b))) - bTa = Pose2.align(ab_pairs) + bTa = align(ab_pairs) aTb = bTa.inverse() assert aTb is not None From bc641f893dc8b8e16f51ff579f8fd802573a6e78 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Tue, 31 Aug 2021 14:14:37 -0400 Subject: [PATCH 198/248] directly import only classes from gtsam --- python/gtsam/tests/test_Pose2.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/python/gtsam/tests/test_Pose2.py b/python/gtsam/tests/test_Pose2.py index 213de655e..569046596 100644 --- a/python/gtsam/tests/test_Pose2.py +++ b/python/gtsam/tests/test_Pose2.py @@ -13,7 +13,7 @@ import unittest import numpy as np import gtsam -from gtsam import align, Point2, Point2Pairs, Pose2 +from gtsam import Point2, Point2Pairs, Pose2 from gtsam.utils.test_case import GtsamTestCase @@ -58,7 +58,7 @@ class TestPose2(GtsamTestCase): # fmt: on ab_pairs = Point2Pairs(list(zip(pts_a, pts_b))) - bTa = align(ab_pairs) + bTa = gtsam.align(ab_pairs) aTb = bTa.inverse() assert aTb is not None From 6d57016a51b936c014167a693e7315fe40521a66 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Wed, 1 Sep 2021 10:15:00 -0600 Subject: [PATCH 199/248] use boost::optional in .i file directly --- gtsam/geometry/geometry.i | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index bf906d67f..9baa49e8e 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -438,7 +438,7 @@ class Pose2 { void pickle() const; }; -gtsam::Pose2 align(const gtsam::Point2Pairs& pairs); +boost::optional align(const gtsam::Point2Pairs& pairs); #include class Pose3 { From cd682fecc3301a8601e8f8bd4ffe951a6bf4bf15 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 1 Sep 2021 14:31:29 -0400 Subject: [PATCH 200/248] add a cmake flag for easy toggling BetweenFactor jacobian computations --- cmake/HandleGeneralOptions.cmake | 29 +++++++++++++++-------------- gtsam/config.h.in | 3 +++ gtsam/slam/BetweenFactor.h | 2 +- 3 files changed, 19 insertions(+), 15 deletions(-) diff --git a/cmake/HandleGeneralOptions.cmake b/cmake/HandleGeneralOptions.cmake index ee86066a2..64c239f39 100644 --- a/cmake/HandleGeneralOptions.cmake +++ b/cmake/HandleGeneralOptions.cmake @@ -14,20 +14,21 @@ if(GTSAM_UNSTABLE_AVAILABLE) option(GTSAM_UNSTABLE_BUILD_PYTHON "Enable/Disable Python wrapper for libgtsam_unstable" ON) option(GTSAM_UNSTABLE_INSTALL_MATLAB_TOOLBOX "Enable/Disable MATLAB wrapper for libgtsam_unstable" OFF) endif() -option(BUILD_SHARED_LIBS "Build shared gtsam library, instead of static" ON) -option(GTSAM_USE_QUATERNIONS "Enable/Disable using an internal Quaternion representation for rotations instead of rotation matrices. If enable, Rot3::EXPMAP is enforced by default." OFF) -option(GTSAM_POSE3_EXPMAP "Enable/Disable using Pose3::EXPMAP as the default mode. If disabled, Pose3::FIRST_ORDER will be used." ON) -option(GTSAM_ROT3_EXPMAP "Ignore if GTSAM_USE_QUATERNIONS is OFF (Rot3::EXPMAP by default). Otherwise, enable Rot3::EXPMAP, or if disabled, use Rot3::CAYLEY." ON) -option(GTSAM_ENABLE_CONSISTENCY_CHECKS "Enable/Disable expensive consistency checks" OFF) -option(GTSAM_WITH_TBB "Use Intel Threaded Building Blocks (TBB) if available" ON) -option(GTSAM_WITH_EIGEN_MKL "Eigen will use Intel MKL if available" OFF) -option(GTSAM_WITH_EIGEN_MKL_OPENMP "Eigen, when using Intel MKL, will also use OpenMP for multithreading if available" OFF) -option(GTSAM_THROW_CHEIRALITY_EXCEPTION "Throw exception when a triangulated point is behind a camera" ON) -option(GTSAM_BUILD_PYTHON "Enable/Disable building & installation of Python module with pybind11" OFF) -option(GTSAM_INSTALL_MATLAB_TOOLBOX "Enable/Disable installation of matlab toolbox" OFF) -option(GTSAM_ALLOW_DEPRECATED_SINCE_V41 "Allow use of methods/functions deprecated in GTSAM 4.1" ON) -option(GTSAM_SUPPORT_NESTED_DISSECTION "Support Metis-based nested dissection" ON) -option(GTSAM_TANGENT_PREINTEGRATION "Use new ImuFactor with integration on tangent space" ON) +option(BUILD_SHARED_LIBS "Build shared gtsam library, instead of static" ON) +option(GTSAM_USE_QUATERNIONS "Enable/Disable using an internal Quaternion representation for rotations instead of rotation matrices. If enable, Rot3::EXPMAP is enforced by default." OFF) +option(GTSAM_POSE3_EXPMAP "Enable/Disable using Pose3::EXPMAP as the default mode. If disabled, Pose3::FIRST_ORDER will be used." ON) +option(GTSAM_ROT3_EXPMAP "Ignore if GTSAM_USE_QUATERNIONS is OFF (Rot3::EXPMAP by default). Otherwise, enable Rot3::EXPMAP, or if disabled, use Rot3::CAYLEY." ON) +option(GTSAM_ENABLE_CONSISTENCY_CHECKS "Enable/Disable expensive consistency checks" OFF) +option(GTSAM_WITH_TBB "Use Intel Threaded Building Blocks (TBB) if available" ON) +option(GTSAM_WITH_EIGEN_MKL "Eigen will use Intel MKL if available" OFF) +option(GTSAM_WITH_EIGEN_MKL_OPENMP "Eigen, when using Intel MKL, will also use OpenMP for multithreading if available" OFF) +option(GTSAM_THROW_CHEIRALITY_EXCEPTION "Throw exception when a triangulated point is behind a camera" ON) +option(GTSAM_BUILD_PYTHON "Enable/Disable building & installation of Python module with pybind11" OFF) +option(GTSAM_INSTALL_MATLAB_TOOLBOX "Enable/Disable installation of matlab toolbox" OFF) +option(GTSAM_ALLOW_DEPRECATED_SINCE_V41 "Allow use of methods/functions deprecated in GTSAM 4.1" ON) +option(GTSAM_SUPPORT_NESTED_DISSECTION "Support Metis-based nested dissection" ON) +option(GTSAM_TANGENT_PREINTEGRATION "Use new ImuFactor with integration on tangent space" ON) +option(GTSAM_SLOW_BUT_CORRECT_BETWEENFACTOR "Use the slower but correct version of BetweenFactor" OFF) if(NOT MSVC AND NOT XCODE_VERSION) option(GTSAM_BUILD_WITH_CCACHE "Use ccache compiler cache" ON) endif() diff --git a/gtsam/config.h.in b/gtsam/config.h.in index 9d1bd4ebd..cebc1d0b6 100644 --- a/gtsam/config.h.in +++ b/gtsam/config.h.in @@ -77,3 +77,6 @@ // Support Metis-based nested dissection #cmakedefine GTSAM_TANGENT_PREINTEGRATION + +// Toggle switch for BetweenFactor jacobian computation +#cmakedefine GTSAM_SLOW_BUT_CORRECT_BETWEENFACTOR diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h index aef41d5fd..8a1ffdd72 100644 --- a/gtsam/slam/BetweenFactor.h +++ b/gtsam/slam/BetweenFactor.h @@ -103,7 +103,7 @@ namespace gtsam { boost::none, boost::optional H2 = boost::none) const override { T hx = traits::Between(p1, p2, H1, H2); // h(x) // manifold equivalent of h(x)-z -> log(z,h(x)) -#ifdef SLOW_BUT_CORRECT_BETWEENFACTOR +#ifdef GTSAM_SLOW_BUT_CORRECT_BETWEENFACTOR typename traits::ChartJacobian::Jacobian Hlocal; Vector rval = traits::Local(measured_, hx, boost::none, (H1 || H2) ? &Hlocal : 0); if (H1) *H1 = Hlocal * (*H1); From fbdef91c54aff13b6cdb1e80095627b4cd12b217 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 1 Sep 2021 15:29:29 -0400 Subject: [PATCH 201/248] add support for boost::optional return type in geometry.i --- python/gtsam/preamble/geometry.h | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/python/gtsam/preamble/geometry.h b/python/gtsam/preamble/geometry.h index 498c7dc58..9ddb6e208 100644 --- a/python/gtsam/preamble/geometry.h +++ b/python/gtsam/preamble/geometry.h @@ -10,9 +10,18 @@ * Without this they will be automatically converted to a Python object, and all * mutations on Python side will not be reflected on C++. */ +#include + +// Support for binding boost::optional types. +// https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html +namespace pybind11 { namespace detail { + template + struct type_caster> : optional_caster> {}; +}} PYBIND11_MAKE_OPAQUE( std::vector>); +PYBIND11_MAKE_OPAQUE(gtsam::Point2Pairs); PYBIND11_MAKE_OPAQUE(gtsam::Point3Pairs); PYBIND11_MAKE_OPAQUE(gtsam::Pose3Pairs); PYBIND11_MAKE_OPAQUE(std::vector); From 9f661c01cf0e7ce144a94a39f1a83343e03f29ed Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 1 Sep 2021 15:29:39 -0400 Subject: [PATCH 202/248] formatting --- python/gtsam/tests/test_Pose2.py | 12 +++++------- 1 file changed, 5 insertions(+), 7 deletions(-) diff --git a/python/gtsam/tests/test_Pose2.py b/python/gtsam/tests/test_Pose2.py index 569046596..e5ffbad7d 100644 --- a/python/gtsam/tests/test_Pose2.py +++ b/python/gtsam/tests/test_Pose2.py @@ -19,7 +19,6 @@ from gtsam.utils.test_case import GtsamTestCase class TestPose2(GtsamTestCase): """Test selected Pose2 methods.""" - def test_adjoint(self) -> None: """Test adjoint method.""" xi = np.array([1, 2, 3]) @@ -29,9 +28,9 @@ class TestPose2(GtsamTestCase): def test_align(self) -> None: """Ensure estimation of the Pose2 element to align two 2d point clouds succeeds. - + Two point clouds represent horseshoe-shapes of the same size, just rotated and translated: - + | X---X | | | X---X @@ -42,18 +41,17 @@ class TestPose2(GtsamTestCase): | | | O---O """ - # fmt: off pts_a = [ Point2(3, 1), Point2(1, 1), Point2(1, 3), - Point2(3, 3) + Point2(3, 3), ] pts_b = [ Point2(1, -3), Point2(1, -5), Point2(-1, -5), - Point2(-1, -3) + Point2(-1, -3), ] # fmt: on @@ -65,7 +63,7 @@ class TestPose2(GtsamTestCase): for pt_a, pt_b in zip(pts_a, pts_b): pt_a_ = aTb.transformFrom(pt_b) assert np.allclose(pt_a, pt_a_) - + if __name__ == "__main__": unittest.main() From 1205df2c07757d20b01d43827bc88f8b0f696884 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 1 Sep 2021 15:32:43 -0400 Subject: [PATCH 203/248] update documentation for boost::optional binding --- python/gtsam/preamble/geometry.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/gtsam/preamble/geometry.h b/python/gtsam/preamble/geometry.h index 9ddb6e208..35fe2a577 100644 --- a/python/gtsam/preamble/geometry.h +++ b/python/gtsam/preamble/geometry.h @@ -12,7 +12,7 @@ */ #include -// Support for binding boost::optional types. +// Support for binding boost::optional types in C++11. // https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html namespace pybind11 { namespace detail { template From 62b1e2ce9cf72d664fd540b2fb0eb78ea674d0d8 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Thu, 2 Sep 2021 15:27:49 -0400 Subject: [PATCH 204/248] use updated wrap syntax for Verbosity enum in .i file --- gtsam/nonlinear/nonlinear.i | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index c3f17c02e..39b4f5933 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -527,13 +527,15 @@ template virtual class GncParams { GncParams(const PARAMS& baseOptimizerParams); GncParams(); - void setVerbosityGNC(const gtsam::GncParams::Verbosity value); + void setVerbosityGNC(const This::Verbosity value); void print(const string& str) const; -}; -typedef gtsam::GncParams::Verbosity::SILENT GncVerbositySilent; -typedef gtsam::GncParams::Verbosity::SUMMARY GncVerbositySummary; -typedef gtsam::GncParams::Verbosity::VALUES GncVerbosityValues; + enum Verbosity { + SILENT, + SUMMARY, + VALUES + }; +}; typedef gtsam::GncParams GncGaussNewtonParams; typedef gtsam::GncParams GncLMParams; From 67a26c1f9359d68e39d3efe7414d8958e4c10e03 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 3 Sep 2021 08:04:59 -0400 Subject: [PATCH 205/248] refactor to remove all pylint errors --- python/gtsam/examples/ImuFactorExample.py | 114 +++++++++++++--------- 1 file changed, 66 insertions(+), 48 deletions(-) diff --git a/python/gtsam/examples/ImuFactorExample.py b/python/gtsam/examples/ImuFactorExample.py index bb707a8f5..86613234d 100644 --- a/python/gtsam/examples/ImuFactorExample.py +++ b/python/gtsam/examples/ImuFactorExample.py @@ -10,28 +10,30 @@ A script validating and demonstrating the ImuFactor inference. Author: Frank Dellaert, Varun Agrawal """ +# pylint: disable=no-name-in-module,unused-import,arguments-differ + from __future__ import print_function import argparse import math -import gtsam import matplotlib.pyplot as plt import numpy as np +from mpl_toolkits.mplot3d import Axes3D + +import gtsam from gtsam.symbol_shorthand import B, V, X from gtsam.utils.plot import plot_pose3 -from mpl_toolkits.mplot3d import Axes3D from PreintegrationExample import POSES_FIG, PreintegrationExample BIAS_KEY = B(0) - np.set_printoptions(precision=3, suppress=True) class ImuFactorExample(PreintegrationExample): - + """Class to run example of the Imu Factor.""" def __init__(self, twist_scenario="sick_twist"): self.velocity = np.array([2, 0, 0]) self.priorNoise = gtsam.noiseModel.Isotropic.Sigma(6, 0.1) @@ -42,9 +44,8 @@ class ImuFactorExample(PreintegrationExample): zero_twist=(np.zeros(3), np.zeros(3)), forward_twist=(np.zeros(3), self.velocity), loop_twist=(np.array([0, -math.radians(30), 0]), self.velocity), - sick_twist=(np.array([math.radians(30), -math.radians(30), 0]), - self.velocity) - ) + sick_twist=(np.array([math.radians(30), -math.radians(30), + 0]), self.velocity)) accBias = np.array([-0.3, 0.1, 0.2]) gyroBias = np.array([0.1, 0.3, -0.1]) @@ -55,19 +56,44 @@ class ImuFactorExample(PreintegrationExample): bias, dt) def addPrior(self, i, graph): + """Add priors at time step `i`.""" state = self.scenario.navState(i) - graph.push_back(gtsam.PriorFactorPose3( - X(i), state.pose(), self.priorNoise)) - graph.push_back(gtsam.PriorFactorVector( - V(i), state.velocity(), self.velNoise)) + graph.push_back( + gtsam.PriorFactorPose3(X(i), state.pose(), self.priorNoise)) + graph.push_back( + gtsam.PriorFactorVector(V(i), state.velocity(), self.velNoise)) + + def optimize(self, graph, initial): + """Optimize using Levenberg-Marquardt optimization.""" + params = gtsam.LevenbergMarquardtParams() + params.setVerbosityLM("SUMMARY") + optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params) + result = optimizer.optimize() + return result + + def plot(self, result): + """Plot resulting poses.""" + i = 0 + while result.exists(X(i)): + pose_i = result.atPose3(X(i)) + plot_pose3(POSES_FIG + 1, pose_i, 1) + i += 1 + plt.title("Estimated Trajectory") + + gtsam.utils.plot.set_axes_equal(POSES_FIG + 1) + + print("Bias Values", result.atConstantBias(BIAS_KEY)) + + plt.ioff() + plt.show() def run(self, T=12, compute_covariances=False, verbose=True): + """Main runner.""" graph = gtsam.NonlinearFactorGraph() # initialize data structure for pre-integrated IMU measurements pim = gtsam.PreintegratedImuMeasurements(self.params, self.actualBias) - T = 12 num_poses = T # assumes 1 factor per second initial = gtsam.Values() initial.insert(BIAS_KEY, self.actualBias) @@ -91,14 +117,13 @@ class ImuFactorExample(PreintegrationExample): if k % 10 == 0: self.plotImu(t, measuredOmega, measuredAcc) - if (k+1) % int(1 / self.dt) == 0: + if (k + 1) % int(1 / self.dt) == 0: # Plot every second self.plotGroundTruthPose(t, scale=1) plt.title("Ground Truth Trajectory") # create IMU factor every second - factor = gtsam.ImuFactor(X(i), V(i), - X(i + 1), V(i + 1), + factor = gtsam.ImuFactor(X(i), V(i), X(i + 1), V(i + 1), BIAS_KEY, pim) graph.push_back(factor) @@ -108,34 +133,34 @@ class ImuFactorExample(PreintegrationExample): pim.resetIntegration() - rotationNoise = gtsam.Rot3.Expmap(np.random.randn(3)*0.1) - translationNoise = gtsam.Point3(*np.random.randn(3)*1) + rotationNoise = gtsam.Rot3.Expmap(np.random.randn(3) * 0.1) + translationNoise = gtsam.Point3(*np.random.randn(3) * 1) poseNoise = gtsam.Pose3(rotationNoise, translationNoise) actual_state_i = self.scenario.navState(t + self.dt) print("Actual state at {0}:\n{1}".format( - t+self.dt, actual_state_i)) + t + self.dt, actual_state_i)) noisy_state_i = gtsam.NavState( actual_state_i.pose().compose(poseNoise), - actual_state_i.velocity() + np.random.randn(3)*0.1) + actual_state_i.velocity() + np.random.randn(3) * 0.1) - initial.insert(X(i+1), noisy_state_i.pose()) - initial.insert(V(i+1), noisy_state_i.velocity()) + initial.insert(X(i + 1), noisy_state_i.pose()) + initial.insert(V(i + 1), noisy_state_i.velocity()) i += 1 # add priors on end self.addPrior(num_poses - 1, graph) - initial.print_("Initial values:") + initial.print("Initial values:") - # optimize using Levenberg-Marquardt optimization - params = gtsam.LevenbergMarquardtParams() - params.setVerbosityLM("SUMMARY") - optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params) - result = optimizer.optimize() + result = self.optimize(graph, initial) - result.print_("Optimized values:") + result.print("Optimized values:") + print("------------------") + print(graph.error(initial)) + print(graph.error(result)) + print("------------------") if compute_covariances: # Calculate and print marginal covariances @@ -148,33 +173,26 @@ class ImuFactorExample(PreintegrationExample): print("Covariance on vel {}:\n{}\n".format( i, marginals.marginalCovariance(V(i)))) - # Plot resulting poses - i = 0 - while result.exists(X(i)): - pose_i = result.atPose3(X(i)) - plot_pose3(POSES_FIG+1, pose_i, 1) - i += 1 - plt.title("Estimated Trajectory") - - gtsam.utils.plot.set_axes_equal(POSES_FIG+1) - - print("Bias Values", result.atConstantBias(BIAS_KEY)) - - plt.ioff() - plt.show() + self.plot(result) if __name__ == '__main__': parser = argparse.ArgumentParser("ImuFactorExample.py") parser.add_argument("--twist_scenario", default="sick_twist", - choices=("zero_twist", "forward_twist", "loop_twist", "sick_twist")) - parser.add_argument("--time", "-T", default=12, - type=int, help="Total time in seconds") + choices=("zero_twist", "forward_twist", "loop_twist", + "sick_twist")) + parser.add_argument("--time", + "-T", + default=12, + type=int, + help="Total time in seconds") parser.add_argument("--compute_covariances", - default=False, action='store_true') + default=False, + action='store_true') parser.add_argument("--verbose", default=False, action='store_true') args = parser.parse_args() - ImuFactorExample(args.twist_scenario).run( - args.time, args.compute_covariances, args.verbose) + ImuFactorExample(args.twist_scenario).run(args.time, + args.compute_covariances, + args.verbose) From 5131f6b0a60a11980236edda2bfa0ba29c8bcb5a Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 3 Sep 2021 08:05:11 -0400 Subject: [PATCH 206/248] fix matplotlib deprecation --- python/gtsam/utils/plot.py | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/python/gtsam/utils/plot.py b/python/gtsam/utils/plot.py index f324da63a..7ea393077 100644 --- a/python/gtsam/utils/plot.py +++ b/python/gtsam/utils/plot.py @@ -23,7 +23,10 @@ def set_axes_equal(fignum: int) -> None: fignum: An integer representing the figure number for Matplotlib. """ fig = plt.figure(fignum) - ax = fig.gca(projection='3d') + if not fig.axes: + ax = fig.add_subplot(projection='3d') + else: + ax = fig.axes[0] limits = np.array([ ax.get_xlim3d(), @@ -339,7 +342,11 @@ def plot_pose3( """ # get figure object fig = plt.figure(fignum) - axes = fig.gca(projection='3d') + if not fig.axes: + axes = fig.add_subplot(projection='3d') + else: + axes = fig.axes[0] + plot_pose3_on_axes(axes, pose, P=P, axis_length=axis_length) axes.set_xlabel(axis_labels[0]) From 9bc3c0b6a06f47902f15cdf5bcc090c01399e471 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 5 Sep 2021 11:48:54 -0400 Subject: [PATCH 207/248] removed duplicate --- gtsam/navigation/tests/testImuFactor.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 801d87895..585da38b1 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -819,7 +819,6 @@ struct ImuFactorMergeTest { loop_(Vector3(0, -kAngularVelocity, 0), Vector3(kVelocity, 0, 0)) { // arbitrary noise values p_->gyroscopeCovariance = I_3x3 * 0.01; - p_->accelerometerCovariance = I_3x3 * 0.02; p_->accelerometerCovariance = I_3x3 * 0.03; } From 617014f2714d95fb692c079ba591346d8ba9aeb4 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 5 Sep 2021 11:49:15 -0400 Subject: [PATCH 208/248] wrap smart flags for various noise models --- gtsam/linear/linear.i | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/gtsam/linear/linear.i b/gtsam/linear/linear.i index 63047bf4f..f687acdec 100644 --- a/gtsam/linear/linear.i +++ b/gtsam/linear/linear.i @@ -16,9 +16,9 @@ virtual class Base { }; virtual class Gaussian : gtsam::noiseModel::Base { - static gtsam::noiseModel::Gaussian* Information(Matrix R); - static gtsam::noiseModel::Gaussian* SqrtInformation(Matrix R); - static gtsam::noiseModel::Gaussian* Covariance(Matrix R); + static gtsam::noiseModel::Gaussian* Information(Matrix R, bool smart = true); + static gtsam::noiseModel::Gaussian* SqrtInformation(Matrix R, bool smart = true); + static gtsam::noiseModel::Gaussian* Covariance(Matrix R, bool smart = true); bool equals(gtsam::noiseModel::Base& expected, double tol); @@ -37,9 +37,9 @@ virtual class Gaussian : gtsam::noiseModel::Base { }; virtual class Diagonal : gtsam::noiseModel::Gaussian { - static gtsam::noiseModel::Diagonal* Sigmas(Vector sigmas); - static gtsam::noiseModel::Diagonal* Variances(Vector variances); - static gtsam::noiseModel::Diagonal* Precisions(Vector precisions); + static gtsam::noiseModel::Diagonal* Sigmas(Vector sigmas, bool smart = true); + static gtsam::noiseModel::Diagonal* Variances(Vector variances, bool smart = true); + static gtsam::noiseModel::Diagonal* Precisions(Vector precisions, bool smart = true); Matrix R() const; // access to noise model @@ -69,9 +69,9 @@ virtual class Constrained : gtsam::noiseModel::Diagonal { }; virtual class Isotropic : gtsam::noiseModel::Diagonal { - static gtsam::noiseModel::Isotropic* Sigma(size_t dim, double sigma); - static gtsam::noiseModel::Isotropic* Variance(size_t dim, double varianace); - static gtsam::noiseModel::Isotropic* Precision(size_t dim, double precision); + static gtsam::noiseModel::Isotropic* Sigma(size_t dim, double sigma, bool smart = true); + static gtsam::noiseModel::Isotropic* Variance(size_t dim, double varianace, bool smart = true); + static gtsam::noiseModel::Isotropic* Precision(size_t dim, double precision, bool smart = true); // access to noise model double sigma() const; From 9cc594e9e55396ee20e170d967730c6d6a1bb234 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 6 Sep 2021 11:26:10 -0400 Subject: [PATCH 209/248] Added virtual constructor and re-formatted constructors --- gtsam/inference/BayesTreeCliqueBase.h | 26 +++++++++++++++++++------- 1 file changed, 19 insertions(+), 7 deletions(-) diff --git a/gtsam/inference/BayesTreeCliqueBase.h b/gtsam/inference/BayesTreeCliqueBase.h index 7b79ccf68..c9bb6db94 100644 --- a/gtsam/inference/BayesTreeCliqueBase.h +++ b/gtsam/inference/BayesTreeCliqueBase.h @@ -70,16 +70,23 @@ namespace gtsam { /// @name Standard Constructors /// @{ - /** Default constructor */ + /// Default constructor BayesTreeCliqueBase() : problemSize_(1) {} - /** Construct from a conditional, leaving parent and child pointers uninitialized */ - BayesTreeCliqueBase(const sharedConditional& conditional) : conditional_(conditional), problemSize_(1) {} + /// Construct from a conditional, leaving parent and child pointers + /// uninitialized. + BayesTreeCliqueBase(const sharedConditional& conditional) + : conditional_(conditional), problemSize_(1) {} - /** Shallow copy constructor */ - BayesTreeCliqueBase(const BayesTreeCliqueBase& c) : conditional_(c.conditional_), parent_(c.parent_), children(c.children), problemSize_(c.problemSize_), is_root(c.is_root) {} + /// Shallow copy constructor. + BayesTreeCliqueBase(const BayesTreeCliqueBase& c) + : conditional_(c.conditional_), + parent_(c.parent_), + children(c.children), + problemSize_(c.problemSize_), + is_root(c.is_root) {} - /** Shallow copy assignment constructor */ + /// Shallow copy assignment constructor BayesTreeCliqueBase& operator=(const BayesTreeCliqueBase& c) { conditional_ = c.conditional_; parent_ = c.parent_; @@ -89,6 +96,9 @@ namespace gtsam { return *this; } + // Virtual destructor. + virtual ~BayesTreeCliqueBase() {} + /// @} /// This stores the Cached separator marginal P(S) @@ -119,7 +129,9 @@ namespace gtsam { bool equals(const DERIVED& other, double tol = 1e-9) const; /** print this node */ - virtual void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + virtual void print( + const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; /// @} /// @name Standard Interface From 711968900d3061688bac2c233c952daf27fc1bc5 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 6 Sep 2021 16:14:23 -0400 Subject: [PATCH 210/248] Switched to borglab hub --- docker/ubuntu-boost-tbb/build.sh | 4 ++-- docker/ubuntu-gtsam-python-vnc/Dockerfile | 2 +- docker/ubuntu-gtsam-python-vnc/build.sh | 4 ++-- docker/ubuntu-gtsam-python-vnc/vnc.sh | 2 +- docker/ubuntu-gtsam-python/Dockerfile | 6 ++++-- docker/ubuntu-gtsam-python/build.sh | 4 ++-- docker/ubuntu-gtsam/Dockerfile | 2 +- docker/ubuntu-gtsam/build.sh | 4 ++-- 8 files changed, 15 insertions(+), 13 deletions(-) diff --git a/docker/ubuntu-boost-tbb/build.sh b/docker/ubuntu-boost-tbb/build.sh index 2dac4c3db..ff465c970 100755 --- a/docker/ubuntu-boost-tbb/build.sh +++ b/docker/ubuntu-boost-tbb/build.sh @@ -1,3 +1,3 @@ # Build command for Docker image -# TODO(dellaert): use docker compose and/or cmake -docker build --no-cache -t dellaert/ubuntu-boost-tbb:bionic . +# TODO(borglab): use docker compose and/or cmake +docker build --no-cache -t borglab/ubuntu-boost-tbb:bionic . diff --git a/docker/ubuntu-gtsam-python-vnc/Dockerfile b/docker/ubuntu-gtsam-python-vnc/Dockerfile index 61ecd9b9a..8039698c3 100644 --- a/docker/ubuntu-gtsam-python-vnc/Dockerfile +++ b/docker/ubuntu-gtsam-python-vnc/Dockerfile @@ -1,7 +1,7 @@ # This GTSAM image connects to the host X-server via VNC to provide a Graphical User Interface for interaction. # Get the base Ubuntu/GTSAM image from Docker Hub -FROM dellaert/ubuntu-gtsam-python:bionic +FROM borglab/ubuntu-gtsam-python:bionic # Things needed to get a python GUI ENV DEBIAN_FRONTEND noninteractive diff --git a/docker/ubuntu-gtsam-python-vnc/build.sh b/docker/ubuntu-gtsam-python-vnc/build.sh index 8d280252f..95d7dd575 100755 --- a/docker/ubuntu-gtsam-python-vnc/build.sh +++ b/docker/ubuntu-gtsam-python-vnc/build.sh @@ -1,4 +1,4 @@ # Build command for Docker image -# TODO(dellaert): use docker compose and/or cmake +# TODO(borglab): use docker compose and/or cmake # Needs to be run in docker/ubuntu-gtsam-python-vnc directory -docker build -t dellaert/ubuntu-gtsam-python-vnc:bionic . +docker build -t borglab/ubuntu-gtsam-python-vnc:bionic . diff --git a/docker/ubuntu-gtsam-python-vnc/vnc.sh b/docker/ubuntu-gtsam-python-vnc/vnc.sh index c0ab692c6..b749093af 100755 --- a/docker/ubuntu-gtsam-python-vnc/vnc.sh +++ b/docker/ubuntu-gtsam-python-vnc/vnc.sh @@ -2,4 +2,4 @@ docker run -it \ --workdir="/usr/src/gtsam" \ -p 5900:5900 \ - dellaert/ubuntu-gtsam-python-vnc:bionic \ No newline at end of file + borglab/ubuntu-gtsam-python-vnc:bionic \ No newline at end of file diff --git a/docker/ubuntu-gtsam-python/Dockerfile b/docker/ubuntu-gtsam-python/Dockerfile index ce5d8fdca..85eed4d4e 100644 --- a/docker/ubuntu-gtsam-python/Dockerfile +++ b/docker/ubuntu-gtsam-python/Dockerfile @@ -1,7 +1,7 @@ # GTSAM Ubuntu image with Python wrapper support. # Get the base Ubuntu/GTSAM image from Docker Hub -FROM dellaert/ubuntu-gtsam:bionic +FROM borglab/ubuntu-gtsam:bionic # Install pip RUN apt-get install -y python3-pip python3-dev @@ -22,7 +22,9 @@ RUN cmake \ .. # Build again, as ubuntu-gtsam image cleaned -RUN make -j4 install && make clean +RUN make -j4 install +RUN make python-install +RUN make clean # Needed to run python wrapper: RUN echo 'export PYTHONPATH=/usr/local/python/:$PYTHONPATH' >> /root/.bashrc diff --git a/docker/ubuntu-gtsam-python/build.sh b/docker/ubuntu-gtsam-python/build.sh index 1696f6c61..367847ba5 100755 --- a/docker/ubuntu-gtsam-python/build.sh +++ b/docker/ubuntu-gtsam-python/build.sh @@ -1,3 +1,3 @@ # Build command for Docker image -# TODO(dellaert): use docker compose and/or cmake -docker build --no-cache -t dellaert/ubuntu-gtsam-python:bionic . +# TODO(borglab): use docker compose and/or cmake +docker build --no-cache -t borglab/ubuntu-gtsam-python:bionic . diff --git a/docker/ubuntu-gtsam/Dockerfile b/docker/ubuntu-gtsam/Dockerfile index f2b476f15..ce6927fe8 100644 --- a/docker/ubuntu-gtsam/Dockerfile +++ b/docker/ubuntu-gtsam/Dockerfile @@ -1,7 +1,7 @@ # Ubuntu image with GTSAM installed. Configured with Boost and TBB support. # Get the base Ubuntu image from Docker Hub -FROM dellaert/ubuntu-boost-tbb:bionic +FROM borglab/ubuntu-boost-tbb:bionic # Install git RUN apt-get update && \ diff --git a/docker/ubuntu-gtsam/build.sh b/docker/ubuntu-gtsam/build.sh index bf545e9c2..24a7d4f7f 100755 --- a/docker/ubuntu-gtsam/build.sh +++ b/docker/ubuntu-gtsam/build.sh @@ -1,3 +1,3 @@ # Build command for Docker image -# TODO(dellaert): use docker compose and/or cmake -docker build --no-cache -t dellaert/ubuntu-gtsam:bionic . +# TODO(borglab): use docker compose and/or cmake +docker build --no-cache -t borglab/ubuntu-gtsam:bionic . From cfc06b244701500c07707db1c864c11fc62edc4b Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 6 Sep 2021 16:14:36 -0400 Subject: [PATCH 211/248] Updated README --- docker/README.md | 64 +++++++++++++++++++++++++++++++++++++++--------- 1 file changed, 53 insertions(+), 11 deletions(-) diff --git a/docker/README.md b/docker/README.md index 0c136f94c..41cae6128 100644 --- a/docker/README.md +++ b/docker/README.md @@ -1,6 +1,57 @@ # Instructions -Build all docker images, in order: +# Images on Docker Hub + +There are 4 images available on https://hub.docker.com/orgs/borglab/repositories: + +- `borglab/ubuntu-boost-tbb`: 18.06 Linux (nicknamed `bionic`) base image, with Boost and TBB installed. +- `borglab/ubuntu-gtsam`: GTSAM Release version installed in `/usr/local`. +- `borglab/ubuntu-gtsam-python`: installed GTSAM with python wrapper. +- `borglab/ubuntu-gtsam-python-vnc`: image with GTSAM+python wrapper that will run a VNC server to connect to. + +# Using the images + +## Just GTSAM + +To start the Docker image, execute +```bash +docker run -it borglab/ubuntu-gtsam:bionic +``` +after you will find yourself in a bash shell, in the directory `/usr/src/gtsam/build`. +## GTSAM with Python wrapper + +To use GTSAM via the python wrapper, similarly execute +```bash +docker run -it borglab/ubuntu-gtsam-python:bionic +``` +and then launch `python3`: +```bash +python3 +>>> import gtsam +>>> gtsam.Pose2(1,2,3) +(1, 2, 3) +``` + +## GTSAM with Python wrapper and VNC + +First, start the docker image, which will run a VNC server on port 5900: +```bash +docker run -p 5900:5900 borglab/ubuntu-gtsam-python-vnc:bionic +``` + +Then open a remote VNC X client, for example: + +### Linux +```bash +sudo apt-get install tigervnc-viewer +xtigervncviewer :5900 +``` +### Mac +In the finder, in the Go menu, select "Connect to Server...", enter `vnc://127.0.0.1`, and then press "Connect". When prompted for a password, leave blank and press "Sign In". + +# Re-building the images locally + +To build all docker images, in order: ```bash (cd ubuntu-boost-tbb && ./build.sh) @@ -9,13 +60,4 @@ Build all docker images, in order: (cd ubuntu-gtsam-python-vnc && ./build.sh) ``` -Then launch with: - - docker run -p 5900:5900 dellaert/ubuntu-gtsam-python-vnc:bionic - -Then open a remote VNC X client, for example: - - sudo apt-get install tigervnc-viewer - xtigervncviewer :5900 - - +Note: building GTSAM can take a lot of memory because of the heavy templating. It is advisable to give Docker enough resources, e.g., 8GB, to avoid OOM errors while compiling. \ No newline at end of file From f49e35ecf085a59830b55248593143271a188db4 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 6 Sep 2021 16:19:54 -0400 Subject: [PATCH 212/248] Fixed TODOs --- docker/ubuntu-boost-tbb/build.sh | 2 +- docker/ubuntu-gtsam-python-vnc/build.sh | 2 +- docker/ubuntu-gtsam-python/build.sh | 2 +- docker/ubuntu-gtsam/build.sh | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/docker/ubuntu-boost-tbb/build.sh b/docker/ubuntu-boost-tbb/build.sh index ff465c970..35b349c6a 100755 --- a/docker/ubuntu-boost-tbb/build.sh +++ b/docker/ubuntu-boost-tbb/build.sh @@ -1,3 +1,3 @@ # Build command for Docker image -# TODO(borglab): use docker compose and/or cmake +# TODO(dellaert): use docker compose and/or cmake docker build --no-cache -t borglab/ubuntu-boost-tbb:bionic . diff --git a/docker/ubuntu-gtsam-python-vnc/build.sh b/docker/ubuntu-gtsam-python-vnc/build.sh index 95d7dd575..a0bbb6a96 100755 --- a/docker/ubuntu-gtsam-python-vnc/build.sh +++ b/docker/ubuntu-gtsam-python-vnc/build.sh @@ -1,4 +1,4 @@ # Build command for Docker image -# TODO(borglab): use docker compose and/or cmake +# TODO(dellaert): use docker compose and/or cmake # Needs to be run in docker/ubuntu-gtsam-python-vnc directory docker build -t borglab/ubuntu-gtsam-python-vnc:bionic . diff --git a/docker/ubuntu-gtsam-python/build.sh b/docker/ubuntu-gtsam-python/build.sh index 367847ba5..68827074d 100755 --- a/docker/ubuntu-gtsam-python/build.sh +++ b/docker/ubuntu-gtsam-python/build.sh @@ -1,3 +1,3 @@ # Build command for Docker image -# TODO(borglab): use docker compose and/or cmake +# TODO(dellaert): use docker compose and/or cmake docker build --no-cache -t borglab/ubuntu-gtsam-python:bionic . diff --git a/docker/ubuntu-gtsam/build.sh b/docker/ubuntu-gtsam/build.sh index 24a7d4f7f..790ee1575 100755 --- a/docker/ubuntu-gtsam/build.sh +++ b/docker/ubuntu-gtsam/build.sh @@ -1,3 +1,3 @@ # Build command for Docker image -# TODO(borglab): use docker compose and/or cmake +# TODO(dellaert): use docker compose and/or cmake docker build --no-cache -t borglab/ubuntu-gtsam:bionic . From 63f651b1d4e2aa1668ac4dd4662a5c556841ba3d Mon Sep 17 00:00:00 2001 From: Frank dellaert Date: Mon, 6 Sep 2021 22:35:58 -0400 Subject: [PATCH 213/248] Fixed VNC docs for Mac --- docker/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docker/README.md b/docker/README.md index 41cae6128..37c47a27f 100644 --- a/docker/README.md +++ b/docker/README.md @@ -47,7 +47,7 @@ sudo apt-get install tigervnc-viewer xtigervncviewer :5900 ``` ### Mac -In the finder, in the Go menu, select "Connect to Server...", enter `vnc://127.0.0.1`, and then press "Connect". When prompted for a password, leave blank and press "Sign In". +The Finder's "Connect to Server..." with `vnc://127.0.0.1` does not work, for some reason. Using the free [VNC Viewer](https://www.realvnc.com/en/connect/download/viewer/), enter `0.0.0.0:5900` as the server. # Re-building the images locally From b3da7e0697f9c3b539a4c5dd0b02e90b1e3a1055 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 10 Sep 2021 11:43:51 -0400 Subject: [PATCH 214/248] formatting --- gtsam/inference/Key.h | 30 +++++++++++++++++------------- 1 file changed, 17 insertions(+), 13 deletions(-) diff --git a/gtsam/inference/Key.h b/gtsam/inference/Key.h index 2cc19d07a..31428a50e 100644 --- a/gtsam/inference/Key.h +++ b/gtsam/inference/Key.h @@ -32,7 +32,7 @@ namespace gtsam { /// Typedef for a function to format a key, i.e. to convert it to a string -typedef std::function KeyFormatter; +using KeyFormatter = std::function; // Helper function for DefaultKeyFormatter GTSAM_EXPORT std::string _defaultKeyFormatter(Key key); @@ -83,28 +83,32 @@ class key_formatter { }; /// Define collection type once and for all - also used in wrappers -typedef FastVector KeyVector; +using KeyVector = FastVector; // TODO(frank): Nothing fast about these :-( -typedef FastList KeyList; -typedef FastSet KeySet; -typedef FastMap KeyGroupMap; +using KeyList = FastList; +using KeySet = FastSet; +using KeyGroupMap = FastMap; /// Utility function to print one key with optional prefix -GTSAM_EXPORT void PrintKey(Key key, const std::string& s = "", - const KeyFormatter& keyFormatter = DefaultKeyFormatter); +GTSAM_EXPORT void PrintKey( + Key key, const std::string &s = "", + const KeyFormatter &keyFormatter = DefaultKeyFormatter); /// Utility function to print sets of keys with optional prefix -GTSAM_EXPORT void PrintKeyList(const KeyList& keys, const std::string& s = "", - const KeyFormatter& keyFormatter = DefaultKeyFormatter); +GTSAM_EXPORT void PrintKeyList( + const KeyList &keys, const std::string &s = "", + const KeyFormatter &keyFormatter = DefaultKeyFormatter); /// Utility function to print sets of keys with optional prefix -GTSAM_EXPORT void PrintKeyVector(const KeyVector& keys, const std::string& s = - "", const KeyFormatter& keyFormatter = DefaultKeyFormatter); +GTSAM_EXPORT void PrintKeyVector( + const KeyVector &keys, const std::string &s = "", + const KeyFormatter &keyFormatter = DefaultKeyFormatter); /// Utility function to print sets of keys with optional prefix -GTSAM_EXPORT void PrintKeySet(const KeySet& keys, const std::string& s = "", - const KeyFormatter& keyFormatter = DefaultKeyFormatter); +GTSAM_EXPORT void PrintKeySet( + const KeySet &keys, const std::string &s = "", + const KeyFormatter &keyFormatter = DefaultKeyFormatter); // Define Key to be Testable by specializing gtsam::traits template struct traits; From 0a8080f2fcf3fc1127073042df1272f96a071b0f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 10 Sep 2021 11:53:06 -0400 Subject: [PATCH 215/248] wrap key printing funcs and remove redundancy --- gtsam/nonlinear/nonlinear.i | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index 61f164b43..fdd795c93 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -75,12 +75,15 @@ size_t Z(size_t j); } // namespace symbol_shorthand // Default keyformatter -void PrintKeyList(const gtsam::KeyList& keys); -void PrintKeyList(const gtsam::KeyList& keys, string s); -void PrintKeyVector(const gtsam::KeyVector& keys); -void PrintKeyVector(const gtsam::KeyVector& keys, string s); -void PrintKeySet(const gtsam::KeySet& keys); -void PrintKeySet(const gtsam::KeySet& keys, string s); +void PrintKeyList( + const gtsam::KeyList& keys, const string& s = "", + const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter); +void PrintKeyVector( + const gtsam::KeyVector& keys, const string& s = "", + const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter); +void PrintKeySet( + const gtsam::KeySet& keys, const string& s = "", + const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter); #include class LabeledSymbol { From 0257f2118d9a36691503e618069d1a7b8333ae62 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 10 Sep 2021 11:53:21 -0400 Subject: [PATCH 216/248] wrap more JacobianFactor constructors --- gtsam/linear/linear.i | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/gtsam/linear/linear.i b/gtsam/linear/linear.i index f687acdec..8635c55f8 100644 --- a/gtsam/linear/linear.i +++ b/gtsam/linear/linear.i @@ -289,6 +289,13 @@ virtual class JacobianFactor : gtsam::GaussianFactor { JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, size_t i3, Matrix A3, Vector b, const gtsam::noiseModel::Diagonal* model); JacobianFactor(const gtsam::GaussianFactorGraph& graph); + JacobianFactor(const gtsam::GaussianFactorGraph& graph, + const gtsam::VariableSlots& p_variableSlots); + JacobianFactor(const gtsam::GaussianFactorGraph& graph, + const gtsam::Ordering& ordering); + JacobianFactor(const gtsam::GaussianFactorGraph& graph, + const gtsam::Ordering& ordering, + const gtsam::VariableSlots& p_variableSlots); //Testable void print(string s = "", const gtsam::KeyFormatter& keyFormatter = From 7076244b60c89119c6b4163d3f0faf32c9104b6b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 10 Sep 2021 11:53:41 -0400 Subject: [PATCH 217/248] update template to record correct name of cpp file --- python/gtsam/gtsam.tpl | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/python/gtsam/gtsam.tpl b/python/gtsam/gtsam.tpl index 93e7ffbaf..b760e4eb5 100644 --- a/python/gtsam/gtsam.tpl +++ b/python/gtsam/gtsam.tpl @@ -1,8 +1,8 @@ /** - * @file gtsam.cpp + * @file {module_name}.cpp * @brief The auto-generated wrapper C++ source code. - * @author Duy-Nguyen Ta, Fan Jiang, Matthew Sklar - * @date Aug. 18, 2020 + * @author Duy-Nguyen Ta, Fan Jiang, Matthew Sklar, Varun Agrawal + * @date Aug. 18, 2020 * * ** THIS FILE IS AUTO-GENERATED, DO NOT MODIFY! ** */ From d99a9432bcd68eb0c872648965344922eb586afb Mon Sep 17 00:00:00 2001 From: Akash Sharma Date: Fri, 17 Sep 2021 17:59:46 -0400 Subject: [PATCH 218/248] Update Find TBB to handle TBB installed with homebrew on OS X Better Error message for TBB handling with versions greater than 2021.1 on OS X --- cmake/FindTBB.cmake | 16 ++++++++++++++-- cmake/HandleTBB.cmake | 4 ++++ 2 files changed, 18 insertions(+), 2 deletions(-) diff --git a/cmake/FindTBB.cmake b/cmake/FindTBB.cmake index e2b1df6e3..0ecd4ca0e 100644 --- a/cmake/FindTBB.cmake +++ b/cmake/FindTBB.cmake @@ -144,7 +144,8 @@ if(NOT TBB_FOUND) elseif(CMAKE_SYSTEM_NAME STREQUAL "Darwin") # OS X - set(TBB_DEFAULT_SEARCH_DIR "/opt/intel/tbb") + set(TBB_DEFAULT_SEARCH_DIR "/opt/intel/tbb" + "/usr/local/opt/tbb") # TODO: Check to see which C++ library is being used by the compiler. if(NOT ${CMAKE_SYSTEM_VERSION} VERSION_LESS 13.0) @@ -181,7 +182,18 @@ if(NOT TBB_FOUND) ################################## if(TBB_INCLUDE_DIRS) - file(READ "${TBB_INCLUDE_DIRS}/tbb/tbb_stddef.h" _tbb_version_file) + set(_tbb_version_file_prior_to_tbb_2021_1 "${TBB_INCLUDE_DIRS}/tbb/tbb_stddef.h") + set(_tbb_version_file_after_tbb_2021_1 "${TBB_INCLUDE_DIRS}/oneapi/tbb/version.h") + + if (EXISTS "${_tbb_version_file_prior_to_tbb_2021_1}") + file(READ "${_tbb_version_file_prior_to_tbb_2021_1}" _tbb_version_file ) + elseif (EXISTS "${_tbb_version_file_after_tbb_2021_1}") + file(READ "${_tbb_version_file_after_tbb_2021_1}" _tbb_version_file ) + else() + message(FATAL_ERROR "Found TBB installation: ${TBB_INCLUDE_DIRS} " + "missing version header.") + endif() + string(REGEX REPLACE ".*#define TBB_VERSION_MAJOR ([0-9]+).*" "\\1" TBB_VERSION_MAJOR "${_tbb_version_file}") string(REGEX REPLACE ".*#define TBB_VERSION_MINOR ([0-9]+).*" "\\1" diff --git a/cmake/HandleTBB.cmake b/cmake/HandleTBB.cmake index cedee55ea..e2ba02d94 100644 --- a/cmake/HandleTBB.cmake +++ b/cmake/HandleTBB.cmake @@ -5,6 +5,10 @@ find_package(TBB 4.4 COMPONENTS tbb tbbmalloc) # Set up variables if we're using TBB if(TBB_FOUND AND GTSAM_WITH_TBB) set(GTSAM_USE_TBB 1) # This will go into config.h + if ((${TBB_VERSION} VERSION_GREATER "2021.1") OR (${TBB_VERSION} VERSION_EQUAL "2021.1")) + message(FATAL_ERROR "TBB version greater than 2021.1 (oneTBB API) is not yet supported. Use an older version instead.") + endif() + if ((${TBB_VERSION_MAJOR} GREATER 2020) OR (${TBB_VERSION_MAJOR} EQUAL 2020)) set(TBB_GREATER_EQUAL_2020 1) else() From 536a1035d60f0d243d106c036f59d330caeca126 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 18 Sep 2021 02:17:47 -0400 Subject: [PATCH 219/248] Squashed 'wrap/' changes from 571c23952..add6075e8 add6075e8 Merge pull request #121 from borglab/feature/constructor-templates 42d4145bb update instantiate_ctors to handle constructor level templates 455ce6169 update test fixtures 3c37fc2a0 update wrapper test fixtures ffdad925d update interface_parser to pass the tests bf7416213 add interface_parser test for templated constructor 9fe05d0c9 Merge pull request #120 from borglab/feature/templated-namespace 7622e6432 typo fix 88779c5e6 update instantiator to handle templates in the namespace 0ee86f9a3 add test for templated type in namespace git-subtree-dir: wrap git-subtree-split: add6075e8ec0e28d5f47d0a2ecab00deaa9a3da7 --- gtwrap/interface_parser/classes.py | 23 ++--- gtwrap/interface_parser/function.py | 2 +- gtwrap/template_instantiator.py | 47 ++++++++- tests/expected/matlab/MyFactorPosePoint2.m | 8 +- tests/expected/matlab/class_wrapper.cpp | 111 +++++++++++++++++++-- tests/expected/python/class_pybind.cpp | 6 ++ tests/expected/python/enum_pybind.cpp | 10 ++ tests/fixtures/class.i | 10 ++ tests/fixtures/enum.i | 13 +++ tests/test_interface_parser.py | 19 ++++ 10 files changed, 217 insertions(+), 32 deletions(-) diff --git a/gtwrap/interface_parser/classes.py b/gtwrap/interface_parser/classes.py index 11317962d..841c963c2 100644 --- a/gtwrap/interface_parser/classes.py +++ b/gtwrap/interface_parser/classes.py @@ -119,24 +119,27 @@ class Constructor: Can have 0 or more arguments. """ rule = ( - IDENT("name") # + Optional(Template.rule("template")) # + + IDENT("name") # + LPAREN # + ArgumentList.rule("args_list") # + RPAREN # + SEMI_COLON # BR - ).setParseAction(lambda t: Constructor(t.name, t.args_list)) + ).setParseAction(lambda t: Constructor(t.name, t.args_list, t.template)) def __init__(self, name: str, args: ArgumentList, + template: Union[Template, Any], parent: Union["Class", Any] = ''): self.name = name self.args = args + self.template = template self.parent = parent def __repr__(self) -> str: - return "Constructor: {}".format(self.name) + return "Constructor: {}{}".format(self.name, self.args) class Operator: @@ -260,17 +263,9 @@ class Class: + RBRACE # + SEMI_COLON # BR ).setParseAction(lambda t: Class( - t.template, - t.is_virtual, - t.name, - t.parent_class, - t.members.ctors, - t.members.methods, - t.members.static_methods, - t.members.properties, - t.members.operators, - t.members.enums - )) + t.template, t.is_virtual, t.name, t.parent_class, t.members.ctors, t. + members.methods, t.members.static_methods, t.members.properties, t. + members.operators, t.members.enums)) def __init__( self, diff --git a/gtwrap/interface_parser/function.py b/gtwrap/interface_parser/function.py index 9fe1f56f0..9e68c6ece 100644 --- a/gtwrap/interface_parser/function.py +++ b/gtwrap/interface_parser/function.py @@ -81,7 +81,7 @@ class ArgumentList: return ArgumentList([]) def __repr__(self) -> str: - return self.args_list.__repr__() + return repr(tuple(self.args_list)) def __len__(self) -> int: return len(self.args_list) diff --git a/gtwrap/template_instantiator.py b/gtwrap/template_instantiator.py index 0cda93d5d..f5beb0c69 100644 --- a/gtwrap/template_instantiator.py +++ b/gtwrap/template_instantiator.py @@ -41,6 +41,8 @@ def instantiate_type(ctype: parser.Type, str_arg_typename = str(ctype.typename) + # Instantiate templates which have enumerated instantiations in the template. + # E.g. `template`. if str_arg_typename in template_typenames: idx = template_typenames.index(str_arg_typename) return parser.Type( @@ -51,14 +53,15 @@ def instantiate_type(ctype: parser.Type, is_ref=ctype.is_ref, is_basic=ctype.is_basic, ) + + # If a method has the keyword `This`, we replace it with the (instantiated) class. elif str_arg_typename == 'This': + # Check if the class is template instantiated + # so we can replace it with the instantiated version. if instantiated_class: name = instantiated_class.original.name namespaces_name = instantiated_class.namespaces() namespaces_name.append(name) - # print("INST: {}, {}, CPP: {}, CLS: {}".format( - # ctype, instantiations, cpp_typename, instantiated_class.instantiations - # ), file=sys.stderr) cpp_typename = parser.Typename( namespaces_name, instantiations=instantiated_class.instantiations) @@ -71,6 +74,14 @@ def instantiate_type(ctype: parser.Type, is_ref=ctype.is_ref, is_basic=ctype.is_basic, ) + + # Case when 'This' is present in the type namespace, e.g `This::Subclass`. + elif 'This' in str_arg_typename: + # Simply get the index of `This` in the namespace and replace it with the instantiated name. + namespace_idx = ctype.typename.namespaces.index('This') + ctype.typename.namespaces[namespace_idx] = cpp_typename.name + return ctype + else: return ctype @@ -368,19 +379,45 @@ class InstantiatedClass(parser.Class): """ instantiated_ctors = [] - for ctor in self.original.ctors: + def instantiate(instantiated_ctors, ctor, typenames, instantiations): instantiated_args = instantiate_args_list( ctor.args.list(), typenames, - self.instantiations, + instantiations, self.cpp_typename(), ) instantiated_ctors.append( parser.Constructor( name=self.name, args=parser.ArgumentList(instantiated_args), + template=self.original.template, parent=self, )) + return instantiated_ctors + + for ctor in self.original.ctors: + # Add constructor templates to the typenames and instantiations + if isinstance(ctor.template, parser.template.Template): + typenames.extend(ctor.template.typenames) + + # Get all combinations of template args + for instantiations in itertools.product( + *ctor.template.instantiations): + instantiations = self.instantiations + list(instantiations) + + instantiated_ctors = instantiate( + instantiated_ctors, + ctor, + typenames=typenames, + instantiations=instantiations) + + else: + # If no constructor level templates, just use the class templates + instantiated_ctors = instantiate( + instantiated_ctors, + ctor, + typenames=typenames, + instantiations=self.instantiations) return instantiated_ctors def instantiate_static_methods(self, typenames): diff --git a/tests/expected/matlab/MyFactorPosePoint2.m b/tests/expected/matlab/MyFactorPosePoint2.m index 2dd4b5428..e4cd92196 100644 --- a/tests/expected/matlab/MyFactorPosePoint2.m +++ b/tests/expected/matlab/MyFactorPosePoint2.m @@ -15,9 +15,9 @@ classdef MyFactorPosePoint2 < handle function obj = MyFactorPosePoint2(varargin) if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682) my_ptr = varargin{2}; - class_wrapper(56, my_ptr); + class_wrapper(62, 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 = class_wrapper(57, varargin{1}, varargin{2}, varargin{3}, varargin{4}); + my_ptr = class_wrapper(63, varargin{1}, varargin{2}, varargin{3}, varargin{4}); else error('Arguments do not match any overload of MyFactorPosePoint2 constructor'); end @@ -25,7 +25,7 @@ classdef MyFactorPosePoint2 < handle end function delete(obj) - class_wrapper(58, obj.ptr_MyFactorPosePoint2); + class_wrapper(64, obj.ptr_MyFactorPosePoint2); end function display(obj), obj.print(''); end @@ -36,7 +36,7 @@ classdef MyFactorPosePoint2 < handle % PRINT usage: print(string s, KeyFormatter keyFormatter) : returns void % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 2 && isa(varargin{1},'char') && isa(varargin{2},'gtsam.KeyFormatter') - class_wrapper(59, this, varargin{:}); + class_wrapper(65, this, varargin{:}); return end error('Arguments do not match any overload of function MyFactorPosePoint2.print'); diff --git a/tests/expected/matlab/class_wrapper.cpp b/tests/expected/matlab/class_wrapper.cpp index fab9c1450..48d6b2dce 100644 --- a/tests/expected/matlab/class_wrapper.cpp +++ b/tests/expected/matlab/class_wrapper.cpp @@ -33,6 +33,8 @@ typedef std::set*> Collector_Multip static Collector_MultipleTemplatesIntFloat collector_MultipleTemplatesIntFloat; typedef std::set*> Collector_ForwardKinematics; static Collector_ForwardKinematics collector_ForwardKinematics; +typedef std::set*> Collector_TemplatedConstructor; +static Collector_TemplatedConstructor collector_TemplatedConstructor; typedef std::set*> Collector_MyFactorPosePoint2; static Collector_MyFactorPosePoint2 collector_MyFactorPosePoint2; @@ -97,6 +99,12 @@ void _deleteAllObjects() collector_ForwardKinematics.erase(iter++); anyDeleted = true; } } + { for(Collector_TemplatedConstructor::iterator iter = collector_TemplatedConstructor.begin(); + iter != collector_TemplatedConstructor.end(); ) { + delete *iter; + collector_TemplatedConstructor.erase(iter++); + anyDeleted = true; + } } { for(Collector_MyFactorPosePoint2::iterator iter = collector_MyFactorPosePoint2.begin(); iter != collector_MyFactorPosePoint2.end(); ) { delete *iter; @@ -682,7 +690,76 @@ void ForwardKinematics_deconstructor_55(int nargout, mxArray *out[], int nargin, } } -void MyFactorPosePoint2_collectorInsertAndMakeBase_56(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void TemplatedConstructor_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_TemplatedConstructor.insert(self); +} + +void TemplatedConstructor_constructor_57(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + Shared *self = new Shared(new TemplatedConstructor()); + collector_TemplatedConstructor.insert(self); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast (mxGetData(out[0])) = self; +} + +void TemplatedConstructor_constructor_58(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + string& arg = *unwrap_shared_ptr< string >(in[0], "ptr_string"); + Shared *self = new Shared(new TemplatedConstructor(arg)); + collector_TemplatedConstructor.insert(self); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast (mxGetData(out[0])) = self; +} + +void TemplatedConstructor_constructor_59(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + int arg = unwrap< int >(in[0]); + Shared *self = new Shared(new TemplatedConstructor(arg)); + collector_TemplatedConstructor.insert(self); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast (mxGetData(out[0])) = self; +} + +void TemplatedConstructor_constructor_60(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + double arg = unwrap< double >(in[0]); + Shared *self = new Shared(new TemplatedConstructor(arg)); + collector_TemplatedConstructor.insert(self); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast (mxGetData(out[0])) = self; +} + +void TemplatedConstructor_deconstructor_61(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("delete_TemplatedConstructor",nargout,nargin,1); + Shared *self = *reinterpret_cast(mxGetData(in[0])); + Collector_TemplatedConstructor::iterator item; + item = collector_TemplatedConstructor.find(self); + if(item != collector_TemplatedConstructor.end()) { + delete self; + collector_TemplatedConstructor.erase(item); + } +} + +void MyFactorPosePoint2_collectorInsertAndMakeBase_62(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr> Shared; @@ -691,7 +768,7 @@ void MyFactorPosePoint2_collectorInsertAndMakeBase_56(int nargout, mxArray *out[ collector_MyFactorPosePoint2.insert(self); } -void MyFactorPosePoint2_constructor_57(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MyFactorPosePoint2_constructor_63(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr> Shared; @@ -706,7 +783,7 @@ void MyFactorPosePoint2_constructor_57(int nargout, mxArray *out[], int nargin, *reinterpret_cast (mxGetData(out[0])) = self; } -void MyFactorPosePoint2_deconstructor_58(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MyFactorPosePoint2_deconstructor_64(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { typedef boost::shared_ptr> Shared; checkArguments("delete_MyFactorPosePoint2",nargout,nargin,1); @@ -719,7 +796,7 @@ void MyFactorPosePoint2_deconstructor_58(int nargout, mxArray *out[], int nargin } } -void MyFactorPosePoint2_print_59(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MyFactorPosePoint2_print_65(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("print",nargout,nargin-1,2); auto obj = unwrap_shared_ptr>(in[0], "ptr_MyFactorPosePoint2"); @@ -909,16 +986,34 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) ForwardKinematics_deconstructor_55(nargout, out, nargin-1, in+1); break; case 56: - MyFactorPosePoint2_collectorInsertAndMakeBase_56(nargout, out, nargin-1, in+1); + TemplatedConstructor_collectorInsertAndMakeBase_56(nargout, out, nargin-1, in+1); break; case 57: - MyFactorPosePoint2_constructor_57(nargout, out, nargin-1, in+1); + TemplatedConstructor_constructor_57(nargout, out, nargin-1, in+1); break; case 58: - MyFactorPosePoint2_deconstructor_58(nargout, out, nargin-1, in+1); + TemplatedConstructor_constructor_58(nargout, out, nargin-1, in+1); break; case 59: - MyFactorPosePoint2_print_59(nargout, out, nargin-1, in+1); + TemplatedConstructor_constructor_59(nargout, out, nargin-1, in+1); + break; + case 60: + TemplatedConstructor_constructor_60(nargout, out, nargin-1, in+1); + break; + case 61: + TemplatedConstructor_deconstructor_61(nargout, out, nargin-1, in+1); + break; + case 62: + MyFactorPosePoint2_collectorInsertAndMakeBase_62(nargout, out, nargin-1, in+1); + break; + case 63: + MyFactorPosePoint2_constructor_63(nargout, out, nargin-1, in+1); + break; + case 64: + MyFactorPosePoint2_deconstructor_64(nargout, out, nargin-1, in+1); + break; + case 65: + MyFactorPosePoint2_print_65(nargout, out, nargin-1, in+1); break; } } catch(const std::exception& e) { diff --git a/tests/expected/python/class_pybind.cpp b/tests/expected/python/class_pybind.cpp index 4c2371a42..f36c0a84c 100644 --- a/tests/expected/python/class_pybind.cpp +++ b/tests/expected/python/class_pybind.cpp @@ -86,6 +86,12 @@ PYBIND11_MODULE(class_py, m_) { py::class_>(m_, "ForwardKinematics") .def(py::init(), py::arg("robot"), py::arg("start_link_name"), py::arg("end_link_name"), py::arg("joint_angles"), py::arg("l2Tp") = gtsam::Pose3()); + py::class_>(m_, "TemplatedConstructor") + .def(py::init<>()) + .def(py::init(), py::arg("arg")) + .def(py::init(), py::arg("arg")) + .def(py::init(), py::arg("arg")); + py::class_, std::shared_ptr>>(m_, "MyFactorPosePoint2") .def(py::init>(), py::arg("key1"), py::arg("key2"), py::arg("measured"), py::arg("noiseModel")) .def("print",[](MyFactor* self, const string& s, const gtsam::KeyFormatter& keyFormatter){ py::scoped_ostream_redirect output; self->print(s, keyFormatter);}, py::arg("s") = "factor: ", py::arg("keyFormatter") = gtsam::DefaultKeyFormatter) diff --git a/tests/expected/python/enum_pybind.cpp b/tests/expected/python/enum_pybind.cpp index ffc68ece0..73b74bd86 100644 --- a/tests/expected/python/enum_pybind.cpp +++ b/tests/expected/python/enum_pybind.cpp @@ -69,6 +69,16 @@ PYBIND11_MODULE(enum_py, m_) { .value("Groot", gtsam::MCU::GotG::Groot); + py::class_, std::shared_ptr>> optimizergaussnewtonparams(m_gtsam, "OptimizerGaussNewtonParams"); + optimizergaussnewtonparams + .def("setVerbosity",[](gtsam::Optimizer* self, const Optimizer::Verbosity value){ self->setVerbosity(value);}, py::arg("value")); + + py::enum_::Verbosity>(optimizergaussnewtonparams, "Verbosity", py::arithmetic()) + .value("SILENT", gtsam::Optimizer::Verbosity::SILENT) + .value("SUMMARY", gtsam::Optimizer::Verbosity::SUMMARY) + .value("VERBOSE", gtsam::Optimizer::Verbosity::VERBOSE); + + #include "python/specializations.h" diff --git a/tests/fixtures/class.i b/tests/fixtures/class.i index 9e30b17b5..9923ffce7 100644 --- a/tests/fixtures/class.i +++ b/tests/fixtures/class.i @@ -7,6 +7,7 @@ class FunRange { template class Fun { + static This staticMethodWithThis(); template @@ -118,5 +119,14 @@ class ForwardKinematics { const gtsam::Pose3& l2Tp = gtsam::Pose3()); }; +// Test for templated constructor +class TemplatedConstructor { + TemplatedConstructor(); + + template + TemplatedConstructor(const T& arg); +}; + + class SuperCoolFactor; typedef SuperCoolFactor SuperCoolFactorPose3; diff --git a/tests/fixtures/enum.i b/tests/fixtures/enum.i index 9386a33df..71918c25a 100644 --- a/tests/fixtures/enum.i +++ b/tests/fixtures/enum.i @@ -42,4 +42,17 @@ class MCU { }; +template +class Optimizer { + enum Verbosity { + SILENT, + SUMMARY, + VERBOSE + }; + + void setVerbosity(const This::Verbosity value); +}; + +typedef gtsam::Optimizer OptimizerGaussNewtonParams; + } // namespace gtsam diff --git a/tests/test_interface_parser.py b/tests/test_interface_parser.py index 95987f42f..25e4178a6 100644 --- a/tests/test_interface_parser.py +++ b/tests/test_interface_parser.py @@ -314,6 +314,25 @@ class TestInterfaceParser(unittest.TestCase): self.assertEqual(5, len(ret.args)) self.assertEqual("gtsam::Pose3()", ret.args.list()[4].default) + def test_constructor_templated(self): + """Test for templated class constructor.""" + f = """ + template + Class(); + """ + ret = Constructor.rule.parseString(f)[0] + self.assertEqual("Class", ret.name) + self.assertEqual(0, len(ret.args)) + + f = """ + template + Class(const T& name); + """ + ret = Constructor.rule.parseString(f)[0] + self.assertEqual("Class", ret.name) + self.assertEqual(1, len(ret.args)) + self.assertEqual("const T & name", ret.args.args_list[0].to_cpp()) + def test_operator_overload(self): """Test for operator overloading.""" # Unary operator From d8b6f1524719ba7538fd9d5887194af4d81941f9 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 18 Sep 2021 02:48:31 -0400 Subject: [PATCH 220/248] make TBB finding depend on the GTSAM_WITH_TBB flag --- cmake/HandleTBB.cmake | 41 ++++++++++++++++++++++------------------- 1 file changed, 22 insertions(+), 19 deletions(-) diff --git a/cmake/HandleTBB.cmake b/cmake/HandleTBB.cmake index cedee55ea..56c443932 100644 --- a/cmake/HandleTBB.cmake +++ b/cmake/HandleTBB.cmake @@ -1,24 +1,27 @@ ############################################################################### -# Find TBB -find_package(TBB 4.4 COMPONENTS tbb tbbmalloc) +if (GTSAM_WITH_TBB) + # Find TBB + find_package(TBB 4.4 COMPONENTS tbb tbbmalloc) -# Set up variables if we're using TBB -if(TBB_FOUND AND GTSAM_WITH_TBB) - set(GTSAM_USE_TBB 1) # This will go into config.h - if ((${TBB_VERSION_MAJOR} GREATER 2020) OR (${TBB_VERSION_MAJOR} EQUAL 2020)) - set(TBB_GREATER_EQUAL_2020 1) + # Set up variables if we're using TBB + if(TBB_FOUND) + set(GTSAM_USE_TBB 1) # This will go into config.h + if ((${TBB_VERSION_MAJOR} GREATER 2020) OR (${TBB_VERSION_MAJOR} EQUAL 2020)) + set(TBB_GREATER_EQUAL_2020 1) + else() + set(TBB_GREATER_EQUAL_2020 0) + endif() + # all definitions and link requisites will go via imported targets: + # tbb & tbbmalloc + list(APPEND GTSAM_ADDITIONAL_LIBRARIES tbb tbbmalloc) else() - set(TBB_GREATER_EQUAL_2020 0) + set(GTSAM_USE_TBB 0) # This will go into config.h endif() - # all definitions and link requisites will go via imported targets: - # tbb & tbbmalloc - list(APPEND GTSAM_ADDITIONAL_LIBRARIES tbb tbbmalloc) -else() - set(GTSAM_USE_TBB 0) # This will go into config.h -endif() -############################################################################### -# Prohibit Timing build mode in combination with TBB -if(GTSAM_USE_TBB AND (CMAKE_BUILD_TYPE STREQUAL "Timing")) - message(FATAL_ERROR "Timing build mode cannot be used together with TBB. Use a sampling profiler such as Instruments or Intel VTune Amplifier instead.") -endif() + ############################################################################### + # Prohibit Timing build mode in combination with TBB + if(GTSAM_USE_TBB AND (CMAKE_BUILD_TYPE STREQUAL "Timing")) + message(FATAL_ERROR "Timing build mode cannot be used together with TBB. Use a sampling profiler such as Instruments or Intel VTune Amplifier instead.") + endif() + +endif() \ No newline at end of file From 4307b842c18a28fddd09eed95716cc4965e23b0d Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 28 Sep 2021 10:09:10 -0400 Subject: [PATCH 221/248] Squashed 'wrap/' changes from add6075e8..d6350c214 d6350c214 Merge pull request #123 from borglab/feature/constructor-arg-templates c667aa0ed add matlab test b9513b0db update docs 2b44678c9 instantiate scoped templates f99a75110 add tests for templates git-subtree-dir: wrap git-subtree-split: d6350c214bb4e012f1a0003b1ffc052bb7f35f33 --- DOCS.md | 3 +- gtwrap/template_instantiator.py | 35 +++- tests/expected/matlab/template_wrapper.cpp | 225 +++++++++++++++++++++ tests/expected/python/templates_pybind.cpp | 38 ++++ tests/fixtures/templates.i | 15 ++ tests/test_matlab_wrapper.py | 17 ++ tests/test_pybind_wrapper.py | 8 + 7 files changed, 339 insertions(+), 2 deletions(-) create mode 100644 tests/expected/matlab/template_wrapper.cpp create mode 100644 tests/expected/python/templates_pybind.cpp create mode 100644 tests/fixtures/templates.i diff --git a/DOCS.md b/DOCS.md index c8285baef..f08f741ff 100644 --- a/DOCS.md +++ b/DOCS.md @@ -133,9 +133,10 @@ The python wrapper supports keyword arguments for functions/methods. Hence, the template class Class2 { ... }; typedef Class2 MyInstantiatedClass; ``` - - Templates can also be defined for methods, properties and static methods. + - Templates can also be defined for constructors, methods, properties and static methods. - In the class definition, appearances of the template argument(s) will be replaced with their instantiated types, e.g. `void setValue(const T& value);`. + - Values scoped within templates are supported. E.g. one can use the form `T::Value` where T is a template, as an argument to a method. - To refer to the instantiation of the template class itself, use `This`, i.e. `static This Create();`. - To create new instantiations in other modules, you must copy-and-paste the whole class definition into the new module, but use only your new instantiation types. diff --git a/gtwrap/template_instantiator.py b/gtwrap/template_instantiator.py index f5beb0c69..b4d79655d 100644 --- a/gtwrap/template_instantiator.py +++ b/gtwrap/template_instantiator.py @@ -9,6 +9,17 @@ from typing import Any, Iterable, List, Sequence import gtwrap.interface_parser as parser +def is_scoped_template(template_typenames, str_arg_typename): + """ + Check if the template given by `str_arg_typename` is a scoped template, + and if so, return what template and index matches the scoped template correctly. + """ + for idx, template in enumerate(template_typenames): + if template in str_arg_typename.split("::"): + return template, idx + return False, -1 + + def instantiate_type(ctype: parser.Type, template_typenames: List[str], instantiations: List[parser.Typename], @@ -41,9 +52,30 @@ def instantiate_type(ctype: parser.Type, str_arg_typename = str(ctype.typename) + # Check if template is a scoped template e.g. T::Value where T is the template + scoped_template, scoped_idx = is_scoped_template(template_typenames, + str_arg_typename) + # Instantiate templates which have enumerated instantiations in the template. # E.g. `template`. - if str_arg_typename in template_typenames: + + # Instantiate scoped templates, e.g. T::Value. + if scoped_template: + # Create a copy of the instantiation so we can modify it. + instantiation = deepcopy(instantiations[scoped_idx]) + # Replace the part of the template with the instantiation + instantiation.name = str_arg_typename.replace(scoped_template, + instantiation.name) + return parser.Type( + typename=instantiation, + is_const=ctype.is_const, + is_shared_ptr=ctype.is_shared_ptr, + is_ptr=ctype.is_ptr, + is_ref=ctype.is_ref, + is_basic=ctype.is_basic, + ) + # Check for exact template match. + elif str_arg_typename in template_typenames: idx = template_typenames.index(str_arg_typename) return parser.Type( typename=instantiations[idx], @@ -418,6 +450,7 @@ class InstantiatedClass(parser.Class): ctor, typenames=typenames, instantiations=self.instantiations) + return instantiated_ctors def instantiate_static_methods(self, typenames): diff --git a/tests/expected/matlab/template_wrapper.cpp b/tests/expected/matlab/template_wrapper.cpp new file mode 100644 index 000000000..532bdd57a --- /dev/null +++ b/tests/expected/matlab/template_wrapper.cpp @@ -0,0 +1,225 @@ +#include +#include + +#include +#include +#include + + + +typedef ScopedTemplate ScopedTemplateResult; + +typedef std::set*> Collector_TemplatedConstructor; +static Collector_TemplatedConstructor collector_TemplatedConstructor; +typedef std::set*> Collector_ScopedTemplateResult; +static Collector_ScopedTemplateResult collector_ScopedTemplateResult; + + +void _deleteAllObjects() +{ + mstream mout; + std::streambuf *outbuf = std::cout.rdbuf(&mout); + + bool anyDeleted = false; + { for(Collector_TemplatedConstructor::iterator iter = collector_TemplatedConstructor.begin(); + iter != collector_TemplatedConstructor.end(); ) { + delete *iter; + collector_TemplatedConstructor.erase(iter++); + anyDeleted = true; + } } + { for(Collector_ScopedTemplateResult::iterator iter = collector_ScopedTemplateResult.begin(); + iter != collector_ScopedTemplateResult.end(); ) { + delete *iter; + collector_ScopedTemplateResult.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 _template_RTTIRegister() { + const mxArray *alreadyCreated = mexGetVariablePtr("global", "gtsam_template_rttiRegistry_created"); + if(!alreadyCreated) { + std::map types; + + + + mxArray *registry = mexGetVariable("global", "gtsamwrap_rttiRegistry"); + if(!registry) + registry = mxCreateStructMatrix(1, 1, 0, NULL); + typedef std::pair StringPair; + for(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 TemplatedConstructor_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_TemplatedConstructor.insert(self); +} + +void TemplatedConstructor_constructor_1(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + Shared *self = new Shared(new TemplatedConstructor()); + collector_TemplatedConstructor.insert(self); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast (mxGetData(out[0])) = self; +} + +void TemplatedConstructor_constructor_2(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + string& arg = *unwrap_shared_ptr< string >(in[0], "ptr_string"); + Shared *self = new Shared(new TemplatedConstructor(arg)); + collector_TemplatedConstructor.insert(self); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast (mxGetData(out[0])) = self; +} + +void TemplatedConstructor_constructor_3(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + int arg = unwrap< int >(in[0]); + Shared *self = new Shared(new TemplatedConstructor(arg)); + collector_TemplatedConstructor.insert(self); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast (mxGetData(out[0])) = self; +} + +void TemplatedConstructor_constructor_4(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + double arg = unwrap< double >(in[0]); + Shared *self = new Shared(new TemplatedConstructor(arg)); + collector_TemplatedConstructor.insert(self); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast (mxGetData(out[0])) = self; +} + +void TemplatedConstructor_deconstructor_5(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("delete_TemplatedConstructor",nargout,nargin,1); + Shared *self = *reinterpret_cast(mxGetData(in[0])); + Collector_TemplatedConstructor::iterator item; + item = collector_TemplatedConstructor.find(self); + if(item != collector_TemplatedConstructor.end()) { + delete self; + collector_TemplatedConstructor.erase(item); + } +} + +void ScopedTemplateResult_collectorInsertAndMakeBase_6(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr> Shared; + + Shared *self = *reinterpret_cast (mxGetData(in[0])); + collector_ScopedTemplateResult.insert(self); +} + +void ScopedTemplateResult_constructor_7(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr> Shared; + + Result::Value& arg = *unwrap_shared_ptr< Result::Value >(in[0], "ptr_Result::Value"); + Shared *self = new Shared(new ScopedTemplate(arg)); + collector_ScopedTemplateResult.insert(self); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast (mxGetData(out[0])) = self; +} + +void ScopedTemplateResult_deconstructor_8(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr> Shared; + checkArguments("delete_ScopedTemplateResult",nargout,nargin,1); + Shared *self = *reinterpret_cast(mxGetData(in[0])); + Collector_ScopedTemplateResult::iterator item; + item = collector_ScopedTemplateResult.find(self); + if(item != collector_ScopedTemplateResult.end()) { + delete self; + collector_ScopedTemplateResult.erase(item); + } +} + + +void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mstream mout; + std::streambuf *outbuf = std::cout.rdbuf(&mout); + + _template_RTTIRegister(); + + int id = unwrap(in[0]); + + try { + switch(id) { + case 0: + TemplatedConstructor_collectorInsertAndMakeBase_0(nargout, out, nargin-1, in+1); + break; + case 1: + TemplatedConstructor_constructor_1(nargout, out, nargin-1, in+1); + break; + case 2: + TemplatedConstructor_constructor_2(nargout, out, nargin-1, in+1); + break; + case 3: + TemplatedConstructor_constructor_3(nargout, out, nargin-1, in+1); + break; + case 4: + TemplatedConstructor_constructor_4(nargout, out, nargin-1, in+1); + break; + case 5: + TemplatedConstructor_deconstructor_5(nargout, out, nargin-1, in+1); + break; + case 6: + ScopedTemplateResult_collectorInsertAndMakeBase_6(nargout, out, nargin-1, in+1); + break; + case 7: + ScopedTemplateResult_constructor_7(nargout, out, nargin-1, in+1); + break; + case 8: + ScopedTemplateResult_deconstructor_8(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/tests/expected/python/templates_pybind.cpp b/tests/expected/python/templates_pybind.cpp new file mode 100644 index 000000000..4d13d3731 --- /dev/null +++ b/tests/expected/python/templates_pybind.cpp @@ -0,0 +1,38 @@ + + +#include +#include +#include +#include +#include "gtsam/nonlinear/utilities.h" // for RedirectCout. + + +#include "wrap/serialization.h" +#include + + + + + +using namespace std; + +namespace py = pybind11; + +PYBIND11_MODULE(templates_py, m_) { + m_.doc() = "pybind11 wrapper of templates_py"; + + + py::class_>(m_, "TemplatedConstructor") + .def(py::init<>()) + .def(py::init(), py::arg("arg")) + .def(py::init(), py::arg("arg")) + .def(py::init(), py::arg("arg")); + + py::class_, std::shared_ptr>>(m_, "ScopedTemplateResult") + .def(py::init(), py::arg("arg")); + + +#include "python/specializations.h" + +} + diff --git a/tests/fixtures/templates.i b/tests/fixtures/templates.i new file mode 100644 index 000000000..c485041c6 --- /dev/null +++ b/tests/fixtures/templates.i @@ -0,0 +1,15 @@ +// Test for templated constructor +class TemplatedConstructor { + TemplatedConstructor(); + + template + TemplatedConstructor(const T& arg); +}; + +// Test for a scoped value inside a template +template +class ScopedTemplate { + // T should be properly substituted here. + ScopedTemplate(const T::Value& arg); +}; + diff --git a/tests/test_matlab_wrapper.py b/tests/test_matlab_wrapper.py index 112750721..89e53ab21 100644 --- a/tests/test_matlab_wrapper.py +++ b/tests/test_matlab_wrapper.py @@ -123,6 +123,23 @@ class TestWrap(unittest.TestCase): for file in files: self.compare_and_diff(file) + def test_templates(self): + """Test interface file with template info.""" + file = osp.join(self.INTERFACE_DIR, 'templates.i') + + wrapper = MatlabWrapper( + module_name='template', + top_module_namespace=['gtsam'], + ignore_classes=[''], + ) + + wrapper.wrap([file], path=self.MATLAB_ACTUAL_DIR) + + files = ['template_wrapper.cpp'] + + for file in files: + self.compare_and_diff(file) + def test_inheritance(self): """Test interface file with class inheritance definitions.""" file = osp.join(self.INTERFACE_DIR, 'inheritance.i') diff --git a/tests/test_pybind_wrapper.py b/tests/test_pybind_wrapper.py index 67c637d14..b47b4aca1 100644 --- a/tests/test_pybind_wrapper.py +++ b/tests/test_pybind_wrapper.py @@ -95,6 +95,14 @@ class TestWrap(unittest.TestCase): self.compare_and_diff('class_pybind.cpp', output) + def test_templates(self): + """Test interface file with templated class.""" + source = osp.join(self.INTERFACE_DIR, 'templates.i') + output = self.wrap_content([source], 'templates_py', + self.PYTHON_ACTUAL_DIR) + + self.compare_and_diff('templates_pybind.cpp', output) + def test_inheritance(self): """Test interface file with class inheritance definitions.""" source = osp.join(self.INTERFACE_DIR, 'inheritance.i') From 658effa99958c63843493e3e516d0ce3e898f271 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Mon, 4 Oct 2021 02:16:47 -0400 Subject: [PATCH 222/248] OptionalJacobian fixed constructor with dynamic pointer --- gtsam/base/OptionalJacobian.h | 7 +++ gtsam/base/tests/testOptionalJacobian.cpp | 64 ++++++++++++++--------- 2 files changed, 45 insertions(+), 26 deletions(-) diff --git a/gtsam/base/OptionalJacobian.h b/gtsam/base/OptionalJacobian.h index 4b580f82e..07801df7a 100644 --- a/gtsam/base/OptionalJacobian.h +++ b/gtsam/base/OptionalJacobian.h @@ -89,6 +89,13 @@ public: usurp(dynamic.data()); } + /// Constructor that will resize a dynamic matrix (unless already correct) + OptionalJacobian(Eigen::MatrixXd* dynamic) : + map_(nullptr) { + dynamic->resize(Rows, Cols); // no malloc if correct size + usurp(dynamic->data()); + } + #ifndef OPTIONALJACOBIAN_NOBOOST /// Constructor with boost::none just makes empty diff --git a/gtsam/base/tests/testOptionalJacobian.cpp b/gtsam/base/tests/testOptionalJacobian.cpp index 128576107..ae91642f4 100644 --- a/gtsam/base/tests/testOptionalJacobian.cpp +++ b/gtsam/base/tests/testOptionalJacobian.cpp @@ -24,40 +24,33 @@ using namespace std; using namespace gtsam; //****************************************************************************** +#define TEST_CONSTRUCTOR(DIM1, DIM2, X, TRUTHY) \ + { \ + OptionalJacobian H(X); \ + EXPECT(H == TRUTHY); \ + } TEST( OptionalJacobian, Constructors ) { Matrix23 fixed; - - OptionalJacobian<2, 3> H1; - EXPECT(!H1); - - OptionalJacobian<2, 3> H2(fixed); - EXPECT(H2); - - OptionalJacobian<2, 3> H3(&fixed); - EXPECT(H3); - Matrix dynamic; - OptionalJacobian<2, 3> H4(dynamic); - EXPECT(H4); - - OptionalJacobian<2, 3> H5(boost::none); - EXPECT(!H5); - boost::optional optional(dynamic); - OptionalJacobian<2, 3> H6(optional); - EXPECT(H6); + OptionalJacobian<2, 3> H; + EXPECT(!H); + + TEST_CONSTRUCTOR(2, 3, fixed, true); + TEST_CONSTRUCTOR(2, 3, &fixed, true); + TEST_CONSTRUCTOR(2, 3, dynamic, true); + TEST_CONSTRUCTOR(2, 3, &dynamic, true); + TEST_CONSTRUCTOR(2, 3, boost::none, false); + TEST_CONSTRUCTOR(2, 3, optional, true); + + // Test dynamic OptionalJacobian<-1, -1> H7; EXPECT(!H7); - OptionalJacobian<-1, -1> H8(dynamic); - EXPECT(H8); - - OptionalJacobian<-1, -1> H9(boost::none); - EXPECT(!H9); - - OptionalJacobian<-1, -1> H10(optional); - EXPECT(H10); + TEST_CONSTRUCTOR(-1, -1, dynamic, true); + TEST_CONSTRUCTOR(-1, -1, boost::none, false); + TEST_CONSTRUCTOR(-1, -1, optional, true); } //****************************************************************************** @@ -101,6 +94,25 @@ TEST( OptionalJacobian, Fixed) { dynamic2.setOnes(); test(dynamic2); EXPECT(assert_equal(kTestMatrix, dynamic2)); + + { // Dynamic pointer + // Passing in an empty matrix means we want it resized + Matrix dynamic0; + test(&dynamic0); + EXPECT(assert_equal(kTestMatrix, dynamic0)); + + // Dynamic wrong size + Matrix dynamic1(3, 5); + dynamic1.setOnes(); + test(&dynamic1); + EXPECT(assert_equal(kTestMatrix, dynamic1)); + + // Dynamic right size + Matrix dynamic2(2, 5); + dynamic2.setOnes(); + test(&dynamic2); + EXPECT(assert_equal(kTestMatrix, dynamic2)); + } } //****************************************************************************** From 7083de35a4aea261ab5a2e16585a94307b890c23 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Thu, 7 Oct 2021 15:54:56 -0400 Subject: [PATCH 223/248] add failing unit test on axisAngle for Rot3 in c++ --- gtsam/geometry/tests/testRot3.cpp | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 34f90c8cc..5f0187ed9 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -122,6 +122,21 @@ TEST( Rot3, AxisAngle) CHECK(assert_equal(expected,actual3,1e-5)); } +/* ************************************************************************* */ +TEST( Rot3, AxisAngle2) +{ + // constructor from a rotation matrix, as doubles in *row-major* order. + Rot3 R1(-0.999957, 0.00922903, 0.00203116, 0.00926964, 0.999739, 0.0208927, -0.0018374, 0.0209105, -0.999781); + + Unit3 actualAxis; + double actualAngle; + // convert Rot3 to quaternion using GTSAM + std::tie(actualAxis, actualAngle) = R1.axisAngle(); + + double expectedAngle = 3.1396582; + CHECK(assert_equal(expectedAngle, actualAngle, 1e-7)); +} + /* ************************************************************************* */ TEST( Rot3, Rodrigues) { From bb87dcf759964f7d4aa5dffa751e8c729129bf8e Mon Sep 17 00:00:00 2001 From: John Lambert Date: Thu, 7 Oct 2021 17:20:47 -0400 Subject: [PATCH 224/248] add python unit test for Rot3 --- python/gtsam/tests/test_Rot3.py | 41 +++++++++++++++++++++++++++++++++ 1 file changed, 41 insertions(+) create mode 100644 python/gtsam/tests/test_Rot3.py diff --git a/python/gtsam/tests/test_Rot3.py b/python/gtsam/tests/test_Rot3.py new file mode 100644 index 000000000..273e0e210 --- /dev/null +++ b/python/gtsam/tests/test_Rot3.py @@ -0,0 +1,41 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved +See LICENSE for the license information +Rot3 unit tests. +Author: John Lambert +""" +# pylint: disable=no-name-in-module + +import unittest + +import numpy as np + +import gtsam +from gtsam import Rot3 +from gtsam.utils.test_case import GtsamTestCase + + +class TestRot3(GtsamTestCase): + """Test selected Rot3 methods.""" + + def test_axisangle(self) -> None: + """Test .axisAngle() method.""" + # fmt: off + R = np.array( + [ + [ -0.999957, 0.00922903, 0.00203116], + [ 0.00926964, 0.999739, 0.0208927], + [ -0.0018374, 0.0209105, -0.999781] + ]) + # fmt: on + + # get back angle in radians + _, actual_angle = Rot3(R).axisAngle() + expected_angle = 3.1396582; + self.gtsamAssertEquals(actual_angle, expected_angle, 1e-7) + + +if __name__ == "__main__": + unittest.main() From 0b0897d465f5121f2cfad0888fc3bf045d9bf3ac Mon Sep 17 00:00:00 2001 From: John Lambert Date: Thu, 7 Oct 2021 17:21:22 -0400 Subject: [PATCH 225/248] fix typo --- python/gtsam/tests/test_Rot3.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/gtsam/tests/test_Rot3.py b/python/gtsam/tests/test_Rot3.py index 273e0e210..d22182433 100644 --- a/python/gtsam/tests/test_Rot3.py +++ b/python/gtsam/tests/test_Rot3.py @@ -33,7 +33,7 @@ class TestRot3(GtsamTestCase): # get back angle in radians _, actual_angle = Rot3(R).axisAngle() - expected_angle = 3.1396582; + expected_angle = 3.1396582 self.gtsamAssertEquals(actual_angle, expected_angle, 1e-7) From 225ac77f2f524ef4828cb7728628deb03d1aeddf Mon Sep 17 00:00:00 2001 From: John Lambert Date: Thu, 7 Oct 2021 21:04:58 -0400 Subject: [PATCH 226/248] fix assert --- python/gtsam/tests/test_Rot3.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/gtsam/tests/test_Rot3.py b/python/gtsam/tests/test_Rot3.py index d22182433..90c80ab31 100644 --- a/python/gtsam/tests/test_Rot3.py +++ b/python/gtsam/tests/test_Rot3.py @@ -34,7 +34,7 @@ class TestRot3(GtsamTestCase): # get back angle in radians _, actual_angle = Rot3(R).axisAngle() expected_angle = 3.1396582 - self.gtsamAssertEquals(actual_angle, expected_angle, 1e-7) + np.testing.assert_almost_equal(actual_angle, expected_angle, 1e-7) if __name__ == "__main__": From 9c9ea6551a7eb63457c0035f8ec4a800761d1725 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 8 Oct 2021 11:45:56 -0400 Subject: [PATCH 227/248] run tests for C++ CI --- .github/scripts/unix.sh | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/scripts/unix.sh b/.github/scripts/unix.sh index 7fb925593..9689d346c 100644 --- a/.github/scripts/unix.sh +++ b/.github/scripts/unix.sh @@ -113,9 +113,9 @@ function test () # Actual testing if [ "$(uname)" == "Linux" ]; then - make -j$(nproc) + make -j$(nproc) check elif [ "$(uname)" == "Darwin" ]; then - make -j$(sysctl -n hw.physicalcpu) + make -j$(sysctl -n hw.physicalcpu) check fi finish From 21279e6d5107f85021ba0788681b0733502a79bb Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sun, 10 Oct 2021 16:20:30 -0400 Subject: [PATCH 228/248] Revert "Revert "replace deprecated tbb functionality"" This reverts commit f819b1a03f74f289f50b96125610026618601a2f. --- gtsam/base/treeTraversal-inst.h | 5 +- .../treeTraversal/parallelTraversalTasks.h | 70 +++++++------------ 2 files changed, 29 insertions(+), 46 deletions(-) diff --git a/gtsam/base/treeTraversal-inst.h b/gtsam/base/treeTraversal-inst.h index 7a88f72eb..30cec3b9a 100644 --- a/gtsam/base/treeTraversal-inst.h +++ b/gtsam/base/treeTraversal-inst.h @@ -158,9 +158,8 @@ void DepthFirstForestParallel(FOREST& forest, DATA& rootData, // Typedefs typedef typename FOREST::Node Node; - tbb::task::spawn_root_and_wait( - internal::CreateRootTask(forest.roots(), rootData, visitorPre, - visitorPost, problemSizeThreshold)); + internal::CreateRootTask(forest.roots(), rootData, visitorPre, + visitorPost, problemSizeThreshold); #else DepthFirstForest(forest, rootData, visitorPre, visitorPost); #endif diff --git a/gtsam/base/treeTraversal/parallelTraversalTasks.h b/gtsam/base/treeTraversal/parallelTraversalTasks.h index 87d5b0d4c..dc1b45906 100644 --- a/gtsam/base/treeTraversal/parallelTraversalTasks.h +++ b/gtsam/base/treeTraversal/parallelTraversalTasks.h @@ -22,7 +22,7 @@ #include #ifdef GTSAM_USE_TBB -#include // tbb::task, tbb::task_list +#include // tbb::task_group #include // tbb::scalable_allocator namespace gtsam { @@ -34,7 +34,7 @@ namespace gtsam { /* ************************************************************************* */ template - class PreOrderTask : public tbb::task + class PreOrderTask { public: const boost::shared_ptr& treeNode; @@ -42,28 +42,30 @@ namespace gtsam { VISITOR_PRE& visitorPre; VISITOR_POST& visitorPost; int problemSizeThreshold; + tbb::task_group& tg; bool makeNewTasks; - bool isPostOrderPhase; + // Keep track of order phase across multiple calls to the same functor + mutable bool isPostOrderPhase; PreOrderTask(const boost::shared_ptr& treeNode, const boost::shared_ptr& myData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, int problemSizeThreshold, - bool makeNewTasks = true) + tbb::task_group& tg, bool makeNewTasks = true) : treeNode(treeNode), myData(myData), visitorPre(visitorPre), visitorPost(visitorPost), problemSizeThreshold(problemSizeThreshold), + tg(tg), makeNewTasks(makeNewTasks), isPostOrderPhase(false) {} - tbb::task* execute() override + void operator()() const { if(isPostOrderPhase) { // Run the post-order visitor since this task was recycled to run the post-order visitor (void) visitorPost(treeNode, *myData); - return nullptr; } else { @@ -71,14 +73,10 @@ namespace gtsam { { if(!treeNode->children.empty()) { - // Allocate post-order task as a continuation - isPostOrderPhase = true; - recycle_as_continuation(); - bool overThreshold = (treeNode->problemSize() >= problemSizeThreshold); - tbb::task* firstChild = 0; - tbb::task_list childTasks; + // If we have child tasks, start subtasks and wait for them to complete + tbb::task_group ctg; for(const boost::shared_ptr& child: treeNode->children) { // Process child in a subtask. Important: Run visitorPre before calling @@ -86,37 +84,30 @@ namespace gtsam { // allocated an extra child, this causes a TBB error. boost::shared_ptr childData = boost::allocate_shared( tbb::scalable_allocator(), visitorPre(child, *myData)); - tbb::task* childTask = - new (allocate_child()) PreOrderTask(child, childData, visitorPre, visitorPost, - problemSizeThreshold, overThreshold); - if (firstChild) - childTasks.push_back(*childTask); - else - firstChild = childTask; + ctg.run(PreOrderTask(child, childData, visitorPre, visitorPost, + problemSizeThreshold, ctg, overThreshold)); } + ctg.wait(); - // If we have child tasks, start subtasks and wait for them to complete - set_ref_count((int)treeNode->children.size()); - spawn(childTasks); - return firstChild; + // Allocate post-order task as a continuation + isPostOrderPhase = true; + tg.run(*this); } else { // Run the post-order visitor in this task if we have no children (void) visitorPost(treeNode, *myData); - return nullptr; } } else { // Process this node and its children in this task processNodeRecursively(treeNode, *myData); - return nullptr; } } } - void processNodeRecursively(const boost::shared_ptr& node, DATA& myData) + void processNodeRecursively(const boost::shared_ptr& node, DATA& myData) const { for(const boost::shared_ptr& child: node->children) { @@ -131,7 +122,7 @@ namespace gtsam { /* ************************************************************************* */ template - class RootTask : public tbb::task + class RootTask { public: const ROOTS& roots; @@ -139,38 +130,31 @@ namespace gtsam { VISITOR_PRE& visitorPre; VISITOR_POST& visitorPost; int problemSizeThreshold; + tbb::task_group& tg; RootTask(const ROOTS& roots, DATA& myData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, - int problemSizeThreshold) : + int problemSizeThreshold, tbb::task_group& tg) : roots(roots), myData(myData), visitorPre(visitorPre), visitorPost(visitorPost), - problemSizeThreshold(problemSizeThreshold) {} + problemSizeThreshold(problemSizeThreshold), tg(tg) {} - tbb::task* execute() override + void operator()() const { typedef PreOrderTask PreOrderTask; // Create data and tasks for our children - tbb::task_list tasks; for(const boost::shared_ptr& root: roots) { boost::shared_ptr rootData = boost::allocate_shared(tbb::scalable_allocator(), visitorPre(root, myData)); - tasks.push_back(*new(allocate_child()) - PreOrderTask(root, rootData, visitorPre, visitorPost, problemSizeThreshold)); + tg.run(PreOrderTask(root, rootData, visitorPre, visitorPost, problemSizeThreshold, tg)); } - // Set TBB ref count - set_ref_count(1 + (int) roots.size()); - // Spawn tasks - spawn_and_wait_for_all(tasks); - // Return nullptr - return nullptr; } }; template - RootTask& - CreateRootTask(const ROOTS& roots, DATA& rootData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, int problemSizeThreshold) + void CreateRootTask(const ROOTS& roots, DATA& rootData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, int problemSizeThreshold) { typedef RootTask RootTask; - return *new(tbb::task::allocate_root()) RootTask(roots, rootData, visitorPre, visitorPost, problemSizeThreshold); - } + tbb::task_group tg; + tg.run_and_wait(RootTask(roots, rootData, visitorPre, visitorPost, problemSizeThreshold, tg)); + } } From 1a97b1413825045d7ff8f6e4a15fab64b8bcb7e4 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Tue, 12 Oct 2021 19:02:40 -0400 Subject: [PATCH 229/248] Fixed numerical problems near +-pi --- gtsam/geometry/SO3.cpp | 30 ++++++++++++++++++++--------- gtsam/geometry/tests/testRot3.cpp | 32 +++++++++++++++---------------- 2 files changed, 37 insertions(+), 25 deletions(-) diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index 80c0171ad..a2309e1f4 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -261,25 +261,37 @@ Vector3 SO3::Logmap(const SO3& Q, ChartJacobian H) { // when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc. // we do something special - if (tr + 1.0 < 1e-10) { - if (std::abs(R33 + 1.0) > 1e-5) - omega = (M_PI / sqrt(2.0 + 2.0 * R33)) * Vector3(R13, R23, 1.0 + R33); - else if (std::abs(R22 + 1.0) > 1e-5) - omega = (M_PI / sqrt(2.0 + 2.0 * R22)) * Vector3(R12, 1.0 + R22, R32); - else + if (tr + 1.0 < 1e-4) { + std::vector diags = {R11, R22, R33}; + size_t max_elem = std::distance(diags.begin(), std::max_element(diags.begin(), diags.end())); + if (max_elem == 2) { + const double sgn_w = (R21 - R12) < 0 ? -1.0 : 1.0; + const double r = sqrt(2.0 + 2.0 * R33); + const double correction = 1.0 - M_1_PI / r * (R21 - R12); + omega = sgn_w * correction * (M_PI_2 / sqrt(2.0 + 2.0 * R33)) * Vector3(R31 + R13, R32 + R23, 2.0 + 2.0 * R33); + } else if (max_elem == 1) { + const double sgn_w = (R13 - R31) < 0 ? -1.0 : 1.0; + const double r = sqrt(2.0 + 2.0 * R22); + const double correction = 1.0 - M_1_PI / r * (R13 - R31); + omega = sgn_w * correction * (M_PI_2 / sqrt(2.0 + 2.0 * R22)) * Vector3(R21 + R12, 2.0 + 2.0 * R22, R23 + R32); + } else { // if(std::abs(R.r1_.x()+1.0) > 1e-5) This is implicit - omega = (M_PI / sqrt(2.0 + 2.0 * R11)) * Vector3(1.0 + R11, R21, R31); + const double sgn_w = (R32 - R23) < 0 ? -1.0 : 1.0; + const double r = sqrt(2.0 + 2.0 * R11); + const double correction = 1.0 - M_1_PI / r * (R32 - R23); + omega = sgn_w * correction * (M_PI_2 / sqrt(2.0 + 2.0 * R11)) * Vector3(2.0 + 2.0 * R11, R12 + R21, R13 + R31); + } } else { double magnitude; const double tr_3 = tr - 3.0; // always negative - if (tr_3 < -1e-7) { + if (tr_3 < -1e-6) { double theta = acos((tr - 1.0) / 2.0); magnitude = theta / (2.0 * sin(theta)); } else { // when theta near 0, +-2pi, +-4pi, etc. (trace near 3.0) // use Taylor expansion: theta \approx 1/2-(t-3)/12 + O((t-3)^2) // see https://github.com/borglab/gtsam/issues/746 for details - magnitude = 0.5 - tr_3 / 12.0; + magnitude = 0.5 - tr_3 / 12.0 + tr_3*tr_3/60.0; } omega = magnitude * Vector3(R32 - R23, R13 - R31, R21 - R12); } diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 5f0187ed9..bb6f1a77a 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -134,7 +134,7 @@ TEST( Rot3, AxisAngle2) std::tie(actualAxis, actualAngle) = R1.axisAngle(); double expectedAngle = 3.1396582; - CHECK(assert_equal(expectedAngle, actualAngle, 1e-7)); + CHECK(assert_equal(expectedAngle, actualAngle, 1e-5)); } /* ************************************************************************* */ @@ -196,13 +196,13 @@ TEST( Rot3, retract) } /* ************************************************************************* */ -TEST(Rot3, log) { +TEST( Rot3, log) { static const double PI = boost::math::constants::pi(); Vector w; Rot3 R; #define CHECK_OMEGA(X, Y, Z) \ - w = (Vector(3) << X, Y, Z).finished(); \ + w = (Vector(3) << (X), (Y), (Z)).finished(); \ R = Rot3::Rodrigues(w); \ EXPECT(assert_equal(w, Rot3::Logmap(R), 1e-12)); @@ -234,17 +234,17 @@ TEST(Rot3, log) { CHECK_OMEGA(0, 0, PI) // Windows and Linux have flipped sign in quaternion mode -#if !defined(__APPLE__) && defined(GTSAM_USE_QUATERNIONS) +//#if !defined(__APPLE__) && defined(GTSAM_USE_QUATERNIONS) w = (Vector(3) << x * PI, y * PI, z * PI).finished(); R = Rot3::Rodrigues(w); EXPECT(assert_equal(Vector(-w), Rot3::Logmap(R), 1e-12)); -#else - CHECK_OMEGA(x * PI, y * PI, z * PI) -#endif +//#else +// CHECK_OMEGA(x * PI, y * PI, z * PI) +//#endif // Check 360 degree rotations #define CHECK_OMEGA_ZERO(X, Y, Z) \ - w = (Vector(3) << X, Y, Z).finished(); \ + w = (Vector(3) << (X), (Y), (Z)).finished(); \ R = Rot3::Rodrigues(w); \ EXPECT(assert_equal((Vector)Z_3x1, Rot3::Logmap(R))); @@ -262,15 +262,15 @@ TEST(Rot3, log) { // Rot3's Logmap returns different, but equivalent compacted // axis-angle vectors depending on whether Rot3 is implemented // by Quaternions or SO3. - #if defined(GTSAM_USE_QUATERNIONS) - // Quaternion bounds angle to [-pi, pi] resulting in ~179.9 degrees - EXPECT(assert_equal(Vector3(0.264451979, -0.742197651, -3.04098211), +#if defined(GTSAM_USE_QUATERNIONS) + // Quaternion bounds angle to [-pi, pi] resulting in ~179.9 degrees + EXPECT(assert_equal(Vector3(0.264451979, -0.742197651, -3.04098211), + (Vector)Rot3::Logmap(Rlund), 1e-8)); +#else + // SO3 will be approximate because of the non-orthogonality + EXPECT(assert_equal(Vector3(0.264485272, -0.742291088, -3.04136444), (Vector)Rot3::Logmap(Rlund), 1e-8)); - #else - // SO3 does not bound angle resulting in ~180.1 degrees - EXPECT(assert_equal(Vector3(-0.264544406, 0.742217405, 3.04117314), - (Vector)Rot3::Logmap(Rlund), 1e-8)); - #endif +#endif } /* ************************************************************************* */ From dae409373c66abd6c41cb851a9c4a54b8e625dec Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Tue, 12 Oct 2021 19:03:56 -0400 Subject: [PATCH 230/248] Revert "Revert "Revert "replace deprecated tbb functionality""" This reverts commit 21279e6d5107f85021ba0788681b0733502a79bb. --- gtsam/base/treeTraversal-inst.h | 5 +- .../treeTraversal/parallelTraversalTasks.h | 70 ++++++++++++------- 2 files changed, 46 insertions(+), 29 deletions(-) diff --git a/gtsam/base/treeTraversal-inst.h b/gtsam/base/treeTraversal-inst.h index 30cec3b9a..7a88f72eb 100644 --- a/gtsam/base/treeTraversal-inst.h +++ b/gtsam/base/treeTraversal-inst.h @@ -158,8 +158,9 @@ void DepthFirstForestParallel(FOREST& forest, DATA& rootData, // Typedefs typedef typename FOREST::Node Node; - internal::CreateRootTask(forest.roots(), rootData, visitorPre, - visitorPost, problemSizeThreshold); + tbb::task::spawn_root_and_wait( + internal::CreateRootTask(forest.roots(), rootData, visitorPre, + visitorPost, problemSizeThreshold)); #else DepthFirstForest(forest, rootData, visitorPre, visitorPost); #endif diff --git a/gtsam/base/treeTraversal/parallelTraversalTasks.h b/gtsam/base/treeTraversal/parallelTraversalTasks.h index dc1b45906..87d5b0d4c 100644 --- a/gtsam/base/treeTraversal/parallelTraversalTasks.h +++ b/gtsam/base/treeTraversal/parallelTraversalTasks.h @@ -22,7 +22,7 @@ #include #ifdef GTSAM_USE_TBB -#include // tbb::task_group +#include // tbb::task, tbb::task_list #include // tbb::scalable_allocator namespace gtsam { @@ -34,7 +34,7 @@ namespace gtsam { /* ************************************************************************* */ template - class PreOrderTask + class PreOrderTask : public tbb::task { public: const boost::shared_ptr& treeNode; @@ -42,30 +42,28 @@ namespace gtsam { VISITOR_PRE& visitorPre; VISITOR_POST& visitorPost; int problemSizeThreshold; - tbb::task_group& tg; bool makeNewTasks; - // Keep track of order phase across multiple calls to the same functor - mutable bool isPostOrderPhase; + bool isPostOrderPhase; PreOrderTask(const boost::shared_ptr& treeNode, const boost::shared_ptr& myData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, int problemSizeThreshold, - tbb::task_group& tg, bool makeNewTasks = true) + bool makeNewTasks = true) : treeNode(treeNode), myData(myData), visitorPre(visitorPre), visitorPost(visitorPost), problemSizeThreshold(problemSizeThreshold), - tg(tg), makeNewTasks(makeNewTasks), isPostOrderPhase(false) {} - void operator()() const + tbb::task* execute() override { if(isPostOrderPhase) { // Run the post-order visitor since this task was recycled to run the post-order visitor (void) visitorPost(treeNode, *myData); + return nullptr; } else { @@ -73,10 +71,14 @@ namespace gtsam { { if(!treeNode->children.empty()) { + // Allocate post-order task as a continuation + isPostOrderPhase = true; + recycle_as_continuation(); + bool overThreshold = (treeNode->problemSize() >= problemSizeThreshold); - // If we have child tasks, start subtasks and wait for them to complete - tbb::task_group ctg; + tbb::task* firstChild = 0; + tbb::task_list childTasks; for(const boost::shared_ptr& child: treeNode->children) { // Process child in a subtask. Important: Run visitorPre before calling @@ -84,30 +86,37 @@ namespace gtsam { // allocated an extra child, this causes a TBB error. boost::shared_ptr childData = boost::allocate_shared( tbb::scalable_allocator(), visitorPre(child, *myData)); - ctg.run(PreOrderTask(child, childData, visitorPre, visitorPost, - problemSizeThreshold, ctg, overThreshold)); + tbb::task* childTask = + new (allocate_child()) PreOrderTask(child, childData, visitorPre, visitorPost, + problemSizeThreshold, overThreshold); + if (firstChild) + childTasks.push_back(*childTask); + else + firstChild = childTask; } - ctg.wait(); - // Allocate post-order task as a continuation - isPostOrderPhase = true; - tg.run(*this); + // If we have child tasks, start subtasks and wait for them to complete + set_ref_count((int)treeNode->children.size()); + spawn(childTasks); + return firstChild; } else { // Run the post-order visitor in this task if we have no children (void) visitorPost(treeNode, *myData); + return nullptr; } } else { // Process this node and its children in this task processNodeRecursively(treeNode, *myData); + return nullptr; } } } - void processNodeRecursively(const boost::shared_ptr& node, DATA& myData) const + void processNodeRecursively(const boost::shared_ptr& node, DATA& myData) { for(const boost::shared_ptr& child: node->children) { @@ -122,7 +131,7 @@ namespace gtsam { /* ************************************************************************* */ template - class RootTask + class RootTask : public tbb::task { public: const ROOTS& roots; @@ -130,31 +139,38 @@ namespace gtsam { VISITOR_PRE& visitorPre; VISITOR_POST& visitorPost; int problemSizeThreshold; - tbb::task_group& tg; RootTask(const ROOTS& roots, DATA& myData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, - int problemSizeThreshold, tbb::task_group& tg) : + int problemSizeThreshold) : roots(roots), myData(myData), visitorPre(visitorPre), visitorPost(visitorPost), - problemSizeThreshold(problemSizeThreshold), tg(tg) {} + problemSizeThreshold(problemSizeThreshold) {} - void operator()() const + tbb::task* execute() override { typedef PreOrderTask PreOrderTask; // Create data and tasks for our children + tbb::task_list tasks; for(const boost::shared_ptr& root: roots) { boost::shared_ptr rootData = boost::allocate_shared(tbb::scalable_allocator(), visitorPre(root, myData)); - tg.run(PreOrderTask(root, rootData, visitorPre, visitorPost, problemSizeThreshold, tg)); + tasks.push_back(*new(allocate_child()) + PreOrderTask(root, rootData, visitorPre, visitorPost, problemSizeThreshold)); } + // Set TBB ref count + set_ref_count(1 + (int) roots.size()); + // Spawn tasks + spawn_and_wait_for_all(tasks); + // Return nullptr + return nullptr; } }; template - void CreateRootTask(const ROOTS& roots, DATA& rootData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, int problemSizeThreshold) + RootTask& + CreateRootTask(const ROOTS& roots, DATA& rootData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, int problemSizeThreshold) { typedef RootTask RootTask; - tbb::task_group tg; - tg.run_and_wait(RootTask(roots, rootData, visitorPre, visitorPost, problemSizeThreshold, tg)); - } + return *new(tbb::task::allocate_root()) RootTask(roots, rootData, visitorPre, visitorPost, problemSizeThreshold); + } } From aa6b4329110368c6cd6985f3d77f6af9e3572bd9 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Tue, 12 Oct 2021 20:30:06 -0400 Subject: [PATCH 231/248] Cleanup equation --- gtsam/geometry/SO3.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index a2309e1f4..0cb6c93d1 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -267,19 +267,19 @@ Vector3 SO3::Logmap(const SO3& Q, ChartJacobian H) { if (max_elem == 2) { const double sgn_w = (R21 - R12) < 0 ? -1.0 : 1.0; const double r = sqrt(2.0 + 2.0 * R33); - const double correction = 1.0 - M_1_PI / r * (R21 - R12); - omega = sgn_w * correction * (M_PI_2 / sqrt(2.0 + 2.0 * R33)) * Vector3(R31 + R13, R32 + R23, 2.0 + 2.0 * R33); + const double scale = M_PI_2 / r - 0.5 / (r * r) * (R21 - R12); + omega = sgn_w * scale * Vector3(R31 + R13, R32 + R23, 2.0 + 2.0 * R33); } else if (max_elem == 1) { const double sgn_w = (R13 - R31) < 0 ? -1.0 : 1.0; const double r = sqrt(2.0 + 2.0 * R22); - const double correction = 1.0 - M_1_PI / r * (R13 - R31); - omega = sgn_w * correction * (M_PI_2 / sqrt(2.0 + 2.0 * R22)) * Vector3(R21 + R12, 2.0 + 2.0 * R22, R23 + R32); + const double scale = M_PI_2 / r - 0.5 / (r * r) * (R13 - R31); + omega = sgn_w * scale * Vector3(R21 + R12, 2.0 + 2.0 * R22, R23 + R32); } else { // if(std::abs(R.r1_.x()+1.0) > 1e-5) This is implicit const double sgn_w = (R32 - R23) < 0 ? -1.0 : 1.0; const double r = sqrt(2.0 + 2.0 * R11); - const double correction = 1.0 - M_1_PI / r * (R32 - R23); - omega = sgn_w * correction * (M_PI_2 / sqrt(2.0 + 2.0 * R11)) * Vector3(2.0 + 2.0 * R11, R12 + R21, R13 + R31); + const double scale = M_PI_2 / r - 0.5 / (r * r) * (R32 - R23); + omega = sgn_w * scale * Vector3(2.0 + 2.0 * R11, R12 + R21, R13 + R31); } } else { double magnitude; From 35061ca1356b081f94712ec5f343fb9592cf0d21 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Wed, 13 Oct 2021 13:46:33 -0400 Subject: [PATCH 232/248] simplify logic of biggest diagonal --- gtsam/geometry/SO3.cpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index 0cb6c93d1..37ea3840b 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -262,20 +262,20 @@ Vector3 SO3::Logmap(const SO3& Q, ChartJacobian H) { // when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc. // we do something special if (tr + 1.0 < 1e-4) { - std::vector diags = {R11, R22, R33}; - size_t max_elem = std::distance(diags.begin(), std::max_element(diags.begin(), diags.end())); - if (max_elem == 2) { + if (R33 > R22 && R33 > R11) { + // R33 is the largest diagonal const double sgn_w = (R21 - R12) < 0 ? -1.0 : 1.0; const double r = sqrt(2.0 + 2.0 * R33); const double scale = M_PI_2 / r - 0.5 / (r * r) * (R21 - R12); omega = sgn_w * scale * Vector3(R31 + R13, R32 + R23, 2.0 + 2.0 * R33); - } else if (max_elem == 1) { + } else if (R22 > R11) { + // R22 is the largest diagonal const double sgn_w = (R13 - R31) < 0 ? -1.0 : 1.0; const double r = sqrt(2.0 + 2.0 * R22); const double scale = M_PI_2 / r - 0.5 / (r * r) * (R13 - R31); omega = sgn_w * scale * Vector3(R21 + R12, 2.0 + 2.0 * R22, R23 + R32); } else { - // if(std::abs(R.r1_.x()+1.0) > 1e-5) This is implicit + // R33 is the largest diagonal const double sgn_w = (R32 - R23) < 0 ? -1.0 : 1.0; const double r = sqrt(2.0 + 2.0 * R11); const double scale = M_PI_2 / r - 0.5 / (r * r) * (R32 - R23); @@ -283,8 +283,9 @@ Vector3 SO3::Logmap(const SO3& Q, ChartJacobian H) { } } else { double magnitude; - const double tr_3 = tr - 3.0; // always negative + const double tr_3 = tr - 3.0; // could be non-negative if the matrix is off orthogonal if (tr_3 < -1e-6) { + // this is the normal case -1 < trace < 3 double theta = acos((tr - 1.0) / 2.0); magnitude = theta / (2.0 * sin(theta)); } else { From 48cd9794daaa87517d217bd714118202bda1f427 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Wed, 13 Oct 2021 13:48:41 -0400 Subject: [PATCH 233/248] Fix typo --- gtsam/geometry/SO3.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index 37ea3840b..890bcd206 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -275,7 +275,7 @@ Vector3 SO3::Logmap(const SO3& Q, ChartJacobian H) { const double scale = M_PI_2 / r - 0.5 / (r * r) * (R13 - R31); omega = sgn_w * scale * Vector3(R21 + R12, 2.0 + 2.0 * R22, R23 + R32); } else { - // R33 is the largest diagonal + // R11 is the largest diagonal const double sgn_w = (R32 - R23) < 0 ? -1.0 : 1.0; const double r = sqrt(2.0 + 2.0 * R11); const double scale = M_PI_2 / r - 0.5 / (r * r) * (R32 - R23); From 80ebd5a63b23736d8cb81e8e55833a905baabe75 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Wed, 13 Oct 2021 19:41:04 -0400 Subject: [PATCH 234/248] Add specific examples to stress-test the log map --- python/gtsam/tests/test_Rot3.py | 1998 ++++++++++++++++++++++++++++++- 1 file changed, 1997 insertions(+), 1 deletion(-) diff --git a/python/gtsam/tests/test_Rot3.py b/python/gtsam/tests/test_Rot3.py index 90c80ab31..a1ce01ba2 100644 --- a/python/gtsam/tests/test_Rot3.py +++ b/python/gtsam/tests/test_Rot3.py @@ -17,6 +17,1990 @@ from gtsam import Rot3 from gtsam.utils.test_case import GtsamTestCase +R1_R2_pairs = [ + ( + [ + [0.994283, -0.10356, 0.0260251], + [0.102811, 0.994289, 0.0286205], + [-0.0288404, -0.0257812, 0.999251], + ], + [ + [-0.994235, 0.0918291, -0.0553602], + [-0.0987317, -0.582632, 0.806718], + [0.0418251, 0.807532, 0.588339], + ], + ), + ( + [ + [0.999823, -0.000724729, 0.0187896], + [0.00220672, 0.996874, -0.0789728], + [-0.0186736, 0.0790003, 0.9967], + ], + [ + [-0.99946, -0.0155217, -0.0289749], + [-0.0306159, 0.760422, 0.648707], + [0.0119641, 0.649244, -0.760487], + ], + ), + ( + [ + [0.999976, 0.00455542, -0.00529608], + [-0.00579633, 0.964214, -0.265062], + [0.00389908, 0.265086, 0.964217], + ], + [ + [-0.999912, -0.0123323, -0.00489179], + [-0.00739095, 0.21159, 0.977331], + [-0.0110179, 0.977281, -0.211663], + ], + ), + ( + [ + [0.998801, 0.0449026, 0.019479], + [-0.0448727, 0.998991, -0.00197348], + [-0.0195479, 0.00109704, 0.999808], + ], + [ + [-0.999144, -0.0406154, -0.00800012], + [0.0406017, -0.999174, 0.00185875], + [-0.00806909, 0.00153352, 0.999966], + ], + ), + ( + [ + [0.587202, 0.00034062, -0.80944], + [0.394859, 0.872825, 0.286815], + [0.706597, -0.488034, 0.51239], + ], + [ + [-0.999565, -0.028095, -0.00905389], + [0.0192863, -0.853838, 0.520182], + [-0.0223455, 0.519782, 0.854007], + ], + ), + ( + [ + [0.998798, 0.0370584, 0.0320815], + [-0.0355966, 0.998353, -0.0449943], + [-0.033696, 0.0437982, 0.998472], + ], + [ + [-0.999942, -0.010745, -0.00132538], + [-0.000998705, -0.0304045, 0.999538], + [-0.0107807, 0.999481, 0.0303914], + ], + ), + ( + [ + [0.998755, 0.00708291, -0.0493744], + [-0.00742097, 0.99995, -0.00666709], + [0.0493247, 0.0070252, 0.998758], + ], + [ + [-0.998434, 0.0104672, 0.0549825], + [0.0115323, 0.999751, 0.0190859], + [-0.0547691, 0.01969, -0.998307], + ], + ), + ( + [ + [0.990471, 0.0997485, -0.0949595], + [-0.117924, 0.970427, -0.210631], + [0.0711411, 0.219822, 0.972943], + ], + [ + [-0.99192, -0.125627, 0.0177888], + [0.126478, -0.967866, 0.217348], + [-0.0100874, 0.217839, 0.975933], + ], + ), + ( + [ + [-0.780894, -0.578319, -0.236116], + [0.34478, -0.0838381, -0.934932], + [0.520894, -0.811491, 0.264862], + ], + [ + [-0.99345, 0.00261746, -0.114244], + [-0.112503, 0.152922, 0.981815], + [0.0200403, 0.988236, -0.151626], + ], + ), + ( + [ + [0.968425, 0.0466097, 0.244911], + [-0.218867, 0.629346, 0.745668], + [-0.119378, -0.775726, 0.619676], + ], + [ + [-0.971208, 0.00666431, -0.238143], + [0.0937886, 0.929584, -0.35648], + [0.218998, -0.368551, -0.903444], + ], + ), + ( + [ + [0.998512, 0.0449168, -0.0309146], + [-0.0344032, 0.958823, 0.281914], + [0.0423043, -0.280431, 0.958941], + ], + [ + [-0.999713, 0.00732431, 0.0228168], + [-0.00759688, 0.806166, -0.59164], + [-0.0227275, -0.591644, -0.805879], + ], + ), + ( + [ + [0.981814, 0.00930728, 0.189617], + [-0.0084101, 0.999949, -0.00553563], + [-0.189659, 0.00384026, 0.981843], + ], + [ + [-0.981359, 0.00722349, -0.192051], + [0.00148564, 0.999549, 0.0300036], + [0.192182, 0.0291591, -0.980927], + ], + ), + ( + [ + [0.972544, -0.215591, 0.0876242], + [0.220661, 0.973915, -0.0529018], + [-0.0739333, 0.0707846, 0.994748], + ], + [ + [-0.971294, 0.215675, -0.100371], + [-0.23035, -0.747337, 0.62324], + [0.0594069, 0.628469, 0.775564], + ], + ), + ( + [ + [0.989488, 0.0152447, 0.143808], + [-0.0160974, 0.999859, 0.00476753], + [-0.143715, -0.00703235, 0.989594], + ], + [ + [-0.988492, 0.0124362, -0.150766], + [0.00992423, 0.999799, 0.0174037], + [0.150952, 0.0157072, -0.988417], + ], + ), + ( + [ + [0.99026, 0.109934, -0.0854388], + [-0.0973012, 0.985345, 0.140096], + [0.099588, -0.130418, 0.986445], + ], + [ + [-0.994239, 0.0206112, 0.1052], + [0.0227944, 0.999548, 0.0195944], + [-0.104748, 0.0218794, -0.994259], + ], + ), + ( + [ + [0.988981, 0.132742, -0.0655406], + [-0.113134, 0.963226, 0.243712], + [0.0954813, -0.233612, 0.96763], + ], + [ + [-0.989473, -0.144453, 0.00888153], + [0.112318, -0.727754, 0.67658], + [-0.0912697, 0.670455, 0.736317], + ], + ), + ( + [ + [0.13315, -0.722685, 0.678231], + [0.255831, 0.686195, 0.680946], + [-0.957508, 0.0828446, 0.276252], + ], + [ + [-0.233019, 0.0127274, -0.97239], + [-0.0143824, 0.99976, 0.0165321], + [0.972367, 0.0178377, -0.23278], + ], + ), + ( + [ + [0.906305, -0.0179617, -0.422243], + [0.0246095, 0.999644, 0.0102984], + [0.421908, -0.0197247, 0.906424], + ], + [ + [-0.90393, 0.0136293, 0.427466], + [0.0169755, 0.999848, 0.0040176], + [-0.427346, 0.0108879, -0.904024], + ], + ), + ( + [ + [0.999808, 0.0177784, -0.00826505], + [-0.0177075, 0.999806, 0.00856939], + [0.0084158, -0.00842139, 0.999929], + ], + [ + [-0.999901, -0.0141114, 0.00072392], + [0.00130602, -0.0413336, 0.999145], + [-0.0140699, 0.999047, 0.0413473], + ], + ), + ( + [ + [0.985811, -0.161425, 0.0460375], + [0.154776, 0.980269, 0.12295], + [-0.0649764, -0.11408, 0.991344], + ], + [ + [-0.985689, 0.137931, -0.09692], + [-0.162627, -0.626622, 0.762168], + [0.0443951, 0.767022, 0.640085], + ], + ), + ( + [ + [0.956652, -0.0116044, 0.291001], + [0.05108, 0.990402, -0.128428], + [-0.286718, 0.137726, 0.948064], + ], + [ + [-0.956189, 0.00996594, -0.292585], + [-0.0397033, 0.985772, 0.16333], + [0.29005, 0.167791, -0.942189], + ], + ), + ( + [ + [0.783763, -0.0181248, -0.620796], + [-0.0386421, 0.996214, -0.0778717], + [0.619857, 0.0850218, 0.780095], + ], + [ + [-0.780275, 0.0093644, 0.625368], + [-0.0221791, 0.998845, -0.0426297], + [-0.625045, -0.0471329, -0.779165], + ], + ), + ( + [ + [0.890984, 0.0232464, -0.453439], + [-0.0221215, 0.999725, 0.00778511], + [0.453495, 0.00309433, 0.891253], + ], + [ + [-0.890178, 0.0290103, 0.45469], + [0.0337152, 0.999429, 0.0022403], + [-0.454366, 0.0173244, -0.890648], + ], + ), + ( + [ + [0.998177, -0.0119546, 0.0591504], + [0.00277494, 0.988238, 0.152901], + [-0.0602825, -0.152458, 0.98647], + ], + [ + [-0.997444, 0.00871865, -0.0709414], + [0.0197108, 0.987598, -0.155762], + [0.0687035, -0.156762, -0.985246], + ], + ), + ( + [ + [0.985214, 0.164929, 0.0463837], + [-0.166966, 0.984975, 0.0441225], + [-0.0384096, -0.0512146, 0.997949], + ], + [ + [-0.999472, -0.000819214, -0.0325087], + [-0.00344291, 0.99673, 0.0807324], + [0.0323362, 0.0808019, -0.996206], + ], + ), + ( + [ + [0.998499, 0.0465241, 0.0288955], + [-0.0454764, 0.99832, -0.0359142], + [-0.0305178, 0.0345463, 0.998937], + ], + [ + [-0.999441, 0.00412484, -0.0332105], + [0.00374685, 0.999928, 0.0114307], + [0.0332552, 0.0112999, -0.999384], + ], + ), + ( + [ + [0.10101, -0.943239, -0.316381], + [0.841334, -0.0887423, 0.533182], + [-0.530994, -0.320039, 0.784615], + ], + [ + [-0.725665, 0.0153749, -0.687878], + [-0.304813, 0.889109, 0.34143], + [0.616848, 0.457438, -0.640509], + ], + ), + ( + [ + [0.843428, 0.174952, 0.507958], + [0.0435637, 0.920106, -0.389239], + [-0.535473, 0.350423, 0.768422], + ], + [ + [-0.835464, 0.0040872, -0.549533], + [0.00227251, 0.999989, 0.00398241], + [0.549543, 0.00207845, -0.835464], + ], + ), + ( + [ + [0.999897, -0.0142888, -0.00160177], + [0.0141561, 0.997826, -0.064364], + [0.00251798, 0.0643346, 0.997925], + ], + [ + [-0.999956, 0.00898988, 0.00296485], + [0.00900757, 0.999941, 0.00601779], + [-0.00291058, 0.00604429, -0.999978], + ], + ), + ( + [ + [0.999477, -0.0204859, 0.0250096], + [0.0126204, 0.959462, 0.281557], + [-0.0297637, -0.281094, 0.959219], + ], + [ + [-0.999384, 0.0172602, -0.0305795], + [-0.0254425, 0.24428, 0.969371], + [0.0242012, 0.969551, -0.24369], + ], + ), + ( + [ + [0.984597, 0.173474, -0.0218106], + [-0.15145, 0.783891, -0.602145], + [-0.0873592, 0.596173, 0.798089], + ], + [ + [-0.998608, -0.0432858, 0.0301827], + [-0.00287128, 0.615692, 0.787983], + [-0.0526917, 0.786797, -0.61496], + ], + ), + ( + [ + [0.917099, -0.3072, 0.254083], + [0.303902, 0.951219, 0.0531566], + [-0.258018, 0.0284665, 0.965721], + ], + [ + [-0.931191, 0.347008, -0.111675], + [-0.352102, -0.77686, 0.522032], + [0.0943935, 0.52543, 0.845586], + ], + ), + ( + [ + [0.991706, 0.0721037, -0.106393], + [-0.0995017, 0.954693, -0.280464], + [0.0813505, 0.288725, 0.95395], + ], + [ + [-0.995306, 0.00106317, 0.0967833], + [0.0167662, 0.986717, 0.161583], + [-0.0953259, 0.162447, -0.982103], + ], + ), + ( + [ + [0.997078, 0.0478273, -0.0595641], + [-0.0348316, 0.978617, 0.202719], + [0.067986, -0.200052, 0.977424], + ], + [ + [-0.997925, -0.0439691, 0.0470461], + [0.0643829, -0.695474, 0.715663], + [0.00125305, 0.717207, 0.696861], + ], + ), + ( + [ + [0.972749, -0.0233882, -0.230677], + [0.0255773, 0.999652, 0.00650349], + [0.230445, -0.0122264, 0.973009], + ], + [ + [-0.973286, 0.0147558, 0.229126], + [0.0145644, 0.999891, -0.00252631], + [-0.229138, 0.000878362, -0.973394], + ], + ), + ( + [ + [0.999271, 0.00700481, 0.0375381], + [-0.0348202, 0.57069, 0.820427], + [-0.0156757, -0.821136, 0.570517], + ], + [ + [-0.999805, -0.0198049, 0.000539906], + [0.0179848, -0.89584, 0.444015], + [-0.00831113, 0.443938, 0.89602], + ], + ), + ( + [ + [0.975255, -0.0207895, 0.220104], + [0.0252764, 0.999526, -0.0175888], + [-0.219634, 0.022717, 0.975318], + ], + [ + [-0.975573, 0.0128154, -0.219304], + [0.0106554, 0.999882, 0.0110292], + [0.219419, 0.00842303, -0.975594], + ], + ), + ( + [ + [-0.433961, -0.658151, -0.615236], + [0.610442, -0.717039, 0.336476], + [-0.6626, -0.229548, 0.71293], + ], + [ + [-0.998516, -0.00675119, -0.054067], + [-0.00405539, 0.99875, -0.0498174], + [0.0543358, -0.0495237, -0.997296], + ], + ), + ( + [ + [0.942764, -0.0126807, -0.333221], + [-0.0017175, 0.999079, -0.042879], + [0.333458, 0.0409971, 0.941873], + ], + [ + [-0.942228, -0.0109444, 0.334798], + [0.0110573, 0.997905, 0.0637396], + [-0.334794, 0.0637589, -0.940133], + ], + ), + ( + [ + [0.962038, 0.0147987, 0.272515], + [-0.0185974, 0.999762, 0.0113615], + [-0.272283, -0.0159982, 0.962084], + ], + [ + [-0.959802, 0.0113708, -0.280451], + [0.00982126, 0.999928, 0.00692958], + [0.280509, 0.00389678, -0.959845], + ], + ), + ( + [ + [0.998414, 0.0139348, 0.0545528], + [-0.0226877, 0.986318, 0.163283], + [-0.0515311, -0.164262, 0.98507], + ], + [ + [-0.998641, -0.000695993, -0.0521343], + [0.0182534, 0.931965, -0.362087], + [0.0488394, -0.362547, -0.930686], + ], + ), + ( + [ + [0.999705, -0.0234518, -0.00633743], + [0.0235916, 0.999458, 0.0229643], + [0.00579544, -0.023107, 0.999716], + ], + [ + [-0.999901, 0.004436, 0.0133471], + [-0.00306106, 0.85758, -0.514342], + [-0.0137278, -0.514332, -0.857481], + ], + ), + ( + [ + [0.998617, -0.051704, 0.0094837], + [0.0484292, 0.975079, 0.216506], + [-0.0204416, -0.215748, 0.976235], + ], + [ + [-0.999959, -0.00295958, -0.00862907], + [-0.00313279, 0.999792, 0.0201361], + [0.00856768, 0.0201625, -0.999761], + ], + ), + ( + [ + [0.999121, 0.0345472, -0.023733], + [-0.0333175, 0.998174, 0.0503881], + [0.0254304, -0.0495531, 0.998448], + ], + [ + [-0.999272, -0.0337466, 0.0178065], + [0.0200629, -0.0677624, 0.9975], + [-0.0324556, 0.997131, 0.0683898], + ], + ), + ( + [ + [0.989017, 0.139841, -0.0478525], + [-0.131355, 0.683201, -0.718319], + [-0.0677572, 0.716715, 0.694067], + ], + [ + [-0.995236, 0.00457798, 0.097401], + [0.097488, 0.0258334, 0.994902], + [0.00203912, 0.999657, -0.0261574], + ], + ), + ( + [ + [0.961528, 0.249402, 0.11516], + [-0.204522, 0.9298, -0.306009], + [-0.183395, 0.270684, 0.945038], + ], + [ + [-0.999566, -0.0233216, 0.0180679], + [0.012372, 0.224583, 0.974377], + [-0.0267822, 0.974177, -0.224197], + ], + ), + ( + [ + [0.865581, 0.0252563, -0.500131], + [0.0302583, 0.994265, 0.102578], + [0.499853, -0.103923, 0.859853], + ], + [ + [-0.866693, 0.0042288, 0.498824], + [0.0129331, 0.999818, 0.0139949], + [-0.498674, 0.0185807, -0.866591], + ], + ), + ( + [ + [0.998999, -0.0213419, -0.0393009], + [-0.0007582, 0.870578, -0.492031], + [0.0447153, 0.491568, 0.86969], + ], + [ + [-0.999207, -0.0184688, 0.0353073], + [0.00153266, 0.867625, 0.497218], + [-0.0398164, 0.496876, -0.866908], + ], + ), + ( + [ + [0.96567, -0.00482973, 0.259728], + [0.00349956, 0.999978, 0.00558359], + [-0.259749, -0.00448297, 0.965666], + ], + [ + [-0.962691, -0.00113074, -0.270609], + [-5.93716e-05, 0.999992, -0.00396767], + [0.270612, -0.00380337, -0.962683], + ], + ), + ( + [ + [0.948799, 0.287027, -0.131894], + [-0.300257, 0.949181, -0.0943405], + [0.0981135, 0.129112, 0.986764], + ], + [ + [-0.993593, -0.0406684, 0.105449], + [-0.0506857, 0.994269, -0.0941326], + [-0.101017, -0.0988741, -0.98996], + ], + ), + ( + [ + [0.998935, 0.0451118, 0.0097202], + [-0.0418086, 0.973879, -0.223183], + [-0.0195345, 0.222539, 0.974728], + ], + [ + [-0.999821, 0.00821522, -0.0170658], + [0.00742187, 0.998912, 0.046048], + [0.0174255, 0.0459131, -0.998794], + ], + ), + ( + [ + [0.99577, 0.00458459, 0.0917705], + [-0.00244288, 0.999722, -0.0234365], + [-0.0918524, 0.0231131, 0.995504], + ], + [ + [-0.995956, 0.0137721, -0.0887945], + [0.0122932, 0.999777, 0.0171801], + [0.0890113, 0.0160191, -0.995903], + ], + ), + ( + [ + [0.97931, 0.0219899, 0.201169], + [-0.0159226, 0.99937, -0.0317288], + [-0.201739, 0.0278692, 0.979043], + ], + [ + [-0.980952, 0.00507266, -0.19419], + [0.00310821, 0.999941, 0.010419], + [0.194231, 0.00961706, -0.98091], + ], + ), + ( + [ + [0.999616, 0.00550326, -0.0271537], + [-0.0048286, 0.99968, 0.0248495], + [0.0272817, -0.0247088, 0.999322], + ], + [ + [-0.999689, -0.00054899, 0.0249588], + [-0.00125497, 0.999599, -0.0282774], + [-0.0249333, -0.0282998, -0.999289], + ], + ), + ( + [ + [0.998036, -0.00755259, -0.0621791], + [0.0417502, 0.820234, 0.570502], + [0.0466927, -0.571978, 0.818939], + ], + [ + [-0.999135, -0.0278203, 0.0309173], + [-0.00855238, 0.864892, 0.501886], + [-0.0407029, 0.501187, -0.864382], + ], + ), + ( + [ + [0.958227, 0.00271545, 0.285997], + [-0.00426128, 0.999979, 0.00478282], + [-0.285979, -0.00580174, 0.958218], + ], + [ + [-0.958726, 0.011053, -0.284121], + [0.0138068, 0.999875, -0.00769161], + [0.284001, -0.0112968, -0.958759], + ], + ), + ( + [ + [-0.804547, -0.48558, -0.341929], + [0.517913, -0.855425, -0.00382581], + [-0.290637, -0.180168, 0.939718], + ], + [ + [0.993776, -0.0469383, -0.101033], + [-0.110087, -0.274676, -0.955214], + [0.0170842, 0.96039, -0.278134], + ], + ), + ( + [ + [0.991875, -0.0022313, -0.127195], + [-0.00198041, 0.999454, -0.0329762], + [0.127199, 0.0329602, 0.991329], + ], + [ + [-0.992632, -0.0090772, 0.120844], + [-0.00870494, 0.999956, 0.00360636], + [-0.120871, 0.00252786, -0.992667], + ], + ), + ( + [ + [0.999305, -0.0252534, 0.0274367], + [0.026144, 0.999126, -0.0326002], + [-0.0265895, 0.0332948, 0.999092], + ], + [ + [-0.999314, -0.0038532, -0.0368519], + [-0.00441323, 0.999876, 0.0151263], + [0.036789, 0.0152787, -0.999207], + ], + ), + ( + [ + [0.999843, -0.00958823, 0.0148803], + [0.00982469, 0.999825, -0.0159002], + [-0.0147253, 0.0160439, 0.999763], + ], + [ + [-0.999973, 0.00673608, -0.00308692], + [-0.0067409, -0.999977, 0.00116827], + [-0.00307934, 0.00119013, 0.999995], + ], + ), + ( + [ + [0.981558, -0.00727741, 0.191028], + [-0.00866166, 0.996556, 0.0824708], + [-0.190971, -0.0826044, 0.978114], + ], + [ + [-0.980202, 0.0179519, -0.197188], + [0.00957606, 0.999014, 0.0433472], + [0.197772, 0.0406008, -0.979408], + ], + ), + ( + [ + [0.966044, 0.0143709, 0.257977], + [-0.0157938, 0.999869, 0.00344404], + [-0.257894, -0.00740153, 0.966145], + ], + [ + [-0.965532, 0.0100318, -0.260094], + [0.00950897, 0.999949, 0.00326797], + [0.260113, 0.000682242, -0.965579], + ], + ), + ( + [ + [0.999965, 0.00727991, -0.00412134], + [-0.00802642, 0.973769, -0.227397], + [0.00235781, 0.227422, 0.973794], + ], + [ + [-0.999877, 0.00698241, 0.0141441], + [0.0103867, 0.966295, 0.257228], + [-0.0118713, 0.257343, -0.966248], + ], + ), + ( + [ + [0.951385, -0.0297966, 0.306561], + [-0.0314555, 0.980706, 0.19294], + [-0.306395, -0.193204, 0.932092], + ], + [ + [-0.99981, 0.00389172, -0.0191159], + [-0.00386326, -0.999991, -0.00150593], + [-0.0191215, -0.00143146, 0.999816], + ], + ), + ( + [ + [0.986772, -0.120673, 0.10825], + [0.0543962, 0.875511, 0.480126], + [-0.152713, -0.467887, 0.870495], + ], + [ + [-0.991246, 0.125848, -0.0399414], + [-0.129021, -0.85897, 0.495507], + [0.0280503, 0.496321, 0.867686], + ], + ), + ( + [ + [-0.804799, -0.588418, 0.0778637], + [-0.514399, 0.756902, 0.403104], + [-0.296129, 0.284365, -0.911836], + ], + [ + [0.98676, -0.0939473, 0.132227], + [0.162179, 0.557277, -0.814336], + [0.0028177, 0.824995, 0.565135], + ], + ), + ( + [ + [0.878935, 0.115231, 0.462813], + [0.0845639, 0.917349, -0.388998], + [-0.469386, 0.381041, 0.796546], + ], + [ + [-0.869533, 0.00193279, -0.493873], + [-0.00419575, 0.999927, 0.0113007], + [0.493859, 0.0118986, -0.869462], + ], + ), + ( + [ + [0.951881, 0.20828, 0.224816], + [-0.305582, 0.700797, 0.644595], + [-0.023294, -0.682277, 0.730722], + ], + [ + [-0.999787, 0.0141074, -0.0151097], + [-0.000971554, 0.698061, 0.716038], + [0.0206489, 0.7159, -0.697898], + ], + ), + ( + [ + [0.999538, 0.0192173, 0.0235334], + [-0.0189064, 0.999732, -0.0133635], + [-0.0237839, 0.0129124, 0.999634], + ], + [ + [-0.999807, 0.00286378, -0.0194776], + [0.0026258, 0.999922, 0.0122308], + [0.0195111, 0.0121774, -0.999736], + ], + ), + ( + [ + [0.998468, 0.041362, -0.0367422], + [-0.0364453, 0.991404, 0.125658], + [0.0416238, -0.124127, 0.991393], + ], + [ + [-0.997665, -0.0658235, 0.0183602], + [0.0216855, -0.0501652, 0.998507], + [-0.064804, 0.99657, 0.0514739], + ], + ), + ( + [ + [0.995563, 0.0493669, 0.0801057], + [-0.0272233, 0.966027, -0.257002], + [-0.0900717, 0.253681, 0.963085], + ], + [ + [-0.999228, -0.034399, -0.0190572], + [0.0250208, -0.929986, 0.366743], + [-0.0303386, 0.365984, 0.930127], + ], + ), + ( + [ + [0.952898, 0.0122933, 0.303043], + [-0.00568444, 0.999727, -0.0226807], + [-0.303239, 0.0198898, 0.952707], + ], + [ + [-0.951155, 0.0127759, -0.308452], + [0.000612627, 0.999219, 0.0394978], + [0.308716, 0.0373795, -0.95042], + ], + ), + ( + [ + [0.923096, -0.000313887, 0.38457], + [0.00948258, 0.999714, -0.0219453], + [-0.384454, 0.0239044, 0.922835], + ], + [ + [-0.922662, -0.00403523, -0.385589], + [-0.0119834, 0.999762, 0.0182116], + [0.385424, 0.0214239, -0.922491], + ], + ), + ( + [ + [0.991575, 0.0945042, -0.0885834], + [-0.10112, 0.99216, -0.0734349], + [0.080949, 0.0817738, 0.993358], + ], + [ + [-0.990948, -0.127974, 0.0405639], + [0.096351, -0.467557, 0.878697], + [-0.0934839, 0.874651, 0.475655], + ], + ), + ( + [ + [0.997148, 0.010521, 0.0747407], + [-0.0079726, 0.999379, -0.034313], + [-0.0750553, 0.0336192, 0.996612], + ], + [ + [-0.996543, 0.00988805, -0.0825019], + [0.00939476, 0.999936, 0.0063645], + [0.0825595, 0.00556751, -0.996572], + ], + ), + ( + [ + [0.991261, 0.00474444, -0.131831], + [-0.00205841, 0.999788, 0.0205036], + [0.131901, -0.020053, 0.99106], + ], + [ + [-0.990924, 4.45275e-05, 0.134427], + [0.00614714, 0.998969, 0.0449827], + [-0.134286, 0.0454008, -0.989903], + ], + ), + ( + [ + [0.992266, -0.0947916, 0.0801474], + [0.100889, 0.992006, -0.0757987], + [-0.0723216, 0.0832984, 0.993897], + ], + [ + [-0.992701, 0.0817686, -0.0886652], + [-0.114283, -0.40263, 0.908203], + [0.0385633, 0.911704, 0.409035], + ], + ), + ( + [ + [0.99696, -0.00808565, -0.0774951], + [0.0585083, 0.734519, 0.676061], + [0.0514552, -0.67854, 0.732759], + ], + [ + [-0.9998, 0.0053398, -0.0193164], + [-0.0162677, -0.779206, 0.626556], + [-0.0117055, 0.626745, 0.779137], + ], + ), + ( + [ + [0.961501, 0.0133645, -0.274475], + [-0.016255, 0.999834, -0.00825889], + [0.274319, 0.0124025, 0.961559], + ], + [ + [-0.963687, 0.000179203, 0.267042], + [0.00670194, 0.999701, 0.023515], + [-0.266958, 0.0244509, -0.9634], + ], + ), + ( + [ + [0.99877, 0.0413462, -0.0273572], + [-0.0263673, 0.91029, 0.413131], + [0.0419844, -0.411902, 0.910261], + ], + [ + [-0.998035, -0.0613039, 0.0130407], + [-0.00146496, 0.230815, 0.972998], + [-0.0626594, 0.971065, -0.230452], + ], + ), + ( + [ + [0.999657, 0.0261608, 0.00141675], + [-0.0261957, 0.998937, 0.0379393], + [-0.000422719, -0.0379634, 0.999279], + ], + [ + [-0.998896, -0.0310033, -0.0353275], + [0.0315452, -0.999392, -0.0148857], + [-0.0348445, -0.0159846, 0.999265], + ], + ), + ( + [ + [0.77369, 0.0137861, 0.633415], + [-0.0186509, 0.999826, 0.00102049], + [-0.63329, -0.0126033, 0.773812], + ], + [ + [-0.773069, 0.0156632, -0.634129], + [0.00418312, 0.999799, 0.0195956], + [0.634308, 0.0124961, -0.772979], + ], + ), + ( + [ + [0.952827, -0.024521, -0.302522], + [-0.00541318, 0.9952, -0.0977158], + [0.303465, 0.0947439, 0.94812], + ], + [ + [-0.952266, -0.00806089, 0.305165], + [0.00351941, 0.999295, 0.037378], + [-0.305252, 0.0366678, -0.951567], + ], + ), + ( + [ + [-0.172189, 0.949971, 0.260587], + [-0.86961, -0.0223234, -0.493235], + [-0.462741, -0.311539, 0.829948], + ], + [ + [-0.672964, 0.0127645, -0.739567], + [0.00429523, 0.999902, 0.0133494], + [0.739664, 0.00580721, -0.672953], + ], + ), + ( + [ + [0.637899, -0.440017, 0.632036], + [-0.52883, 0.346333, 0.774849], + [-0.559842, -0.828516, -0.0117683], + ], + [ + [-0.0627307, -0.0314554, -0.997536], + [-0.733537, 0.679201, 0.0247117], + [0.67675, 0.733279, -0.0656804], + ], + ), + ( + [ + [0.998402, 0.00284932, -0.0564372], + [0.000393713, 0.998353, 0.0573683], + [0.0565077, -0.0572989, 0.996757], + ], + [ + [-0.997878, 0.000941416, 0.0651252], + [-2.16756e-05, 0.999891, -0.0147853], + [-0.065132, -0.0147552, -0.997768], + ], + ), + ( + [ + [0.9999, 0.0141438, -0.000431687], + [-0.0140882, 0.9979, 0.063225], + [0.00132502, -0.0632125, 0.997999], + ], + [ + [-0.999515, -0.0308197, -0.00482715], + [-0.00160551, -0.103741, 0.994605], + [-0.0311554, 0.994128, 0.10364], + ], + ), + ( + [ + [-0.201909, 0.0267804, 0.979038], + [-0.0159062, 0.999405, -0.0306179], + [-0.979275, -0.0217548, -0.201363], + ], + [ + [0.261235, 0.951613, -0.161839], + [0.0758567, 0.146901, 0.986239], + [0.962292, -0.269916, -0.03381], + ], + ), + ( + [ + [0.998335, -0.0191576, -0.0544038], + [0.0163271, 0.998513, -0.0520045], + [0.0553192, 0.0510297, 0.997164], + ], + [ + [-0.998811, -0.00846127, 0.0480344], + [-0.0051736, 0.997661, 0.0681593], + [-0.0484988, 0.0678295, -0.996519], + ], + ), + ( + [ + [0.999973, 0.00227282, -0.00699658], + [-0.00137504, 0.992062, 0.125744], + [0.00722684, -0.125731, 0.992038], + ], + [ + [-0.999995, -0.00337061, 4.25756e-05], + [-0.00333677, 0.991528, 0.129853], + [-0.00047993, 0.129852, -0.991534], + ], + ), + ( + [ + [0.998908, 0.0216581, -0.041392], + [-0.0327304, 0.956678, -0.289302], + [0.0333331, 0.290341, 0.956342], + ], + [ + [-0.998254, -0.0377592, 0.0454422], + [0.00744647, 0.682591, 0.730764], + [-0.0586112, 0.729825, -0.681118], + ], + ), + ( + [ + [0.999387, -0.0042571, -0.0347599], + [0.00485203, 0.999843, 0.017049], + [0.0346819, -0.0172072, 0.99925], + ], + [ + [-0.999976, 0.00260242, -0.00669664], + [-0.00250352, -0.999889, -0.0147361], + [-0.00673422, -0.0147175, 0.99987], + ], + ), + ( + [ + [0.906103, -0.398828, -0.141112], + [0.381512, 0.914475, -0.13485], + [0.182826, 0.0683519, 0.980766], + ], + [ + [-0.996568, -0.0321282, -0.0763021], + [-0.0823787, 0.476597, 0.875254], + [0.00824509, 0.878535, -0.477609], + ], + ), + ( + [ + [0.908356, 0.316033, -0.273884], + [-0.231421, -0.165634, -0.95865], + [-0.34833, 0.934178, -0.0773183], + ], + [ + [-0.999889, -0.0146322, -0.00295739], + [-0.0149238, 0.974974, 0.221815], + [-0.000362257, 0.221835, -0.975085], + ], + ), + ( + [ + [0.999507, -0.00834631, 0.0302637], + [0.00899248, 0.999733, -0.0212785], + [-0.030078, 0.0215401, 0.999315], + ], + [ + [-0.999538, 0.00785187, -0.0293621], + [0.00739788, 0.999852, 0.0155394], + [0.0294797, 0.0153149, -0.999448], + ], + ), + ( + [ + [0.999951, -0.00729441, -0.00672921], + [0.00313753, 0.87564, -0.482954], + [0.00941523, 0.48291, 0.87562], + ], + [ + [-0.999984, -0.005202, -0.00277372], + [0.00340465, -0.893745, 0.448565], + [-0.00481353, 0.448548, 0.893747], + ], + ), + ( + [ + [0.998028, -0.0569885, 0.0263322], + [0.0489091, 0.968801, 0.242967], + [-0.039357, -0.2412, 0.969677], + ], + [ + [-0.997066, 0.0422415, -0.0638525], + [-0.0760293, -0.448184, 0.890703], + [0.00900662, 0.892944, 0.45008], + ], + ), + ( + [ + [0.999745, 0.00860777, 0.0208747], + [-0.00827114, 0.999835, -0.0161595], + [-0.0210103, 0.0159827, 0.999651], + ], + [ + [-0.999576, 0.0148733, -0.0251161], + [0.0151027, 0.999846, -0.00898035], + [0.0249787, -0.00935575, -0.999646], + ], + ), + ( + [ + [0.91924, 0.0372116, -0.391934], + [-0.00675798, 0.996868, 0.0787959], + [0.393639, -0.0697837, 0.916613], + ], + [ + [-0.921919, 0.00882585, 0.387286], + [0.00588498, 0.999944, -0.00877866], + [-0.387342, -0.00581387, -0.921919], + ], + ), + ( + [ + [0.998324, -0.0029024, 0.0577924], + [0.00236766, 0.999954, 0.00931901], + [-0.0578167, -0.00916657, 0.998285], + ], + [ + [-0.99892, -0.0025688, -0.0464413], + [-0.00203721, 0.999932, -0.0114927], + [0.0464676, -0.0113855, -0.998857], + ], + ), + ( + [ + [0.993986, 0.0163462, -0.108279], + [-0.0612924, 0.902447, -0.426418], + [0.090746, 0.43049, 0.898022], + ], + [ + [-0.994519, -0.0767804, 0.0709843], + [0.0579273, 0.160607, 0.985318], + [-0.0870543, 0.984028, -0.15528], + ], + ), + ( + [ + [0.997351, 0.0715122, -0.0132892], + [-0.0707087, 0.996067, 0.0533919], + [0.0170551, -0.0523108, 0.998485], + ], + [ + [-0.997704, -0.066002, 0.015281], + [0.064101, -0.846657, 0.528267], + [-0.0219278, 0.528033, 0.848942], + ], + ), + ( + [ + [0.999839, 0.00714662, -0.0164633], + [-0.00859425, 0.99594, -0.0896085], + [0.0157561, 0.0897356, 0.995841], + ], + [ + [-0.999773, 0.0079918, 0.0197854], + [0.00864136, 0.999419, 0.0329623], + [-0.0195105, 0.0331255, -0.999262], + ], + ), + ( + [ + [-0.773738, 0.630074, 0.0658454], + [-0.622848, -0.737618, -0.260731], + [-0.115711, -0.242749, 0.963163], + ], + [ + [-0.740005, 0.000855199, -0.672604], + [-0.0106008, 0.99986, 0.0129348], + [0.672521, 0.0167018, -0.739892], + ], + ), + ( + [ + [0.969039, -0.00110643, -0.246907], + [-0.121454, 0.868509, -0.480564], + [0.214973, 0.495673, 0.841484], + ], + [ + [-0.981168, -0.150714, 0.120811], + [0.172426, -0.401504, 0.89948], + [-0.0870583, 0.903372, 0.419929], + ], + ), + ( + [ + [0.589015, 0.80692, 0.0440651], + [-0.806467, 0.583447, 0.0959135], + [0.0516848, -0.0920316, 0.994414], + ], + [ + [-0.99998, 0.00434293, -0.00486489], + [0.00437139, 0.999973, -0.00588975], + [0.00483918, -0.00591087, -0.999972], + ], + ), + ( + [ + [0.999972, 0.000781564, 0.00750023], + [-0.0031568, 0.946655, 0.322235], + [-0.00684828, -0.322249, 0.94663], + ], + [ + [-0.999817, -0.0178453, -0.00691725], + [-0.0189272, 0.975556, 0.218934], + [0.00284118, 0.219025, -0.975716], + ], + ), + ( + [ + [-0.969668, 0.219101, -0.108345], + [0.172364, 0.298654, -0.938667], + [-0.173305, -0.928871, -0.32736], + ], + [ + [-0.999917, 0.0111423, -0.00656864], + [-0.00977865, -0.318874, 0.947748], + [0.00846644, 0.947733, 0.318955], + ], + ), + ( + [ + [-0.808574, -0.185515, -0.558383], + [0.174641, -0.981898, 0.0733309], + [-0.561879, -0.038223, 0.826336], + ], + [ + [-0.873416, 0.0121808, -0.486824], + [-0.00495714, 0.999413, 0.0338998], + [0.486951, 0.032022, -0.872843], + ], + ), + ( + [ + [0.999295, 0.0295658, -0.0231234], + [-0.0251771, 0.984904, 0.17126], + [0.0278378, -0.170557, 0.984954], + ], + [ + [-0.998834, -0.040128, 0.026921], + [0.0327412, -0.152276, 0.987798], + [-0.0355388, 0.987524, 0.153411], + ], + ), + ( + [ + [0.996021, -0.0050677, -0.0889802], + [0.0042919, 0.999951, -0.00890794], + [0.089021, 0.0084906, 0.995994], + ], + [ + [-0.995726, -0.00858132, 0.0919686], + [-0.00615004, 0.999625, 0.0266854], + [-0.0921631, 0.0260058, -0.995405], + ], + ), + ( + [ + [0.563325, 0.812296, 0.151129], + [-0.316559, 0.381143, -0.868632], + [-0.763188, 0.441481, 0.471847], + ], + [ + [-0.980048, -0.0115108, -0.198437], + [-0.168991, 0.573853, 0.801335], + [0.104649, 0.818877, -0.564348], + ], + ), + ( + [ + [0.984844, -0.0288271, 0.17103], + [0.0260588, 0.999491, 0.0184094], + [-0.171474, -0.0136736, 0.985094], + ], + [ + [-0.984637, -0.00367691, -0.174577], + [-0.00649229, 0.999858, 0.0155587], + [0.174495, 0.0164532, -0.984521], + ], + ), + ( + [ + [0.99985, 0.000720773, -0.0172841], + [-0.00075051, 0.999998, -0.0017141], + [0.0172828, 0.00172682, 0.999849], + ], + [ + [-0.999926, -0.00413456, 0.0114842], + [-0.00368343, 0.999231, 0.0390359], + [-0.0116368, 0.0389908, -0.999172], + ], + ), + ( + [ + [0.997976, 0.0603523, -0.0200139], + [-0.0558618, 0.982551, 0.177404], + [0.0303714, -0.175927, 0.983935], + ], + [ + [-0.996867, -0.0790953, 0.00217996], + [0.0318842, -0.376338, 0.925935], + [-0.0724181, 0.923101, 0.37768], + ], + ), + ( + [ + [0.94678, -0.00538407, -0.321837], + [0.00249113, 0.999953, -0.0094], + [0.321872, 0.008098, 0.946749], + ], + [ + [-0.945694, 0.0255694, 0.324053], + [0.0240377, 0.999673, -0.00872898], + [-0.32417, -0.000465377, -0.945999], + ], + ), + ( + [ + [0.846059, 0.435245, -0.307807], + [0.318073, 0.0512036, 0.946682], + [0.4278, -0.898855, -0.0951187], + ], + [ + [-0.217213, -0.0389124, 0.975352], + [0.742195, 0.642416, 0.190918], + [-0.634011, 0.765368, -0.11066], + ], + ), + ( + [ + [0.914988, -0.0538229, -0.399875], + [-0.0459455, 0.970717, -0.23579], + [0.400857, 0.234117, 0.885722], + ], + [ + [-0.919706, 0.00194642, 0.392606], + [0.105539, 0.964406, 0.242451], + [-0.378159, 0.264418, -0.887176], + ], + ), + ( + [ + [0.970915, -0.183858, 0.153365], + [0.209801, 0.96196, -0.174974], + [-0.115361, 0.202061, 0.972555], + ], + [ + [-0.975509, 0.21077, -0.0629391], + [-0.218082, -0.964089, 0.151576], + [-0.0287314, 0.161588, 0.986441], + ], + ), + ( + [ + [0.99369, -0.00515149, -0.112044], + [0.00366664, 0.999903, -0.0134545], + [0.112102, 0.0129588, 0.993612], + ], + [ + [-0.99406, 0.00631892, 0.108668], + [0.00878985, 0.999713, 0.022273], + [-0.108496, 0.0230956, -0.99383], + ], + ), + ( + [ + [0.995917, 0.0137529, 0.089215], + [-0.0145079, 0.999864, 0.00781912], + [-0.0890954, -0.00908151, 0.995982], + ], + [ + [-0.996188, 0.012059, -0.0864113], + [0.0126654, 0.999899, -0.00647346], + [0.0863245, -0.00754306, -0.99624], + ], + ), + ( + [ + [0.84563, -0.0032436, -0.533759], + [0.0040093, 0.999992, 0.000275049], + [0.533754, -0.00237259, 0.845636], + ], + [ + [-0.849818, -0.00755214, 0.527023], + [-0.00734806, 0.99997, 0.00248074], + [-0.527026, -0.00176415, -0.849848], + ], + ), + ( + [ + [0.736067, -0.212675, -0.642631], + [-0.447028, 0.560168, -0.697408], + [0.508303, 0.800613, 0.31725], + ], + [ + [-0.684029, 0.0061039, 0.729431], + [0.0260275, 0.999532, 0.0160434], + [-0.728992, 0.0299595, -0.683868], + ], + ), + ( + [ + [0.993949, 0.00461705, -0.109742], + [-0.00653155, 0.999833, -0.0170925], + [0.109644, 0.0177058, 0.993813], + ], + [ + [-0.994446, 0.0218439, 0.102965], + [0.0227578, 0.999711, 0.00770966], + [-0.102767, 0.0100102, -0.994656], + ], + ), + ( + [ + [0.996005, -0.0103388, 0.0886959], + [-0.0291635, 0.901147, 0.432531], + [-0.0843999, -0.43339, 0.897246], + ], + [ + [-0.999947, 0.00833193, -0.00598923], + [-0.0101526, -0.887864, 0.459993], + [-0.00148526, 0.46003, 0.887902], + ], + ), + ( + [ + [0.981518, 0.0114609, 0.191025], + [-0.0104683, 0.999926, -0.00620422], + [-0.191082, 0.00408984, 0.981565], + ], + [ + [-0.979556, 0.000134379, -0.201176], + [-0.00817302, 0.999148, 0.0404628], + [0.20101, 0.0412799, -0.97872], + ], + ), + ( + [ + [0.997665, -0.0372296, -0.0572574], + [0.0379027, 0.999224, 0.0107148], + [0.0568141, -0.01286, 0.998302], + ], + [ + [-0.997794, 0.00389749, 0.0662921], + [0.00639122, 0.999278, 0.0374446], + [-0.0660983, 0.0377856, -0.997099], + ], + ), + ( + [ + [0.981618, -0.0105643, -0.190564], + [0.00329498, 0.999256, -0.0384229], + [0.190828, 0.0370887, 0.980923], + ], + [ + [-0.981673, -0.000810695, 0.190576], + [0.00398375, 0.999685, 0.0247729], + [-0.190536, 0.0250779, -0.981361], + ], + ), + ( + [ + [-0.544941, -0.812151, -0.208446], + [0.812337, -0.449791, -0.37121], + [0.207722, -0.371617, 0.90485], + ], + [ + [-0.121327, -0.000366672, -0.992614], + [-0.955208, 0.271977, 0.116655], + [0.269926, 0.962303, -0.0333484], + ], + ), + ( + [ + [0.637701, -0.219537, 0.738336], + [0.735715, 0.457522, -0.499397], + [-0.228168, 0.861671, 0.453279], + ], + [ + [-0.741797, 0.0196167, -0.670339], + [-0.00209087, 0.9995, 0.0315629], + [0.670623, 0.0248149, -0.741385], + ], + ), + ( + [ + [0.99813, -0.0590625, -0.0157485], + [0.0589086, 0.998213, -0.0100649], + [0.0163148, 0.00911833, 0.999825], + ], + [ + [-0.99893, 0.0258783, -0.0383385], + [-0.0440455, -0.279068, 0.959261], + [0.014125, 0.959924, 0.279908], + ], + ), + ( + [ + [0.999558, 0.0028395, -0.0296019], + [-0.00492321, 0.997496, -0.0705578], + [0.0293274, 0.0706723, 0.997068], + ], + [ + [-0.999532, -0.0305627, -0.00231546], + [0.00957406, -0.38309, 0.923664], + [-0.0291167, 0.923206, 0.383202], + ], + ), + ( + [ + [0.99814, -0.0528437, -0.0303853], + [0.0590889, 0.96123, 0.269341], + [0.0149743, -0.270636, 0.962565], + ], + [ + [-0.999464, 0.00781117, 0.0318024], + [-0.000588355, 0.966696, -0.255928], + [-0.0327423, -0.255809, -0.966173], + ], + ), + ( + [ + [-0.936685, 0.234194, 0.260336], + [-0.233325, -0.97178, 0.034698], + [0.261116, -0.0282419, 0.964894], + ], + [ + [0.999511, 0.00582072, 0.0307461], + [0.0289012, 0.204922, -0.978352], + [-0.0119956, 0.978762, 0.204654], + ], + ), + ( + [ + [0.973616, -0.019218, -0.227384], + [0.0030011, 0.99744, -0.0714512], + [0.228175, 0.0688836, 0.97118], + ], + [ + [-0.974738, 0.0190271, 0.222547], + [0.0222378, 0.999682, 0.0119297], + [-0.222249, 0.0165771, -0.97485], + ], + ), + ( + [ + [0.997273, 0.0453471, -0.0582173], + [-0.0234007, 0.942529, 0.333303], + [0.0699858, -0.331032, 0.941021], + ], + [ + [-0.996269, -0.0613496, 0.0607196], + [-0.0100285, 0.780948, 0.624516], + [-0.0857328, 0.621576, -0.77865], + ], + ), + ( + [ + [0.999511, 0.0274482, -0.0149865], + [-0.0305945, 0.957511, -0.286769], + [0.00647846, 0.287087, 0.957883], + ], + [ + [-0.999443, -0.0260559, 0.0209038], + [0.0148505, 0.213942, 0.976734], + [-0.0299225, 0.976499, -0.213437], + ], + ), + ( + [ + [0.621123, 0.722893, 0.302708], + [-0.48353, 0.657448, -0.577894], + [-0.61677, 0.212574, 0.757896], + ], + [ + [-0.996888, -0.0217614, -0.0757776], + [-0.0783897, 0.376159, 0.923234], + [0.00841386, 0.926299, -0.376694], + ], + ), + ( + [ + [0.974426, 0.0128384, -0.224341], + [-0.0123842, 0.999917, 0.00343166], + [0.224367, -0.00056561, 0.974505], + ], + [ + [-0.973234, -0.00506667, 0.229763], + [-0.000498848, 0.999801, 0.0199346], + [-0.229818, 0.0192865, -0.973043], + ], + ), + ( + [ + [0.994721, -0.0881097, 0.0526082], + [0.0972904, 0.972774, -0.210345], + [-0.0326424, 0.214353, 0.976211], + ], + [ + [-0.994309, 0.0920529, -0.0536268], + [-0.105538, -0.782431, 0.613729], + [0.0145358, 0.615896, 0.787694], + ], + ), + ( + [ + [0.998677, -0.0372894, 0.0354002], + [0.0242326, 0.948589, 0.315583], + [-0.0453481, -0.314308, 0.948237], + ], + [ + [-0.999066, -0.00910724, -0.0422707], + [-0.024629, 0.923353, 0.383161], + [0.0355411, 0.383844, -0.922715], + ], + ), + ( + [ + [0.931525, 0.00831028, 0.363583], + [0.0192806, 0.997204, -0.0721909], + [-0.363167, 0.0742577, 0.92876], + ], + [ + [-0.930052, -0.00174384, -0.367425], + [-0.0268673, 0.997634, 0.0632737], + [0.366445, 0.0687194, -0.927899], + ], + ), + ( + [ + [-0.50483, -0.819216, 0.272087], + [0.775688, -0.568816, -0.273414], + [0.378753, 0.0730272, 0.922612], + ], + [ + [-0.981596, 0.00031926, 0.190974], + [0.00652401, 0.999471, 0.0318616], + [-0.190863, 0.0325211, -0.981079], + ], + ), + ( + [ + [0.990518, -0.00195099, -0.137368], + [-0.00164696, 0.999659, -0.0260735], + [0.137372, 0.0260526, 0.990177], + ], + [ + [-0.991078, 0.00934835, 0.132961], + [0.0106057, 0.999905, 0.00875176], + [-0.132866, 0.0100839, -0.991083], + ], + ), + ( + [ + [0.935049, -0.353081, 0.0318997], + [0.257018, 0.737114, 0.624984], + [-0.244184, -0.576192, 0.779985], + ], + [ + [-0.977342, -0.00167896, -0.211667], + [-0.0448634, 0.978894, 0.199386], + [0.206864, 0.204364, -0.956789], + ], + ), + ( + [ + [0.998464, 0.0501172, 0.0236119], + [-0.0498618, 0.998692, -0.0112844], + [-0.0241466, 0.0100898, 0.999658], + ], + [ + [-0.999931, -0.0037971, -0.0112195], + [-0.00640916, 0.970027, 0.242913], + [0.00996085, 0.242968, -0.969984], + ], + ), + ( + [ + [0.999893, -0.0108217, 0.00984537], + [0.011201, 0.999164, -0.0393194], + [-0.00941164, 0.0394255, 0.999178], + ], + [ + [-0.999886, 0.00730461, -0.0133396], + [-0.0118202, -0.925163, 0.379391], + [-0.00956982, 0.379504, 0.925142], + ], + ), + ( + [ + [0.990922, -0.086745, 0.102709], + [0.0847349, 0.99612, 0.0237834], + [-0.104373, -0.0148644, 0.994427], + ], + [ + [-0.994922, -0.00197458, -0.10064], + [-0.00242513, 0.999988, 0.00435525], + [0.10063, 0.00457739, -0.994914], + ], + ), + ( + [ + [0.999856, 0.00210734, -0.0168511], + [-0.00557165, 0.978053, -0.20828], + [0.0160424, 0.208344, 0.977924], + ], + [ + [-0.999698, 0.0048691, 0.0241226], + [-0.00154306, 0.965899, -0.258915], + [-0.0245606, -0.258874, -0.9656], + ], + ), + ( + [ + [0.992858, -0.0249864, -0.116659], + [0.0419872, 0.988447, 0.145634], + [0.111673, -0.149492, 0.982436], + ], + [ + [-0.992324, 0.0357741, 0.118384], + [-0.0419528, 0.803113, -0.594348], + [-0.116338, -0.594752, -0.795447], + ], + ), + ( + [ + [0.986821, -0.00531913, 0.161729], + [0.00797365, 0.999844, -0.0157688], + [-0.16162, 0.0168505, 0.986709], + ], + [ + [-0.985867, 0.0119402, -0.167109], + [0.0141227, 0.99983, -0.0118784], + [0.166939, -0.0140704, -0.985868], + ], + ), + ( + [ + [0.999693, -0.0158939, -0.0190113], + [0.0103599, 0.96501, -0.262007], + [0.0225104, 0.261729, 0.964879], + ], + [ + [-0.999344, -0.0314781, -0.0180051], + [-0.0250895, 0.241673, 0.970034], + [-0.0261833, 0.969847, -0.242305], + ], + ), + ( + [ + [0.977445, 0.0293661, 0.209138], + [-0.0723687, 0.976903, 0.201057], + [-0.198403, -0.211657, 0.956994], + ], + [ + [-0.976437, 0.00895131, -0.215624], + [0.0552894, 0.976169, -0.20985], + [0.208607, -0.216827, -0.953663], + ], + ), + ( + [ + [0.994593, 0.0974797, -0.0358119], + [-0.0822288, 0.949838, 0.301737], + [0.0634288, -0.297161, 0.952718], + ], + [ + [-0.994192, -0.10746, -0.00604622], + [0.078812, -0.7651, 0.639071], + [-0.0733003, 0.634882, 0.769124], + ], + ), + ( + [ + [0.365674, 0.282077, -0.88697], + [-0.609177, 0.793033, 0.00105565], + [0.703694, 0.539936, 0.461826], + ], + [ + [-0.469534, 0.0109062, 0.882848], + [0.0060785, 0.99994, -0.00911984], + [-0.882894, 0.00108445, -0.469572], + ], + ), + ( + [ + [0.999956, 0.00903085, 0.0025358], + [-0.00862738, 0.991574, -0.129252], + [-0.00368169, 0.129224, 0.991609], + ], + [ + [-0.999976, 0.00322491, -0.00637541], + [0.00379751, 0.995755, -0.0919687], + [0.00605176, -0.0919907, -0.995743], + ], + ), + ( + [ + [0.999982, -0.00398882, -0.00441072], + [0.00411881, 0.999545, 0.0298655], + [0.00428959, -0.0298832, 0.999544], + ], + [ + [-0.999931, -0.00315547, -0.0114491], + [0.00300966, -0.999914, 0.0128304], + [-0.0114875, 0.012796, 0.999853], + ], + ), + ( + [ + [0.996613, 0.0781452, -0.0256245], + [-0.0610516, 0.91178, 0.406116], + [0.0550999, -0.403175, 0.913462], + ], + [ + [-0.996368, -0.084671, 0.00909851], + [0.0540149, -0.545774, 0.83619], + [-0.0658352, 0.833644, 0.548365], + ], + ), + ( + [ + [0.961059, 0.139318, 0.238654], + [-0.117488, 0.987672, -0.103448], + [-0.250124, 0.0713812, 0.965579], + ], + [ + [-0.973397, 0.00782581, -0.228998], + [-0.0621109, 0.952986, 0.296581], + [0.220553, 0.302913, -0.927147], + ], + ), + ( + [ + [0.156415, -0.982138, 0.104589], + [-0.568896, -0.176149, -0.803323], + [0.807398, 0.0661518, -0.586287], + ], + [ + [-0.992155, 0.0934304, -0.0830664], + [-0.121171, -0.555137, 0.822887], + [0.0307694, 0.826496, 0.562102], + ], + ), + ( + [ + [0.997973, 0.0130328, -0.0622976], + [-0.011111, 0.999455, 0.0310968], + [0.0626689, -0.0303416, 0.997573], + ], + [ + [-0.997391, -0.00094697, 0.0722014], + [-0.00271076, 0.9997, -0.024334], + [-0.0721567, -0.024466, -0.997094], + ], + ), + ( + [ + [0.913504, -0.0125928, -0.406634], + [-0.108363, 0.95588, -0.27304], + [0.392132, 0.293487, 0.871836], + ], + [ + [-0.909813, 0.0115348, 0.414861], + [0.128636, 0.958223, 0.255464], + [-0.394582, 0.28579, -0.873287], + ], + ), + ( + [ + [0.932595, -0.0693644, 0.354197], + [0.0984415, 0.993036, -0.0647231], + [-0.347241, 0.0952281, 0.932928], + ], + [ + [-0.930498, 0.00578599, -0.366252], + [-0.106202, 0.952666, 0.284867], + [0.350564, 0.303964, -0.885839], + ], + ), + ( + [ + [0.995668, -0.00475737, 0.0928567], + [0.00890154, 0.99898, -0.0442667], + [-0.0925514, 0.0449015, 0.994695], + ], + [ + [-0.996077, -0.0107986, -0.0878355], + [0.00749423, 0.978669, -0.205305], + [0.0881789, -0.205158, -0.974749], + ], + ), + ( + [ + [0.99948, 0.0321999, 0.00146151], + [-0.0321302, 0.998886, -0.0345513], + [-0.00257243, 0.0344864, 0.999402], + ], + [ + [-0.999953, 0.00726142, -0.0065326], + [0.00488529, 0.950962, 0.30927], + [0.00845801, 0.309223, -0.950953], + ], + ), +] + + class TestRot3(GtsamTestCase): """Test selected Rot3 methods.""" @@ -30,12 +2014,24 @@ class TestRot3(GtsamTestCase): [ -0.0018374, 0.0209105, -0.999781] ]) # fmt: on - + # get back angle in radians _, actual_angle = Rot3(R).axisAngle() expected_angle = 3.1396582 np.testing.assert_almost_equal(actual_angle, expected_angle, 1e-7) + def test_axis_angle_stress_test(self) -> None: + """Test that .axisAngle() yields angles less than 180 degrees for specific inputs.""" + for (R1, R2) in R1_R2_pairs: + R1 = Rot3(np.array(R1)) + R2 = Rot3(np.array(R2)) + + i1Ri2 = R1.between(R2) + + axis, angle = i1Ri2.axisAngle() + angle_deg = np.rad2deg(angle) + assert angle_deg < 180 + if __name__ == "__main__": unittest.main() From 9ec3f791f55b9b49d285ddf32615e96e0650c6dd Mon Sep 17 00:00:00 2001 From: John Lambert Date: Wed, 13 Oct 2021 22:03:40 -0400 Subject: [PATCH 235/248] loosen tolerance on trace(R) == -1 --- gtsam/geometry/SO3.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index 890bcd206..5f1bf7460 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -261,7 +261,7 @@ Vector3 SO3::Logmap(const SO3& Q, ChartJacobian H) { // when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc. // we do something special - if (tr + 1.0 < 1e-4) { + if (tr + 1.0 < 1e-3) { if (R33 > R22 && R33 > R11) { // R33 is the largest diagonal const double sgn_w = (R21 - R12) < 0 ? -1.0 : 1.0; From 462fdb4ff0bf0d806a12ae157fc6d8cce793a89d Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Wed, 13 Oct 2021 22:05:23 -0400 Subject: [PATCH 236/248] Fix again with better approximation --- gtsam/geometry/SO3.cpp | 53 +++++++++++++++++++++---------- gtsam/geometry/tests/testRot3.cpp | 2 +- 2 files changed, 38 insertions(+), 17 deletions(-) diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index 890bcd206..2585c37be 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -261,25 +261,46 @@ Vector3 SO3::Logmap(const SO3& Q, ChartJacobian H) { // when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc. // we do something special - if (tr + 1.0 < 1e-4) { + if (tr + 1.0 < 1e-3) { if (R33 > R22 && R33 > R11) { - // R33 is the largest diagonal - const double sgn_w = (R21 - R12) < 0 ? -1.0 : 1.0; - const double r = sqrt(2.0 + 2.0 * R33); - const double scale = M_PI_2 / r - 0.5 / (r * r) * (R21 - R12); - omega = sgn_w * scale * Vector3(R31 + R13, R32 + R23, 2.0 + 2.0 * R33); + // R33 is the largest diagonal, a=3, b=1, c=2 + const double W = R21 - R12; + const double Q1 = 2.0 + 2.0 * R33; + const double Q2 = R31 + R13; + const double Q3 = R23 + R32; + const double r = sqrt(Q1); + const double one_over_r = 1 / r; + const double norm = sqrt(Q1*Q1 + Q2*Q2 + Q3*Q3 + W*W); + const double sgn_w = W < 0 ? -1.0 : 1.0; + const double mag = M_PI - (2 * sgn_w * W) / norm; + const double scale = 0.5 * one_over_r * mag; + omega = sgn_w * scale * Vector3(Q2, Q3, Q1); } else if (R22 > R11) { - // R22 is the largest diagonal - const double sgn_w = (R13 - R31) < 0 ? -1.0 : 1.0; - const double r = sqrt(2.0 + 2.0 * R22); - const double scale = M_PI_2 / r - 0.5 / (r * r) * (R13 - R31); - omega = sgn_w * scale * Vector3(R21 + R12, 2.0 + 2.0 * R22, R23 + R32); + // R22 is the largest diagonal, a=2, b=3, c=1 + const double W = R13 - R31; + const double Q1 = 2.0 + 2.0 * R22; + const double Q2 = R23 + R32; + const double Q3 = R12 + R21; + const double r = sqrt(Q1); + const double one_over_r = 1 / r; + const double norm = sqrt(Q1*Q1 + Q2*Q2 + Q3*Q3 + W*W); + const double sgn_w = W < 0 ? -1.0 : 1.0; + const double mag = M_PI - (2 * sgn_w * W) / norm; + const double scale = 0.5 * one_over_r * mag; + omega = sgn_w * scale * Vector3(Q3, Q1, Q2); } else { - // R11 is the largest diagonal - const double sgn_w = (R32 - R23) < 0 ? -1.0 : 1.0; - const double r = sqrt(2.0 + 2.0 * R11); - const double scale = M_PI_2 / r - 0.5 / (r * r) * (R32 - R23); - omega = sgn_w * scale * Vector3(2.0 + 2.0 * R11, R12 + R21, R13 + R31); + // R11 is the largest diagonal, a=1, b=2, c=3 + const double W = R32 - R23; + const double Q1 = 2.0 + 2.0 * R11; + const double Q2 = R12 + R21; + const double Q3 = R31 + R13; + const double r = sqrt(Q1); + const double one_over_r = 1 / r; + const double norm = sqrt(Q1*Q1 + Q2*Q2 + Q3*Q3 + W*W); + const double sgn_w = W < 0 ? -1.0 : 1.0; + const double mag = M_PI - (2 * sgn_w * W) / norm; + const double scale = 0.5 * one_over_r * mag; + omega = sgn_w * scale * Vector3(Q1, Q2, Q3); } } else { double magnitude; diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index bb6f1a77a..dc4b888b3 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -268,7 +268,7 @@ TEST( Rot3, log) { (Vector)Rot3::Logmap(Rlund), 1e-8)); #else // SO3 will be approximate because of the non-orthogonality - EXPECT(assert_equal(Vector3(0.264485272, -0.742291088, -3.04136444), + EXPECT(assert_equal(Vector3(0.264452, -0.742197708, -3.04098184), (Vector)Rot3::Logmap(Rlund), 1e-8)); #endif } From 108c77b57a48c322ad2b70201d4e1d2d652b923c Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 18 Oct 2021 08:52:00 -0400 Subject: [PATCH 237/248] use variables to store targets --- python/CMakeLists.txt | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index c3524adad..eabf5872a 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -65,8 +65,10 @@ set(interface_headers ${PROJECT_SOURCE_DIR}/gtsam/basis/basis.i ) +set(GTSAM_TARGET gtsam_py) +set(GTSAM_UNSTABLE_TARGET gtsam_unstable_py) -pybind_wrap(gtsam_py # target +pybind_wrap(${GTSAM_TARGET} # target "${interface_headers}" # interface_headers "gtsam.cpp" # generated_cpp "gtsam" # module_name @@ -78,7 +80,7 @@ pybind_wrap(gtsam_py # target ON # use_boost ) -set_target_properties(gtsam_py PROPERTIES +set_target_properties(${GTSAM_TARGET} PROPERTIES INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib" INSTALL_RPATH_USE_LINK_PATH TRUE OUTPUT_NAME "gtsam" @@ -98,7 +100,7 @@ create_symlinks("${CMAKE_CURRENT_SOURCE_DIR}/gtsam" file(COPY "${GTSAM_SOURCE_DIR}/examples/Data" DESTINATION "${GTSAM_MODULE_PATH}") # Add gtsam as a dependency to the install target -set(GTSAM_PYTHON_DEPENDENCIES gtsam_py) +set(GTSAM_PYTHON_DEPENDENCIES ${GTSAM_TARGET}) if(GTSAM_UNSTABLE_BUILD_PYTHON) @@ -122,7 +124,7 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON) gtsam::CameraSetCal3Fisheye gtsam::KeyPairDoubleMap) - pybind_wrap(gtsam_unstable_py # target + pybind_wrap(${GTSAM_UNSTABLE_TARGET} # target ${PROJECT_SOURCE_DIR}/gtsam_unstable/gtsam_unstable.i # interface_header "gtsam_unstable.cpp" # generated_cpp "gtsam_unstable" # module_name @@ -134,7 +136,7 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON) ON # use_boost ) - set_target_properties(gtsam_unstable_py PROPERTIES + set_target_properties(${GTSAM_UNSTABLE_TARGET} PROPERTIES INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib" INSTALL_RPATH_USE_LINK_PATH TRUE OUTPUT_NAME "gtsam_unstable" @@ -150,7 +152,7 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON) "${GTSAM_UNSTABLE_MODULE_PATH}") # Add gtsam_unstable to the install target - list(APPEND GTSAM_PYTHON_DEPENDENCIES gtsam_unstable_py) + list(APPEND GTSAM_PYTHON_DEPENDENCIES ${GTSAM_UNSTABLE_TARGET}) endif() From 7793a2ddc1476ebf4f81b6f9c84f79bca2107e64 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 18 Oct 2021 09:16:49 -0400 Subject: [PATCH 238/248] clean up the __init__ file --- python/gtsam/__init__.py | 16 +++++++++++----- 1 file changed, 11 insertions(+), 5 deletions(-) diff --git a/python/gtsam/__init__.py b/python/gtsam/__init__.py index 70a00c3dc..d00e47b65 100644 --- a/python/gtsam/__init__.py +++ b/python/gtsam/__init__.py @@ -1,6 +1,12 @@ -from . import utils -from .gtsam import * -from .utils import findExampleDataFile +"""Module definition file for GTSAM""" + +# pylint: disable=import-outside-toplevel, global-variable-not-assigned, possibly-unused-variable, import-error, import-self + +import sys + +from gtsam import gtsam, utils +from gtsam.gtsam import * +from gtsam.utils import findExampleDataFile def _init(): @@ -13,7 +19,7 @@ def _init(): def Point2(x=np.nan, y=np.nan): """Shim for the deleted Point2 type.""" if isinstance(x, np.ndarray): - assert x.shape == (2,), "Point2 takes 2-vector" + assert x.shape == (2, ), "Point2 takes 2-vector" return x # "copy constructor" return np.array([x, y], dtype=float) @@ -22,7 +28,7 @@ def _init(): def Point3(x=np.nan, y=np.nan, z=np.nan): """Shim for the deleted Point3 type.""" if isinstance(x, np.ndarray): - assert x.shape == (3,), "Point3 takes 3-vector" + assert x.shape == (3, ), "Point3 takes 3-vector" return x # "copy constructor" return np.array([x, y, z], dtype=float) From 21c19453346598c679992673b70b72e777242bfa Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 18 Oct 2021 12:23:38 -0400 Subject: [PATCH 239/248] address review comments --- python/CMakeLists.txt | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index eabf5872a..2b3ed3852 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -65,10 +65,10 @@ set(interface_headers ${PROJECT_SOURCE_DIR}/gtsam/basis/basis.i ) -set(GTSAM_TARGET gtsam_py) -set(GTSAM_UNSTABLE_TARGET gtsam_unstable_py) +set(GTSAM_PYTHON_TARGET gtsam_py) +set(GTSAM_PYTHON_UNSTABLE_TARGET gtsam_unstable_py) -pybind_wrap(${GTSAM_TARGET} # target +pybind_wrap(${GTSAM_PYTHON_TARGET} # target "${interface_headers}" # interface_headers "gtsam.cpp" # generated_cpp "gtsam" # module_name @@ -80,7 +80,7 @@ pybind_wrap(${GTSAM_TARGET} # target ON # use_boost ) -set_target_properties(${GTSAM_TARGET} PROPERTIES +set_target_properties(${GTSAM_PYTHON_TARGET} PROPERTIES INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib" INSTALL_RPATH_USE_LINK_PATH TRUE OUTPUT_NAME "gtsam" @@ -100,7 +100,7 @@ create_symlinks("${CMAKE_CURRENT_SOURCE_DIR}/gtsam" file(COPY "${GTSAM_SOURCE_DIR}/examples/Data" DESTINATION "${GTSAM_MODULE_PATH}") # Add gtsam as a dependency to the install target -set(GTSAM_PYTHON_DEPENDENCIES ${GTSAM_TARGET}) +set(GTSAM_PYTHON_DEPENDENCIES ${GTSAM_PYTHON_TARGET}) if(GTSAM_UNSTABLE_BUILD_PYTHON) @@ -124,7 +124,7 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON) gtsam::CameraSetCal3Fisheye gtsam::KeyPairDoubleMap) - pybind_wrap(${GTSAM_UNSTABLE_TARGET} # target + pybind_wrap(${GTSAM_PYTHON_UNSTABLE_TARGET} # target ${PROJECT_SOURCE_DIR}/gtsam_unstable/gtsam_unstable.i # interface_header "gtsam_unstable.cpp" # generated_cpp "gtsam_unstable" # module_name @@ -136,7 +136,7 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON) ON # use_boost ) - set_target_properties(${GTSAM_UNSTABLE_TARGET} PROPERTIES + set_target_properties(${GTSAM_PYTHON_UNSTABLE_TARGET} PROPERTIES INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib" INSTALL_RPATH_USE_LINK_PATH TRUE OUTPUT_NAME "gtsam_unstable" @@ -152,7 +152,7 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON) "${GTSAM_UNSTABLE_MODULE_PATH}") # Add gtsam_unstable to the install target - list(APPEND GTSAM_PYTHON_DEPENDENCIES ${GTSAM_UNSTABLE_TARGET}) + list(APPEND GTSAM_PYTHON_DEPENDENCIES ${GTSAM_PYTHON_UNSTABLE_TARGET}) endif() From e0425f3548f7340184d47fb9a461215e428880e5 Mon Sep 17 00:00:00 2001 From: yetongumich Date: Wed, 20 Oct 2021 14:27:42 -0400 Subject: [PATCH 240/248] add logmap expression --- gtsam/slam/expressions.h | 17 +++++++++++++++++ gtsam/slam/tests/testSlamExpressions.cpp | 6 ++++++ 2 files changed, 23 insertions(+) diff --git a/gtsam/slam/expressions.h b/gtsam/slam/expressions.h index c6aa02774..3b8ea86d3 100644 --- a/gtsam/slam/expressions.h +++ b/gtsam/slam/expressions.h @@ -138,4 +138,21 @@ Point2_ uncalibrate(const Expression& K, const Point2_& xy_hat) { return Point2_(K, &CALIBRATION::uncalibrate, xy_hat); } + +/// logmap +// TODO(dellaert): Should work but fails because of a type deduction conflict. +// template +// gtsam::Expression::TangentVector> logmap( +// const gtsam::Expression &x1, const gtsam::Expression &x2) { +// return gtsam::Expression::TangentVector>( +// x1, &T::logmap, x2); +// } + +template +gtsam::Expression::TangentVector> logmap( + const gtsam::Expression &x1, const gtsam::Expression &x2) { + return Expression::TangentVector>( + gtsam::traits::Logmap, between(x1, x2)); +} + } // \namespace gtsam diff --git a/gtsam/slam/tests/testSlamExpressions.cpp b/gtsam/slam/tests/testSlamExpressions.cpp index 294b821d3..186c32ac3 100644 --- a/gtsam/slam/tests/testSlamExpressions.cpp +++ b/gtsam/slam/tests/testSlamExpressions.cpp @@ -58,6 +58,12 @@ TEST(SlamExpressions, unrotate) { const Point3_ q_ = unrotate(R_, p_); } +TEST(SlamExpressions, logmap) { + Pose3_ T1_(0); + Pose3_ T2_(1); + const Vector6_ l = logmap(T1_, T2_); +} + /* ************************************************************************* */ int main() { TestResult tr; From 60d7514e15b39eb6751456e11793a7eb12ee6955 Mon Sep 17 00:00:00 2001 From: yetongumich Date: Wed, 20 Oct 2021 14:27:56 -0400 Subject: [PATCH 241/248] add logmap expression --- gtsam/slam/tests/testSlamExpressions.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam/slam/tests/testSlamExpressions.cpp b/gtsam/slam/tests/testSlamExpressions.cpp index 186c32ac3..b5298989f 100644 --- a/gtsam/slam/tests/testSlamExpressions.cpp +++ b/gtsam/slam/tests/testSlamExpressions.cpp @@ -58,6 +58,7 @@ TEST(SlamExpressions, unrotate) { const Point3_ q_ = unrotate(R_, p_); } +/* ************************************************************************* */ TEST(SlamExpressions, logmap) { Pose3_ T1_(0); Pose3_ T2_(1); From 2a9ac7c166560957ecaaab2b8608eec37ad9dff0 Mon Sep 17 00:00:00 2001 From: Ivor Wanders Date: Thu, 21 Oct 2021 10:23:32 -0400 Subject: [PATCH 242/248] Fix dangling reference in static allocation. --- gtsam/slam/lago.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/slam/lago.cpp b/gtsam/slam/lago.cpp index 70caa424f..f8b092f86 100644 --- a/gtsam/slam/lago.cpp +++ b/gtsam/slam/lago.cpp @@ -36,7 +36,7 @@ static const Matrix I = I_1x1; static const Matrix I3 = I_3x3; static const noiseModel::Diagonal::shared_ptr priorOrientationNoise = - noiseModel::Diagonal::Sigmas((Vector(1) << 0).finished()); + noiseModel::Diagonal::Sigmas(Vector1(0)); static const noiseModel::Diagonal::shared_ptr priorPose2Noise = noiseModel::Diagonal::Variances(Vector3(1e-6, 1e-6, 1e-8)); From 6145466decfe06eaf311dd651ce244cfcc8f92a1 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 21 Oct 2021 15:02:24 -0400 Subject: [PATCH 243/248] add type annotations --- python/gtsam/examples/IMUKittiExampleGPS.py | 30 +++++++++++---------- 1 file changed, 16 insertions(+), 14 deletions(-) diff --git a/python/gtsam/examples/IMUKittiExampleGPS.py b/python/gtsam/examples/IMUKittiExampleGPS.py index a23f98186..5a9811522 100644 --- a/python/gtsam/examples/IMUKittiExampleGPS.py +++ b/python/gtsam/examples/IMUKittiExampleGPS.py @@ -4,11 +4,11 @@ Example of application of ISAM2 for GPS-aided navigation on the KITTI VISION BEN Author: Varun Agrawal """ import argparse - -import numpy as np +from typing import List import gtsam -from gtsam import Pose3, noiseModel +import numpy as np +from gtsam import ISAM2, Point3, Pose3, noiseModel from gtsam.symbol_shorthand import B, V, X @@ -31,7 +31,8 @@ class KittiCalibration: class ImuMeasurement: """An instance of an IMU measurement.""" - def __init__(self, time, dt, accelerometer, gyroscope): + def __init__(self, time: float, dt: float, accelerometer: gtsam.Point3, + gyroscope: gtsam.Point3): self.time = time self.dt = dt self.accelerometer = accelerometer @@ -40,14 +41,14 @@ class ImuMeasurement: class GpsMeasurement: """An instance of a GPS measurement.""" - def __init__(self, time, position: gtsam.Point3): + def __init__(self, time: float, position: gtsam.Point3): self.time = time self.position = position -def loadKittiData(imu_data_file="KittiEquivBiasedImu.txt", - gps_data_file="KittiGps_converted.txt", - imu_metadata_file="KittiEquivBiasedImu_metadata.txt"): +def loadKittiData(imu_data_file: str = "KittiEquivBiasedImu.txt", + gps_data_file: str = "KittiGps_converted.txt", + imu_metadata_file: str = "KittiEquivBiasedImu_metadata.txt"): """ Load the KITTI Dataset. """ @@ -56,7 +57,7 @@ def loadKittiData(imu_data_file="KittiEquivBiasedImu.txt", # GyroscopeSigma IntegrationSigma AccelerometerBiasSigma GyroscopeBiasSigma # AverageDeltaT imu_metadata_file = gtsam.findExampleDataFile(imu_metadata_file) - with open(imu_metadata_file) as imu_metadata: + with open(imu_metadata_file, encoding='UTF-8') as imu_metadata: print("-- Reading sensor metadata") line = imu_metadata.readline() # Ignore the first line line = imu_metadata.readline().strip() @@ -70,7 +71,7 @@ def loadKittiData(imu_data_file="KittiEquivBiasedImu.txt", imu_measurements = [] print("-- Reading IMU measurements from file") - with open(imu_data_file) as imu_data: + with open(imu_data_file, encoding='UTF-8') as imu_data: data = imu_data.readlines() for i in range(1, len(data)): # ignore the first line time, dt, acc_x, acc_y, acc_z, gyro_x, gyro_y, gyro_z = map( @@ -86,7 +87,7 @@ def loadKittiData(imu_data_file="KittiEquivBiasedImu.txt", gps_measurements = [] print("-- Reading GPS measurements from file") - with open(gps_data_file) as gps_data: + with open(gps_data_file, encoding='UTF-8') as gps_data: data = gps_data.readlines() for i in range(1, len(data)): time, x, y, z = map(float, data[i].split(',')) @@ -96,7 +97,7 @@ def loadKittiData(imu_data_file="KittiEquivBiasedImu.txt", return kitti_calibration, imu_measurements, gps_measurements -def getImuParams(kitti_calibration): +def getImuParams(kitti_calibration: KittiCalibration): """Get the IMU parameters from the KITTI calibration data.""" GRAVITY = 9.8 w_coriolis = np.zeros(3) @@ -122,11 +123,12 @@ def getImuParams(kitti_calibration): return imu_params -def save_results(isam, output_filename, first_gps_pose, gps_measurements): +def save_results(isam: gtsam.ISAM2, output_filename: str, first_gps_pose: int, + gps_measurements: List): """Write the results from `isam` to `output_filename`.""" # Save results to file print("Writing results to file...") - with open(output_filename, 'w') as fp_out: + with open(output_filename, 'w', encoding='UTF-8') as fp_out: fp_out.write( "#time(s),x(m),y(m),z(m),qx,qy,qz,qw,gt_x(m),gt_y(m),gt_z(m)\n") From 279c6450280e6512892849384a9794a708d01487 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 21 Oct 2021 15:08:21 -0400 Subject: [PATCH 244/248] fix type annotation --- python/gtsam/examples/IMUKittiExampleGPS.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/gtsam/examples/IMUKittiExampleGPS.py b/python/gtsam/examples/IMUKittiExampleGPS.py index 5a9811522..a9fb17ecb 100644 --- a/python/gtsam/examples/IMUKittiExampleGPS.py +++ b/python/gtsam/examples/IMUKittiExampleGPS.py @@ -124,7 +124,7 @@ def getImuParams(kitti_calibration: KittiCalibration): def save_results(isam: gtsam.ISAM2, output_filename: str, first_gps_pose: int, - gps_measurements: List): + gps_measurements: List[GpsMeasurement]): """Write the results from `isam` to `output_filename`.""" # Save results to file print("Writing results to file...") From 03ac36c8c3694ec0cfa414be04c58a72c4cdacc3 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 21 Oct 2021 15:12:31 -0400 Subject: [PATCH 245/248] use python f-strings --- python/gtsam/examples/IMUKittiExampleGPS.py | 20 +++++++------------- 1 file changed, 7 insertions(+), 13 deletions(-) diff --git a/python/gtsam/examples/IMUKittiExampleGPS.py b/python/gtsam/examples/IMUKittiExampleGPS.py index a9fb17ecb..68c93bf19 100644 --- a/python/gtsam/examples/IMUKittiExampleGPS.py +++ b/python/gtsam/examples/IMUKittiExampleGPS.py @@ -145,10 +145,10 @@ def save_results(isam: gtsam.ISAM2, output_filename: str, first_gps_pose: int, pose_quat = pose.rotation().toQuaternion() gps = gps_measurements[i].position - print("State at #{}".format(i)) - print("Pose:\n", pose) - print("Velocity:\n", velocity) - print("Bias:\n", bias) + print(f"State at #{i}") + print(f"Pose:\n{pose}") + print(f"Velocity:\n{velocity}") + print(f"Bias:\n{bias}") fp_out.write("{},{},{},{},{},{},{},{},{},{},{}\n".format( gps_measurements[i].time, pose.x(), pose.y(), pose.z(), @@ -290,9 +290,7 @@ def main(): noise_model_gps) new_values.insert(current_pose_key, gps_pose) - print( - "################ POSE INCLUDED AT TIME {} ################" - .format(t)) + print(f"############ POSE INCLUDED AT TIME {t} ############") print(gps_pose.translation(), "\n") else: new_values.insert(current_pose_key, current_pose_global) @@ -307,9 +305,7 @@ def main(): # We accumulate 2*GPSskip GPS measurements before updating the solver at # first so that the heading becomes observable. if i > (first_gps_pose + 2 * gps_skip): - print( - "################ NEW FACTORS AT TIME {:.6f} ################" - .format(t)) + print(f"############ NEW FACTORS AT TIME {t:.6f} ############") new_factors.print() isam.update(new_factors, new_values) @@ -325,9 +321,7 @@ def main(): current_velocity_global = result.atVector(current_vel_key) current_bias = result.atConstantBias(current_bias_key) - print( - "################ POSE AT TIME {} ################".format( - t)) + print(f"############ POSE AT TIME {t} ############") current_pose_global.print() print("\n") From 1e84fd9cc41f14aab04e7dfc6ee75917c3a2ad15 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 21 Oct 2021 16:33:32 -0400 Subject: [PATCH 246/248] refactor the example to make it cleaner --- python/gtsam/examples/IMUKittiExampleGPS.py | 126 ++++++++++++-------- 1 file changed, 79 insertions(+), 47 deletions(-) diff --git a/python/gtsam/examples/IMUKittiExampleGPS.py b/python/gtsam/examples/IMUKittiExampleGPS.py index 68c93bf19..071065260 100644 --- a/python/gtsam/examples/IMUKittiExampleGPS.py +++ b/python/gtsam/examples/IMUKittiExampleGPS.py @@ -4,13 +4,15 @@ Example of application of ISAM2 for GPS-aided navigation on the KITTI VISION BEN Author: Varun Agrawal """ import argparse -from typing import List +from typing import List, Tuple import gtsam import numpy as np -from gtsam import ISAM2, Point3, Pose3, noiseModel +from gtsam import ISAM2, Pose3, noiseModel from gtsam.symbol_shorthand import B, V, X +GRAVITY = 9.8 + class KittiCalibration: """Class to hold KITTI calibration info.""" @@ -46,25 +48,8 @@ class GpsMeasurement: self.position = position -def loadKittiData(imu_data_file: str = "KittiEquivBiasedImu.txt", - gps_data_file: str = "KittiGps_converted.txt", - imu_metadata_file: str = "KittiEquivBiasedImu_metadata.txt"): - """ - Load the KITTI Dataset. - """ - # Read IMU metadata and compute relative sensor pose transforms - # BodyPtx BodyPty BodyPtz BodyPrx BodyPry BodyPrz AccelerometerSigma - # GyroscopeSigma IntegrationSigma AccelerometerBiasSigma GyroscopeBiasSigma - # AverageDeltaT - imu_metadata_file = gtsam.findExampleDataFile(imu_metadata_file) - with open(imu_metadata_file, encoding='UTF-8') as imu_metadata: - print("-- Reading sensor metadata") - line = imu_metadata.readline() # Ignore the first line - line = imu_metadata.readline().strip() - data = list(map(float, line.split(' '))) - kitti_calibration = KittiCalibration(*data) - print("IMU metadata:", data) - +def loadImuData(imu_data_file: str) -> List[ImuMeasurement]: + """Helper to load the IMU data.""" # Read IMU data # Time dt accelX accelY accelZ omegaX omegaY omegaZ imu_data_file = gtsam.findExampleDataFile(imu_data_file) @@ -81,6 +66,11 @@ def loadKittiData(imu_data_file: str = "KittiEquivBiasedImu.txt", np.asarray([gyro_x, gyro_y, gyro_z])) imu_measurements.append(imu_measurement) + return imu_measurements + + +def loadGpsData(gps_data_file: str) -> List[GpsMeasurement]: + """Helper to load the GPS data.""" # Read GPS data # Time,X,Y,Z gps_data_file = gtsam.findExampleDataFile(gps_data_file) @@ -94,12 +84,38 @@ def loadKittiData(imu_data_file: str = "KittiEquivBiasedImu.txt", gps_measurement = GpsMeasurement(time, np.asarray([x, y, z])) gps_measurements.append(gps_measurement) + return gps_measurements + + +def loadKittiData( + imu_data_file: str = "KittiEquivBiasedImu.txt", + gps_data_file: str = "KittiGps_converted.txt", + imu_metadata_file: str = "KittiEquivBiasedImu_metadata.txt" +) -> Tuple[KittiCalibration, List[ImuMeasurement], List[GpsMeasurement]]: + """ + Load the KITTI Dataset. + """ + # Read IMU metadata and compute relative sensor pose transforms + # BodyPtx BodyPty BodyPtz BodyPrx BodyPry BodyPrz AccelerometerSigma + # GyroscopeSigma IntegrationSigma AccelerometerBiasSigma GyroscopeBiasSigma + # AverageDeltaT + imu_metadata_file = gtsam.findExampleDataFile(imu_metadata_file) + with open(imu_metadata_file, encoding='UTF-8') as imu_metadata: + print("-- Reading sensor metadata") + line = imu_metadata.readline() # Ignore the first line + line = imu_metadata.readline().strip() + data = list(map(float, line.split(' '))) + kitti_calibration = KittiCalibration(*data) + print("IMU metadata:", data) + + imu_measurements = loadImuData(imu_data_file) + gps_measurements = loadGpsData(gps_data_file) + return kitti_calibration, imu_measurements, gps_measurements def getImuParams(kitti_calibration: KittiCalibration): """Get the IMU parameters from the KITTI calibration data.""" - GRAVITY = 9.8 w_coriolis = np.zeros(3) # Set IMU preintegration parameters @@ -156,7 +172,7 @@ def save_results(isam: gtsam.ISAM2, output_filename: str, first_gps_pose: int, gps[0], gps[1], gps[2])) -def parse_args(): +def parse_args() -> argparse.Namespace: """Parse command line arguments.""" parser = argparse.ArgumentParser() parser.add_argument("--output_filename", @@ -164,24 +180,15 @@ def parse_args(): return parser.parse_args() -def main(): - """Main runner.""" - args = parse_args() - kitti_calibration, imu_measurements, gps_measurements = loadKittiData() - - if not kitti_calibration.bodyTimu.equals(Pose3(), 1e-8): - raise ValueError( - "Currently only support IMUinBody is identity, i.e. IMU and body frame are the same" - ) - - # Configure different variables - first_gps_pose = 1 - gps_skip = 10 - - # Configure noise models - noise_model_gps = noiseModel.Diagonal.Precisions( - np.asarray([0, 0, 0] + [1.0 / 0.07] * 3)) - +def optimize(gps_measurements: List[GpsMeasurement], + imu_measurements: List[ImuMeasurement], + sigma_init_x: gtsam.noiseModel.Diagonal, + sigma_init_v: gtsam.noiseModel.Diagonal, + sigma_init_b: gtsam.noiseModel.Diagonal, + noise_model_gps: gtsam.noiseModel.Diagonal, + kitti_calibration: KittiCalibration, first_gps_pose: int, + gps_skip: int) -> gtsam.ISAM2: + """Run ISAM2 optimization on the measurements.""" # Set initial conditions for the estimated trajectory # initial pose is the reference frame (navigation frame) current_pose_global = Pose3(gtsam.Rot3(), @@ -191,12 +198,6 @@ def main(): current_velocity_global = np.zeros(3) current_bias = gtsam.imuBias.ConstantBias() # init with zero bias - sigma_init_x = noiseModel.Diagonal.Precisions( - np.asarray([0, 0, 0, 1, 1, 1])) - sigma_init_v = noiseModel.Diagonal.Sigmas(np.ones(3) * 1000.0) - sigma_init_b = noiseModel.Diagonal.Sigmas( - np.asarray([0.1] * 3 + [5.00e-05] * 3)) - imu_params = getImuParams(kitti_calibration) # Set ISAM2 parameters and create ISAM2 solver object @@ -325,6 +326,37 @@ def main(): current_pose_global.print() print("\n") + return isam + + +def main(): + """Main runner.""" + args = parse_args() + kitti_calibration, imu_measurements, gps_measurements = loadKittiData() + + if not kitti_calibration.bodyTimu.equals(Pose3(), 1e-8): + raise ValueError( + "Currently only support IMUinBody is identity, i.e. IMU and body frame are the same" + ) + + # Configure different variables + first_gps_pose = 1 + gps_skip = 10 + + # Configure noise models + noise_model_gps = noiseModel.Diagonal.Precisions( + np.asarray([0, 0, 0] + [1.0 / 0.07] * 3)) + + sigma_init_x = noiseModel.Diagonal.Precisions( + np.asarray([0, 0, 0, 1, 1, 1])) + sigma_init_v = noiseModel.Diagonal.Sigmas(np.ones(3) * 1000.0) + sigma_init_b = noiseModel.Diagonal.Sigmas( + np.asarray([0.1] * 3 + [5.00e-05] * 3)) + + isam = optimize(gps_measurements, imu_measurements, sigma_init_x, + sigma_init_v, sigma_init_b, noise_model_gps, + kitti_calibration, first_gps_pose, gps_skip) + save_results(isam, args.output_filename, first_gps_pose, gps_measurements) From 9aa0dbf49394d0cabca4eaa45e269d84f8ff80a3 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 21 Oct 2021 18:42:46 -0400 Subject: [PATCH 247/248] replace static variable with variable of greater scope in cpp example --- examples/IMUKittiExampleGPS.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/examples/IMUKittiExampleGPS.cpp b/examples/IMUKittiExampleGPS.cpp index a2c82575f..cb60b2516 100644 --- a/examples/IMUKittiExampleGPS.cpp +++ b/examples/IMUKittiExampleGPS.cpp @@ -241,6 +241,8 @@ int main(int argc, char* argv[]) { "-- Starting main loop: inference is performed at each time step, but we " "plot trajectory every 10 steps\n"); size_t j = 0; + size_t included_imu_measurement_count = 0; + for (size_t i = first_gps_pose; i < gps_measurements.size() - 1; i++) { // At each non=IMU measurement we initialize a new node in the graph auto current_pose_key = X(i); @@ -266,7 +268,7 @@ int main(int argc, char* argv[]) { current_summarized_measurement = std::make_shared(imu_params, current_bias); - static size_t included_imu_measurement_count = 0; + while (j < imu_measurements.size() && imu_measurements[j].time <= t) { if (imu_measurements[j].time >= t_previous) { current_summarized_measurement->integrateMeasurement( @@ -306,7 +308,7 @@ int main(int argc, char* argv[]) { current_pose_key, gps_pose, noise_model_gps); new_values.insert(current_pose_key, gps_pose); - printf("################ POSE INCLUDED AT TIME %lf ################\n", + printf("############ POSE INCLUDED AT TIME %.6lf ############\n", t); cout << gps_pose.translation(); printf("\n\n"); @@ -324,7 +326,7 @@ int main(int argc, char* argv[]) { // We accumulate 2*GPSskip GPS measurements before updating the solver at // first so that the heading becomes observable. if (i > (first_gps_pose + 2 * gps_skip)) { - printf("################ NEW FACTORS AT TIME %lf ################\n", + printf("############ NEW FACTORS AT TIME %.6lf ############\n", t); new_factors.print(); @@ -341,7 +343,7 @@ int main(int argc, char* argv[]) { current_velocity_global = result.at(current_vel_key); current_bias = result.at(current_bias_key); - printf("\n################ POSE AT TIME %lf ################\n", t); + printf("\n############ POSE AT TIME %lf ############\n", t); current_pose_global.print(); printf("\n\n"); } From b3e8bf2325848f4600a57b977201cbeed38d1090 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 21 Oct 2021 18:45:57 -0400 Subject: [PATCH 248/248] fix the included_imu_measurement_count scope --- python/gtsam/examples/IMUKittiExampleGPS.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/python/gtsam/examples/IMUKittiExampleGPS.py b/python/gtsam/examples/IMUKittiExampleGPS.py index 071065260..8b6b4fdf0 100644 --- a/python/gtsam/examples/IMUKittiExampleGPS.py +++ b/python/gtsam/examples/IMUKittiExampleGPS.py @@ -219,7 +219,10 @@ def optimize(gps_measurements: List[GpsMeasurement], # (3) we solve the graph to obtain and optimal estimate of robot trajectory print("-- Starting main loop: inference is performed at each time step, " "but we plot trajectory every 10 steps") + j = 0 + included_imu_measurement_count = 0 + for i in range(first_gps_pose, len(gps_measurements)): # At each non=IMU measurement we initialize a new node in the graph current_pose_key = X(i) @@ -246,7 +249,6 @@ def optimize(gps_measurements: List[GpsMeasurement], current_summarized_measurement = gtsam.PreintegratedImuMeasurements( imu_params, current_bias) - included_imu_measurement_count = 0 while (j < len(imu_measurements) and imu_measurements[j].time <= t): if imu_measurements[j].time >= t_previous: