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

added using keyword to expose the evaluateError overloads to the derived classes

release/4.3a0
kartik arcot 2023-01-06 10:40:48 -08:00
parent f7c683a794
commit a070cf3130
12 changed files with 32 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
CMakeLists.txt
View File

@ -8,6 +8,7 @@ endif()
option(GTSAM_NO_BOOST_CPP17 "Require and use boost" ON)
add_definitions(-Wno-deprecated-declarations)
add_definitions(-ftemplate-backtrace-limit=0)
set(CMAKE_CXX_STANDARD 17)
if (GTSAM_NO_BOOST_CPP17)

2
gtsam/navigation/AttitudeFactor.h
View File

@ -82,6 +82,7 @@ class GTSAM_EXPORT Rot3AttitudeFactor: public NoiseModelFactorN<Rot3>, public At
public:
using Base::evaluateError;
/// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<Rot3AttitudeFactor> shared_ptr;
@ -156,6 +157,7 @@ class GTSAM_EXPORT Pose3AttitudeFactor: public NoiseModelFactorN<Pose3>,
typedef NoiseModelFactorN<Pose3> Base;
public:
using Base::evaluateError;
/// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<Pose3AttitudeFactor> shared_ptr;

7
gtsam/navigation/BarometricFactor.h
View File

@ -38,6 +38,7 @@ class GTSAM_EXPORT BarometricFactor : public NoiseModelFactorN<Pose3, double> {
double nT_; ///< Height Measurement based on a standard atmosphere
public:
using Base::evaluateError;
/// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<BarometricFactor> shared_ptr;
@ -76,10 +77,8 @@ class GTSAM_EXPORT BarometricFactor : public NoiseModelFactorN<Pose3, double> {
double tol = 1e-9) const override;
/// vector of errors
Vector evaluateError(
const Pose3& p, const double& b,
OptionalMatrixType H = OptionalNone,
OptionalMatrixType H2 = OptionalNone) const override;
Vector evaluateError(const Pose3& p, const double& b,
OptionalMatrixType H, OptionalMatrixType H2) const override;
inline const double& measurementIn() const { return nT_; }

6
gtsam/navigation/ConstantVelocityFactor.h
View File

@ -30,7 +30,8 @@ class ConstantVelocityFactor : public NoiseModelFactorN<NavState, NavState> {
double dt_;
public:
using Base = NoiseModelFactorN<NavState, NavState>;
using Base = NoiseModelFactor2<NavState, NavState>;
using Base::evaluateError;
public:
ConstantVelocityFactor(Key i, Key j, double dt, const SharedNoiseModel &model)
@ -48,8 +49,7 @@ class ConstantVelocityFactor : public NoiseModelFactorN<NavState, NavState> {
* @return * Vector
*/
gtsam::Vector evaluateError(const NavState &x1, const NavState &x2,
OptionalMatrixType H1 = OptionalNone,
OptionalMatrixType H2 = OptionalNone) const override {
OptionalMatrixType H1, OptionalMatrixType H2) const override {
// only used to use update() below
static const Vector3 b_accel{0.0, 0.0, 0.0};
static const Vector3 b_omega{0.0, 0.0, 0.0};

1
gtsam/navigation/ImuFactor.h
View File

@ -179,6 +179,7 @@ private:
PreintegratedImuMeasurements _PIM_;
public:
using Base::evaluateError;
/** Shorthand for a smart pointer to a factor */
#if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5

6
gtsam/nonlinear/FunctorizedFactor.h
View File

@ -87,8 +87,7 @@ class FunctorizedFactor : public NoiseModelFactorN<T> {
NonlinearFactor::shared_ptr(new FunctorizedFactor<R, T>(*this)));
}
Vector evaluateError(const T &params, OptionalMatrixType H =
OptionalNone) const override {
Vector evaluateError(const T &params, OptionalMatrixType H) const override {
R x = func_(params, H);
Vector error = traits<R>::Local(measured_, x);
return error;
@ -193,8 +192,7 @@ class FunctorizedFactor2 : public NoiseModelFactorN<T1, T2> {
Vector evaluateError(
const T1 &params1, const T2 &params2,
OptionalMatrixType H1 = OptionalNone,
OptionalMatrixType H2 = OptionalNone) const override {
OptionalMatrixType H1, OptionalMatrixType H2) const override {
R x = func_(params1, params2, H1, H2);
Vector error = traits<R>::Local(measured_, x);
return error;

1
gtsam/nonlinear/NonlinearEquality.h
View File

@ -46,6 +46,7 @@ class NonlinearEquality: public NoiseModelFactorN<VALUE> {
public:
typedef VALUE T;
using NoiseModelFactor1<VALUE>::evaluateError;
private:

6
gtsam/sfm/ShonanFactor.h
View File

@ -42,6 +42,7 @@ class GTSAM_EXPORT ShonanFactor : public NoiseModelFactorN<SOn, SOn> {
using Rot = typename std::conditional<d == 2, Rot2, Rot3>::type;
public:
using NoiseModelFactor2<SOn, SOn>::evaluateError;
/// @name Constructor
/// @{
@ -71,10 +72,7 @@ public:
/// Error is Frobenius norm between Q1*P*R12 and Q2*P, where P=[I_3x3;0]
/// projects down from SO(p) to the Stiefel manifold of px3 matrices.
Vector
evaluateError(const SOn &Q1, const SOn &Q2,
OptionalMatrixType H1 = OptionalNone,
OptionalMatrixType H2 = OptionalNone) const override;
Vector evaluateError(const SOn& Q1, const SOn& Q2, OptionalMatrixType H1, OptionalMatrixType H2) const override;
/// @}
private:

5
gtsam/sfm/TranslationFactor.h
View File

@ -45,6 +45,7 @@ class TranslationFactor : public NoiseModelFactorN<Point3, Point3> {
Point3 measured_w_aZb_;
public:
using NoiseModelFactor2<Point3, Point3>::evaluateError;
/// default constructor
TranslationFactor() {}
@ -65,8 +66,8 @@ class TranslationFactor : public NoiseModelFactorN<Point3, Point3> {
*/
Vector evaluateError(
const Point3& Ta, const Point3& Tb,
OptionalMatrixType H1 = OptionalNone,
OptionalMatrixType H2 = OptionalNone) const override {
OptionalMatrixType H1,
OptionalMatrixType H2) const override {
const Point3 dir = Tb - Ta;
Matrix33 H_predicted_dir;
const Point3 predicted = normalize(dir, H1 || H2 ? &H_predicted_dir : nullptr);

16
gtsam/slam/EssentialMatrixFactor.h
View File

@ -38,6 +38,7 @@ class EssentialMatrixFactor : public NoiseModelFactorN<EssentialMatrix> {
typedef EssentialMatrixFactor This;
public:
using Base::evaluateError;
/**
* Constructor
* @param key Essential Matrix variable key
@ -90,8 +91,7 @@ class EssentialMatrixFactor : public NoiseModelFactorN<EssentialMatrix> {
/// vector of errors returns 1D vector
Vector evaluateError(
const EssentialMatrix& E,
OptionalMatrixType H = OptionalNone) const override {
const EssentialMatrix& E, OptionalMatrixType H) const override {
Vector error(1);
error << E.error(vA_, vB_, H);
return error;
@ -115,6 +115,7 @@ class EssentialMatrixFactor2
typedef EssentialMatrixFactor2 This;
public:
using Base::evaluateError;
/**
* Constructor
* @param key1 Essential Matrix variable key
@ -173,8 +174,7 @@ class EssentialMatrixFactor2
*/
Vector evaluateError(
const EssentialMatrix& E, const double& d,
OptionalMatrixType DE = OptionalNone,
OptionalMatrixType Dd = OptionalNone) const override {
OptionalMatrixType DE, OptionalMatrixType Dd) const override {
// We have point x,y in image 1
// Given a depth Z, the corresponding 3D point P1 = Z*(x,y,1) = (x,y,1)/d
// We then convert to second camera by P2 = 1R2'*(P1-1T2)
@ -235,6 +235,7 @@ class EssentialMatrixFactor3 : public EssentialMatrixFactor2 {
Rot3 cRb_; ///< Rotation from body to camera frame
public:
using Base::evaluateError;
/**
* Constructor
* @param key1 Essential Matrix variable key
@ -284,8 +285,7 @@ class EssentialMatrixFactor3 : public EssentialMatrixFactor2 {
*/
Vector evaluateError(
const EssentialMatrix& E, const double& d,
OptionalMatrixType DE = OptionalNone,
OptionalMatrixType Dd = OptionalNone) const override {
OptionalMatrixType DE, OptionalMatrixType Dd) const override {
if (!DE) {
// Convert E from body to camera frame
EssentialMatrix cameraE = cRb_ * E;
@ -332,6 +332,7 @@ class EssentialMatrixFactor4
typedef Eigen::Matrix<double, 2, DimK> JacobianCalibration;
public:
using Base::evaluateError;
/**
* Constructor
* @param keyE Essential Matrix (from camera B to A) variable key
@ -372,8 +373,7 @@ class EssentialMatrixFactor4
*/
Vector evaluateError(
const EssentialMatrix& E, const CALIBRATION& K,
OptionalMatrixType H1 = OptionalNone,
OptionalMatrixType H2 = OptionalNone) const override {
OptionalMatrixType H1, OptionalMatrixType H2) const override {
// converting from pixel coordinates to normalized coordinates cA and cB
JacobianCalibration cA_H_K; // dcA/dK
JacobianCalibration cB_H_K; // dcB/dK

5
gtsam/slam/FrobeniusFactor.h
View File

@ -54,6 +54,7 @@ class FrobeniusPrior : public NoiseModelFactorN<Rot> {
Eigen::Matrix<double, Dim, 1> vecM_; ///< vectorized matrix to approximate
public:
using NoiseModelFactor1<Rot>::evaluateError;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/// Constructor
@ -64,8 +65,7 @@ class FrobeniusPrior : public NoiseModelFactorN<Rot> {
}
/// Error is just Frobenius norm between Rot element and vectorized matrix M.
Vector evaluateError(const Rot& R,
OptionalMatrixType H = OptionalNone) const override {
Vector evaluateError(const Rot& R, OptionalMatrixType H) const override {
return R.vec(H) - vecM_; // Jacobian is computed only when needed.
}
};
@ -108,6 +108,7 @@ class FrobeniusBetweenFactor : public NoiseModelFactorN<Rot, Rot> {
enum { Dim = Rot::VectorN2::RowsAtCompileTime };
public:
using NoiseModelFactor2<Rot, Rot>::evaluateError;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/// @name Constructor

6
gtsam/slam/ReferenceFrameFactor.h
View File

@ -62,6 +62,7 @@ protected:
public:
typedef NoiseModelFactorN<POINT, TRANSFORM, POINT> Base;
typedef ReferenceFrameFactor<POINT, TRANSFORM> This;
using Base::evaluateError;
typedef POINT Point;
typedef TRANSFORM Transform;
@ -95,9 +96,8 @@ public:
/** Combined cost and derivative function using boost::optional */
Vector evaluateError(const Point& global, const Transform& trans, const Point& local,
OptionalMatrixType Dforeign = OptionalNone,
OptionalMatrixType Dtrans = OptionalNone,
OptionalMatrixType Dlocal = OptionalNone) const override {
OptionalMatrixType Dforeign, OptionalMatrixType Dtrans,
OptionalMatrixType Dlocal) const override {
Point newlocal = transform_point<Transform,Point>(trans, global, Dtrans, Dforeign);
if (Dlocal) {
*Dlocal = -1* Matrix::Identity(traits<Point>::dimension, traits<Point>::dimension);