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

Finish 4.2a1

release/4.3a0
Frank Dellaert 2022-01-04 21:59:43 -05:00
parent 811369d883 6f2c9f3815
commit 18b40367c7
105 changed files with 1653 additions and 1206 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

2
CMakeLists.txt
View File

@ -11,7 +11,7 @@ endif()
set (GTSAM_VERSION_MAJOR 4)
set (GTSAM_VERSION_MINOR 2)
set (GTSAM_VERSION_PATCH 0)
set (GTSAM_PRERELEASE_VERSION "a0")
set (GTSAM_PRERELEASE_VERSION "a1")
math (EXPR GTSAM_VERSION_NUMERIC "10000 * ${GTSAM_VERSION_MAJOR} + 100 * ${GTSAM_VERSION_MINOR} + ${GTSAM_VERSION_PATCH}")
if (${GTSAM_VERSION_PATCH} EQUAL 0)

4
README.md
View File

@ -4,7 +4,7 @@
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.
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_V41`.
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/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

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);
}
/* ************************************************************************* */

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

76
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,10 +154,28 @@ 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

299
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,16 +240,19 @@ 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";
size_t B = branches_.size();
@ -254,8 +261,8 @@ namespace gtsam {
// 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() << "\"";
@ -264,7 +271,7 @@ namespace gtsam {
if (i > 1) os << " [style=bold]";
}
os << std::endl;
branch->dot(os, showZero);
branch->dot(os, labelFormatter, valueFormatter, showZero);
}
}
@ -278,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;
}
@ -318,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);
}
@ -333,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);
}
@ -360,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);
}
@ -387,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);
@ -398,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);
@ -450,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);
}
/*********************************************************************************/
@ -483,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
@ -548,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);
@ -561,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>
@ -625,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));
}
@ -632,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"
@ -660,26 +769,34 @@ 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(bool showZero) const {
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, showZero);
dot(ss, labelFormatter, valueFormatter, showZero);
return ss.str();
}

168
gtsam/discrete/DecisionTree.h
View File

@ -20,14 +20,15 @@
#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 {
@ -39,14 +40,24 @@ namespace gtsam {
template<typename L, typename Y>
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;
@ -55,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;
@ -79,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;
@ -97,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:
@ -108,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);
@ -144,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
@ -167,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;
@ -195,13 +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(bool showZero = true) const;
std::string dot(const LabelFormatter& labelFormatter,
const ValueFormatter& valueFormatter,
bool showZero = true) const;
/// @name Advanced Interface
/// @{
@ -219,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) {

36
gtsam/discrete/DecisionTreeFactor.cpp
View File

@ -154,9 +154,34 @@ namespace gtsam {
}
/* ************************************************************************* */
std::string DecisionTreeFactor::markdown(
const KeyFormatter& keyFormatter) const {
std::stringstream ss;
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 {
Potentials::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 {
Potentials::dot(name, keyFormatter, valueFormatter, showZero);
}
/** output to graphviz format string */
std::string DecisionTreeFactor::dot(const KeyFormatter& keyFormatter,
bool showZero) const {
return Potentials::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 << "|";
@ -175,7 +200,10 @@ namespace gtsam {
for (const auto& kv : rows) {
ss << "|";
auto assignment = kv.first;
for (auto& key : keys()) ss << assignment.at(key) << "|";
for (auto& key : keys()) {
size_t index = assignment.at(key);
ss << Translate(names, key, index) << "|";
}
ss << kv.second << "|\n";
}
return ss.str();

26
gtsam/discrete/DecisionTreeFactor.h
View File

@ -178,9 +178,29 @@ namespace gtsam {
/// @name Wrapper support
/// @{
/// Render as markdown table.
std::string markdown(
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
/** 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;
/// @}

5
gtsam/discrete/DiscreteBayesNet.cpp
View File

@ -63,12 +63,13 @@ namespace gtsam {
/* ************************************************************************* */
std::string DiscreteBayesNet::markdown(
const KeyFormatter& keyFormatter) const {
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) << endl;
ss << conditional->markdown(keyFormatter, names) << endl;
return ss.str();
}
/* ************************************************************************* */

4
gtsam/discrete/DiscreteBayesNet.h
View File

@ -108,8 +108,8 @@ namespace gtsam {
/// @{
/// Render as markdown table.
std::string markdown(
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
std::string markdown(const KeyFormatter& keyFormatter = DefaultKeyFormatter,
const DiscreteFactor::Names& names = {}) const;
/// @}

5
gtsam/discrete/DiscreteBayesTree.cpp
View File

@ -57,13 +57,14 @@ namespace gtsam {
/* **************************************************************************/
std::string DiscreteBayesTree::markdown(
const KeyFormatter& keyFormatter) const {
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);
ss << "\n" << clique->conditional()->markdown(keyFormatter, names);
return indent + 1;
};
size_t indent;

4
gtsam/discrete/DiscreteBayesTree.h
View File

@ -93,8 +93,8 @@ class GTSAM_EXPORT DiscreteBayesTree
/// @{
/// Render as markdown table.
std::string markdown(
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
std::string markdown(const KeyFormatter& keyFormatter = DefaultKeyFormatter,
const DiscreteFactor::Names& names = {}) const;
/// @}
};

28
gtsam/discrete/DiscreteConditional.cpp
View File

@ -282,9 +282,18 @@ size_t DiscreteConditional::sample(size_t parent_value) const {
return sample(values);
}
/* ******************************************************************************** */
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 {
std::string DiscreteConditional::markdown(const KeyFormatter& keyFormatter,
const Names& names) const {
std::stringstream ss;
// Print out signature.
@ -298,7 +307,7 @@ std::string DiscreteConditional::markdown(
if (nrParents() == 0) {
// We have no parents, call factor method.
ss << ")*:\n" << std::endl;
ss << DecisionTreeFactor::markdown(keyFormatter);
ss << DecisionTreeFactor::markdown(keyFormatter, names);
return ss.str();
}
@ -317,7 +326,7 @@ std::string DiscreteConditional::markdown(
ss << "|";
const_iterator it;
for(Key parent: parents()) {
ss << keyFormatter(parent) << "|";
ss << "*" << keyFormatter(parent) << "*|";
pairs.emplace_back(parent, cardinalities_.at(parent));
}
@ -331,7 +340,10 @@ std::string DiscreteConditional::markdown(
pairs.rend() - nrParents());
const auto frontal_assignments = cartesianProduct(slatnorf);
for (const auto& a : frontal_assignments) {
for (it = beginFrontals(); it != endFrontals(); ++it) ss << a.at(*it);
for (it = beginFrontals(); it != endFrontals(); ++it) {
size_t index = a.at(*it);
ss << Translate(names, *it, index);
}
ss << "|";
}
ss << "\n";
@ -348,8 +360,10 @@ std::string DiscreteConditional::markdown(
for (const auto& a : assignments) {
if (count == 0) {
ss << "|";
for (it = beginParents(); it != endParents(); ++it)
ss << a.at(*it) << "|";
for (it = beginParents(); it != endParents(); ++it) {
size_t index = a.at(*it);
ss << Translate(names, *it, index) << "|";
}
}
ss << operator()(a) << "|";
count = (count + 1) % n;

9
gtsam/discrete/DiscreteConditional.h
View File

@ -162,9 +162,12 @@ public:
size_t sample(const DiscreteValues& parentsValues) const;
/// Single value version.
/// Single parent version.
size_t sample(size_t parent_value) const;
/// Zero parent version.
size_t sample() const;
/// @}
/// @name Advanced Interface
/// @{
@ -180,8 +183,8 @@ public:
/// @{
/// Render as markdown table.
std::string markdown(
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
std::string markdown(const KeyFormatter& keyFormatter = DefaultKeyFormatter,
const Names& names = {}) const override;
/// @}
};

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

20
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
/// @{
@ -92,9 +89,22 @@ public:
/// @name Wrapper support
/// @{
/// Render as markdown table.
/// 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 = 0;
const KeyFormatter& keyFormatter = DefaultKeyFormatter,
const Names& names = {}) const = 0;
/// @}
};

21
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;
@ -130,14 +132,15 @@ namespace gtsam {
}
/* ************************************************************************* */
std::string DiscreteFactorGraph::markdown(
const KeyFormatter& keyFormatter) const {
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) << endl;
ss << factors_[i]->markdown(keyFormatter, names) << endl;
}
return ss.str();
}

15
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 {
@ -140,9 +143,15 @@ public:
/// @name Wrapper support
/// @{
/// Render as markdown table.
std::string markdown(
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
/**
* @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

2
gtsam/discrete/DiscretePrior.h
View File

@ -98,7 +98,7 @@ class GTSAM_EXPORT DiscretePrior : public DiscreteConditional {
* sample
* @return sample from conditional
*/
size_t sample() const { return Base::sample({}); }
size_t sample() const { return Base::sample(); }
/// @}
};

4
gtsam/discrete/Potentials.cpp
View File

@ -51,11 +51,11 @@ bool Potentials::equals(const Potentials& other, double tol) const {
/* ************************************************************************* */
void Potentials::print(const string& s, const KeyFormatter& formatter) const {
cout << s << "\n Cardinalities: {";
cout << s << "\n Cardinalities: { ";
for (const std::pair<const Key,size_t>& key : cardinalities_)
cout << formatter(key.first) << ":" << key.second << ", ";
cout << "}" << endl;
ADT::print(" ");
ADT::print(" ", formatter);
}
//
// /* ************************************************************************* */

16
gtsam/discrete/discrete.i
View File

@ -46,10 +46,14 @@ virtual class DecisionTreeFactor : gtsam::DiscreteFactor {
const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
bool equals(const gtsam::DecisionTreeFactor& other, double tol = 1e-9) const;
string dot(bool showZero = false) const;
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>
@ -82,10 +86,13 @@ virtual class DiscreteConditional : gtsam::DecisionTreeFactor {
size_t solve(const gtsam::DiscreteValues& parentsValues) const;
size_t sample(const 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>
@ -99,7 +106,6 @@ virtual class DiscretePrior : gtsam::DiscreteConditional {
double operator()(size_t value) const;
std::vector<double> pmf() const;
size_t solve() const;
size_t sample() const;
};
#include <gtsam/discrete/DiscreteBayesNet.h>
@ -128,6 +134,8 @@ class DiscreteBayesNet {
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>
@ -162,6 +170,8 @@ class DiscreteBayesTree {
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>
@ -209,6 +219,8 @@ class DiscreteFactorGraph {
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;

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

@ -100,6 +100,20 @@ TEST(DecisionTreeFactor, enumerate) {
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) {
@ -119,6 +133,27 @@ TEST(DecisionTreeFactor, markdown) {
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);
}
/* ************************************************************************* */
int main() {
TestResult tr;

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

@ -187,7 +187,7 @@ TEST(DiscreteBayesNet, markdown) {
"|1|0.01|\n"
"\n"
" *P(Smoking|Asia)*:\n\n"
"|Asia|0|1|\n"
"|*Asia*|0|1|\n"
"|:-:|:-:|:-:|\n"
"|0|0.8|0.2|\n"
"|1|0.7|0.3|\n\n";

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

@ -135,6 +135,24 @@ TEST(DiscreteConditional, markdown_prior) {
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) {
@ -143,7 +161,7 @@ TEST(DiscreteConditional, markdown_multivalued) {
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"
"|*b1*|0|1|2|\n"
"|:-:|:-:|:-:|:-:|\n"
"|0|0.02|0.88|0.1|\n"
"|1|0.02|0.2|0.78|\n"
@ -155,23 +173,25 @@ TEST(DiscreteConditional, markdown_multivalued) {
}
/* ************************************************************************* */
// Check markdown representation looks as expected, two parents.
// 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|0|1|\n"
"|*B*|*C*|T|F|\n"
"|:-:|:-:|:-:|:-:|\n"
"|0|0|0|1|\n"
"|0|1|0.25|0.75|\n"
"|0|2|0.5|0.5|\n"
"|1|0|0.75|0.25|\n"
"|1|1|0|1|\n"
"|1|2|1|0|\n";
vector<string> names{"C", "B", "A"};
auto formatter = [names](Key key) { return names[key]; };
string actual = conditional.markdown(formatter);
"|-|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);
}

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

@ -382,6 +382,31 @@ TEST(DiscreteFactorGraph, Dot) {
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) {

10
gtsam/discrete/tests/testDiscretePrior.cpp
View File

@ -28,6 +28,8 @@ 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));
@ -41,12 +43,18 @@ TEST(DiscretePrior, operator) {
}
/* ************************************************************************* */
TEST(DiscretePrior, to_vector) {
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;

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

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

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;

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.

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:

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

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

4
gtsam/nonlinear/ExpressionFactor.h
View File

@ -295,14 +295,14 @@ struct traits<ExpressionFactorN<T, Args...>>
// ExpressionFactorN
#if defined(GTSAM_ALLOW_DEPRECATED_SINCE_V41)
#if defined(GTSAM_ALLOW_DEPRECATED_SINCE_V42)
/**
* Binary specialization of ExpressionFactor meant as a base class for binary
* factors. Enforces an 'expression' method with two keys, and provides
* 'evaluateError'. Derived class (a binary factor!) needs to call 'initialize'.
*
* \sa ExpressionFactorN
* \deprecated Prefer the more general ExpressionFactorN<>.
* @deprecated Prefer the more general ExpressionFactorN<>.
*/
template <typename T, typename A1, typename A2>
class GTSAM_DEPRECATED ExpressionFactor2 : public ExpressionFactorN<T, A1, A2> {

2
gtsam/nonlinear/ExtendedKalmanFilter.h
View File

@ -51,9 +51,11 @@ class ExtendedKalmanFilter {
typedef boost::shared_ptr<ExtendedKalmanFilter<VALUE> > shared_ptr;
typedef VALUE T;
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
//@deprecated: any NoiseModelFactor will do, as long as they have the right keys
typedef NoiseModelFactor2<VALUE, VALUE> MotionFactor;
typedef NoiseModelFactor1<VALUE> MeasurementFactor;
#endif
protected:
T x_; // linearization point

8
gtsam/nonlinear/Marginals.h
View File

@ -131,16 +131,16 @@ protected:
void computeBayesTree(const Ordering& ordering);
public:
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
/** \deprecated argument order changed due to removing boost::optional<Ordering> */
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
/** @deprecated argument order changed due to removing boost::optional<Ordering> */
GTSAM_DEPRECATED Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization,
const Ordering& ordering) : Marginals(graph, solution, ordering, factorization) {}
/** \deprecated argument order changed due to removing boost::optional<Ordering> */
/** @deprecated argument order changed due to removing boost::optional<Ordering> */
GTSAM_DEPRECATED Marginals(const GaussianFactorGraph& graph, const Values& solution, Factorization factorization,
const Ordering& ordering) : Marginals(graph, solution, ordering, factorization) {}
/** \deprecated argument order changed due to removing boost::optional<Ordering> */
/** @deprecated argument order changed due to removing boost::optional<Ordering> */
GTSAM_DEPRECATED Marginals(const GaussianFactorGraph& graph, const VectorValues& solution, Factorization factorization,
const Ordering& ordering) : Marginals(graph, solution, ordering, factorization) {}
#endif

18
gtsam/nonlinear/NonlinearFactorGraph.h
View File

@ -250,29 +250,29 @@ namespace gtsam {
public:
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
/** \deprecated */
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
/** @deprecated */
boost::shared_ptr<HessianFactor> GTSAM_DEPRECATED linearizeToHessianFactor(
const Values& values, boost::none_t, const Dampen& dampen = nullptr) const
{return linearizeToHessianFactor(values, dampen);}
/** \deprecated */
/** @deprecated */
Values GTSAM_DEPRECATED updateCholesky(const Values& values, boost::none_t,
const Dampen& dampen = nullptr) const
{return updateCholesky(values, dampen);}
/** \deprecated */
/** @deprecated */
void GTSAM_DEPRECATED saveGraph(
std::ostream& os, const Values& values = Values(),
const GraphvizFormatting& graphvizFormatting = GraphvizFormatting(),
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
dot(os, values, keyFormatter, graphvizFormatting);
}
/** \deprecated */
void GTSAM_DEPRECATED saveGraph(
const std::string& filename, const Values& values,
const GraphvizFormatting& graphvizFormatting = GraphvizFormatting(),
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
/** @deprecated */
void GTSAM_DEPRECATED
saveGraph(const std::string& filename, const Values& values,
const GraphvizFormatting& graphvizFormatting,
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
saveGraph(filename, values, keyFormatter, graphvizFormatting);
}
#endif

4
gtsam/slam/RegularImplicitSchurFactor.h
View File

@ -260,10 +260,6 @@ public:
"RegularImplicitSchurFactor::clone non implemented");
}
bool empty() const override {
return false;
}
GaussianFactor::shared_ptr negate() const override {
return boost::make_shared<RegularImplicitSchurFactor<CAMERA> >(keys_,
FBlocks_, PointCovariance_, E_, b_);

10
gtsam/slam/dataset.cpp
View File

@ -1304,14 +1304,14 @@ parse3DFactors(const std::string &filename,
return parseFactors<Pose3>(filename, model, maxIndex);
}
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
std::map<size_t, Pose3> parse3DPoses(const std::string &filename,
size_t maxIndex) {
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
std::map<size_t, Pose3> GTSAM_DEPRECATED
parse3DPoses(const std::string &filename, size_t maxIndex) {
return parseVariables<Pose3>(filename, maxIndex);
}
std::map<size_t, Point3> parse3DLandmarks(const std::string &filename,
size_t maxIndex) {
std::map<size_t, Point3> GTSAM_DEPRECATED
parse3DLandmarks(const std::string &filename, size_t maxIndex) {
return parseVariables<Point3>(filename, maxIndex);
}
#endif

22
gtsam/slam/dataset.h
View File

@ -172,10 +172,6 @@ GTSAM_EXPORT GraphAndValues load2D(const std::string& filename,
false, bool smart = true, NoiseFormat noiseFormat = NoiseFormatAUTO, //
KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE);
/// @deprecated load2D now allows for arbitrary models and wrapping a robust kernel
GTSAM_EXPORT GraphAndValues load2D_robust(const std::string& filename,
const noiseModel::Base::shared_ptr& model, size_t maxIndex = 0);
/** save 2d graph */
GTSAM_EXPORT void save2D(const NonlinearFactorGraph& graph,
const Values& config, const noiseModel::Diagonal::shared_ptr model,
@ -504,17 +500,21 @@ parse3DFactors(const std::string &filename,
size_t maxIndex = 0);
using BinaryMeasurementsUnit3 = std::vector<BinaryMeasurement<Unit3>>;
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
inline boost::optional<IndexedPose> parseVertex(std::istream &is,
const std::string &tag) {
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
inline boost::optional<IndexedPose> GTSAM_DEPRECATED
parseVertex(std::istream& is, const std::string& tag) {
return parseVertexPose(is, tag);
}
GTSAM_EXPORT std::map<size_t, Pose3> parse3DPoses(const std::string &filename,
size_t maxIndex = 0);
GTSAM_EXPORT std::map<size_t, Pose3> GTSAM_DEPRECATED
parse3DPoses(const std::string& filename, size_t maxIndex = 0);
GTSAM_EXPORT std::map<size_t, Point3>
parse3DLandmarks(const std::string &filename, size_t maxIndex = 0);
GTSAM_EXPORT std::map<size_t, Point3> GTSAM_DEPRECATED
parse3DLandmarks(const std::string& filename, size_t maxIndex = 0);
GTSAM_EXPORT GraphAndValues GTSAM_DEPRECATED
load2D_robust(const std::string& filename,
const noiseModel::Base::shared_ptr& model, size_t maxIndex = 0);
#endif
} // namespace gtsam

2
gtsam/slam/slam.i
View File

@ -264,8 +264,6 @@ pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(
string filename, gtsam::noiseModel::Diagonal* model);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D_robust(
string filename, gtsam::noiseModel::Base* model, int maxIndex);
void save2D(const gtsam::NonlinearFactorGraph& graph,
const gtsam::Values& config, gtsam::noiseModel::Diagonal* model,
string filename);

3
gtsam/symbolic/SymbolicFactor.h
View File

@ -144,9 +144,6 @@ namespace gtsam {
/// @name Standard Interface
/// @{
/** Whether the factor is empty (involves zero variables). */
bool empty() const { return keys_.empty(); }
/** Eliminate the variables in \c keys, in the order specified in \c keys, returning a
* conditional and marginal. */
std::pair<boost::shared_ptr<SymbolicConditional>, boost::shared_ptr<SymbolicFactor> >

4
gtsam_unstable/discrete/Constraint.h
View File

@ -86,8 +86,8 @@ class GTSAM_EXPORT Constraint : public DiscreteFactor {
/// @{
/// Render as markdown table.
std::string markdown(
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
std::string markdown(const KeyFormatter& keyFormatter = DefaultKeyFormatter,
const Names& names = {}) const override {
return (boost::format("`Constraint` on %1% variables\n") % (size())).str();
}

2
gtsam_unstable/discrete/examples/schedulingExample.cpp
View File

@ -115,7 +115,7 @@ void runLargeExample() {
// Do brute force product and output that to file
if (scheduler.nrStudents() == 1) { // otherwise too slow
DecisionTreeFactor product = scheduler.product();
product.dot("scheduling-large", false);
product.dot("scheduling-large", DefaultKeyFormatter, false);
}
// Do exact inference

2
gtsam_unstable/discrete/examples/schedulingQuals12.cpp
View File

@ -115,7 +115,7 @@ void runLargeExample() {
// Do brute force product and output that to file
if (scheduler.nrStudents() == 1) { // otherwise too slow
DecisionTreeFactor product = scheduler.product();
product.dot("scheduling-large", false);
product.dot("scheduling-large", DefaultKeyFormatter, false);
}
// Do exact inference

2
gtsam_unstable/discrete/examples/schedulingQuals13.cpp
View File

@ -139,7 +139,7 @@ void runLargeExample() {
// Do brute force product and output that to file
if (scheduler.nrStudents() == 1) { // otherwise too slow
DecisionTreeFactor product = scheduler.product();
product.dot("scheduling-large", false);
product.dot("scheduling-large", DefaultKeyFormatter, false);
}
// Do exact inference

4
gtsam_unstable/slam/BetweenFactorEM.h
View File

@ -421,4 +421,8 @@ private:
};
// \class BetweenFactorEM
/// traits
template<class VALUE>
struct traits<BetweenFactorEM<VALUE> > : public Testable<BetweenFactorEM<VALUE> > {};
} // namespace gtsam

10
gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h
View File

@ -372,15 +372,15 @@ public:
Matrix Z_3x3 = Z_3x3;
Matrix I_3x3 = I_3x3;
Matrix H_pos_pos = numericalDerivative11<LieVector, LieVector>(std::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, _1, delta_vel_in_t0), delta_pos_in_t0);
Matrix H_pos_vel = numericalDerivative11<LieVector, LieVector>(std::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, delta_pos_in_t0, _1), delta_vel_in_t0);
Matrix H_pos_pos = numericalDerivative11<Vector, Vector>(std::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, _1, delta_vel_in_t0), delta_pos_in_t0);
Matrix H_pos_vel = numericalDerivative11<Vector, Vector>(std::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, delta_pos_in_t0, _1), delta_vel_in_t0);
Matrix H_pos_angles = Z_3x3;
Matrix H_vel_vel = numericalDerivative11<LieVector, LieVector>(std::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, delta_angles, _1, flag_use_body_P_sensor, body_P_sensor), delta_vel_in_t0);
Matrix H_vel_angles = numericalDerivative11<LieVector, LieVector>(std::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, _1, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor), delta_angles);
Matrix H_vel_vel = numericalDerivative11<Vector, Vector>(std::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, delta_angles, _1, flag_use_body_P_sensor, body_P_sensor), delta_vel_in_t0);
Matrix H_vel_angles = numericalDerivative11<Vector, Vector>(std::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, _1, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor), delta_angles);
Matrix H_vel_pos = Z_3x3;
Matrix H_angles_angles = numericalDerivative11<LieVector, LieVector>(std::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, msr_dt, _1, flag_use_body_P_sensor, body_P_sensor), delta_angles);
Matrix H_angles_angles = numericalDerivative11<Vector, Vector>(std::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, msr_dt, _1, flag_use_body_P_sensor, body_P_sensor), delta_angles);
Matrix H_angles_pos = Z_3x3;
Matrix H_angles_vel = Z_3x3;

6
gtsam_unstable/slam/LocalOrientedPlane3Factor.h
View File

@ -43,7 +43,7 @@ public:
/// Constructor
LocalOrientedPlane3Factor() {}
virtual ~LocalOrientedPlane3Factor() {}
~LocalOrientedPlane3Factor() override {}
/** Constructor with measured plane (a,b,c,d) coefficients
* @param z measured plane (a,b,c,d) coefficients as 4D vector
@ -56,12 +56,12 @@ public:
* Note: The anchorPoseKey can simply be chosen as the first pose a plane
* is observed.
*/
LocalOrientedPlane3Factor(const Vector4& z, const SharedGaussian& noiseModel,
LocalOrientedPlane3Factor(const Vector4& z, const SharedNoiseModel& noiseModel,
Key poseKey, Key anchorPoseKey, Key landmarkKey)
: Base(noiseModel, poseKey, anchorPoseKey, landmarkKey), measured_p_(z) {}
LocalOrientedPlane3Factor(const OrientedPlane3& z,
const SharedGaussian& noiseModel,
const SharedNoiseModel& noiseModel,
Key poseKey, Key anchorPoseKey, Key landmarkKey)
: Base(noiseModel, poseKey, anchorPoseKey, landmarkKey), measured_p_(z) {}

31
gtsam_unstable/slam/ProjectionFactorPPPC.h
View File

@ -57,34 +57,25 @@ class GTSAM_UNSTABLE_EXPORT ProjectionFactorPPPC
measured_(0.0, 0.0), throwCheirality_(false), verboseCheirality_(false) {
}
/**
* Constructor
* TODO: Mark argument order standard (keys, measurement, parameters)
* @param measured is the 2 dimensional location of point in image (the measurement)
* @param model is the standard deviation
* @param poseKey is the index of the camera
* @param pointKey is the index of the landmark
* @param K shared pointer to the constant calibration
*/
ProjectionFactorPPPC(const Point2& measured, const SharedNoiseModel& model,
Key poseKey, Key transformKey, Key pointKey, Key calibKey) :
Base(model, poseKey, transformKey, pointKey, calibKey), measured_(measured),
throwCheirality_(false), verboseCheirality_(false) {}
/**
* Constructor with exception-handling flags
* TODO: Mark argument order standard (keys, measurement, parameters)
* @param measured is the 2 dimensional location of point in image (the measurement)
* @param measured is the 2 dimensional location of point in image (the
* measurement)
* @param model is the standard deviation
* @param poseKey is the index of the camera
* @param transformKey is the index of the extrinsic calibration
* @param pointKey is the index of the landmark
* @param K shared pointer to the constant calibration
* @param throwCheirality determines whether Cheirality exceptions are rethrown
* @param verboseCheirality determines whether exceptions are printed for Cheirality
* @param calibKey is the index of the intrinsic calibration
* @param throwCheirality determines whether Cheirality exceptions are
* rethrown
* @param verboseCheirality determines whether exceptions are printed for
* Cheirality
*/
ProjectionFactorPPPC(const Point2& measured, const SharedNoiseModel& model,
Key poseKey, Key transformKey, Key pointKey, Key calibKey,
bool throwCheirality, bool verboseCheirality) :
bool throwCheirality = false, bool verboseCheirality = false) :
Base(model, poseKey, transformKey, pointKey, calibKey), measured_(measured),
throwCheirality_(throwCheirality), verboseCheirality_(verboseCheirality) {}
@ -124,8 +115,8 @@ class GTSAM_UNSTABLE_EXPORT ProjectionFactorPPPC
try {
if(H1 || H2 || H3 || H4) {
Matrix H0, H02;
PinholeCamera<CALIBRATION> camera(pose.compose(transform, H0, H02), K);
Point2 reprojectionError(camera.project(point, H1, H3, H4) - measured_);
const PinholeCamera<CALIBRATION> camera(pose.compose(transform, H0, H02), K);
const Point2 reprojectionError(camera.project(point, H1, H3, H4) - measured_);
*H2 = *H1 * H02;
*H1 = *H1 * H0;
return reprojectionError;

18
gtsam_unstable/slam/serialization.cpp
View File

@ -5,8 +5,6 @@
* @author Alex Cunningham
*/
#include <gtsam/base/deprecated/LieMatrix_Deprecated.h>
#include <gtsam/base/deprecated/LieVector_Deprecated.h>
#include <gtsam/slam/serialization.h>
#include <gtsam/base/serialization.h>
@ -31,8 +29,6 @@
using namespace gtsam;
// Creating as many permutations of factors as possible
typedef PriorFactor<LieVector> PriorFactorLieVector;
typedef PriorFactor<LieMatrix> PriorFactorLieMatrix;
typedef PriorFactor<Point2> PriorFactorPoint2;
typedef PriorFactor<StereoPoint2> PriorFactorStereoPoint2;
typedef PriorFactor<Point3> PriorFactorPoint3;
@ -46,8 +42,6 @@ typedef PriorFactor<CalibratedCamera> PriorFactorCalibratedCamera;
typedef PriorFactor<PinholeCameraCal3_S2> PriorFactorPinholeCameraCal3_S2;
typedef PriorFactor<StereoCamera> PriorFactorStereoCamera;
typedef BetweenFactor<LieVector> BetweenFactorLieVector;
typedef BetweenFactor<LieMatrix> BetweenFactorLieMatrix;
typedef BetweenFactor<Point2> BetweenFactorPoint2;
typedef BetweenFactor<Point3> BetweenFactorPoint3;
typedef BetweenFactor<Rot2> BetweenFactorRot2;
@ -55,8 +49,6 @@ typedef BetweenFactor<Rot3> BetweenFactorRot3;
typedef BetweenFactor<Pose2> BetweenFactorPose2;
typedef BetweenFactor<Pose3> BetweenFactorPose3;
typedef NonlinearEquality<LieVector> NonlinearEqualityLieVector;
typedef NonlinearEquality<LieMatrix> NonlinearEqualityLieMatrix;
typedef NonlinearEquality<Point2> NonlinearEqualityPoint2;
typedef NonlinearEquality<StereoPoint2> NonlinearEqualityStereoPoint2;
typedef NonlinearEquality<Point3> NonlinearEqualityPoint3;
@ -112,8 +104,6 @@ BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal");
/* Create GUIDs for geometry */
/* ************************************************************************* */
GTSAM_VALUE_EXPORT(gtsam::LieVector);
GTSAM_VALUE_EXPORT(gtsam::LieMatrix);
GTSAM_VALUE_EXPORT(gtsam::Point2);
GTSAM_VALUE_EXPORT(gtsam::StereoPoint2);
GTSAM_VALUE_EXPORT(gtsam::Point3);
@ -133,8 +123,6 @@ GTSAM_VALUE_EXPORT(gtsam::StereoCamera);
BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor");
BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor");
BOOST_CLASS_EXPORT_GUID(PriorFactorLieVector, "gtsam::PriorFactorLieVector");
BOOST_CLASS_EXPORT_GUID(PriorFactorLieMatrix, "gtsam::PriorFactorLieMatrix");
BOOST_CLASS_EXPORT_GUID(PriorFactorPoint2, "gtsam::PriorFactorPoint2");
BOOST_CLASS_EXPORT_GUID(PriorFactorStereoPoint2, "gtsam::PriorFactorStereoPoint2");
BOOST_CLASS_EXPORT_GUID(PriorFactorPoint3, "gtsam::PriorFactorPoint3");
@ -147,8 +135,6 @@ BOOST_CLASS_EXPORT_GUID(PriorFactorCal3DS2, "gtsam::PriorFactorCal3DS2");
BOOST_CLASS_EXPORT_GUID(PriorFactorCalibratedCamera, "gtsam::PriorFactorCalibratedCamera");
BOOST_CLASS_EXPORT_GUID(PriorFactorStereoCamera, "gtsam::PriorFactorStereoCamera");
BOOST_CLASS_EXPORT_GUID(BetweenFactorLieVector, "gtsam::BetweenFactorLieVector");
BOOST_CLASS_EXPORT_GUID(BetweenFactorLieMatrix, "gtsam::BetweenFactorLieMatrix");
BOOST_CLASS_EXPORT_GUID(BetweenFactorPoint2, "gtsam::BetweenFactorPoint2");
BOOST_CLASS_EXPORT_GUID(BetweenFactorPoint3, "gtsam::BetweenFactorPoint3");
BOOST_CLASS_EXPORT_GUID(BetweenFactorRot2, "gtsam::BetweenFactorRot2");
@ -156,8 +142,6 @@ BOOST_CLASS_EXPORT_GUID(BetweenFactorRot3, "gtsam::BetweenFactorRot3");
BOOST_CLASS_EXPORT_GUID(BetweenFactorPose2, "gtsam::BetweenFactorPose2");
BOOST_CLASS_EXPORT_GUID(BetweenFactorPose3, "gtsam::BetweenFactorPose3");
BOOST_CLASS_EXPORT_GUID(NonlinearEqualityLieVector, "gtsam::NonlinearEqualityLieVector");
BOOST_CLASS_EXPORT_GUID(NonlinearEqualityLieMatrix, "gtsam::NonlinearEqualityLieMatrix");
BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPoint2, "gtsam::NonlinearEqualityPoint2");
BOOST_CLASS_EXPORT_GUID(NonlinearEqualityStereoPoint2, "gtsam::NonlinearEqualityStereoPoint2");
BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPoint3, "gtsam::NonlinearEqualityPoint3");
@ -189,7 +173,7 @@ BOOST_CLASS_EXPORT_GUID(GeneralSFMFactor2Cal3_S2, "gtsam::GeneralSFMFactor2Cal3_
BOOST_CLASS_EXPORT_GUID(GenericStereoFactor3D, "gtsam::GenericStereoFactor3D");
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
typedef PriorFactor<SimpleCamera> PriorFactorSimpleCamera;
typedef NonlinearEquality<SimpleCamera> NonlinearEqualitySimpleCamera;

52
gtsam_unstable/slam/tests/testBetweenFactorEM.cpp
View File

@ -11,6 +11,7 @@
#include <gtsam/geometry/Pose2.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/Vector.h>
#include <gtsam/slam/BetweenFactor.h>
@ -21,26 +22,24 @@ using namespace gtsam;
// Disabled this test because it is currently failing - remove the lines "#if 0" and "#endif" below
// to reenable the test.
#if 0
// #if 0
/* ************************************************************************* */
LieVector predictionError(const Pose2& p1, const Pose2& p2, const gtsam::Key& key1, const gtsam::Key& key2, const BetweenFactorEM<gtsam::Pose2>& factor){
Vector predictionError(const Pose2& p1, const Pose2& p2, const gtsam::Key& key1, const gtsam::Key& key2, const BetweenFactorEM<gtsam::Pose2>& factor){
gtsam::Values values;
values.insert(key1, p1);
values.insert(key2, p2);
// LieVector err = factor.whitenedError(values);
// return err;
return LieVector::Expmap(factor.whitenedError(values));
return factor.whitenedError(values);
}
/* ************************************************************************* */
LieVector predictionError_standard(const Pose2& p1, const Pose2& p2, const gtsam::Key& key1, const gtsam::Key& key2, const BetweenFactor<gtsam::Pose2>& factor){
Vector predictionError_standard(const Pose2& p1, const Pose2& p2, const gtsam::Key& key1, const gtsam::Key& key2, const BetweenFactor<gtsam::Pose2>& factor){
gtsam::Values values;
values.insert(key1, p1);
values.insert(key2, p2);
// LieVector err = factor.whitenedError(values);
// Vector err = factor.whitenedError(values);
// return err;
return LieVector::Expmap(factor.whitenedError(values));
return factor.whitenedError(values);
}
/* ************************************************************************* */
@ -99,8 +98,8 @@ TEST( BetweenFactorEM, EvaluateError)
Vector actual_err_wh = f.whitenedError(values);
Vector actual_err_wh_inlier = (Vector(3) << actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]);
Vector actual_err_wh_outlier = (Vector(3) << actual_err_wh[3], actual_err_wh[4], actual_err_wh[5]);
Vector3 actual_err_wh_inlier = Vector3(actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]);
Vector3 actual_err_wh_outlier = Vector3(actual_err_wh[3], actual_err_wh[4], actual_err_wh[5]);
// cout << "Inlier test. norm of actual_err_wh_inlier, actual_err_wh_outlier: "<<actual_err_wh_inlier.norm()<<","<<actual_err_wh_outlier.norm()<<endl;
// cout<<actual_err_wh[0]<<" "<<actual_err_wh[1]<<" "<<actual_err_wh[2]<<actual_err_wh[3]<<" "<<actual_err_wh[4]<<" "<<actual_err_wh[5]<<endl;
@ -117,8 +116,8 @@ TEST( BetweenFactorEM, EvaluateError)
actual_err_wh = g.whitenedError(values);
actual_err_wh_inlier = (Vector(3) << actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]);
actual_err_wh_outlier = (Vector(3) << actual_err_wh[3], actual_err_wh[4], actual_err_wh[5]);
actual_err_wh_inlier = Vector3(actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]);
actual_err_wh_outlier = Vector3(actual_err_wh[3], actual_err_wh[4], actual_err_wh[5]);
// in case of outlier, outlier-mode whitented error should be dominant
// CHECK(actual_err_wh_inlier.norm() < 1000.0*actual_err_wh_outlier.norm());
@ -132,7 +131,7 @@ TEST( BetweenFactorEM, EvaluateError)
BetweenFactorEM<gtsam::Pose2> h_EM(key1, key2, rel_pose_msr, model_inlier, model_outlier,
prior_inlier, prior_outlier);
actual_err_wh = h_EM.whitenedError(values);
actual_err_wh_inlier = (Vector(3) << actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]);
actual_err_wh_inlier = Vector3(actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]);
BetweenFactor<gtsam::Pose2> h(key1, key2, rel_pose_msr, model_inlier );
Vector actual_err_wh_stnd = h.whitenedError(values);
@ -178,7 +177,7 @@ TEST (BetweenFactorEM, jacobian ) {
// compare to standard between factor
BetweenFactor<gtsam::Pose2> h(key1, key2, rel_pose_msr, model_inlier );
Vector actual_err_wh_stnd = h.whitenedError(values);
Vector actual_err_wh_inlier = (Vector(3) << actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]);
Vector actual_err_wh_inlier = Vector3(actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]);
// CHECK( assert_equal(actual_err_wh_stnd, actual_err_wh_inlier, 1e-8));
std::vector<gtsam::Matrix> H_actual_stnd_unwh(2);
(void)h.unwhitenedError(values, H_actual_stnd_unwh);
@ -190,12 +189,13 @@ TEST (BetweenFactorEM, jacobian ) {
// CHECK( assert_equal(H2_actual_stnd, H2_actual, 1e-8));
double stepsize = 1.0e-9;
Matrix H1_expected = gtsam::numericalDerivative11<LieVector, Pose2>(std::bind(&predictionError, _1, p2, key1, key2, f), p1, stepsize);
Matrix H2_expected = gtsam::numericalDerivative11<LieVector, Pose2>(std::bind(&predictionError, p1, _1, key1, key2, f), p2, stepsize);
using std::placeholders::_1;
Matrix H1_expected = gtsam::numericalDerivative11<Vector, Pose2>(std::bind(&predictionError, _1, p2, key1, key2, f), p1, stepsize);
Matrix H2_expected = gtsam::numericalDerivative11<Vector, Pose2>(std::bind(&predictionError, p1, _1, key1, key2, f), p2, stepsize);
// try to check numerical derivatives of a standard between factor
Matrix H1_expected_stnd = gtsam::numericalDerivative11<LieVector, Pose2>(std::bind(&predictionError_standard, _1, p2, key1, key2, h), p1, stepsize);
Matrix H1_expected_stnd = gtsam::numericalDerivative11<Vector, Pose2>(std::bind(&predictionError_standard, _1, p2, key1, key2, h), p1, stepsize);
// CHECK( assert_equal(H1_expected_stnd, H1_actual_stnd, 1e-5));
//
//
@ -240,8 +240,8 @@ TEST( BetweenFactorEM, CaseStudy)
Vector actual_err_unw = f.unwhitenedError(values);
Vector actual_err_wh = f.whitenedError(values);
Vector actual_err_wh_inlier = (Vector(3) << actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]);
Vector actual_err_wh_outlier = (Vector(3) << actual_err_wh[3], actual_err_wh[4], actual_err_wh[5]);
Vector3 actual_err_wh_inlier = Vector3(actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]);
Vector3 actual_err_wh_outlier = Vector3(actual_err_wh[3], actual_err_wh[4], actual_err_wh[5]);
if (debug){
cout << "p_inlier_outler: "<<p_inlier_outler[0]<<", "<<p_inlier_outler[1]<<endl;
@ -263,8 +263,8 @@ TEST (BetweenFactorEM, updateNoiseModel ) {
gtsam::Pose2 rel_pose_ideal = p1.between(p2);
gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(noise);
SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas( (gtsam::Vector(3) << 1.5, 2.5, 4.05)));
SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas( (gtsam::Vector(3) << 50.0, 50.0, 10.0)));
SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(Vector3(1.5, 2.5, 4.05)));
SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(Vector3(50.0, 50.0, 10.0)));
gtsam::Values values;
values.insert(key1, p1);
@ -287,14 +287,14 @@ TEST (BetweenFactorEM, updateNoiseModel ) {
SharedGaussian model_inlier_new = f.get_model_inlier();
SharedGaussian model_outlier_new = f.get_model_outlier();
model_inlier->print("model_inlier:");
model_outlier->print("model_outlier:");
model_inlier_new->print("model_inlier_new:");
model_outlier_new->print("model_outlier_new:");
// model_inlier->print("model_inlier:");
// model_outlier->print("model_outlier:");
// model_inlier_new->print("model_inlier_new:");
// model_outlier_new->print("model_outlier_new:");
}
#endif
// #endif
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}

41
gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp
View File

@ -16,22 +16,23 @@
* @date Jan 17, 2012
*/
#include <gtsam_unstable/slam/GaussMarkov1stOrderFactor.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/inference/Key.h>
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/deprecated/LieVector.h>
#include <gtsam/base/Vector.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/inference/Key.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam_unstable/slam/GaussMarkov1stOrderFactor.h>
using namespace std::placeholders;
using namespace std;
using namespace gtsam;
//! Factors
typedef GaussMarkov1stOrderFactor<LieVector> GaussMarkovFactor;
typedef GaussMarkov1stOrderFactor<Vector3> GaussMarkovFactor;
/* ************************************************************************* */
LieVector predictionError(const LieVector& v1, const LieVector& v2, const GaussMarkovFactor factor) {
Vector predictionError(const Vector& v1, const Vector& v2,
const GaussMarkovFactor factor) {
return factor.evaluateError(v1, v2);
}
@ -58,29 +59,29 @@ TEST( GaussMarkovFactor, error )
Key x1(1);
Key x2(2);
double delta_t = 0.10;
Vector tau = Vector3(100.0, 150.0, 10.0);
Vector3 tau(100.0, 150.0, 10.0);
SharedGaussian model = noiseModel::Isotropic::Sigma(3, 1.0);
LieVector v1 = LieVector(Vector3(10.0, 12.0, 13.0));
LieVector v2 = LieVector(Vector3(10.0, 15.0, 14.0));
Vector3 v1(10.0, 12.0, 13.0);
Vector3 v2(10.0, 15.0, 14.0);
// Create two nodes
linPoint.insert(x1, v1);
linPoint.insert(x2, v2);
GaussMarkovFactor factor(x1, x2, delta_t, tau, model);
Vector Err1( factor.evaluateError(v1, v2) );
Vector3 error1 = factor.evaluateError(v1, v2);
// Manually calculate the error
Vector alpha(tau.size());
Vector alpha_v1(tau.size());
Vector3 alpha(tau.size());
Vector3 alpha_v1(tau.size());
for(int i=0; i<tau.size(); i++){
alpha(i) = exp(- 1/tau(i)*delta_t );
alpha_v1(i) = alpha(i) * v1(i);
}
Vector Err2( v2 - alpha_v1 );
Vector3 error2 = v2 - alpha_v1;
CHECK(assert_equal(Err1, Err2, 1e-9));
CHECK(assert_equal(error1, error2, 1e-8));
}
/* ************************************************************************* */
@ -90,14 +91,14 @@ TEST (GaussMarkovFactor, jacobian ) {
Key x1(1);
Key x2(2);
double delta_t = 0.10;
Vector tau = Vector3(100.0, 150.0, 10.0);
Vector3 tau(100.0, 150.0, 10.0);
SharedGaussian model = noiseModel::Isotropic::Sigma(3, 1.0);
GaussMarkovFactor factor(x1, x2, delta_t, tau, model);
// Update the linearization point
LieVector v1_upd = LieVector(Vector3(0.5, -0.7, 0.3));
LieVector v2_upd = LieVector(Vector3(-0.7, 0.4, 0.9));
Vector3 v1_upd(0.5, -0.7, 0.3);
Vector3 v2_upd(-0.7, 0.4, 0.9);
// Calculate the Jacobian matrix using the factor
Matrix computed_H1, computed_H2;
@ -115,8 +116,8 @@ TEST (GaussMarkovFactor, jacobian ) {
v1_upd, v2_upd);
// Verify they are equal for this choice of state
CHECK( assert_equal(numerical_H1, computed_H1, 1e-9));
CHECK( assert_equal(numerical_H2, computed_H2, 1e-9));
CHECK(assert_equal(numerical_H1, computed_H1, 1e-9));
CHECK(assert_equal(numerical_H2, computed_H2, 1e-9));
}
/* ************************************************************************* */

2
gtsam_unstable/slam/tests/testSerialization.cpp
View File

@ -10,7 +10,7 @@
#include <CppUnitLite/TestHarness.h>
#include <iostream>
#include <gtsam/slam/serialization.h>
#include <gtsam_unstable/slam/serialization.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Pose3.h>

12
matlab/+gtsam/Contents.m
View File

@ -49,9 +49,6 @@
% Ordering - class Ordering, see Doxygen page for details
% Value - class Value, see Doxygen page for details
% Values - class Values, see Doxygen page for details
% LieScalar - class LieScalar, see Doxygen page for details
% LieVector - class LieVector, see Doxygen page for details
% LieMatrix - class LieMatrix, see Doxygen page for details
% NonlinearFactor - class NonlinearFactor, see Doxygen page for details
% NonlinearFactorGraph - class NonlinearFactorGraph, see Doxygen page for details
%
@ -101,9 +98,6 @@
% BearingFactor2D - class BearingFactor2D, see Doxygen page for details
% BearingFactor3D - class BearingFactor3D, see Doxygen page for details
% BearingRangeFactor2D - class BearingRangeFactor2D, see Doxygen page for details
% BetweenFactorLieMatrix - class BetweenFactorLieMatrix, see Doxygen page for details
% BetweenFactorLieScalar - class BetweenFactorLieScalar, see Doxygen page for details
% BetweenFactorLieVector - class BetweenFactorLieVector, see Doxygen page for details
% BetweenFactorPoint2 - class BetweenFactorPoint2, see Doxygen page for details
% BetweenFactorPoint3 - class BetweenFactorPoint3, see Doxygen page for details
% BetweenFactorPose2 - class BetweenFactorPose2, see Doxygen page for details
@ -116,9 +110,6 @@
% GenericStereoFactor3D - class GenericStereoFactor3D, see Doxygen page for details
% NonlinearEqualityCal3_S2 - class NonlinearEqualityCal3_S2, see Doxygen page for details
% NonlinearEqualityCalibratedCamera - class NonlinearEqualityCalibratedCamera, see Doxygen page for details
% NonlinearEqualityLieMatrix - class NonlinearEqualityLieMatrix, see Doxygen page for details
% NonlinearEqualityLieScalar - class NonlinearEqualityLieScalar, see Doxygen page for details
% NonlinearEqualityLieVector - class NonlinearEqualityLieVector, see Doxygen page for details
% NonlinearEqualityPoint2 - class NonlinearEqualityPoint2, see Doxygen page for details
% NonlinearEqualityPoint3 - class NonlinearEqualityPoint3, see Doxygen page for details
% NonlinearEqualityPose2 - class NonlinearEqualityPose2, see Doxygen page for details
@ -129,9 +120,6 @@
% NonlinearEqualityStereoPoint2 - class NonlinearEqualityStereoPoint2, see Doxygen page for details
% PriorFactorCal3_S2 - class PriorFactorCal3_S2, see Doxygen page for details
% PriorFactorCalibratedCamera - class PriorFactorCalibratedCamera, see Doxygen page for details
% PriorFactorLieMatrix - class PriorFactorLieMatrix, see Doxygen page for details
% PriorFactorLieScalar - class PriorFactorLieScalar, see Doxygen page for details
% PriorFactorLieVector - class PriorFactorLieVector, see Doxygen page for details
% PriorFactorPoint2 - class PriorFactorPoint2, see Doxygen page for details
% PriorFactorPoint3 - class PriorFactorPoint3, see Doxygen page for details
% PriorFactorPose2 - class PriorFactorPose2, see Doxygen page for details

8
matlab/unstable_examples/+imuSimulator/IMUComparison.m
View File

@ -51,13 +51,13 @@ isam = gtsam.ISAM2(isamParams);
initialValues = Values;
initialValues.insert(symbol('x',0), currentPoseGlobal);
initialValues.insert(symbol('v',0), LieVector(currentVelocityGlobal));
initialValues.insert(symbol('v',0), currentVelocityGlobal);
initialValues.insert(symbol('b',0), imuBias.ConstantBias([0;0;0],[0;0;0]));
initialFactors = NonlinearFactorGraph;
initialFactors.add(PriorFactorPose3(symbol('x',0), ...
currentPoseGlobal, noiseModel.Isotropic.Sigma(6, 1.0)));
initialFactors.add(PriorFactorLieVector(symbol('v',0), ...
LieVector(currentVelocityGlobal), noiseModel.Isotropic.Sigma(3, 1.0)));
initialFactors.add(PriorFactorVector(symbol('v',0), ...
currentVelocityGlobal, noiseModel.Isotropic.Sigma(3, 1.0)));
initialFactors.add(PriorFactorConstantBias(symbol('b',0), ...
imuBias.ConstantBias([0;0;0],[0;0;0]), noiseModel.Isotropic.Sigma(6, 1.0)));
@ -96,7 +96,7 @@ for t = times
initialVel = isam.calculateEstimate(symbol('v',lastSummaryIndex));
else
initialPose = Pose3;
initialVel = LieVector(velocity);
initialVel = velocity;
end
initialValues.insert(symbol('x',lastSummaryIndex+1), initialPose);
initialValues.insert(symbol('v',lastSummaryIndex+1), initialVel);

8
matlab/unstable_examples/+imuSimulator/IMUComparison_with_cov.m
View File

@ -43,15 +43,15 @@ sigma_init_b = 1.0;
initialValues = Values;
initialValues.insert(symbol('x',0), currentPoseGlobal);
initialValues.insert(symbol('v',0), LieVector(currentVelocityGlobal));
initialValues.insert(symbol('v',0), currentVelocityGlobal);
initialValues.insert(symbol('b',0), imuBias.ConstantBias([0;0;0],[0;0;0]));
initialFactors = NonlinearFactorGraph;
% Prior on initial pose
initialFactors.add(PriorFactorPose3(symbol('x',0), ...
currentPoseGlobal, noiseModel.Isotropic.Sigma(6, sigma_init_x)));
% Prior on initial velocity
initialFactors.add(PriorFactorLieVector(symbol('v',0), ...
LieVector(currentVelocityGlobal), noiseModel.Isotropic.Sigma(3, sigma_init_v)));
initialFactors.add(PriorFactorVector(symbol('v',0), ...
currentVelocityGlobal, noiseModel.Isotropic.Sigma(3, sigma_init_v)));
% Prior on initial bias
initialFactors.add(PriorFactorConstantBias(symbol('b',0), ...
imuBias.ConstantBias([0;0;0],[0;0;0]), noiseModel.Isotropic.Sigma(6, sigma_init_b)));
@ -91,7 +91,7 @@ for t = times
initialVel = isam.calculateEstimate(symbol('v',lastSummaryIndex));
else
initialPose = Pose3;
initialVel = LieVector(velocity);
initialVel = velocity;
end
initialValues.insert(symbol('x',lastSummaryIndex+1), initialPose);
initialValues.insert(symbol('v',lastSummaryIndex+1), initialVel);

6
matlab/unstable_examples/+imuSimulator/coriolisExample.m
View File

@ -175,9 +175,9 @@ for i = 1:length(times)
% known initial conditions
currentPoseEstimate = currentPoseFixedGT;
if navFrameRotating == 1
currentVelocityEstimate = LieVector(currentVelocityRotatingGT);
currentVelocityEstimate = currentVelocityRotatingGT;
else
currentVelocityEstimate = LieVector(currentVelocityFixedGT);
currentVelocityEstimate = currentVelocityFixedGT;
end
% Set Priors
@ -186,7 +186,7 @@ for i = 1:length(times)
newValues.insert(currentBiasKey, zeroBias);
% Initial values, same for IMU types 1 and 2
newFactors.add(PriorFactorPose3(currentPoseKey, currentPoseEstimate, sigma_init_x));
newFactors.add(PriorFactorLieVector(currentVelKey, currentVelocityEstimate, sigma_init_v));
newFactors.add(PriorFactorVector(currentVelKey, currentVelocityEstimate, sigma_init_v));
newFactors.add(PriorFactorConstantBias(currentBiasKey, zeroBias, sigma_init_b));
% Store data

2
matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m
View File

@ -27,7 +27,7 @@ for i=0:length(measurements)
if options.includeIMUFactors == 1
currentVelKey = symbol('v', 0);
currentVel = values.atPoint3(currentVelKey);
graph.add(PriorFactorLieVector(currentVelKey, LieVector(currentVel), noiseModels.noiseVel));
graph.add(PriorFactorVector(currentVelKey, currentVel, noiseModels.noiseVel));
currentBiasKey = symbol('b', 0);
currentBias = values.atPoint3(currentBiasKey);

2
matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m
View File

@ -82,7 +82,7 @@ if options.useRealData == 1
end
% Add Values: velocity and bias
values.insert(currentVelKey, LieVector(currentVel));
values.insert(currentVelKey, currentVel);
values.insert(currentBiasKey, metadata.imu.zeroBias);
end

2
matlab/unstable_examples/FlightCameraTransformIMU.m
View File

@ -167,7 +167,7 @@ for i=1:size(trajectory)-1
%% priors on first two poses
if i < 3
% fg.add(PriorFactorLieVector(currentVelKey, currentVelocityGlobal, sigma_init_v));
% fg.add(PriorFactorVector(currentVelKey, currentVelocityGlobal, sigma_init_v));
fg.add(PriorFactorConstantBias(currentBiasKey, currentBias, sigma_init_b));
end

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