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

Merge branch 'feature/robustTriangulation' into feature/refinementsForPoseToPointFactor

release/4.3a0
lcarlone 2022-01-12 20:43:13 -05:00
parent 39286f6672 c407609b8f
commit e0082d746c
167 changed files with 4851 additions and 2091 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
.github/scripts/python.sh vendored
View File

@ -75,7 +75,7 @@ cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=${CMAKE_BUILD_TYPE} \
-DGTSAM_UNSTABLE_BUILD_PYTHON=${GTSAM_BUILD_UNSTABLE:-ON} \
-DGTSAM_PYTHON_VERSION=$PYTHON_VERSION \
-DPYTHON_EXECUTABLE:FILEPATH=$(which $PYTHON) \
-DGTSAM_ALLOW_DEPRECATED_SINCE_V41=OFF \
-DGTSAM_ALLOW_DEPRECATED_SINCE_V42=OFF \
-DCMAKE_INSTALL_PREFIX=$GITHUB_WORKSPACE/gtsam_install

2
.github/scripts/unix.sh vendored
View File

@ -64,7 +64,7 @@ function configure()
-DGTSAM_BUILD_UNSTABLE=${GTSAM_BUILD_UNSTABLE:-ON} \
-DGTSAM_WITH_TBB=${GTSAM_WITH_TBB:-OFF} \
-DGTSAM_BUILD_EXAMPLES_ALWAYS=${GTSAM_BUILD_EXAMPLES_ALWAYS:-ON} \
-DGTSAM_ALLOW_DEPRECATED_SINCE_V41=${GTSAM_ALLOW_DEPRECATED_SINCE_V41:-OFF} \
-DGTSAM_ALLOW_DEPRECATED_SINCE_V42=${GTSAM_ALLOW_DEPRECATED_SINCE_V42:-OFF} \
-DGTSAM_USE_QUATERNIONS=${GTSAM_USE_QUATERNIONS:-OFF} \
-DGTSAM_ROT3_EXPMAP=${GTSAM_ROT3_EXPMAP:-ON} \
-DGTSAM_POSE3_EXPMAP=${GTSAM_POSE3_EXPMAP:-ON} \

2
.github/workflows/build-special.yml vendored
View File

@ -110,7 +110,7 @@ jobs:
- name: Set Allow Deprecated Flag
if: matrix.flag == 'deprecated'
run: |
echo "GTSAM_ALLOW_DEPRECATED_SINCE_V41=ON" >> $GITHUB_ENV
echo "GTSAM_ALLOW_DEPRECATED_SINCE_V42=ON" >> $GITHUB_ENV
echo "Allow deprecated since version 4.1"
- name: Set Use Quaternions Flag

12
CMakeLists.txt
View File

@ -9,12 +9,18 @@ endif()
# Set the version number for the library
set (GTSAM_VERSION_MAJOR 4)
set (GTSAM_VERSION_MINOR 1)
set (GTSAM_VERSION_MINOR 2)
set (GTSAM_VERSION_PATCH 0)
set (GTSAM_PRERELEASE_VERSION "a1")
math (EXPR GTSAM_VERSION_NUMERIC "10000 * ${GTSAM_VERSION_MAJOR} + 100 * ${GTSAM_VERSION_MINOR} + ${GTSAM_VERSION_PATCH}")
set (GTSAM_VERSION_STRING "${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH}")
set (CMAKE_PROJECT_VERSION ${GTSAM_VERSION_STRING})
if (${GTSAM_VERSION_PATCH} EQUAL 0)
set (GTSAM_VERSION_STRING "${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}${GTSAM_PRERELEASE_VERSION}")
else()
set (GTSAM_VERSION_STRING "${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH}${GTSAM_PRERELEASE_VERSION}")
endif()
message(STATUS "GTSAM Version: ${GTSAM_VERSION_STRING}")
set (CMAKE_PROJECT_VERSION_MAJOR ${GTSAM_VERSION_MAJOR})
set (CMAKE_PROJECT_VERSION_MINOR ${GTSAM_VERSION_MINOR})
set (CMAKE_PROJECT_VERSION_PATCH ${GTSAM_VERSION_PATCH})

6
README.md
View File

@ -2,9 +2,9 @@
**Important Note**
As of August 1 2020, the `develop` branch is officially in "Pre 4.1" mode, and features deprecated in 4.0 have been removed. Please use the last [4.0.3 release](https://github.com/borglab/gtsam/releases/tag/4.0.3) if you need those features.
As of Dec 2021, the `develop` branch is officially in "Pre 4.2" mode. A great new feature we will be adding in 4.2 is *hybrid inference* a la DCSLAM (Kevin Doherty et al) and we envision several API-breaking changes will happen in the discrete folder.
However, most are easily converted and can be tracked down (in 4.0.3) by disabling the cmake flag `GTSAM_ALLOW_DEPRECATED_SINCE_V4`.
In addition, features deprecated in 4.1 will be removed. Please use the last [4.1.1 release](https://github.com/borglab/gtsam/releases/tag/4.1.1) if you need those features. However, most (not all, unfortunately) are easily converted and can be tracked down (in 4.1.1) by disabling the cmake flag `GTSAM_ALLOW_DEPRECATED_SINCE_V42`.
## What is GTSAM?
@ -57,7 +57,7 @@ GTSAM 4 introduces several new features, most notably Expressions and a Python t
GTSAM 4 also deprecated some legacy functionality and wrongly named methods. If you are on a 4.0.X release, you can define the flag `GTSAM_ALLOW_DEPRECATED_SINCE_V4` to use the deprecated methods.
GTSAM 4.1 added a new pybind wrapper, and **removed** the deprecated functionality. There is a flag `GTSAM_ALLOW_DEPRECATED_SINCE_V41` for newly deprecated methods since the 4.1 release, which is on by default, allowing anyone to just pull version 4.1 and compile.
GTSAM 4.1 added a new pybind wrapper, and **removed** the deprecated functionality. There is a flag `GTSAM_ALLOW_DEPRECATED_SINCE_V42` for newly deprecated methods since the 4.1 release, which is on by default, allowing anyone to just pull version 4.1 and compile.
## Wrappers

2
Using-GTSAM-EXPORT.md
View File

@ -29,7 +29,7 @@ Rule #1 doesn't seem very bad, until you combine it with rule #2
***Compiler Rule #2*** Anything declared in a header file is not included in a DLL.
When these two rules are combined, you get some very confusing results. For example, a class which is completely defined in a header (e.g. LieMatrix) cannot use `GTSAM_EXPORT` in its definition. If LieMatrix is defined with `GTSAM_EXPORT`, then the compiler _must_ find LieMatrix in a DLL. Because LieMatrix is a header-only class, however, it can't find it, leading to a very confusing "I can't find this symbol" type of error. Note that the linker says it can't find the symbol even though the compiler found the header file that completely defines the class.
When these two rules are combined, you get some very confusing results. For example, a class which is completely defined in a header (e.g. Foo) cannot use `GTSAM_EXPORT` in its definition. If Foo is defined with `GTSAM_EXPORT`, then the compiler _must_ find Foo in a DLL. Because Foo is a header-only class, however, it can't find it, leading to a very confusing "I can't find this symbol" type of error. Note that the linker says it can't find the symbol even though the compiler found the header file that completely defines the class.
Also note that when a class that you want to export inherits from another class that is not exportable, this can cause significant issues. According to this [MSVC Warning page](https://docs.microsoft.com/en-us/cpp/error-messages/compiler-warnings/compiler-warning-level-2-c4275?view=vs-2019), it may not strictly be a rule, but we have seen several linker errors when a class that is defined with `GTSAM_EXPORT` extended an Eigen class. In general, it appears that any inheritance of non-exportable class by an exportable class is a bad idea.

2
cmake/HandleGeneralOptions.cmake
View File

@ -25,7 +25,7 @@ option(GTSAM_WITH_EIGEN_MKL_OPENMP "Eigen, when using Intel MKL, will a
option(GTSAM_THROW_CHEIRALITY_EXCEPTION "Throw exception when a triangulated point is behind a camera" ON)
option(GTSAM_BUILD_PYTHON "Enable/Disable building & installation of Python module with pybind11" OFF)
option(GTSAM_INSTALL_MATLAB_TOOLBOX "Enable/Disable installation of matlab toolbox" OFF)
option(GTSAM_ALLOW_DEPRECATED_SINCE_V41 "Allow use of methods/functions deprecated in GTSAM 4.1" ON)
option(GTSAM_ALLOW_DEPRECATED_SINCE_V42 "Allow use of methods/functions deprecated in GTSAM 4.1" ON)
option(GTSAM_SUPPORT_NESTED_DISSECTION "Support Metis-based nested dissection" ON)
option(GTSAM_TANGENT_PREINTEGRATION "Use new ImuFactor with integration on tangent space" ON)
option(GTSAM_SLOW_BUT_CORRECT_BETWEENFACTOR "Use the slower but correct version of BetweenFactor" OFF)

2
cmake/HandlePrintConfiguration.cmake
View File

@ -86,7 +86,7 @@ print_enabled_config(${GTSAM_USE_QUATERNIONS} "Quaternions as defaul
print_enabled_config(${GTSAM_ENABLE_CONSISTENCY_CHECKS} "Runtime consistency checking ")
print_enabled_config(${GTSAM_ROT3_EXPMAP} "Rot3 retract is full ExpMap ")
print_enabled_config(${GTSAM_POSE3_EXPMAP} "Pose3 retract is full ExpMap ")
print_enabled_config(${GTSAM_ALLOW_DEPRECATED_SINCE_V41} "Allow features deprecated in GTSAM 4.1")
print_enabled_config(${GTSAM_ALLOW_DEPRECATED_SINCE_V42} "Allow features deprecated in GTSAM 4.1")
print_enabled_config(${GTSAM_SUPPORT_NESTED_DISSECTION} "Metis-based Nested Dissection ")
print_enabled_config(${GTSAM_TANGENT_PREINTEGRATION} "Use tangent-space preintegration")

3
examples/FisheyeExample.cpp
View File

@ -122,8 +122,7 @@ int main(int argc, char *argv[]) {
std::cout << "initial error=" << graph.error(initialEstimate) << std::endl;
std::cout << "final error=" << graph.error(result) << std::endl;
std::ofstream os("examples/vio_batch.dot");
graph.saveGraph(os, result);
graph.saveGraph("examples/vio_batch.dot", result);
return 0;
}

5
examples/Pose2SLAMExample_graphviz.cpp
View File

@ -60,11 +60,10 @@ int main(int argc, char** argv) {
// save factor graph as graphviz dot file
// Render to PDF using "fdp Pose2SLAMExample.dot -Tpdf > graph.pdf"
ofstream os("Pose2SLAMExample.dot");
graph.saveGraph(os, result);
graph.saveGraph("Pose2SLAMExample.dot", result);
// Also print out to console
graph.saveGraph(cout, result);
graph.dot(cout, result);
return 0;
}

3
examples/UGM_small.cpp
View File

@ -50,8 +50,7 @@ int main(int argc, char** argv) {
// Print the UGM distribution
cout << "\nUGM distribution:" << endl;
vector<DiscreteFactor::Values> allPosbValues = cartesianProduct(
Cathy & Heather & Mark & Allison);
auto allPosbValues = cartesianProduct(Cathy & Heather & Mark & Allison);
for (size_t i = 0; i < allPosbValues.size(); ++i) {
DiscreteFactor::Values values = allPosbValues[i];
double prodPot = graph(values);

4
gtsam/3rdparty/Eigen/Eigen/src/Core/TriangularMatrix.h vendored
View File

@ -440,7 +440,7 @@ template<typename _MatrixType, unsigned int _Mode> class TriangularViewImpl<_Mat
EIGEN_DEVICE_FUNC
void lazyAssign(const TriangularBase<OtherDerived>& other);
/** \deprecated */
/** @deprecated */
template<typename OtherDerived>
EIGEN_DEVICE_FUNC
void lazyAssign(const MatrixBase<OtherDerived>& other);
@ -523,7 +523,7 @@ template<typename _MatrixType, unsigned int _Mode> class TriangularViewImpl<_Mat
call_assignment(derived(), other.const_cast_derived(), internal::swap_assign_op<Scalar>());
}
/** \deprecated
/** @deprecated
* Shortcut for \code (*this).swap(other.triangularView<(*this)::Mode>()) \endcode */
template<typename OtherDerived>
EIGEN_DEVICE_FUNC

2
gtsam/CMakeLists.txt
View File

@ -15,7 +15,7 @@ set (gtsam_subdirs
sam
sfm
slam
navigation
navigation
)
set(gtsam_srcs)

3
gtsam/base/CMakeLists.txt
View File

@ -5,8 +5,5 @@ install(FILES ${base_headers} DESTINATION include/gtsam/base)
file(GLOB base_headers_tree "treeTraversal/*.h")
install(FILES ${base_headers_tree} DESTINATION include/gtsam/base/treeTraversal)
file(GLOB deprecated_headers "deprecated/*.h")
install(FILES ${deprecated_headers} DESTINATION include/gtsam/base/deprecated)
# Build tests
add_subdirectory(tests)

26
gtsam/base/LieMatrix.h
View File

@ -1,26 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file LieMatrix.h
* @brief External deprecation warning, see deprecated/LieMatrix.h for details
* @author Paul Drews
*/
#pragma once
#ifdef _MSC_VER
#pragma message("LieMatrix.h is deprecated. Please use Eigen::Matrix instead.")
#else
#warning "LieMatrix.h is deprecated. Please use Eigen::Matrix instead."
#endif
#include "gtsam/base/deprecated/LieMatrix.h"

26
gtsam/base/LieScalar.h
View File

@ -1,26 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file LieScalar.h
* @brief External deprecation warning, see deprecated/LieScalar.h for details
* @author Kai Ni
*/
#pragma once
#ifdef _MSC_VER
#pragma message("LieScalar.h is deprecated. Please use double/float instead.")
#else
#warning "LieScalar.h is deprecated. Please use double/float instead."
#endif
#include <gtsam/base/deprecated/LieScalar.h>

26
gtsam/base/LieVector.h
View File

@ -1,26 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file LieVector.h
* @brief Deprecation warning for LieVector, see deprecated/LieVector.h for details.
* @author Paul Drews
*/
#pragma once
#ifdef _MSC_VER
#pragma message("LieVector.h is deprecated. Please use Eigen::Vector instead.")
#else
#warning "LieVector.h is deprecated. Please use Eigen::Vector instead."
#endif
#include <gtsam/base/deprecated/LieVector.h>

4
gtsam/base/TestableAssertions.h
View File

@ -80,9 +80,10 @@ bool assert_equal(const V& expected, const boost::optional<const V&>& actual, do
return assert_equal(expected, *actual, tol);
}
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
/**
* Version of assert_equals to work with vectors
* \deprecated: use container equals instead
* @deprecated: use container equals instead
*/
template<class V>
bool GTSAM_DEPRECATED assert_equal(const std::vector<V>& expected, const std::vector<V>& actual, double tol = 1e-9) {
@ -108,6 +109,7 @@ bool GTSAM_DEPRECATED assert_equal(const std::vector<V>& expected, const std::ve
}
return true;
}
#endif
/**
* Function for comparing maps of testable->testable

6
gtsam/base/Vector.h
View File

@ -203,15 +203,16 @@ inline double inner_prod(const V1 &a, const V2& b) {
return a.dot(b);
}
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
/**
* BLAS Level 1 scal: x <- alpha*x
* \deprecated: use operators instead
* @deprecated: use operators instead
*/
inline void GTSAM_DEPRECATED scal(double alpha, Vector& x) { x *= alpha; }
/**
* BLAS Level 1 axpy: y <- alpha*x + y
* \deprecated: use operators instead
* @deprecated: use operators instead
*/
template<class V1, class V2>
inline void GTSAM_DEPRECATED axpy(double alpha, const V1& x, V2& y) {
@ -222,6 +223,7 @@ inline void axpy(double alpha, const Vector& x, SubVector y) {
assert (y.size()==x.size());
y += alpha * x;
}
#endif
/**
* house(x,j) computes HouseHolder vector v and scaling factor beta

152
gtsam/base/deprecated/LieMatrix.h
View File

@ -1,152 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file LieMatrix.h
* @brief A wrapper around Matrix providing Lie compatibility
* @author Richard Roberts and Alex Cunningham
*/
#pragma once
#include <cstdarg>
#include <gtsam/base/VectorSpace.h>
#include <boost/serialization/nvp.hpp>
namespace gtsam {
/**
* @deprecated: LieMatrix, LieVector and LieMatrix are obsolete in GTSAM 4.0 as
* we can directly add double, Vector, and Matrix into values now, because of
* gtsam::traits.
*/
struct LieMatrix : public Matrix {
/// @name Constructors
/// @{
enum { dimension = Eigen::Dynamic };
/** default constructor - only for serialize */
LieMatrix() {}
/** initialize from a normal matrix */
LieMatrix(const Matrix& v) : Matrix(v) {}
template <class M>
LieMatrix(const M& v) : Matrix(v) {}
// Currently TMP constructor causes ICE on MSVS 2013
#if (_MSC_VER < 1800)
/** initialize from a fixed size normal vector */
template<int M, int N>
LieMatrix(const Eigen::Matrix<double, M, N>& v) : Matrix(v) {}
#endif
/** constructor with size and initial data, row order ! */
LieMatrix(size_t m, size_t n, const double* const data) :
Matrix(Eigen::Map<const Matrix>(data, m, n)) {}
/// @}
/// @name Testable interface
/// @{
/** print @param s optional string naming the object */
void print(const std::string& name = "") const {
gtsam::print(matrix(), name);
}
/** equality up to tolerance */
inline bool equals(const LieMatrix& expected, double tol=1e-5) const {
return gtsam::equal_with_abs_tol(matrix(), expected.matrix(), tol);
}
/// @}
/// @name Standard Interface
/// @{
/** get the underlying matrix */
inline Matrix matrix() const {
return static_cast<Matrix>(*this);
}
/// @}
/// @name Group
/// @{
LieMatrix compose(const LieMatrix& q) { return (*this)+q;}
LieMatrix between(const LieMatrix& q) { return q-(*this);}
LieMatrix inverse() { return -(*this);}
/// @}
/// @name Manifold
/// @{
Vector localCoordinates(const LieMatrix& q) { return between(q).vector();}
LieMatrix retract(const Vector& v) {return compose(LieMatrix(v));}
/// @}
/// @name Lie Group
/// @{
static Vector Logmap(const LieMatrix& p) {return p.vector();}
static LieMatrix Expmap(const Vector& v) { return LieMatrix(v);}
/// @}
/// @name VectorSpace requirements
/// @{
/** Returns dimensionality of the tangent space */
inline size_t dim() const { return size(); }
/** Convert to vector, is done row-wise - TODO why? */
inline Vector vector() const {
Vector result(size());
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
Eigen::RowMajor> RowMajor;
Eigen::Map<RowMajor>(&result(0), rows(), cols()) = *this;
return result;
}
/** identity - NOTE: no known size at compile time - so zero length */
inline static LieMatrix identity() {
throw std::runtime_error("LieMatrix::identity(): Don't use this function");
return LieMatrix();
}
/// @}
private:
// Serialization function
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int /*version*/) {
ar & boost::serialization::make_nvp("Matrix",
boost::serialization::base_object<Matrix>(*this));
}
};
template<>
struct traits<LieMatrix> : public internal::VectorSpace<LieMatrix> {
// Override Retract, as the default version does not know how to initialize
static LieMatrix Retract(const LieMatrix& origin, const TangentVector& v,
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
if (H1) *H1 = Eye(origin);
if (H2) *H2 = Eye(origin);
typedef const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
Eigen::RowMajor> RowMajor;
return origin + Eigen::Map<RowMajor>(&v(0), origin.rows(), origin.cols());
}
};
} // \namespace gtsam

88
gtsam/base/deprecated/LieScalar.h
View File

@ -1,88 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file LieScalar.h
* @brief A wrapper around scalar providing Lie compatibility
* @author Kai Ni
*/
#pragma once
#include <gtsam/dllexport.h>
#include <gtsam/base/VectorSpace.h>
#include <iostream>
namespace gtsam {
/**
* @deprecated: LieScalar, LieVector and LieMatrix are obsolete in GTSAM 4.0 as
* we can directly add double, Vector, and Matrix into values now, because of
* gtsam::traits.
*/
struct LieScalar {
enum { dimension = 1 };
/** default constructor */
LieScalar() : d_(0.0) {}
/** wrap a double */
/*explicit*/ LieScalar(double d) : d_(d) {}
/** access the underlying value */
double value() const { return d_; }
/** Automatic conversion to underlying value */
operator double() const { return d_; }
/** convert vector */
Vector1 vector() const { Vector1 v; v<<d_; return v; }
/// @name Testable
/// @{
void print(const std::string& name = "") const {
std::cout << name << ": " << d_ << std::endl;
}
bool equals(const LieScalar& expected, double tol = 1e-5) const {
return std::abs(expected.d_ - d_) <= tol;
}
/// @}
/// @name Group
/// @{
static LieScalar identity() { return LieScalar(0);}
LieScalar compose(const LieScalar& q) { return (*this)+q;}
LieScalar between(const LieScalar& q) { return q-(*this);}
LieScalar inverse() { return -(*this);}
/// @}
/// @name Manifold
/// @{
size_t dim() const { return 1; }
Vector1 localCoordinates(const LieScalar& q) { return between(q).vector();}
LieScalar retract(const Vector1& v) {return compose(LieScalar(v[0]));}
/// @}
/// @name Lie Group
/// @{
static Vector1 Logmap(const LieScalar& p) { return p.vector();}
static LieScalar Expmap(const Vector1& v) { return LieScalar(v[0]);}
/// @}
private:
double d_;
};
template<>
struct traits<LieScalar> : public internal::ScalarTraits<LieScalar> {};
} // \namespace gtsam

121
gtsam/base/deprecated/LieVector.h
View File

@ -1,121 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file LieVector.h
* @brief A wrapper around vector providing Lie compatibility
* @author Alex Cunningham
*/
#pragma once
#include <gtsam/base/VectorSpace.h>
#include <cstdarg>
namespace gtsam {
/**
* @deprecated: LieVector, LieVector and LieMatrix are obsolete in GTSAM 4.0 as
* we can directly add double, Vector, and Matrix into values now, because of
* gtsam::traits.
*/
struct LieVector : public Vector {
enum { dimension = Eigen::Dynamic };
/** default constructor - should be unnecessary */
LieVector() {}
/** initialize from a normal vector */
LieVector(const Vector& v) : Vector(v) {}
template <class V>
LieVector(const V& v) : Vector(v) {}
// Currently TMP constructor causes ICE on MSVS 2013
#if (_MSC_VER < 1800)
/** initialize from a fixed size normal vector */
template<int N>
LieVector(const Eigen::Matrix<double, N, 1>& v) : Vector(v) {}
#endif
/** wrap a double */
LieVector(double d) : Vector((Vector(1) << d).finished()) {}
/** constructor with size and initial data, row order ! */
LieVector(size_t m, const double* const data) : Vector(m) {
for (size_t i = 0; i < m; i++) (*this)(i) = data[i];
}
/// @name Testable
/// @{
void print(const std::string& name="") const {
gtsam::print(vector(), name);
}
bool equals(const LieVector& expected, double tol=1e-5) const {
return gtsam::equal(vector(), expected.vector(), tol);
}
/// @}
/// @name Group
/// @{
LieVector compose(const LieVector& q) { return (*this)+q;}
LieVector between(const LieVector& q) { return q-(*this);}
LieVector inverse() { return -(*this);}
/// @}
/// @name Manifold
/// @{
Vector localCoordinates(const LieVector& q) { return between(q).vector();}
LieVector retract(const Vector& v) {return compose(LieVector(v));}
/// @}
/// @name Lie Group
/// @{
static Vector Logmap(const LieVector& p) {return p.vector();}
static LieVector Expmap(const Vector& v) { return LieVector(v);}
/// @}
/// @name VectorSpace requirements
/// @{
/** get the underlying vector */
Vector vector() const {
return static_cast<Vector>(*this);
}
/** Returns dimensionality of the tangent space */
size_t dim() const { return this->size(); }
/** identity - NOTE: no known size at compile time - so zero length */
static LieVector identity() {
throw std::runtime_error("LieVector::identity(): Don't use this function");
return LieVector();
}
/// @}
private:
// Serialization function
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int /*version*/) {
ar & boost::serialization::make_nvp("Vector",
boost::serialization::base_object<Vector>(*this));
}
};
template<>
struct traits<LieVector> : public internal::VectorSpace<LieVector> {};
} // \namespace gtsam

70
gtsam/base/tests/testLieMatrix.cpp
View File

@ -1,70 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file testLieMatrix.cpp
* @author Richard Roberts
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/deprecated/LieMatrix.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/Manifold.h>
using namespace gtsam;
GTSAM_CONCEPT_TESTABLE_INST(LieMatrix)
GTSAM_CONCEPT_LIE_INST(LieMatrix)
/* ************************************************************************* */
TEST( LieMatrix, construction ) {
Matrix m = (Matrix(2,2) << 1.0,2.0, 3.0,4.0).finished();
LieMatrix lie1(m), lie2(m);
EXPECT(traits<LieMatrix>::GetDimension(m) == 4);
EXPECT(assert_equal(m, lie1.matrix()));
EXPECT(assert_equal(lie1, lie2));
}
/* ************************************************************************* */
TEST( LieMatrix, other_constructors ) {
Matrix init = (Matrix(2,2) << 10.0,20.0, 30.0,40.0).finished();
LieMatrix exp(init);
double data[] = {10,30,20,40};
LieMatrix b(2,2,data);
EXPECT(assert_equal(exp, b));
}
/* ************************************************************************* */
TEST(LieMatrix, retract) {
LieMatrix init((Matrix(2,2) << 1.0,2.0,3.0,4.0).finished());
Vector update = (Vector(4) << 3.0, 4.0, 6.0, 7.0).finished();
LieMatrix expected((Matrix(2,2) << 4.0, 6.0, 9.0, 11.0).finished());
LieMatrix actual = traits<LieMatrix>::Retract(init,update);
EXPECT(assert_equal(expected, actual));
Vector expectedUpdate = update;
Vector actualUpdate = traits<LieMatrix>::Local(init,actual);
EXPECT(assert_equal(expectedUpdate, actualUpdate));
Vector expectedLogmap = (Vector(4) << 1, 2, 3, 4).finished();
Vector actualLogmap = traits<LieMatrix>::Logmap(LieMatrix((Matrix(2,2) << 1.0, 2.0, 3.0, 4.0).finished()));
EXPECT(assert_equal(expectedLogmap, actualLogmap));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

64
gtsam/base/tests/testLieScalar.cpp
View File

@ -1,64 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file testLieScalar.cpp
* @author Kai Ni
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/deprecated/LieScalar.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/Manifold.h>
using namespace gtsam;
GTSAM_CONCEPT_TESTABLE_INST(LieScalar)
GTSAM_CONCEPT_LIE_INST(LieScalar)
const double tol=1e-9;
//******************************************************************************
TEST(LieScalar , Concept) {
BOOST_CONCEPT_ASSERT((IsGroup<LieScalar>));
BOOST_CONCEPT_ASSERT((IsManifold<LieScalar>));
BOOST_CONCEPT_ASSERT((IsLieGroup<LieScalar>));
}
//******************************************************************************
TEST(LieScalar , Invariants) {
LieScalar lie1(2), lie2(3);
CHECK(check_group_invariants(lie1, lie2));
CHECK(check_manifold_invariants(lie1, lie2));
}
/* ************************************************************************* */
TEST( testLieScalar, construction ) {
double d = 2.;
LieScalar lie1(d), lie2(d);
EXPECT_DOUBLES_EQUAL(2., lie1.value(),tol);
EXPECT_DOUBLES_EQUAL(2., lie2.value(),tol);
EXPECT(traits<LieScalar>::dimension == 1);
EXPECT(assert_equal(lie1, lie2));
}
/* ************************************************************************* */
TEST( testLieScalar, localCoordinates ) {
LieScalar lie1(1.), lie2(3.);
Vector1 actual = traits<LieScalar>::Local(lie1, lie2);
EXPECT( assert_equal((Vector)(Vector(1) << 2).finished(), actual));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

66
gtsam/base/tests/testLieVector.cpp
View File

@ -1,66 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file testLieVector.cpp
* @author Alex Cunningham
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/deprecated/LieVector.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/Manifold.h>
using namespace gtsam;
GTSAM_CONCEPT_TESTABLE_INST(LieVector)
GTSAM_CONCEPT_LIE_INST(LieVector)
//******************************************************************************
TEST(LieVector , Concept) {
BOOST_CONCEPT_ASSERT((IsGroup<LieVector>));
BOOST_CONCEPT_ASSERT((IsManifold<LieVector>));
BOOST_CONCEPT_ASSERT((IsLieGroup<LieVector>));
}
//******************************************************************************
TEST(LieVector , Invariants) {
Vector v = Vector3(1.0, 2.0, 3.0);
LieVector lie1(v), lie2(v);
check_manifold_invariants(lie1, lie2);
}
//******************************************************************************
TEST( testLieVector, construction ) {
Vector v = Vector3(1.0, 2.0, 3.0);
LieVector lie1(v), lie2(v);
EXPECT(lie1.dim() == 3);
EXPECT(assert_equal(v, lie1.vector()));
EXPECT(assert_equal(lie1, lie2));
}
//******************************************************************************
TEST( testLieVector, other_constructors ) {
Vector init = Vector2(10.0, 20.0);
LieVector exp(init);
double data[] = { 10, 20 };
LieVector b(2, data);
EXPECT(assert_equal(exp, b));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

4
gtsam/base/tests/testMatrix.cpp
View File

@ -173,7 +173,7 @@ TEST(Matrix, stack )
{
Matrix A = (Matrix(2, 2) << -5.0, 3.0, 00.0, -5.0).finished();
Matrix B = (Matrix(3, 2) << -0.5, 2.1, 1.1, 3.4, 2.6, 7.1).finished();
Matrix AB = stack(2, &A, &B);
Matrix AB = gtsam::stack(2, &A, &B);
Matrix C(5, 2);
for (int i = 0; i < 2; i++)
for (int j = 0; j < 2; j++)
@ -187,7 +187,7 @@ TEST(Matrix, stack )
std::vector<gtsam::Matrix> matrices;
matrices.push_back(A);
matrices.push_back(B);
Matrix AB2 = stack(matrices);
Matrix AB2 = gtsam::stack(matrices);
EQUALITY(C,AB2);
}

35
gtsam/base/tests/testTestableAssertions.cpp
View File

@ -1,35 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file testTestableAssertions
* @author Alex Cunningham
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/deprecated/LieScalar.h>
#include <gtsam/base/TestableAssertions.h>
using namespace gtsam;
/* ************************************************************************* */
TEST( testTestableAssertions, optional ) {
typedef boost::optional<LieScalar> OptionalScalar;
LieScalar x(1.0);
OptionalScalar ox(x), dummy = boost::none;
EXPECT(assert_equal(ox, ox));
EXPECT(assert_equal(x, ox));
EXPECT(assert_equal(dummy, dummy));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

2
gtsam/basis/ParameterMatrix.h
View File

@ -153,7 +153,7 @@ class ParameterMatrix {
return matrix_ * other;
}
/// @name Vector Space requirements, following LieMatrix
/// @name Vector Space requirements
/// @{
/**

2
gtsam/config.h.in
View File

@ -70,7 +70,7 @@
#cmakedefine GTSAM_THROW_CHEIRALITY_EXCEPTION
// Make sure dependent projects that want it can see deprecated functions
#cmakedefine GTSAM_ALLOW_DEPRECATED_SINCE_V41
#cmakedefine GTSAM_ALLOW_DEPRECATED_SINCE_V42
// Support Metis-based nested dissection
#cmakedefine GTSAM_SUPPORT_NESTED_DISSECTION

77
gtsam/discrete/AlgebraicDecisionTree.h
View File

@ -28,11 +28,22 @@ namespace gtsam {
* TODO: consider eliminating this class altogether?
*/
template<typename L>
class AlgebraicDecisionTree: public DecisionTree<L, double> {
class GTSAM_EXPORT AlgebraicDecisionTree: public DecisionTree<L, double> {
/**
* @brief Default method used by `labelFormatter` or `valueFormatter` when printing.
*
* @param x The value passed to format.
* @return std::string
*/
static std::string DefaultFormatter(const L& x) {
std::stringstream ss;
ss << x;
return ss.str();
}
public:
public:
typedef DecisionTree<L, double> Super;
using Base = DecisionTree<L, double>;
/** The Real ring with addition and multiplication */
struct Ring {
@ -60,33 +71,33 @@ namespace gtsam {
};
AlgebraicDecisionTree() :
Super(1.0) {
Base(1.0) {
}
AlgebraicDecisionTree(const Super& add) :
Super(add) {
AlgebraicDecisionTree(const Base& add) :
Base(add) {
}
/** Create a new leaf function splitting on a variable */
AlgebraicDecisionTree(const L& label, double y1, double y2) :
Super(label, y1, y2) {
Base(label, y1, y2) {
}
/** Create a new leaf function splitting on a variable */
AlgebraicDecisionTree(const typename Super::LabelC& labelC, double y1, double y2) :
Super(labelC, y1, y2) {
AlgebraicDecisionTree(const typename Base::LabelC& labelC, double y1, double y2) :
Base(labelC, y1, y2) {
}
/** Create from keys and vector table */
AlgebraicDecisionTree //
(const std::vector<typename Super::LabelC>& labelCs, const std::vector<double>& ys) {
this->root_ = Super::create(labelCs.begin(), labelCs.end(), ys.begin(),
(const std::vector<typename Base::LabelC>& labelCs, const std::vector<double>& ys) {
this->root_ = Base::create(labelCs.begin(), labelCs.end(), ys.begin(),
ys.end());
}
/** Create from keys and string table */
AlgebraicDecisionTree //
(const std::vector<typename Super::LabelC>& labelCs, const std::string& table) {
(const std::vector<typename Base::LabelC>& labelCs, const std::string& table) {
// Convert string to doubles
std::vector<double> ys;
std::istringstream iss(table);
@ -94,23 +105,32 @@ namespace gtsam {
std::istream_iterator<double>(), std::back_inserter(ys));
// now call recursive Create
this->root_ = Super::create(labelCs.begin(), labelCs.end(), ys.begin(),
this->root_ = Base::create(labelCs.begin(), labelCs.end(), ys.begin(),
ys.end());
}
/** Create a new function splitting on a variable */
template<typename Iterator>
AlgebraicDecisionTree(Iterator begin, Iterator end, const L& label) :
Super(nullptr) {
Base(nullptr) {
this->root_ = compose(begin, end, label);
}
/** Convert */
/**
* Convert labels from type M to type L.
*
* @param other: The AlgebraicDecisionTree with label type M to convert.
* @param map: Map from label type M to label type L.
*/
template<typename M>
AlgebraicDecisionTree(const AlgebraicDecisionTree<M>& other,
const std::map<M, L>& map) {
this->root_ = this->template convert<M, double>(other.root_, map,
Ring::id);
const std::map<M, L>& map) {
// Functor for label conversion so we can use `convertFrom`.
std::function<L(const M&)> L_of_M = [&map](const M& label) -> L {
return map.at(label);
};
std::function<double(const double&)> op = Ring::id;
this->root_ = this->template convertFrom(other.root_, L_of_M, op);
}
/** sum */
@ -134,12 +154,31 @@ namespace gtsam {
}
/** sum out variable */
AlgebraicDecisionTree sum(const typename Super::LabelC& labelC) const {
AlgebraicDecisionTree sum(const typename Base::LabelC& labelC) const {
return this->combine(labelC, &Ring::add);
}
/// print method customized to value type `double`.
void print(const std::string& s,
const typename Base::LabelFormatter& labelFormatter =
&DefaultFormatter) const {
auto valueFormatter = [](const double& v) {
return (boost::format("%4.2g") % v).str();
};
Base::print(s, labelFormatter, valueFormatter);
}
/// Equality method customized to value type `double`.
bool equals(const AlgebraicDecisionTree& other, double tol = 1e-9) const {
// lambda for comparison of two doubles upto some tolerance.
auto compare = [tol](double a, double b) {
return std::abs(a - b) < tol;
};
return Base::equals(other, compare);
}
};
// AlgebraicDecisionTree
template<typename T> struct traits<AlgebraicDecisionTree<T>> : public Testable<AlgebraicDecisionTree<T>> {};
}
// namespace gtsam

313
gtsam/discrete/DecisionTree-inl.h
View File

@ -20,21 +20,22 @@
#pragma once
#include <gtsam/discrete/DecisionTree.h>
#include <gtsam/base/Testable.h>
#include <boost/assign/std/vector.hpp>
#include <boost/format.hpp>
#include <boost/noncopyable.hpp>
#include <boost/optional.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/assign/std/vector.hpp>
using boost::assign::operator+=;
#include <boost/type_traits/has_dereference.hpp>
#include <boost/unordered_set.hpp>
#include <boost/noncopyable.hpp>
#include <list>
#include <boost/make_shared.hpp>
#include <cmath>
#include <fstream>
#include <list>
#include <sstream>
using boost::assign::operator+=;
namespace gtsam {
/*********************************************************************************/
@ -76,23 +77,26 @@ namespace gtsam {
}
/** equality up to tolerance */
bool equals(const Node& q, double tol) const override {
const Leaf* other = dynamic_cast<const Leaf*> (&q);
bool equals(const Node& q, const CompareFunc& compare) const override {
const Leaf* other = dynamic_cast<const Leaf*>(&q);
if (!other) return false;
return std::abs(double(this->constant_ - other->constant_)) < tol;
return compare(this->constant_, other->constant_);
}
/** print */
void print(const std::string& s) const override {
bool showZero = true;
if (showZero || constant_) std::cout << s << " Leaf " << constant_ << std::endl;
void print(const std::string& s, const LabelFormatter& labelFormatter,
const ValueFormatter& valueFormatter) const override {
std::cout << s << " Leaf " << valueFormatter(constant_) << std::endl;
}
/** to graphviz file */
void dot(std::ostream& os, bool showZero) const override {
if (showZero || constant_) os << "\"" << this->id() << "\" [label=\""
<< boost::format("%4.2g") % constant_
<< "\", shape=box, rank=sink, height=0.35, fixedsize=true]\n"; // width=0.55,
/** Write graphviz format to stream `os`. */
void dot(std::ostream& os, const LabelFormatter& labelFormatter,
const ValueFormatter& valueFormatter,
bool showZero) const override {
std::string value = valueFormatter(constant_);
if (showZero || value.compare("0"))
os << "\"" << this->id() << "\" [label=\"" << value
<< "\", shape=box, rank=sink, height=0.35, fixedsize=true]\n"; // width=0.55,
}
/** evaluate */
@ -151,7 +155,7 @@ namespace gtsam {
/** incremental allSame */
size_t allSame_;
typedef boost::shared_ptr<const Choice> ChoicePtr;
using ChoicePtr = boost::shared_ptr<const Choice>;
public:
@ -236,32 +240,38 @@ namespace gtsam {
}
/** print (as a tree) */
void print(const std::string& s) const override {
void print(const std::string& s, const LabelFormatter& labelFormatter,
const ValueFormatter& valueFormatter) const override {
std::cout << s << " Choice(";
// std::cout << this << ",";
std::cout << label_ << ") " << std::endl;
std::cout << labelFormatter(label_) << ") " << std::endl;
for (size_t i = 0; i < branches_.size(); i++)
branches_[i]->print((boost::format("%s %d") % s % i).str());
branches_[i]->print((boost::format("%s %d") % s % i).str(),
labelFormatter, valueFormatter);
}
/** output to graphviz (as a a graph) */
void dot(std::ostream& os, bool showZero) const override {
void dot(std::ostream& os, const LabelFormatter& labelFormatter,
const ValueFormatter& valueFormatter,
bool showZero) const override {
os << "\"" << this->id() << "\" [shape=circle, label=\"" << label_
<< "\"]\n";
for (size_t i = 0; i < branches_.size(); i++) {
NodePtr branch = branches_[i];
size_t B = branches_.size();
for (size_t i = 0; i < B; i++) {
const NodePtr& branch = branches_[i];
// Check if zero
if (!showZero) {
const Leaf* leaf = dynamic_cast<const Leaf*> (branch.get());
if (leaf && !leaf->constant()) continue;
const Leaf* leaf = dynamic_cast<const Leaf*>(branch.get());
if (leaf && valueFormatter(leaf->constant()).compare("0")) continue;
}
os << "\"" << this->id() << "\" -> \"" << branch->id() << "\"";
if (i == 0) os << " [style=dashed]";
if (i > 1) os << " [style=bold]";
if (B == 2) {
if (i == 0) os << " [style=dashed]";
if (i > 1) os << " [style=bold]";
}
os << std::endl;
branch->dot(os, showZero);
branch->dot(os, labelFormatter, valueFormatter, showZero);
}
}
@ -275,15 +285,16 @@ namespace gtsam {
return (q.isLeaf() && q.sameLeaf(*this));
}
/** equality up to tolerance */
bool equals(const Node& q, double tol) const override {
const Choice* other = dynamic_cast<const Choice*> (&q);
/** equality */
bool equals(const Node& q, const CompareFunc& compare) const override {
const Choice* other = dynamic_cast<const Choice*>(&q);
if (!other) return false;
if (this->label_ != other->label_) return false;
if (branches_.size() != other->branches_.size()) return false;
// we don't care about shared pointers being equal here
for (size_t i = 0; i < branches_.size(); i++)
if (!(branches_[i]->equals(*(other->branches_[i]), tol))) return false;
if (!(branches_[i]->equals(*(other->branches_[i]), compare)))
return false;
return true;
}
@ -315,7 +326,7 @@ namespace gtsam {
/** apply unary operator */
NodePtr apply(const Unary& op) const override {
boost::shared_ptr<Choice> r(new Choice(label_, *this, op));
auto r = boost::make_shared<Choice>(label_, *this, op);
return Unique(r);
}
@ -330,24 +341,24 @@ namespace gtsam {
// If second argument of binary op is Leaf node, recurse on branches
NodePtr apply_g_op_fL(const Leaf& fL, const Binary& op) const override {
boost::shared_ptr<Choice> h(new Choice(label(), nrChoices()));
for(NodePtr branch: branches_)
h->push_back(fL.apply_f_op_g(*branch, op));
auto h = boost::make_shared<Choice>(label(), nrChoices());
for (auto&& branch : branches_)
h->push_back(fL.apply_f_op_g(*branch, op));
return Unique(h);
}
// If second argument of binary op is Choice, call constructor
NodePtr apply_g_op_fC(const Choice& fC, const Binary& op) const override {
boost::shared_ptr<Choice> h(new Choice(fC, *this, op));
auto h = boost::make_shared<Choice>(fC, *this, op);
return Unique(h);
}
// If second argument of binary op is Leaf
template<typename OP>
NodePtr apply_fC_op_gL(const Leaf& gL, OP op) const {
boost::shared_ptr<Choice> h(new Choice(label(), nrChoices()));
for(const NodePtr& branch: branches_)
h->push_back(branch->apply_f_op_g(gL, op));
auto h = boost::make_shared<Choice>(label(), nrChoices());
for (auto&& branch : branches_)
h->push_back(branch->apply_f_op_g(gL, op));
return Unique(h);
}
@ -357,9 +368,9 @@ namespace gtsam {
return branches_[index]; // choose branch
// second case, not label of interest, just recurse
boost::shared_ptr<Choice> r(new Choice(label_, branches_.size()));
for(const NodePtr& branch: branches_)
r->push_back(branch->choose(label, index));
auto r = boost::make_shared<Choice>(label_, branches_.size());
for (auto&& branch : branches_)
r->push_back(branch->choose(label, index));
return Unique(r);
}
@ -384,10 +395,9 @@ namespace gtsam {
}
/*********************************************************************************/
template<typename L, typename Y>
DecisionTree<L, Y>::DecisionTree(//
const L& label, const Y& y1, const Y& y2) {
boost::shared_ptr<Choice> a(new Choice(label, 2));
template <typename L, typename Y>
DecisionTree<L, Y>::DecisionTree(const L& label, const Y& y1, const Y& y2) {
auto a = boost::make_shared<Choice>(label, 2);
NodePtr l1(new Leaf(y1)), l2(new Leaf(y2));
a->push_back(l1);
a->push_back(l2);
@ -395,12 +405,12 @@ namespace gtsam {
}
/*********************************************************************************/
template<typename L, typename Y>
DecisionTree<L, Y>::DecisionTree(//
const LabelC& labelC, const Y& y1, const Y& y2) {
template <typename L, typename Y>
DecisionTree<L, Y>::DecisionTree(const LabelC& labelC, const Y& y1,
const Y& y2) {
if (labelC.second != 2) throw std::invalid_argument(
"DecisionTree: binary constructor called with non-binary label");
boost::shared_ptr<Choice> a(new Choice(labelC.first, 2));
auto a = boost::make_shared<Choice>(labelC.first, 2);
NodePtr l1(new Leaf(y1)), l2(new Leaf(y2));
a->push_back(l1);
a->push_back(l2);
@ -447,11 +457,22 @@ namespace gtsam {
}
/*********************************************************************************/
template<typename L, typename Y>
template<typename M, typename X>
template <typename L, typename Y>
template <typename X, typename Func>
DecisionTree<L, Y>::DecisionTree(const DecisionTree<L, X>& other,
Func Y_of_X) {
// Define functor for identity mapping of node label.
auto L_of_L = [](const L& label) { return label; };
root_ = convertFrom<L, X>(other.root_, L_of_L, Y_of_X);
}
/*********************************************************************************/
template <typename L, typename Y>
template <typename M, typename X, typename Func>
DecisionTree<L, Y>::DecisionTree(const DecisionTree<M, X>& other,
const std::map<M, L>& map, std::function<Y(const X&)> op) {
root_ = convert(other.root_, map, op);
const std::map<M, L>& map, Func Y_of_X) {
auto L_of_M = [&map](const M& label) -> L { return map.at(label); };
root_ = convertFrom<M, X>(other.root_, L_of_M, Y_of_X);
}
/*********************************************************************************/
@ -480,13 +501,14 @@ namespace gtsam {
// if label is already in correct order, just put together a choice on label
if (!nrChoices || !highestLabel || label > *highestLabel) {
boost::shared_ptr<Choice> choiceOnLabel(new Choice(label, end - begin));
auto choiceOnLabel = boost::make_shared<Choice>(label, end - begin);
for (Iterator it = begin; it != end; it++)
choiceOnLabel->push_back(it->root_);
return Choice::Unique(choiceOnLabel);
} else {
// Set up a new choice on the highest label
boost::shared_ptr<Choice> choiceOnHighestLabel(new Choice(*highestLabel, nrChoices));
auto choiceOnHighestLabel =
boost::make_shared<Choice>(*highestLabel, nrChoices);
// now, for all possible values of highestLabel
for (size_t index = 0; index < nrChoices; index++) {
// make a new set of functions for composing by iterating over the given
@ -545,7 +567,7 @@ namespace gtsam {
std::cout << boost::format("DecisionTree::create: expected %d values but got %d instead") % nrChoices % size << std::endl;
throw std::invalid_argument("DecisionTree::create invalid argument");
}
boost::shared_ptr<Choice> choice(new Choice(begin->first, endY - beginY));
auto choice = boost::make_shared<Choice>(begin->first, endY - beginY);
for (ValueIt y = beginY; y != endY; y++)
choice->push_back(NodePtr(new Leaf(*y)));
return Choice::Unique(choice);
@ -558,56 +580,136 @@ namespace gtsam {
size_t split = size / nrChoices;
for (size_t i = 0; i < nrChoices; i++, beginY += split) {
NodePtr f = create<It, ValueIt>(labelC, end, beginY, beginY + split);
functions += DecisionTree(f);
functions.emplace_back(f);
}
return compose(functions.begin(), functions.end(), begin->first);
}
/*********************************************************************************/
template<typename L, typename Y>
template<typename M, typename X>
typename DecisionTree<L, Y>::NodePtr DecisionTree<L, Y>::convert(
const typename DecisionTree<M, X>::NodePtr& f, const std::map<M, L>& map,
std::function<Y(const X&)> op) {
typedef DecisionTree<M, X> MX;
typedef typename MX::Leaf MXLeaf;
typedef typename MX::Choice MXChoice;
typedef typename MX::NodePtr MXNodePtr;
typedef DecisionTree<L, Y> LY;
template <typename L, typename Y>
template <typename M, typename X>
typename DecisionTree<L, Y>::NodePtr DecisionTree<L, Y>::convertFrom(
const typename DecisionTree<M, X>::NodePtr& f,
std::function<L(const M&)> L_of_M,
std::function<Y(const X&)> Y_of_X) const {
using LY = DecisionTree<L, Y>;
// ugliness below because apparently we can't have templated virtual functions
// If leaf, apply unary conversion "op" and create a unique leaf
const MXLeaf* leaf = dynamic_cast<const MXLeaf*> (f.get());
if (leaf) return NodePtr(new Leaf(op(leaf->constant())));
using MXLeaf = typename DecisionTree<M, X>::Leaf;
if (auto leaf = boost::dynamic_pointer_cast<const MXLeaf>(f))
return NodePtr(new Leaf(Y_of_X(leaf->constant())));
// Check if Choice
boost::shared_ptr<const MXChoice> choice = boost::dynamic_pointer_cast<const MXChoice> (f);
using MXChoice = typename DecisionTree<M, X>::Choice;
auto choice = boost::dynamic_pointer_cast<const MXChoice>(f);
if (!choice) throw std::invalid_argument(
"DecisionTree::Convert: Invalid NodePtr");
// get new label
M oldLabel = choice->label();
L newLabel = map.at(oldLabel);
const M oldLabel = choice->label();
const L newLabel = L_of_M(oldLabel);
// put together via Shannon expansion otherwise not sorted.
std::vector<LY> functions;
for(const MXNodePtr& branch: choice->branches()) {
LY converted(convert<M, X>(branch, map, op));
functions += converted;
for(auto && branch: choice->branches()) {
functions.emplace_back(convertFrom<M, X>(branch, L_of_M, Y_of_X));
}
return LY::compose(functions.begin(), functions.end(), newLabel);
}
/*********************************************************************************/
template<typename L, typename Y>
bool DecisionTree<L, Y>::equals(const DecisionTree& other, double tol) const {
return root_->equals(*other.root_, tol);
// Functor performing depth-first visit without Assignment<L> argument.
template <typename L, typename Y>
struct Visit {
using F = std::function<void(const Y&)>;
Visit(F f) : f(f) {} ///< Construct from folding function.
F f; ///< folding function object.
/// Do a depth-first visit on the tree rooted at node.
void operator()(const typename DecisionTree<L, Y>::NodePtr& node) const {
using Leaf = typename DecisionTree<L, Y>::Leaf;
if (auto leaf = boost::dynamic_pointer_cast<const Leaf>(node))
return f(leaf->constant());
using Choice = typename DecisionTree<L, Y>::Choice;
auto choice = boost::dynamic_pointer_cast<const Choice>(node);
for (auto&& branch : choice->branches()) (*this)(branch); // recurse!
}
};
template <typename L, typename Y>
template <typename Func>
void DecisionTree<L, Y>::visit(Func f) const {
Visit<L, Y> visit(f);
visit(root_);
}
template<typename L, typename Y>
void DecisionTree<L, Y>::print(const std::string& s) const {
root_->print(s);
/*********************************************************************************/
// Functor performing depth-first visit with Assignment<L> argument.
template <typename L, typename Y>
struct VisitWith {
using Choices = Assignment<L>;
using F = std::function<void(const Choices&, const Y&)>;
VisitWith(F f) : f(f) {} ///< Construct from folding function.
Choices choices; ///< Assignment, mutating through recursion.
F f; ///< folding function object.
/// Do a depth-first visit on the tree rooted at node.
void operator()(const typename DecisionTree<L, Y>::NodePtr& node) {
using Leaf = typename DecisionTree<L, Y>::Leaf;
if (auto leaf = boost::dynamic_pointer_cast<const Leaf>(node))
return f(choices, leaf->constant());
using Choice = typename DecisionTree<L, Y>::Choice;
auto choice = boost::dynamic_pointer_cast<const Choice>(node);
for (size_t i = 0; i < choice->nrChoices(); i++) {
choices[choice->label()] = i; // Set assignment for label to i
(*this)(choice->branches()[i]); // recurse!
}
}
};
template <typename L, typename Y>
template <typename Func>
void DecisionTree<L, Y>::visitWith(Func f) const {
VisitWith<L, Y> visit(f);
visit(root_);
}
/*********************************************************************************/
// fold is just done with a visit
template <typename L, typename Y>
template <typename Func, typename X>
X DecisionTree<L, Y>::fold(Func f, X x0) const {
visit([&](const Y& y) { x0 = f(y, x0); });
return x0;
}
/*********************************************************************************/
// labels is just done with a visit
template <typename L, typename Y>
std::set<L> DecisionTree<L, Y>::labels() const {
std::set<L> unique;
auto f = [&](const Assignment<L>& choices, const Y&) {
for (auto&& kv : choices) unique.insert(kv.first);
};
visitWith(f);
return unique;
}
/*********************************************************************************/
template <typename L, typename Y>
bool DecisionTree<L, Y>::equals(const DecisionTree& other,
const CompareFunc& compare) const {
return root_->equals(*other.root_, compare);
}
template <typename L, typename Y>
void DecisionTree<L, Y>::print(const std::string& s,
const LabelFormatter& labelFormatter,
const ValueFormatter& valueFormatter) const {
root_->print(s, labelFormatter, valueFormatter);
}
template<typename L, typename Y>
@ -622,6 +724,11 @@ namespace gtsam {
template<typename L, typename Y>
DecisionTree<L, Y> DecisionTree<L, Y>::apply(const Unary& op) const {
// It is unclear what should happen if tree is empty:
if (empty()) {
throw std::runtime_error(
"DecisionTree::apply(unary op) undefined for empty tree.");
}
return DecisionTree(root_->apply(op));
}
@ -629,6 +736,11 @@ namespace gtsam {
template<typename L, typename Y>
DecisionTree<L, Y> DecisionTree<L, Y>::apply(const DecisionTree& g,
const Binary& op) const {
// It is unclear what should happen if either tree is empty:
if (empty() || g.empty()) {
throw std::runtime_error(
"DecisionTree::apply(binary op) undefined for empty trees.");
}
// apply the operaton on the root of both diagrams
NodePtr h = root_->apply_f_op_g(*g.root_, op);
// create a new class with the resulting root "h"
@ -657,21 +769,36 @@ namespace gtsam {
}
/*********************************************************************************/
template<typename L, typename Y>
void DecisionTree<L, Y>::dot(std::ostream& os, bool showZero) const {
template <typename L, typename Y>
void DecisionTree<L, Y>::dot(std::ostream& os,
const LabelFormatter& labelFormatter,
const ValueFormatter& valueFormatter,
bool showZero) const {
os << "digraph G {\n";
root_->dot(os, showZero);
root_->dot(os, labelFormatter, valueFormatter, showZero);
os << " [ordering=out]}" << std::endl;
}
template<typename L, typename Y>
void DecisionTree<L, Y>::dot(const std::string& name, bool showZero) const {
template <typename L, typename Y>
void DecisionTree<L, Y>::dot(const std::string& name,
const LabelFormatter& labelFormatter,
const ValueFormatter& valueFormatter,
bool showZero) const {
std::ofstream os((name + ".dot").c_str());
dot(os, showZero);
dot(os, labelFormatter, valueFormatter, showZero);
int result = system(
("dot -Tpdf " + name + ".dot -o " + name + ".pdf >& /dev/null").c_str());
if (result==-1) throw std::runtime_error("DecisionTree::dot system call failed");
}
}
template <typename L, typename Y>
std::string DecisionTree<L, Y>::dot(const LabelFormatter& labelFormatter,
const ValueFormatter& valueFormatter,
bool showZero) const {
std::stringstream ss;
dot(ss, labelFormatter, valueFormatter, showZero);
return ss.str();
}
/*********************************************************************************/

171
gtsam/discrete/DecisionTree.h
View File

@ -19,13 +19,16 @@
#pragma once
#include <gtsam/base/types.h>
#include <gtsam/discrete/Assignment.h>
#include <boost/function.hpp>
#include <functional>
#include <iostream>
#include <map>
#include <sstream>
#include <vector>
#include <set>
namespace gtsam {
@ -35,16 +38,26 @@ namespace gtsam {
* Y = function range (any algebra), e.g., bool, int, double
*/
template<typename L, typename Y>
class DecisionTree {
class GTSAM_EXPORT DecisionTree {
protected:
/// Default method for comparison of two objects of type Y.
static bool DefaultCompare(const Y& a, const Y& b) {
return a == b;
}
public:
using LabelFormatter = std::function<std::string(L)>;
using ValueFormatter = std::function<std::string(Y)>;
using CompareFunc = std::function<bool(const Y&, const Y&)>;
/** Handy typedefs for unary and binary function types */
typedef std::function<Y(const Y&)> Unary;
typedef std::function<Y(const Y&, const Y&)> Binary;
using Unary = std::function<Y(const Y&)>;
using Binary = std::function<Y(const Y&, const Y&)>;
/** A label annotated with cardinality */
typedef std::pair<L,size_t> LabelC;
using LabelC = std::pair<L,size_t>;
/** DTs consist of Leaf and Choice nodes, both subclasses of Node */
class Leaf;
@ -53,7 +66,7 @@ namespace gtsam {
/** ------------------------ Node base class --------------------------- */
class Node {
public:
typedef boost::shared_ptr<const Node> Ptr;
using Ptr = boost::shared_ptr<const Node>;
#ifdef DT_DEBUG_MEMORY
static int nrNodes;
@ -77,11 +90,16 @@ namespace gtsam {
const void* id() const { return this; }
// everything else is virtual, no documentation here as internal
virtual void print(const std::string& s = "") const = 0;
virtual void dot(std::ostream& os, bool showZero) const = 0;
virtual void print(const std::string& s,
const LabelFormatter& labelFormatter,
const ValueFormatter& valueFormatter) const = 0;
virtual void dot(std::ostream& os, const LabelFormatter& labelFormatter,
const ValueFormatter& valueFormatter,
bool showZero) const = 0;
virtual bool sameLeaf(const Leaf& q) const = 0;
virtual bool sameLeaf(const Node& q) const = 0;
virtual bool equals(const Node& other, double tol = 1e-9) const = 0;
virtual bool equals(const Node& other, const CompareFunc& compare =
&DefaultCompare) const = 0;
virtual const Y& operator()(const Assignment<L>& x) const = 0;
virtual Ptr apply(const Unary& op) const = 0;
virtual Ptr apply_f_op_g(const Node&, const Binary&) const = 0;
@ -95,9 +113,9 @@ namespace gtsam {
public:
/** A function is a shared pointer to the root of a DT */
typedef typename Node::Ptr NodePtr;
using NodePtr = typename Node::Ptr;
/* a DecisionTree just contains the root */
/// A DecisionTree just contains the root. TODO(dellaert): make protected.
NodePtr root_;
protected:
@ -106,19 +124,29 @@ namespace gtsam {
template<typename It, typename ValueIt>
NodePtr create(It begin, It end, ValueIt beginY, ValueIt endY) const;
/** Convert to a different type */
template<typename M, typename X> NodePtr
convert(const typename DecisionTree<M, X>::NodePtr& f, const std::map<M,
L>& map, std::function<Y(const X&)> op);
/**
* @brief Convert from a DecisionTree<M, X> to DecisionTree<L, Y>.
*
* @tparam M The previous label type.
* @tparam X The previous value type.
* @param f The node pointer to the root of the previous DecisionTree.
* @param L_of_M Functor to convert from label type M to type L.
* @param Y_of_X Functor to convert from value type X to type Y.
* @return NodePtr
*/
template <typename M, typename X>
NodePtr convertFrom(const typename DecisionTree<M, X>::NodePtr& f,
std::function<L(const M&)> L_of_M,
std::function<Y(const X&)> Y_of_X) const;
/** Default constructor */
DecisionTree();
public:
public:
/// @name Standard Constructors
/// @{
/** Default constructor (for serialization) */
DecisionTree();
/** Create a constant */
DecisionTree(const Y& y);
@ -142,20 +170,47 @@ namespace gtsam {
DecisionTree(const L& label, //
const DecisionTree& f0, const DecisionTree& f1);
/** Convert from a different type */
template<typename M, typename X>
DecisionTree(const DecisionTree<M, X>& other,
const std::map<M, L>& map, std::function<Y(const X&)> op);
/**
* @brief Convert from a different value type.
*
* @tparam X The previous value type.
* @param other The DecisionTree to convert from.
* @param Y_of_X Functor to convert from value type X to type Y.
*/
template <typename X, typename Func>
DecisionTree(const DecisionTree<L, X>& other, Func Y_of_X);
/**
* @brief Convert from a different value type X to value type Y, also transate
* labels via map from type M to L.
*
* @tparam M Previous label type.
* @tparam X Previous value type.
* @param other The decision tree to convert.
* @param L_of_M Map from label type M to type L.
* @param Y_of_X Functor to convert from type X to type Y.
*/
template <typename M, typename X, typename Func>
DecisionTree(const DecisionTree<M, X>& other, const std::map<M, L>& map,
Func Y_of_X);
/// @}
/// @name Testable
/// @{
/** GTSAM-style print */
void print(const std::string& s = "DecisionTree") const;
/**
* @brief GTSAM-style print
*
* @param s Prefix string.
* @param labelFormatter Functor to format the node label.
* @param valueFormatter Functor to format the node value.
*/
void print(const std::string& s, const LabelFormatter& labelFormatter,
const ValueFormatter& valueFormatter) const;
// Testable
bool equals(const DecisionTree& other, double tol = 1e-9) const;
bool equals(const DecisionTree& other,
const CompareFunc& compare = &DefaultCompare) const;
/// @}
/// @name Standard Interface
@ -165,12 +220,61 @@ namespace gtsam {
virtual ~DecisionTree() {
}
/// Check if tree is empty.
bool empty() const { return !root_; }
/** equality */
bool operator==(const DecisionTree& q) const;
/** evaluate */
const Y& operator()(const Assignment<L>& x) const;
/**
* @brief Visit all leaves in depth-first fashion.
*
* @param f side-effect taking a value.
*
* Example:
* int sum = 0;
* auto visitor = [&](int y) { sum += y; };
* tree.visitWith(visitor);
*/
template <typename Func>
void visit(Func f) const;
/**
* @brief Visit all leaves in depth-first fashion.
*
* @param f side-effect taking an assignment and a value.
*
* Example:
* int sum = 0;
* auto visitor = [&](const Assignment<L>& choices, int y) { sum += y; };
* tree.visitWith(visitor);
*/
template <typename Func>
void visitWith(Func f) const;
/**
* @brief Fold a binary function over the tree, returning accumulator.
*
* @tparam X type for accumulator.
* @param f binary function: Y * X -> X returning an updated accumulator.
* @param x0 initial value for accumulator.
* @return X final value for accumulator.
*
* @note X is always passed by value.
*
* Example:
* auto add = [](const double& y, double x) { return y + x; };
* double sum = tree.fold(add, 0.0);
*/
template <typename Func, typename X>
X fold(Func f, X x0) const;
/** Retrieve all unique labels as a set. */
std::set<L> labels() const;
/** apply Unary operation "op" to f */
DecisionTree apply(const Unary& op) const;
@ -193,10 +297,17 @@ namespace gtsam {
}
/** output to graphviz format, stream version */
void dot(std::ostream& os, bool showZero = true) const;
void dot(std::ostream& os, const LabelFormatter& labelFormatter,
const ValueFormatter& valueFormatter, bool showZero = true) const;
/** output to graphviz format, open a file */
void dot(const std::string& name, bool showZero = true) const;
void dot(const std::string& name, const LabelFormatter& labelFormatter,
const ValueFormatter& valueFormatter, bool showZero = true) const;
/** output to graphviz format string */
std::string dot(const LabelFormatter& labelFormatter,
const ValueFormatter& valueFormatter,
bool showZero = true) const;
/// @name Advanced Interface
/// @{
@ -214,13 +325,15 @@ namespace gtsam {
/** free versions of apply */
template<typename Y, typename L>
/// Apply unary operator `op` to DecisionTree `f`.
template<typename L, typename Y>
DecisionTree<L, Y> apply(const DecisionTree<L, Y>& f,
const typename DecisionTree<L, Y>::Unary& op) {
return f.apply(op);
}
template<typename Y, typename L>
/// Apply binary operator `op` to DecisionTree `f`.
template<typename L, typename Y>
DecisionTree<L, Y> apply(const DecisionTree<L, Y>& f,
const DecisionTree<L, Y>& g,
const typename DecisionTree<L, Y>::Binary& op) {

106
gtsam/discrete/DecisionTreeFactor.cpp
View File

@ -34,12 +34,13 @@ namespace gtsam {
/* ******************************************************************************** */
DecisionTreeFactor::DecisionTreeFactor(const DiscreteKeys& keys,
const ADT& potentials) :
DiscreteFactor(keys.indices()), Potentials(keys, potentials) {
DiscreteFactor(keys.indices()), ADT(potentials),
cardinalities_(keys.cardinalities()) {
}
/* *************************************************************************/
DecisionTreeFactor::DecisionTreeFactor(const DiscreteConditional& c) :
DiscreteFactor(c.keys()), Potentials(c) {
DiscreteFactor(c.keys()), AlgebraicDecisionTree<Key>(c), cardinalities_(c.cardinalities_) {
}
/* ************************************************************************* */
@ -48,16 +49,24 @@ namespace gtsam {
return false;
}
else {
const DecisionTreeFactor& f(static_cast<const DecisionTreeFactor&>(other));
return Potentials::equals(f, tol);
const auto& f(static_cast<const DecisionTreeFactor&>(other));
return ADT::equals(f, tol);
}
}
/* ************************************************************************* */
double DecisionTreeFactor::safe_div(const double &a, const double &b) {
// The use for safe_div is when we divide the product factor by the sum
// factor. If the product or sum is zero, we accord zero probability to the
// event.
return (a == 0 || b == 0) ? 0 : (a / b);
}
/* ************************************************************************* */
void DecisionTreeFactor::print(const string& s,
const KeyFormatter& formatter) const {
cout << s;
Potentials::print("Potentials:",formatter);
ADT::print("Potentials:",formatter);
}
/* ************************************************************************* */
@ -134,5 +143,90 @@ namespace gtsam {
return boost::make_shared<DecisionTreeFactor>(dkeys, result);
}
/* ************************************************************************* */
/* ************************************************************************* */
std::vector<std::pair<DiscreteValues, double>> DecisionTreeFactor::enumerate() const {
// Get all possible assignments
std::vector<std::pair<Key, size_t>> pairs;
for (auto& key : keys()) {
pairs.emplace_back(key, cardinalities_.at(key));
}
// Reverse to make cartesianProduct output a more natural ordering.
std::vector<std::pair<Key, size_t>> rpairs(pairs.rbegin(), pairs.rend());
const auto assignments = cartesianProduct(rpairs);
// Construct unordered_map with values
std::vector<std::pair<DiscreteValues, double>> result;
for (const auto& assignment : assignments) {
result.emplace_back(assignment, operator()(assignment));
}
return result;
}
/* ************************************************************************* */
static std::string valueFormatter(const double& v) {
return (boost::format("%4.2g") % v).str();
}
/** output to graphviz format, stream version */
void DecisionTreeFactor::dot(std::ostream& os,
const KeyFormatter& keyFormatter,
bool showZero) const {
ADT::dot(os, keyFormatter, valueFormatter, showZero);
}
/** output to graphviz format, open a file */
void DecisionTreeFactor::dot(const std::string& name,
const KeyFormatter& keyFormatter,
bool showZero) const {
ADT::dot(name, keyFormatter, valueFormatter, showZero);
}
/** output to graphviz format string */
std::string DecisionTreeFactor::dot(const KeyFormatter& keyFormatter,
bool showZero) const {
return ADT::dot(keyFormatter, valueFormatter, showZero);
}
/* ************************************************************************* */
string DecisionTreeFactor::markdown(const KeyFormatter& keyFormatter,
const Names& names) const {
stringstream ss;
// Print out header and construct argument for `cartesianProduct`.
ss << "|";
for (auto& key : keys()) {
ss << keyFormatter(key) << "|";
}
ss << "value|\n";
// Print out separator with alignment hints.
ss << "|";
for (size_t j = 0; j < size(); j++) ss << ":-:|";
ss << ":-:|\n";
// Print out all rows.
auto rows = enumerate();
for (const auto& kv : rows) {
ss << "|";
auto assignment = kv.first;
for (auto& key : keys()) {
size_t index = assignment.at(key);
ss << Translate(names, key, index) << "|";
}
ss << kv.second << "|\n";
}
return ss.str();
}
DecisionTreeFactor::DecisionTreeFactor(const DiscreteKeys &keys, const vector<double> &table) :
DiscreteFactor(keys.indices()), AlgebraicDecisionTree<Key>(keys, table),
cardinalities_(keys.cardinalities()) {
}
DecisionTreeFactor::DecisionTreeFactor(const DiscreteKeys &keys, const string &table) :
DiscreteFactor(keys.indices()), AlgebraicDecisionTree<Key>(keys, table),
cardinalities_(keys.cardinalities()) {
}
/* ************************************************************************* */
} // namespace gtsam

68
gtsam/discrete/DecisionTreeFactor.h
View File

@ -19,7 +19,8 @@
#pragma once
#include <gtsam/discrete/DiscreteFactor.h>
#include <gtsam/discrete/Potentials.h>
#include <gtsam/discrete/DiscreteKey.h>
#include <gtsam/discrete/AlgebraicDecisionTree.h>
#include <gtsam/inference/Ordering.h>
#include <boost/shared_ptr.hpp>
@ -35,7 +36,7 @@ namespace gtsam {
/**
* A discrete probabilistic factor
*/
class GTSAM_EXPORT DecisionTreeFactor: public DiscreteFactor, public Potentials {
class GTSAM_EXPORT DecisionTreeFactor: public DiscreteFactor, public AlgebraicDecisionTree<Key> {
public:
@ -43,6 +44,10 @@ namespace gtsam {
typedef DecisionTreeFactor This;
typedef DiscreteFactor Base; ///< Typedef to base class
typedef boost::shared_ptr<DecisionTreeFactor> shared_ptr;
typedef AlgebraicDecisionTree<Key> ADT;
protected:
std::map<Key,size_t> cardinalities_;
public:
@ -55,14 +60,23 @@ namespace gtsam {
/** Constructor from Indices, Ordering, and AlgebraicDecisionDiagram */
DecisionTreeFactor(const DiscreteKeys& keys, const ADT& potentials);
/** Constructor from Indices and (string or doubles) */
template<class SOURCE>
DecisionTreeFactor(const DiscreteKeys& keys, SOURCE table) :
DiscreteFactor(keys.indices()), Potentials(keys, table) {
}
/** Constructor from doubles */
DecisionTreeFactor(const DiscreteKeys& keys, const std::vector<double>& table);
/** Constructor from string */
DecisionTreeFactor(const DiscreteKeys& keys, const std::string& table);
/// Single-key specialization
template <class SOURCE>
DecisionTreeFactor(const DiscreteKey& key, SOURCE table)
: DecisionTreeFactor(DiscreteKeys{key}, table) {}
/// Single-key specialization, with vector of doubles.
DecisionTreeFactor(const DiscreteKey& key, const std::vector<double>& row)
: DecisionTreeFactor(DiscreteKeys{key}, row) {}
/** Construct from a DiscreteConditional type */
DecisionTreeFactor(const DiscreteConditional& c);
explicit DecisionTreeFactor(const DiscreteConditional& c);
/// @}
/// @name Testable
@ -81,7 +95,7 @@ namespace gtsam {
/// Value is just look up in AlgebraicDecisonTree
double operator()(const DiscreteValues& values) const override {
return Potentials::operator()(values);
return ADT::operator()(values);
}
/// multiply two factors
@ -89,6 +103,10 @@ namespace gtsam {
return apply(f, ADT::Ring::mul);
}
static double safe_div(const double& a, const double& b);
size_t cardinality(Key j) const { return cardinalities_.at(j);}
/// divide by factor f (safely)
DecisionTreeFactor operator/(const DecisionTreeFactor& f) const {
return apply(f, safe_div);
@ -162,7 +180,39 @@ namespace gtsam {
// Potentials::reduceWithInverse(inverseReduction);
// }
/// Enumerate all values into a map from values to double.
std::vector<std::pair<DiscreteValues, double>> enumerate() const;
/// @}
/// @name Wrapper support
/// @{
/** output to graphviz format, stream version */
void dot(std::ostream& os,
const KeyFormatter& keyFormatter = DefaultKeyFormatter,
bool showZero = true) const;
/** output to graphviz format, open a file */
void dot(const std::string& name,
const KeyFormatter& keyFormatter = DefaultKeyFormatter,
bool showZero = true) const;
/** output to graphviz format string */
std::string dot(const KeyFormatter& keyFormatter = DefaultKeyFormatter,
bool showZero = true) const;
/**
* @brief Render as markdown table
*
* @param keyFormatter GTSAM-style Key formatter.
* @param names optional, category names corresponding to choices.
* @return std::string a markdown string.
*/
std::string markdown(const KeyFormatter& keyFormatter = DefaultKeyFormatter,
const Names& names = {}) const override;
/// @}
};
// DecisionTreeFactor

13
gtsam/discrete/DiscreteBayesNet.cpp
View File

@ -38,7 +38,7 @@ namespace gtsam {
double DiscreteBayesNet::evaluate(const DiscreteValues & values) const {
// evaluate all conditionals and multiply
double result = 1.0;
for(DiscreteConditional::shared_ptr conditional: *this)
for(const DiscreteConditional::shared_ptr& conditional: *this)
result *= (*conditional)(values);
return result;
}
@ -61,5 +61,16 @@ namespace gtsam {
return result;
}
/* ************************************************************************* */
std::string DiscreteBayesNet::markdown(
const KeyFormatter& keyFormatter,
const DiscreteFactor::Names& names) const {
using std::endl;
std::stringstream ss;
ss << "`DiscreteBayesNet` of size " << size() << endl << endl;
for(const DiscreteConditional::shared_ptr& conditional: *this)
ss << conditional->markdown(keyFormatter, names) << endl;
return ss.str();
}
/* ************************************************************************* */
} // namespace

15
gtsam/discrete/DiscreteBayesNet.h
View File

@ -13,6 +13,7 @@
* @file DiscreteBayesNet.h
* @date Feb 15, 2011
* @author Duy-Nguyen Ta
* @author Frank dellaert
*/
#pragma once
@ -22,6 +23,7 @@
#include <boost/shared_ptr.hpp>
#include <gtsam/inference/BayesNet.h>
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/discrete/DiscretePrior.h>
#include <gtsam/discrete/DiscreteConditional.h>
namespace gtsam {
@ -74,6 +76,11 @@ namespace gtsam {
// Add inherited versions of add.
using Base::add;
/** Add a DiscretePrior using a table or a string */
void add(const DiscreteKey& key, const std::string& spec) {
emplace_shared<DiscretePrior>(key, spec);
}
/** Add a DiscreteCondtional */
template <typename... Args>
void add(Args&&... args) {
@ -97,6 +104,14 @@ namespace gtsam {
DiscreteValues sample() const;
///@}
/// @name Wrapper support
/// @{
/// Render as markdown table.
std::string markdown(const KeyFormatter& keyFormatter = DefaultKeyFormatter,
const DiscreteFactor::Names& names = {}) const;
/// @}
private:
/** Serialization function */

22
gtsam/discrete/DiscreteBayesTree.cpp
View File

@ -55,8 +55,22 @@ namespace gtsam {
return result;
}
} // \namespace gtsam
/* **************************************************************************/
std::string DiscreteBayesTree::markdown(
const KeyFormatter& keyFormatter,
const DiscreteFactor::Names& names) const {
using std::endl;
std::stringstream ss;
ss << "`DiscreteBayesTree` of size " << nodes_.size() << endl << endl;
auto visitor = [&](const DiscreteBayesTreeClique::shared_ptr& clique,
size_t& indent) {
ss << "\n" << clique->conditional()->markdown(keyFormatter, names);
return indent + 1;
};
size_t indent;
treeTraversal::DepthFirstForest(*this, indent, visitor);
return ss.str();
}
/* **************************************************************************/
} // namespace gtsam

13
gtsam/discrete/DiscreteBayesTree.h
View File

@ -72,6 +72,8 @@ class GTSAM_EXPORT DiscreteBayesTree
typedef DiscreteBayesTree This;
typedef boost::shared_ptr<This> shared_ptr;
/// @name Standard interface
/// @{
/** Default constructor, creates an empty Bayes tree */
DiscreteBayesTree() {}
@ -82,10 +84,19 @@ class GTSAM_EXPORT DiscreteBayesTree
double evaluate(const DiscreteValues& values) const;
//** (Preferred) sugar for the above for given DiscreteValues */
double operator()(const DiscreteValues & values) const {
double operator()(const DiscreteValues& values) const {
return evaluate(values);
}
/// @}
/// @name Wrapper support
/// @{
/// Render as markdown table.
std::string markdown(const KeyFormatter& keyFormatter = DefaultKeyFormatter,
const DiscreteFactor::Names& names = {}) const;
/// @}
};
} // namespace gtsam

199
gtsam/discrete/DiscreteConditional.cpp
View File

@ -80,7 +80,7 @@ void DiscreteConditional::print(const string& s,
}
}
cout << ")";
Potentials::print("");
ADT::print("");
cout << endl;
}
@ -97,45 +97,90 @@ bool DiscreteConditional::equals(const DiscreteFactor& other,
}
/* ******************************************************************************** */
Potentials::ADT DiscreteConditional::choose(
const DiscreteValues& parentsValues) const {
static DiscreteConditional::ADT Choose(const DiscreteConditional& conditional,
const DiscreteValues& parentsValues) {
// Get the big decision tree with all the levels, and then go down the
// branches based on the value of the parent variables.
ADT pFS(*this);
DiscreteConditional::ADT adt(conditional);
size_t value;
for (Key j : parents()) {
for (Key j : conditional.parents()) {
try {
value = parentsValues.at(j);
pFS = pFS.choose(j, value); // ADT keeps getting smaller.
} catch (exception&) {
cout << "Key: " << j << " Value: " << value << endl;
adt = adt.choose(j, value); // ADT keeps getting smaller.
} catch (std::out_of_range&) {
parentsValues.print("parentsValues: ");
throw runtime_error("DiscreteConditional::choose: parent value missing");
};
}
return pFS;
return adt;
}
/* ******************************************************************************** */
DecisionTreeFactor::shared_ptr DiscreteConditional::chooseAsFactor(
DecisionTreeFactor::shared_ptr DiscreteConditional::choose(
const DiscreteValues& parentsValues) const {
ADT pFS = choose(parentsValues);
// Get the big decision tree with all the levels, and then go down the
// branches based on the value of the parent variables.
ADT adt(*this);
size_t value;
for (Key j : parents()) {
try {
value = parentsValues.at(j);
adt = adt.choose(j, value); // ADT keeps getting smaller.
} catch (exception&) {
parentsValues.print("parentsValues: ");
throw runtime_error("DiscreteConditional::choose: parent value missing");
};
}
// Convert ADT to factor.
if (nrFrontals() != 1) {
throw std::runtime_error("Expected only one frontal variable in choose.");
DiscreteKeys discreteKeys;
for (Key j : frontals()) {
discreteKeys.emplace_back(j, this->cardinality(j));
}
DiscreteKeys keys;
const Key frontalKey = keys_[0];
size_t frontalCardinality = this->cardinality(frontalKey);
keys.push_back(DiscreteKey(frontalKey, frontalCardinality));
return boost::make_shared<DecisionTreeFactor>(keys, pFS);
return boost::make_shared<DecisionTreeFactor>(discreteKeys, adt);
}
/* ******************************************************************************** */
DecisionTreeFactor::shared_ptr DiscreteConditional::likelihood(
const DiscreteValues& frontalValues) const {
// Get the big decision tree with all the levels, and then go down the
// branches based on the value of the frontal variables.
ADT adt(*this);
size_t value;
for (Key j : frontals()) {
try {
value = frontalValues.at(j);
adt = adt.choose(j, value); // ADT keeps getting smaller.
} catch (exception&) {
frontalValues.print("frontalValues: ");
throw runtime_error("DiscreteConditional::choose: frontal value missing");
};
}
// Convert ADT to factor.
DiscreteKeys discreteKeys;
for (Key j : parents()) {
discreteKeys.emplace_back(j, this->cardinality(j));
}
return boost::make_shared<DecisionTreeFactor>(discreteKeys, adt);
}
/* ******************************************************************************** */
DecisionTreeFactor::shared_ptr DiscreteConditional::likelihood(
size_t parent_value) const {
if (nrFrontals() != 1)
throw std::invalid_argument(
"Single value likelihood can only be invoked on single-variable "
"conditional");
DiscreteValues values;
values.emplace(keys_[0], parent_value);
return likelihood(values);
}
/* ******************************************************************************** */
void DiscreteConditional::solveInPlace(DiscreteValues* values) const {
// TODO: Abhijit asks: is this really the fastest way? He thinks it is.
ADT pFS = choose(*values); // P(F|S=parentsValues)
ADT pFS = Choose(*this, *values); // P(F|S=parentsValues)
// Initialize
DiscreteValues mpe;
@ -147,10 +192,10 @@ void DiscreteConditional::solveInPlace(DiscreteValues* values) const {
keys & dk;
}
// Get all Possible Configurations
vector<DiscreteValues> allPosbValues = cartesianProduct(keys);
const auto allPosbValues = cartesianProduct(keys);
// Find the MPE
for(DiscreteValues& frontalVals: allPosbValues) {
for(const auto& frontalVals: allPosbValues) {
double pValueS = pFS(frontalVals); // P(F=value|S=parentsValues)
// Update MPE solution if better
if (pValueS > maxP) {
@ -177,7 +222,7 @@ void DiscreteConditional::sampleInPlace(DiscreteValues* values) const {
size_t DiscreteConditional::solve(const DiscreteValues& parentsValues) const {
// TODO: is this really the fastest way? I think it is.
ADT pFS = choose(parentsValues); // P(F|S=parentsValues)
ADT pFS = Choose(*this, parentsValues); // P(F|S=parentsValues)
// Then, find the max over all remaining
// TODO, only works for one key now, seems horribly slow this way
@ -203,10 +248,14 @@ size_t DiscreteConditional::sample(const DiscreteValues& parentsValues) const {
static mt19937 rng(2); // random number generator
// Get the correct conditional density
ADT pFS = choose(parentsValues); // P(F|S=parentsValues)
ADT pFS = Choose(*this, parentsValues); // P(F|S=parentsValues)
// TODO(Duy): only works for one key now, seems horribly slow this way
assert(nrFrontals() == 1);
if (nrFrontals() != 1) {
throw std::invalid_argument(
"DiscreteConditional::sample can only be called on single variable "
"conditionals");
}
Key key = firstFrontalKey();
size_t nj = cardinality(key);
vector<double> p(nj);
@ -223,5 +272,105 @@ size_t DiscreteConditional::sample(const DiscreteValues& parentsValues) const {
}
/* ******************************************************************************** */
size_t DiscreteConditional::sample(size_t parent_value) const {
if (nrParents() != 1)
throw std::invalid_argument(
"Single value sample() can only be invoked on single-parent "
"conditional");
DiscreteValues values;
values.emplace(keys_.back(), parent_value);
return sample(values);
}
}// namespace
/* ******************************************************************************** */
size_t DiscreteConditional::sample() const {
if (nrParents() != 0)
throw std::invalid_argument(
"sample() can only be invoked on no-parent prior");
DiscreteValues values;
return sample(values);
}
/* ************************************************************************* */
std::string DiscreteConditional::markdown(const KeyFormatter& keyFormatter,
const Names& names) const {
std::stringstream ss;
// Print out signature.
ss << " *P(";
bool first = true;
for (Key key : frontals()) {
if (!first) ss << ",";
ss << keyFormatter(key);
first = false;
}
if (nrParents() == 0) {
// We have no parents, call factor method.
ss << ")*:\n" << std::endl;
ss << DecisionTreeFactor::markdown(keyFormatter, names);
return ss.str();
}
// We have parents, continue signature and do custom print.
ss << "|";
first = true;
for (Key parent : parents()) {
if (!first) ss << ",";
ss << keyFormatter(parent);
first = false;
}
ss << ")*:\n" << std::endl;
// Print out header and construct argument for `cartesianProduct`.
std::vector<std::pair<Key, size_t>> pairs;
ss << "|";
const_iterator it;
for(Key parent: parents()) {
ss << "*" << keyFormatter(parent) << "*|";
pairs.emplace_back(parent, cardinalities_.at(parent));
}
size_t n = 1;
for(Key key: frontals()) {
size_t k = cardinalities_.at(key);
pairs.emplace_back(key, k);
n *= k;
}
std::vector<std::pair<Key, size_t>> slatnorf(pairs.rbegin(),
pairs.rend() - nrParents());
const auto frontal_assignments = cartesianProduct(slatnorf);
for (const auto& a : frontal_assignments) {
for (it = beginFrontals(); it != endFrontals(); ++it) {
size_t index = a.at(*it);
ss << Translate(names, *it, index);
}
ss << "|";
}
ss << "\n";
// Print out separator with alignment hints.
ss << "|";
for (size_t j = 0; j < nrParents() + n; j++) ss << ":-:|";
ss << "\n";
// Print out all rows.
std::vector<std::pair<Key, size_t>> rpairs(pairs.rbegin(), pairs.rend());
const auto assignments = cartesianProduct(rpairs);
size_t count = 0;
for (const auto& a : assignments) {
if (count == 0) {
ss << "|";
for (it = beginParents(); it != endParents(); ++it) {
size_t index = a.at(*it);
ss << Translate(names, *it, index) << "|";
}
}
ss << operator()(a) << "|";
count = (count + 1) % n;
if (count == 0) ss << "\n";
}
return ss.str();
}
/* ************************************************************************* */
} // namespace gtsam

37
gtsam/discrete/DiscreteConditional.h
View File

@ -62,8 +62,6 @@ public:
* conditional probability table (CPT) in 00 01 10 11 order. For
* three-valued, it would be 00 01 02 10 11 12 20 21 22, etc....
*
* The first string is parsed to add a key and parents.
*
* Example: DiscreteConditional P(D, {B,E}, table);
*/
DiscreteConditional(const DiscreteKey& key, const DiscreteKeys& parents,
@ -75,8 +73,7 @@ public:
* probability table (CPT) in 00 01 10 11 order. For three-valued, it would
* be 00 01 02 10 11 12 20 21 22, etc....
*
* The first string is parsed to add a key and parents. The second string
* parses into a table.
* The string is parsed into a Signature::Table.
*
* Example: DiscreteConditional P(D, {B,E}, "9/1 2/8 3/7 1/9");
*/
@ -84,6 +81,10 @@ public:
const std::string& spec)
: DiscreteConditional(Signature(key, parents, spec)) {}
/// No-parent specialization; can also use DiscretePrior.
DiscreteConditional(const DiscreteKey& key, const std::string& spec)
: DiscreteConditional(Signature(key, {}, spec)) {}
/** construct P(X|Y)=P(X,Y)/P(Y) from P(X,Y) and P(Y) */
DiscreteConditional(const DecisionTreeFactor& joint,
const DecisionTreeFactor& marginal);
@ -127,7 +128,7 @@ public:
/// Evaluate, just look up in AlgebraicDecisonTree
double operator()(const DiscreteValues& values) const override {
return Potentials::operator()(values);
return ADT::operator()(values);
}
/** Convert to a factor */
@ -135,13 +136,17 @@ public:
return DecisionTreeFactor::shared_ptr(new DecisionTreeFactor(*this));
}
/** Restrict to given parent values, returns AlgebraicDecisionDiagram */
ADT choose(const DiscreteValues& parentsValues) const;
/** Restrict to given parent values, returns DecisionTreeFactor */
DecisionTreeFactor::shared_ptr chooseAsFactor(
DecisionTreeFactor::shared_ptr choose(
const DiscreteValues& parentsValues) const;
/** Convert to a likelihood factor by providing value before bar. */
DecisionTreeFactor::shared_ptr likelihood(
const DiscreteValues& frontalValues) const;
/** Single variable version of likelihood. */
DecisionTreeFactor::shared_ptr likelihood(size_t parent_value) const;
/**
* solve a conditional
* @param parentsValues Known values of the parents
@ -156,6 +161,13 @@ public:
*/
size_t sample(const DiscreteValues& parentsValues) const;
/// Single parent version.
size_t sample(size_t parent_value) const;
/// Zero parent version.
size_t sample() const;
/// @}
/// @name Advanced Interface
/// @{
@ -167,7 +179,14 @@ public:
void sampleInPlace(DiscreteValues* parentsValues) const;
/// @}
/// @name Wrapper support
/// @{
/// Render as markdown table.
std::string markdown(const KeyFormatter& keyFormatter = DefaultKeyFormatter,
const Names& names = {}) const override;
/// @}
};
// DiscreteConditional

15
gtsam/discrete/DiscreteFactor.cpp
View File

@ -19,9 +19,20 @@
#include <gtsam/discrete/DiscreteFactor.h>
#include <sstream>
using namespace std;
namespace gtsam {
/* ************************************************************************* */
} // namespace gtsam
string DiscreteFactor::Translate(const Names& names, Key key, size_t index) {
if (names.empty()) {
stringstream ss;
ss << index;
return ss.str();
} else {
return names.at(key)[index];
}
}
} // namespace gtsam

24
gtsam/discrete/DiscreteFactor.h
View File

@ -73,9 +73,6 @@ public:
Base::print(s, formatter);
}
/** Test whether the factor is empty */
virtual bool empty() const { return size() == 0; }
/// @}
/// @name Standard Interface
/// @{
@ -88,6 +85,27 @@ public:
virtual DecisionTreeFactor toDecisionTreeFactor() const = 0;
/// @}
/// @name Wrapper support
/// @{
/// Translation table from values to strings.
using Names = std::map<Key, std::vector<std::string>>;
/// Translate an integer index value for given key to a string.
static std::string Translate(const Names& names, Key key, size_t index);
/**
* @brief Render as markdown table
*
* @param keyFormatter GTSAM-style Key formatter.
* @param names optional, category names corresponding to choices.
* @return std::string a markdown string.
*/
virtual std::string markdown(
const KeyFormatter& keyFormatter = DefaultKeyFormatter,
const Names& names = {}) const = 0;
/// @}
};
// DiscreteFactor

31
gtsam/discrete/DiscreteFactorGraph.cpp
View File

@ -16,15 +16,17 @@
* @author Frank Dellaert
*/
//#define ENABLE_TIMING
#include <gtsam/discrete/DiscreteFactorGraph.h>
#include <gtsam/discrete/DiscreteConditional.h>
#include <gtsam/discrete/DiscreteBayesTree.h>
#include <gtsam/discrete/DiscreteConditional.h>
#include <gtsam/discrete/DiscreteEliminationTree.h>
#include <gtsam/discrete/DiscreteFactorGraph.h>
#include <gtsam/discrete/DiscreteJunctionTree.h>
#include <gtsam/inference/FactorGraph-inst.h>
#include <gtsam/inference/EliminateableFactorGraph-inst.h>
#include <boost/make_shared.hpp>
#include <gtsam/inference/FactorGraph-inst.h>
using std::vector;
using std::string;
using std::map;
namespace gtsam {
@ -64,7 +66,7 @@ namespace gtsam {
}
/* ************************************************************************* */
void DiscreteFactorGraph::print(const std::string& s,
void DiscreteFactorGraph::print(const string& s,
const KeyFormatter& formatter) const {
std::cout << s << std::endl;
std::cout << "size: " << size() << std::endl;
@ -129,6 +131,19 @@ namespace gtsam {
return std::make_pair(cond, sum);
}
/* ************************************************************************* */
} // namespace
/* ************************************************************************* */
string DiscreteFactorGraph::markdown(
const KeyFormatter& keyFormatter,
const DiscreteFactor::Names& names) const {
using std::endl;
std::stringstream ss;
ss << "`DiscreteFactorGraph` of size " << size() << endl << endl;
for (size_t i = 0; i < factors_.size(); i++) {
ss << "factor " << i << ":\n";
ss << factors_[i]->markdown(keyFormatter, names) << endl;
}
return ss.str();
}
/* ************************************************************************* */
} // namespace gtsam

44
gtsam/discrete/DiscreteFactorGraph.h
View File

@ -24,7 +24,10 @@
#include <gtsam/discrete/DecisionTreeFactor.h>
#include <gtsam/discrete/DiscreteBayesNet.h>
#include <gtsam/base/FastSet.h>
#include <boost/make_shared.hpp>
#include <string>
#include <vector>
namespace gtsam {
@ -101,29 +104,12 @@ public:
/// @}
// Add single key decision-tree factor.
template <class SOURCE>
void add(const DiscreteKey& j, SOURCE table) {
DiscreteKeys keys;
keys.push_back(j);
emplace_shared<DecisionTreeFactor>(keys, table);
/** Add a decision-tree factor */
template <typename... Args>
void add(Args&&... args) {
emplace_shared<DecisionTreeFactor>(std::forward<Args>(args)...);
}
// Add binary key decision-tree factor.
template <class SOURCE>
void add(const DiscreteKey& j1, const DiscreteKey& j2, SOURCE table) {
DiscreteKeys keys;
keys.push_back(j1);
keys.push_back(j2);
emplace_shared<DecisionTreeFactor>(keys, table);
}
// Add shared discreteFactor immediately from arguments.
template <class SOURCE>
void add(const DiscreteKeys& keys, SOURCE table) {
emplace_shared<DecisionTreeFactor>(keys, table);
}
/** Return the set of variables involved in the factors (set union) */
KeySet keys() const;
@ -154,6 +140,20 @@ public:
// /** Apply a reduction, which is a remapping of variable indices. */
// GTSAM_EXPORT void reduceWithInverse(const internal::Reduction& inverseReduction);
/// @name Wrapper support
/// @{
/**
* @brief Render as markdown table
*
* @param keyFormatter GTSAM-style Key formatter.
* @param names optional, a map from Key to category names.
* @return std::string a (potentially long) markdown string.
*/
std::string markdown(const KeyFormatter& keyFormatter = DefaultKeyFormatter,
const DiscreteFactor::Names& names = {}) const;
/// @}
}; // \ DiscreteFactorGraph
/// traits

18
gtsam/discrete/DiscreteKey.h
View File

@ -34,32 +34,30 @@ namespace gtsam {
using DiscreteKey = std::pair<Key,size_t>;
/// DiscreteKeys is a set of keys that can be assembled using the & operator
struct DiscreteKeys: public std::vector<DiscreteKey> {
struct GTSAM_EXPORT DiscreteKeys: public std::vector<DiscreteKey> {
// Forward all constructors.
using std::vector<DiscreteKey>::vector;
/// Constructor for serialization
GTSAM_EXPORT DiscreteKeys() : std::vector<DiscreteKey>::vector() {}
DiscreteKeys() : std::vector<DiscreteKey>::vector() {}
/// Construct from a key
GTSAM_EXPORT DiscreteKeys(const DiscreteKey& key) {
push_back(key);
}
explicit DiscreteKeys(const DiscreteKey& key) { push_back(key); }
/// Construct from a vector of keys
GTSAM_EXPORT DiscreteKeys(const std::vector<DiscreteKey>& keys) :
DiscreteKeys(const std::vector<DiscreteKey>& keys) :
std::vector<DiscreteKey>(keys) {
}
/// Construct from cardinalities with default names
GTSAM_EXPORT DiscreteKeys(const std::vector<int>& cs);
DiscreteKeys(const std::vector<int>& cs);
/// Return a vector of indices
GTSAM_EXPORT KeyVector indices() const;
KeyVector indices() const;
/// Return a map from index to cardinality
GTSAM_EXPORT std::map<Key,size_t> cardinalities() const;
std::map<Key,size_t> cardinalities() const;
/// Add a key (non-const!)
DiscreteKeys& operator&(const DiscreteKey& key) {
@ -69,5 +67,5 @@ namespace gtsam {
}; // DiscreteKeys
/// Create a list from two keys
GTSAM_EXPORT DiscreteKeys operator&(const DiscreteKey& key1, const DiscreteKey& key2);
DiscreteKeys operator&(const DiscreteKey& key1, const DiscreteKey& key2);
}

2
gtsam/discrete/DiscreteMarginals.h
View File

@ -29,7 +29,7 @@ namespace gtsam {
/**
* A class for computing marginals of variables in a DiscreteFactorGraph
*/
class DiscreteMarginals {
class GTSAM_EXPORT DiscreteMarginals {
protected:

50
gtsam/discrete/DiscretePrior.cpp Normal file
View File

@ -0,0 +1,50 @@
/* ----------------------------------------------------------------------------
* 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 DiscretePrior.cpp
* @date December 2021
* @author Frank Dellaert
*/
#include <gtsam/discrete/DiscretePrior.h>
namespace gtsam {
void DiscretePrior::print(const std::string& s,
const KeyFormatter& formatter) const {
Base::print(s, formatter);
}
double DiscretePrior::operator()(size_t value) const {
if (nrFrontals() != 1)
throw std::invalid_argument(
"Single value operator can only be invoked on single-variable "
"priors");
DiscreteValues values;
values.emplace(keys_[0], value);
return Base::operator()(values);
}
std::vector<double> DiscretePrior::pmf() const {
if (nrFrontals() != 1)
throw std::invalid_argument(
"DiscretePrior::pmf only defined for single-variable priors");
const size_t nrValues = cardinalities_.at(keys_[0]);
std::vector<double> array;
array.reserve(nrValues);
for (size_t v = 0; v < nrValues; v++) {
array.push_back(operator()(v));
}
return array;
}
} // namespace gtsam

111
gtsam/discrete/DiscretePrior.h Normal file
View File

@ -0,0 +1,111 @@
/* ----------------------------------------------------------------------------
* 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 DiscretePrior.h
* @date December 2021
* @author Frank Dellaert
*/
#pragma once
#include <gtsam/discrete/DiscreteConditional.h>
#include <string>
namespace gtsam {
/**
* A prior probability on a set of discrete variables.
* Derives from DiscreteConditional
*/
class GTSAM_EXPORT DiscretePrior : public DiscreteConditional {
public:
using Base = DiscreteConditional;
/// @name Standard Constructors
/// @{
/// Default constructor needed for serialization.
DiscretePrior() {}
/// Constructor from factor.
DiscretePrior(const DecisionTreeFactor& f) : Base(f.size(), f) {}
/**
* Construct from a Signature.
*
* Example: DiscretePrior P(D % "3/2");
*/
DiscretePrior(const Signature& s) : Base(s) {}
/**
* Construct from key and a Signature::Table specifying the
* conditional probability table (CPT).
*
* Example: DiscretePrior P(D, table);
*/
DiscretePrior(const DiscreteKey& key, const Signature::Table& table)
: Base(Signature(key, {}, table)) {}
/**
* Construct from key and a string specifying the conditional
* probability table (CPT).
*
* Example: DiscretePrior P(D, "9/1 2/8 3/7 1/9");
*/
DiscretePrior(const DiscreteKey& key, const std::string& spec)
: DiscretePrior(Signature(key, {}, spec)) {}
/// @}
/// @name Testable
/// @{
/// GTSAM-style print
void print(
const std::string& s = "Discrete Prior: ",
const KeyFormatter& formatter = DefaultKeyFormatter) const override;
/// @}
/// @name Standard interface
/// @{
/// Evaluate given a single value.
double operator()(size_t value) const;
/// We also want to keep the Base version, taking DiscreteValues:
// TODO(dellaert): does not play well with wrapper!
// using Base::operator();
/// Return entire probability mass function.
std::vector<double> pmf() const;
/**
* solve a conditional
* @return MPE value of the child (1 frontal variable).
*/
size_t solve() const { return Base::solve({}); }
/**
* sample
* @return sample from conditional
*/
size_t sample() const { return Base::sample(); }
/// @}
};
// DiscretePrior
// traits
template <>
struct traits<DiscretePrior> : public Testable<DiscretePrior> {};
} // namespace gtsam

20
gtsam/discrete/DiscreteValues.h
View File

@ -32,7 +32,25 @@ namespace gtsam {
* stores cardinality of a Discrete variable. It should be handled naturally in
* the new class DiscreteValue, as the variable's type (domain)
*/
using DiscreteValues = Assignment<Key>;
class DiscreteValues : public Assignment<Key> {
public:
using Assignment::Assignment; // all constructors
// Define the implicit default constructor.
DiscreteValues() = default;
// Construct from assignment.
DiscreteValues(const Assignment<Key>& a) : Assignment<Key>(a) {}
void print(const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
std::cout << s << ": ";
for (const typename Assignment::value_type& keyValue : *this)
std::cout << "(" << keyFormatter(keyValue.first) << ", "
<< keyValue.second << ")";
std::cout << std::endl;
}
};
// traits
template<> struct traits<DiscreteValues> : public Testable<DiscreteValues> {};

100
gtsam/discrete/Potentials.cpp
View File

@ -1,100 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file Potentials.cpp
* @date March 24, 2011
* @author Frank Dellaert
*/
#include <gtsam/discrete/DecisionTree-inl.h>
#include <gtsam/discrete/Potentials.h>
#include <boost/format.hpp>
#include <string>
using namespace std;
namespace gtsam {
// explicit instantiation
template class DecisionTree<Key, double>;
template class AlgebraicDecisionTree<Key>;
/* ************************************************************************* */
double Potentials::safe_div(const double& a, const double& b) {
// cout << boost::format("%g / %g = %g\n") % a % b % ((a == 0) ? 0 : (a / b));
// The use for safe_div is when we divide the product factor by the sum
// factor. If the product or sum is zero, we accord zero probability to the
// event.
return (a == 0 || b == 0) ? 0 : (a / b);
}
/* ********************************************************************************
*/
Potentials::Potentials() : ADT(1.0) {}
/* ********************************************************************************
*/
Potentials::Potentials(const DiscreteKeys& keys, const ADT& decisionTree)
: ADT(decisionTree), cardinalities_(keys.cardinalities()) {}
/* ************************************************************************* */
bool Potentials::equals(const Potentials& other, double tol) const {
return ADT::equals(other, tol);
}
/* ************************************************************************* */
void Potentials::print(const string& s, const KeyFormatter& formatter) const {
cout << s << "\n Cardinalities: {";
for (const std::pair<const Key,size_t>& key : cardinalities_)
cout << formatter(key.first) << ":" << key.second << ", ";
cout << "}" << endl;
ADT::print(" ");
}
//
// /* ************************************************************************* */
// template<class P>
// void Potentials::remapIndices(const P& remapping) {
// // Permute the _cardinalities (TODO: Inefficient Consider Improving)
// DiscreteKeys keys;
// map<Key, Key> ordering;
//
// // Get the original keys from cardinalities_
// for(const DiscreteKey& key: cardinalities_)
// keys & key;
//
// // Perform Permutation
// for(DiscreteKey& key: keys) {
// ordering[key.first] = remapping[key.first];
// key.first = ordering[key.first];
// }
//
// // Change *this
// AlgebraicDecisionTree<Key> permuted((*this), ordering);
// *this = permuted;
// cardinalities_ = keys.cardinalities();
// }
//
// /* ************************************************************************* */
// void Potentials::permuteWithInverse(const Permutation& inversePermutation) {
// remapIndices(inversePermutation);
// }
//
// /* ************************************************************************* */
// void Potentials::reduceWithInverse(const internal::Reduction& inverseReduction) {
// remapIndices(inverseReduction);
// }
/* ************************************************************************* */
} // namespace gtsam

97
gtsam/discrete/Potentials.h
View File

@ -1,97 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file Potentials.h
* @date March 24, 2011
* @author Frank Dellaert
*/
#pragma once
#include <gtsam/discrete/AlgebraicDecisionTree.h>
#include <gtsam/discrete/DiscreteKey.h>
#include <gtsam/inference/Key.h>
#include <boost/shared_ptr.hpp>
#include <set>
namespace gtsam {
/**
* A base class for both DiscreteFactor and DiscreteConditional
*/
class Potentials: public AlgebraicDecisionTree<Key> {
public:
typedef AlgebraicDecisionTree<Key> ADT;
protected:
/// Cardinality for each key, used in combine
std::map<Key,size_t> cardinalities_;
/** Constructor from ColumnIndex, and ADT */
Potentials(const ADT& potentials) :
ADT(potentials) {
}
// Safe division for probabilities
GTSAM_EXPORT static double safe_div(const double& a, const double& b);
// // Apply either a permutation or a reduction
// template<class P>
// void remapIndices(const P& remapping);
public:
/** Default constructor for I/O */
GTSAM_EXPORT Potentials();
/** Constructor from Indices and ADT */
GTSAM_EXPORT Potentials(const DiscreteKeys& keys, const ADT& decisionTree);
/** Constructor from Indices and (string or doubles) */
template<class SOURCE>
Potentials(const DiscreteKeys& keys, SOURCE table) :
ADT(keys, table), cardinalities_(keys.cardinalities()) {
}
// Testable
GTSAM_EXPORT bool equals(const Potentials& other, double tol = 1e-9) const;
GTSAM_EXPORT void print(const std::string& s = "Potentials: ",
const KeyFormatter& formatter = DefaultKeyFormatter) const;
size_t cardinality(Key j) const { return cardinalities_.at(j);}
// /**
// * @brief Permutes the keys in Potentials
// *
// * This permutes the Indices and performs necessary re-ordering of ADD.
// * This is virtual so that derived types e.g. DecisionTreeFactor can
// * re-implement it.
// */
// GTSAM_EXPORT virtual void permuteWithInverse(const Permutation& inversePermutation);
//
// /**
// * Apply a reduction, which is a remapping of variable indices.
// */
// GTSAM_EXPORT virtual void reduceWithInverse(const internal::Reduction& inverseReduction);
}; // Potentials
// traits
template<> struct traits<Potentials> : public Testable<Potentials> {};
template<> struct traits<Potentials::ADT> : public Testable<Potentials::ADT> {};
} // namespace gtsam

127
gtsam/discrete/discrete.i
View File

@ -30,30 +30,46 @@ class DiscreteFactor {
};
#include <gtsam/discrete/DecisionTreeFactor.h>
virtual class DecisionTreeFactor: gtsam::DiscreteFactor {
virtual class DecisionTreeFactor : gtsam::DiscreteFactor {
DecisionTreeFactor();
DecisionTreeFactor(const gtsam::DiscreteKey& key,
const std::vector<double>& spec);
DecisionTreeFactor(const gtsam::DiscreteKey& key, string table);
DecisionTreeFactor(const gtsam::DiscreteKeys& keys, string table);
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;
double operator()(const gtsam::DiscreteValues& values) const; // TODO(dellaert): why do I have to repeat???
string dot(
const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter,
bool showZero = true) const;
std::vector<std::pair<DiscreteValues, double>> enumerate() const;
string markdown(const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
string markdown(const gtsam::KeyFormatter& keyFormatter,
std::map<gtsam::Key, std::vector<std::string>> names) const;
};
#include <gtsam/discrete/DiscreteConditional.h>
virtual class DiscreteConditional : gtsam::DecisionTreeFactor {
DiscreteConditional();
DiscreteConditional(size_t nFrontals, const gtsam::DecisionTreeFactor& f);
DiscreteConditional(const gtsam::DiscreteKey& key, string spec);
DiscreteConditional(const gtsam::DiscreteKey& key,
const gtsam::DiscreteKeys& parents, string spec);
DiscreteConditional(const gtsam::DiscreteKey& key,
const std::vector<gtsam::DiscreteKey>& parents, string spec);
DiscreteConditional(const gtsam::DecisionTreeFactor& joint,
const gtsam::DecisionTreeFactor& marginal);
DiscreteConditional(const gtsam::DecisionTreeFactor& joint,
const gtsam::DecisionTreeFactor& marginal,
const gtsam::Ordering& orderedKeys);
size_t size() const; // TODO(dellaert): why do I have to repeat???
double operator()(const gtsam::DiscreteValues& values) const; // TODO(dellaert): why do I have to repeat???
void print(string s = "Discrete Conditional\n",
const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
@ -62,18 +78,45 @@ virtual class DiscreteConditional : gtsam::DecisionTreeFactor {
string s = "Discrete Conditional: ",
const gtsam::KeyFormatter& formatter = gtsam::DefaultKeyFormatter) const;
gtsam::DecisionTreeFactor* toFactor() const;
gtsam::DecisionTreeFactor* chooseAsFactor(const gtsam::DiscreteValues& parentsValues) const;
gtsam::DecisionTreeFactor* choose(
const gtsam::DiscreteValues& parentsValues) const;
gtsam::DecisionTreeFactor* likelihood(
const gtsam::DiscreteValues& frontalValues) const;
gtsam::DecisionTreeFactor* likelihood(size_t value) const;
size_t solve(const gtsam::DiscreteValues& parentsValues) const;
size_t sample(const gtsam::DiscreteValues& parentsValues) const;
void solveInPlace(gtsam::DiscreteValues@ parentsValues) const;
void sampleInPlace(gtsam::DiscreteValues@ parentsValues) const;
size_t sample(size_t value) const;
size_t sample() const;
void solveInPlace(gtsam::DiscreteValues @parentsValues) const;
void sampleInPlace(gtsam::DiscreteValues @parentsValues) const;
string markdown(const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
string markdown(const gtsam::KeyFormatter& keyFormatter,
std::map<gtsam::Key, std::vector<std::string>> names) const;
};
#include <gtsam/discrete/DiscretePrior.h>
virtual class DiscretePrior : gtsam::DiscreteConditional {
DiscretePrior();
DiscretePrior(const gtsam::DecisionTreeFactor& f);
DiscretePrior(const gtsam::DiscreteKey& key, string spec);
void print(string s = "Discrete Prior\n",
const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
double operator()(size_t value) const;
std::vector<double> pmf() const;
size_t solve() const;
};
#include <gtsam/discrete/DiscreteBayesNet.h>
class DiscreteBayesNet {
class DiscreteBayesNet {
DiscreteBayesNet();
void add(const gtsam::DiscreteConditional& s);
void add(const gtsam::DiscreteKey& key, string spec);
void add(const gtsam::DiscreteKey& key, const gtsam::DiscreteKeys& parents,
string spec);
void add(const gtsam::DiscreteKey& key,
const gtsam::DiscreteKeys& parents, string spec);
const std::vector<gtsam::DiscreteKey>& parents, string spec);
bool empty() const;
size_t size() const;
gtsam::KeySet keys() const;
@ -82,34 +125,73 @@ class DiscreteBayesNet {
const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
bool equals(const gtsam::DiscreteBayesNet& other, double tol = 1e-9) const;
void saveGraph(string s,
const gtsam::KeyFormatter& keyFormatter =
string dot(const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
void add(const gtsam::DiscreteConditional& s);
void saveGraph(string s, const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
double operator()(const gtsam::DiscreteValues& values) const;
gtsam::DiscreteValues optimize() const;
gtsam::DiscreteValues sample() const;
string markdown(const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
string markdown(const gtsam::KeyFormatter& keyFormatter,
std::map<gtsam::Key, std::vector<std::string>> names) const;
};
#include <gtsam/discrete/DiscreteBayesTree.h>
class DiscreteBayesTreeClique {
DiscreteBayesTreeClique();
DiscreteBayesTreeClique(const gtsam::DiscreteConditional* conditional);
const gtsam::DiscreteConditional* conditional() const;
bool isRoot() const;
void printSignature(
const string& s = "Clique: ",
const gtsam::KeyFormatter& formatter = gtsam::DefaultKeyFormatter) const;
double evaluate(const gtsam::DiscreteValues& values) const;
};
class DiscreteBayesTree {
DiscreteBayesTree();
void print(string s = "DiscreteBayesTree\n",
const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
bool equals(const gtsam::DiscreteBayesTree& other, double tol = 1e-9) const;
size_t size() const;
bool empty() const;
const DiscreteBayesTreeClique* operator[](size_t j) const;
string dot(const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
void saveGraph(string s,
const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
double operator()(const gtsam::DiscreteValues& values) const;
string markdown(const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
string markdown(const gtsam::KeyFormatter& keyFormatter,
std::map<gtsam::Key, std::vector<std::string>> names) const;
};
#include <gtsam/inference/DotWriter.h>
class DotWriter {
DotWriter(double figureWidthInches = 5, double figureHeightInches = 5,
bool plotFactorPoints = true, bool connectKeysToFactor = true,
bool binaryEdges = true);
};
#include <gtsam/discrete/DiscreteFactorGraph.h>
class DiscreteFactorGraph {
DiscreteFactorGraph();
DiscreteFactorGraph(const gtsam::DiscreteBayesNet& bayesNet);
void add(const gtsam::DiscreteKey& j, string table);
void add(const gtsam::DiscreteKey& j1, const gtsam::DiscreteKey& j2, string table);
void add(const gtsam::DiscreteKey& j, const std::vector<double>& spec);
void add(const gtsam::DiscreteKeys& keys, string table);
void add(const std::vector<gtsam::DiscreteKey>& keys, string table);
bool empty() const;
size_t size() const;
gtsam::KeySet keys() const;
@ -117,7 +199,15 @@ class DiscreteFactorGraph {
void print(string s = "") const;
bool equals(const gtsam::DiscreteFactorGraph& fg, double tol = 1e-9) const;
string dot(
const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter,
const gtsam::DotWriter& dotWriter = gtsam::DotWriter()) const;
void saveGraph(
string s,
const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter,
const gtsam::DotWriter& dotWriter = gtsam::DotWriter()) const;
gtsam::DecisionTreeFactor product() const;
double operator()(const gtsam::DiscreteValues& values) const;
gtsam::DiscreteValues optimize() const;
@ -126,6 +216,11 @@ class DiscreteFactorGraph {
gtsam::DiscreteBayesNet eliminateSequential(const gtsam::Ordering& ordering);
gtsam::DiscreteBayesTree eliminateMultifrontal();
gtsam::DiscreteBayesTree eliminateMultifrontal(const gtsam::Ordering& ordering);
string markdown(const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
string markdown(const gtsam::KeyFormatter& keyFormatter,
std::map<gtsam::Key, std::vector<std::string>> names) const;
};
} // namespace gtsam

12
gtsam/discrete/tests/testAlgebraicDecisionTree.cpp
View File

@ -136,8 +136,8 @@ ADT create(const Signature& signature) {
ADT p(signature.discreteKeys(), signature.cpt());
static size_t count = 0;
const DiscreteKey& key = signature.key();
string dotfile = (boost::format("CPT-%03d-%d") % ++count % key.first).str();
dot(p, dotfile);
string DOTfile = (boost::format("CPT-%03d-%d") % ++count % key.first).str();
dot(p, DOTfile);
return p;
}
@ -414,13 +414,13 @@ TEST(ADT, equality_noparser)
// Check straight equality
ADT pA1 = create(A % tableA);
ADT pA2 = create(A % tableA);
EXPECT(pA1 == pA2); // should be equal
EXPECT(pA1.equals(pA2)); // should be equal
// Check equality after apply
ADT pB = create(B % tableB);
ADT pAB1 = apply(pA1, pB, &mul);
ADT pAB2 = apply(pB, pA1, &mul);
EXPECT(pAB2 == pAB1);
EXPECT(pAB2.equals(pAB1));
}
/* ************************************************************************* */
@ -431,13 +431,13 @@ TEST(ADT, equality_parser)
// Check straight equality
ADT pA1 = create(A % "80/20");
ADT pA2 = create(A % "80/20");
EXPECT(pA1 == pA2); // should be equal
EXPECT(pA1.equals(pA2)); // should be equal
// Check equality after apply
ADT pB = create(B % "60/40");
ADT pAB1 = apply(pA1, pB, &mul);
ADT pAB2 = apply(pB, pA1, &mul);
EXPECT(pAB2 == pAB1);
EXPECT(pAB2.equals(pAB1));
}
/* ******************************************************************************** */

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

@ -40,25 +40,69 @@ void dot(const T&f, const string& filename) {
#define DOT(x)(dot(x,#x))
struct Crazy { int a; double b; };
typedef DecisionTree<string,Crazy> CrazyDecisionTree; // check that DecisionTree is actually generic (as it pretends to be)
struct Crazy {
int a;
double b;
};
struct CrazyDecisionTree : public DecisionTree<string, Crazy> {
/// print to stdout
void print(const std::string& s = "") const {
auto keyFormatter = [](const std::string& s) { return s; };
auto valueFormatter = [](const Crazy& v) {
return (boost::format("{%d,%4.2g}") % v.a % v.b).str();
};
DecisionTree<string, Crazy>::print("", keyFormatter, valueFormatter);
}
/// Equality method customized to Crazy node type
bool equals(const CrazyDecisionTree& other, double tol = 1e-9) const {
auto compare = [tol](const Crazy& v, const Crazy& w) {
return v.a == w.a && std::abs(v.b - w.b) < tol;
};
return DecisionTree<string, Crazy>::equals(other, compare);
}
};
// traits
namespace gtsam {
template<> struct traits<CrazyDecisionTree> : public Testable<CrazyDecisionTree> {};
}
GTSAM_CONCEPT_TESTABLE_INST(CrazyDecisionTree)
/* ******************************************************************************** */
// Test string labels and int range
/* ******************************************************************************** */
typedef DecisionTree<string, int> DT;
struct DT : public DecisionTree<string, int> {
using Base = DecisionTree<string, int>;
using DecisionTree::DecisionTree;
DT() = default;
DT(const Base& dt) : Base(dt) {}
/// print to stdout
void print(const std::string& s = "") const {
auto keyFormatter = [](const std::string& s) { return s; };
auto valueFormatter = [](const int& v) {
return (boost::format("%d") % v).str();
};
Base::print("", keyFormatter, valueFormatter);
}
/// Equality method customized to int node type
bool equals(const Base& other, double tol = 1e-9) const {
auto compare = [](const int& v, const int& w) { return v == w; };
return Base::equals(other, compare);
}
};
// traits
namespace gtsam {
template<> struct traits<DT> : public Testable<DT> {};
}
GTSAM_CONCEPT_TESTABLE_INST(DT)
struct Ring {
static inline int zero() {
return 0;
@ -66,6 +110,9 @@ struct Ring {
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;
}
@ -76,8 +123,7 @@ struct Ring {
/* ******************************************************************************** */
// test DT
TEST(DT, example)
{
TEST(DecisionTree, example) {
// Create labels
string A("A"), B("B"), C("C");
@ -88,6 +134,9 @@ TEST(DT, example)
x10[A] = 1, x10[B] = 0;
x11[A] = 1, x11[B] = 1;
// empty
DT empty;
// A
DT a(A, 0, 5);
LONGS_EQUAL(0,a(x00))
@ -106,6 +155,11 @@ TEST(DT, example)
LONGS_EQUAL(5,notb(x10))
DOT(notb);
// Check supplying empty trees yields an exception
CHECK_EXCEPTION(apply(empty, &Ring::id), std::runtime_error);
CHECK_EXCEPTION(apply(empty, a, &Ring::mul), std::runtime_error);
CHECK_EXCEPTION(apply(a, empty, &Ring::mul), std::runtime_error);
// apply, two nodes, in natural order
DT anotb = apply(a, notb, &Ring::mul);
LONGS_EQUAL(0,anotb(x00))
@ -175,17 +229,34 @@ TEST(DT, example)
}
/* ******************************************************************************** */
// test Conversion
// test Conversion of values
bool bool_of_int(const int& y) { return y != 0; };
typedef DecisionTree<string, bool> StringBoolTree;
TEST(DecisionTree, ConvertValuesOnly) {
// Create labels
string A("A"), B("B");
// apply, two nodes, in natural order
DT f1 = apply(DT(A, 0, 5), DT(B, 5, 0), &Ring::mul);
// convert
StringBoolTree f2(f1, bool_of_int);
// Check a value
Assignment<string> x00;
x00["A"] = 0, x00["B"] = 0;
EXPECT(!f2(x00));
}
/* ******************************************************************************** */
// test Conversion of both values and labels.
enum Label {
U, V, X, Y, Z
};
typedef DecisionTree<Label, bool> BDT;
bool convert(const int& y) {
return y != 0;
}
typedef DecisionTree<Label, bool> LabelBoolTree;
TEST(DT, conversion)
{
TEST(DecisionTree, ConvertBoth) {
// Create labels
string A("A"), B("B");
@ -196,12 +267,9 @@ TEST(DT, conversion)
map<string, Label> ordering;
ordering[A] = X;
ordering[B] = Y;
std::function<bool(const int&)> op = convert;
BDT f2(f1, ordering, op);
// f1.print("f1");
// f2.print("f2");
LabelBoolTree f2(f1, ordering, &bool_of_int);
// create a value
// Check some values
Assignment<Label> x00, x01, x10, x11;
x00[X] = 0, x00[Y] = 0;
x01[X] = 0, x01[Y] = 1;
@ -215,8 +283,7 @@ TEST(DT, conversion)
/* ******************************************************************************** */
// test Compose expansion
TEST(DT, Compose)
{
TEST(DecisionTree, Compose) {
// Create labels
string A("A"), B("B"), C("C");
@ -241,6 +308,73 @@ TEST(DT, Compose)
DOT(f5);
}
/* ******************************************************************************** */
// Check we can create a decision tree of containers.
TEST(DecisionTree, Containers) {
using Container = std::vector<double>;
using StringContainerTree = DecisionTree<string, Container>;
// Check default constructor
StringContainerTree tree;
// Create small two-level tree
string A("A"), B("B"), C("C");
DT stringIntTree(B, DT(A, 0, 1), DT(A, 2, 3));
// Check conversion
auto container_of_int = [](const int& i) {
Container c;
c.emplace_back(i);
return c;
};
StringContainerTree converted(stringIntTree, container_of_int);
}
/* ******************************************************************************** */
// Test visit.
TEST(DecisionTree, visit) {
// Create small two-level tree
string A("A"), B("B"), C("C");
DT tree(B, DT(A, 0, 1), DT(A, 2, 3));
double sum = 0.0;
auto visitor = [&](int y) { sum += y; };
tree.visit(visitor);
EXPECT_DOUBLES_EQUAL(6.0, sum, 1e-9);
}
/* ******************************************************************************** */
// Test visit, with Choices argument.
TEST(DecisionTree, visitWith) {
// Create small two-level tree
string A("A"), B("B"), C("C");
DT tree(B, DT(A, 0, 1), DT(A, 2, 3));
double sum = 0.0;
auto visitor = [&](const Assignment<string>& choices, int y) { sum += y; };
tree.visitWith(visitor);
EXPECT_DOUBLES_EQUAL(6.0, sum, 1e-9);
}
/* ******************************************************************************** */
// Test fold.
TEST(DecisionTree, fold) {
// Create small two-level tree
string A("A"), B("B"), C("C");
DT tree(B, DT(A, 0, 1), DT(A, 2, 3));
auto add = [](const int& y, double x) { return y + x; };
double sum = tree.fold(add, 0.0);
EXPECT_DOUBLES_EQUAL(6.0, sum, 1e-9);
}
/* ******************************************************************************** */
// Test retrieving all labels.
TEST(DecisionTree, labels) {
// Create small two-level tree
string A("A"), B("B"), C("C");
DT tree(B, DT(A, 0, 1), DT(A, 2, 3));
auto labels = tree.labels();
EXPECT_LONGS_EQUAL(2, labels.size());
}
/* ************************************************************************* */
int main() {
TestResult tr;

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

@ -30,19 +30,17 @@ using namespace gtsam;
/* ************************************************************************* */
TEST( DecisionTreeFactor, constructors)
{
// Declare a bunch of keys
DiscreteKey X(0,2), Y(1,3), Z(2,2);
DecisionTreeFactor f1(X, "2 8");
// Create factors
DecisionTreeFactor f1(X, {2, 8});
DecisionTreeFactor f2(X & Y, "2 5 3 6 4 7");
DecisionTreeFactor f3(X & Y & Z, "2 5 3 6 4 7 25 55 35 65 45 75");
EXPECT_LONGS_EQUAL(1,f1.size());
EXPECT_LONGS_EQUAL(2,f2.size());
EXPECT_LONGS_EQUAL(3,f3.size());
// f1.print("f1:");
// f2.print("f2:");
// f3.print("f3:");
DiscreteValues values;
values[0] = 1; // x
values[1] = 2; // y
@ -55,37 +53,26 @@ TEST( DecisionTreeFactor, constructors)
/* ************************************************************************* */
TEST_UNSAFE( DecisionTreeFactor, multiplication)
{
// Declare a bunch of keys
DiscreteKey v0(0,2), v1(1,2), v2(2,2);
// Create a factor
DecisionTreeFactor f1(v0 & v1, "1 2 3 4");
DecisionTreeFactor f2(v1 & v2, "5 6 7 8");
// f1.print("f1:");
// f2.print("f2:");
DecisionTreeFactor expected(v0 & v1 & v2, "5 6 14 16 15 18 28 32");
DecisionTreeFactor actual = f1 * f2;
// actual.print("actual: ");
CHECK(assert_equal(expected, actual));
}
/* ************************************************************************* */
TEST( DecisionTreeFactor, sum_max)
{
// Declare a bunch of keys
DiscreteKey v0(0,3), v1(1,2);
// Create a factor
DecisionTreeFactor f1(v0 & v1, "1 2 3 4 5 6");
DecisionTreeFactor expected(v1, "9 12");
DecisionTreeFactor::shared_ptr actual = f1.sum(1);
CHECK(assert_equal(expected, *actual, 1e-5));
// f1.print("f1:");
// actual->print("actual: ");
// actual->printCache("actual cache: ");
DecisionTreeFactor expected2(v1, "5 6");
DecisionTreeFactor::shared_ptr actual2 = f1.max(1);
@ -93,9 +80,78 @@ TEST( DecisionTreeFactor, sum_max)
DecisionTreeFactor f2(v1 & v0, "1 2 3 4 5 6");
DecisionTreeFactor::shared_ptr actual22 = f2.sum(1);
// f2.print("f2: ");
// actual22->print("actual22: ");
}
/* ************************************************************************* */
// Check enumerate yields the correct list of assignment/value pairs.
TEST(DecisionTreeFactor, enumerate) {
DiscreteKey A(12, 3), B(5, 2);
DecisionTreeFactor f(A & B, "1 2 3 4 5 6");
auto actual = f.enumerate();
std::vector<std::pair<DiscreteValues, double>> expected;
DiscreteValues values;
for (size_t a : {0, 1, 2}) {
for (size_t b : {0, 1}) {
values[12] = a;
values[5] = b;
expected.emplace_back(values, f(values));
}
}
EXPECT(actual == expected);
}
/* ************************************************************************* */
TEST(DiscreteFactorGraph, DotWithNames) {
DiscreteKey A(12, 3), B(5, 2);
DecisionTreeFactor f(A & B, "1 2 3 4 5 6");
auto formatter = [](Key key) { return key == 12 ? "A" : "B"; };
for (bool showZero:{true, false}) {
string actual = f.dot(formatter, showZero);
// pretty weak test, as ids are pointers and not stable across platforms.
string expected = "digraph G {";
EXPECT(actual.substr(0, 11) == expected);
}
}
/* ************************************************************************* */
// Check markdown representation looks as expected.
TEST(DecisionTreeFactor, markdown) {
DiscreteKey A(12, 3), B(5, 2);
DecisionTreeFactor f(A & B, "1 2 3 4 5 6");
string expected =
"|A|B|value|\n"
"|:-:|:-:|:-:|\n"
"|0|0|1|\n"
"|0|1|2|\n"
"|1|0|3|\n"
"|1|1|4|\n"
"|2|0|5|\n"
"|2|1|6|\n";
auto formatter = [](Key key) { return key == 12 ? "A" : "B"; };
string actual = f.markdown(formatter);
EXPECT(actual == expected);
}
/* ************************************************************************* */
// Check markdown representation with a value formatter.
TEST(DecisionTreeFactor, markdownWithValueFormatter) {
DiscreteKey A(12, 3), B(5, 2);
DecisionTreeFactor f(A & B, "1 2 3 4 5 6");
string expected =
"|A|B|value|\n"
"|:-:|:-:|:-:|\n"
"|Zero|-|1|\n"
"|Zero|+|2|\n"
"|One|-|3|\n"
"|One|+|4|\n"
"|Two|-|5|\n"
"|Two|+|6|\n";
auto keyFormatter = [](Key key) { return key == 12 ? "A" : "B"; };
DecisionTreeFactor::Names names{{12, {"Zero", "One", "Two"}},
{5, {"-", "+"}}};
string actual = f.markdown(keyFormatter, names);
EXPECT(actual == expected);
}
/* ************************************************************************* */

67
gtsam/discrete/tests/testDiscreteBayesNet.cpp
View File

@ -38,21 +38,26 @@ using namespace boost::assign;
using namespace std;
using namespace gtsam;
static const DiscreteKey Asia(0, 2), Smoking(4, 2), Tuberculosis(3, 2),
LungCancer(6, 2), Bronchitis(7, 2), Either(5, 2), XRay(2, 2), Dyspnea(1, 2);
using ADT = AlgebraicDecisionTree<Key>;
/* ************************************************************************* */
TEST(DiscreteBayesNet, bayesNet) {
DiscreteBayesNet bayesNet;
DiscreteKey Parent(0, 2), Child(1, 2);
auto prior = boost::make_shared<DiscreteConditional>(Parent % "6/4");
CHECK(assert_equal(Potentials::ADT({Parent}, "0.6 0.4"),
(Potentials::ADT)*prior));
CHECK(assert_equal(ADT({Parent}, "0.6 0.4"),
(ADT)*prior));
bayesNet.push_back(prior);
auto conditional =
boost::make_shared<DiscreteConditional>(Child | Parent = "7/3 8/2");
EXPECT_LONGS_EQUAL(1, *(conditional->beginFrontals()));
Potentials::ADT expected(Child & Parent, "0.7 0.8 0.3 0.2");
CHECK(assert_equal(expected, (Potentials::ADT)*conditional));
ADT expected(Child & Parent, "0.7 0.8 0.3 0.2");
CHECK(assert_equal(expected, (ADT)*conditional));
bayesNet.push_back(conditional);
DiscreteFactorGraph fg(bayesNet);
@ -71,11 +76,9 @@ TEST(DiscreteBayesNet, bayesNet) {
/* ************************************************************************* */
TEST(DiscreteBayesNet, Asia) {
DiscreteBayesNet asia;
DiscreteKey Asia(0, 2), Smoking(4, 2), Tuberculosis(3, 2), LungCancer(6, 2),
Bronchitis(7, 2), Either(5, 2), XRay(2, 2), Dyspnea(1, 2);
asia.add(Asia % "99/1");
asia.add(Smoking % "50/50");
asia.add(Asia, "99/1");
asia.add(Smoking % "50/50"); // Signature version
asia.add(Tuberculosis | Asia = "99/1 95/5");
asia.add(LungCancer | Smoking = "99/1 90/10");
@ -135,7 +138,7 @@ TEST(DiscreteBayesNet, Asia) {
}
/* ************************************************************************* */
TEST_UNSAFE(DiscreteBayesNet, Sugar) {
TEST(DiscreteBayesNet, Sugar) {
DiscreteKey T(0, 2), L(1, 2), E(2, 2), C(8, 3), S(7, 2);
DiscreteBayesNet bn;
@ -149,6 +152,52 @@ TEST_UNSAFE(DiscreteBayesNet, Sugar) {
bn.add(C | S = "1/1/2 5/2/3");
}
/* ************************************************************************* */
TEST(DiscreteBayesNet, Dot) {
DiscreteBayesNet fragment;
fragment.add(Asia % "99/1");
fragment.add(Smoking % "50/50");
fragment.add(Tuberculosis | Asia = "99/1 95/5");
fragment.add(LungCancer | Smoking = "99/1 90/10");
fragment.add((Either | Tuberculosis, LungCancer) = "F T T T");
string actual = fragment.dot();
EXPECT(actual ==
"digraph G{\n"
"0->3\n"
"4->6\n"
"3->5\n"
"6->5\n"
"}");
}
/* ************************************************************************* */
// Check markdown representation looks as expected.
TEST(DiscreteBayesNet, markdown) {
DiscreteBayesNet fragment;
fragment.add(Asia % "99/1");
fragment.add(Smoking | Asia = "8/2 7/3");
string expected =
"`DiscreteBayesNet` of size 2\n"
"\n"
" *P(Asia)*:\n\n"
"|Asia|value|\n"
"|:-:|:-:|\n"
"|0|0.99|\n"
"|1|0.01|\n"
"\n"
" *P(Smoking|Asia)*:\n\n"
"|*Asia*|0|1|\n"
"|:-:|:-:|:-:|\n"
"|0|0.8|0.2|\n"
"|1|0.7|0.3|\n\n";
auto formatter = [](Key key) { return key == 0 ? "Asia" : "Smoking"; };
string actual = fragment.markdown(formatter);
EXPECT(actual == expected);
}
/* ************************************************************************* */
int main() {
TestResult tr;

159
gtsam/discrete/tests/testDiscreteBayesTree.cpp
View File

@ -26,76 +26,89 @@ using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
#include <iostream>
#include <vector>
using namespace std;
using namespace gtsam;
static bool debug = false;
static constexpr bool debug = false;
/* ************************************************************************* */
TEST_UNSAFE(DiscreteBayesTree, ThinTree) {
const int nrNodes = 15;
const size_t nrStates = 2;
// define variables
vector<DiscreteKey> key;
for (int i = 0; i < nrNodes; i++) {
DiscreteKey key_i(i, nrStates);
key.push_back(key_i);
}
// create a thin-tree Bayesnet, a la Jean-Guillaume
struct TestFixture {
vector<DiscreteKey> keys;
DiscreteBayesNet bayesNet;
bayesNet.add(key[14] % "1/3");
boost::shared_ptr<DiscreteBayesTree> bayesTree;
bayesNet.add(key[13] | key[14] = "1/3 3/1");
bayesNet.add(key[12] | key[14] = "3/1 3/1");
/**
* Create a thin-tree Bayesnet, a la Jean-Guillaume Durand (former student),
* and then create the Bayes tree from it.
*/
TestFixture() {
// Define variables.
for (int i = 0; i < 15; i++) {
DiscreteKey key_i(i, 2);
keys.push_back(key_i);
}
bayesNet.add((key[11] | key[13], key[14]) = "1/4 2/3 3/2 4/1");
bayesNet.add((key[10] | key[13], key[14]) = "1/4 3/2 2/3 4/1");
bayesNet.add((key[9] | key[12], key[14]) = "4/1 2/3 F 1/4");
bayesNet.add((key[8] | key[12], key[14]) = "T 1/4 3/2 4/1");
// Create thin-tree Bayesnet.
bayesNet.add(keys[14] % "1/3");
bayesNet.add((key[7] | key[11], key[13]) = "1/4 2/3 3/2 4/1");
bayesNet.add((key[6] | key[11], key[13]) = "1/4 3/2 2/3 4/1");
bayesNet.add((key[5] | key[10], key[13]) = "4/1 2/3 3/2 1/4");
bayesNet.add((key[4] | key[10], key[13]) = "2/3 1/4 3/2 4/1");
bayesNet.add(keys[13] | keys[14] = "1/3 3/1");
bayesNet.add(keys[12] | keys[14] = "3/1 3/1");
bayesNet.add((key[3] | key[9], key[12]) = "1/4 2/3 3/2 4/1");
bayesNet.add((key[2] | key[9], key[12]) = "1/4 8/2 2/3 4/1");
bayesNet.add((key[1] | key[8], key[12]) = "4/1 2/3 3/2 1/4");
bayesNet.add((key[0] | key[8], key[12]) = "2/3 1/4 3/2 4/1");
bayesNet.add((keys[11] | keys[13], keys[14]) = "1/4 2/3 3/2 4/1");
bayesNet.add((keys[10] | keys[13], keys[14]) = "1/4 3/2 2/3 4/1");
bayesNet.add((keys[9] | keys[12], keys[14]) = "4/1 2/3 F 1/4");
bayesNet.add((keys[8] | keys[12], keys[14]) = "T 1/4 3/2 4/1");
bayesNet.add((keys[7] | keys[11], keys[13]) = "1/4 2/3 3/2 4/1");
bayesNet.add((keys[6] | keys[11], keys[13]) = "1/4 3/2 2/3 4/1");
bayesNet.add((keys[5] | keys[10], keys[13]) = "4/1 2/3 3/2 1/4");
bayesNet.add((keys[4] | keys[10], keys[13]) = "2/3 1/4 3/2 4/1");
bayesNet.add((keys[3] | keys[9], keys[12]) = "1/4 2/3 3/2 4/1");
bayesNet.add((keys[2] | keys[9], keys[12]) = "1/4 8/2 2/3 4/1");
bayesNet.add((keys[1] | keys[8], keys[12]) = "4/1 2/3 3/2 1/4");
bayesNet.add((keys[0] | keys[8], keys[12]) = "2/3 1/4 3/2 4/1");
// Create a BayesTree out of the Bayes net.
bayesTree = DiscreteFactorGraph(bayesNet).eliminateMultifrontal();
}
};
/* ************************************************************************* */
TEST(DiscreteBayesTree, ThinTree) {
const TestFixture self;
const auto& keys = self.keys;
if (debug) {
GTSAM_PRINT(bayesNet);
bayesNet.saveGraph("/tmp/discreteBayesNet.dot");
GTSAM_PRINT(self.bayesNet);
self.bayesNet.saveGraph("/tmp/discreteBayesNet.dot");
}
// create a BayesTree out of a Bayes net
auto bayesTree = DiscreteFactorGraph(bayesNet).eliminateMultifrontal();
if (debug) {
GTSAM_PRINT(*bayesTree);
bayesTree->saveGraph("/tmp/discreteBayesTree.dot");
GTSAM_PRINT(*self.bayesTree);
self.bayesTree->saveGraph("/tmp/discreteBayesTree.dot");
}
// Check frontals and parents
for (size_t i : {13, 14, 9, 3, 2, 8, 1, 0, 10, 5, 4}) {
auto clique_i = (*bayesTree)[i];
auto clique_i = (*self.bayesTree)[i];
EXPECT_LONGS_EQUAL(i, *(clique_i->conditional_->beginFrontals()));
}
auto R = bayesTree->roots().front();
auto R = self.bayesTree->roots().front();
// Check whether BN and BT give the same answer on all configurations
vector<DiscreteValues> allPosbValues = cartesianProduct(
key[0] & key[1] & key[2] & key[3] & key[4] & key[5] & key[6] & key[7] &
key[8] & key[9] & key[10] & key[11] & key[12] & key[13] & key[14]);
auto allPosbValues =
cartesianProduct(keys[0] & keys[1] & keys[2] & keys[3] & keys[4] &
keys[5] & keys[6] & keys[7] & keys[8] & keys[9] &
keys[10] & keys[11] & keys[12] & keys[13] & keys[14]);
for (size_t i = 0; i < allPosbValues.size(); ++i) {
DiscreteValues x = allPosbValues[i];
double expected = bayesNet.evaluate(x);
double actual = bayesTree->evaluate(x);
double expected = self.bayesNet.evaluate(x);
double actual = self.bayesTree->evaluate(x);
DOUBLES_EQUAL(expected, actual, 1e-9);
}
@ -107,7 +120,7 @@ TEST_UNSAFE(DiscreteBayesTree, ThinTree) {
joint_11_12_13_14 = 0, joint_9_11_12_13 = 0, joint_8_11_12_13 = 0;
for (size_t i = 0; i < allPosbValues.size(); ++i) {
DiscreteValues x = allPosbValues[i];
double px = bayesTree->evaluate(x);
double px = self.bayesTree->evaluate(x);
for (size_t i = 0; i < 15; i++)
if (x[i]) marginals[i] += px;
if (x[12] && x[14]) {
@ -141,46 +154,46 @@ TEST_UNSAFE(DiscreteBayesTree, ThinTree) {
DiscreteValues all1 = allPosbValues.back();
// check separator marginal P(S0)
auto clique = (*bayesTree)[0];
auto clique = (*self.bayesTree)[0];
DiscreteFactorGraph separatorMarginal0 =
clique->separatorMarginal(EliminateDiscrete);
DOUBLES_EQUAL(joint_8_12, separatorMarginal0(all1), 1e-9);
// check separator marginal P(S9), should be P(14)
clique = (*bayesTree)[9];
clique = (*self.bayesTree)[9];
DiscreteFactorGraph separatorMarginal9 =
clique->separatorMarginal(EliminateDiscrete);
DOUBLES_EQUAL(marginals[14], separatorMarginal9(all1), 1e-9);
// check separator marginal of root, should be empty
clique = (*bayesTree)[11];
clique = (*self.bayesTree)[11];
DiscreteFactorGraph separatorMarginal11 =
clique->separatorMarginal(EliminateDiscrete);
LONGS_EQUAL(0, separatorMarginal11.size());
// check shortcut P(S9||R) to root
clique = (*bayesTree)[9];
clique = (*self.bayesTree)[9];
DiscreteBayesNet shortcut = clique->shortcut(R, EliminateDiscrete);
LONGS_EQUAL(1, shortcut.size());
DOUBLES_EQUAL(joint_11_13_14 / joint_11_13, shortcut.evaluate(all1), 1e-9);
// check shortcut P(S8||R) to root
clique = (*bayesTree)[8];
clique = (*self.bayesTree)[8];
shortcut = clique->shortcut(R, EliminateDiscrete);
DOUBLES_EQUAL(joint_11_12_13_14 / joint_11_13, shortcut.evaluate(all1), 1e-9);
// check shortcut P(S2||R) to root
clique = (*bayesTree)[2];
clique = (*self.bayesTree)[2];
shortcut = clique->shortcut(R, EliminateDiscrete);
DOUBLES_EQUAL(joint_9_11_12_13 / joint_11_13, shortcut.evaluate(all1), 1e-9);
// check shortcut P(S0||R) to root
clique = (*bayesTree)[0];
clique = (*self.bayesTree)[0];
shortcut = clique->shortcut(R, EliminateDiscrete);
DOUBLES_EQUAL(joint_8_11_12_13 / joint_11_13, shortcut.evaluate(all1), 1e-9);
// calculate all shortcuts to root
DiscreteBayesTree::Nodes cliques = bayesTree->nodes();
DiscreteBayesTree::Nodes cliques = self.bayesTree->nodes();
for (auto clique : cliques) {
DiscreteBayesNet shortcut = clique.second->shortcut(R, EliminateDiscrete);
if (debug) {
@ -192,7 +205,7 @@ TEST_UNSAFE(DiscreteBayesTree, ThinTree) {
// Check all marginals
DiscreteFactor::shared_ptr marginalFactor;
for (size_t i = 0; i < 15; i++) {
marginalFactor = bayesTree->marginalFactor(i, EliminateDiscrete);
marginalFactor = self.bayesTree->marginalFactor(i, EliminateDiscrete);
double actual = (*marginalFactor)(all1);
DOUBLES_EQUAL(marginals[i], actual, 1e-9);
}
@ -200,30 +213,60 @@ TEST_UNSAFE(DiscreteBayesTree, ThinTree) {
DiscreteBayesNet::shared_ptr actualJoint;
// Check joint P(8, 2)
actualJoint = bayesTree->jointBayesNet(8, 2, EliminateDiscrete);
actualJoint = self.bayesTree->jointBayesNet(8, 2, EliminateDiscrete);
DOUBLES_EQUAL(joint82, actualJoint->evaluate(all1), 1e-9);
// Check joint P(1, 2)
actualJoint = bayesTree->jointBayesNet(1, 2, EliminateDiscrete);
actualJoint = self.bayesTree->jointBayesNet(1, 2, EliminateDiscrete);
DOUBLES_EQUAL(joint12, actualJoint->evaluate(all1), 1e-9);
// Check joint P(2, 4)
actualJoint = bayesTree->jointBayesNet(2, 4, EliminateDiscrete);
actualJoint = self.bayesTree->jointBayesNet(2, 4, EliminateDiscrete);
DOUBLES_EQUAL(joint24, actualJoint->evaluate(all1), 1e-9);
// Check joint P(4, 5)
actualJoint = bayesTree->jointBayesNet(4, 5, EliminateDiscrete);
actualJoint = self.bayesTree->jointBayesNet(4, 5, EliminateDiscrete);
DOUBLES_EQUAL(joint45, actualJoint->evaluate(all1), 1e-9);
// Check joint P(4, 6)
actualJoint = bayesTree->jointBayesNet(4, 6, EliminateDiscrete);
actualJoint = self.bayesTree->jointBayesNet(4, 6, EliminateDiscrete);
DOUBLES_EQUAL(joint46, actualJoint->evaluate(all1), 1e-9);
// Check joint P(4, 11)
actualJoint = bayesTree->jointBayesNet(4, 11, EliminateDiscrete);
actualJoint = self.bayesTree->jointBayesNet(4, 11, EliminateDiscrete);
DOUBLES_EQUAL(joint_4_11, actualJoint->evaluate(all1), 1e-9);
}
/* ************************************************************************* */
TEST(DiscreteBayesTree, Dot) {
const TestFixture self;
string actual = self.bayesTree->dot();
EXPECT(actual ==
"digraph G{\n"
"0[label=\"13,11,6,7\"];\n"
"0->1\n"
"1[label=\"14 : 11,13\"];\n"
"1->2\n"
"2[label=\"9,12 : 14\"];\n"
"2->3\n"
"3[label=\"3 : 9,12\"];\n"
"2->4\n"
"4[label=\"2 : 9,12\"];\n"
"2->5\n"
"5[label=\"8 : 12,14\"];\n"
"5->6\n"
"6[label=\"1 : 8,12\"];\n"
"5->7\n"
"7[label=\"0 : 8,12\"];\n"
"1->8\n"
"8[label=\"10 : 13,14\"];\n"
"8->9\n"
"9[label=\"5 : 10,13\"];\n"
"8->10\n"
"10[label=\"4 : 10,13\"];\n"
"}");
}
/* ************************************************************************* */
int main() {
TestResult tr;

127
gtsam/discrete/tests/testDiscreteConditional.cpp
View File

@ -10,10 +10,11 @@
* -------------------------------------------------------------------------- */
/*
* @file testDecisionTreeFactor.cpp
* @file testDiscreteConditional.cpp
* @brief unit tests for DiscreteConditional
* @author Duy-Nguyen Ta
* @date Feb 14, 2011
* @author Frank dellaert
* @date Feb 14, 2011
*/
#include <boost/assign/std/map.hpp>
@ -24,29 +25,27 @@ using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
#include <gtsam/discrete/DecisionTreeFactor.h>
#include <gtsam/discrete/DiscreteConditional.h>
#include <gtsam/inference/Symbol.h>
using namespace std;
using namespace gtsam;
/* ************************************************************************* */
TEST( DiscreteConditional, constructors)
{
DiscreteKey X(0, 2), Y(2, 3), Z(1, 2); // watch ordering !
TEST(DiscreteConditional, constructors) {
DiscreteKey X(0, 2), Y(2, 3), Z(1, 2); // watch ordering !
DiscreteConditional expected(X | Y = "1/1 2/3 1/4");
EXPECT_LONGS_EQUAL(0, *(expected.beginFrontals()));
EXPECT_LONGS_EQUAL(2, *(expected.beginParents()));
EXPECT(expected.endParents() == expected.end());
EXPECT(expected.endFrontals() == expected.beginParents());
DiscreteConditional::shared_ptr expected1 = //
boost::make_shared<DiscreteConditional>(X | Y = "1/1 2/3 1/4");
EXPECT(expected1);
EXPECT_LONGS_EQUAL(0, *(expected1->beginFrontals()));
EXPECT_LONGS_EQUAL(2, *(expected1->beginParents()));
EXPECT(expected1->endParents() == expected1->end());
EXPECT(expected1->endFrontals() == expected1->beginParents());
DecisionTreeFactor f1(X & Y, "0.5 0.4 0.2 0.5 0.6 0.8");
DiscreteConditional actual1(1, f1);
EXPECT(assert_equal(*expected1, actual1, 1e-9));
EXPECT(assert_equal(expected, actual1, 1e-9));
DecisionTreeFactor f2(X & Y & Z,
"0.2 0.5 0.3 0.6 0.4 0.7 0.25 0.55 0.35 0.65 0.45 0.75");
DecisionTreeFactor f2(
X & Y & Z, "0.2 0.5 0.3 0.6 0.4 0.7 0.25 0.55 0.35 0.65 0.45 0.75");
DiscreteConditional actual2(1, f2);
EXPECT(assert_equal(f2 / *f2.sum(1), *actual2.toFactor(), 1e-9));
}
@ -101,9 +100,99 @@ TEST(DiscreteConditional, Combine) {
c.push_back(boost::make_shared<DiscreteConditional>(A | B = "1/2 2/1"));
c.push_back(boost::make_shared<DiscreteConditional>(B % "1/2"));
DecisionTreeFactor factor(A & B, "0.111111 0.444444 0.222222 0.222222");
DiscreteConditional actual(2, factor);
auto expected = DiscreteConditional::Combine(c.begin(), c.end());
EXPECT(assert_equal(*expected, actual, 1e-5));
DiscreteConditional expected(2, factor);
auto actual = DiscreteConditional::Combine(c.begin(), c.end());
EXPECT(assert_equal(expected, *actual, 1e-5));
}
/* ************************************************************************* */
TEST(DiscreteConditional, likelihood) {
DiscreteKey X(0, 2), Y(1, 3);
DiscreteConditional conditional(X | Y = "2/8 4/6 5/5");
auto actual0 = conditional.likelihood(0);
DecisionTreeFactor expected0(Y, "0.2 0.4 0.5");
EXPECT(assert_equal(expected0, *actual0, 1e-9));
auto actual1 = conditional.likelihood(1);
DecisionTreeFactor expected1(Y, "0.8 0.6 0.5");
EXPECT(assert_equal(expected1, *actual1, 1e-9));
}
/* ************************************************************************* */
// Check markdown representation looks as expected, no parents.
TEST(DiscreteConditional, markdown_prior) {
DiscreteKey A(Symbol('x', 1), 3);
DiscreteConditional conditional(A % "1/2/2");
string expected =
" *P(x1)*:\n\n"
"|x1|value|\n"
"|:-:|:-:|\n"
"|0|0.2|\n"
"|1|0.4|\n"
"|2|0.4|\n";
string actual = conditional.markdown();
EXPECT(actual == expected);
}
/* ************************************************************************* */
// Check markdown representation looks as expected, no parents + names.
TEST(DiscreteConditional, markdown_prior_names) {
Symbol x1('x', 1);
DiscreteKey A(x1, 3);
DiscreteConditional conditional(A % "1/2/2");
string expected =
" *P(x1)*:\n\n"
"|x1|value|\n"
"|:-:|:-:|\n"
"|A0|0.2|\n"
"|A1|0.4|\n"
"|A2|0.4|\n";
DecisionTreeFactor::Names names{{x1, {"A0", "A1", "A2"}}};
string actual = conditional.markdown(DefaultKeyFormatter, names);
EXPECT(actual == expected);
}
/* ************************************************************************* */
// Check markdown representation looks as expected, multivalued.
TEST(DiscreteConditional, markdown_multivalued) {
DiscreteKey A(Symbol('a', 1), 3), B(Symbol('b', 1), 5);
DiscreteConditional conditional(
A | B = "2/88/10 2/20/78 33/33/34 33/33/34 95/2/3");
string expected =
" *P(a1|b1)*:\n\n"
"|*b1*|0|1|2|\n"
"|:-:|:-:|:-:|:-:|\n"
"|0|0.02|0.88|0.1|\n"
"|1|0.02|0.2|0.78|\n"
"|2|0.33|0.33|0.34|\n"
"|3|0.33|0.33|0.34|\n"
"|4|0.95|0.02|0.03|\n";
string actual = conditional.markdown();
EXPECT(actual == expected);
}
/* ************************************************************************* */
// Check markdown representation looks as expected, two parents + names.
TEST(DiscreteConditional, markdown) {
DiscreteKey A(2, 2), B(1, 2), C(0, 3);
DiscreteConditional conditional(A, {B, C}, "0/1 1/3 1/1 3/1 0/1 1/0");
string expected =
" *P(A|B,C)*:\n\n"
"|*B*|*C*|T|F|\n"
"|:-:|:-:|:-:|:-:|\n"
"|-|Zero|0|1|\n"
"|-|One|0.25|0.75|\n"
"|-|Two|0.5|0.5|\n"
"|+|Zero|0.75|0.25|\n"
"|+|One|0|1|\n"
"|+|Two|1|0|\n";
vector<string> keyNames{"C", "B", "A"};
auto formatter = [keyNames](Key key) { return keyNames[key]; };
DecisionTreeFactor::Names names{
{0, {"Zero", "One", "Two"}}, {1, {"-", "+"}}, {2, {"T", "F"}}};
string actual = conditional.markdown(formatter, names);
EXPECT(actual == expected);
}
/* ************************************************************************* */

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

@ -359,6 +359,92 @@ cout << unicorns;
}
#endif
/* ************************************************************************* */
TEST(DiscreteFactorGraph, Dot) {
// Create Factor graph
DiscreteFactorGraph graph;
DiscreteKey C(0, 2), A(1, 2), B(2, 2);
graph.add(C & A, "0.2 0.8 0.3 0.7");
graph.add(C & B, "0.1 0.9 0.4 0.6");
string actual = graph.dot();
string expected =
"graph {\n"
" size=\"5,5\";\n"
"\n"
" var0[label=\"0\"];\n"
" var1[label=\"1\"];\n"
" var2[label=\"2\"];\n"
"\n"
" var0--var1;\n"
" var0--var2;\n"
"}\n";
EXPECT(actual == expected);
}
/* ************************************************************************* */
TEST(DiscreteFactorGraph, DotWithNames) {
// Create Factor graph
DiscreteFactorGraph graph;
DiscreteKey C(0, 2), A(1, 2), B(2, 2);
graph.add(C & A, "0.2 0.8 0.3 0.7");
graph.add(C & B, "0.1 0.9 0.4 0.6");
vector<string> names{"C", "A", "B"};
auto formatter = [names](Key key) { return names[key]; };
string actual = graph.dot(formatter);
string expected =
"graph {\n"
" size=\"5,5\";\n"
"\n"
" var0[label=\"C\"];\n"
" var1[label=\"A\"];\n"
" var2[label=\"B\"];\n"
"\n"
" var0--var1;\n"
" var0--var2;\n"
"}\n";
EXPECT(actual == expected);
}
/* ************************************************************************* */
// Check markdown representation looks as expected.
TEST(DiscreteFactorGraph, markdown) {
// Create Factor graph
DiscreteFactorGraph graph;
DiscreteKey C(0, 2), A(1, 2), B(2, 2);
graph.add(C & A, "0.2 0.8 0.3 0.7");
graph.add(C & B, "0.1 0.9 0.4 0.6");
string expected =
"`DiscreteFactorGraph` of size 2\n"
"\n"
"factor 0:\n"
"|C|A|value|\n"
"|:-:|:-:|:-:|\n"
"|0|0|0.2|\n"
"|0|1|0.8|\n"
"|1|0|0.3|\n"
"|1|1|0.7|\n"
"\n"
"factor 1:\n"
"|C|B|value|\n"
"|:-:|:-:|:-:|\n"
"|0|0|0.1|\n"
"|0|1|0.9|\n"
"|1|0|0.4|\n"
"|1|1|0.6|\n\n";
vector<string> names{"C", "A", "B"};
auto formatter = [names](Key key) { return names[key]; };
string actual = graph.markdown(formatter);
EXPECT(actual == expected);
// Make sure values are correctly displayed.
DiscreteValues values;
values[0] = 1;
values[1] = 0;
EXPECT_DOUBLES_EQUAL(0.3, graph[0]->operator()(values), 1e-9);
}
/* ************************************************************************* */
int main() {
TestResult tr;

2
gtsam/discrete/tests/testDiscreteMarginals.cpp
View File

@ -164,7 +164,7 @@ TEST_UNSAFE(DiscreteMarginals, truss2) {
graph.add(key[2] & key[3] & key[4], "1 2 3 4 5 6 7 8");
// Calculate the marginals by brute force
vector<DiscreteValues> allPosbValues =
auto allPosbValues =
cartesianProduct(key[0] & key[1] & key[2] & key[3] & key[4]);
Vector T = Z_5x1, F = Z_5x1;
for (size_t i = 0; i < allPosbValues.size(); ++i) {

63
gtsam/discrete/tests/testDiscretePrior.cpp Normal file
View File

@ -0,0 +1,63 @@
/* ----------------------------------------------------------------------------
* 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 testDiscretePrior.cpp
* @brief unit tests for DiscretePrior
* @author Frank dellaert
* @date December 2021
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/discrete/DiscretePrior.h>
#include <gtsam/discrete/Signature.h>
using namespace std;
using namespace gtsam;
static const DiscreteKey X(0, 2);
/* ************************************************************************* */
TEST(DiscretePrior, constructors) {
DiscretePrior actual(X % "2/3");
EXPECT_LONGS_EQUAL(1, actual.nrFrontals());
EXPECT_LONGS_EQUAL(0, actual.nrParents());
DecisionTreeFactor f(X, "0.4 0.6");
DiscretePrior expected(f);
EXPECT(assert_equal(expected, actual, 1e-9));
}
/* ************************************************************************* */
TEST(DiscretePrior, operator) {
DiscretePrior prior(X % "2/3");
EXPECT_DOUBLES_EQUAL(prior(0), 0.4, 1e-9);
EXPECT_DOUBLES_EQUAL(prior(1), 0.6, 1e-9);
}
/* ************************************************************************* */
TEST(DiscretePrior, pmf) {
DiscretePrior prior(X % "2/3");
vector<double> expected {0.4, 0.6};
EXPECT(prior.pmf() == expected);
}
/* ************************************************************************* */
TEST(DiscretePrior, sample) {
DiscretePrior prior(X % "2/3");
prior.sample();
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

4
gtsam/geometry/Cal3.h
View File

@ -170,9 +170,9 @@ class GTSAM_EXPORT Cal3 {
return K;
}
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
/** @deprecated The following function has been deprecated, use K above */
Matrix3 matrix() const { return K(); }
Matrix3 GTSAM_DEPRECATED matrix() const { return K(); }
#endif
/// Return inverted calibration matrix inv(K)

6
gtsam/geometry/Cal3Bundler.h
View File

@ -97,12 +97,12 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 {
Vector3 vector() const;
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
/// get parameter u0
inline double u0() const { return u0_; }
inline double GTSAM_DEPRECATED u0() const { return u0_; }
/// get parameter v0
inline double v0() const { return v0_; }
inline double GTSAM_DEPRECATED v0() const { return v0_; }
#endif
/**

4
gtsam/geometry/Similarity3.cpp
View File

@ -40,6 +40,8 @@ static Point3Pairs subtractCentroids(const Point3Pairs &abPointPairs,
}
/// Form inner products x and y and calculate scale.
// We force the scale to be a non-negative quantity
// (see Section 10.1 of https://ethaneade.com/lie_groups.pdf)
static double calculateScale(const Point3Pairs &d_abPointPairs,
const Rot3 &aRb) {
double x = 0, y = 0;
@ -50,7 +52,7 @@ static double calculateScale(const Point3Pairs &d_abPointPairs,
y += da.transpose() * da_prime;
x += da_prime.transpose() * da_prime;
}
const double s = y / x;
const double s = std::fabs(y / x);
return s;
}

4
gtsam/geometry/SimpleCamera.cpp
View File

@ -21,8 +21,8 @@
namespace gtsam {
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
SimpleCamera simpleCamera(const Matrix34& P) {
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
SimpleCamera GTSAM_DEPRECATED simpleCamera(const Matrix34& P) {
// P = [A|a] = s K cRw [I|-T], with s the unknown scale
Matrix3 A = P.topLeftCorner(3, 3);

2
gtsam/geometry/SimpleCamera.h
View File

@ -37,7 +37,7 @@ namespace gtsam {
using PinholeCameraCal3Unified = gtsam::PinholeCamera<gtsam::Cal3Unified>;
using PinholeCameraCal3Fisheye = gtsam::PinholeCamera<gtsam::Cal3Fisheye>;
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
/**
* @deprecated: SimpleCamera for backwards compatability with GTSAM 3.x
* Use PinholeCameraCal3_S2 instead

28
gtsam/geometry/SphericalCamera.h
View File

@ -43,9 +43,22 @@ class GTSAM_EXPORT EmptyCal {
EmptyCal() {}
virtual ~EmptyCal() = default;
using shared_ptr = boost::shared_ptr<EmptyCal>;
/// return DOF, dimensionality of tangent space
inline static size_t Dim() { return dimension; }
void print(const std::string& s) const {
std::cout << "empty calibration: " << s << std::endl;
}
private:
/// Serialization function
friend class boost::serialization::access;
template <class Archive>
void serialize(Archive& ar, const unsigned int /*version*/) {
ar& boost::serialization::make_nvp(
"EmptyCal", boost::serialization::base_object<EmptyCal>(*this));
}
};
/**
@ -58,9 +71,9 @@ class GTSAM_EXPORT SphericalCamera {
public:
enum { dimension = 6 };
typedef Unit3 Measurement;
typedef std::vector<Unit3> MeasurementVector;
typedef EmptyCal CalibrationType;
using Measurement = Unit3;
using MeasurementVector = std::vector<Unit3>;
using CalibrationType = EmptyCal;
private:
Pose3 pose_; ///< 3D pose of camera
@ -83,8 +96,8 @@ class GTSAM_EXPORT SphericalCamera {
/// Constructor with empty intrinsics (needed for smart factors)
explicit SphericalCamera(const Pose3& pose,
const boost::shared_ptr<EmptyCal>& cal)
: pose_(pose), emptyCal_(boost::make_shared<EmptyCal>()) {}
const EmptyCal::shared_ptr& cal)
: pose_(pose), emptyCal_(cal) {}
/// @}
/// @name Advanced Constructors
@ -95,7 +108,7 @@ class GTSAM_EXPORT SphericalCamera {
virtual ~SphericalCamera() = default;
/// return shared pointer to calibration
const boost::shared_ptr<EmptyCal>& sharedCalibration() const {
const EmptyCal::shared_ptr& sharedCalibration() const {
return emptyCal_;
}
@ -213,6 +226,9 @@ class GTSAM_EXPORT SphericalCamera {
void serialize(Archive& ar, const unsigned int /*version*/) {
ar& BOOST_SERIALIZATION_NVP(pose_);
}
public:
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};
// end of class SphericalCamera

2
gtsam/geometry/tests/testSimpleCamera.cpp
View File

@ -26,7 +26,7 @@
using namespace std;
using namespace gtsam;
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
static const Cal3_S2 K(625, 625, 0, 0, 0);

88
gtsam/geometry/tests/testTriangulation.cpp
View File

@ -182,6 +182,94 @@ TEST(triangulation, fourPoses) {
#endif
}
//******************************************************************************
TEST(triangulation, threePoses_robustNoiseModel) {
Pose3 pose3 = pose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1));
PinholeCamera<Cal3_S2> camera3(pose3, *sharedCal);
Point2 z3 = camera3.project(landmark);
vector<Pose3> poses;
Point2Vector measurements;
poses += pose1, pose2, pose3;
measurements += z1, z2, z3;
// noise free, so should give exactly the landmark
boost::optional<Point3> actual =
triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements);
EXPECT(assert_equal(landmark, *actual, 1e-2));
// Add outlier
measurements.at(0) += Point2(100, 120); // very large pixel noise!
// now estimate does not match landmark
boost::optional<Point3> actual2 = //
triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements);
// DLT is surprisingly robust, but still off (actual error is around 0.26m):
EXPECT( (landmark - *actual2).norm() >= 0.2);
EXPECT( (landmark - *actual2).norm() <= 0.5);
// Again with nonlinear optimization
boost::optional<Point3> actual3 =
triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements, 1e-9, true);
// result from nonlinear (but non-robust optimization) is close to DLT and still off
EXPECT(assert_equal(*actual2, *actual3, 0.1));
// Again with nonlinear optimization, this time with robust loss
auto model = noiseModel::Robust::Create(
noiseModel::mEstimator::Huber::Create(1.345), noiseModel::Unit::Create(2));
boost::optional<Point3> actual4 = triangulatePoint3<Cal3_S2>(
poses, sharedCal, measurements, 1e-9, true, model);
// using the Huber loss we now have a quite small error!! nice!
EXPECT(assert_equal(landmark, *actual4, 0.05));
}
//******************************************************************************
TEST(triangulation, fourPoses_robustNoiseModel) {
Pose3 pose3 = pose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1));
PinholeCamera<Cal3_S2> camera3(pose3, *sharedCal);
Point2 z3 = camera3.project(landmark);
vector<Pose3> poses;
Point2Vector measurements;
poses += pose1, pose1, pose2, pose3; // 2 measurements from pose 1
measurements += z1, z1, z2, z3;
// noise free, so should give exactly the landmark
boost::optional<Point3> actual =
triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements);
EXPECT(assert_equal(landmark, *actual, 1e-2));
// Add outlier
measurements.at(0) += Point2(100, 120); // very large pixel noise!
// add noise on other measurements:
measurements.at(1) += Point2(0.1, 0.2); // small noise
measurements.at(2) += Point2(0.2, 0.2);
measurements.at(3) += Point2(0.3, 0.1);
// now estimate does not match landmark
boost::optional<Point3> actual2 = //
triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements);
// DLT is surprisingly robust, but still off (actual error is around 0.17m):
EXPECT( (landmark - *actual2).norm() >= 0.1);
EXPECT( (landmark - *actual2).norm() <= 0.5);
// Again with nonlinear optimization
boost::optional<Point3> actual3 =
triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements, 1e-9, true);
// result from nonlinear (but non-robust optimization) is close to DLT and still off
EXPECT(assert_equal(*actual2, *actual3, 0.1));
// Again with nonlinear optimization, this time with robust loss
auto model = noiseModel::Robust::Create(
noiseModel::mEstimator::Huber::Create(1.345), noiseModel::Unit::Create(2));
boost::optional<Point3> actual4 = triangulatePoint3<Cal3_S2>(
poses, sharedCal, measurements, 1e-9, true, model);
// using the Huber loss we now have a quite small error!! nice!
EXPECT(assert_equal(landmark, *actual4, 0.05));
}
//******************************************************************************
TEST(triangulation, fourPoses_distinct_Ks) {
Cal3_S2 K1(1500, 1200, 0, 640, 480);

52
gtsam/geometry/triangulation.h
View File

@ -14,6 +14,7 @@
* @brief Functions for triangulation
* @date July 31, 2013
* @author Chris Beall
* @author Luca Carlone
*/
#pragma once
@ -105,18 +106,18 @@ template<class CALIBRATION>
std::pair<NonlinearFactorGraph, Values> triangulationGraph(
const std::vector<Pose3>& poses, boost::shared_ptr<CALIBRATION> sharedCal,
const Point2Vector& measurements, Key landmarkKey,
const Point3& initialEstimate) {
const Point3& initialEstimate,
const SharedNoiseModel& model = nullptr) {
Values values;
values.insert(landmarkKey, initialEstimate); // Initial landmark value
NonlinearFactorGraph graph;
static SharedNoiseModel unit2(noiseModel::Unit::Create(2));
static SharedNoiseModel prior_model(noiseModel::Isotropic::Sigma(6, 1e-6));
for (size_t i = 0; i < measurements.size(); i++) {
const Pose3& pose_i = poses[i];
typedef PinholePose<CALIBRATION> Camera;
Camera camera_i(pose_i, sharedCal);
graph.emplace_shared<TriangulationFactor<Camera> > //
(camera_i, measurements[i], unit2, landmarkKey);
(camera_i, measurements[i], model? model : unit2, landmarkKey);
}
return std::make_pair(graph, values);
}
@ -134,7 +135,8 @@ template<class CAMERA>
std::pair<NonlinearFactorGraph, Values> triangulationGraph(
const CameraSet<CAMERA>& cameras,
const typename CAMERA::MeasurementVector& measurements, Key landmarkKey,
const Point3& initialEstimate) {
const Point3& initialEstimate,
const SharedNoiseModel& model = nullptr) {
Values values;
values.insert(landmarkKey, initialEstimate); // Initial landmark value
NonlinearFactorGraph graph;
@ -143,7 +145,7 @@ std::pair<NonlinearFactorGraph, Values> triangulationGraph(
for (size_t i = 0; i < measurements.size(); i++) {
const CAMERA& camera_i = cameras[i];
graph.emplace_shared<TriangulationFactor<CAMERA> > //
(camera_i, measurements[i], unit, landmarkKey);
(camera_i, measurements[i], model? model : unit, landmarkKey);
}
return std::make_pair(graph, values);
}
@ -169,13 +171,14 @@ GTSAM_EXPORT Point3 optimize(const NonlinearFactorGraph& graph,
template<class CALIBRATION>
Point3 triangulateNonlinear(const std::vector<Pose3>& poses,
boost::shared_ptr<CALIBRATION> sharedCal,
const Point2Vector& measurements, const Point3& initialEstimate) {
const Point2Vector& measurements, const Point3& initialEstimate,
const SharedNoiseModel& model = nullptr) {
// Create a factor graph and initial values
Values values;
NonlinearFactorGraph graph;
boost::tie(graph, values) = triangulationGraph<CALIBRATION> //
(poses, sharedCal, measurements, Symbol('p', 0), initialEstimate);
(poses, sharedCal, measurements, Symbol('p', 0), initialEstimate, model);
return optimize(graph, values, Symbol('p', 0));
}
@ -190,13 +193,14 @@ Point3 triangulateNonlinear(const std::vector<Pose3>& poses,
template<class CAMERA>
Point3 triangulateNonlinear(
const CameraSet<CAMERA>& cameras,
const typename CAMERA::MeasurementVector& measurements, const Point3& initialEstimate) {
const typename CAMERA::MeasurementVector& measurements, const Point3& initialEstimate,
const SharedNoiseModel& model = nullptr) {
// Create a factor graph and initial values
Values values;
NonlinearFactorGraph graph;
boost::tie(graph, values) = triangulationGraph<CAMERA> //
(cameras, measurements, Symbol('p', 0), initialEstimate);
(cameras, measurements, Symbol('p', 0), initialEstimate, model);
return optimize(graph, values, Symbol('p', 0));
}
@ -239,7 +243,8 @@ template<class CALIBRATION>
Point3 triangulatePoint3(const std::vector<Pose3>& poses,
boost::shared_ptr<CALIBRATION> sharedCal,
const Point2Vector& measurements, double rank_tol = 1e-9,
bool optimize = false) {
bool optimize = false,
const SharedNoiseModel& model = nullptr) {
assert(poses.size() == measurements.size());
if (poses.size() < 2)
@ -254,7 +259,7 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,
// Then refine using non-linear optimization
if (optimize)
point = triangulateNonlinear<CALIBRATION> //
(poses, sharedCal, measurements, point);
(poses, sharedCal, measurements, point, model);
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
// verify that the triangulated point lies in front of all cameras
@ -284,7 +289,8 @@ template<class CAMERA>
Point3 triangulatePoint3(
const CameraSet<CAMERA>& cameras,
const typename CAMERA::MeasurementVector& measurements, double rank_tol = 1e-9,
bool optimize = false) {
bool optimize = false,
const SharedNoiseModel& model = nullptr) {
size_t m = cameras.size();
assert(measurements.size() == m);
@ -298,7 +304,7 @@ Point3 triangulatePoint3(
// The n refine using non-linear optimization
if (optimize)
point = triangulateNonlinear<CAMERA>(cameras, measurements, point);
point = triangulateNonlinear<CAMERA>(cameras, measurements, point, model);
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
// verify that the triangulated point lies in front of all cameras
@ -317,9 +323,10 @@ template<class CALIBRATION>
Point3 triangulatePoint3(
const CameraSet<PinholeCamera<CALIBRATION> >& cameras,
const Point2Vector& measurements, double rank_tol = 1e-9,
bool optimize = false) {
bool optimize = false,
const SharedNoiseModel& model = nullptr) {
return triangulatePoint3<PinholeCamera<CALIBRATION> > //
(cameras, measurements, rank_tol, optimize);
(cameras, measurements, rank_tol, optimize, model);
}
struct GTSAM_EXPORT TriangulationParameters {
@ -341,20 +348,25 @@ struct GTSAM_EXPORT TriangulationParameters {
*/
double dynamicOutlierRejectionThreshold;
SharedNoiseModel noiseModel; ///< used in the nonlinear triangulation
/**
* Constructor
* @param rankTol tolerance used to check if point triangulation is degenerate
* @param enableEPI if true refine triangulation with embedded LM iterations
* @param landmarkDistanceThreshold flag as degenerate if point further than this
* @param dynamicOutlierRejectionThreshold or if average error larger than this
* @param noiseModel noise model to use during nonlinear triangulation
*
*/
TriangulationParameters(const double _rankTolerance = 1.0,
const bool _enableEPI = false, double _landmarkDistanceThreshold = -1,
double _dynamicOutlierRejectionThreshold = -1) :
double _dynamicOutlierRejectionThreshold = -1,
const SharedNoiseModel& _noiseModel = nullptr) :
rankTolerance(_rankTolerance), enableEPI(_enableEPI), //
landmarkDistanceThreshold(_landmarkDistanceThreshold), //
dynamicOutlierRejectionThreshold(_dynamicOutlierRejectionThreshold) {
dynamicOutlierRejectionThreshold(_dynamicOutlierRejectionThreshold),
noiseModel(_noiseModel){
}
// stream to output
@ -366,6 +378,7 @@ struct GTSAM_EXPORT TriangulationParameters {
<< std::endl;
os << "dynamicOutlierRejectionThreshold = "
<< p.dynamicOutlierRejectionThreshold << std::endl;
os << "noise model" << std::endl;
return os;
}
@ -468,8 +481,9 @@ TriangulationResult triangulateSafe(const CameraSet<CAMERA>& cameras,
else
// We triangulate the 3D position of the landmark
try {
Point3 point = triangulatePoint3<CAMERA>(cameras, measured,
params.rankTolerance, params.enableEPI);
Point3 point =
triangulatePoint3<CAMERA>(cameras, measured, params.rankTolerance,
params.enableEPI, params.noiseModel);
// Check landmark distance and re-projection errors to avoid outliers
size_t i = 0;

40
gtsam/inference/BayesNet-inst.h
View File

@ -35,21 +35,39 @@ void BayesNet<CONDITIONAL>::print(
/* ************************************************************************* */
template <class CONDITIONAL>
void BayesNet<CONDITIONAL>::saveGraph(const std::string& s,
const KeyFormatter& keyFormatter) const {
std::ofstream of(s.c_str());
of << "digraph G{\n";
void BayesNet<CONDITIONAL>::dot(std::ostream& os,
const KeyFormatter& keyFormatter) const {
os << "digraph G{\n";
for (auto conditional : boost::adaptors::reverse(*this)) {
typename CONDITIONAL::Frontals frontals = conditional->frontals();
Key me = frontals.front();
typename CONDITIONAL::Parents parents = conditional->parents();
for (Key p : parents)
of << keyFormatter(p) << "->" << keyFormatter(me) << std::endl;
for (auto conditional : *this) {
auto frontals = conditional->frontals();
const Key me = frontals.front();
auto parents = conditional->parents();
for (const Key& p : parents)
os << keyFormatter(p) << "->" << keyFormatter(me) << "\n";
}
of << "}";
os << "}";
std::flush(os);
}
/* ************************************************************************* */
template <class CONDITIONAL>
std::string BayesNet<CONDITIONAL>::dot(const KeyFormatter& keyFormatter) const {
std::stringstream ss;
dot(ss, keyFormatter);
return ss.str();
}
/* ************************************************************************* */
template <class CONDITIONAL>
void BayesNet<CONDITIONAL>::saveGraph(const std::string& filename,
const KeyFormatter& keyFormatter) const {
std::ofstream of(filename.c_str());
dot(of, keyFormatter);
of.close();
}
/* ************************************************************************* */
} // namespace gtsam

14
gtsam/inference/BayesNet.h
View File

@ -64,11 +64,21 @@ namespace gtsam {
/// @}
/// @name Standard Interface
/// @name Graph Display
/// @{
void saveGraph(const std::string& s,
/// Output to graphviz format, stream version.
void dot(std::ostream& os, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
/// Output to graphviz format string.
std::string dot(
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
/// output to file with graphviz format.
void saveGraph(const std::string& filename,
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
/// @}
};
}

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

@ -63,20 +63,40 @@ namespace gtsam {
}
/* ************************************************************************* */
template<class CLIQUE>
void BayesTree<CLIQUE>::saveGraph(const std::string &s, const KeyFormatter& keyFormatter) const {
if (roots_.empty()) throw std::invalid_argument("the root of Bayes tree has not been initialized!");
std::ofstream of(s.c_str());
of<< "digraph G{\n";
for(const sharedClique& root: roots_)
saveGraph(of, root, keyFormatter);
of<<"}";
template <class CLIQUE>
void BayesTree<CLIQUE>::dot(std::ostream& os,
const KeyFormatter& keyFormatter) const {
if (roots_.empty())
throw std::invalid_argument(
"the root of Bayes tree has not been initialized!");
os << "digraph G{\n";
for (const sharedClique& root : roots_) dot(os, root, keyFormatter);
os << "}";
std::flush(os);
}
/* ************************************************************************* */
template <class CLIQUE>
std::string BayesTree<CLIQUE>::dot(const KeyFormatter& keyFormatter) const {
std::stringstream ss;
dot(ss, keyFormatter);
return ss.str();
}
/* ************************************************************************* */
template <class CLIQUE>
void BayesTree<CLIQUE>::saveGraph(const std::string& filename,
const KeyFormatter& keyFormatter) const {
std::ofstream of(filename.c_str());
dot(of, keyFormatter);
of.close();
}
/* ************************************************************************* */
template<class CLIQUE>
void BayesTree<CLIQUE>::saveGraph(std::ostream &s, sharedClique clique, const KeyFormatter& indexFormatter, int parentnum) const {
template <class CLIQUE>
void BayesTree<CLIQUE>::dot(std::ostream& s, sharedClique clique,
const KeyFormatter& indexFormatter,
int parentnum) const {
static int num = 0;
bool first = true;
std::stringstream out;
@ -107,7 +127,7 @@ namespace gtsam {
for (sharedClique c : clique->children) {
num++;
saveGraph(s, c, indexFormatter, parentnum);
dot(s, c, indexFormatter, parentnum);
}
}

25
gtsam/inference/BayesTree.h
View File

@ -143,7 +143,7 @@ namespace gtsam {
const Nodes& nodes() const { return nodes_; }
/** Access node by variable */
const sharedNode operator[](Key j) const { return nodes_.at(j); }
sharedClique operator[](Key j) const { return nodes_.at(j); }
/** return root cliques */
const Roots& roots() const { return roots_; }
@ -182,13 +182,20 @@ namespace gtsam {
*/
sharedBayesNet jointBayesNet(Key j1, Key j2, const Eliminate& function = EliminationTraitsType::DefaultEliminate) const;
/**
* Read only with side effects
*/
/// @name Graph Display
/// @{
/** saves the Tree to a text file in GraphViz format */
void saveGraph(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
/// Output to graphviz format, stream version.
void dot(std::ostream& os, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
/// Output to graphviz format string.
std::string dot(
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
/// output to file with graphviz format.
void saveGraph(const std::string& filename,
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
/// @}
/// @name Advanced Interface
/// @{
@ -236,8 +243,8 @@ namespace gtsam {
protected:
/** private helper method for saving the Tree to a text file in GraphViz format */
void saveGraph(std::ostream &s, sharedClique clique, const KeyFormatter& keyFormatter,
int parentnum = 0) const;
void dot(std::ostream &s, sharedClique clique, const KeyFormatter& keyFormatter,
int parentnum = 0) const;
/** Gather data on a single clique */
void getCliqueData(sharedClique clique, BayesTreeCliqueData* stats) const;
@ -249,7 +256,7 @@ namespace gtsam {
void fillNodesIndex(const sharedClique& subtree);
// Friend JunctionTree because it directly fills roots and nodes index.
template<class BAYESRTEE, class GRAPH> friend class EliminatableClusterTree;
template<class BAYESTREE, class GRAPH> friend class EliminatableClusterTree;
private:
/** Serialization function */

93
gtsam/inference/DotWriter.cpp Normal file
View File

@ -0,0 +1,93 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010-2021, 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 DotWriter.cpp
* @brief Graphviz formatting for factor graphs.
* @author Frank Dellaert
* @date December, 2021
*/
#include <gtsam/base/Vector.h>
#include <gtsam/inference/DotWriter.h>
#include <ostream>
using namespace std;
namespace gtsam {
void DotWriter::writePreamble(ostream* os) const {
*os << "graph {\n";
*os << " size=\"" << figureWidthInches << "," << figureHeightInches
<< "\";\n\n";
}
void DotWriter::DrawVariable(Key key, const KeyFormatter& keyFormatter,
const boost::optional<Vector2>& position,
ostream* os) {
// Label the node with the label from the KeyFormatter
*os << " var" << key << "[label=\"" << keyFormatter(key) << "\"";
if (position) {
*os << ", pos=\"" << position->x() << "," << position->y() << "!\"";
}
*os << "];\n";
}
void DotWriter::DrawFactor(size_t i, const boost::optional<Vector2>& position,
ostream* os) {
*os << " factor" << i << "[label=\"\", shape=point";
if (position) {
*os << ", pos=\"" << position->x() << "," << position->y() << "!\"";
}
*os << "];\n";
}
void DotWriter::ConnectVariables(Key key1, Key key2, ostream* os) {
*os << " var" << key1 << "--"
<< "var" << key2 << ";\n";
}
void DotWriter::ConnectVariableFactor(Key key, size_t i, ostream* os) {
*os << " var" << key << "--"
<< "factor" << i << ";\n";
}
void DotWriter::processFactor(size_t i, const KeyVector& keys,
const boost::optional<Vector2>& position,
ostream* os) const {
if (plotFactorPoints) {
if (binaryEdges && keys.size() == 2) {
ConnectVariables(keys[0], keys[1], os);
} else {
// Create dot for the factor.
DrawFactor(i, position, os);
// Make factor-variable connections
if (connectKeysToFactor) {
for (Key key : keys) {
ConnectVariableFactor(key, i, os);
}
}
}
} else {
// just connect variables in a clique
for (Key key1 : keys) {
for (Key key2 : keys) {
if (key2 > key1) {
ConnectVariables(key1, key2, os);
}
}
}
}
}
} // namespace gtsam

72
gtsam/inference/DotWriter.h Normal file
View File

@ -0,0 +1,72 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010-2021, 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 DotWriter.h
* @brief Graphviz formatter
* @author Frank Dellaert
* @date December, 2021
*/
#pragma once
#include <gtsam/base/FastVector.h>
#include <gtsam/base/Vector.h>
#include <gtsam/inference/Key.h>
#include <iosfwd>
namespace gtsam {
/// Graphviz formatter.
struct GTSAM_EXPORT DotWriter {
double figureWidthInches; ///< The figure width on paper in inches
double figureHeightInches; ///< The figure height on paper in inches
bool plotFactorPoints; ///< Plots each factor as a dot between the variables
bool connectKeysToFactor; ///< Draw a line from each key within a factor to
///< the dot of the factor
bool binaryEdges; ///< just use non-dotted edges for binary factors
explicit DotWriter(double figureWidthInches = 5,
double figureHeightInches = 5,
bool plotFactorPoints = true,
bool connectKeysToFactor = true, bool binaryEdges = true)
: figureWidthInches(figureWidthInches),
figureHeightInches(figureHeightInches),
plotFactorPoints(plotFactorPoints),
connectKeysToFactor(connectKeysToFactor),
binaryEdges(binaryEdges) {}
/// Write out preamble, including size.
void writePreamble(std::ostream* os) const;
/// Create a variable dot fragment.
static void DrawVariable(Key key, const KeyFormatter& keyFormatter,
const boost::optional<Vector2>& position,
std::ostream* os);
/// Create factor dot.
static void DrawFactor(size_t i, const boost::optional<Vector2>& position,
std::ostream* os);
/// Connect two variables.
static void ConnectVariables(Key key1, Key key2, std::ostream* os);
/// Connect variable and factor.
static void ConnectVariableFactor(Key key, size_t i, std::ostream* os);
/// Draw a single factor, specified by its index i and its variable keys.
void processFactor(size_t i, const KeyVector& keys,
const boost::optional<Vector2>& position,
std::ostream* os) const;
};
} // namespace gtsam

14
gtsam/inference/EliminateableFactorGraph.h
View File

@ -288,8 +288,8 @@ namespace gtsam {
FactorGraphType& asDerived() { return static_cast<FactorGraphType&>(*this); }
public:
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
/** \deprecated ordering and orderingType shouldn't both be specified */
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
/** @deprecated ordering and orderingType shouldn't both be specified */
boost::shared_ptr<BayesNetType> GTSAM_DEPRECATED eliminateSequential(
const Ordering& ordering,
const Eliminate& function,
@ -298,7 +298,7 @@ namespace gtsam {
return eliminateSequential(ordering, function, variableIndex);
}
/** \deprecated orderingType specified first for consistency */
/** @deprecated orderingType specified first for consistency */
boost::shared_ptr<BayesNetType> GTSAM_DEPRECATED eliminateSequential(
const Eliminate& function,
OptionalVariableIndex variableIndex = boost::none,
@ -306,7 +306,7 @@ namespace gtsam {
return eliminateSequential(orderingType, function, variableIndex);
}
/** \deprecated ordering and orderingType shouldn't both be specified */
/** @deprecated ordering and orderingType shouldn't both be specified */
boost::shared_ptr<BayesTreeType> GTSAM_DEPRECATED eliminateMultifrontal(
const Ordering& ordering,
const Eliminate& function,
@ -315,7 +315,7 @@ namespace gtsam {
return eliminateMultifrontal(ordering, function, variableIndex);
}
/** \deprecated orderingType specified first for consistency */
/** @deprecated orderingType specified first for consistency */
boost::shared_ptr<BayesTreeType> GTSAM_DEPRECATED eliminateMultifrontal(
const Eliminate& function,
OptionalVariableIndex variableIndex = boost::none,
@ -323,7 +323,7 @@ namespace gtsam {
return eliminateMultifrontal(orderingType, function, variableIndex);
}
/** \deprecated */
/** @deprecated */
boost::shared_ptr<BayesNetType> GTSAM_DEPRECATED marginalMultifrontalBayesNet(
boost::variant<const Ordering&, const KeyVector&> variables,
boost::none_t,
@ -332,7 +332,7 @@ namespace gtsam {
return marginalMultifrontalBayesNet(variables, function, variableIndex);
}
/** \deprecated */
/** @deprecated */
boost::shared_ptr<BayesTreeType> GTSAM_DEPRECATED marginalMultifrontalBayesTree(
boost::variant<const Ordering&, const KeyVector&> variables,
boost::none_t,

4
gtsam/inference/Factor.h
View File

@ -112,6 +112,9 @@ typedef FastSet<FactorIndex> FactorIndexSet;
/// @name Standard Interface
/// @{
/// Whether the factor is empty (involves zero variables).
bool empty() const { return keys_.empty(); }
/// First key
Key front() const { return keys_.front(); }
@ -150,7 +153,6 @@ typedef FastSet<FactorIndex> FactorIndexSet;
const std::string& s = "Factor",
const KeyFormatter& formatter = DefaultKeyFormatter) const;
protected:
/// check equality
bool equals(const This& other, double tol = 1e-9) const;

46
gtsam/inference/FactorGraph-inst.h
View File

@ -26,6 +26,7 @@
#include <stdio.h>
#include <algorithm>
#include <iostream> // for cout :-(
#include <fstream>
#include <sstream>
#include <string>
@ -125,4 +126,49 @@ FactorIndices FactorGraph<FACTOR>::add_factors(const CONTAINER& factors,
return newFactorIndices;
}
/* ************************************************************************* */
template <class FACTOR>
void FactorGraph<FACTOR>::dot(std::ostream& os,
const KeyFormatter& keyFormatter,
const DotWriter& writer) const {
writer.writePreamble(&os);
// Create nodes for each variable in the graph
for (Key key : keys()) {
writer.DrawVariable(key, keyFormatter, boost::none, &os);
}
os << "\n";
// Create factors and variable connections
for (size_t i = 0; i < size(); ++i) {
const auto& factor = at(i);
if (factor) {
const KeyVector& factorKeys = factor->keys();
writer.processFactor(i, factorKeys, boost::none, &os);
}
}
os << "}\n";
std::flush(os);
}
/* ************************************************************************* */
template <class FACTOR>
std::string FactorGraph<FACTOR>::dot(const KeyFormatter& keyFormatter,
const DotWriter& writer) const {
std::stringstream ss;
dot(ss, keyFormatter, writer);
return ss.str();
}
/* ************************************************************************* */
template <class FACTOR>
void FactorGraph<FACTOR>::saveGraph(const std::string& filename,
const KeyFormatter& keyFormatter,
const DotWriter& writer) const {
std::ofstream of(filename.c_str());
dot(of, keyFormatter, writer);
of.close();
}
} // namespace gtsam

22
gtsam/inference/FactorGraph.h
View File

@ -22,9 +22,10 @@
#pragma once
#include <gtsam/inference/DotWriter.h>
#include <gtsam/inference/Key.h>
#include <gtsam/base/FastVector.h>
#include <gtsam/base/Testable.h>
#include <gtsam/inference/Key.h>
#include <Eigen/Core> // for Eigen::aligned_allocator
@ -36,6 +37,7 @@
#include <string>
#include <type_traits>
#include <utility>
#include <iosfwd>
namespace gtsam {
/// Define collection type:
@ -371,6 +373,24 @@ class FactorGraph {
return factors_.erase(first, last);
}
/// @}
/// @name Graph Display
/// @{
/// Output to graphviz format, stream version.
void dot(std::ostream& os,
const KeyFormatter& keyFormatter = DefaultKeyFormatter,
const DotWriter& writer = DotWriter()) const;
/// Output to graphviz format string.
std::string dot(const KeyFormatter& keyFormatter = DefaultKeyFormatter,
const DotWriter& writer = DotWriter()) const;
/// output to file with graphviz format.
void saveGraph(const std::string& filename,
const KeyFormatter& keyFormatter = DefaultKeyFormatter,
const DotWriter& writer = DotWriter()) const;
/// @}
/// @name Advanced Interface
/// @{

1
gtsam/linear/Errors.cpp
View File

@ -110,7 +110,6 @@ double dot(const Errors& a, const Errors& b) {
}
/* ************************************************************************* */
template<>
void axpy(double alpha, const Errors& x, Errors& y) {
Errors::const_iterator it = x.begin();
for(Vector& yi: y)

1
gtsam/linear/Errors.h
View File

@ -65,7 +65,6 @@ namespace gtsam {
/**
* BLAS level 2 style
*/
template <>
GTSAM_EXPORT void axpy(double alpha, const Errors& x, Errors& y);
/** print with optional string */

5
gtsam/linear/GaussianConditional.cpp
View File

@ -193,12 +193,15 @@ namespace gtsam {
}
/* ************************************************************************* */
void GaussianConditional::scaleFrontalsBySigma(VectorValues& gy) const {
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
void GTSAM_DEPRECATED
GaussianConditional::scaleFrontalsBySigma(VectorValues& gy) const {
DenseIndex vectorPosition = 0;
for (const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) {
gy[*frontal].array() *= model_->sigmas().segment(vectorPosition, getDim(frontal)).array();
vectorPosition += getDim(frontal);
}
}
#endif
} // namespace gtsam

7
gtsam/linear/GaussianConditional.h
View File

@ -125,12 +125,11 @@ namespace gtsam {
/** Performs transpose backsubstition in place on values */
void solveTransposeInPlace(VectorValues& gy) const;
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
/** Scale the values in \c gy according to the sigmas for the frontal variables in this
* conditional. */
void scaleFrontalsBySigma(VectorValues& gy) const;
// FIXME: deprecated flag doesn't appear to exist?
// __declspec(deprecated) void scaleFrontalsBySigma(VectorValues& gy) const;
void GTSAM_DEPRECATED scaleFrontalsBySigma(VectorValues& gy) const;
#endif
private:
/** Serialization function */

3
gtsam/linear/GaussianFactor.h
View File

@ -117,9 +117,6 @@ namespace gtsam {
/** Clone a factor (make a deep copy) */
virtual GaussianFactor::shared_ptr clone() const = 0;
/** Test whether the factor is empty */
virtual bool empty() const = 0;
/**
* Construct the corresponding anti-factor to negate information
* stored stored in this factor.

2
gtsam/linear/GaussianFactorGraph.cpp
View File

@ -379,7 +379,7 @@ namespace gtsam {
gttic(Compute_minimizing_step_size);
// Compute minimizing step size
double step = -gradientSqNorm / dot(Rg, Rg);
double step = -gradientSqNorm / gtsam::dot(Rg, Rg);
gttoc(Compute_minimizing_step_size);
gttic(Compute_point);

10
gtsam/linear/GaussianFactorGraph.h
View File

@ -396,11 +396,11 @@ namespace gtsam {
public:
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
/** \deprecated */
VectorValues optimize(boost::none_t,
const Eliminate& function =
EliminationTraitsType::DefaultEliminate) const {
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
/** @deprecated */
VectorValues GTSAM_DEPRECATED
optimize(boost::none_t, const Eliminate& function =
EliminationTraitsType::DefaultEliminate) const {
return optimize(function);
}
#endif

3
gtsam/linear/HessianFactor.h
View File

@ -221,9 +221,6 @@ namespace gtsam {
*/
GaussianFactor::shared_ptr negate() const override;
/** Check if the factor is empty. TODO: How should this be defined? */
bool empty() const override { return size() == 0 /*|| rows() == 0*/; }
/** Return the constant term \f$ f \f$ as described above
* @return The constant term \f$ f \f$
*/

3
gtsam/linear/JacobianFactor.h
View File

@ -260,9 +260,6 @@ namespace gtsam {
*/
GaussianFactor::shared_ptr negate() const override;
/** Check if the factor is empty. TODO: How should this be defined? */
bool empty() const override { return size() == 0 /*|| rows() == 0*/; }
/** is noise model constrained ? */
bool isConstrained() const {
return model_ && model_->isConstrained();

4
gtsam/linear/NoiseModel.h
View File

@ -730,8 +730,8 @@ namespace gtsam {
} // namespace noiseModel
/** Note, deliberately not in noiseModel namespace.
* Deprecated. Only for compatibility with previous version.
/**
* Aliases. Deliberately not in noiseModel namespace.
*/
typedef noiseModel::Base::shared_ptr SharedNoiseModel;
typedef noiseModel::Gaussian::shared_ptr SharedGaussian;

1
gtsam/linear/linear.i
View File

@ -467,7 +467,6 @@ virtual class GaussianConditional : gtsam::JacobianFactor {
gtsam::VectorValues solveOtherRHS(const gtsam::VectorValues& parents,
const gtsam::VectorValues& rhs) const;
void solveTransposeInPlace(gtsam::VectorValues& gy) const;
void scaleFrontalsBySigma(gtsam::VectorValues& gy) const;
Matrix R() const;
Matrix S() const;
Vector d() const;

24
gtsam/navigation/AHRSFactor.cpp
View File

@ -168,13 +168,12 @@ Vector AHRSFactor::evaluateError(const Rot3& Ri, const Rot3& Rj,
}
//------------------------------------------------------------------------------
Rot3 AHRSFactor::Predict(
const Rot3& rot_i, const Vector3& bias,
const PreintegratedAhrsMeasurements preintegratedMeasurements) {
const Vector3 biascorrectedOmega = preintegratedMeasurements.predict(bias);
Rot3 AHRSFactor::Predict(const Rot3& rot_i, const Vector3& bias,
const PreintegratedAhrsMeasurements& pim) {
const Vector3 biascorrectedOmega = pim.predict(bias);
// Coriolis term
const Vector3 coriolis = preintegratedMeasurements.integrateCoriolis(rot_i);
const Vector3 coriolis = pim.integrateCoriolis(rot_i);
const Vector3 correctedOmega = biascorrectedOmega - coriolis;
const Rot3 correctedDeltaRij = Rot3::Expmap(correctedOmega);
@ -184,27 +183,26 @@ Rot3 AHRSFactor::Predict(
//------------------------------------------------------------------------------
AHRSFactor::AHRSFactor(Key rot_i, Key rot_j, Key bias,
const PreintegratedMeasurements& pim,
const PreintegratedAhrsMeasurements& pim,
const Vector3& omegaCoriolis,
const boost::optional<Pose3>& body_P_sensor)
: Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), rot_i, rot_j, bias),
: Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), rot_i, rot_j,
bias),
_PIM_(pim) {
boost::shared_ptr<PreintegratedMeasurements::Params> p =
boost::make_shared<PreintegratedMeasurements::Params>(pim.p());
auto p = boost::make_shared<PreintegratedAhrsMeasurements::Params>(pim.p());
p->body_P_sensor = body_P_sensor;
_PIM_.p_ = p;
}
//------------------------------------------------------------------------------
Rot3 AHRSFactor::predict(const Rot3& rot_i, const Vector3& bias,
const PreintegratedMeasurements pim,
const PreintegratedAhrsMeasurements& pim,
const Vector3& omegaCoriolis,
const boost::optional<Pose3>& body_P_sensor) {
boost::shared_ptr<PreintegratedMeasurements::Params> p =
boost::make_shared<PreintegratedMeasurements::Params>(pim.p());
auto p = boost::make_shared<PreintegratedAhrsMeasurements::Params>(pim.p());
p->omegaCoriolis = omegaCoriolis;
p->body_P_sensor = body_P_sensor;
PreintegratedMeasurements newPim = pim;
PreintegratedAhrsMeasurements newPim = pim;
newPim.p_ = p;
return Predict(rot_i, bias, newPim);
}

35
gtsam/navigation/AHRSFactor.h
View File

@ -104,11 +104,10 @@ class GTSAM_EXPORT PreintegratedAhrsMeasurements : public PreintegratedRotation
static Vector DeltaAngles(const Vector& msr_gyro_t, const double msr_dt,
const Vector3& delta_angles);
/// @deprecated constructor
/// @deprecated constructor, but used in tests.
PreintegratedAhrsMeasurements(const Vector3& biasHat,
const Matrix3& measuredOmegaCovariance)
: PreintegratedRotation(boost::make_shared<Params>()),
biasHat_(biasHat) {
: PreintegratedRotation(boost::make_shared<Params>()), biasHat_(biasHat) {
p_->gyroscopeCovariance = measuredOmegaCovariance;
resetIntegration();
}
@ -182,24 +181,26 @@ public:
/// predicted states from IMU
/// TODO(frank): relationship with PIM predict ??
static Rot3 Predict(
const Rot3& rot_i, const Vector3& bias,
const PreintegratedAhrsMeasurements preintegratedMeasurements);
static Rot3 Predict(const Rot3& rot_i, const Vector3& bias,
const PreintegratedAhrsMeasurements& pim);
/// @deprecated constructor, but used in tests.
AHRSFactor(Key rot_i, Key rot_j, Key bias,
const PreintegratedAhrsMeasurements& pim,
const Vector3& omegaCoriolis,
const boost::optional<Pose3>& body_P_sensor = boost::none);
/// @deprecated static function, but used in tests.
static Rot3 predict(
const Rot3& rot_i, const Vector3& bias,
const PreintegratedAhrsMeasurements& pim, const Vector3& omegaCoriolis,
const boost::optional<Pose3>& body_P_sensor = boost::none);
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
/// @deprecated name
typedef PreintegratedAhrsMeasurements PreintegratedMeasurements;
/// @deprecated constructor
AHRSFactor(Key rot_i, Key rot_j, Key bias,
const PreintegratedMeasurements& preintegratedMeasurements,
const Vector3& omegaCoriolis,
const boost::optional<Pose3>& body_P_sensor = boost::none);
/// @deprecated static function
static Rot3 predict(const Rot3& rot_i, const Vector3& bias,
const PreintegratedMeasurements preintegratedMeasurements,
const Vector3& omegaCoriolis,
const boost::optional<Pose3>& body_P_sensor = boost::none);
#endif
private:

55
gtsam/navigation/BarometricFactor.cpp Normal file
View File

@ -0,0 +1,55 @@
/* ----------------------------------------------------------------------------
* 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 BarometricFactor.cpp
* @author Peter Milani
* @brief Implementation file for Barometric factor
* @date December 16, 2021
**/
#include "BarometricFactor.h"
using namespace std;
namespace gtsam {
//***************************************************************************
void BarometricFactor::print(const string& s,
const KeyFormatter& keyFormatter) const {
cout << (s.empty() ? "" : s + " ") << "Barometric Factor on "
<< keyFormatter(key1()) << "Barometric Bias on "
<< keyFormatter(key2()) << "\n";
cout << " Baro measurement: " << nT_ << "\n";
noiseModel_->print(" noise model: ");
}
//***************************************************************************
bool BarometricFactor::equals(const NonlinearFactor& expected,
double tol) const {
const This* e = dynamic_cast<const This*>(&expected);
return e != nullptr && Base::equals(*e, tol) &&
traits<double>::Equals(nT_, e->nT_, tol);
}
//***************************************************************************
Vector BarometricFactor::evaluateError(const Pose3& p, const double& bias,
boost::optional<Matrix&> H,
boost::optional<Matrix&> H2) const {
Matrix tH;
Vector ret = (Vector(1) << (p.translation(tH).z() + bias - nT_)).finished();
if (H) (*H) = tH.block<1, 6>(2, 0);
if (H2) (*H2) = (Matrix(1, 1) << 1.0).finished();
return ret;
}
} // namespace gtsam

109
gtsam/navigation/BarometricFactor.h Normal file
View File

@ -0,0 +1,109 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file BarometricFactor.h
* @author Peter Milani
* @brief Header file for Barometric factor
* @date December 16, 2021
**/
#pragma once
#include <gtsam/geometry/Pose3.h>
#include <gtsam/navigation/NavState.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
namespace gtsam {
/**
* Prior on height in a cartesian frame.
* Receive barometric pressure in kilopascals
* Model with a slowly moving bias to capture differences
* between the height and the standard atmosphere
* https://www.grc.nasa.gov/www/k-12/airplane/atmosmet.html
* @addtogroup Navigation
*/
class GTSAM_EXPORT BarometricFactor : public NoiseModelFactor2<Pose3, double> {
private:
typedef NoiseModelFactor2<Pose3, double> Base;
double nT_; ///< Height Measurement based on a standard atmosphere
public:
/// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<BarometricFactor> shared_ptr;
/// Typedef to this class
typedef BarometricFactor This;
/** default constructor - only use for serialization */
BarometricFactor() : nT_(0) {}
~BarometricFactor() override {}
/**
* @brief Constructor from a measurement of pressure in KPa.
* @param key of the Pose3 variable that will be constrained
* @param key of the barometric bias that will be constrained
* @param baroIn measurement in KPa
* @param model Gaussian noise model 1 dimension
*/
BarometricFactor(Key key, Key baroKey, const double& baroIn,
const SharedNoiseModel& model)
: Base(model, key, baroKey), nT_(heightOut(baroIn)) {}
/// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}
/// print
void print(
const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
/// equals
bool equals(const NonlinearFactor& expected,
double tol = 1e-9) const override;
/// vector of errors
Vector evaluateError(
const Pose3& p, const double& b,
boost::optional<Matrix&> H = boost::none,
boost::optional<Matrix&> H2 = boost::none) const override;
inline const double& measurementIn() const { return nT_; }
inline double heightOut(double n) const {
// From https://www.grc.nasa.gov/www/k-12/airplane/atmosmet.html
return (std::pow(n / 101.29, 1. / 5.256) * 288.08 - 273.1 - 15.04) /
-0.00649;
};
inline double baroOut(const double& meters) {
double temp = 15.04 - 0.00649 * meters;
return 101.29 * std::pow(((temp + 273.1) / 288.08), 5.256);
};
private:
/// Serialization function
friend class boost::serialization::access;
template <class ARCHIVE>
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
ar& boost::serialization::make_nvp(
"NoiseModelFactor1",
boost::serialization::base_object<Base>(*this));
ar& BOOST_SERIALIZATION_NVP(nT_);
}
};
} // namespace gtsam

22
gtsam/navigation/ImuBias.h
View File

@ -131,30 +131,30 @@ public:
/// @}
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
/// @name Deprecated
/// @{
ConstantBias inverse() {
return -(*this);
}
ConstantBias compose(const ConstantBias& q) {
ConstantBias GTSAM_DEPRECATED inverse() { return -(*this); }
ConstantBias GTSAM_DEPRECATED compose(const ConstantBias& q) {
return (*this) + q;
}
ConstantBias between(const ConstantBias& q) {
ConstantBias GTSAM_DEPRECATED between(const ConstantBias& q) {
return q - (*this);
}
Vector6 localCoordinates(const ConstantBias& q) {
return between(q).vector();
Vector6 GTSAM_DEPRECATED localCoordinates(const ConstantBias& q) {
return (q - (*this)).vector();
}
ConstantBias retract(const Vector6& v) {
return compose(ConstantBias(v));
ConstantBias GTSAM_DEPRECATED retract(const Vector6& v) {
return (*this) + ConstantBias(v);
}
static Vector6 Logmap(const ConstantBias& p) {
static Vector6 GTSAM_DEPRECATED Logmap(const ConstantBias& p) {
return p.vector();
}
static ConstantBias Expmap(const Vector6& v) {
static ConstantBias GTSAM_DEPRECATED Expmap(const Vector6& v) {
return ConstantBias(v);
}
/// @}
#endif
private:

11
gtsam/navigation/navigation.i
View File

@ -18,23 +18,12 @@ class ConstantBias {
// Group
static gtsam::imuBias::ConstantBias identity();
gtsam::imuBias::ConstantBias inverse() const;
gtsam::imuBias::ConstantBias compose(const gtsam::imuBias::ConstantBias& b) const;
gtsam::imuBias::ConstantBias between(const gtsam::imuBias::ConstantBias& b) const;
// Operator Overloads
gtsam::imuBias::ConstantBias operator-() const;
gtsam::imuBias::ConstantBias operator+(const gtsam::imuBias::ConstantBias& b) const;
gtsam::imuBias::ConstantBias operator-(const gtsam::imuBias::ConstantBias& b) const;
// Manifold
gtsam::imuBias::ConstantBias retract(Vector v) const;
Vector localCoordinates(const gtsam::imuBias::ConstantBias& b) const;
// Lie Group
static gtsam::imuBias::ConstantBias Expmap(Vector v);
static Vector Logmap(const gtsam::imuBias::ConstantBias& b);
// Standard Interface
Vector vector() const;
Vector accelerometer() const;

28
gtsam/navigation/tests/testAHRSFactor.cpp
View File

@ -54,11 +54,11 @@ Rot3 evaluateRotationError(const AHRSFactor& factor, const Rot3 rot_i,
return Rot3::Expmap(factor.evaluateError(rot_i, rot_j, bias).tail(3));
}
AHRSFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements(
PreintegratedAhrsMeasurements evaluatePreintegratedMeasurements(
const Vector3& bias, const list<Vector3>& measuredOmegas,
const list<double>& deltaTs,
const Vector3& initialRotationRate = Vector3::Zero()) {
AHRSFactor::PreintegratedMeasurements result(bias, I_3x3);
PreintegratedAhrsMeasurements result(bias, I_3x3);
list<Vector3>::const_iterator itOmega = measuredOmegas.begin();
list<double>::const_iterator itDeltaT = deltaTs.begin();
@ -86,10 +86,10 @@ Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega,
Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta) {
return Rot3::Logmap(Rot3::Expmap(thetahat).compose(Rot3::Expmap(deltatheta)));
}
}
//******************************************************************************
TEST( AHRSFactor, PreintegratedMeasurements ) {
TEST( AHRSFactor, PreintegratedAhrsMeasurements ) {
// Linearization point
Vector3 bias(0,0,0); ///< Current estimate of angular rate bias
@ -102,7 +102,7 @@ TEST( AHRSFactor, PreintegratedMeasurements ) {
double expectedDeltaT1(0.5);
// Actual preintegrated values
AHRSFactor::PreintegratedMeasurements actual1(bias, Z_3x3);
PreintegratedAhrsMeasurements actual1(bias, Z_3x3);
actual1.integrateMeasurement(measuredOmega, deltaT);
EXPECT(assert_equal(expectedDeltaR1, Rot3(actual1.deltaRij()), 1e-6));
@ -113,7 +113,7 @@ TEST( AHRSFactor, PreintegratedMeasurements ) {
double expectedDeltaT2(1);
// Actual preintegrated values
AHRSFactor::PreintegratedMeasurements actual2 = actual1;
PreintegratedAhrsMeasurements actual2 = actual1;
actual2.integrateMeasurement(measuredOmega, deltaT);
EXPECT(assert_equal(expectedDeltaR2, Rot3(actual2.deltaRij()), 1e-6));
@ -159,7 +159,7 @@ TEST(AHRSFactor, Error) {
Vector3 measuredOmega;
measuredOmega << M_PI / 100, 0, 0;
double deltaT = 1.0;
AHRSFactor::PreintegratedMeasurements pim(bias, Z_3x3);
PreintegratedAhrsMeasurements pim(bias, Z_3x3);
pim.integrateMeasurement(measuredOmega, deltaT);
// Create factor
@ -217,7 +217,7 @@ TEST(AHRSFactor, ErrorWithBiases) {
measuredOmega << 0, 0, M_PI / 10.0 + 0.3;
double deltaT = 1.0;
AHRSFactor::PreintegratedMeasurements pim(Vector3(0,0,0),
PreintegratedAhrsMeasurements pim(Vector3(0,0,0),
Z_3x3);
pim.integrateMeasurement(measuredOmega, deltaT);
@ -360,7 +360,7 @@ TEST( AHRSFactor, FirstOrderPreIntegratedMeasurements ) {
}
// Actual preintegrated values
AHRSFactor::PreintegratedMeasurements preintegrated =
PreintegratedAhrsMeasurements preintegrated =
evaluatePreintegratedMeasurements(bias, measuredOmegas, deltaTs,
Vector3(M_PI / 100.0, 0.0, 0.0));
@ -397,7 +397,7 @@ TEST( AHRSFactor, ErrorWithBiasesAndSensorBodyDisplacement ) {
const Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.10, 0.10)),
Point3(1, 0, 0));
AHRSFactor::PreintegratedMeasurements pim(Vector3::Zero(), kMeasuredAccCovariance);
PreintegratedAhrsMeasurements pim(Vector3::Zero(), kMeasuredAccCovariance);
pim.integrateMeasurement(measuredOmega, deltaT);
@ -439,7 +439,7 @@ TEST (AHRSFactor, predictTest) {
Vector3 measuredOmega;
measuredOmega << 0, 0, M_PI / 10.0;
double deltaT = 0.2;
AHRSFactor::PreintegratedMeasurements pim(bias, kMeasuredAccCovariance);
PreintegratedAhrsMeasurements pim(bias, kMeasuredAccCovariance);
for (int i = 0; i < 1000; ++i) {
pim.integrateMeasurement(measuredOmega, deltaT);
}
@ -456,9 +456,9 @@ TEST (AHRSFactor, predictTest) {
Rot3 actualRot = factor.predict(x, bias, pim, kZeroOmegaCoriolis);
EXPECT(assert_equal(expectedRot, actualRot, 1e-6));
// AHRSFactor::PreintegratedMeasurements::predict
// PreintegratedAhrsMeasurements::predict
Matrix expectedH = numericalDerivative11<Vector3, Vector3>(
std::bind(&AHRSFactor::PreintegratedMeasurements::predict,
std::bind(&PreintegratedAhrsMeasurements::predict,
&pim, std::placeholders::_1, boost::none), bias);
// Actual Jacobians
@ -478,7 +478,7 @@ TEST (AHRSFactor, graphTest) {
// PreIntegrator
Vector3 biasHat(0, 0, 0);
AHRSFactor::PreintegratedMeasurements pim(biasHat, kMeasuredAccCovariance);
PreintegratedAhrsMeasurements pim(biasHat, kMeasuredAccCovariance);
// Pre-integrate measurements
Vector3 measuredOmega(0, M_PI / 20, 0);

129
gtsam/navigation/tests/testBarometricFactor.cpp Normal file
View File

@ -0,0 +1,129 @@
/* ----------------------------------------------------------------------------
* 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 testBarometricFactor.cpp
* @brief Unit test for BarometricFactor
* @author Peter Milani
* @date 16 Dec, 2021
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/navigation/BarometricFactor.h>
#include <boost/bind/bind.hpp>
using namespace std::placeholders;
using namespace std;
using namespace gtsam;
// *************************************************************************
namespace example {}
double metersToBaro(const double& meters) {
double temp = 15.04 - 0.00649 * meters;
return 101.29 * std::pow(((temp + 273.1) / 288.08), 5.256);
}
// *************************************************************************
TEST(BarometricFactor, Constructor) {
using namespace example;
// meters to barometric.
double baroMeasurement = metersToBaro(10.);
// Factor
Key key(1);
Key key2(2);
SharedNoiseModel model = noiseModel::Isotropic::Sigma(1, 0.25);
BarometricFactor factor(key, key2, baroMeasurement, model);
// Create a linearization point at zero error
Pose3 T(Rot3::RzRyRx(0., 0., 0.), Point3(0., 0., 10.));
double baroBias = 0.;
Vector1 zero;
zero << 0.;
EXPECT(assert_equal(zero, factor.evaluateError(T, baroBias), 1e-5));
// Calculate numerical derivatives
Matrix expectedH = numericalDerivative21<Vector, Pose3, double>(
std::bind(&BarometricFactor::evaluateError, &factor,
std::placeholders::_1, std::placeholders::_2, boost::none,
boost::none),
T, baroBias);
Matrix expectedH2 = numericalDerivative22<Vector, Pose3, double>(
std::bind(&BarometricFactor::evaluateError, &factor,
std::placeholders::_1, std::placeholders::_2, boost::none,
boost::none),
T, baroBias);
// Use the factor to calculate the derivative
Matrix actualH, actualH2;
factor.evaluateError(T, baroBias, actualH, actualH2);
// Verify we get the expected error
EXPECT(assert_equal(expectedH, actualH, 1e-8));
EXPECT(assert_equal(expectedH2, actualH2, 1e-8));
}
// *************************************************************************
//***************************************************************************
TEST(BarometricFactor, nonZero) {
using namespace example;
// meters to barometric.
double baroMeasurement = metersToBaro(10.);
// Factor
Key key(1);
Key key2(2);
SharedNoiseModel model = noiseModel::Isotropic::Sigma(1, 0.25);
BarometricFactor factor(key, key2, baroMeasurement, model);
Pose3 T(Rot3::RzRyRx(0.5, 1., 1.), Point3(20., 30., 1.));
double baroBias = 5.;
// Calculate numerical derivatives
Matrix expectedH = numericalDerivative21<Vector, Pose3, double>(
std::bind(&BarometricFactor::evaluateError, &factor,
std::placeholders::_1, std::placeholders::_2, boost::none,
boost::none),
T, baroBias);
Matrix expectedH2 = numericalDerivative22<Vector, Pose3, double>(
std::bind(&BarometricFactor::evaluateError, &factor,
std::placeholders::_1, std::placeholders::_2, boost::none,
boost::none),
T, baroBias);
// Use the factor to calculate the derivative and the error
Matrix actualH, actualH2;
Vector error = factor.evaluateError(T, baroBias, actualH, actualH2);
Vector actual = (Vector(1) << -4.0).finished();
// Verify we get the expected error
EXPECT(assert_equal(expectedH, actualH, 1e-8));
EXPECT(assert_equal(expectedH2, actualH2, 1e-8));
EXPECT(assert_equal(error, actual, 1e-8));
}
// *************************************************************************
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
// *************************************************************************

8
gtsam/navigation/tests/testImuBias.cpp
View File

@ -47,20 +47,19 @@ TEST(ImuBias, Constructor) {
}
/* ************************************************************************* */
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
TEST(ImuBias, inverse) {
Bias biasActual = bias1.inverse();
Bias biasExpected = Bias(-biasAcc1, -biasGyro1);
EXPECT(assert_equal(biasExpected, biasActual));
}
/* ************************************************************************* */
TEST(ImuBias, compose) {
Bias biasActual = bias2.compose(bias1);
Bias biasExpected = Bias(biasAcc1 + biasAcc2, biasGyro1 + biasGyro2);
EXPECT(assert_equal(biasExpected, biasActual));
}
/* ************************************************************************* */
TEST(ImuBias, between) {
// p.between(q) == q - p
Bias biasActual = bias2.between(bias1);
@ -68,7 +67,6 @@ TEST(ImuBias, between) {
EXPECT(assert_equal(biasExpected, biasActual));
}
/* ************************************************************************* */
TEST(ImuBias, localCoordinates) {
Vector deltaActual = Vector(bias2.localCoordinates(bias1));
Vector deltaExpected =
@ -76,7 +74,6 @@ TEST(ImuBias, localCoordinates) {
EXPECT(assert_equal(deltaExpected, deltaActual));
}
/* ************************************************************************* */
TEST(ImuBias, retract) {
Vector6 delta;
delta << 0.1, 0.2, -0.3, 0.1, -0.1, 0.2;
@ -86,14 +83,12 @@ TEST(ImuBias, retract) {
EXPECT(assert_equal(biasExpected, biasActual));
}
/* ************************************************************************* */
TEST(ImuBias, Logmap) {
Vector deltaActual = bias2.Logmap(bias1);
Vector deltaExpected = bias1.vector();
EXPECT(assert_equal(deltaExpected, deltaActual));
}
/* ************************************************************************* */
TEST(ImuBias, Expmap) {
Vector6 delta;
delta << 0.1, 0.2, -0.3, 0.1, -0.1, 0.2;
@ -101,6 +96,7 @@ TEST(ImuBias, Expmap) {
Bias biasExpected = Bias(delta);
EXPECT(assert_equal(biasExpected, biasActual));
}
#endif
/* ************************************************************************* */
TEST(ImuBias, operatorSub) {

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