diff --git a/CMakeLists.txt b/CMakeLists.txt index aa6082cb3..eedc42c9e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -14,6 +14,11 @@ set (GTSAM_VERSION_PATCH 0) math (EXPR GTSAM_VERSION_NUMERIC "10000 * ${GTSAM_VERSION_MAJOR} + 100 * ${GTSAM_VERSION_MINOR} + ${GTSAM_VERSION_PATCH}") set (GTSAM_VERSION_STRING "${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH}") +set (CMAKE_PROJECT_VERSION ${GTSAM_VERSION_STRING}) +set (CMAKE_PROJECT_VERSION_MAJOR ${GTSAM_VERSION_MAJOR}) +set (CMAKE_PROJECT_VERSION_MINOR ${GTSAM_VERSION_MINOR}) +set (CMAKE_PROJECT_VERSION_PATCH ${GTSAM_VERSION_PATCH}) + ############################################################################### # Gather information, perform checks, set defaults @@ -113,6 +118,22 @@ if(GTSAM_INSTALL_MATLAB_TOOLBOX AND NOT BUILD_SHARED_LIBS) endif() if(GTSAM_BUILD_PYTHON) + # Get info about the Python3 interpreter + # https://cmake.org/cmake/help/latest/module/FindPython3.html#module:FindPython3 + find_package(Python3 COMPONENTS Interpreter Development) + + if(NOT ${Python3_FOUND}) + message(FATAL_ERROR "Cannot find Python3 interpreter. Please install Python >= 3.6.") + endif() + + if(${GTSAM_PYTHON_VERSION} STREQUAL "Default") + set(GTSAM_PYTHON_VERSION "${Python3_VERSION_MAJOR}.${Python3_VERSION_MINOR}" + CACHE + STRING + "The version of Python to build the wrappers against." + FORCE) + endif() + if(GTSAM_UNSTABLE_BUILD_PYTHON) if (NOT GTSAM_BUILD_UNSTABLE) message(WARNING "GTSAM_UNSTABLE_BUILD_PYTHON requires the unstable module to be enabled.") @@ -529,9 +550,9 @@ print_build_options_for_target(gtsam) print_config("Use System Eigen" "${GTSAM_USE_SYSTEM_EIGEN} (Using version: ${GTSAM_EIGEN_VERSION})") if(GTSAM_USE_TBB) - print_config("Use Intel TBB" "Yes") + print_config("Use Intel TBB" "Yes (Version: ${TBB_VERSION})") elseif(TBB_FOUND) - print_config("Use Intel TBB" "TBB found but GTSAM_WITH_TBB is disabled") + print_config("Use Intel TBB" "TBB (Version: ${TBB_VERSION}) found but GTSAM_WITH_TBB is disabled") else() print_config("Use Intel TBB" "TBB not found") endif() diff --git a/README.md b/README.md index cc8af7248..60eff197a 100644 --- a/README.md +++ b/README.md @@ -62,7 +62,7 @@ GTSAM 4.1 added a new pybind wrapper, and **removed** the deprecated functionali ## Wrappers -We provide support for [MATLAB](matlab/README.md) and [Python](cython/README.md) wrappers for GTSAM. Please refer to the linked documents for more details. +We provide support for [MATLAB](matlab/README.md) and [Python](python/README.md) wrappers for GTSAM. Please refer to the linked documents for more details. ## The Preintegrated IMU Factor diff --git a/cmake/FindNumPy.cmake b/cmake/FindNumPy.cmake index 4f5743aa6..d55a760c6 100644 --- a/cmake/FindNumPy.cmake +++ b/cmake/FindNumPy.cmake @@ -40,17 +40,9 @@ # Finding NumPy involves calling the Python interpreter if(NumPy_FIND_REQUIRED) - if(GTSAM_PYTHON_VERSION STREQUAL "Default") - find_package(PythonInterp REQUIRED) - else() - find_package(PythonInterp ${GTSAM_PYTHON_VERSION} EXACT REQUIRED) - endif() + find_package(PythonInterp ${GTSAM_PYTHON_VERSION} EXACT REQUIRED) else() - if(GTSAM_PYTHON_VERSION STREQUAL "Default") - find_package(PythonInterp) - else() - find_package(PythonInterp ${GTSAM_PYTHON_VERSION} EXACT) - endif() + find_package(PythonInterp ${GTSAM_PYTHON_VERSION} EXACT) endif() if(NOT PYTHONINTERP_FOUND) diff --git a/cmake/GtsamMatlabWrap.cmake b/cmake/GtsamMatlabWrap.cmake index 111114a7b..4c44d2cb3 100644 --- a/cmake/GtsamMatlabWrap.cmake +++ b/cmake/GtsamMatlabWrap.cmake @@ -215,19 +215,15 @@ function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs ex # Set up generation of module source file file(MAKE_DIRECTORY "${generated_files_path}") - if(GTSAM_PYTHON_VERSION STREQUAL "Default") - find_package(PythonInterp REQUIRED) - find_package(PythonLibs REQUIRED) - else() - find_package(PythonInterp - ${GTSAM_PYTHON_VERSION} - EXACT - REQUIRED) - find_package(PythonLibs - ${GTSAM_PYTHON_VERSION} - EXACT - REQUIRED) - endif() + find_package(PythonInterp + ${GTSAM_PYTHON_VERSION} + EXACT + REQUIRED) + find_package(PythonLibs + ${GTSAM_PYTHON_VERSION} + EXACT + REQUIRED) + set(_ignore gtsam::Point2 gtsam::Point3) diff --git a/cmake/GtsamPrinting.cmake b/cmake/GtsamPrinting.cmake index 5757f5ee1..c68679667 100644 --- a/cmake/GtsamPrinting.cmake +++ b/cmake/GtsamPrinting.cmake @@ -46,16 +46,16 @@ endfunction() # Prints all the relevant CMake build options for a given target: function(print_build_options_for_target target_name_) print_padded(GTSAM_COMPILE_FEATURES_PUBLIC) - print_padded(GTSAM_COMPILE_OPTIONS_PRIVATE) + # print_padded(GTSAM_COMPILE_OPTIONS_PRIVATE) print_padded(GTSAM_COMPILE_OPTIONS_PUBLIC) - print_padded(GTSAM_COMPILE_DEFINITIONS_PRIVATE) + # print_padded(GTSAM_COMPILE_DEFINITIONS_PRIVATE) print_padded(GTSAM_COMPILE_DEFINITIONS_PUBLIC) foreach(build_type ${GTSAM_CMAKE_CONFIGURATION_TYPES}) string(TOUPPER "${build_type}" build_type_toupper) - print_padded(GTSAM_COMPILE_OPTIONS_PRIVATE_${build_type_toupper}) + # print_padded(GTSAM_COMPILE_OPTIONS_PRIVATE_${build_type_toupper}) print_padded(GTSAM_COMPILE_OPTIONS_PUBLIC_${build_type_toupper}) - print_padded(GTSAM_COMPILE_DEFINITIONS_PRIVATE_${build_type_toupper}) + # print_padded(GTSAM_COMPILE_DEFINITIONS_PRIVATE_${build_type_toupper}) print_padded(GTSAM_COMPILE_DEFINITIONS_PUBLIC_${build_type_toupper}) endforeach() endfunction() diff --git a/gtsam/base/OptionalJacobian.h b/gtsam/base/OptionalJacobian.h index 602c5915e..4b580f82e 100644 --- a/gtsam/base/OptionalJacobian.h +++ b/gtsam/base/OptionalJacobian.h @@ -112,7 +112,7 @@ public: // template // OptionalJacobian(Eigen::Block block) : map_(nullptr) { ?? } - /// Return true is allocated, false if default constructor was used + /// Return true if allocated, false if default constructor was used operator bool() const { return map_.data() != nullptr; } @@ -197,7 +197,7 @@ public: #endif - /// Return true is allocated, false if default constructor was used + /// Return true if allocated, false if default constructor was used operator bool() const { return pointer_!=nullptr; } diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 0a56e2ef5..ea822b796 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -190,15 +190,7 @@ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& pose, ChartJacobian Hpose) { } /* ************************************************************************* */ -/** - * Compute the 3x3 bottom-left block Q of the SE3 Expmap derivative matrix - * J(xi) = [J_(w) Z_3x3; - * Q J_(w)] - * where J_(w) is the SO3 Expmap derivative. - * (see Chirikjian11book2, pg 44, eq 10.95. - * The closed-form formula is similar to formula 102 in Barfoot14tro) - */ -static Matrix3 computeQforExpmapDerivative(const Vector6& xi) { +Matrix3 Pose3::ComputeQforExpmapDerivative(const Vector6& xi, double nearZeroThreshold) { const auto w = xi.head<3>(); const auto v = xi.tail<3>(); const Matrix3 V = skewSymmetric(v); @@ -220,7 +212,7 @@ static Matrix3 computeQforExpmapDerivative(const Vector6& xi) { #else // The closed-form formula in Barfoot14tro eq. (102) double phi = w.norm(); - if (std::abs(phi)>1e-5) { + if (std::abs(phi)>nearZeroThreshold) { const double sinPhi = sin(phi), cosPhi = cos(phi); const double phi2 = phi * phi, phi3 = phi2 * phi, phi4 = phi3 * phi, phi5 = phi4 * phi; // Invert the sign of odd-order terms to have the right Jacobian @@ -230,8 +222,8 @@ static Matrix3 computeQforExpmapDerivative(const Vector6& xi) { } else { Q = -0.5*V + 1./6.*(W*V + V*W - W*V*W) - + 1./24.*(W*W*V + V*W*W - 3*W*V*W) - - 0.5*(1./24. + 3./120.)*(W*V*W*W + W*W*V*W); + - 1./24.*(W*W*V + V*W*W - 3*W*V*W) + + 1./120.*(W*V*W*W + W*W*V*W); } #endif @@ -242,7 +234,7 @@ static Matrix3 computeQforExpmapDerivative(const Vector6& xi) { Matrix6 Pose3::ExpmapDerivative(const Vector6& xi) { const Vector3 w = xi.head<3>(); const Matrix3 Jw = Rot3::ExpmapDerivative(w); - const Matrix3 Q = computeQforExpmapDerivative(xi); + const Matrix3 Q = ComputeQforExpmapDerivative(xi); Matrix6 J; J << Jw, Z_3x3, Q, Jw; return J; @@ -253,7 +245,7 @@ Matrix6 Pose3::LogmapDerivative(const Pose3& pose) { const Vector6 xi = Logmap(pose); const Vector3 w = xi.head<3>(); const Matrix3 Jw = Rot3::LogmapDerivative(w); - const Matrix3 Q = computeQforExpmapDerivative(xi); + const Matrix3 Q = ComputeQforExpmapDerivative(xi); const Matrix3 Q2 = -Jw*Q*Jw; Matrix6 J; J << Jw, Z_3x3, Q2, Jw; diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 159fd2927..2f0802cab 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -181,6 +181,18 @@ public: static Vector6 Local(const Pose3& pose, ChartJacobian Hpose = boost::none); }; + /** + * Compute the 3x3 bottom-left block Q of SE3 Expmap right derivative matrix + * J_r(xi) = [J_(w) Z_3x3; + * Q_r J_(w)] + * where J_(w) is the SO3 Expmap right derivative. + * (see Chirikjian11book2, pg 44, eq 10.95. + * The closed-form formula is identical to formula 102 in Barfoot14tro where + * Q_l of the SE3 Expmap left derivative matrix is given. + */ + static Matrix3 ComputeQforExpmapDerivative( + const Vector6& xi, double nearZeroThreshold = 1e-5); + using LieGroup::inverse; // version with derivative /** diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 8586f35a0..9639ffbcf 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -807,6 +807,17 @@ TEST(Pose3, ExpmapDerivative2) { } } +TEST( Pose3, ExpmapDerivativeQr) { + Vector6 w = Vector6::Random(); + w.head<3>().normalize(); + w.head<3>() = w.head<3>() * 0.9e-2; + Matrix3 actualQr = Pose3::ComputeQforExpmapDerivative(w, 0.01); + Matrix expectedH = numericalDerivative21 >(&Pose3::Expmap, w, boost::none); + Matrix3 expectedQr = expectedH.bottomLeftCorner<3, 3>(); + EXPECT(assert_equal(expectedQr, actualQr, 1e-6)); +} + /* ************************************************************************* */ TEST( Pose3, LogmapDerivative) { Matrix6 actualH; diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 5f031dd1d..6492b70b1 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -567,6 +567,7 @@ class Rot2 { // 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; @@ -727,6 +728,7 @@ class Rot3 { // 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; @@ -772,6 +774,7 @@ class Pose2 { // 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; @@ -825,6 +828,7 @@ class Pose3 { // 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); @@ -2847,6 +2851,7 @@ virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor { #include class SfmTrack { + Point3 point3() const; size_t number_measurements() const; pair measurement(size_t idx) const; pair siftIndex(size_t idx) const; diff --git a/gtsam/sfm/MFAS.cpp b/gtsam/sfm/MFAS.cpp new file mode 100644 index 000000000..6dccc2dee --- /dev/null +++ b/gtsam/sfm/MFAS.cpp @@ -0,0 +1,173 @@ +/** + * @file MFAS.cpp + * @brief Source file for the MFAS class + * @author Akshay Krishnan + * @date July 2020 + */ + +#include + +#include +#include +#include +#include +#include + +using namespace gtsam; +using std::map; +using std::pair; +using std::unordered_map; +using std::vector; +using std::unordered_set; + +// A node in the graph. +struct GraphNode { + double inWeightSum; // Sum of absolute weights of incoming edges + double outWeightSum; // Sum of absolute weights of outgoing edges + unordered_set inNeighbors; // Nodes from which there is an incoming edge + unordered_set outNeighbors; // Nodes to which there is an outgoing edge + + // Heuristic for the node that is to select nodes in MFAS. + double heuristic() const { return (outWeightSum + 1) / (inWeightSum + 1); } +}; + +// A graph is a map from key to GraphNode. This function returns the graph from +// the edgeWeights between keys. +unordered_map graphFromEdges( + const map& edgeWeights) { + unordered_map graph; + + for (const auto& edgeWeight : edgeWeights) { + // The weights can be either negative or positive. The direction of the edge + // is the direction of positive weight. This means that the edges is from + // edge.first -> edge.second if weight is positive and edge.second -> + // edge.first if weight is negative. + const MFAS::KeyPair& edge = edgeWeight.first; + const double& weight = edgeWeight.second; + + Key edgeSource = weight >= 0 ? edge.first : edge.second; + Key edgeDest = weight >= 0 ? edge.second : edge.first; + + // Update the in weight and neighbors for the destination. + graph[edgeDest].inWeightSum += std::abs(weight); + graph[edgeDest].inNeighbors.insert(edgeSource); + + // Update the out weight and neighbors for the source. + graph[edgeSource].outWeightSum += std::abs(weight); + graph[edgeSource].outNeighbors.insert(edgeDest); + } + return graph; +} + +// Selects the next node in the ordering from the graph. +Key selectNextNodeInOrdering(const unordered_map& graph) { + // Find the root nodes in the graph. + for (const auto& keyNode : graph) { + // It is a root node if the inWeightSum is close to zero. + if (keyNode.second.inWeightSum < 1e-8) { + // TODO(akshay-krishnan) if there are multiple roots, it is better to + // choose the one with highest heuristic. This is missing in the 1dsfm + // solution. + return keyNode.first; + } + } + // If there are no root nodes, return the node with the highest heuristic. + return std::max_element(graph.begin(), graph.end(), + [](const std::pair& keyNode1, + const std::pair& keyNode2) { + return keyNode1.second.heuristic() < + keyNode2.second.heuristic(); + }) + ->first; +} + +// Returns the absolute weight of the edge between node1 and node2. +double absWeightOfEdge(const Key key1, const Key key2, + const map& edgeWeights) { + // Check the direction of the edge before returning. + return edgeWeights.find(MFAS::KeyPair(key1, key2)) != edgeWeights.end() + ? std::abs(edgeWeights.at(MFAS::KeyPair(key1, key2))) + : std::abs(edgeWeights.at(MFAS::KeyPair(key2, key1))); +} + +// Removes a node from the graph and updates edge weights of its neighbors. +void removeNodeFromGraph(const Key node, + const map edgeWeights, + unordered_map& graph) { + // Update the outweights and outNeighbors of node's inNeighbors + for (const Key neighbor : graph[node].inNeighbors) { + // the edge could be either (*it, choice) with a positive weight or + // (choice, *it) with a negative weight + graph[neighbor].outWeightSum -= + absWeightOfEdge(node, neighbor, edgeWeights); + graph[neighbor].outNeighbors.erase(node); + } + // Update the inWeights and inNeighbors of node's outNeighbors + for (const Key neighbor : graph[node].outNeighbors) { + graph[neighbor].inWeightSum -= absWeightOfEdge(node, neighbor, edgeWeights); + graph[neighbor].inNeighbors.erase(node); + } + // Erase node. + graph.erase(node); +} + +MFAS::MFAS(const std::shared_ptr>& nodes, + const TranslationEdges& relativeTranslations, + const Unit3& projectionDirection) + : nodes_(nodes) { + // Iterate over edges, obtain weights by projecting + // their relativeTranslations along the projection direction + for (const auto& measurement : relativeTranslations) { + edgeWeights_[std::make_pair(measurement.key1(), measurement.key2())] = + measurement.measured().dot(projectionDirection); + } +} + +vector MFAS::computeOrdering() const { + vector ordering; // Nodes in MFAS order (result). + + // A graph is an unordered map from keys to nodes. Each node contains a list + // of its adjacent nodes. Create the graph from the edgeWeights. + unordered_map graph = graphFromEdges(edgeWeights_); + + // In each iteration, one node is removed from the graph and appended to the + // ordering. + while (!graph.empty()) { + Key selection = selectNextNodeInOrdering(graph); + removeNodeFromGraph(selection, edgeWeights_, graph); + ordering.push_back(selection); + } + return ordering; +} + +map MFAS::computeOutlierWeights() const { + // Find the ordering. + vector ordering = computeOrdering(); + + // Create a map from the node key to its position in the ordering. This makes + // it easier to lookup positions of different nodes. + unordered_map orderingPositions; + for (size_t i = 0; i < ordering.size(); i++) { + orderingPositions[ordering[i]] = i; + } + + map outlierWeights; + // Check if the direction of each edge is consistent with the ordering. + for (const auto& edgeWeight : edgeWeights_) { + // Find edge source and destination. + Key source = edgeWeight.first.first; + Key dest = edgeWeight.first.second; + if (edgeWeight.second < 0) { + std::swap(source, dest); + } + + // If the direction is not consistent with the ordering (i.e dest occurs + // before src), it is an outlier edge, and has non-zero outlier weight. + if (orderingPositions.at(dest) < orderingPositions.at(source)) { + outlierWeights[edgeWeight.first] = std::abs(edgeWeight.second); + } else { + outlierWeights[edgeWeight.first] = 0; + } + } + return outlierWeights; +} diff --git a/gtsam/sfm/MFAS.h b/gtsam/sfm/MFAS.h new file mode 100644 index 000000000..929aa5ff0 --- /dev/null +++ b/gtsam/sfm/MFAS.h @@ -0,0 +1,111 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010-2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +#pragma once + +/** + * @file MFAS.h + * @brief MFAS class to solve Minimum Feedback Arc Set graph problem + * @author Akshay Krishnan + * @date September 2020 + */ + +#include +#include +#include + +#include +#include +#include + +namespace gtsam { + +/** + The MFAS class to solve a Minimum feedback arc set (MFAS) + problem. We implement the solution from: + Kyle Wilson and Noah Snavely, "Robust Global Translations with 1DSfM", + Proceedings of the European Conference on Computer Vision, ECCV 2014 + + Given a weighted directed graph, the objective in a Minimum feedback arc set + problem is to obtain a directed acyclic graph by removing + edges such that the total weight of removed edges is minimum. + + Although MFAS is a general graph problem and can be applied in many areas, + this classed was designed for the purpose of outlier rejection in a + translation averaging for SfM setting. For more details, refer to the above + paper. The nodes of the graph in this context represents cameras in 3D and the + edges between them represent unit translations in the world coordinate frame, + i.e w_aZb is the unit translation from a to b expressed in the world + coordinate frame. The weights for the edges are obtained by projecting the + unit translations in a projection direction. + @addtogroup SFM +*/ +class MFAS { + public: + // used to represent edges between two nodes in the graph. When used in + // translation averaging for global SfM + using KeyPair = std::pair; + using TranslationEdges = std::vector>; + + private: + // pointer to nodes in the graph + const std::shared_ptr> nodes_; + + // edges with a direction such that all weights are positive + // i.e, edges that originally had negative weights are flipped + std::map edgeWeights_; + + public: + /** + * @brief Construct from the nodes in a graph and weighted directed edges + * between the nodes. Each node is identified by a Key. + * A shared pointer to the nodes is used as input parameter + * because, MFAS ordering is usually used to compute the ordering of a + * large graph that is already stored in memory. It is unnecessary to make a + * copy of the nodes in this class. + * @param nodes: Nodes in the graph + * @param edgeWeights: weights of edges in the graph + */ + MFAS(const std::shared_ptr> &nodes, + const std::map &edgeWeights) + : nodes_(nodes), edgeWeights_(edgeWeights) {} + + /** + * @brief Constructor to be used in the context of translation averaging. + * Here, the nodes of the graph are cameras in 3D and the edges have a unit + * translation direction between them. The weights of the edges is computed by + * projecting them along a projection direction. + * @param nodes cameras in the epipolar graph (each camera is identified by a + * Key) + * @param relativeTranslations translation directions between the cameras + * @param projectionDirection direction in which edges are to be projected + */ + MFAS(const std::shared_ptr> &nodes, + const TranslationEdges &relativeTranslations, + const Unit3 &projectionDirection); + + /** + * @brief Computes the 1D MFAS ordering of nodes in the graph + * @return orderedNodes: vector of nodes in the obtained order + */ + std::vector computeOrdering() const; + + /** + * @brief Computes the "outlier weights" of the graph. We define the outlier + * weight of a edge to be zero if the edge is an inlier and the magnitude of + * its edgeWeight if it is an outlier. This function internally calls + * computeOrdering and uses the obtained ordering to identify outlier edges. + * @return outlierWeights: map from an edge to its outlier weight. + */ + std::map computeOutlierWeights() const; +}; + +} // namespace gtsam diff --git a/gtsam/sfm/TranslationRecovery.cpp b/gtsam/sfm/TranslationRecovery.cpp index 665336cc4..319129840 100644 --- a/gtsam/sfm/TranslationRecovery.cpp +++ b/gtsam/sfm/TranslationRecovery.cpp @@ -45,9 +45,8 @@ NonlinearFactorGraph TranslationRecovery::buildGraph() const { } void TranslationRecovery::addPrior(const double scale, - NonlinearFactorGraph *graph) const { - //TODO(akshay-krishnan): make this an input argument - auto priorNoiseModel = noiseModel::Isotropic::Sigma(3, 0.01); + NonlinearFactorGraph *graph, + const SharedNoiseModel &priorNoiseModel) const { auto edge = relativeTranslations_.begin(); graph->emplace_shared >(edge->key1(), Point3(0, 0, 0), priorNoiseModel); graph->emplace_shared >(edge->key2(), scale * edge->measured().point3(), diff --git a/gtsam/sfm/TranslationRecovery.h b/gtsam/sfm/TranslationRecovery.h index 4c6a95cbe..d5538f91b 100644 --- a/gtsam/sfm/TranslationRecovery.h +++ b/gtsam/sfm/TranslationRecovery.h @@ -68,9 +68,7 @@ class TranslationRecovery { */ TranslationRecovery(const TranslationEdges &relativeTranslations, const LevenbergMarquardtParams &lmParams = LevenbergMarquardtParams()) - : relativeTranslations_(relativeTranslations), params_(lmParams) { - params_.setVerbosityLM("Summary"); - } + : relativeTranslations_(relativeTranslations), params_(lmParams) {} /** * @brief Build the factor graph to do the optimization. @@ -84,8 +82,11 @@ class TranslationRecovery { * * @param scale scale for first relative translation which fixes gauge. * @param graph factor graph to which prior is added. + * @param priorNoiseModel the noise model to use with the prior. */ - void addPrior(const double scale, NonlinearFactorGraph *graph) const; + void addPrior(const double scale, NonlinearFactorGraph *graph, + const SharedNoiseModel &priorNoiseModel = + noiseModel::Isotropic::Sigma(3, 0.01)) const; /** * @brief Create random initial translations. diff --git a/gtsam/sfm/tests/testMFAS.cpp b/gtsam/sfm/tests/testMFAS.cpp new file mode 100644 index 000000000..58ea4cc84 --- /dev/null +++ b/gtsam/sfm/tests/testMFAS.cpp @@ -0,0 +1,105 @@ +/** + * @file testMFAS.cpp + * @brief Unit tests for the MFAS class + * @author Akshay Krishnan + * @date July 2020 + */ + +#include +#include +#include + +using namespace std; +using namespace gtsam; + +/** + * We (partially) use the example from the paper on 1dsfm + * (https://research.cs.cornell.edu/1dsfm/docs/1DSfM_ECCV14.pdf, Fig 1, Page 5) + * for the unit tests here. The only change is that we leave out node 4 and use + * only nodes 0-3. This makes the test easier to understand and also + * avoids an ambiguity in the ground truth ordering that arises due to + * insufficient edges in the geaph when using the 4th node. + */ + +// edges in the graph - last edge from node 3 to 0 is an outlier +vector edges = {make_pair(3, 2), make_pair(0, 1), make_pair(3, 1), + make_pair(1, 2), make_pair(0, 2), make_pair(3, 0)}; +// nodes in the graph +vector nodes = {Key(0), Key(1), Key(2), Key(3)}; +// weights from projecting in direction-1 (bad direction, outlier accepted) +vector weights1 = {2, 1.5, 0.5, 0.25, 1, 0.75}; +// weights from projecting in direction-2 (good direction, outlier rejected) +vector weights2 = {0.5, 0.75, -0.25, 0.75, 1, 0.5}; + +// helper function to obtain map from keypairs to weights from the +// vector representations +map getEdgeWeights(const vector &edges, + const vector &weights) { + map edgeWeights; + for (size_t i = 0; i < edges.size(); i++) { + edgeWeights[edges[i]] = weights[i]; + } + cout << "returning edge weights " << edgeWeights.size() << endl; + return edgeWeights; +} + +// test the ordering and the outlierWeights function using weights2 - outlier +// edge is rejected when projected in a direction that gives weights2 +TEST(MFAS, OrderingWeights2) { + MFAS mfas_obj(make_shared>(nodes), getEdgeWeights(edges, weights2)); + + vector ordered_nodes = mfas_obj.computeOrdering(); + + // ground truth (expected) ordering in this example + vector gt_ordered_nodes = {0, 1, 3, 2}; + + // check if the expected ordering is obtained + for (size_t i = 0; i < ordered_nodes.size(); i++) { + EXPECT_LONGS_EQUAL(gt_ordered_nodes[i], ordered_nodes[i]); + } + + map outlier_weights = mfas_obj.computeOutlierWeights(); + + // since edge between 3 and 0 is inconsistent with the ordering, it must have + // positive outlier weight, other outlier weights must be zero + for (auto &edge : edges) { + if (edge == make_pair(Key(3), Key(0)) || + edge == make_pair(Key(0), Key(3))) { + EXPECT_DOUBLES_EQUAL(outlier_weights[edge], 0.5, 1e-6); + } else { + EXPECT_DOUBLES_EQUAL(outlier_weights[edge], 0, 1e-6); + } + } +} + +// test the ordering function and the outlierWeights method using +// weights1 (outlier edge is accepted when projected in a direction that +// produces weights1) +TEST(MFAS, OrderingWeights1) { + MFAS mfas_obj(make_shared>(nodes), getEdgeWeights(edges, weights1)); + + vector ordered_nodes = mfas_obj.computeOrdering(); + + // "ground truth" expected ordering in this example + vector gt_ordered_nodes = {3, 0, 1, 2}; + + // check if the expected ordering is obtained + for (size_t i = 0; i < ordered_nodes.size(); i++) { + EXPECT_LONGS_EQUAL(gt_ordered_nodes[i], ordered_nodes[i]); + } + + map outlier_weights = mfas_obj.computeOutlierWeights(); + + // since edge between 3 and 0 is inconsistent with the ordering, it must have + // positive outlier weight, other outlier weights must be zero + for (auto &edge : edges) { + EXPECT_DOUBLES_EQUAL(outlier_weights[edge], 0, 1e-6); + } +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 91ceaa5fd..a8fddcfe4 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -233,6 +233,9 @@ struct SfmTrack { SiftIndex siftIndex(size_t idx) const { return siftIndices[idx]; } + Point3 point3() const { + return p; + } }; /// Define the structure for the camera poses diff --git a/python/README.md b/python/README.md index b1a3a865f..6e2a30d2e 100644 --- a/python/README.md +++ b/python/README.md @@ -18,8 +18,10 @@ This is the Python wrapper around the GTSAM C++ library. We use our custom [wrap ## Install -- Run cmake with the `GTSAM_BUILD_PYTHON` cmake flag enabled to configure building the wrapper. The wrapped module will be built and copied to the directory `/python`. - +- Run cmake with the `GTSAM_BUILD_PYTHON` cmake flag enabled to configure building the wrapper. The wrapped module will be built and copied to the directory `/python`. For example, if your local Python version is 3.6.10, then you should run: + ```bash + cmake .. -DGTSAM_BUILD_PYTHON=1 -DGTSAM_PYTHON_VERSION=3.6.10 + ``` - Build GTSAM and the wrapper with `make` (or `ninja` if you use `-GNinja`). - To install, simply run `make python-install` (`ninja python-install`). diff --git a/python/gtsam/examples/ImuFactorExample.py b/python/gtsam/examples/ImuFactorExample.py index eec7c5ebd..bb707a8f5 100644 --- a/python/gtsam/examples/ImuFactorExample.py +++ b/python/gtsam/examples/ImuFactorExample.py @@ -104,7 +104,7 @@ class ImuFactorExample(PreintegrationExample): if verbose: print(factor) - print(pim.predict(actual_state_i, self.actualBias)) + print(pim.predict(initial_state_i, self.actualBias)) pim.resetIntegration() @@ -125,7 +125,7 @@ class ImuFactorExample(PreintegrationExample): i += 1 # add priors on end - # self.addPrior(num_poses - 1, graph) + self.addPrior(num_poses - 1, graph) initial.print_("Initial values:") diff --git a/python/gtsam/examples/README.md b/python/gtsam/examples/README.md index e998e4dcd..e05a0c7e1 100644 --- a/python/gtsam/examples/README.md +++ b/python/gtsam/examples/README.md @@ -3,50 +3,62 @@ | C++ Example Name | Ported | |-------------------------------------------------------|--------| | CameraResectioning | | +| CombinedImuFactorsExample | | | CreateSFMExampleData | | +| DiscreteBayesNetExample | | | DiscreteBayesNet_FG | none of the required discrete functionality is exposed through Python | | easyPoint2KalmanFilter | ExtendedKalmanFilter not yet exposed through Python | | elaboratePoint2KalmanFilter | GaussianSequentialSolver not yet exposed through Python | -| ImuFactorExample2 | X | +| FisheyeExample | | +| HMMExample | | +| ImuFactorsExample2 | :heavy_check_mark: | | ImuFactorsExample | | +| IMUKittiExampleGPS | | +| InverseKinematicsExampleExpressions.cpp | | | ISAM2Example_SmartFactor | | | ISAM2_SmartFactorStereo_IMU | | | LocalizationExample | impossible? | | METISOrderingExample | | -| OdometryExample | X | -| PlanarSLAMExample | X | -| Pose2SLAMExample | X | +| OdometryExample | :heavy_check_mark: | +| PlanarSLAMExample | :heavy_check_mark: | +| Pose2SLAMExample | :heavy_check_mark: | | Pose2SLAMExampleExpressions | | -| Pose2SLAMExample_g2o | X | +| Pose2SLAMExample_g2o | :heavy_check_mark: | | Pose2SLAMExample_graph | | | Pose2SLAMExample_graphviz | | | Pose2SLAMExample_lago | lago not yet exposed through Python | | Pose2SLAMStressTest | | | Pose2SLAMwSPCG | | +| Pose3Localization | | | Pose3SLAMExample_changeKeys | | | Pose3SLAMExampleExpressions_BearingRangeWithTransform | | -| Pose3SLAMExample_g2o | X | -| Pose3SLAMExample_initializePose3Chordal | | +| Pose3SLAMExample_g2o | :heavy_check_mark: | +| Pose3SLAMExample_initializePose3Chordal | :heavy_check_mark: | | Pose3SLAMExample_initializePose3Gradient | | | RangeISAMExample_plaza2 | | | SelfCalibrationExample | | +| SFMdata | | | SFMExample_bal_COLAMD_METIS | | -| SFMExample_bal | | -| SFMExample | X | +| SFMExample_bal | | +| SFMExample | :heavy_check_mark: | | SFMExampleExpressions_bal | | | SFMExampleExpressions | | | SFMExample_SmartFactor | | | SFMExample_SmartFactorPCG | | -| SimpleRotation | X | +| ShonanAveragingCLI | :heavy_check_mark: | +| SimpleRotation | :heavy_check_mark: | | SolverComparer | | | StereoVOExample | | | StereoVOExample_large | | | TimeTBB | | | UGM_chain | discrete functionality not yet exposed | | UGM_small | discrete functionality not yet exposed | -| VisualISAM2Example | X | -| VisualISAMExample | X | +| VisualISAM2Example | :heavy_check_mark: | +| VisualISAMExample | :heavy_check_mark: | Extra Examples (with no C++ equivalent) +- DogLegOptimizerExample +- GPSFactorExample - PlanarManipulatorExample -- SFMData \ No newline at end of file +- PreintegrationExample +- SFMData diff --git a/python/gtsam/tests/test_TranslationRecovery.py b/python/gtsam/tests/test_TranslationRecovery.py index ec6d6a83f..0fb0518b6 100644 --- a/python/gtsam/tests/test_TranslationRecovery.py +++ b/python/gtsam/tests/test_TranslationRecovery.py @@ -40,7 +40,12 @@ class TestTranslationRecovery(unittest.TestCase): def test_run(self): gt_poses = ExampleValues() measurements = SimulateMeasurements(gt_poses, [[0, 1], [0, 2], [1, 2]]) - algorithm = gtsam.TranslationRecovery(measurements) + + # Set verbosity to Silent for tests + lmParams = gtsam.LevenbergMarquardtParams() + lmParams.setVerbosityLM("silent") + + algorithm = gtsam.TranslationRecovery(measurements, lmParams) scale = 2.0 result = algorithm.run(scale) diff --git a/wrap/matlab_wrapper.py b/wrap/matlab_wrapper.py index 23f56c8b0..1b6b75a49 100755 --- a/wrap/matlab_wrapper.py +++ b/wrap/matlab_wrapper.py @@ -210,18 +210,26 @@ class MatlabWrapper(object): else: formatted_type_name += name - if len(type_name.instantiations) == 1: - if separator == "::": # C++ - formatted_type_name += '<{}>'.format(cls._format_type_name(type_name.instantiations[0], - include_namespace=include_namespace, - constructor=constructor, method=method)) - else: + 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[0], + type_name.instantiations[idx], separator=separator, include_namespace=False, constructor=constructor, method=method )) + return formatted_type_name @classmethod