return before using statement
parent
5575dc1f69
commit
200aa13701
|
@ -71,6 +71,7 @@ class UnaryFactor: public NoiseModelFactorN<Pose2> {
|
||||||
double mx_, my_;
|
double mx_, my_;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
// Provide access to Matrix& version of evaluateError:
|
// Provide access to Matrix& version of evaluateError:
|
||||||
using NoiseModelFactor1<Pose2>::evaluateError;
|
using NoiseModelFactor1<Pose2>::evaluateError;
|
||||||
|
|
||||||
|
|
|
@ -158,6 +158,7 @@ class GTSAM_EXPORT Pose3AttitudeFactor: public NoiseModelFactorN<Pose3>,
|
||||||
typedef NoiseModelFactorN<Pose3> Base;
|
typedef NoiseModelFactorN<Pose3> Base;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
// Provide access to the Matrix& version of evaluateError:
|
// Provide access to the Matrix& version of evaluateError:
|
||||||
using Base::evaluateError;
|
using Base::evaluateError;
|
||||||
|
|
||||||
|
|
|
@ -38,6 +38,7 @@ class GTSAM_EXPORT BarometricFactor : public NoiseModelFactorN<Pose3, double> {
|
||||||
double nT_; ///< Height Measurement based on a standard atmosphere
|
double nT_; ///< Height Measurement based on a standard atmosphere
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
// Provide access to the Matrix& version of evaluateError:
|
// Provide access to the Matrix& version of evaluateError:
|
||||||
using Base::evaluateError;
|
using Base::evaluateError;
|
||||||
|
|
||||||
|
|
|
@ -268,6 +268,7 @@ private:
|
||||||
PreintegratedCombinedMeasurements _PIM_;
|
PreintegratedCombinedMeasurements _PIM_;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
// Provide access to Matrix& version of evaluateError:
|
// Provide access to Matrix& version of evaluateError:
|
||||||
using Base::evaluateError;
|
using Base::evaluateError;
|
||||||
|
|
||||||
|
|
|
@ -31,6 +31,7 @@ class ConstantVelocityFactor : public NoiseModelFactorN<NavState, NavState> {
|
||||||
|
|
||||||
public:
|
public:
|
||||||
using Base = NoiseModelFactor2<NavState, NavState>;
|
using Base = NoiseModelFactor2<NavState, NavState>;
|
||||||
|
|
||||||
// Provide access to the Matrix& version of evaluateError:
|
// Provide access to the Matrix& version of evaluateError:
|
||||||
using Base::evaluateError;
|
using Base::evaluateError;
|
||||||
|
|
||||||
|
|
|
@ -122,6 +122,7 @@ private:
|
||||||
Point3 nT_; ///< Position measurement in cartesian coordinates
|
Point3 nT_; ///< Position measurement in cartesian coordinates
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
// Provide access to the Matrix& version of evaluateError:
|
// Provide access to the Matrix& version of evaluateError:
|
||||||
using Base::evaluateError;
|
using Base::evaluateError;
|
||||||
|
|
||||||
|
|
|
@ -179,6 +179,7 @@ private:
|
||||||
PreintegratedImuMeasurements _PIM_;
|
PreintegratedImuMeasurements _PIM_;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
// Provide access to the Matrix& version of evaluateError:
|
// Provide access to the Matrix& version of evaluateError:
|
||||||
using Base::evaluateError;
|
using Base::evaluateError;
|
||||||
|
|
||||||
|
|
|
@ -37,6 +37,7 @@ class MagFactor: public NoiseModelFactorN<Rot2> {
|
||||||
const Point3 bias_; ///< bias
|
const Point3 bias_; ///< bias
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
// Provide access to Matrix& version of evaluateError:
|
// Provide access to Matrix& version of evaluateError:
|
||||||
using NoiseModelFactor1<Rot2>::evaluateError;
|
using NoiseModelFactor1<Rot2>::evaluateError;
|
||||||
|
|
||||||
|
@ -95,6 +96,7 @@ class MagFactor1: public NoiseModelFactorN<Rot3> {
|
||||||
const Point3 bias_; ///< bias
|
const Point3 bias_; ///< bias
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
// Provide access to Matrix& version of evaluateError:
|
// Provide access to Matrix& version of evaluateError:
|
||||||
using NoiseModelFactor1<Rot3>::evaluateError;
|
using NoiseModelFactor1<Rot3>::evaluateError;
|
||||||
|
|
||||||
|
@ -134,6 +136,7 @@ class MagFactor2: public NoiseModelFactorN<Point3, Point3> {
|
||||||
const Rot3 bRn_; ///< The assumed known rotation from nav to body
|
const Rot3 bRn_; ///< The assumed known rotation from nav to body
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
// Provide access to Matrix& version of evaluateError:
|
// Provide access to Matrix& version of evaluateError:
|
||||||
using NoiseModelFactor2<Point3, Point3>::evaluateError;
|
using NoiseModelFactor2<Point3, Point3>::evaluateError;
|
||||||
|
|
||||||
|
@ -177,6 +180,7 @@ class MagFactor3: public NoiseModelFactorN<double, Unit3, Point3> {
|
||||||
const Rot3 bRn_; ///< The assumed known rotation from nav to body
|
const Rot3 bRn_; ///< The assumed known rotation from nav to body
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
// Provide access to Matrix& version of evaluateError:
|
// Provide access to Matrix& version of evaluateError:
|
||||||
using NoiseModelFactor3<double, Unit3, Point3>::evaluateError;
|
using NoiseModelFactor3<double, Unit3, Point3>::evaluateError;
|
||||||
|
|
||||||
|
|
|
@ -49,6 +49,7 @@ class MagPoseFactor: public NoiseModelFactorN<POSE> {
|
||||||
GTSAM_CONCEPT_POSE_TYPE(POSE)
|
GTSAM_CONCEPT_POSE_TYPE(POSE)
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
// Provide access to the Matrix& version of evaluateError:
|
// Provide access to the Matrix& version of evaluateError:
|
||||||
using Base::evaluateError;
|
using Base::evaluateError;
|
||||||
|
|
||||||
|
|
|
@ -55,6 +55,7 @@ protected:
|
||||||
|
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
// Provide access to the Matrix& version of unwhitenedError:
|
// Provide access to the Matrix& version of unwhitenedError:
|
||||||
using NoiseModelFactor::unwhitenedError;
|
using NoiseModelFactor::unwhitenedError;
|
||||||
typedef boost::shared_ptr<ExpressionFactor<T> > shared_ptr;
|
typedef boost::shared_ptr<ExpressionFactor<T> > shared_ptr;
|
||||||
|
|
|
@ -65,6 +65,7 @@ class FunctorizedFactor : public NoiseModelFactorN<T> {
|
||||||
std::function<R(T, OptionalMatrixType)> func_; ///< functor instance
|
std::function<R(T, OptionalMatrixType)> func_; ///< functor instance
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
// Provide access to the Matrix& version of evaluateError:
|
// Provide access to the Matrix& version of evaluateError:
|
||||||
using Base::evaluateError;
|
using Base::evaluateError;
|
||||||
|
|
||||||
|
@ -168,6 +169,7 @@ class FunctorizedFactor2 : public NoiseModelFactorN<T1, T2> {
|
||||||
FunctionType func_; ///< functor instance
|
FunctionType func_; ///< functor instance
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
// Provide access to the Matrix& version of evaluateError:
|
// Provide access to the Matrix& version of evaluateError:
|
||||||
using Base::evaluateError;
|
using Base::evaluateError;
|
||||||
|
|
||||||
|
|
|
@ -46,6 +46,7 @@ class NonlinearEquality: public NoiseModelFactorN<VALUE> {
|
||||||
|
|
||||||
public:
|
public:
|
||||||
typedef VALUE T;
|
typedef VALUE T;
|
||||||
|
|
||||||
// Provide access to the Matrix& version of evaluateError:
|
// Provide access to the Matrix& version of evaluateError:
|
||||||
using NoiseModelFactor1<VALUE>::evaluateError;
|
using NoiseModelFactor1<VALUE>::evaluateError;
|
||||||
|
|
||||||
|
@ -307,6 +308,7 @@ class NonlinearEquality2 : public NoiseModelFactorN<T, T> {
|
||||||
|
|
||||||
public:
|
public:
|
||||||
typedef boost::shared_ptr<NonlinearEquality2<T>> shared_ptr;
|
typedef boost::shared_ptr<NonlinearEquality2<T>> shared_ptr;
|
||||||
|
|
||||||
// Provide access to the Matrix& version of evaluateError:
|
// Provide access to the Matrix& version of evaluateError:
|
||||||
using Base::evaluateError;
|
using Base::evaluateError;
|
||||||
|
|
||||||
|
|
|
@ -31,6 +31,7 @@ namespace gtsam {
|
||||||
|
|
||||||
public:
|
public:
|
||||||
typedef VALUE T;
|
typedef VALUE T;
|
||||||
|
|
||||||
// Provide access to the Matrix& version of evaluateError:
|
// Provide access to the Matrix& version of evaluateError:
|
||||||
using NoiseModelFactor1<VALUE>::evaluateError;
|
using NoiseModelFactor1<VALUE>::evaluateError;
|
||||||
|
|
||||||
|
|
|
@ -42,6 +42,7 @@ class GTSAM_EXPORT ShonanFactor : public NoiseModelFactorN<SOn, SOn> {
|
||||||
using Rot = typename std::conditional<d == 2, Rot2, Rot3>::type;
|
using Rot = typename std::conditional<d == 2, Rot2, Rot3>::type;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
// Provide access to the Matrix& version of evaluateError:
|
// Provide access to the Matrix& version of evaluateError:
|
||||||
using NoiseModelFactor2<SOn, SOn>::evaluateError;
|
using NoiseModelFactor2<SOn, SOn>::evaluateError;
|
||||||
|
|
||||||
|
|
|
@ -45,6 +45,7 @@ class TranslationFactor : public NoiseModelFactorN<Point3, Point3> {
|
||||||
Point3 measured_w_aZb_;
|
Point3 measured_w_aZb_;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
// Provide access to the Matrix& version of evaluateError:
|
// Provide access to the Matrix& version of evaluateError:
|
||||||
using NoiseModelFactor2<Point3, Point3>::evaluateError;
|
using NoiseModelFactor2<Point3, Point3>::evaluateError;
|
||||||
|
|
||||||
|
|
|
@ -55,6 +55,7 @@ namespace gtsam {
|
||||||
VALUE measured_; /** The measurement */
|
VALUE measured_; /** The measurement */
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
// Provide access to the Matrix& version of evaluateError:
|
// Provide access to the Matrix& version of evaluateError:
|
||||||
using Base::evaluateError;
|
using Base::evaluateError;
|
||||||
|
|
||||||
|
|
|
@ -37,6 +37,7 @@ private:
|
||||||
EssentialMatrix measuredE_; /** The measurement is an essential matrix */
|
EssentialMatrix measuredE_; /** The measurement is an essential matrix */
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
// Provide access to the Matrix& version of evaluateError:
|
// Provide access to the Matrix& version of evaluateError:
|
||||||
using Base::evaluateError;
|
using Base::evaluateError;
|
||||||
|
|
||||||
|
|
|
@ -118,6 +118,7 @@ class EssentialMatrixFactor2
|
||||||
typedef EssentialMatrixFactor2 This;
|
typedef EssentialMatrixFactor2 This;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
// Provide access to the Matrix& version of evaluateError:
|
// Provide access to the Matrix& version of evaluateError:
|
||||||
using Base::evaluateError;
|
using Base::evaluateError;
|
||||||
|
|
||||||
|
@ -240,6 +241,7 @@ class EssentialMatrixFactor3 : public EssentialMatrixFactor2 {
|
||||||
Rot3 cRb_; ///< Rotation from body to camera frame
|
Rot3 cRb_; ///< Rotation from body to camera frame
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
// Provide access to the Matrix& version of evaluateError:
|
// Provide access to the Matrix& version of evaluateError:
|
||||||
using Base::evaluateError;
|
using Base::evaluateError;
|
||||||
|
|
||||||
|
@ -343,6 +345,7 @@ class EssentialMatrixFactor4
|
||||||
typedef Eigen::Matrix<double, 2, DimK> JacobianCalibration;
|
typedef Eigen::Matrix<double, 2, DimK> JacobianCalibration;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
// Provide access to the Matrix& version of evaluateError:
|
// Provide access to the Matrix& version of evaluateError:
|
||||||
using Base::evaluateError;
|
using Base::evaluateError;
|
||||||
|
|
||||||
|
|
|
@ -114,6 +114,7 @@ class FrobeniusBetweenFactor : public NoiseModelFactorN<Rot, Rot> {
|
||||||
enum { Dim = Rot::VectorN2::RowsAtCompileTime };
|
enum { Dim = Rot::VectorN2::RowsAtCompileTime };
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
// Provide access to the Matrix& version of evaluateError:
|
// Provide access to the Matrix& version of evaluateError:
|
||||||
using NoiseModelFactor2<Rot, Rot>::evaluateError;
|
using NoiseModelFactor2<Rot, Rot>::evaluateError;
|
||||||
|
|
||||||
|
|
|
@ -21,6 +21,7 @@ class GTSAM_EXPORT OrientedPlane3Factor: public NoiseModelFactorN<Pose3, Oriente
|
||||||
typedef NoiseModelFactorN<Pose3, OrientedPlane3> Base;
|
typedef NoiseModelFactorN<Pose3, OrientedPlane3> Base;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
// Provide access to the Matrix& version of evaluateError:
|
// Provide access to the Matrix& version of evaluateError:
|
||||||
using NoiseModelFactor2<Pose3, OrientedPlane3>::evaluateError;
|
using NoiseModelFactor2<Pose3, OrientedPlane3>::evaluateError;
|
||||||
|
|
||||||
|
|
|
@ -74,6 +74,7 @@ class RotateDirectionsFactor: public NoiseModelFactorN<Rot3> {
|
||||||
typedef RotateDirectionsFactor This;
|
typedef RotateDirectionsFactor This;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
// Provide access to the Matrix& version of evaluateError:
|
// Provide access to the Matrix& version of evaluateError:
|
||||||
using Base::evaluateError;
|
using Base::evaluateError;
|
||||||
|
|
||||||
|
|
|
@ -32,6 +32,7 @@ protected:
|
||||||
double dt_; /// time between measurements
|
double dt_; /// time between measurements
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
// Provide access to the Matrix& version of evaluateError:
|
// Provide access to the Matrix& version of evaluateError:
|
||||||
using Base::evaluateError;
|
using Base::evaluateError;
|
||||||
|
|
||||||
|
|
|
@ -191,6 +191,7 @@ protected:
|
||||||
double alpha_; //! in [0,1], define the mid-point between [q_k,q_{k+1}] for approximation. The sympletic rule above can be obtained as a special case when alpha = 0.
|
double alpha_; //! in [0,1], define the mid-point between [q_k,q_{k+1}] for approximation. The sympletic rule above can be obtained as a special case when alpha = 0.
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
// Provide access to the Matrix& version of evaluateError:
|
// Provide access to the Matrix& version of evaluateError:
|
||||||
using Base::evaluateError;
|
using Base::evaluateError;
|
||||||
|
|
||||||
|
|
|
@ -29,6 +29,7 @@ class Reconstruction : public NoiseModelFactorN<Pose3, Pose3, Vector6> {
|
||||||
double h_; // time step
|
double h_; // time step
|
||||||
typedef NoiseModelFactorN<Pose3, Pose3, Vector6> Base;
|
typedef NoiseModelFactorN<Pose3, Pose3, Vector6> Base;
|
||||||
public:
|
public:
|
||||||
|
|
||||||
// Provide access to the Matrix& version of evaluateError:
|
// Provide access to the Matrix& version of evaluateError:
|
||||||
using Base::evaluateError;
|
using Base::evaluateError;
|
||||||
|
|
||||||
|
@ -91,6 +92,7 @@ class DiscreteEulerPoincareHelicopter : public NoiseModelFactorN<Vector6, Vector
|
||||||
typedef NoiseModelFactorN<Vector6, Vector6, Pose3> Base;
|
typedef NoiseModelFactorN<Vector6, Vector6, Pose3> Base;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
// Provide access to the Matrix& version of evaluateError:
|
// Provide access to the Matrix& version of evaluateError:
|
||||||
using Base::evaluateError;
|
using Base::evaluateError;
|
||||||
|
|
||||||
|
|
|
@ -35,6 +35,7 @@ typedef enum {
|
||||||
class VelocityConstraint : public gtsam::NoiseModelFactorN<PoseRTV,PoseRTV> {
|
class VelocityConstraint : public gtsam::NoiseModelFactorN<PoseRTV,PoseRTV> {
|
||||||
public:
|
public:
|
||||||
typedef gtsam::NoiseModelFactor2<PoseRTV,PoseRTV> Base;
|
typedef gtsam::NoiseModelFactor2<PoseRTV,PoseRTV> Base;
|
||||||
|
|
||||||
// Provide access to the Matrix& version of evaluateError:
|
// Provide access to the Matrix& version of evaluateError:
|
||||||
using Base::evaluateError;
|
using Base::evaluateError;
|
||||||
|
|
||||||
|
|
|
@ -37,6 +37,7 @@ namespace gtsam {
|
||||||
Point3 measured_; /** The measurement */
|
Point3 measured_; /** The measurement */
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
// Provide access to the Matrix& version of evaluateError:
|
// Provide access to the Matrix& version of evaluateError:
|
||||||
using Base::evaluateError;
|
using Base::evaluateError;
|
||||||
|
|
||||||
|
|
|
@ -53,6 +53,7 @@ private:
|
||||||
Vector tau_;
|
Vector tau_;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
// Provide access to the Matrix& version of evaluateError:
|
// Provide access to the Matrix& version of evaluateError:
|
||||||
using Base::evaluateError;
|
using Base::evaluateError;
|
||||||
|
|
||||||
|
|
|
@ -95,6 +95,7 @@ private:
|
||||||
boost::optional<POSE> body_P_sensor_; // The pose of the sensor in the body frame
|
boost::optional<POSE> body_P_sensor_; // The pose of the sensor in the body frame
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
// Provide access to the Matrix& version of evaluateError:
|
// Provide access to the Matrix& version of evaluateError:
|
||||||
using Base::evaluateError;
|
using Base::evaluateError;
|
||||||
|
|
||||||
|
|
|
@ -35,6 +35,7 @@ public:
|
||||||
|
|
||||||
/// shorthand for base class type
|
/// shorthand for base class type
|
||||||
typedef NoiseModelFactor3<POSE, LANDMARK, INVDEPTH> Base;
|
typedef NoiseModelFactor3<POSE, LANDMARK, INVDEPTH> Base;
|
||||||
|
|
||||||
// Provide access to the Matrix& version of evaluateError:
|
// Provide access to the Matrix& version of evaluateError:
|
||||||
using Base::evaluateError;
|
using Base::evaluateError;
|
||||||
|
|
||||||
|
|
|
@ -35,6 +35,7 @@ public:
|
||||||
|
|
||||||
/// shorthand for base class type
|
/// shorthand for base class type
|
||||||
typedef NoiseModelFactor2<Pose3, Vector6> Base;
|
typedef NoiseModelFactor2<Pose3, Vector6> Base;
|
||||||
|
|
||||||
// Provide access to the Matrix& version of evaluateError:
|
// Provide access to the Matrix& version of evaluateError:
|
||||||
using Base::evaluateError;
|
using Base::evaluateError;
|
||||||
|
|
||||||
|
|
|
@ -37,6 +37,7 @@ public:
|
||||||
|
|
||||||
/// shorthand for base class type
|
/// shorthand for base class type
|
||||||
typedef NoiseModelFactor2<Pose3, Vector3> Base;
|
typedef NoiseModelFactor2<Pose3, Vector3> Base;
|
||||||
|
|
||||||
// Provide access to the Matrix& version of evaluateError:
|
// Provide access to the Matrix& version of evaluateError:
|
||||||
using Base::evaluateError;
|
using Base::evaluateError;
|
||||||
|
|
||||||
|
|
|
@ -35,6 +35,7 @@ public:
|
||||||
|
|
||||||
/// shorthand for base class type
|
/// shorthand for base class type
|
||||||
typedef NoiseModelFactor2<Pose3, Vector3> Base;
|
typedef NoiseModelFactor2<Pose3, Vector3> Base;
|
||||||
|
|
||||||
// Provide access to the Matrix& version of evaluateError:
|
// Provide access to the Matrix& version of evaluateError:
|
||||||
using Base::evaluateError;
|
using Base::evaluateError;
|
||||||
|
|
||||||
|
|
|
@ -40,6 +40,7 @@ class GTSAM_UNSTABLE_EXPORT LocalOrientedPlane3Factor
|
||||||
OrientedPlane3 measured_p_;
|
OrientedPlane3 measured_p_;
|
||||||
typedef NoiseModelFactorN<Pose3, Pose3, OrientedPlane3> Base;
|
typedef NoiseModelFactorN<Pose3, Pose3, OrientedPlane3> Base;
|
||||||
public:
|
public:
|
||||||
|
|
||||||
// Provide access to the Matrix& version of evaluateError:
|
// Provide access to the Matrix& version of evaluateError:
|
||||||
using Base::evaluateError;
|
using Base::evaluateError;
|
||||||
|
|
||||||
|
|
|
@ -61,6 +61,7 @@ namespace gtsam {
|
||||||
: Base(model, key) {}
|
: Base(model, key) {}
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
// Provide access to the Matrix& version of evaluateError:
|
// Provide access to the Matrix& version of evaluateError:
|
||||||
using Base::evaluateError;
|
using Base::evaluateError;
|
||||||
|
|
||||||
|
|
|
@ -43,6 +43,7 @@ namespace gtsam {
|
||||||
GTSAM_CONCEPT_TESTABLE_TYPE(POSE)
|
GTSAM_CONCEPT_TESTABLE_TYPE(POSE)
|
||||||
GTSAM_CONCEPT_POSE_TYPE(POSE)
|
GTSAM_CONCEPT_POSE_TYPE(POSE)
|
||||||
public:
|
public:
|
||||||
|
|
||||||
// Provide access to the Matrix& version of evaluateError:
|
// Provide access to the Matrix& version of evaluateError:
|
||||||
using Base::evaluateError;
|
using Base::evaluateError;
|
||||||
|
|
||||||
|
|
|
@ -40,6 +40,7 @@ namespace gtsam {
|
||||||
GTSAM_CONCEPT_TESTABLE_TYPE(POSE)
|
GTSAM_CONCEPT_TESTABLE_TYPE(POSE)
|
||||||
GTSAM_CONCEPT_POSE_TYPE(POSE)
|
GTSAM_CONCEPT_POSE_TYPE(POSE)
|
||||||
public:
|
public:
|
||||||
|
|
||||||
// Provide access to the Matrix& version of evaluateError:
|
// Provide access to the Matrix& version of evaluateError:
|
||||||
using Base::evaluateError;
|
using Base::evaluateError;
|
||||||
|
|
||||||
|
|
|
@ -29,6 +29,7 @@ class PoseToPointFactor : public NoiseModelFactorN<POSE, POINT> {
|
||||||
POINT measured_; /** the point measurement in local coordinates */
|
POINT measured_; /** the point measurement in local coordinates */
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
// Provide access to the Matrix& version of evaluateError:
|
// Provide access to the Matrix& version of evaluateError:
|
||||||
using Base::evaluateError;
|
using Base::evaluateError;
|
||||||
|
|
||||||
|
|
|
@ -46,6 +46,7 @@ namespace gtsam {
|
||||||
|
|
||||||
/// shorthand for base class type
|
/// shorthand for base class type
|
||||||
typedef NoiseModelFactor3<POSE, POSE, LANDMARK> Base;
|
typedef NoiseModelFactor3<POSE, POSE, LANDMARK> Base;
|
||||||
|
|
||||||
// Provide access to the Matrix& version of evaluateError:
|
// Provide access to the Matrix& version of evaluateError:
|
||||||
using Base::evaluateError;
|
using Base::evaluateError;
|
||||||
|
|
||||||
|
|
|
@ -45,6 +45,7 @@ class GTSAM_UNSTABLE_EXPORT ProjectionFactorPPPC
|
||||||
public:
|
public:
|
||||||
/// shorthand for base class type
|
/// shorthand for base class type
|
||||||
typedef NoiseModelFactor4<POSE, POSE, LANDMARK, CALIBRATION> Base;
|
typedef NoiseModelFactor4<POSE, POSE, LANDMARK, CALIBRATION> Base;
|
||||||
|
|
||||||
// Provide access to the Matrix& version of evaluateError:
|
// Provide access to the Matrix& version of evaluateError:
|
||||||
using Base::evaluateError;
|
using Base::evaluateError;
|
||||||
|
|
||||||
|
|
|
@ -61,6 +61,7 @@ class GTSAM_UNSTABLE_EXPORT ProjectionFactorRollingShutter
|
||||||
public:
|
public:
|
||||||
/// shorthand for base class type
|
/// shorthand for base class type
|
||||||
typedef NoiseModelFactor3<Pose3, Pose3, Point3> Base;
|
typedef NoiseModelFactor3<Pose3, Pose3, Point3> Base;
|
||||||
|
|
||||||
// Provide access to the Matrix& version of evaluateError:
|
// Provide access to the Matrix& version of evaluateError:
|
||||||
using Base::evaluateError;
|
using Base::evaluateError;
|
||||||
|
|
||||||
|
|
|
@ -34,6 +34,7 @@ private:
|
||||||
typedef NoiseModelFactorN<Pose3, Point3> Base;
|
typedef NoiseModelFactorN<Pose3, Point3> Base;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
// Provide access to the Matrix& version of evaluateError:
|
// Provide access to the Matrix& version of evaluateError:
|
||||||
using Base::evaluateError;
|
using Base::evaluateError;
|
||||||
|
|
||||||
|
|
|
@ -42,6 +42,7 @@ class SmartRangeFactor: public NoiseModelFactor {
|
||||||
double variance_; ///< variance on noise
|
double variance_; ///< variance on noise
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
// Provide access to the Matrix& version of unwhitenedError
|
// Provide access to the Matrix& version of unwhitenedError
|
||||||
using NoiseModelFactor::unwhitenedError;
|
using NoiseModelFactor::unwhitenedError;
|
||||||
|
|
||||||
|
|
|
@ -37,6 +37,7 @@ private:
|
||||||
Point2 measured_; ///< the measurement
|
Point2 measured_; ///< the measurement
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
// Provide access to the Matrix& version of evaluateError:
|
// Provide access to the Matrix& version of evaluateError:
|
||||||
using Base::evaluateError;
|
using Base::evaluateError;
|
||||||
|
|
||||||
|
@ -67,6 +68,7 @@ private:
|
||||||
Point2 measured_; ///< the measurement
|
Point2 measured_; ///< the measurement
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
// Provide access to the Matrix& version of evaluateError:
|
// Provide access to the Matrix& version of evaluateError:
|
||||||
using Base::evaluateError;
|
using Base::evaluateError;
|
||||||
|
|
||||||
|
@ -122,6 +124,7 @@ private:
|
||||||
Pose2 measured_; ///< the measurement
|
Pose2 measured_; ///< the measurement
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
// Provide access to the Matrix& version of evaluateError:
|
// Provide access to the Matrix& version of evaluateError:
|
||||||
using Base::evaluateError;
|
using Base::evaluateError;
|
||||||
|
|
||||||
|
|
|
@ -108,7 +108,7 @@ namespace simulated2DOriented {
|
||||||
typedef GenericOdometry<VALUE> This;
|
typedef GenericOdometry<VALUE> This;
|
||||||
|
|
||||||
// Provide access to the Matrix& version of evaluateError:
|
// Provide access to the Matrix& version of evaluateError:
|
||||||
using NoiseModelFactor2<VALUE, VALUE>::evaluateError;
|
using NoiseModelFactor2<VALUE, VALUE>::evaluateError;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Creates an odometry factor between two poses
|
* Creates an odometry factor between two poses
|
||||||
|
|
|
@ -101,6 +101,7 @@ protected:
|
||||||
Matrix Q_invsqrt_;
|
Matrix Q_invsqrt_;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
// Provide access to the Matrix& version of evaluateError:
|
// Provide access to the Matrix& version of evaluateError:
|
||||||
using Base::evaluateError;
|
using Base::evaluateError;
|
||||||
|
|
||||||
|
@ -247,6 +248,7 @@ protected:
|
||||||
Matrix R_invsqrt_; /** The inv sqrt of the measurement error covariance */
|
Matrix R_invsqrt_; /** The inv sqrt of the measurement error covariance */
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
// Provide access to the Matrix& version of evaluateError:
|
// Provide access to the Matrix& version of evaluateError:
|
||||||
using Base::evaluateError;
|
using Base::evaluateError;
|
||||||
|
|
||||||
|
|
|
@ -337,10 +337,12 @@ class TestFactor1 : public NoiseModelFactor1<double> {
|
||||||
|
|
||||||
public:
|
public:
|
||||||
typedef NoiseModelFactor1<double> Base;
|
typedef NoiseModelFactor1<double> Base;
|
||||||
|
|
||||||
// Provide access to the Matrix& version of evaluateError:
|
// Provide access to the Matrix& version of evaluateError:
|
||||||
using Base::evaluateError;
|
using Base::evaluateError;
|
||||||
|
|
||||||
TestFactor1() : Base(noiseModel::Diagonal::Sigmas(Vector1(2.0)), L(1)) {}
|
TestFactor1() : Base(noiseModel::Diagonal::Sigmas(Vector1(2.0)), L(1)) {}
|
||||||
|
|
||||||
// Provide access to the Matrix& version of evaluateError:
|
// Provide access to the Matrix& version of evaluateError:
|
||||||
using Base::NoiseModelFactor1; // inherit constructors
|
using Base::NoiseModelFactor1; // inherit constructors
|
||||||
|
|
||||||
|
@ -392,10 +394,12 @@ class TestFactor4 : public NoiseModelFactor4<double, double, double, double> {
|
||||||
|
|
||||||
public:
|
public:
|
||||||
typedef NoiseModelFactor4<double, double, double, double> Base;
|
typedef NoiseModelFactor4<double, double, double, double> Base;
|
||||||
|
|
||||||
// Provide access to the Matrix& version of evaluateError:
|
// Provide access to the Matrix& version of evaluateError:
|
||||||
using Base::evaluateError;
|
using Base::evaluateError;
|
||||||
|
|
||||||
TestFactor4() : Base(noiseModel::Diagonal::Sigmas((Vector(1) << 2.0).finished()), X(1), X(2), X(3), X(4)) {}
|
TestFactor4() : Base(noiseModel::Diagonal::Sigmas((Vector(1) << 2.0).finished()), X(1), X(2), X(3), X(4)) {}
|
||||||
|
|
||||||
// Provide access to the Matrix& version of evaluateError:
|
// Provide access to the Matrix& version of evaluateError:
|
||||||
using Base::NoiseModelFactor4; // inherit constructors
|
using Base::NoiseModelFactor4; // inherit constructors
|
||||||
|
|
||||||
|
@ -485,6 +489,7 @@ TEST(NonlinearFactor, NoiseModelFactor4) {
|
||||||
class TestFactor5 : public NoiseModelFactor5<double, double, double, double, double> {
|
class TestFactor5 : public NoiseModelFactor5<double, double, double, double, double> {
|
||||||
public:
|
public:
|
||||||
typedef NoiseModelFactor5<double, double, double, double, double> Base;
|
typedef NoiseModelFactor5<double, double, double, double, double> Base;
|
||||||
|
|
||||||
// Provide access to the Matrix& version of evaluateError:
|
// Provide access to the Matrix& version of evaluateError:
|
||||||
using Base::evaluateError;
|
using Base::evaluateError;
|
||||||
|
|
||||||
|
@ -535,6 +540,7 @@ TEST(NonlinearFactor, NoiseModelFactor5) {
|
||||||
class TestFactor6 : public NoiseModelFactor6<double, double, double, double, double, double> {
|
class TestFactor6 : public NoiseModelFactor6<double, double, double, double, double, double> {
|
||||||
public:
|
public:
|
||||||
typedef NoiseModelFactor6<double, double, double, double, double, double> Base;
|
typedef NoiseModelFactor6<double, double, double, double, double, double> Base;
|
||||||
|
|
||||||
// Provide access to the Matrix& version of evaluateError:
|
// Provide access to the Matrix& version of evaluateError:
|
||||||
using Base::evaluateError;
|
using Base::evaluateError;
|
||||||
|
|
||||||
|
@ -592,6 +598,7 @@ TEST(NonlinearFactor, NoiseModelFactor6) {
|
||||||
class TestFactorN : public NoiseModelFactorN<double, double, double, double> {
|
class TestFactorN : public NoiseModelFactorN<double, double, double, double> {
|
||||||
public:
|
public:
|
||||||
typedef NoiseModelFactorN<double, double, double, double> Base;
|
typedef NoiseModelFactorN<double, double, double, double> Base;
|
||||||
|
|
||||||
// Provide access to the Matrix& version of evaluateError:
|
// Provide access to the Matrix& version of evaluateError:
|
||||||
using Base::evaluateError;
|
using Base::evaluateError;
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue