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

some changes that get testPriorFactor compiling

release/4.3a0
kartik arcot 2023-01-05 18:02:10 -08:00
parent 841dc6005a
commit f7c683a794
11 changed files with 40 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
gtsam/navigation/CombinedImuFactor.cpp
View File

@ -222,9 +222,9 @@ bool CombinedImuFactor::equals(const NonlinearFactor& other, double tol) const {
Vector CombinedImuFactor::evaluateError(const Pose3& pose_i,
const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j,
const imuBias::ConstantBias& bias_i, const imuBias::ConstantBias& bias_j,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2,
boost::optional<Matrix&> H3, boost::optional<Matrix&> H4,
boost::optional<Matrix&> H5, boost::optional<Matrix&> H6) const {
OptionalMatrixType H1, OptionalMatrixType H2,
OptionalMatrixType H3, OptionalMatrixType H4,
OptionalMatrixType H5, OptionalMatrixType H6) const {
// error wrt bias evolution model (random walk)
Matrix6 Hbias_i, Hbias_j;

2
gtsam/navigation/GPSFactor.cpp
View File

@ -80,7 +80,7 @@ bool GPSFactor2::equals(const NonlinearFactor& expected, double tol) const {
//***************************************************************************
Vector GPSFactor2::evaluateError(const NavState& p,
boost::optional<Matrix&> H) const {
OptionalMatrixType H) const {
return p.position(H) -nT_;
}

2
gtsam/navigation/GPSFactor.h
View File

@ -152,7 +152,7 @@ public:
/// vector of errors
Vector evaluateError(const NavState& p,
boost::optional<Matrix&> H = boost::none) const override;
OptionalMatrixType H = OptionalNone) const override;
inline const Point3 & measurementIn() const {
return nT_;

2
gtsam/nonlinear/CustomFactor.cpp
View File

@ -43,7 +43,7 @@ Vector CustomFactor::unwhitenedError(const Values& x, OptionalMatrixVecType H) c
* return error
* ```
*/
return this->error_function_(*this, x, H.get_ptr());
return this->error_function_(*this, x, &(*H));
} else {
/*
* In this case, we pass the a `nullptr` to pybind, and it will translate to `None` in Python.

2
gtsam/nonlinear/CustomFactor.h
View File

@ -75,7 +75,7 @@ public:
* Calls the errorFunction closure, which is a std::function object
* One can check if a derivative is needed in the errorFunction by checking the length of Jacobian array
*/
Vector unwhitenedError(const Values &x, OptionalMatrixVecType H = boost::none) const override;
Vector unwhitenedError(const Values &x, OptionalMatrixVecType H = OptionalMatrixVecNone) const override;
/** print */
void print(const std::string &s,

2
gtsam/nonlinear/ExpressionFactor.h
View File

@ -97,7 +97,7 @@ protected:
* both the function evaluation and its derivative(s) in H.
*/
Vector unwhitenedError(const Values& x,
OptionalMatrixVecType H = OptionalNone) const override {
OptionalMatrixVecType H = OptionalMatrixVecNone) const override {
if (H) {
const T value = expression_.valueAndDerivatives(x, keys_, dims_, *H);
// NOTE(hayk): Doing the reverse, AKA Local(measured_, value) is not correct here

28
gtsam/nonlinear/NonlinearFactor.h
View File

@ -41,13 +41,17 @@ namespace gtsam {
// These typedefs and aliases will help with making the evaluateError interface
// independent of boost
using OptionalNoneType = std::nullptr_t;
#define OptionalNone = nullptr
// TODO: Change this to OptionalMatrixNone
#define OptionalNone static_cast<Matrix*>(nullptr)
template <typename T = void>
using OptionalMatrixTypeT = Matrix*;
template <typename T = void>
using MatrixTypeT = Matrix;
using OptionalMatrixType = Matrix*;
// These typedefs and aliases will help with making the unwhitenedError interface
// independent of boost
using OptionalMatrixVecType = std::vector<Matrix>*;
#define OptionalMatrixVecNone static_cast<std::vector<Matrix>*>(nullptr)
#else
// creating a none value to use when declaring our interfaces
using OptionalNoneType = boost::none_t;
@ -56,6 +60,7 @@ template <typename T = void>
using OptionalMatrixTypeT = boost::optional<Matrix&>;
using OptionalMatrixType = boost::optional<Matrix&>;
using OptionalMatrixVecType = boost::optional<std::vector<Matrix>&>;
#define OptionalMatrixVecNone boost::none
#endif
/**
* Nonlinear factor base class
@ -252,10 +257,10 @@ public:
* If the optional arguments is specified, it should compute
* both the function evaluation and its derivative(s) in H.
*/
virtual Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = OptionalNone) const = 0;
#ifdef NO_BOOST_C17
virtual Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = OptionalMatrixVecNone) const = 0;
#ifdef NO_BOOST_CPP17
// support taking in the actual vector instead of the pointer as well
Vector unwhitenedError(const Values& x, std::vector<Matrix>& H) {
Vector unwhitenedError(const Values& x, std::vector<Matrix>& H) const {
return unwhitenedError(x, &H);
}
#endif
@ -565,7 +570,7 @@ protected:
*/
Vector unwhitenedError(
const Values& x,
OptionalMatrixVecType H = OptionalNone) const override {
OptionalMatrixVecType H = OptionalMatrixVecNone) const override {
return unwhitenedError(boost::mp11::index_sequence_for<ValueTypes...>{}, x,
H);
}
@ -599,8 +604,15 @@ protected:
virtual Vector evaluateError(const ValueTypes&... x,
OptionalMatrixTypeT<ValueTypes>... H) const = 0;
/// @}
#ifdef NO_BOOST_CPP17
// if someone uses the evaluateError function by supplying all the optional
// arguments then redirect the call to the one which takes pointers
Vector evaluateError(const ValueTypes&... x, MatrixTypeT<ValueTypes>&... H) const {
return evaluateError(x..., (&H)...);
}
#endif
/// @}
/// @name Convenience method overloads
/// @{
@ -630,7 +642,7 @@ protected:
"or Matrix*");
// if they pass all matrices then we want to pass their pointers instead
if constexpr (are_all_mat) {
return evaluateError(x..., (&OptionalJacArgs)...);
return evaluateError(x..., (&H)...);
} else {
return evaluateError(x..., std::forward<OptionalJacArgs>(H)..., static_cast<OptionalMatrixType>(OptionalNone));
}
@ -659,7 +671,7 @@ protected:
inline Vector unwhitenedError(
boost::mp11::index_sequence<Indices...>, //
const Values& x,
OptionalMatrixVecType H = OptionalNone) const {
OptionalMatrixVecType H = OptionalMatrixVecNone) const {
if (this->active(x)) {
if (H) {
return evaluateError(x.at<ValueTypes>(keys_[Indices])...,

8
gtsam/sfm/ShonanFactor.cpp
View File

@ -75,8 +75,8 @@ bool ShonanFactor<d>::equals(const NonlinearFactor &expected,
//******************************************************************************
template <size_t d>
void ShonanFactor<d>::fillJacobians(const Matrix &M1, const Matrix &M2,
boost::optional<Matrix &> H1,
boost::optional<Matrix &> H2) const {
OptionalMatrixType H1,
OptionalMatrixType H2) const {
gttic(ShonanFactor_Jacobians);
const size_t dim = p_ * d; // Stiefel manifold dimension
@ -106,8 +106,8 @@ void ShonanFactor<d>::fillJacobians(const Matrix &M1, const Matrix &M2,
//******************************************************************************
template <size_t d>
Vector ShonanFactor<d>::evaluateError(const SOn &Q1, const SOn &Q2,
boost::optional<Matrix &> H1,
boost::optional<Matrix &> H2) const {
OptionalMatrixType H1,
OptionalMatrixType H2) const {
gttic(ShonanFactor_evaluateError);
const Matrix &M1 = Q1.matrix();

8
gtsam/sfm/ShonanFactor.h
View File

@ -73,15 +73,15 @@ public:
/// projects down from SO(p) to the Stiefel manifold of px3 matrices.
Vector
evaluateError(const SOn &Q1, const SOn &Q2,
boost::optional<Matrix &> H1 = boost::none,
boost::optional<Matrix &> H2 = boost::none) const override;
OptionalMatrixType H1 = OptionalNone,
OptionalMatrixType H2 = OptionalNone) const override;
/// @}
private:
/// Calculate Jacobians if asked, Only implemented for d=2 and 3 in .cpp
void fillJacobians(const Matrix &M1, const Matrix &M2,
boost::optional<Matrix &> H1,
boost::optional<Matrix &> H2) const;
OptionalMatrixType H1,
OptionalMatrixType H2) const;
};
// Explicit instantiation for d=2 and d=3 in .cpp file:

6
gtsam/slam/OrientedPlane3Factor.h
View File

@ -44,8 +44,8 @@ class GTSAM_EXPORT OrientedPlane3Factor: public NoiseModelFactorN<Pose3, Oriente
/// evaluateError
Vector evaluateError(
const Pose3& pose, const OrientedPlane3& plane,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const override;
OptionalMatrixType H1 = OptionalNone,
OptionalMatrixType H2 = OptionalNone) const override;
};
// TODO: Convert this factor to dimension two, three dimensions is redundant for direction prior
@ -73,7 +73,7 @@ class GTSAM_EXPORT OrientedPlane3DirectionPrior : public NoiseModelFactorN<Orien
bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
Vector evaluateError(const OrientedPlane3& plane,
boost::optional<Matrix&> H = boost::none) const override;
OptionalMatrixType H = OptionalNone) const override;
};
} // gtsam

2
gtsam_unstable/slam/MultiProjectionFactor.h
View File

@ -129,7 +129,7 @@ namespace gtsam {
}
/// Evaluate error h(x)-z and optionally derivatives
Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = OptionalNone) const override {
Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = OptionalMatrixVecNone) const override {
Vector a;
return a;