Merge pull request #999 from borglab/feature/deprecate_in_v42
Frank Dellaert
GitHub
commit
3e768247ef
|
|
@ -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
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -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} \
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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.
|
||||
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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")
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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"
|
||||
|
|
@ -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>
|
||||
|
|
@ -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>
|
||||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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); }
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
||||
|
|
@ -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); }
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -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);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
|
@ -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); }
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -153,7 +153,7 @@ class ParameterMatrix {
|
|||
return matrix_ * other;
|
||||
}
|
||||
|
||||
/// @name Vector Space requirements, following LieMatrix
|
||||
/// @name Vector Space requirements
|
||||
/// @{
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
||||
|
|
|
|||
|
|
@ -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,
|
||||
|
|
|
|||
|
|
@ -153,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;
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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 */
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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:
|
||||
|
||||
|
|
|
|||
|
|
@ -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) {
|
||||
Vector6 GTSAM_DEPRECATED localCoordinates(const ConstantBias& q) {
|
||||
return between(q).vector();
|
||||
}
|
||||
ConstantBias retract(const Vector6& v) {
|
||||
ConstantBias GTSAM_DEPRECATED retract(const Vector6& v) {
|
||||
return compose(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:
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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) {
|
||||
|
|
|
|||
|
|
@ -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> {
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -250,25 +250,25 @@ 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 */
|
||||
/** @deprecated */
|
||||
void GTSAM_DEPRECATED
|
||||
saveGraph(const std::string& filename, const Values& values,
|
||||
const GraphvizFormatting& graphvizFormatting,
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -421,4 +421,8 @@ private:
|
|||
};
|
||||
// \class BetweenFactorEM
|
||||
|
||||
/// traits
|
||||
template<class VALUE>
|
||||
struct traits<BetweenFactorEM<VALUE> > : public Testable<BetweenFactorEM<VALUE> > {};
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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);}
|
||||
|
|
|
|||
|
|
@ -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));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -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>
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -46,7 +46,7 @@ clear logposes relposes
|
|||
|
||||
%% Get initial conditions for the estimated trajectory
|
||||
currentPoseGlobal = Pose3;
|
||||
currentVelocityGlobal = LieVector([0;0;0]); % the vehicle is stationary at the beginning
|
||||
currentVelocityGlobal = [0;0;0]; % the vehicle is stationary at the beginning
|
||||
currentBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1));
|
||||
sigma_init_x = noiseModel.Isotropic.Sigmas([ 1.0; 1.0; 0.01; 0.01; 0.01; 0.01 ]);
|
||||
sigma_init_v = noiseModel.Isotropic.Sigma(3, 1000.0);
|
||||
|
|
@ -88,7 +88,7 @@ for measurementIndex = 1:length(timestamps)
|
|||
newValues.insert(currentVelKey, currentVelocityGlobal);
|
||||
newValues.insert(currentBiasKey, currentBias);
|
||||
newFactors.add(PriorFactorPose3(currentPoseKey, currentPoseGlobal, sigma_init_x));
|
||||
newFactors.add(PriorFactorLieVector(currentVelKey, currentVelocityGlobal, sigma_init_v));
|
||||
newFactors.add(PriorFactorVector(currentVelKey, currentVelocityGlobal, sigma_init_v));
|
||||
newFactors.add(PriorFactorConstantBias(currentBiasKey, currentBias, sigma_init_b));
|
||||
else
|
||||
t_previous = timestamps(measurementIndex-1, 1);
|
||||
|
|
|
|||
|
|
@ -33,8 +33,6 @@ add_custom_target(gtsam_unstable_header DEPENDS "${PROJECT_SOURCE_DIR}/gtsam_uns
|
|||
set(ignore
|
||||
gtsam::Point2
|
||||
gtsam::Point3
|
||||
gtsam::LieVector
|
||||
gtsam::LieMatrix
|
||||
gtsam::ISAM2ThresholdMapValue
|
||||
gtsam::FactorIndices
|
||||
gtsam::FactorIndexSet
|
||||
|
|
@ -116,8 +114,6 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON)
|
|||
set(ignore
|
||||
gtsam::Point2
|
||||
gtsam::Point3
|
||||
gtsam::LieVector
|
||||
gtsam::LieMatrix
|
||||
gtsam::ISAM2ThresholdMapValue
|
||||
gtsam::FactorIndices
|
||||
gtsam::FactorIndexSet
|
||||
|
|
|
|||
Loading…
Reference in New Issue