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

Merge branch 'develop' into hybrid-timing

release/4.3a0
Varun Agrawal 2024-12-23 15:29:21 -05:00
parent 53cf49b1ba 6f7365e1f3
commit ad3967e37f
177 changed files with 3112 additions and 877 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
CMakeLists.txt
View File

@ -1,4 +1,4 @@
cmake_minimum_required(VERSION 3.5)
cmake_minimum_required(VERSION 3.9)
if (POLICY CMP0082)
cmake_policy(SET CMP0082 NEW) # install from sub-directories immediately
endif()

2
CppUnitLite/Test.h
View File

@ -129,7 +129,7 @@ protected:
result_.addFailure(Failure(name_, __FILE__, __LINE__, #expected, #actual)); }
#define CHECK_EQUAL(expected,actual)\
{ if ((expected) == (actual)) return; result_.addFailure(Failure(name_, __FILE__, __LINE__, std::to_string(expected), std::to_string(actual))); }
{ if (!((expected) == (actual))) { result_.addFailure(Failure(name_, __FILE__, __LINE__, std::to_string(expected), std::to_string(actual))); return; } }
#define LONGS_EQUAL(expected,actual)\
{ long actualTemp = actual; \

2
GTSAM-Concepts.md
View File

@ -22,7 +22,7 @@ In GTSAM, all properties and operations needed to use a type must be defined thr
In detail, we ask that the following items are defined in the traits object (although, not all are needed for optimization):
* values:
* `enum { dimension = D};`, an enum that indicates the dimensionality *n* of the manifold. In Eigen-fashion, we also support manifolds whose dimensionality is only defined at runtime, by specifying the value -1.
* `inline constexpr static auto dimension = D;`, a constexpr that indicates the dimensionality *n* of the manifold. In Eigen-fashion, we also support manifolds whose dimensionality is only defined at runtime, by specifying the value -1.
* types:
* `TangentVector`, type that lives in tangent space. This will almost always be an `Eigen::Matrix<double,n,1>`.
* `ChartJacobian`, a typedef for `OptionalJacobian<dimension, dimension>`.

20
INSTALL.md
View File

@ -27,7 +27,7 @@ $ make install
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
achieved with MKL disabled. We therefore advise you to benchmark your problem
before using MKL.
Tested compilers:
@ -128,12 +128,12 @@ We support several build configurations for GTSAM (case insensitive)
#### CMAKE_INSTALL_PREFIX
The install folder. The default is typically `/usr/local/`.
The install folder. The default is typically `/usr/local/`.
To configure to install to your home directory, you could execute:
```cmake -DCMAKE_INSTALL_PREFIX:PATH=$HOME ..```
#### GTSAM_TOOLBOX_INSTALL_PATH
#### GTSAM_TOOLBOX_INSTALL_PATH
The Matlab toolbox will be installed in a subdirectory
of this folder, called 'gtsam'.
@ -142,7 +142,7 @@ of this folder, called 'gtsam'.
#### GTSAM_BUILD_CONVENIENCE_LIBRARIES
This is a build option to allow for tests in subfolders to be linked against convenience libraries rather than the full libgtsam.
This is a build option to allow for tests in subfolders to be linked against convenience libraries rather than the full libgtsam.
Set with the command line as follows:
```cmake -DGTSAM_BUILD_CONVENIENCE_LIBRARIES:OPTION=ON ..```
@ -159,6 +159,16 @@ Set with the command line as follows:
ON: When enabled, libgtsam_unstable will be built and installed with the same options as libgtsam. In addition, if tests are enabled, the unit tests will be built as well. The Matlab toolbox will also be generated if the matlab toolbox is enabled, installing into a folder called `gtsam_unstable`.
OFF (Default) If disabled, no `gtsam_unstable` code will be included in build or install.
## Convenience Options:
#### GTSAM_BUILD_EXAMPLES_ALWAYS
Whether or not to force building examples, can be true or false.
#### GTSAM_BUILD_TESTS
Whether or not to build tests, can be true or false.
## Check
`make check` will build and run all of the tests. Note that the tests will only be
@ -179,7 +189,7 @@ Here are some tips to get the best possible performance out of GTSAM.
1. Build in `Release` mode. GTSAM will run up to 10x faster compared to `Debug` mode.
2. Enable TBB. On modern processors with multiple cores, this can easily speed up
optimization by 30-50%. Please note that this may not be true for very small
optimization by 30-50%. Please note that this may not be true for very small
problems where the overhead of dispatching work to multiple threads outweighs
the benefit. We recommend that you benchmark your problem with/without TBB.
3. Use `GTSAM_BUILD_WITH_MARCH_NATIVE`. A performance gain of

56
cmake/FindGooglePerfTools.cmake
View File

@ -1,42 +1,40 @@
# -*- cmake -*-
# - Find Google perftools
# Find the Google perftools includes and libraries
# This module defines
# GOOGLE_PERFTOOLS_INCLUDE_DIR, where to find heap-profiler.h, etc.
# GOOGLE_PERFTOOLS_FOUND, If false, do not try to use Google perftools.
# also defined for general use are
# TCMALLOC_LIBRARY, where to find the tcmalloc library.
FIND_PATH(GOOGLE_PERFTOOLS_INCLUDE_DIR google/heap-profiler.h
/usr/local/include
/usr/include
)
# - Find GPerfTools (formerly Google perftools)
# Find the GPerfTools libraries
# If false, do not try to use Google perftools.
# Also defined for general use are
# - GPERFTOOLS_TCMALLOC: where to find the tcmalloc library
# - GPERFTOOLS_PROFILER: where to find the profiler library
SET(TCMALLOC_NAMES ${TCMALLOC_NAMES} tcmalloc)
FIND_LIBRARY(TCMALLOC_LIBRARY
find_library(GPERFTOOLS_TCMALLOC
NAMES ${TCMALLOC_NAMES}
PATHS /usr/lib /usr/local/lib
)
)
find_library(GPERFTOOLS_PROFILER
NAMES profiler
PATHS /usr/lib /usr/local/lib
)
IF (TCMALLOC_LIBRARY AND GOOGLE_PERFTOOLS_INCLUDE_DIR)
SET(TCMALLOC_LIBRARIES ${TCMALLOC_LIBRARY})
SET(GOOGLE_PERFTOOLS_FOUND "YES")
ELSE (TCMALLOC_LIBRARY AND GOOGLE_PERFTOOLS_INCLUDE_DIR)
SET(GOOGLE_PERFTOOLS_FOUND "NO")
ENDIF (TCMALLOC_LIBRARY AND GOOGLE_PERFTOOLS_INCLUDE_DIR)
IF (GPERFTOOLS_TCMALLOC AND GPERFTOOLS_PROFILER)
SET(TCMALLOC_LIBRARIES ${GPERFTOOLS_TCMALLOC})
SET(GPERFTOOLS_FOUND "YES")
ELSE (GPERFTOOLS_TCMALLOC AND GPERFTOOLS_PROFILER)
SET(GPERFTOOLS_FOUND "NO")
ENDIF (GPERFTOOLS_TCMALLOC AND GPERFTOOLS_PROFILER)
IF (GOOGLE_PERFTOOLS_FOUND)
IF (NOT GOOGLE_PERFTOOLS_FIND_QUIETLY)
MESSAGE(STATUS "Found Google perftools: ${GOOGLE_PERFTOOLS_LIBRARIES}")
ENDIF (NOT GOOGLE_PERFTOOLS_FIND_QUIETLY)
ELSE (GOOGLE_PERFTOOLS_FOUND)
IF (GPERFTOOLS_FOUND)
MESSAGE(STATUS "Found Gperftools: ${GPERFTOOLS_PROFILER}")
ELSE (GPERFTOOLS_FOUND)
IF (GOOGLE_PERFTOOLS_FIND_REQUIRED)
MESSAGE(FATAL_ERROR "Could not find Google perftools library")
ENDIF (GOOGLE_PERFTOOLS_FIND_REQUIRED)
ENDIF (GOOGLE_PERFTOOLS_FOUND)
ENDIF (GPERFTOOLS_FOUND)
MARK_AS_ADVANCED(
TCMALLOC_LIBRARY
GOOGLE_PERFTOOLS_INCLUDE_DIR
)
GPERFTOOLS_TCMALLOC
GPERFTOOLS_PROFILER
)
option(GTSAM_ENABLE_GPERFTOOLS "Enable/Disable Gperftools" OFF)

2
cmake/HandleAllocators.cmake
View File

@ -7,7 +7,7 @@ else()
list(APPEND possible_allocators BoostPool STL)
set(preferred_allocator STL)
endif()
if(GOOGLE_PERFTOOLS_FOUND)
if(GPERFTOOLS_FOUND)
list(APPEND possible_allocators tcmalloc)
endif()

2
cmake/HandlePerfTools.cmake
View File

@ -1,4 +1,4 @@
###############################################################################
# Find Google perftools
find_package(GooglePerfTools)
find_package(GooglePerfTools)

1
examples/CombinedImuFactorsExample.cpp
View File

@ -48,6 +48,7 @@
#include <cstring>
#include <fstream>
#include <iostream>
#include <cassert>
using namespace gtsam;
using namespace std;

1
examples/ImuFactorsExample.cpp
View File

@ -49,6 +49,7 @@
#include <gtsam/slam/dataset.h>
#include <cstring>
#include <cassert>
#include <fstream>
#include <iostream>

2
examples/elaboratePoint2KalmanFilter.cpp
View File

@ -29,6 +29,8 @@
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/geometry/Point2.h>
#include <cassert>
using namespace std;
using namespace gtsam;

2
gtsam/3rdparty/cephes/CMakeLists.txt vendored
View File

@ -89,7 +89,7 @@ set(CEPHES_SOURCES
cephes/zetac.c)
# Add library source files
add_library(cephes-gtsam SHARED ${CEPHES_SOURCES})
add_library(cephes-gtsam ${GTSAM_LIBRARY_TYPE} ${CEPHES_SOURCES})
# Add include directory (aka headers)
target_include_directories(

4
gtsam/CMakeLists.txt
View File

@ -147,6 +147,10 @@ if (GTSAM_USE_EIGEN_MKL)
target_include_directories(gtsam PUBLIC ${MKL_INCLUDE_DIR})
endif()
if (GTSAM_ENABLE_GPERFTOOLS AND GPERFTOOLS_FOUND)
target_link_libraries(gtsam PRIVATE ${GPERFTOOLS_TCMALLOC} ${GPERFTOOLS_PROFILER})
endif()
# Add includes for source directories 'BEFORE' boost and any system include
# paths so that the compiler uses GTSAM headers in our source directory instead
# of any previously installed GTSAM headers.

3
gtsam/base/GenericValue.h
View File

@ -184,9 +184,8 @@ public:
}
#endif
// Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
enum { NeedsToAlign = (sizeof(T) % 16) == 0 };
constexpr static const bool NeedsToAlign = (sizeof(T) % 16) == 0;
public:
GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
};

4
gtsam/base/Lie.h
View File

@ -36,7 +36,7 @@ namespace gtsam {
template <class Class, int N>
struct LieGroup {
enum { dimension = N };
inline constexpr static auto dimension = N;
typedef OptionalJacobian<N, N> ChartJacobian;
typedef Eigen::Matrix<double, N, N> Jacobian;
typedef Eigen::Matrix<double, N, 1> TangentVector;
@ -183,7 +183,7 @@ struct LieGroupTraits: GetDimensionImpl<Class, Class::dimension> {
/// @name Manifold
/// @{
typedef Class ManifoldType;
enum { dimension = Class::dimension };
inline constexpr static auto dimension = Class::dimension;
typedef Eigen::Matrix<double, dimension, 1> TangentVector;
typedef OptionalJacobian<dimension, dimension> ChartJacobian;

4
gtsam/base/Manifold.h
View File

@ -55,7 +55,7 @@ namespace internal {
template<class Class>
struct HasManifoldPrereqs {
enum { dim = Class::dimension };
inline constexpr static auto dim = Class::dimension;
Class p, q;
Eigen::Matrix<double, dim, 1> v;
@ -95,7 +95,7 @@ struct ManifoldTraits: GetDimensionImpl<Class, Class::dimension> {
GTSAM_CONCEPT_ASSERT(HasManifoldPrereqs<Class>);
// Dimension of the manifold
enum { dimension = Class::dimension };
inline constexpr static auto dimension = Class::dimension;
// Typedefs required by all manifold types.
typedef Class ManifoldType;

1
gtsam/base/Matrix.cpp
View File

@ -24,6 +24,7 @@
#include <Eigen/LU>
#include <cstdarg>
#include <cassert>
#include <cstring>
#include <iomanip>
#include <list>

2
gtsam/base/Matrix.h
View File

@ -479,7 +479,7 @@ struct MultiplyWithInverse {
*/
template <typename T, int N>
struct MultiplyWithInverseFunction {
enum { M = traits<T>::dimension };
inline constexpr static auto M = traits<T>::dimension;
typedef Eigen::Matrix<double, N, 1> VectorN;
typedef Eigen::Matrix<double, N, N> MatrixN;

10
gtsam/base/ProductLieGroup.h
View File

@ -32,8 +32,8 @@ class ProductLieGroup: public std::pair<G, H> {
typedef std::pair<G, H> Base;
protected:
enum {dimension1 = traits<G>::dimension};
enum {dimension2 = traits<H>::dimension};
constexpr static const size_t dimension1 = traits<G>::dimension;
constexpr static const size_t dimension2 = traits<H>::dimension;
public:
/// Default constructor yields identity
@ -67,9 +67,9 @@ public:
/// @name Manifold
/// @{
enum {dimension = dimension1 + dimension2};
inline static size_t Dim() {return dimension;}
inline size_t dim() const {return dimension;}
inline constexpr static auto dimension = dimension1 + dimension2;
inline static size_t Dim() { return dimension; }
inline size_t dim() const { return dimension; }
typedef Eigen::Matrix<double, dimension, 1> TangentVector;
typedef OptionalJacobian<dimension, dimension> ChartJacobian;

1
gtsam/base/Vector.cpp
View File

@ -24,6 +24,7 @@
#include <iostream>
#include <fstream>
#include <sstream>
#include <cassert>
#include <iomanip>
#include <cmath>
#include <cstdio>

3
gtsam/base/Vector.h
View File

@ -16,6 +16,7 @@
* @author Frank Dellaert
* @author Alex Hagiopol
* @author Varun Agrawal
* @author Fan Jiang
*/
// \callgraph
@ -193,14 +194,12 @@ GTSAM_EXPORT Vector ediv_(const Vector &a, const Vector &b);
*/
template<class V1, class V2>
inline double dot(const V1 &a, const V2& b) {
assert (b.size()==a.size());
return a.dot(b);
}
/** compatibility version for ublas' inner_prod() */
template<class V1, class V2>
inline double inner_prod(const V1 &a, const V2& b) {
assert (b.size()==a.size());
return a.dot(b);
}

10
gtsam/base/VectorSpace.h
View File

@ -163,7 +163,7 @@ struct VectorSpaceImpl<Class,Eigen::Dynamic> {
template<class Class>
struct HasVectorSpacePrereqs {
enum { dim = Class::dimension };
inline constexpr static auto dim = Class::dimension;
Class p, q;
Vector v;
@ -197,7 +197,7 @@ GTSAM_CONCEPT_ASSERT(HasVectorSpacePrereqs<Class>);
/// @name Manifold
/// @{
enum { dimension = Class::dimension};
inline constexpr static auto dimension = Class::dimension;
typedef Class ManifoldType;
/// @}
};
@ -232,7 +232,7 @@ struct ScalarTraits : VectorSpaceImpl<Scalar, 1> {
/// @name Manifold
/// @{
typedef Scalar ManifoldType;
enum { dimension = 1 };
inline constexpr static auto dimension = 1;
typedef Eigen::Matrix<double, 1, 1> TangentVector;
typedef OptionalJacobian<1, 1> ChartJacobian;
@ -305,7 +305,7 @@ struct traits<Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > :
/// @name Manifold
/// @{
enum { dimension = M*N};
inline constexpr static auto dimension = M * N;
typedef Fixed ManifoldType;
typedef Eigen::Matrix<double, dimension, 1> TangentVector;
typedef Eigen::Matrix<double, dimension, dimension> Jacobian;
@ -377,7 +377,7 @@ struct DynamicTraits {
/// @name Manifold
/// @{
enum { dimension = Eigen::Dynamic };
inline constexpr static auto dimension = Eigen::Dynamic;
typedef Eigen::VectorXd TangentVector;
typedef Eigen::MatrixXd Jacobian;
typedef OptionalJacobian<dimension, dimension> ChartJacobian;

2
gtsam/base/VerticalBlockMatrix.h
View File

@ -21,6 +21,8 @@
#include <gtsam/base/MatrixSerialization.h>
#include <gtsam/base/FastVector.h>
#include <cassert>
namespace gtsam {
// Forward declarations

1
gtsam/base/cholesky.cpp
View File

@ -21,6 +21,7 @@
#include <gtsam/base/timing.h>
#include <cmath>
#include <cassert>
using namespace std;

2
gtsam/base/tests/testNumericalDerivative.cpp
View File

@ -18,6 +18,8 @@
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>
#include <cassert>
using namespace std;
using namespace gtsam;

2
gtsam/basis/Basis.h
View File

@ -291,7 +291,7 @@ class Basis {
*/
template <class T>
class ManifoldEvaluationFunctor : public VectorEvaluationFunctor {
enum { M = traits<T>::dimension };
inline constexpr static auto M = traits<T>::dimension;
using Base = VectorEvaluationFunctor;
public:

8
gtsam/basis/Chebyshev2.cpp
View File

@ -20,6 +20,14 @@
namespace gtsam {
double Chebyshev2::Point(size_t N, int j, double a, double b) {
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 a + (b - a) * (1. + cos(-M_PI + dtheta * j)) / 2;
}
Weights Chebyshev2::CalculateWeights(size_t N, double x, double a, double b) {
// Allocate space for weights
Weights weights(N);

8
gtsam/basis/Chebyshev2.h
View File

@ -61,13 +61,7 @@ class GTSAM_EXPORT Chebyshev2 : public Basis<Chebyshev2> {
* @param b Upper bound of interval (default: 1)
* @return double
*/
static double Point(size_t N, int j, double a = -1, double b = 1) {
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 a + (b - a) * (1. + cos(-M_PI + dtheta * j)) / 2;
}
static double Point(size_t N, int j, double a = -1, double b = 1);
/// All Chebyshev points
static Vector Points(size_t N) {

24
gtsam/discrete/AlgebraicDecisionTree.h
View File

@ -20,12 +20,12 @@
#include <gtsam/base/Testable.h>
#include <gtsam/discrete/DecisionTree-inl.h>
#include <gtsam/discrete/Ring.h>
#include <algorithm>
#include <iomanip>
#include <limits>
#include <map>
#include <string>
#include <iomanip>
#include <vector>
namespace gtsam {
@ -55,26 +55,6 @@ namespace gtsam {
public:
using Base = DecisionTree<L, double>;
/** The Real ring with addition and multiplication */
struct Ring {
static inline double zero() { return 0.0; }
static inline double one() { return 1.0; }
static inline double add(const double& a, const double& b) {
return a + b;
}
static inline double max(const double& a, const double& b) {
return std::max(a, b);
}
static inline double mul(const double& a, const double& b) {
return a * b;
}
static inline double div(const double& a, const double& b) {
return a / b;
}
static inline double id(const double& x) { return x; }
static inline double negate(const double& x) { return -x; }
};
AlgebraicDecisionTree(double leaf = 1.0) : Base(leaf) {}
// Explicitly non-explicit constructor

142
gtsam/discrete/DecisionTreeFactor.cpp
View File

@ -48,7 +48,7 @@ namespace gtsam {
return false;
} else {
const auto& f(static_cast<const DecisionTreeFactor&>(other));
return ADT::equals(f, tol);
return Base::equals(other, tol) && ADT::equals(f, tol);
}
}
@ -83,7 +83,7 @@ namespace gtsam {
}
/* ************************************************************************ */
DecisionTreeFactor DecisionTreeFactor::apply(ADT::Unary op) const {
DecisionTreeFactor DecisionTreeFactor::apply(Unary op) const {
// apply operand
ADT result = ADT::apply(op);
// Make a new factor
@ -91,7 +91,7 @@ namespace gtsam {
}
/* ************************************************************************ */
DecisionTreeFactor DecisionTreeFactor::apply(ADT::UnaryAssignment op) const {
DecisionTreeFactor DecisionTreeFactor::apply(UnaryAssignment op) const {
// apply operand
ADT result = ADT::apply(op);
// Make a new factor
@ -100,7 +100,7 @@ namespace gtsam {
/* ************************************************************************ */
DecisionTreeFactor DecisionTreeFactor::apply(const DecisionTreeFactor& f,
ADT::Binary op) const {
Binary op) const {
map<Key, size_t> cs; // new cardinalities
// make unique key-cardinality map
for (Key j : keys()) cs[j] = cardinality(j);
@ -118,8 +118,8 @@ namespace gtsam {
}
/* ************************************************************************ */
DecisionTreeFactor::shared_ptr DecisionTreeFactor::combine(
size_t nrFrontals, ADT::Binary op) const {
DecisionTreeFactor::shared_ptr DecisionTreeFactor::combine(size_t nrFrontals,
Binary op) const {
if (nrFrontals > size()) {
throw invalid_argument(
"DecisionTreeFactor::combine: invalid number of frontal "
@ -146,7 +146,7 @@ namespace gtsam {
/* ************************************************************************ */
DecisionTreeFactor::shared_ptr DecisionTreeFactor::combine(
const Ordering& frontalKeys, ADT::Binary op) const {
const Ordering& frontalKeys, Binary op) const {
if (frontalKeys.size() > size()) {
throw invalid_argument(
"DecisionTreeFactor::combine: invalid number of frontal "
@ -195,7 +195,7 @@ namespace gtsam {
// Construct unordered_map with values
std::vector<std::pair<DiscreteValues, double>> result;
for (const auto& assignment : assignments) {
result.emplace_back(assignment, operator()(assignment));
result.emplace_back(assignment, evaluate(assignment));
}
return result;
}
@ -239,7 +239,7 @@ namespace gtsam {
};
// Go through the tree
this->apply(op);
this->visitWith(op);
return probs;
}
@ -349,22 +349,122 @@ namespace gtsam {
: DiscreteFactor(keys.indices(), keys.cardinalities()),
AlgebraicDecisionTree<Key>(keys, table) {}
/**
* @brief Min-Heap class to help with pruning.
* The `top` element is always the smallest value.
*/
class MinHeap {
std::vector<double> v_;
public:
/// Default constructor
MinHeap() {}
/// Push value onto the heap
void push(double x) {
v_.push_back(x);
std::push_heap(v_.begin(), v_.end(), std::greater<double>{});
}
/// Push value `x`, `n` number of times.
void push(double x, size_t n) {
for (size_t i = 0; i < n; ++i) {
v_.push_back(x);
std::push_heap(v_.begin(), v_.end(), std::greater<double>{});
}
}
/// Pop the top value of the heap.
double pop() {
std::pop_heap(v_.begin(), v_.end(), std::greater<double>{});
double x = v_.back();
v_.pop_back();
return x;
}
/// Return the top value of the heap without popping it.
double top() { return v_.at(0); }
/**
* @brief Print the heap as a sequence.
*
* @param s A string to prologue the output.
*/
void print(const std::string& s = "") {
std::cout << (s.empty() ? "" : s + " ");
for (size_t i = 0; i < v_.size(); i++) {
std::cout << v_.at(i);
if (v_.size() > 1 && i < v_.size() - 1) std::cout << ", ";
}
std::cout << std::endl;
}
/// Return true if heap is empty.
bool empty() const { return v_.empty(); }
/// Return the size of the heap.
size_t size() const { return v_.size(); }
};
/* ************************************************************************ */
double DecisionTreeFactor::computeThreshold(const size_t N) const {
// Set of all keys
std::set<Key> allKeys = this->labels();
MinHeap min_heap;
auto op = [&](const Assignment<Key>& a, double p) {
// Get all the keys in the current assignment
std::set<Key> assignment_keys;
for (auto&& [k, _] : a) {
assignment_keys.insert(k);
}
// Find the keys missing in the assignment
std::vector<Key> diff;
std::set_difference(allKeys.begin(), allKeys.end(),
assignment_keys.begin(), assignment_keys.end(),
std::back_inserter(diff));
// Compute the total number of assignments in the (pruned) subtree
size_t nrAssignments = 1;
for (auto&& k : diff) {
nrAssignments *= cardinalities_.at(k);
}
// If min-heap is empty, fill it initially.
// This is because there is nothing at the top.
if (min_heap.empty()) {
min_heap.push(p, std::min(nrAssignments, N));
} else {
for (size_t i = 0; i < std::min(nrAssignments, N); ++i) {
// If p is larger than the smallest element,
// then we insert into the min heap.
// We check against the top each time because the
// heap maintains the smallest element at the top.
if (p > min_heap.top()) {
if (min_heap.size() == N) {
min_heap.pop();
}
min_heap.push(p);
} else {
// p is <= min value so move to the next one
break;
}
}
}
return p;
};
this->visitWith(op);
return min_heap.top();
}
/* ************************************************************************ */
DecisionTreeFactor DecisionTreeFactor::prune(size_t maxNrAssignments) const {
const size_t N = maxNrAssignments;
// Get the probabilities in the decision tree so we can threshold.
std::vector<double> probabilities = this->probabilities();
// The number of probabilities can be lower than max_leaves
if (probabilities.size() <= N) {
return *this;
}
std::sort(probabilities.begin(), probabilities.end(),
std::greater<double>{});
double threshold = probabilities[N - 1];
double threshold = computeThreshold(N);
// Now threshold the decision tree
size_t total = 0;

49
gtsam/discrete/DecisionTreeFactor.h
View File

@ -21,11 +21,12 @@
#include <gtsam/discrete/AlgebraicDecisionTree.h>
#include <gtsam/discrete/DiscreteFactor.h>
#include <gtsam/discrete/DiscreteKey.h>
#include <gtsam/discrete/Ring.h>
#include <gtsam/inference/Ordering.h>
#include <algorithm>
#include <memory>
#include <map>
#include <memory>
#include <stdexcept>
#include <string>
#include <utility>
@ -50,6 +51,11 @@ namespace gtsam {
typedef std::shared_ptr<DecisionTreeFactor> shared_ptr;
typedef AlgebraicDecisionTree<Key> ADT;
// Needed since we have definitions in both DiscreteFactor and DecisionTree
using Base::Binary;
using Base::Unary;
using Base::UnaryAssignment;
/// @name Standard Constructors
/// @{
@ -129,23 +135,21 @@ namespace gtsam {
/// @name Standard Interface
/// @{
/// Calculate probability for given values `x`,
/// Calculate probability for given values,
/// is just look up in AlgebraicDecisionTree.
double evaluate(const Assignment<Key>& values) const {
virtual double evaluate(const Assignment<Key>& values) const override {
return ADT::operator()(values);
}
/// Evaluate probability distribution, sugar.
double operator()(const DiscreteValues& values) const override {
return ADT::operator()(values);
}
/// Disambiguate to use DiscreteFactor version. Mainly for wrapper
using DiscreteFactor::operator();
/// Calculate error for DiscreteValues `x`, is -log(probability).
double error(const DiscreteValues& values) const override;
/// multiply two factors
DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override {
return apply(f, ADT::Ring::mul);
return apply(f, Ring::mul);
}
static double safe_div(const double& a, const double& b);
@ -160,22 +164,22 @@ namespace gtsam {
/// Create new factor by summing all values with the same separator values
shared_ptr sum(size_t nrFrontals) const {
return combine(nrFrontals, ADT::Ring::add);
return combine(nrFrontals, Ring::add);
}
/// Create new factor by summing all values with the same separator values
shared_ptr sum(const Ordering& keys) const {
return combine(keys, ADT::Ring::add);
return combine(keys, Ring::add);
}
/// Create new factor by maximizing over all values with the same separator.
shared_ptr max(size_t nrFrontals) const {
return combine(nrFrontals, ADT::Ring::max);
return combine(nrFrontals, Ring::max);
}
/// Create new factor by maximizing over all values with the same separator.
shared_ptr max(const Ordering& keys) const {
return combine(keys, ADT::Ring::max);
return combine(keys, Ring::max);
}
/// @}
@ -186,21 +190,21 @@ namespace gtsam {
* Apply unary operator (*this) "op" f
* @param op a unary operator that operates on AlgebraicDecisionTree
*/
DecisionTreeFactor apply(ADT::Unary op) const;
DecisionTreeFactor apply(Unary op) const;
/**
* Apply unary operator (*this) "op" f
* @param op a unary operator that operates on AlgebraicDecisionTree. Takes
* both the assignment and the value.
*/
DecisionTreeFactor apply(ADT::UnaryAssignment op) const;
DecisionTreeFactor apply(UnaryAssignment op) const;
/**
* Apply binary operator (*this) "op" f
* @param f the second argument for op
* @param op a binary operator that operates on AlgebraicDecisionTree
*/
DecisionTreeFactor apply(const DecisionTreeFactor& f, ADT::Binary op) const;
DecisionTreeFactor apply(const DecisionTreeFactor& f, Binary op) const;
/**
* Combine frontal variables using binary operator "op"
@ -208,7 +212,7 @@ namespace gtsam {
* @param op a binary operator that operates on AlgebraicDecisionTree
* @return shared pointer to newly created DecisionTreeFactor
*/
shared_ptr combine(size_t nrFrontals, ADT::Binary op) const;
shared_ptr combine(size_t nrFrontals, Binary op) const;
/**
* Combine frontal variables in an Ordering using binary operator "op"
@ -216,7 +220,7 @@ namespace gtsam {
* @param op a binary operator that operates on AlgebraicDecisionTree
* @return shared pointer to newly created DecisionTreeFactor
*/
shared_ptr combine(const Ordering& keys, ADT::Binary op) const;
shared_ptr combine(const Ordering& keys, Binary op) const;
/// Enumerate all values into a map from values to double.
std::vector<std::pair<DiscreteValues, double>> enumerate() const;
@ -224,6 +228,17 @@ namespace gtsam {
/// Get all the probabilities in order of assignment values
std::vector<double> probabilities() const;
/**
* @brief Compute the probability value which is the threshold above which
* only `N` leaves are present.
*
* This is used for pruning out the smaller probabilities.
*
* @param N The number of leaves to keep post pruning.
* @return double
*/
double computeThreshold(const size_t N) const;
/**
* @brief Prune the decision tree of discrete variables.
*

4
gtsam/discrete/DiscreteConditional.cpp
View File

@ -19,6 +19,7 @@
#include <gtsam/base/Testable.h>
#include <gtsam/base/debug.h>
#include <gtsam/discrete/DiscreteConditional.h>
#include <gtsam/discrete/Ring.h>
#include <gtsam/discrete/Signature.h>
#include <gtsam/hybrid/HybridValues.h>
@ -29,6 +30,7 @@
#include <string>
#include <utility>
#include <vector>
#include <cassert>
using namespace std;
using std::pair;
@ -104,7 +106,7 @@ DiscreteConditional DiscreteConditional::operator*(
// Finally, add parents to keys, in order
for (auto&& dk : parents) discreteKeys.push_back(dk);
ADT product = ADT::apply(other, ADT::Ring::mul);
ADT product = ADT::apply(other, Ring::mul);
return DiscreteConditional(newFrontals.size(), discreteKeys, product);
}

10
gtsam/discrete/DiscreteConditional.h
View File

@ -168,13 +168,9 @@ class GTSAM_EXPORT DiscreteConditional
static_cast<const BaseConditional*>(this)->print(s, formatter);
}
/// Evaluate, just look up in AlgebraicDecisionTree
double evaluate(const DiscreteValues& values) const {
return ADT::operator()(values);
}
using DecisionTreeFactor::error; ///< DiscreteValues version
using DecisionTreeFactor::operator(); ///< DiscreteValues version
using BaseFactor::error; ///< DiscreteValues version
using BaseFactor::evaluate; ///< DiscreteValues version
using BaseFactor::operator(); ///< DiscreteValues version
/**
* @brief restrict to given *parent* values.

2
gtsam/discrete/DiscreteDistribution.h
View File

@ -40,7 +40,7 @@ class GTSAM_EXPORT DiscreteDistribution : public DiscreteConditional {
/// Default constructor needed for serialization.
DiscreteDistribution() {}
/// Constructor from factor.
/// Constructor from DecisionTreeFactor.
explicit DiscreteDistribution(const DecisionTreeFactor& f)
: Base(f.size(), f) {}

5
gtsam/discrete/DiscreteFactor.cpp
View File

@ -28,6 +28,11 @@ using namespace std;
namespace gtsam {
/* ************************************************************************* */
bool DiscreteFactor::equals(const DiscreteFactor& lf, double tol) const {
return Base::equals(lf, tol) && cardinalities_ == lf.cardinalities_;
}
/* ************************************************************************ */
DiscreteKeys DiscreteFactor::discreteKeys() const {
DiscreteKeys result;

22
gtsam/discrete/DiscreteFactor.h
View File

@ -46,6 +46,11 @@ class GTSAM_EXPORT DiscreteFactor : public Factor {
using Values = DiscreteValues; ///< backwards compatibility
using Unary = std::function<double(const double&)>;
using UnaryAssignment =
std::function<double(const Assignment<Key>&, const double&)>;
using Binary = std::function<double(const double, const double)>;
protected:
/// Map of Keys and their cardinalities.
std::map<Key, size_t> cardinalities_;
@ -72,7 +77,7 @@ class GTSAM_EXPORT DiscreteFactor : public Factor {
/// @{
/// equals
virtual bool equals(const DiscreteFactor& lf, double tol = 1e-9) const = 0;
virtual bool equals(const DiscreteFactor& lf, double tol = 1e-9) const;
/// print
void print(
@ -92,8 +97,21 @@ class GTSAM_EXPORT DiscreteFactor : public Factor {
size_t cardinality(Key j) const { return cardinalities_.at(j); }
/**
* @brief Calculate probability for given values.
* Calls specialized evaluation under the hood.
*
* Note: Uses Assignment<Key> as it is the base class of DiscreteValues.
*
* @param values Discrete assignment.
* @return double
*/
virtual double evaluate(const Assignment<Key>& values) const = 0;
/// Find value for given assignment of values to variables
virtual double operator()(const DiscreteValues&) const = 0;
double operator()(const DiscreteValues& values) const {
return evaluate(values);
}
/// Error is just -log(value)
virtual double error(const DiscreteValues& values) const;

76
gtsam/discrete/DiscreteFactorGraph.cpp
View File

@ -14,6 +14,7 @@
* @date Feb 14, 2011
* @author Duy-Nguyen Ta
* @author Frank Dellaert
* @author Varun Agrawal
*/
#include <gtsam/discrete/DiscreteBayesTree.h>
@ -35,13 +36,12 @@ namespace gtsam {
template class FactorGraph<DiscreteFactor>;
template class EliminateableFactorGraph<DiscreteFactorGraph>;
/* ************************************************************************* */
bool DiscreteFactorGraph::equals(const This& fg, double tol) const
{
/* ************************************************************************ */
bool DiscreteFactorGraph::equals(const This& fg, double tol) const {
return Base::equals(fg, tol);
}
/* ************************************************************************* */
/* ************************************************************************ */
KeySet DiscreteFactorGraph::keys() const {
KeySet keys;
for (const sharedFactor& factor : *this) {
@ -50,11 +50,11 @@ namespace gtsam {
return keys;
}
/* ************************************************************************* */
/* ************************************************************************ */
DiscreteKeys DiscreteFactorGraph::discreteKeys() const {
DiscreteKeys result;
for (auto&& factor : *this) {
if (auto p = std::dynamic_pointer_cast<DecisionTreeFactor>(factor)) {
if (auto p = std::dynamic_pointer_cast<DiscreteFactor>(factor)) {
DiscreteKeys factor_keys = p->discreteKeys();
result.insert(result.end(), factor_keys.begin(), factor_keys.end());
}
@ -63,26 +63,27 @@ namespace gtsam {
return result;
}
/* ************************************************************************* */
/* ************************************************************************ */
DecisionTreeFactor DiscreteFactorGraph::product() const {
DecisionTreeFactor result;
for(const sharedFactor& factor: *this)
for (const sharedFactor& factor : *this) {
if (factor) result = (*factor) * result;
}
return result;
}
/* ************************************************************************* */
double DiscreteFactorGraph::operator()(
const DiscreteValues &values) const {
/* ************************************************************************ */
double DiscreteFactorGraph::operator()(const DiscreteValues& values) const {
double product = 1.0;
for( const sharedFactor& factor: factors_ )
product *= (*factor)(values);
for (const sharedFactor& factor : factors_) {
if (factor) product *= (*factor)(values);
}
return product;
}
/* ************************************************************************* */
/* ************************************************************************ */
void DiscreteFactorGraph::print(const string& s,
const KeyFormatter& formatter) const {
const KeyFormatter& formatter) const {
std::cout << s << std::endl;
std::cout << "size: " << size() << std::endl;
for (size_t i = 0; i < factors_.size(); i++) {
@ -110,15 +111,18 @@ namespace gtsam {
// }
// }
/* ************************************************************************ */
// Alternate eliminate function for MPE
std::pair<DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr> //
EliminateForMPE(const DiscreteFactorGraph& factors,
const Ordering& frontalKeys) {
/**
* @brief Multiply all the `factors` and normalize the
* product to prevent underflow.
*
* @param factors The factors to multiply as a DiscreteFactorGraph.
* @return DecisionTreeFactor
*/
static DecisionTreeFactor ProductAndNormalize(
const DiscreteFactorGraph& factors) {
// PRODUCT: multiply all factors
gttic(product);
DecisionTreeFactor product;
for (auto&& factor : factors) product = (*factor) * product;
DecisionTreeFactor product = factors.product();
gttoc(product);
// Max over all the potentials by pretending all keys are frontal:
@ -127,6 +131,16 @@ namespace gtsam {
// Normalize the product factor to prevent underflow.
product = product / (*normalization);
return product;
}
/* ************************************************************************ */
// Alternate eliminate function for MPE
std::pair<DiscreteConditional::shared_ptr, DiscreteFactor::shared_ptr> //
EliminateForMPE(const DiscreteFactorGraph& factors,
const Ordering& frontalKeys) {
DecisionTreeFactor product = ProductAndNormalize(factors);
// max out frontals, this is the factor on the separator
gttic(max);
DecisionTreeFactor::shared_ptr max = product.max(frontalKeys);
@ -142,8 +156,8 @@ namespace gtsam {
// Make lookup with product
gttic(lookup);
size_t nrFrontals = frontalKeys.size();
auto lookup = std::make_shared<DiscreteLookupTable>(nrFrontals,
orderedKeys, product);
auto lookup =
std::make_shared<DiscreteLookupTable>(nrFrontals, orderedKeys, product);
gttoc(lookup);
return {std::dynamic_pointer_cast<DiscreteConditional>(lookup), max};
@ -201,20 +215,10 @@ namespace gtsam {
}
/* ************************************************************************ */
std::pair<DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr> //
std::pair<DiscreteConditional::shared_ptr, DiscreteFactor::shared_ptr> //
EliminateDiscrete(const DiscreteFactorGraph& factors,
const Ordering& frontalKeys) {
// PRODUCT: multiply all factors
gttic(product);
DecisionTreeFactor product;
for (auto&& factor : factors) product = (*factor) * product;
gttoc(product);
// Max over all the potentials by pretending all keys are frontal:
auto normalization = product.max(product.size());
// Normalize the product factor to prevent underflow.
product = product / (*normalization);
DecisionTreeFactor product = ProductAndNormalize(factors);
// sum out frontals, this is the factor on the separator
gttic(sum);

5
gtsam/discrete/DiscreteFactorGraph.h
View File

@ -14,6 +14,7 @@
* @date Feb 14, 2011
* @author Duy-Nguyen Ta
* @author Frank Dellaert
* @author Varun Agrawal
*/
#pragma once
@ -48,7 +49,7 @@ class DiscreteJunctionTree;
* @ingroup discrete
*/
GTSAM_EXPORT
std::pair<DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr>
std::pair<DiscreteConditional::shared_ptr, DiscreteFactor::shared_ptr>
EliminateDiscrete(const DiscreteFactorGraph& factors,
const Ordering& frontalKeys);
@ -61,7 +62,7 @@ EliminateDiscrete(const DiscreteFactorGraph& factors,
* @ingroup discrete
*/
GTSAM_EXPORT
std::pair<DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr>
std::pair<DiscreteConditional::shared_ptr, DiscreteFactor::shared_ptr>
EliminateForMPE(const DiscreteFactorGraph& factors,
const Ordering& frontalKeys);

1
gtsam/discrete/DiscreteLookupDAG.cpp
View File

@ -23,6 +23,7 @@
#include <iterator>
#include <string>
#include <utility>
#include <cassert>
using std::pair;
using std::vector;

37
gtsam/discrete/Ring.h Normal file
View File

@ -0,0 +1,37 @@
/* ----------------------------------------------------------------------------
* 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 Ring.h
* @brief Real Ring definition
* @author Varun Agrawal
* @date Dec 8, 2024
*/
#pragma once
#include <algorithm>
/** The Real ring with addition and multiplication */
struct Ring {
static inline double zero() { return 0.0; }
static inline double one() { return 1.0; }
static inline double add(const double& a, const double& b) { return a + b; }
static inline double max(const double& a, const double& b) {
return std::max(a, b);
}
static inline double mul(const double& a, const double& b) { return a * b; }
static inline double div(const double& a, const double& b) {
return (a == 0 || b == 0) ? 0 : (a / b);
}
static inline double id(const double& x) { return x; }
static inline double negate(const double& x) { return -x; }
};

122
gtsam/discrete/TableFactor.cpp
View File

@ -62,35 +62,99 @@ TableFactor::TableFactor(const DiscreteKeys& dkeys,
: TableFactor(dkeys, DecisionTreeFactor(dkeys, dtree)) {}
/**
* @brief Compute the correct ordering of the leaves in the decision tree.
* @brief Compute the indexing of the leaves in the decision tree based on the
* assignment and add the (index, leaf) pair to a SparseVector.
*
* This is done by first taking all the values which have modulo 0 value with
* the cardinality of the innermost key `n`, and we go up to modulo n.
* We visit each leaf in the tree, and using the cardinalities of the keys,
* compute the correct index to add the leaf to a SparseVector which
* is then used to create the TableFactor.
*
* @param dt The DecisionTree
* @return std::vector<double>
* @return Eigen::SparseVector<double>
*/
std::vector<double> ComputeLeafOrdering(const DiscreteKeys& dkeys,
const DecisionTreeFactor& dt) {
std::vector<double> probs = dt.probabilities();
std::vector<double> ordered;
static Eigen::SparseVector<double> ComputeSparseTable(
const DiscreteKeys& dkeys, const DecisionTreeFactor& dt) {
// SparseVector needs to know the maximum possible index,
// so we compute the product of cardinalities.
size_t cardinalityProduct = 1;
for (auto&& [_, c] : dt.cardinalities()) {
cardinalityProduct *= c;
}
Eigen::SparseVector<double> sparseTable(cardinalityProduct);
size_t nrValues = 0;
dt.visit([&nrValues](double x) {
if (x > 0) nrValues += 1;
});
sparseTable.reserve(nrValues);
size_t n = dkeys[0].second;
std::set<Key> allKeys(dt.keys().begin(), dt.keys().end());
for (size_t k = 0; k < n; ++k) {
for (size_t idx = 0; idx < probs.size(); ++idx) {
if (idx % n == k) {
ordered.push_back(probs[idx]);
/**
* @brief Functor which is called by the DecisionTree for each leaf.
* For each leaf value, we use the corresponding assignment to compute a
* corresponding index into a SparseVector. We then populate sparseTable with
* the value at the computed index.
*
* Takes advantage of the sparsity of the DecisionTree to be efficient. When
* merged branches are encountered, we enumerate over the missing keys.
*
*/
auto op = [&](const Assignment<Key>& assignment, double p) {
if (p > 0) {
// Get all the keys involved in this assignment
std::set<Key> assignmentKeys;
for (auto&& [k, _] : assignment) {
assignmentKeys.insert(k);
}
// Find the keys missing in the assignment
std::vector<Key> diff;
std::set_difference(allKeys.begin(), allKeys.end(),
assignmentKeys.begin(), assignmentKeys.end(),
std::back_inserter(diff));
// Generate all assignments using the missing keys
DiscreteKeys extras;
for (auto&& key : diff) {
extras.push_back({key, dt.cardinality(key)});
}
auto&& extraAssignments = DiscreteValues::CartesianProduct(extras);
for (auto&& extra : extraAssignments) {
// Create new assignment using the extra assignment
DiscreteValues updatedAssignment(assignment);
updatedAssignment.insert(extra);
// Generate index and add to the sparse vector.
Eigen::Index idx = 0;
size_t previousCardinality = 1;
// We go in reverse since a DecisionTree has the highest label first
for (auto&& it = updatedAssignment.rbegin();
it != updatedAssignment.rend(); it++) {
idx += previousCardinality * it->second;
previousCardinality *= dt.cardinality(it->first);
}
sparseTable.coeffRef(idx) = p;
}
}
}
return ordered;
};
// Visit each leaf in `dt` to get the Assignment and leaf value
// to populate the sparseTable.
dt.visitWith(op);
return sparseTable;
}
/* ************************************************************************ */
TableFactor::TableFactor(const DiscreteKeys& dkeys,
const DecisionTreeFactor& dtf)
: TableFactor(dkeys, ComputeLeafOrdering(dkeys, dtf)) {}
: TableFactor(dkeys, ComputeSparseTable(dkeys, dtf)) {}
/* ************************************************************************ */
TableFactor::TableFactor(const DecisionTreeFactor& dtf)
: TableFactor(dtf.discreteKeys(),
ComputeSparseTable(dtf.discreteKeys(), dtf)) {}
/* ************************************************************************ */
TableFactor::TableFactor(const DiscreteConditional& c)
@ -98,7 +162,17 @@ TableFactor::TableFactor(const DiscreteConditional& c)
/* ************************************************************************ */
Eigen::SparseVector<double> TableFactor::Convert(
const std::vector<double>& table) {
const DiscreteKeys& keys, const std::vector<double>& table) {
size_t max_size = 1;
for (auto&& [_, cardinality] : keys.cardinalities()) {
max_size *= cardinality;
}
if (table.size() != max_size) {
throw std::runtime_error(
"The cardinalities of the keys don't match the number of values in the "
"input.");
}
Eigen::SparseVector<double> sparse_table(table.size());
// Count number of nonzero elements in table and reserve the space.
const uint64_t nnz = std::count_if(table.begin(), table.end(),
@ -113,13 +187,14 @@ Eigen::SparseVector<double> TableFactor::Convert(
}
/* ************************************************************************ */
Eigen::SparseVector<double> TableFactor::Convert(const std::string& table) {
Eigen::SparseVector<double> TableFactor::Convert(const DiscreteKeys& keys,
const std::string& table) {
// Convert string to doubles.
std::vector<double> ys;
std::istringstream iss(table);
std::copy(std::istream_iterator<double>(iss), std::istream_iterator<double>(),
std::back_inserter(ys));
return Convert(ys);
return Convert(keys, ys);
}
/* ************************************************************************ */
@ -128,12 +203,13 @@ bool TableFactor::equals(const DiscreteFactor& other, double tol) const {
return false;
} else {
const auto& f(static_cast<const TableFactor&>(other));
return sparse_table_.isApprox(f.sparse_table_, tol);
return Base::equals(other, tol) &&
sparse_table_.isApprox(f.sparse_table_, tol);
}
}
/* ************************************************************************ */
double TableFactor::operator()(const DiscreteValues& values) const {
double TableFactor::evaluate(const Assignment<Key>& values) const {
// a b c d => D * (C * (B * (a) + b) + c) + d
uint64_t idx = 0, card = 1;
for (auto it = sorted_dkeys_.rbegin(); it != sorted_dkeys_.rend(); ++it) {
@ -180,6 +256,7 @@ DecisionTreeFactor TableFactor::toDecisionTreeFactor() const {
for (auto i = 0; i < sparse_table_.size(); i++) {
table.push_back(sparse_table_.coeff(i));
}
// NOTE(Varun): This constructor is really expensive!!
DecisionTreeFactor f(dkeys, table);
return f;
}
@ -249,7 +326,8 @@ void TableFactor::print(const string& s, const KeyFormatter& formatter) const {
for (auto&& kv : assignment) {
cout << "(" << formatter(kv.first) << ", " << kv.second << ")";
}
cout << " | " << it.value() << " | " << it.index() << endl;
cout << " | " << std::setw(10) << std::left << it.value() << " | "
<< it.index() << endl;
}
cout << "number of nnzs: " << sparse_table_.nonZeros() << endl;
}

47
gtsam/discrete/TableFactor.h
View File

@ -19,6 +19,7 @@
#include <gtsam/discrete/DiscreteFactor.h>
#include <gtsam/discrete/DiscreteKey.h>
#include <gtsam/discrete/Ring.h>
#include <gtsam/inference/Ordering.h>
#include <Eigen/Sparse>
@ -79,12 +80,16 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor {
return DiscreteKey(keys_[i], cardinalities_.at(keys_[i]));
}
/// Convert probability table given as doubles to SparseVector.
/// Example) {0, 1, 1, 0, 0, 1, 0} -> values: {1, 1, 1}, indices: {1, 2, 5}
static Eigen::SparseVector<double> Convert(const std::vector<double>& table);
/**
* Convert probability table given as doubles to SparseVector.
* Example: {0, 1, 1, 0, 0, 1, 0} -> values: {1, 1, 1}, indices: {1, 2, 5}
*/
static Eigen::SparseVector<double> Convert(const DiscreteKeys& keys,
const std::vector<double>& table);
/// Convert probability table given as string to SparseVector.
static Eigen::SparseVector<double> Convert(const std::string& table);
static Eigen::SparseVector<double> Convert(const DiscreteKeys& keys,
const std::string& table);
public:
// typedefs needed to play nice with gtsam
@ -93,27 +98,8 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor {
typedef std::shared_ptr<TableFactor> shared_ptr;
typedef Eigen::SparseVector<double>::InnerIterator SparseIt;
typedef std::vector<std::pair<DiscreteValues, double>> AssignValList;
using Unary = std::function<double(const double&)>;
using UnaryAssignment =
std::function<double(const Assignment<Key>&, const double&)>;
using Binary = std::function<double(const double, const double)>;
public:
/** The Real ring with addition and multiplication */
struct Ring {
static inline double zero() { return 0.0; }
static inline double one() { return 1.0; }
static inline double add(const double& a, const double& b) { return a + b; }
static inline double max(const double& a, const double& b) {
return std::max(a, b);
}
static inline double mul(const double& a, const double& b) { return a * b; }
static inline double div(const double& a, const double& b) {
return (a == 0 || b == 0) ? 0 : (a / b);
}
static inline double id(const double& x) { return x; }
};
/// @name Standard Constructors
/// @{
@ -129,11 +115,11 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor {
/** Constructor from doubles */
TableFactor(const DiscreteKeys& keys, const std::vector<double>& table)
: TableFactor(keys, Convert(table)) {}
: TableFactor(keys, Convert(keys, table)) {}
/** Constructor from string */
TableFactor(const DiscreteKeys& keys, const std::string& table)
: TableFactor(keys, Convert(table)) {}
: TableFactor(keys, Convert(keys, table)) {}
/// Single-key specialization
template <class SOURCE>
@ -146,6 +132,7 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor {
/// Constructor from DecisionTreeFactor
TableFactor(const DiscreteKeys& keys, const DecisionTreeFactor& dtf);
TableFactor(const DecisionTreeFactor& dtf);
/// Constructor from DecisionTree<Key, double>/AlgebraicDecisionTree
TableFactor(const DiscreteKeys& keys, const DecisionTree<Key, double>& dtree);
@ -169,14 +156,8 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor {
// /// @name Standard Interface
// /// @{
/// Calculate probability for given values `x`,
/// is just look up in TableFactor.
double evaluate(const DiscreteValues& values) const {
return operator()(values);
}
/// Evaluate probability distribution, sugar.
double operator()(const DiscreteValues& values) const override;
/// Evaluate probability distribution, is just look up in TableFactor.
double evaluate(const Assignment<Key>& values) const override;
/// Calculate error for DiscreteValues `x`, is -log(probability).
double error(const DiscreteValues& values) const override;

4
gtsam/discrete/discrete.i
View File

@ -61,14 +61,14 @@ virtual class DecisionTreeFactor : gtsam::DiscreteFactor {
DecisionTreeFactor(const std::vector<gtsam::DiscreteKey>& keys, string table);
DecisionTreeFactor(const gtsam::DiscreteConditional& c);
void print(string s = "DecisionTreeFactor\n",
const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
bool equals(const gtsam::DecisionTreeFactor& other, double tol = 1e-9) const;
size_t cardinality(gtsam::Key j) const;
double operator()(const gtsam::DiscreteValues& values) const;
gtsam::DecisionTreeFactor operator*(const gtsam::DecisionTreeFactor& f) const;
size_t cardinality(gtsam::Key j) const;

9
gtsam/discrete/tests/testDecisionTree.cpp
View File

@ -23,6 +23,7 @@
#include <gtsam/base/Testable.h>
#include <gtsam/base/serializationTestHelpers.h>
#include <gtsam/discrete/DecisionTree-inl.h>
#include <gtsam/discrete/Ring.h>
#include <gtsam/discrete/Signature.h>
#include <gtsam/inference/Symbol.h>
@ -124,14 +125,6 @@ struct traits<DT> : public Testable<DT> {};
GTSAM_CONCEPT_TESTABLE_INST(DT)
struct Ring {
static inline int zero() { return 0; }
static inline int one() { return 1; }
static inline int id(const int& a) { return a; }
static inline int add(const int& a, const int& b) { return a + b; }
static inline int mul(const int& a, const int& b) { return a * b; }
};
/* ************************************************************************** */
// Check that creating decision trees respects key order.
TEST(DecisionTree, ConstructorOrder) {

45
gtsam/discrete/tests/testDecisionTreeFactor.cpp
View File

@ -140,11 +140,46 @@ TEST(DecisionTreeFactor, enumerate) {
EXPECT(actual == expected);
}
namespace pruning_fixture {
DiscreteKey A(1, 2), B(2, 2), C(3, 2);
DecisionTreeFactor f(A& B& C, "1 5 3 7 2 6 4 8");
DiscreteKey D(4, 2);
DecisionTreeFactor factor(
D& C & B & A,
"0.0 0.0 0.0 0.60658897 0.61241912 0.61241969 0.61247685 0.61247742 0.0 "
"0.0 0.0 0.99995287 1.0 1.0 1.0 1.0");
} // namespace pruning_fixture
/* ************************************************************************* */
// Check if computing the correct threshold works.
TEST(DecisionTreeFactor, ComputeThreshold) {
using namespace pruning_fixture;
// Only keep the leaves with the top 5 values.
double threshold = f.computeThreshold(5);
EXPECT_DOUBLES_EQUAL(4.0, threshold, 1e-9);
// Check for more extreme pruning where we only keep the top 2 leaves
threshold = f.computeThreshold(2);
EXPECT_DOUBLES_EQUAL(7.0, threshold, 1e-9);
threshold = factor.computeThreshold(5);
EXPECT_DOUBLES_EQUAL(0.99995287, threshold, 1e-9);
threshold = factor.computeThreshold(3);
EXPECT_DOUBLES_EQUAL(1.0, threshold, 1e-9);
threshold = factor.computeThreshold(6);
EXPECT_DOUBLES_EQUAL(0.61247742, threshold, 1e-9);
}
/* ************************************************************************* */
// Check pruning of the decision tree works as expected.
TEST(DecisionTreeFactor, Prune) {
DiscreteKey A(1, 2), B(2, 2), C(3, 2);
DecisionTreeFactor f(A & B & C, "1 5 3 7 2 6 4 8");
using namespace pruning_fixture;
// Only keep the leaves with the top 5 values.
size_t maxNrAssignments = 5;
@ -160,12 +195,6 @@ TEST(DecisionTreeFactor, Prune) {
DecisionTreeFactor expected2(A & B & C, "0 0 0 7 0 0 0 8");
EXPECT(assert_equal(expected2, pruned2));
DiscreteKey D(4, 2);
DecisionTreeFactor factor(
D & C & B & A,
"0.0 0.0 0.0 0.60658897 0.61241912 0.61241969 0.61247685 0.61247742 0.0 "
"0.0 0.0 0.99995287 1.0 1.0 1.0 1.0");
DecisionTreeFactor expected3(D & C & B & A,
"0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 "
"0.999952870000 1.0 1.0 1.0 1.0");

3
gtsam/discrete/tests/testDiscreteFactorGraph.cpp
View File

@ -113,7 +113,8 @@ TEST(DiscreteFactorGraph, test) {
const Ordering frontalKeys{0};
const auto [conditional, newFactorPtr] = EliminateDiscrete(graph, frontalKeys);
DecisionTreeFactor newFactor = *newFactorPtr;
DecisionTreeFactor newFactor =
*std::dynamic_pointer_cast<DecisionTreeFactor>(newFactorPtr);
// Normalize newFactor by max for comparison with expected
auto normalization = newFactor.max(newFactor.size());

41
gtsam/discrete/tests/testTableFactor.cpp
View File

@ -134,14 +134,45 @@ TEST(TableFactor, constructors) {
EXPECT(assert_equal(expected, f4));
// Test for 9=3x3 values.
DiscreteKey V(0, 3), W(1, 3);
DiscreteKey V(0, 3), W(1, 3), O(100, 3);
DiscreteConditional conditional5(V | W = "1/2/3 5/6/7 9/10/11");
TableFactor f5(conditional5);
// GTSAM_PRINT(f5);
TableFactor expected_f5(
X & Y,
"0.166667 0.277778 0.3 0.333333 0.333333 0.333333 0.5 0.388889 0.366667");
std::string expected_values =
"0.166667 0.277778 0.3 0.333333 0.333333 0.333333 0.5 0.388889 0.366667";
TableFactor expected_f5(V & W, expected_values);
EXPECT(assert_equal(expected_f5, f5, 1e-6));
TableFactor f5_with_wrong_keys(V & O, expected_values);
EXPECT(assert_inequal(f5_with_wrong_keys, f5, 1e-9));
}
/* ************************************************************************* */
// Check conversion from DecisionTreeFactor.
TEST(TableFactor, Conversion) {
/* This is the DecisionTree we are using
Choice(m2)
0 Choice(m1)
0 0 Leaf 0
0 1 Choice(m0)
0 1 0 Leaf 0
0 1 1 Leaf 0.14649446 // 3
1 Choice(m1)
1 0 Choice(m0)
1 0 0 Leaf 0
1 0 1 Leaf 0.14648756 // 5
1 1 Choice(m0)
1 1 0 Leaf 0.14649446 // 6
1 1 1 Leaf 0.23918345 // 7
*/
DiscreteKeys dkeys = {{0, 2}, {1, 2}, {2, 2}};
DecisionTreeFactor dtf(
dkeys, std::vector<double>{0, 0, 0, 0.14649446, 0, 0.14648756, 0.14649446,
0.23918345});
TableFactor tf(dtf.discreteKeys(), dtf);
EXPECT(assert_equal(dtf, tf.toDecisionTreeFactor()));
}
/* ************************************************************************* */

10
gtsam/geometry/BearingRange.h
View File

@ -55,9 +55,9 @@ private:
R range_;
public:
enum { dimB = traits<B>::dimension };
enum { dimR = traits<R>::dimension };
enum { dimension = dimB + dimR };
constexpr static const size_t dimB = traits<B>::dimension;
constexpr static const size_t dimR = traits<R>::dimension;
constexpr static const size_t dimension = dimB + dimR;
/// @name Standard Constructors
/// @{
@ -162,9 +162,7 @@ private:
/// @}
// Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
enum {
NeedsToAlign = (sizeof(B) % 16) == 0 || (sizeof(R) % 16) == 0
};
inline constexpr static auto NeedsToAlign = (sizeof(B) % 16) == 0 || (sizeof(R) % 16) == 0;
public:
GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
};

2
gtsam/geometry/Cal3.h
View File

@ -73,7 +73,7 @@ class GTSAM_EXPORT Cal3 {
double u0_ = 0.0f, v0_ = 0.0f; ///< principal point
public:
enum { dimension = 5 };
inline constexpr static auto dimension = 5;
///< shared pointer to calibration object
using shared_ptr = std::shared_ptr<Cal3>;

2
gtsam/geometry/Cal3Bundler.h
View File

@ -37,7 +37,7 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3f {
// Note: u0 and v0 are constants and not optimized.
public:
enum { dimension = 3 };
inline constexpr static auto dimension = 3;
using shared_ptr = std::shared_ptr<Cal3Bundler>;
/// @name Constructors

2
gtsam/geometry/Cal3DS2.h
View File

@ -36,7 +36,7 @@ class GTSAM_EXPORT Cal3DS2 : public Cal3DS2_Base {
using Base = Cal3DS2_Base;
public:
enum { dimension = 9 };
inline constexpr static auto dimension = 9;
///< shared pointer to stereo calibration object
using shared_ptr = std::shared_ptr<Cal3DS2>;

2
gtsam/geometry/Cal3DS2_Base.h
View File

@ -46,7 +46,7 @@ class GTSAM_EXPORT Cal3DS2_Base : public Cal3 {
double tol_ = 1e-5; ///< tolerance value when calibrating
public:
enum { dimension = 9 };
inline constexpr static auto dimension = 9;
///< shared pointer to stereo calibration object
using shared_ptr = std::shared_ptr<Cal3DS2_Base>;

2
gtsam/geometry/Cal3Fisheye.h
View File

@ -55,7 +55,7 @@ class GTSAM_EXPORT Cal3Fisheye : public Cal3 {
double tol_ = 1e-5; ///< tolerance value when calibrating
public:
enum { dimension = 9 };
inline constexpr static auto dimension = 9;
///< shared pointer to fisheye calibration object
using shared_ptr = std::shared_ptr<Cal3Fisheye>;

2
gtsam/geometry/Cal3Unified.h
View File

@ -50,7 +50,7 @@ class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base {
double xi_ = 0.0f; ///< mirror parameter
public:
enum { dimension = 10 };
inline constexpr static auto dimension = 10;
///< shared pointer to stereo calibration object
using shared_ptr = std::shared_ptr<Cal3Unified>;

2
gtsam/geometry/Cal3_S2.h
View File

@ -33,7 +33,7 @@ namespace gtsam {
*/
class GTSAM_EXPORT Cal3_S2 : public Cal3 {
public:
enum { dimension = 5 };
inline constexpr static auto dimension = 5;
///< shared pointer to calibration object
using shared_ptr = std::shared_ptr<Cal3_S2>;

2
gtsam/geometry/Cal3_S2Stereo.h
View File

@ -32,7 +32,7 @@ class GTSAM_EXPORT Cal3_S2Stereo : public Cal3_S2 {
double b_ = 1.0f; ///< Stereo baseline.
public:
enum { dimension = 6 };
inline constexpr static auto dimension = 6;
///< shared pointer to stereo calibration object
using shared_ptr = std::shared_ptr<Cal3_S2Stereo>;

1
gtsam/geometry/Cal3f.cpp
View File

@ -28,6 +28,7 @@
#include <gtsam/geometry/Cal3f.h>
#include <iostream>
#include <cassert>
namespace gtsam {

2
gtsam/geometry/Cal3f.h
View File

@ -34,7 +34,7 @@ namespace gtsam {
*/
class GTSAM_EXPORT Cal3f : public Cal3 {
public:
enum { dimension = 1 };
inline constexpr static auto dimension = 1;
using shared_ptr = std::shared_ptr<Cal3f>;
/// @name Constructors

4
gtsam/geometry/CalibratedCamera.h
View File

@ -252,9 +252,7 @@ class GTSAM_EXPORT CalibratedCamera: public PinholeBase {
public:
enum {
dimension = 6
};
inline constexpr static auto dimension = 6;
/// @name Standard Constructors
/// @{

1
gtsam/geometry/CameraSet.h
View File

@ -26,6 +26,7 @@
#include <gtsam/inference/Key.h>
#include <vector>
#include <cassert>
namespace gtsam {

2
gtsam/geometry/EssentialMatrix.h
View File

@ -82,7 +82,7 @@ class EssentialMatrix {
/// @name Manifold
/// @{
enum { dimension = 5 };
inline constexpr static auto dimension = 5;
inline static size_t Dim() { return dimension;}
inline size_t dim() const { return dimension;}

28
gtsam/geometry/FundamentalMatrix.cpp
View File

@ -74,6 +74,20 @@ Matrix3 FundamentalMatrix::matrix() const {
V_.transpose().matrix();
}
Vector3 FundamentalMatrix::epipolarLine(const Point2& p,
OptionalJacobian<3, 7> H) {
Vector3 point(p.x(), p.y(), 1); // Create a point in homogeneous coordinates
Vector3 line = matrix() * point; // Compute the epipolar line
if (H) {
// Compute the Jacobian if requested
throw std::runtime_error(
"FundamentalMatrix::epipolarLine: Jacobian not implemented yet.");
}
return line; // Return the epipolar line
}
void FundamentalMatrix::print(const std::string& s) const {
std::cout << s << matrix() << std::endl;
}
@ -116,6 +130,20 @@ Matrix3 SimpleFundamentalMatrix::matrix() const {
return Ka().transpose().inverse() * E_.matrix() * Kb().inverse();
}
Vector3 SimpleFundamentalMatrix::epipolarLine(const Point2& p,
OptionalJacobian<3, 7> H) {
Vector3 point(p.x(), p.y(), 1); // Create a point in homogeneous coordinates
Vector3 line = matrix() * point; // Compute the epipolar line
if (H) {
// Compute the Jacobian if requested
throw std::runtime_error(
"SimpleFundamentalMatrix::epipolarLine: Jacobian not implemented yet.");
}
return line; // Return the epipolar line
}
void SimpleFundamentalMatrix::print(const std::string& s) const {
std::cout << s << " E:\n"
<< E_.matrix() << "\nfa: " << fa_ << "\nfb: " << fb_

11
gtsam/geometry/FundamentalMatrix.h
View File

@ -7,6 +7,7 @@
#pragma once
#include <gtsam/base/OptionalJacobian.h>
#include <gtsam/geometry/EssentialMatrix.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/Unit3.h>
@ -86,6 +87,9 @@ class GTSAM_EXPORT FundamentalMatrix {
/// Return the fundamental matrix representation
Matrix3 matrix() const;
/// Computes the epipolar line in a (left) for a given point in b (right)
Vector3 epipolarLine(const Point2& p, OptionalJacobian<3, 7> H = {});
/// @name Testable
/// @{
/// Print the FundamentalMatrix
@ -98,7 +102,7 @@ class GTSAM_EXPORT FundamentalMatrix {
/// @name Manifold
/// @{
enum { dimension = 7 }; // 3 for U, 1 for s, 3 for V
inline constexpr static auto dimension = 7; // 3 for U, 1 for s, 3 for V
inline static size_t Dim() { return dimension; }
inline size_t dim() const { return dimension; }
@ -161,6 +165,9 @@ class GTSAM_EXPORT SimpleFundamentalMatrix {
/// F = Ka^(-T) * E * Kb^(-1)
Matrix3 matrix() const;
/// Computes the epipolar line in a (left) for a given point in b (right)
Vector3 epipolarLine(const Point2& p, OptionalJacobian<3, 7> H = {});
/// @name Testable
/// @{
/// Print the SimpleFundamentalMatrix
@ -172,7 +179,7 @@ class GTSAM_EXPORT SimpleFundamentalMatrix {
/// @name Manifold
/// @{
enum { dimension = 7 }; // 5 for E, 1 for fa, 1 for fb
inline constexpr static auto dimension = 7; // 5 for E, 1 for fa, 1 for fb
inline static size_t Dim() { return dimension; }
inline size_t dim() const { return dimension; }

2
gtsam/geometry/Line3.h
View File

@ -47,7 +47,7 @@ class GTSAM_EXPORT Line3 {
double a_, b_; // Intersection of line with the world x-y plane rotated by R_
// Also the closest point on line to origin
public:
enum { dimension = 4 };
inline constexpr static auto dimension = 4;
/** Default constructor is the Z axis **/
Line3() :

4
gtsam/geometry/OrientedPlane3.h
View File

@ -39,9 +39,7 @@ private:
double d_; ///< The perpendicular distance to this plane
public:
enum {
dimension = 3
};
inline constexpr static auto dimension = 3;
/// @name Constructors
/// @{

4
gtsam/geometry/PinholeCamera.h
View File

@ -51,9 +51,7 @@ private:
public:
enum {
dimension = 6 + DimK
}; ///< Dimension depends on calibration
inline constexpr static auto dimension = 6 + DimK; ///< Dimension depends on calibration
/// @name Standard Constructors
/// @{

4
gtsam/geometry/PinholePose.h
View File

@ -245,9 +245,7 @@ private:
public:
enum {
dimension = 6
}; ///< There are 6 DOF to optimize for
inline constexpr static auto dimension = 6; ///< There are 6 DOF to optimize for
/// @name Standard Constructors
/// @{

10
gtsam/geometry/Point3.cpp
View File

@ -69,6 +69,16 @@ Point3 cross(const Point3 &p, const Point3 &q, OptionalJacobian<3, 3> H1,
p.x() * q.y() - p.y() * q.x());
}
Point3 doubleCross(const Point3 &p, const Point3 &q, //
OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) {
if (H1) *H1 = q.dot(p) * I_3x3 + p * q.transpose() - 2 * q * p.transpose();
if (H2) {
const Matrix3 W = skewSymmetric(p);
*H2 = W * W;
}
return gtsam::cross(p, gtsam::cross(p, q));
}
double dot(const Point3 &p, const Point3 &q, OptionalJacobian<1, 3> H1,
OptionalJacobian<1, 3> H2) {
if (H1) *H1 << q.x(), q.y(), q.z();

7
gtsam/geometry/Point3.h
View File

@ -55,11 +55,16 @@ GTSAM_EXPORT double norm3(const Point3& p, OptionalJacobian<1, 3> H = {});
/// normalize, with optional Jacobian
GTSAM_EXPORT Point3 normalize(const Point3& p, OptionalJacobian<3, 3> H = {});
/// cross product @return this x q
/// cross product @return p x q
GTSAM_EXPORT Point3 cross(const Point3& p, const Point3& q,
OptionalJacobian<3, 3> H_p = {},
OptionalJacobian<3, 3> H_q = {});
/// double cross product @return p x (p x q)
GTSAM_EXPORT Point3 doubleCross(const Point3& p, const Point3& q,
OptionalJacobian<3, 3> H1 = {},
OptionalJacobian<3, 3> H2 = {});
/// dot product
GTSAM_EXPORT double dot(const Point3& p, const Point3& q,
OptionalJacobian<1, 3> H_p = {},

1
gtsam/geometry/Pose2.cpp
View File

@ -20,6 +20,7 @@
#include <gtsam/base/concepts.h>
#include <cmath>
#include <cassert>
#include <iostream>
#include <iomanip>

10
gtsam/geometry/Pose2.h
View File

@ -81,9 +81,13 @@ public:
Pose2(const Rot2& r, const Point2& t) : r_(r), t_(t) {}
/** Constructor from 3*3 matrix */
Pose2(const Matrix &T) :
r_(Rot2::atan2(T(1, 0), T(0, 0))), t_(T(0, 2), T(1, 2)) {
assert(T.rows() == 3 && T.cols() == 3);
Pose2(const Matrix &T)
: r_(Rot2::atan2(T(1, 0), T(0, 0))), t_(T(0, 2), T(1, 2)) {
#ifndef NDEBUG
if (T.rows() != 3 || T.cols() != 3) {
throw;
}
#endif
}
/// @}

137
gtsam/geometry/Pose3.cpp
View File

@ -11,23 +11,21 @@
/**
* @file Pose3.cpp
* @brief 3D Pose
* @brief 3D Pose manifold SO(3) x R^3 and group SE(3)
*/
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/concepts.h>
#include <gtsam/base/concepts.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/concepts.h>
#include <cmath>
#include <iostream>
#include <limits>
#include <string>
namespace gtsam {
using std::vector;
/** instantiate concept checks */
GTSAM_CONCEPT_POSE_INST(Pose3)
@ -114,7 +112,7 @@ Matrix6 Pose3::adjointMap(const Vector6& xi) {
/* ************************************************************************* */
Vector6 Pose3::adjoint(const Vector6& xi, const Vector6& y,
OptionalJacobian<6, 6> Hxi, OptionalJacobian<6, 6> H_y) {
OptionalJacobian<6, 6> Hxi, OptionalJacobian<6, 6> H_y) {
if (Hxi) {
Hxi->setZero();
for (int i = 0; i < 6; ++i) {
@ -161,27 +159,31 @@ bool Pose3::equals(const Pose3& pose, double tol) const {
/* ************************************************************************* */
Pose3 Pose3::interpolateRt(const Pose3& T, double t) const {
return Pose3(interpolate<Rot3>(R_, T.R_, t),
interpolate<Point3>(t_, T.t_, t));
interpolate<Point3>(t_, T.t_, t));
}
/* ************************************************************************* */
/** Modified from Murray94book version (which assumes w and v normalized?) */
// Expmap is implemented in so3::ExpmapFunctor::expmap, based on Ethan Eade's
// elegant Lie group document, at https://www.ethaneade.org/lie.pdf.
Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi) {
if (Hxi) *Hxi = ExpmapDerivative(xi);
// Get angular velocity omega and translational velocity v from twist xi
const Vector3 w = xi.head<3>(), v = xi.tail<3>();
// get angular velocity omega and translational velocity v from twist xi
Vector3 omega(xi(0), xi(1), xi(2)), v(xi(3), xi(4), xi(5));
// Compute rotation using Expmap
Matrix3 Jr;
Rot3 R = Rot3::Expmap(w, Hxi ? &Jr : nullptr);
Rot3 R = Rot3::Expmap(omega);
double theta2 = omega.dot(omega);
if (theta2 > std::numeric_limits<double>::epsilon()) {
Vector3 t_parallel = omega * omega.dot(v); // translation parallel to axis
Vector3 omega_cross_v = omega.cross(v); // points towards axis
Vector3 t = (omega_cross_v - R * omega_cross_v + t_parallel) / theta2;
return Pose3(R, t);
} else {
return Pose3(R, v);
// Compute translation and optionally its Jacobian Q in w
// The Jacobian in v is the right Jacobian Jr of SO(3), which we already have.
Matrix3 Q;
const Vector3 t = ExpmapTranslation(w, v, Hxi ? &Q : nullptr);
if (Hxi) {
*Hxi << Jr, Z_3x3, //
Q, Jr;
}
return Pose3(R, t);
}
/* ************************************************************************* */
@ -240,55 +242,55 @@ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& pose, ChartJacobian Hpose) {
}
/* ************************************************************************* */
Matrix3 Pose3::ComputeQforExpmapDerivative(const Vector6& xi, double nearZeroThreshold) {
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);
const Matrix3 W = skewSymmetric(w);
Matrix3 Q;
#ifdef NUMERICAL_EXPMAP_DERIV
Matrix3 Qj = Z_3x3;
double invFac = 1.0;
Q = Z_3x3;
Matrix3 Wj = I_3x3;
for (size_t j=1; j<10; ++j) {
Qj = Qj*W + Wj*V;
invFac = -invFac/(j+1);
Q = Q + invFac*Qj;
Wj = Wj*W;
}
#else
// The closed-form formula in Barfoot14tro eq. (102)
double phi = w.norm();
const Matrix3 WVW = W * V * W;
if (std::abs(phi) > nearZeroThreshold) {
const double s = sin(phi), c = 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
Q = -0.5 * V + (phi - s) / phi3 * (W * V + V * W - WVW) +
(1 - phi2 / 2 - c) / phi4 * (W * W * V + V * W * W - 3 * WVW) -
0.5 * ((1 - phi2 / 2 - c) / phi4 - 3 * (phi - s - phi3 / 6.) / phi5) *
(WVW * W + W * WVW);
} else {
Q = -0.5 * V + 1. / 6. * (W * V + V * W - WVW) -
1. / 24. * (W * W * V + V * W * W - 3 * WVW) +
1. / 120. * (WVW * W + W * WVW);
}
#endif
ExpmapTranslation(w, v, Q, {}, nearZeroThreshold);
return Q;
}
/* ************************************************************************* */
// NOTE(Frank): t = applyLeftJacobian(v) does the same as the intuitive formulas
// t_parallel = w * w.dot(v); // translation parallel to axis
// w_cross_v = w.cross(v); // translation orthogonal to axis
// t = (w_cross_v - Rot3::Expmap(w) * w_cross_v + t_parallel) / theta2;
// but functor does not need R, deals automatically with the case where theta2
// is near zero, and also gives us the machinery for the Jacobians.
Vector3 Pose3::ExpmapTranslation(const Vector3& w, const Vector3& v,
OptionalJacobian<3, 3> Q,
OptionalJacobian<3, 3> J,
double nearZeroThreshold) {
const double theta2 = w.dot(w);
bool nearZero = (theta2 <= nearZeroThreshold);
// Instantiate functor for Dexp-related operations:
so3::DexpFunctor local(w, nearZero);
// Call applyLeftJacobian which is faster than local.leftJacobian() * v if you
// don't need Jacobians, and returns Jacobian of t with respect to w if asked.
Matrix3 H;
Vector t = local.applyLeftJacobian(v, Q ? &H : nullptr);
// We return Jacobians for use in Expmap, so we multiply with X, that
// translates from left to right for our right expmap convention:
if (Q) {
Matrix3 X = local.rightJacobian() * local.leftJacobianInverse();
*Q = X * H;
}
if (J) {
*J = local.rightJacobian(); // = X * local.leftJacobian();
}
return t;
}
/* ************************************************************************* */
Matrix6 Pose3::ExpmapDerivative(const Vector6& xi) {
const Vector3 w = xi.head<3>();
const Matrix3 Jw = Rot3::ExpmapDerivative(w);
const Matrix3 Q = ComputeQforExpmapDerivative(xi);
Matrix6 J;
J << Jw, Z_3x3, Q, Jw;
Expmap(xi, J);
return J;
}
@ -311,7 +313,6 @@ const Point3& Pose3::translation(OptionalJacobian<3, 6> Hself) const {
}
/* ************************************************************************* */
const Rot3& Pose3::rotation(OptionalJacobian<3, 6> Hself) const {
if (Hself) {
*Hself << I_3x3, Z_3x3;
@ -329,14 +330,14 @@ Matrix4 Pose3::matrix() const {
/* ************************************************************************* */
Pose3 Pose3::transformPoseFrom(const Pose3& aTb, OptionalJacobian<6, 6> Hself,
OptionalJacobian<6, 6> HaTb) const {
OptionalJacobian<6, 6> HaTb) const {
const Pose3& wTa = *this;
return wTa.compose(aTb, Hself, HaTb);
}
/* ************************************************************************* */
Pose3 Pose3::transformPoseTo(const Pose3& wTb, OptionalJacobian<6, 6> Hself,
OptionalJacobian<6, 6> HwTb) const {
OptionalJacobian<6, 6> HwTb) const {
if (Hself) *Hself = -wTb.inverse().AdjointMap() * AdjointMap();
if (HwTb) *HwTb = I_6x6;
const Pose3& wTa = *this;
@ -345,7 +346,7 @@ Pose3 Pose3::transformPoseTo(const Pose3& wTb, OptionalJacobian<6, 6> Hself,
/* ************************************************************************* */
Point3 Pose3::transformFrom(const Point3& point, OptionalJacobian<3, 6> Hself,
OptionalJacobian<3, 3> Hpoint) const {
OptionalJacobian<3, 3> Hpoint) const {
// Only get matrix once, to avoid multiple allocations,
// as well as multiple conversions in the Quaternion case
const Matrix3 R = R_.matrix();
@ -369,7 +370,7 @@ Matrix Pose3::transformFrom(const Matrix& points) const {
/* ************************************************************************* */
Point3 Pose3::transformTo(const Point3& point, OptionalJacobian<3, 6> Hself,
OptionalJacobian<3, 3> Hpoint) const {
OptionalJacobian<3, 3> Hpoint) const {
// Only get transpose once, to avoid multiple allocations,
// as well as multiple conversions in the Quaternion case
const Matrix3 Rt = R_.transpose();
@ -475,7 +476,7 @@ std::optional<Pose3> Pose3::Align(const Point3Pairs &abPointPairs) {
std::optional<Pose3> Pose3::Align(const Matrix& a, const Matrix& b) {
if (a.rows() != 3 || b.rows() != 3 || a.cols() != b.cols()) {
throw std::invalid_argument(
"Pose3:Align expects 3*N matrices of equal shape.");
"Pose3:Align expects 3*N matrices of equal shape.");
}
Point3Pairs abPointPairs;
for (Eigen::Index j = 0; j < a.cols(); j++) {

19
gtsam/geometry/Pose3.h
View File

@ -11,7 +11,7 @@
/**
*@file Pose3.h
*@brief 3D Pose
* @brief 3D Pose manifold SO(3) x R^3 and group SE(3)
*/
// \callgraph
@ -217,10 +217,27 @@ public:
* (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.
* This is the Jacobian of ExpmapTranslation and computed there.
*/
static Matrix3 ComputeQforExpmapDerivative(
const Vector6& xi, double nearZeroThreshold = 1e-5);
/**
* Compute the translation part of the exponential map, with Jacobians.
* @param w 3D angular velocity
* @param v 3D velocity
* @param Q Optionally, compute 3x3 Jacobian wrpt w
* @param J Optionally, compute 3x3 Jacobian wrpt v = right Jacobian of SO(3)
* @param nearZeroThreshold threshold for small values
* @note This function returns Jacobians Q and J corresponding to the bottom
* blocks of the SE(3) exponential, and translated from left to right from the
* applyLeftJacobian Jacobians.
*/
static Vector3 ExpmapTranslation(const Vector3& w, const Vector3& v,
OptionalJacobian<3, 3> Q = {},
OptionalJacobian<3, 3> J = {},
double nearZeroThreshold = 1e-5);
using LieGroup<Pose3, 6>::inverse; // version with derivative
/**

4
gtsam/geometry/Quaternion.h
View File

@ -45,9 +45,7 @@ struct traits<QUATERNION_TYPE> {
/// @}
/// @name Basic manifold traits
/// @{
enum {
dimension = 3
};
inline constexpr static auto dimension = 3;
typedef OptionalJacobian<3, 3> ChartJacobian;
typedef Eigen::Matrix<_Scalar, 3, 1, _Options, 3, 1> TangentVector;

1
gtsam/geometry/Rot3.cpp
View File

@ -23,6 +23,7 @@
#include <gtsam/geometry/SO3.h>
#include <cmath>
#include <cassert>
#include <random>
using namespace std;

6
gtsam/geometry/Rot3.h
View File

@ -159,7 +159,11 @@ class GTSAM_EXPORT Rot3 : public LieGroup<Rot3, 3> {
/// Rotations around Z, Y, then X axes as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when looking from unchanging axis.
inline static Rot3 RzRyRx(const Vector& xyz,
OptionalJacobian<3, 3> H = {}) {
assert(xyz.size() == 3);
#ifndef NDEBUG
if (xyz.size() != 3) {
throw;
}
#endif
Rot3 out;
if (H) {
Vector3 Hx, Hy, Hz;

166
gtsam/geometry/SO3.cpp
View File

@ -18,13 +18,14 @@
* @date December 2014
*/
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Vector.h>
#include <gtsam/base/concepts.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/SO3.h>
#include <Eigen/SVD>
#include <cmath>
#include <iostream>
#include <limits>
namespace gtsam {
@ -32,6 +33,15 @@ namespace gtsam {
//******************************************************************************
namespace so3 {
static constexpr double one_6th = 1.0 / 6.0;
static constexpr double one_12th = 1.0 / 12.0;
static constexpr double one_24th = 1.0 / 24.0;
static constexpr double one_60th = 1.0 / 60.0;
static constexpr double one_120th = 1.0 / 120.0;
static constexpr double one_180th = 1.0 / 180.0;
static constexpr double one_720th = 1.0 / 720.0;
static constexpr double one_1260th = 1.0 / 1260.0;
GTSAM_EXPORT Matrix99 Dcompose(const SO3& Q) {
Matrix99 H;
auto R = Q.matrix();
@ -41,7 +51,8 @@ GTSAM_EXPORT Matrix99 Dcompose(const SO3& Q) {
return H;
}
GTSAM_EXPORT Matrix3 compose(const Matrix3& M, const SO3& R, OptionalJacobian<9, 9> H) {
GTSAM_EXPORT Matrix3 compose(const Matrix3& M, const SO3& R,
OptionalJacobian<9, 9> H) {
Matrix3 MR = M * R.matrix();
if (H) *H = Dcompose(R);
return MR;
@ -51,82 +62,128 @@ void ExpmapFunctor::init(bool nearZeroApprox) {
nearZero =
nearZeroApprox || (theta2 <= std::numeric_limits<double>::epsilon());
if (!nearZero) {
sin_theta = std::sin(theta);
const double sin_theta = std::sin(theta);
A = sin_theta / theta;
const double s2 = std::sin(theta / 2.0);
one_minus_cos = 2.0 * s2 * s2; // numerically better than [1 - cos(theta)]
const double one_minus_cos =
2.0 * s2 * s2; // numerically better than [1 - cos(theta)]
B = one_minus_cos / theta2;
} else {
// Taylor expansion at 0
A = 1.0 - theta2 * one_6th;
B = 0.5 - theta2 * one_24th;
}
}
ExpmapFunctor::ExpmapFunctor(const Vector3& omega, bool nearZeroApprox)
: theta2(omega.dot(omega)), theta(std::sqrt(theta2)) {
const double wx = omega.x(), wy = omega.y(), wz = omega.z();
W << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0;
: theta2(omega.dot(omega)),
theta(std::sqrt(theta2)),
W(skewSymmetric(omega)),
WW(W * W) {
init(nearZeroApprox);
if (!nearZero) {
K = W / theta;
KK = K * K;
}
}
ExpmapFunctor::ExpmapFunctor(const Vector3& axis, double angle,
bool nearZeroApprox)
: theta2(angle * angle), theta(angle) {
const double ax = axis.x(), ay = axis.y(), az = axis.z();
K << 0.0, -az, +ay, +az, 0.0, -ax, -ay, +ax, 0.0;
W = K * angle;
: theta2(angle * angle),
theta(angle),
W(skewSymmetric(axis * angle)),
WW(W * W) {
init(nearZeroApprox);
if (!nearZero) {
KK = K * K;
}
}
SO3 ExpmapFunctor::expmap() const {
if (nearZero)
return SO3(I_3x3 + W);
else
return SO3(I_3x3 + sin_theta * K + one_minus_cos * KK);
}
SO3 ExpmapFunctor::expmap() const { return SO3(I_3x3 + A * W + B * WW); }
DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox)
: ExpmapFunctor(omega, nearZeroApprox), omega(omega) {
if (nearZero) {
dexp_ = I_3x3 - 0.5 * W;
if (!nearZero) {
C = (1 - A) / theta2;
D = (1.0 - A / (2.0 * B)) / theta2;
E = (2.0 * B - A) / theta2;
F = (3.0 * C - B) / theta2;
} else {
a = one_minus_cos / theta;
b = 1.0 - sin_theta / theta;
dexp_ = I_3x3 - a * K + b * KK;
// Taylor expansion at 0
// TODO(Frank): flipping signs here does not trigger any tests: harden!
C = one_6th - theta2 * one_120th;
D = one_12th + theta2 * one_720th;
E = one_12th - theta2 * one_180th;
F = one_60th - theta2 * one_1260th;
}
}
Vector3 DexpFunctor::crossB(const Vector3& v, OptionalJacobian<3, 3> H) const {
// Wv = omega x v
const Vector3 Wv = gtsam::cross(omega, v);
if (H) {
// Apply product rule to (B Wv)
// - E * omega.transpose() is 1x3 Jacobian of B with respect to omega
// - skewSymmetric(v) is 3x3 Jacobian of Wv = gtsam::cross(omega, v)
*H = - Wv * E * omega.transpose() - B * skewSymmetric(v);
}
return B * Wv;
}
Vector3 DexpFunctor::doubleCrossC(const Vector3& v,
OptionalJacobian<3, 3> H) const {
// WWv = omega x (omega x * v)
Matrix3 doubleCrossJacobian;
const Vector3 WWv =
gtsam::doubleCross(omega, v, H ? &doubleCrossJacobian : nullptr);
if (H) {
// Apply product rule to (C WWv)
// - F * omega.transpose() is 1x3 Jacobian of C with respect to omega
// doubleCrossJacobian is 3x3 Jacobian of WWv = gtsam::doubleCross(omega, v)
*H = - WWv * F * omega.transpose() + C * doubleCrossJacobian;
}
return C * WWv;
}
// Multiplies v with left Jacobian through vector operations only.
Vector3 DexpFunctor::applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
OptionalJacobian<3, 3> H2) const {
if (H1) {
if (nearZero) {
*H1 = 0.5 * skewSymmetric(v);
} else {
// TODO(frank): Iserles hints that there should be a form I + c*K + d*KK
const Vector3 Kv = K * v;
const double Da = (sin_theta - 2.0 * a) / theta2;
const double Db = (one_minus_cos - 3.0 * b) / theta2;
*H1 = (Db * K - Da * I_3x3) * Kv * omega.transpose() -
skewSymmetric(Kv * b / theta) +
(a * I_3x3 - b * K) * skewSymmetric(v / theta);
}
}
if (H2) *H2 = dexp_;
return dexp_ * v;
Matrix3 D_BWv_w, D_CWWv_w;
const Vector3 BWv = crossB(v, H1 ? &D_BWv_w : nullptr);
const Vector3 CWWv = doubleCrossC(v, H1 ? &D_CWWv_w : nullptr);
if (H1) *H1 = -D_BWv_w + D_CWWv_w;
if (H2) *H2 = rightJacobian();
return v - BWv + CWWv;
}
Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
OptionalJacobian<3, 3> H2) const {
const Matrix3 invDexp = dexp_.inverse();
const Vector3 c = invDexp * v;
const Matrix3 invJr = rightJacobianInverse();
const Vector3 c = invJr * v;
if (H1) {
Matrix3 D_dexpv_omega;
applyDexp(c, D_dexpv_omega); // get derivative H of forward mapping
*H1 = -invDexp * D_dexpv_omega;
Matrix3 H;
applyDexp(c, H); // get derivative H of forward mapping
*H1 = -invJr * H;
}
if (H2) *H2 = invDexp;
if (H2) *H2 = invJr;
return c;
}
Vector3 DexpFunctor::applyLeftJacobian(const Vector3& v,
OptionalJacobian<3, 3> H1,
OptionalJacobian<3, 3> H2) const {
Matrix3 D_BWv_w, D_CWWv_w;
const Vector3 BWv = crossB(v, H1 ? &D_BWv_w : nullptr);
const Vector3 CWWv = doubleCrossC(v, H1 ? &D_CWWv_w : nullptr);
if (H1) *H1 = D_BWv_w + D_CWWv_w;
if (H2) *H2 = leftJacobian();
return v + BWv + CWWv;
}
Vector3 DexpFunctor::applyLeftJacobianInverse(const Vector3& v,
OptionalJacobian<3, 3> H1,
OptionalJacobian<3, 3> H2) const {
const Matrix3 invJl = leftJacobianInverse();
const Vector3 c = invJl * v;
if (H1) {
Matrix3 H;
applyLeftJacobian(c, H); // get derivative H of forward mapping
*H1 = -invJl * H;
}
if (H2) *H2 = invJl;
return c;
}
@ -168,12 +225,7 @@ SO3 SO3::ChordalMean(const std::vector<SO3>& rotations) {
template <>
GTSAM_EXPORT
Matrix3 SO3::Hat(const Vector3& xi) {
// skew symmetric matrix X = xi^
Matrix3 Y = Z_3x3;
Y(0, 1) = -xi(2);
Y(0, 2) = +xi(1);
Y(1, 2) = -xi(0);
return Y - Y.transpose();
return skewSymmetric(xi);
}
//******************************************************************************

81
gtsam/geometry/SO3.h
View File

@ -26,7 +26,6 @@
#include <gtsam/base/Matrix.h>
#include <gtsam/dllexport.h>
#include <cmath>
#include <vector>
namespace gtsam {
@ -133,16 +132,17 @@ GTSAM_EXPORT Matrix99 Dcompose(const SO3& R);
// functor also implements dedicated methods to apply dexp and/or inv(dexp).
/// Functor implementing Exponential map
class GTSAM_EXPORT ExpmapFunctor {
protected:
const double theta2;
Matrix3 W, K, KK;
/// Math is based on Ethan Eade's elegant Lie group document, at
/// https://www.ethaneade.org/lie.pdf.
struct GTSAM_EXPORT ExpmapFunctor {
const double theta2, theta;
const Matrix3 W, WW;
bool nearZero;
double theta, sin_theta, one_minus_cos; // only defined if !nearZero
void init(bool nearZeroApprox = false);
// Ethan Eade's constants:
double A; // A = sin(theta) / theta
double B; // B = (1 - cos(theta))
public:
/// Constructor with element of Lie algebra so(3)
explicit ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false);
@ -151,34 +151,75 @@ class GTSAM_EXPORT ExpmapFunctor {
/// Rodrigues formula
SO3 expmap() const;
protected:
void init(bool nearZeroApprox = false);
};
/// Functor that implements Exponential map *and* its derivatives
class DexpFunctor : public ExpmapFunctor {
/// Math extends Ethan theme of elegant I + aW + bWW expressions.
/// See https://www.ethaneade.org/lie.pdf expmap (82) and left Jacobian (83).
struct GTSAM_EXPORT DexpFunctor : public ExpmapFunctor {
const Vector3 omega;
double a, b;
Matrix3 dexp_;
public:
// Ethan's C constant used in Jacobians
double C; // (1 - A) / theta^2
// Constant used in inverse Jacobians
double D; // (1 - A/2B) / theta2
// Constants used in cross and doubleCross
double E; // (2B - A) / theta2
double F; // (3C - B) / theta2
/// Constructor with element of Lie algebra so(3)
GTSAM_EXPORT explicit DexpFunctor(const Vector3& omega, bool nearZeroApprox = false);
explicit DexpFunctor(const Vector3& omega, bool nearZeroApprox = false);
// NOTE(luca): Right Jacobian for Exponential map in SO(3) - equation
// (10.86) and following equations in G.S. Chirikjian, "Stochastic Models,
// Information Theory, and Lie Groups", Volume 2, 2008.
// expmap(omega + v) \approx expmap(omega) * expmap(dexp * v)
// This maps a perturbation v in the tangent space to
// a perturbation on the manifold Expmap(dexp * v) */
const Matrix3& dexp() const { return dexp_; }
// Expmap(xi + dxi) \approx Expmap(xi) * Expmap(dexp * dxi)
// This maps a perturbation dxi=(w,v) in the tangent space to
// a perturbation on the manifold Expmap(dexp * xi)
Matrix3 rightJacobian() const { return I_3x3 - B * W + C * WW; }
// Compute the left Jacobian for Exponential map in SO(3)
Matrix3 leftJacobian() const { return I_3x3 + B * W + C * WW; }
/// Differential of expmap == right Jacobian
inline Matrix3 dexp() const { return rightJacobian(); }
/// Inverse of right Jacobian
Matrix3 rightJacobianInverse() const { return I_3x3 + 0.5 * W + D * WW; }
// Inverse of left Jacobian
Matrix3 leftJacobianInverse() const { return I_3x3 - 0.5 * W + D * WW; }
/// Synonym for rightJacobianInverse
inline Matrix3 invDexp() const { return rightJacobianInverse(); }
/// Computes B * (omega x v).
Vector3 crossB(const Vector3& v, OptionalJacobian<3, 3> H = {}) const;
/// Computes C * (omega x (omega x v)).
Vector3 doubleCrossC(const Vector3& v, OptionalJacobian<3, 3> H = {}) const;
/// Multiplies with dexp(), with optional derivatives
GTSAM_EXPORT Vector3 applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1 = {},
Vector3 applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1 = {},
OptionalJacobian<3, 3> H2 = {}) const;
/// Multiplies with dexp().inverse(), with optional derivatives
GTSAM_EXPORT Vector3 applyInvDexp(const Vector3& v,
OptionalJacobian<3, 3> H1 = {},
Vector3 applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1 = {},
OptionalJacobian<3, 3> H2 = {}) const;
/// Multiplies with leftJacobian(), with optional derivatives
Vector3 applyLeftJacobian(const Vector3& v, OptionalJacobian<3, 3> H1 = {},
OptionalJacobian<3, 3> H2 = {}) const;
/// Multiplies with leftJacobianInverse(), with optional derivatives
Vector3 applyLeftJacobianInverse(const Vector3& v,
OptionalJacobian<3, 3> H1 = {},
OptionalJacobian<3, 3> H2 = {}) const;
};
} // namespace so3

3
gtsam/geometry/SOn.h
View File

@ -33,6 +33,7 @@
#include <type_traits>
#include <vector>
#include <random>
#include <cassert>
namespace gtsam {
@ -53,7 +54,7 @@ constexpr int NSquaredSO(int N) { return (N < 0) ? Eigen::Dynamic : N * N; }
template <int N>
class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
public:
enum { dimension = internal::DimensionSO(N) };
inline constexpr static auto dimension = internal::DimensionSO(N);
using MatrixNN = Eigen::Matrix<double, N, N>;
using VectorN2 = Eigen::Matrix<double, internal::NSquaredSO(N), 1>;
using MatrixDD = Eigen::Matrix<double, dimension, dimension>;

4
gtsam/geometry/SphericalCamera.h
View File

@ -41,7 +41,7 @@ namespace gtsam {
*/
class GTSAM_EXPORT EmptyCal {
public:
enum { dimension = 0 };
inline constexpr static auto dimension = 0;
EmptyCal() {}
virtual ~EmptyCal() = default;
using shared_ptr = std::shared_ptr<EmptyCal>;
@ -73,7 +73,7 @@ class GTSAM_EXPORT EmptyCal {
*/
class GTSAM_EXPORT SphericalCamera {
public:
enum { dimension = 6 };
inline constexpr static auto dimension = 6;
using Measurement = Unit3;
using MeasurementVector = std::vector<Unit3>;

4
gtsam/geometry/StereoCamera.h
View File

@ -61,9 +61,7 @@ private:
public:
enum {
dimension = 6
};
inline constexpr static auto dimension = 6;
/// @name Standard Constructors
/// @{

2
gtsam/geometry/StereoPoint2.h
View File

@ -37,7 +37,7 @@ private:
double uL_, uR_, v_;
public:
enum { dimension = 3 };
inline constexpr static auto dimension = 3;
/// @name Standard Constructors
/// @{

4
gtsam/geometry/Unit3.h
View File

@ -53,9 +53,7 @@ private:
public:
enum {
dimension = 2
};
inline constexpr static auto dimension = 2;
/// @name Constructors
/// @{

1
gtsam/geometry/geometry.i
View File

@ -360,6 +360,7 @@ class Rot3 {
// Standard Interface
static gtsam::Rot3 Expmap(gtsam::Vector v);
static gtsam::Vector Logmap(const gtsam::Rot3& p);
gtsam::Rot3 expmap(const gtsam::Vector& v);
gtsam::Vector logmap(const gtsam::Rot3& p);
gtsam::Matrix matrix() const;
gtsam::Matrix transpose() const;

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

@ -154,6 +154,17 @@ TEST( Point3, cross2) {
}
}
/* ************************************************************************* */
TEST(Point3, doubleCross) {
Matrix aH1, aH2;
std::function<Point3(const Point3&, const Point3&)> f =
[](const Point3& p, const Point3& q) { return doubleCross(p, q); };
const Point3 omega(1, 2, 3), theta(4, 5, 6);
doubleCross(omega, theta, aH1, aH2);
EXPECT(assert_equal(numericalDerivative21(f, omega, theta), aH1));
EXPECT(assert_equal(numericalDerivative22(f, omega, theta), aH2));
}
//*************************************************************************
TEST (Point3, normalize) {
Matrix actualH;

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

@ -835,17 +835,7 @@ TEST(Pose3, Align2) {
}
/* ************************************************************************* */
TEST( Pose3, ExpmapDerivative1) {
Matrix6 actualH;
Vector6 w; w << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0;
Pose3::Expmap(w,actualH);
Matrix expectedH = numericalDerivative21<Pose3, Vector6,
OptionalJacobian<6, 6> >(&Pose3::Expmap, w, {});
EXPECT(assert_equal(expectedH, actualH));
}
/* ************************************************************************* */
TEST(Pose3, ExpmapDerivative2) {
TEST(Pose3, ExpmapDerivative) {
// Iserles05an (Lie-group Methods) says:
// scalar is easy: d exp(a(t)) / dt = exp(a(t)) a'(t)
// matrix is hard: d exp(A(t)) / dt = exp(A(t)) dexp[-A(t)] A'(t)
@ -879,26 +869,71 @@ 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, {});
Matrix3 expectedQr = expectedH.bottomLeftCorner<3, 3>();
EXPECT(assert_equal(expectedQr, actualQr, 1e-6));
//******************************************************************************
namespace test_cases {
std::vector<Vector3> small{{0, 0, 0}, //
{1e-5, 0, 0}, {0, 1e-5, 0}, {0, 0, 1e-5}, //,
{1e-4, 0, 0}, {0, 1e-4, 0}, {0, 0, 1e-4}};
std::vector<Vector3> large{{0, 0, 0}, {1, 0, 0}, {0, 1, 0},
{0, 0, 1}, {.1, .2, .3}, {1, -2, 3}};
auto omegas = [](bool nearZero) { return nearZero ? small : large; };
std::vector<Vector3> vs{{1, 0, 0}, {0, 1, 0}, {0, 0, 1},
{.4, .3, .2}, {4, 5, 6}, {-10, -20, 30}};
} // namespace test_cases
//******************************************************************************
TEST(Pose3, ExpmapDerivatives) {
for (bool nearZero : {true, false}) {
for (const Vector3& w : test_cases::omegas(nearZero)) {
for (Vector3 v : test_cases::vs) {
const Vector6 xi = (Vector6() << w, v).finished();
const Matrix6 expectedH =
numericalDerivative21<Pose3, Vector6, OptionalJacobian<6, 6> >(
&Pose3::Expmap, xi, {});
Matrix actualH;
Pose3::Expmap(xi, actualH);
EXPECT(assert_equal(expectedH, actualH));
}
}
}
}
/* ************************************************************************* */
TEST( Pose3, LogmapDerivative) {
Matrix6 actualH;
Vector6 w; w << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0;
Pose3 p = Pose3::Expmap(w);
EXPECT(assert_equal(w, Pose3::Logmap(p,actualH), 1e-5));
Matrix expectedH = numericalDerivative21<Vector6, Pose3,
OptionalJacobian<6, 6> >(&Pose3::Logmap, p, {});
EXPECT(assert_equal(expectedH, actualH));
//******************************************************************************
// Check logmap for all small values, as we don't want wrapping.
TEST(Pose3, Logmap) {
static constexpr bool nearZero = true;
for (const Vector3& w : test_cases::omegas(nearZero)) {
for (Vector3 v : test_cases::vs) {
const Vector6 xi = (Vector6() << w, v).finished();
Pose3 pose = Pose3::Expmap(xi);
EXPECT(assert_equal(xi, Pose3::Logmap(pose)));
}
}
}
//******************************************************************************
// Check logmap derivatives for all values
TEST(Pose3, LogmapDerivatives) {
for (bool nearZero : {true, false}) {
for (const Vector3& w : test_cases::omegas(nearZero)) {
for (Vector3 v : test_cases::vs) {
const Vector6 xi = (Vector6() << w, v).finished();
Pose3 pose = Pose3::Expmap(xi);
const Matrix6 expectedH =
numericalDerivative21<Vector6, Pose3, OptionalJacobian<6, 6> >(
&Pose3::Logmap, pose, {});
Matrix actualH;
Pose3::Logmap(pose, actualH);
#ifdef GTSAM_USE_QUATERNIONS
// TODO(Frank): Figure out why quaternions are not as accurate.
// Hint: 6 cases fail on Ubuntu 22.04, but none on MacOS.
EXPECT(assert_equal(expectedH, actualH, 1e-7));
#else
EXPECT(assert_equal(expectedH, actualH));
#endif
}
}
}
}
/* ************************************************************************* */

140
gtsam/geometry/tests/testSO3.cpp
View File

@ -304,18 +304,77 @@ TEST(SO3, JacobianLogmap) {
}
//******************************************************************************
TEST(SO3, ApplyDexp) {
Matrix aH1, aH2;
for (bool nearZeroApprox : {true, false}) {
namespace test_cases {
std::vector<Vector3> small{{0, 0, 0}, //
{1e-5, 0, 0}, {0, 1e-5, 0}, {0, 0, 1e-5}, //,
{1e-4, 0, 0}, {0, 1e-4, 0}, {0, 0, 1e-4}};
std::vector<Vector3> large{
{0, 0, 0}, {1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0.1, 0.2, 0.3}};
auto omegas = [](bool nearZero) { return nearZero ? small : large; };
std::vector<Vector3> vs{{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0.4, 0.3, 0.2}};
} // namespace test_cases
//******************************************************************************
TEST(SO3, JacobianInverses) {
Matrix HR, HL;
for (bool nearZero : {true, false}) {
for (const Vector3& omega : test_cases::omegas(nearZero)) {
so3::DexpFunctor local(omega, nearZero);
EXPECT(assert_equal<Matrix3>(local.rightJacobian().inverse(),
local.rightJacobianInverse()));
EXPECT(assert_equal<Matrix3>(local.leftJacobian().inverse(),
local.leftJacobianInverse()));
}
}
}
//******************************************************************************
TEST(SO3, CrossB) {
Matrix aH1;
for (bool nearZero : {true, false}) {
std::function<Vector3(const Vector3&, const Vector3&)> f =
[=](const Vector3& omega, const Vector3& v) {
return so3::DexpFunctor(omega, nearZeroApprox).applyDexp(v);
return so3::DexpFunctor(omega, nearZero).crossB(v);
};
for (Vector3 omega : {Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(0, 1, 0),
Vector3(0, 0, 1), Vector3(0.1, 0.2, 0.3)}) {
so3::DexpFunctor local(omega, nearZeroApprox);
for (Vector3 v : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1),
Vector3(0.4, 0.3, 0.2)}) {
for (const Vector3& omega : test_cases::omegas(nearZero)) {
so3::DexpFunctor local(omega, nearZero);
for (const Vector3& v : test_cases::vs) {
local.crossB(v, aH1);
EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));
}
}
}
}
//******************************************************************************
TEST(SO3, DoubleCrossC) {
Matrix aH1;
for (bool nearZero : {true, false}) {
std::function<Vector3(const Vector3&, const Vector3&)> f =
[=](const Vector3& omega, const Vector3& v) {
return so3::DexpFunctor(omega, nearZero).doubleCrossC(v);
};
for (const Vector3& omega : test_cases::omegas(nearZero)) {
so3::DexpFunctor local(omega, nearZero);
for (const Vector3& v : test_cases::vs) {
local.doubleCrossC(v, aH1);
EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));
}
}
}
}
//******************************************************************************
TEST(SO3, ApplyDexp) {
Matrix aH1, aH2;
for (bool nearZero : {true, false}) {
std::function<Vector3(const Vector3&, const Vector3&)> f =
[=](const Vector3& omega, const Vector3& v) {
return so3::DexpFunctor(omega, nearZero).applyDexp(v);
};
for (const Vector3& omega : test_cases::omegas(nearZero)) {
so3::DexpFunctor local(omega, nearZero);
for (const Vector3& v : test_cases::vs) {
EXPECT(assert_equal(Vector3(local.dexp() * v),
local.applyDexp(v, aH1, aH2)));
EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));
@ -329,22 +388,63 @@ TEST(SO3, ApplyDexp) {
//******************************************************************************
TEST(SO3, ApplyInvDexp) {
Matrix aH1, aH2;
for (bool nearZeroApprox : {true, false}) {
for (bool nearZero : {true, false}) {
std::function<Vector3(const Vector3&, const Vector3&)> f =
[=](const Vector3& omega, const Vector3& v) {
return so3::DexpFunctor(omega, nearZeroApprox).applyInvDexp(v);
return so3::DexpFunctor(omega, nearZero).applyInvDexp(v);
};
for (Vector3 omega : {Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(0, 1, 0),
Vector3(0, 0, 1), Vector3(0.1, 0.2, 0.3)}) {
so3::DexpFunctor local(omega, nearZeroApprox);
Matrix invDexp = local.dexp().inverse();
for (Vector3 v : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1),
Vector3(0.4, 0.3, 0.2)}) {
EXPECT(assert_equal(Vector3(invDexp * v),
local.applyInvDexp(v, aH1, aH2)));
for (const Vector3& omega : test_cases::omegas(nearZero)) {
so3::DexpFunctor local(omega, nearZero);
Matrix invJr = local.rightJacobianInverse();
for (const Vector3& v : test_cases::vs) {
EXPECT(
assert_equal(Vector3(invJr * v), local.applyInvDexp(v, aH1, aH2)));
EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));
EXPECT(assert_equal(numericalDerivative22(f, omega, v), aH2));
EXPECT(assert_equal(invDexp, aH2));
EXPECT(assert_equal(invJr, aH2));
}
}
}
}
//******************************************************************************
TEST(SO3, ApplyLeftJacobian) {
Matrix aH1, aH2;
for (bool nearZero : {true, false}) {
std::function<Vector3(const Vector3&, const Vector3&)> f =
[=](const Vector3& omega, const Vector3& v) {
return so3::DexpFunctor(omega, nearZero).applyLeftJacobian(v);
};
for (const Vector3& omega : test_cases::omegas(nearZero)) {
so3::DexpFunctor local(omega, nearZero);
for (const Vector3& v : test_cases::vs) {
EXPECT(assert_equal(Vector3(local.leftJacobian() * v),
local.applyLeftJacobian(v, aH1, aH2)));
EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));
EXPECT(assert_equal(numericalDerivative22(f, omega, v), aH2));
EXPECT(assert_equal(local.leftJacobian(), aH2));
}
}
}
}
//******************************************************************************
TEST(SO3, ApplyLeftJacobianInverse) {
Matrix aH1, aH2;
for (bool nearZero : {true, false}) {
std::function<Vector3(const Vector3&, const Vector3&)> f =
[=](const Vector3& omega, const Vector3& v) {
return so3::DexpFunctor(omega, nearZero).applyLeftJacobianInverse(v);
};
for (const Vector3& omega : test_cases::omegas(nearZero)) {
so3::DexpFunctor local(omega, nearZero);
Matrix invJl = local.leftJacobianInverse();
for (const Vector3& v : test_cases::vs) {
EXPECT(assert_equal(Vector3(invJl * v),
local.applyLeftJacobianInverse(v, aH1, aH2)));
EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));
EXPECT(assert_equal(numericalDerivative22(f, omega, v), aH2));
EXPECT(assert_equal(invJl, aH2));
}
}
}

8
gtsam/geometry/triangulation.h
View File

@ -20,7 +20,7 @@
#pragma once
#include "gtsam/geometry/Point3.h"
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/geometry/Cal3Fisheye.h>
#include <gtsam/geometry/Cal3Unified.h>
@ -317,7 +317,11 @@ typename CAMERA::MeasurementVector undistortMeasurements(
const CameraSet<CAMERA>& cameras,
const typename CAMERA::MeasurementVector& measurements) {
const size_t nrMeasurements = measurements.size();
assert(nrMeasurements == cameras.size());
#ifndef NDEBUG
if (nrMeasurements != cameras.size()) {
throw;
}
#endif
typename CAMERA::MeasurementVector undistortedMeasurements(nrMeasurements);
for (size_t ii = 0; ii < nrMeasurements; ++ii) {
// Calibrate with cal and uncalibrate with pinhole version of cal so that

8
gtsam/hybrid/HybridBayesTree.cpp
View File

@ -210,9 +210,11 @@ void HybridBayesTree::prune(const size_t maxNrLeaves) {
if (conditional->isHybrid()) {
auto hybridGaussianCond = conditional->asHybrid();
// Imperative
clique->conditional() = std::make_shared<HybridConditional>(
hybridGaussianCond->prune(parentData.prunedDiscreteProbs));
if (!hybridGaussianCond->pruned()) {
// Imperative
clique->conditional() = std::make_shared<HybridConditional>(
hybridGaussianCond->prune(parentData.prunedDiscreteProbs));
}
}
return parentData;
}

12
gtsam/hybrid/HybridGaussianConditional.cpp
View File

@ -120,7 +120,7 @@ struct HybridGaussianConditional::Helper {
/* *******************************************************************************/
HybridGaussianConditional::HybridGaussianConditional(
const DiscreteKeys &discreteParents, Helper &&helper)
const DiscreteKeys &discreteParents, Helper &&helper, bool pruned)
: BaseFactor(discreteParents,
FactorValuePairs(
[&](const GaussianFactorValuePair
@ -130,7 +130,8 @@ HybridGaussianConditional::HybridGaussianConditional(
},
std::move(helper.pairs))),
BaseConditional(*helper.nrFrontals),
negLogConstant_(helper.minNegLogConstant) {}
negLogConstant_(helper.minNegLogConstant),
pruned_(pruned) {}
HybridGaussianConditional::HybridGaussianConditional(
const DiscreteKey &discreteParent,
@ -166,8 +167,9 @@ HybridGaussianConditional::HybridGaussianConditional(
: HybridGaussianConditional(discreteParents, Helper(conditionals)) {}
HybridGaussianConditional::HybridGaussianConditional(
const DiscreteKeys &discreteParents, const FactorValuePairs &pairs)
: HybridGaussianConditional(discreteParents, Helper(pairs)) {}
const DiscreteKeys &discreteParents, const FactorValuePairs &pairs,
bool pruned)
: HybridGaussianConditional(discreteParents, Helper(pairs), pruned) {}
/* *******************************************************************************/
const HybridGaussianConditional::Conditionals
@ -331,7 +333,7 @@ HybridGaussianConditional::shared_ptr HybridGaussianConditional::prune(
FactorValuePairs prunedConditionals = factors().apply(pruner);
return std::make_shared<HybridGaussianConditional>(discreteKeys(),
prunedConditionals);
prunedConditionals, true);
}
/* *******************************************************************************/

11
gtsam/hybrid/HybridGaussianConditional.h
View File

@ -68,6 +68,9 @@ class GTSAM_EXPORT HybridGaussianConditional
///< Take advantage of the neg-log space so everything is a minimization
double negLogConstant_;
/// Flag to indicate if the conditional has been pruned.
bool pruned_ = false;
public:
/// @name Constructors
/// @{
@ -150,9 +153,10 @@ class GTSAM_EXPORT HybridGaussianConditional
*
* @param discreteParents the discrete parents. Will be placed last.
* @param conditionalPairs Decision tree of GaussianFactor/scalar pairs.
* @param pruned Flag indicating if conditional has been pruned.
*/
HybridGaussianConditional(const DiscreteKeys &discreteParents,
const FactorValuePairs &pairs);
const FactorValuePairs &pairs, bool pruned = false);
/// @}
/// @name Testable
@ -233,6 +237,9 @@ class GTSAM_EXPORT HybridGaussianConditional
HybridGaussianConditional::shared_ptr prune(
const DecisionTreeFactor &discreteProbs) const;
/// Return true if the conditional has already been pruned.
bool pruned() const { return pruned_; }
/// @}
private:
@ -241,7 +248,7 @@ class GTSAM_EXPORT HybridGaussianConditional
/// Private constructor that uses helper struct above.
HybridGaussianConditional(const DiscreteKeys &discreteParents,
Helper &&helper);
Helper &&helper, bool pruned = false);
/// Check whether `given` has values for all frontal keys.
bool allFrontalsGiven(const VectorValues &given) const;

1
gtsam/hybrid/HybridJunctionTree.cpp
View File

@ -22,6 +22,7 @@
#include <gtsam/inference/Key.h>
#include <unordered_map>
#include <cassert>
namespace gtsam {

2
gtsam/inference/BayesTree-inst.h
View File

@ -27,7 +27,7 @@
#include <fstream>
#include <queue>
#include <cassert>
namespace gtsam {
/* ************************************************************************* */

1
gtsam/inference/ClusterTree-inst.h
View File

@ -19,6 +19,7 @@
#include <mutex>
#endif
#include <queue>
#include <cassert>
namespace gtsam {

1
gtsam/inference/EliminationTree-inst.h
View File

@ -19,6 +19,7 @@
#include <stack>
#include <queue>
#include <cassert>
#include <gtsam/base/timing.h>
#include <gtsam/base/treeTraversal-inst.h>

2
gtsam/inference/JunctionTree-inst.h
View File

@ -25,6 +25,8 @@
#include <gtsam/symbolic/SymbolicConditional.h>
#include <gtsam/symbolic/SymbolicFactor-inst.h>
#include <cassert>
namespace gtsam {
template<class BAYESTREE, class GRAPH, class ETREE_NODE>

6
gtsam/inference/MetisIndex.h
View File

@ -92,7 +92,11 @@ public:
return nKeys_;
}
Key intToKey(int32_t value) const {
assert(value >= 0);
#ifndef NDEBUG
if (value < 0) {
throw;
}
#endif
return intKeyBMap_.right.find(value)->second;
}

1
gtsam/inference/Ordering.cpp
View File

@ -18,6 +18,7 @@
#include <vector>
#include <limits>
#include <cassert>
#include <gtsam/inference/Ordering.h>
#include <gtsam/3rdparty/CCOLAMD/Include/ccolamd.h>

6
gtsam/inference/VariableIndex.h
View File

@ -87,9 +87,11 @@ class GTSAM_EXPORT VariableIndex {
const FactorIndices& operator[](Key variable) const {
KeyMap::const_iterator item = index_.find(variable);
if(item == index_.end())
throw std::invalid_argument("Requested non-existent variable from VariableIndex");
throw std::invalid_argument("Requested non-existent variable '" +
DefaultKeyFormatter(variable) +
"' from VariableIndex");
else
return item->second;
return item->second;
}
/// Return true if no factors associated with a variable

3
gtsam/linear/HessianFactor.cpp
View File

@ -25,13 +25,14 @@
#include <gtsam/base/cholesky.h>
#include <gtsam/base/debug.h>
#include <gtsam/base/FastMap.h>
#include <gtsam/base/Vector.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/ThreadsafeException.h>
#include <gtsam/base/timing.h>
#include <sstream>
#include <cassert>
#include <limits>
#include "gtsam/base/Vector.h"
using namespace std;

17
gtsam/linear/HessianFactor.h
View File

@ -23,7 +23,6 @@
#include <gtsam/base/SymmetricBlockMatrix.h>
#include <gtsam/base/FastVector.h>
namespace gtsam {
// Forward declarations
@ -241,14 +240,18 @@ namespace gtsam {
* use, for example, begin() + 2 to get the 3rd variable in this factor.
* @return The linear term \f$ g \f$ */
SymmetricBlockMatrix::constBlock linearTerm(const_iterator j) const {
assert(!empty());
#ifndef NDEBUG
if(empty()) throw;
#endif
return info_.aboveDiagonalBlock(j - begin(), size());
}
/** Return the complete linear term \f$ g \f$ as described above.
* @return The linear term \f$ g \f$ */
SymmetricBlockMatrix::constBlock linearTerm() const {
assert(!empty());
#ifndef NDEBUG
if(empty()) throw;
#endif
// get the last column (except the bottom right block)
return info_.aboveDiagonalRange(0, size(), size(), size() + 1);
}
@ -256,7 +259,9 @@ namespace gtsam {
/** Return the complete linear term \f$ g \f$ as described above.
* @return The linear term \f$ g \f$ */
SymmetricBlockMatrix::Block linearTerm() {
assert(!empty());
#ifndef NDEBUG
if(empty()) throw;
#endif
return info_.aboveDiagonalRange(0, size(), size(), size() + 1);
}
@ -325,7 +330,9 @@ namespace gtsam {
* @param other the HessianFactor to be updated
*/
void updateHessian(HessianFactor* other) const {
assert(other);
#ifndef NDEBUG
if(!other) throw;
#endif
updateHessian(other->keys_, &other->info_);
}

Some files were not shown because too many files have changed in this diff Show More