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

Merge branch 'develop' into fix/wrapper-todos

release/4.3a0
Varun Agrawal 2020-09-26 14:38:52 -04:00
parent 09f09fa44f 23547c76b8
commit de5f002467
21 changed files with 526 additions and 78 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
CMakeLists.txt
View File

@ -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()

2
README.md
View File

@ -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

8
cmake/FindNumPy.cmake
View File

@ -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()
else()
if(GTSAM_PYTHON_VERSION STREQUAL "Default")
find_package(PythonInterp)
else()
find_package(PythonInterp ${GTSAM_PYTHON_VERSION} EXACT)
endif()
endif()
if(NOT PYTHONINTERP_FOUND)

6
cmake/GtsamMatlabWrap.cmake
View File

@ -215,10 +215,6 @@ 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
@ -227,7 +223,7 @@ function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs ex
${GTSAM_PYTHON_VERSION}
EXACT
REQUIRED)
endif()
set(_ignore gtsam::Point2
gtsam::Point3)

8
cmake/GtsamPrinting.cmake
View File

@ -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()

4
gtsam/base/OptionalJacobian.h
View File

@ -112,7 +112,7 @@ public:
// template <typename Derived, bool InnerPanel>
// OptionalJacobian(Eigen::Block<Derived,Rows,Cols,InnerPanel> 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;
}

20
gtsam/geometry/Pose3.cpp
View File

@ -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;

12
gtsam/geometry/Pose3.h
View File

@ -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<Pose3, 6>::inverse; // version with derivative
/**

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

@ -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, Vector6,
OptionalJacobian<6, 6> >(&Pose3::Expmap, w, boost::none);
Matrix3 expectedQr = expectedH.bottomLeftCorner<3, 3>();
EXPECT(assert_equal(expectedQr, actualQr, 1e-6));
}
/* ************************************************************************* */
TEST( Pose3, LogmapDerivative) {
Matrix6 actualH;

5
gtsam/gtsam.i
View File

@ -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 <gtsam/slam/dataset.h>
class SfmTrack {
Point3 point3() const;
size_t number_measurements() const;
pair<size_t, gtsam::Point2> measurement(size_t idx) const;
pair<size_t, size_t> siftIndex(size_t idx) const;

173
gtsam/sfm/MFAS.cpp Normal file
View File

@ -0,0 +1,173 @@
/**
* @file MFAS.cpp
* @brief Source file for the MFAS class
* @author Akshay Krishnan
* @date July 2020
*/
#include <gtsam/sfm/MFAS.h>
#include <algorithm>
#include <map>
#include <unordered_map>
#include <vector>
#include <unordered_set>
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<Key> inNeighbors; // Nodes from which there is an incoming edge
unordered_set<Key> 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<Key, GraphNode> graphFromEdges(
const map<MFAS::KeyPair, double>& edgeWeights) {
unordered_map<Key, GraphNode> 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<Key, GraphNode>& 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<Key, GraphNode>& keyNode1,
const std::pair<Key, GraphNode>& 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<MFAS::KeyPair, double>& 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<MFAS::KeyPair, double> edgeWeights,
unordered_map<Key, GraphNode>& 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<vector<Key>>& 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<Key> MFAS::computeOrdering() const {
vector<Key> 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<Key, GraphNode> 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::KeyPair, double> MFAS::computeOutlierWeights() const {
// Find the ordering.
vector<Key> 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<Key, int> orderingPositions;
for (size_t i = 0; i < ordering.size(); i++) {
orderingPositions[ordering[i]] = i;
}
map<KeyPair, double> 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;
}

111
gtsam/sfm/MFAS.h Normal file
View File

@ -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 <gtsam/geometry/Unit3.h>
#include <gtsam/inference/Key.h>
#include <gtsam/sfm/BinaryMeasurement.h>
#include <memory>
#include <unordered_map>
#include <vector>
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<Key, Key>;
using TranslationEdges = std::vector<BinaryMeasurement<Unit3>>;
private:
// pointer to nodes in the graph
const std::shared_ptr<std::vector<Key>> nodes_;
// edges with a direction such that all weights are positive
// i.e, edges that originally had negative weights are flipped
std::map<KeyPair, double> 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<std::vector<Key>> &nodes,
const std::map<KeyPair, double> &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<std::vector<Key>> &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<Key> 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<KeyPair, double> computeOutlierWeights() const;
};
} // namespace gtsam

5
gtsam/sfm/TranslationRecovery.cpp
View File

@ -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<PriorFactor<Point3> >(edge->key1(), Point3(0, 0, 0), priorNoiseModel);
graph->emplace_shared<PriorFactor<Point3> >(edge->key2(), scale * edge->measured().point3(),

9
gtsam/sfm/TranslationRecovery.h
View File

@ -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.

105
gtsam/sfm/tests/testMFAS.cpp Normal file
View File

@ -0,0 +1,105 @@
/**
* @file testMFAS.cpp
* @brief Unit tests for the MFAS class
* @author Akshay Krishnan
* @date July 2020
*/
#include <gtsam/sfm/MFAS.h>
#include <iostream>
#include <CppUnitLite/TestHarness.h>
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<MFAS::KeyPair> 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<Key> nodes = {Key(0), Key(1), Key(2), Key(3)};
// weights from projecting in direction-1 (bad direction, outlier accepted)
vector<double> weights1 = {2, 1.5, 0.5, 0.25, 1, 0.75};
// weights from projecting in direction-2 (good direction, outlier rejected)
vector<double> 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<MFAS::KeyPair, double> getEdgeWeights(const vector<MFAS::KeyPair> &edges,
const vector<double> &weights) {
map<MFAS::KeyPair, double> 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<vector<Key>>(nodes), getEdgeWeights(edges, weights2));
vector<Key> ordered_nodes = mfas_obj.computeOrdering();
// ground truth (expected) ordering in this example
vector<Key> 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<MFAS::KeyPair, double> 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<vector<Key>>(nodes), getEdgeWeights(edges, weights1));
vector<Key> ordered_nodes = mfas_obj.computeOrdering();
// "ground truth" expected ordering in this example
vector<Key> 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<MFAS::KeyPair, double> 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);
}
/* ************************************************************************* */

3
gtsam/slam/dataset.h
View File

@ -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

6
python/README.md
View File

@ -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 `<PROJECT_BINARY_DIR>/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 `<PROJECT_BINARY_DIR>/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`).

4
python/gtsam/examples/ImuFactorExample.py
View File

@ -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:")

34
python/gtsam/examples/README.md
View File

@ -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 | :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
- PreintegrationExample
- SFMData

7
python/gtsam/tests/test_TranslationRecovery.py
View File

@ -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)

14
wrap/matlab_wrapper.py
View File

@ -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],
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