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

changed signatures to use OptionalMatrix keyword

release/4.3a0
kartik arcot 2023-01-05 14:29:17 -08:00
parent c1fe89cc0d
commit 841dc6005a
50 changed files with 172 additions and 167 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
CMakeLists.txt
View File

@ -98,7 +98,7 @@ add_subdirectory(timing)
# Build gtsam_unstable
if (GTSAM_BUILD_UNSTABLE)
add_subdirectory(gtsam_unstable)
# add_subdirectory(gtsam_unstable)
endif()
# This is the new wrapper

2
doc/Code/LocalizationFactor.cpp
View File

@ -6,7 +6,7 @@ public:
NoiseModelFactor1<Pose2>(model, j), mx_(x), my_(y) {}
Vector evaluateError(const Pose2& q,
boost::optional<Matrix&> H = boost::none) const override {
OptionalMatrixType H = OptionalNone) const override {
const Rot2& R = q.rotation();
if (H) (*H) = (gtsam::Matrix(2, 3) <<
R.c(), -R.s(), 0.0,

6
examples/CameraResectioning.cpp
View File

@ -46,10 +46,10 @@ public:
}
/// evaluate the error
Vector evaluateError(const Pose3& pose, boost::optional<Matrix&> H =
boost::none) const override {
Vector evaluateError(const Pose3& pose, OptionalMatrixType H =
OptionalNone) const override {
PinholeCamera<Cal3_S2> camera(pose, *K_);
return camera.project(P_, H, boost::none, boost::none) - p_;
return camera.project(P_, H, OptionalNone, OptionalNone) - p_;
}
};

2
examples/LocalizationExample.cpp
View File

@ -84,7 +84,7 @@ class UnaryFactor: public NoiseModelFactorN<Pose2> {
// The first is the 'evaluateError' function. This function implements the desired measurement
// function, returning a vector of errors when evaluated at the provided variable value. It
// must also calculate the Jacobians for this measurement function, if requested.
Vector evaluateError(const Pose2& q, boost::optional<Matrix&> H = boost::none) const override {
Vector evaluateError(const Pose2& q, OptionalMatrixType H = OptionalNone) const override {
// The measurement function for a GPS-like measurement h(q) which predicts the measurement (m) is h(q) = q, q = [qx qy qtheta]
// The error is then simply calculated as E(q) = h(q) - m:
// error_x = q.x - mx

4
gtsam/navigation/AHRSFactor.cpp
View File

@ -119,8 +119,8 @@ bool AHRSFactor::equals(const NonlinearFactor& other, double tol) const {
//------------------------------------------------------------------------------
Vector AHRSFactor::evaluateError(const Rot3& Ri, const Rot3& Rj,
const Vector3& bias, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2, boost::optional<Matrix&> H3) const {
const Vector3& bias, OptionalMatrixType H1,
OptionalMatrixType H2, OptionalMatrixType H3) const {
// Do bias correction, if (H3) will contain 3*3 derivative used below
const Vector3 biascorrectedOmega = _PIM_.predict(bias, H3);

6
gtsam/navigation/AHRSFactor.h
View File

@ -179,9 +179,9 @@ public:
/// vector of errors
Vector evaluateError(const Rot3& rot_i, const Rot3& rot_j,
const Vector3& bias, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H3 =
boost::none) const override;
const Vector3& bias, OptionalMatrixType H1 = OptionalNone,
OptionalMatrixType H2 = OptionalNone, OptionalMatrixType H3 =
OptionalNone) const override;
/// predicted states from IMU
/// TODO(frank): relationship with PIM predict ??

4
gtsam/navigation/AttitudeFactor.h
View File

@ -122,7 +122,7 @@ public:
/** vector of errors */
Vector evaluateError(const Rot3& nRb, //
boost::optional<Matrix&> H = boost::none) const override {
OptionalMatrixType H = OptionalNone) const override {
return attitudeError(nRb, H);
}
@ -197,7 +197,7 @@ public:
/** vector of errors */
Vector evaluateError(const Pose3& nTb, //
boost::optional<Matrix&> H = boost::none) const override {
OptionalMatrixType H = OptionalNone) const override {
Vector e = attitudeError(nTb.rotation(), H);
if (H) {
Matrix H23 = *H;

4
gtsam/navigation/BarometricFactor.cpp
View File

@ -43,8 +43,8 @@ bool BarometricFactor::equals(const NonlinearFactor& expected,
//***************************************************************************
Vector BarometricFactor::evaluateError(const Pose3& p, const double& bias,
boost::optional<Matrix&> H,
boost::optional<Matrix&> H2) const {
OptionalMatrixType H,
OptionalMatrixType H2) const {
Matrix tH;
Vector ret = (Vector(1) << (p.translation(tH).z() + bias - nT_)).finished();
if (H) (*H) = tH.block<1, 6>(2, 0);

4
gtsam/navigation/BarometricFactor.h
View File

@ -78,8 +78,8 @@ class GTSAM_EXPORT BarometricFactor : public NoiseModelFactorN<Pose3, double> {
/// vector of errors
Vector evaluateError(
const Pose3& p, const double& b,
boost::optional<Matrix&> H = boost::none,
boost::optional<Matrix&> H2 = boost::none) const override;
OptionalMatrixType H = OptionalNone,
OptionalMatrixType H2 = OptionalNone) const override;
inline const double& measurementIn() const { return nT_; }

10
gtsam/navigation/CombinedImuFactor.h
View File

@ -324,10 +324,12 @@ public:
Vector 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::none, boost::optional<Matrix&> H2 =
boost::none, boost::optional<Matrix&> H3 = boost::none,
boost::optional<Matrix&> H4 = boost::none, boost::optional<Matrix&> H5 =
boost::none, boost::optional<Matrix&> H6 = boost::none) const override;
OptionalMatrixType H1 = OptionalNone,
OptionalMatrixType H2 = OptionalNone,
OptionalMatrixType H3 = OptionalNone,
OptionalMatrixType H4 = OptionalNone,
OptionalMatrixType H5 = OptionalNone,
OptionalMatrixType H6 = OptionalNone) const override;
private:
/** Serialization function */

4
gtsam/navigation/ConstantVelocityFactor.h
View File

@ -48,8 +48,8 @@ class ConstantVelocityFactor : public NoiseModelFactorN<NavState, NavState> {
* @return * Vector
*/
gtsam::Vector evaluateError(const NavState &x1, const NavState &x2,
boost::optional<gtsam::Matrix &> H1 = boost::none,
boost::optional<gtsam::Matrix &> H2 = boost::none) const override {
OptionalMatrixType H1 = OptionalNone,
OptionalMatrixType H2 = OptionalNone) 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};

2
gtsam/navigation/GPSFactor.cpp
View File

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

2
gtsam/navigation/GPSFactor.h
View File

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

10
gtsam/navigation/ImuFactor.cpp
View File

@ -151,9 +151,9 @@ bool ImuFactor::equals(const NonlinearFactor& other, double tol) const {
//------------------------------------------------------------------------------
Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i,
const Pose3& pose_j, const Vector3& vel_j,
const imuBias::ConstantBias& bias_i, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2, boost::optional<Matrix&> H3,
boost::optional<Matrix&> H4, boost::optional<Matrix&> H5) const {
const imuBias::ConstantBias& bias_i, OptionalMatrixType H1,
OptionalMatrixType H2, OptionalMatrixType H3,
OptionalMatrixType H4, OptionalMatrixType H5) const {
return _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, bias_i,
H1, H2, H3, H4, H5);
}
@ -247,8 +247,8 @@ bool ImuFactor2::equals(const NonlinearFactor& other, double tol) const {
Vector ImuFactor2::evaluateError(const NavState& state_i,
const NavState& state_j,
const imuBias::ConstantBias& bias_i, //
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2,
boost::optional<Matrix&> H3) const {
OptionalMatrixType H1, OptionalMatrixType H2,
OptionalMatrixType H3) const {
return _PIM_.computeError(state_i, state_j, bias_i, H1, H2, H3);
}

14
gtsam/navigation/ImuFactor.h
View File

@ -228,10 +228,10 @@ public:
/// vector of errors
Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i,
const Pose3& pose_j, const Vector3& vel_j,
const imuBias::ConstantBias& bias_i, boost::optional<Matrix&> H1 =
boost::none, boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none, boost::optional<Matrix&> H4 =
boost::none, boost::optional<Matrix&> H5 = boost::none) const override;
const imuBias::ConstantBias& bias_i, OptionalMatrixType H1 =
OptionalNone, OptionalMatrixType H2 = OptionalNone,
OptionalMatrixType H3 = OptionalNone, OptionalMatrixType H4 =
OptionalNone, OptionalMatrixType H5 = OptionalNone) const override;
#ifdef GTSAM_TANGENT_PREINTEGRATION
/// Merge two pre-integrated measurement classes
@ -307,9 +307,9 @@ public:
/// vector of errors
Vector evaluateError(const NavState& state_i, const NavState& state_j,
const imuBias::ConstantBias& bias_i, //
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const override;
OptionalMatrixType H1 = OptionalNone,
OptionalMatrixType H2 = OptionalNone,
OptionalMatrixType H3 = OptionalNone) const override;
private:

18
gtsam/navigation/MagFactor.h
View File

@ -75,7 +75,7 @@ public:
* @brief vector of errors
*/
Vector evaluateError(const Rot2& nRb,
boost::optional<Matrix&> H = boost::none) const override {
OptionalMatrixType H = OptionalNone) const override {
// measured bM = nRb<52> * nM + b
Point3 hx = unrotate(nRb, nM_, H) + bias_;
return (hx - measured_);
@ -113,7 +113,7 @@ public:
* @brief vector of errors
*/
Vector evaluateError(const Rot3& nRb,
boost::optional<Matrix&> H = boost::none) const override {
OptionalMatrixType H = OptionalNone) const override {
// measured bM = nRb<52> * nM + b
Point3 hx = nRb.unrotate(nM_, H, boost::none) + bias_;
return (hx - measured_);
@ -151,10 +151,10 @@ public:
* @param bias (unknown) 3D bias
*/
Vector evaluateError(const Point3& nM, const Point3& bias,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
boost::none) const override {
OptionalMatrixType H1 = OptionalNone, OptionalMatrixType H2 =
OptionalNone) const override {
// measured bM = nRb<52> * nM + b, where b is unknown bias
Point3 hx = bRn_.rotate(nM, boost::none, H1) + bias;
Point3 hx = bRn_.rotate(nM, OptionalNone, H1) + bias;
if (H2)
*H2 = I_3x3;
return (hx - measured_);
@ -192,11 +192,11 @@ public:
* @param bias (unknown) 3D bias
*/
Vector evaluateError(const double& scale, const Unit3& direction,
const Point3& bias, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H3 =
boost::none) const override {
const Point3& bias, OptionalMatrixType H1 = OptionalNone,
OptionalMatrixType H2 = OptionalNone, OptionalMatrixType H3 =
OptionalNone) const override {
// measured bM = nRb<52> * nM + b, where b is unknown bias
Unit3 rotated = bRn_.rotate(direction, boost::none, H2);
Unit3 rotated = bRn_.rotate(direction, OptionalNone, H2);
Point3 hx = scale * rotated.point3() + bias;
if (H1)
*H1 = rotated.point3();

4
gtsam/navigation/MagPoseFactor.h
View File

@ -108,11 +108,11 @@ class MagPoseFactor: public NoiseModelFactorN<POSE> {
* Return the factor's error h(x) - z, and the optional Jacobian. Note that
* the measurement error is expressed in the body frame.
*/
Vector evaluateError(const POSE& nPb, boost::optional<Matrix&> H = boost::none) const override {
Vector evaluateError(const POSE& nPb, OptionalMatrixType H = OptionalNone) const override {
// Predict the measured magnetic field h(x) in the *body* frame.
// If body_P_sensor was given, bias_ will have been rotated into the body frame.
Matrix H_rot = Matrix::Zero(MeasDim, RotDim);
const Point hx = nPb.rotation().unrotate(nM_, H_rot, boost::none) + bias_;
const Point hx = nPb.rotation().unrotate(nM_, H_rot, OptionalNone) + bias_;
if (H) {
// Fill in the relevant part of the Jacobian (just rotation columns).

2
gtsam/nonlinear/CustomFactor.cpp
View File

@ -22,7 +22,7 @@ namespace gtsam {
/*
* Calculates the unwhitened error by invoking the callback functor (i.e. from Python).
*/
Vector CustomFactor::unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H) const {
Vector CustomFactor::unwhitenedError(const Values& x, OptionalMatrixVecType H) const {
if(this->active(x)) {
if(H) {

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, boost::optional<std::vector<Matrix> &> H = boost::none) const override;
Vector unwhitenedError(const Values &x, OptionalMatrixVecType H = boost::none) const override;
/** print */
void print(const std::string &s,

6
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,
boost::optional<std::vector<Matrix>&> H = boost::none) const override {
OptionalMatrixVecType H = OptionalNone) 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
@ -312,8 +312,8 @@ public:
/// Backwards compatible evaluateError, to make existing tests compile
Vector evaluateError(const A1 &a1, const A2 &a2,
boost::optional<Matrix &> H1 = boost::none,
boost::optional<Matrix &> H2 = boost::none) const {
OptionalMatrixType H1 = OptionalNone,
OptionalMatrixType H2 = OptionalNone) const {
Values values;
values.insert(this->keys_[0], a1);
values.insert(this->keys_[1], a2);

15
gtsam/nonlinear/FunctorizedFactor.h
View File

@ -62,7 +62,7 @@ class FunctorizedFactor : public NoiseModelFactorN<T> {
R measured_; ///< value that is compared with functor return value
SharedNoiseModel noiseModel_; ///< noise model
std::function<R(T, boost::optional<Matrix &>)> func_; ///< functor instance
std::function<R(T, OptionalMatrixType)> func_; ///< functor instance
public:
/** default constructor - only use for serialization */
@ -76,7 +76,7 @@ class FunctorizedFactor : public NoiseModelFactorN<T> {
* @param func: The instance of the functor object
*/
FunctorizedFactor(Key key, const R &z, const SharedNoiseModel &model,
const std::function<R(T, boost::optional<Matrix &>)> func)
const std::function<R(T, OptionalMatrixType)> func)
: Base(model, key), measured_(z), noiseModel_(model), func_(func) {}
~FunctorizedFactor() override {}
@ -87,8 +87,8 @@ class FunctorizedFactor : public NoiseModelFactorN<T> {
NonlinearFactor::shared_ptr(new FunctorizedFactor<R, T>(*this)));
}
Vector evaluateError(const T &params, boost::optional<Matrix &> H =
boost::none) const override {
Vector evaluateError(const T &params, OptionalMatrixType H =
OptionalNone) const override {
R x = func_(params, H);
Vector error = traits<R>::Local(measured_, x);
return error;
@ -162,8 +162,7 @@ class FunctorizedFactor2 : public NoiseModelFactorN<T1, T2> {
R measured_; ///< value that is compared with functor return value
SharedNoiseModel noiseModel_; ///< noise model
using FunctionType = std::function<R(T1, T2, boost::optional<Matrix &>,
boost::optional<Matrix &>)>;
using FunctionType = std::function<R(T1, T2, OptionalMatrixType, OptionalMatrixType)>;
FunctionType func_; ///< functor instance
public:
@ -194,8 +193,8 @@ class FunctorizedFactor2 : public NoiseModelFactorN<T1, T2> {
Vector evaluateError(
const T1 &params1, const T2 &params2,
boost::optional<Matrix &> H1 = boost::none,
boost::optional<Matrix &> H2 = boost::none) const override {
OptionalMatrixType H1 = OptionalNone,
OptionalMatrixType H2 = OptionalNone) const override {
R x = func_(params1, params2, H1, H2);
Vector error = traits<R>::Local(measured_, x);
return error;

8
gtsam/nonlinear/NonlinearEquality.h
View File

@ -139,7 +139,7 @@ public:
/// Error function
Vector evaluateError(const T& xj,
boost::optional<Matrix&> H = boost::none) const override {
OptionalMatrixType H = OptionalNone) const override {
const size_t nj = traits<T>::GetDimension(feasible_);
if (allow_error_) {
if (H)
@ -249,7 +249,7 @@ public:
/// g(x) with optional derivative
Vector evaluateError(const X& x1,
boost::optional<Matrix&> H = boost::none) const override {
OptionalMatrixType H = OptionalNone) const override {
if (H)
(*H) = Matrix::Identity(traits<X>::GetDimension(x1),traits<X>::GetDimension(x1));
// manifold equivalent of h(x)-z -> log(z,h(x))
@ -324,8 +324,8 @@ class NonlinearEquality2 : public NoiseModelFactorN<T, T> {
/// g(x) with optional derivative2
Vector evaluateError(
const T& x1, const T& x2, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const override {
const T& x1, const T& x2, OptionalMatrixType H1 = OptionalNone,
OptionalMatrixType H2 = OptionalNone) const override {
static const size_t p = traits<T>::dimension;
if (H1) *H1 = -Matrix::Identity(p, p);
if (H2) *H2 = Matrix::Identity(p, p);

10
gtsam/nonlinear/NonlinearFactor.h
View File

@ -47,7 +47,7 @@ using OptionalMatrixTypeT = Matrix*;
using OptionalMatrixType = Matrix*;
// These typedefs and aliases will help with making the unwhitenedError interface
// independent of boost
using OptionalMatrixVec = std::vector<Matrix>*;
using OptionalMatrixVecType = std::vector<Matrix>*;
#else
// creating a none value to use when declaring our interfaces
using OptionalNoneType = boost::none_t;
@ -55,7 +55,7 @@ using OptionalNoneType = boost::none_t;
template <typename T = void>
using OptionalMatrixTypeT = boost::optional<Matrix&>;
using OptionalMatrixType = boost::optional<Matrix&>;
using OptionalMatrixVec = boost::optional<std::vector<Matrix>&>;
using OptionalMatrixVecType = boost::optional<std::vector<Matrix>&>;
#endif
/**
* Nonlinear factor base class
@ -252,7 +252,7 @@ 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, OptionalMatrixVec H = OptionalNone) const = 0;
virtual Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = OptionalNone) const = 0;
#ifdef NO_BOOST_C17
// support taking in the actual vector instead of the pointer as well
Vector unwhitenedError(const Values& x, std::vector<Matrix>& H) {
@ -565,7 +565,7 @@ protected:
*/
Vector unwhitenedError(
const Values& x,
OptionalMatrixVec H = OptionalNone) const override {
OptionalMatrixVecType H = OptionalNone) const override {
return unwhitenedError(boost::mp11::index_sequence_for<ValueTypes...>{}, x,
H);
}
@ -659,7 +659,7 @@ protected:
inline Vector unwhitenedError(
boost::mp11::index_sequence<Indices...>, //
const Values& x,
OptionalMatrixVec H = OptionalNone) const {
OptionalMatrixVecType H = OptionalNone) const {
if (this->active(x)) {
if (H) {
return evaluateError(x.at<ValueTypes>(keys_[Indices])...,

2
gtsam/nonlinear/PriorFactor.h
View File

@ -91,7 +91,7 @@ namespace gtsam {
/** implement functions needed to derive from Factor */
/** vector of errors */
Vector evaluateError(const T& x, boost::optional<Matrix&> H = boost::none) const override {
Vector evaluateError(const T& x, OptionalMatrixType H = OptionalNone) const override {
if (H) (*H) = Matrix::Identity(traits<T>::GetDimension(x),traits<T>::GetDimension(x));
// manifold equivalent of z-x -> Local(x,z)
return -traits<T>::Local(x, prior_);

4
gtsam/sam/BearingFactor.h
View File

@ -62,8 +62,8 @@ struct BearingFactor : public ExpressionFactorN<T, A1, A2> {
}
Vector evaluateError(const A1& a1, const A2& a2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const
OptionalMatrixType H1 = OptionalNone,
OptionalMatrixType H2 = OptionalNone) const
{
std::vector<Matrix> Hs(2);
const auto &keys = Factor::keys();

4
gtsam/sam/BearingRangeFactor.h
View File

@ -74,8 +74,8 @@ class BearingRangeFactor
}
Vector evaluateError(const A1& a1, const A2& a2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const
OptionalMatrixType H1 = OptionalNone,
OptionalMatrixType H2 = OptionalNone) const
{
std::vector<Matrix> Hs(2);
const auto &keys = Factor::keys();

8
gtsam/sam/RangeFactor.h
View File

@ -60,8 +60,8 @@ class RangeFactor : public ExpressionFactorN<T, A1, A2> {
}
Vector evaluateError(const A1& a1, const A2& a2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const
OptionalMatrixType H1 = OptionalNone,
OptionalMatrixType H2 = OptionalNone) const
{
std::vector<Matrix> Hs(2);
const auto &keys = Factor::keys();
@ -137,8 +137,8 @@ class RangeFactorWithTransform : public ExpressionFactorN<T, A1, A2> {
}
Vector evaluateError(const A1& a1, const A2& a2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const
OptionalMatrixType H1 = OptionalNone,
OptionalMatrixType H2 = OptionalNone) const
{
std::vector<Matrix> Hs(2);
const auto &keys = Factor::keys();

4
gtsam/sfm/TranslationFactor.h
View File

@ -65,8 +65,8 @@ class TranslationFactor : public NoiseModelFactorN<Point3, Point3> {
*/
Vector evaluateError(
const Point3& Ta, const Point3& Tb,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const override {
OptionalMatrixType H1 = OptionalNone,
OptionalMatrixType H2 = OptionalNone) const override {
const Point3 dir = Tb - Ta;
Matrix33 H_predicted_dir;
const Point3 predicted = normalize(dir, H1 || H2 ? &H_predicted_dir : nullptr);

6
gtsam/slam/BetweenFactor.h
View File

@ -105,13 +105,13 @@ namespace gtsam {
/// @{
/// evaluate error, returns vector of errors size of tangent space
Vector evaluateError(const T& p1, const T& p2, boost::optional<Matrix&> H1 =
boost::none, boost::optional<Matrix&> H2 = boost::none) const override {
Vector evaluateError(const T& p1, const T& p2, OptionalMatrixType H1 =
OptionalNone, OptionalMatrixType H2 = OptionalNone) const override {
T hx = traits<T>::Between(p1, p2, H1, H2); // h(x)
// manifold equivalent of h(x)-z -> log(z,h(x))
#ifdef GTSAM_SLOW_BUT_CORRECT_BETWEENFACTOR
typename traits<T>::ChartJacobian::Jacobian Hlocal;
Vector rval = traits<T>::Local(measured_, hx, boost::none, (H1 || H2) ? &Hlocal : 0);
Vector rval = traits<T>::Local(measured_, hx, OptionalNone, (H1 || H2) ? &Hlocal : 0);
if (H1) *H1 = Hlocal * (*H1);
if (H2) *H2 = Hlocal * (*H2);
return rval;

8
gtsam/slam/BoundingConstraint.h
View File

@ -64,8 +64,8 @@ struct BoundingConstraint1: public NoiseModelFactorN<VALUE> {
return (isGreaterThan_) ? x <= threshold_ : x >= threshold_;
}
Vector evaluateError(const X& x, boost::optional<Matrix&> H =
boost::none) const override {
Vector evaluateError(const X& x, OptionalMatrixType H =
OptionalNone) const override {
Matrix D;
double error = value(x, D) - threshold_;
if (H) {
@ -134,8 +134,8 @@ struct BoundingConstraint2: public NoiseModelFactorN<VALUE1, VALUE2> {
}
Vector evaluateError(const X1& x1, const X2& x2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const override {
OptionalMatrixType H1 = OptionalNone,
OptionalMatrixType H2 = OptionalNone) const override {
Matrix D1, D2;
double error = value(x1, x2, D1, D2) - threshold_;
if (H1) {

12
gtsam/slam/EssentialMatrixConstraint.cpp
View File

@ -43,16 +43,20 @@ bool EssentialMatrixConstraint::equals(const NonlinearFactor& expected,
/* ************************************************************************* */
Vector EssentialMatrixConstraint::evaluateError(const Pose3& p1,
const Pose3& p2, boost::optional<Matrix&> Hp1,
boost::optional<Matrix&> Hp2) const {
const Pose3& p2, OptionalMatrixType Hp1,
OptionalMatrixType Hp2) const {
// compute relative Pose3 between p1 and p2
Pose3 _1P2_ = p1.between(p2, Hp1, Hp2);
// convert to EssentialMatrix
Matrix D_hx_1P2;
EssentialMatrix hx = EssentialMatrix::FromPose3(_1P2_,
(Hp1 || Hp2) ? boost::optional<Matrix&>(D_hx_1P2) : boost::none);
EssentialMatrix hx;
if (Hp1 || Hp2) {
hx = EssentialMatrix::FromPose3(_1P2_, D_hx_1P2);
} else {
hx = EssentialMatrix::FromPose3(_1P2_, OptionalNone);
}
// Calculate derivatives if needed
if (Hp1) {

4
gtsam/slam/EssentialMatrixConstraint.h
View File

@ -79,8 +79,8 @@ public:
/** vector of errors */
Vector evaluateError(const Pose3& p1, const Pose3& p2,
boost::optional<Matrix&> Hp1 = boost::none, //
boost::optional<Matrix&> Hp2 = boost::none) const override;
OptionalMatrixType Hp1 = OptionalNone, //
OptionalMatrixType Hp2 = OptionalNone) const override;
/** return the measured */
const EssentialMatrix& measured() const {

20
gtsam/slam/EssentialMatrixFactor.h
View File

@ -91,7 +91,7 @@ class EssentialMatrixFactor : public NoiseModelFactorN<EssentialMatrix> {
/// vector of errors returns 1D vector
Vector evaluateError(
const EssentialMatrix& E,
boost::optional<Matrix&> H = boost::none) const override {
OptionalMatrixType H = OptionalNone) const override {
Vector error(1);
error << E.error(vA_, vB_, H);
return error;
@ -173,8 +173,8 @@ class EssentialMatrixFactor2
*/
Vector evaluateError(
const EssentialMatrix& E, const double& d,
boost::optional<Matrix&> DE = boost::none,
boost::optional<Matrix&> Dd = boost::none) const override {
OptionalMatrixType DE = OptionalNone,
OptionalMatrixType Dd = OptionalNone) 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)
@ -284,13 +284,13 @@ class EssentialMatrixFactor3 : public EssentialMatrixFactor2 {
*/
Vector evaluateError(
const EssentialMatrix& E, const double& d,
boost::optional<Matrix&> DE = boost::none,
boost::optional<Matrix&> Dd = boost::none) const override {
OptionalMatrixType DE = OptionalNone,
OptionalMatrixType Dd = OptionalNone) const override {
if (!DE) {
// Convert E from body to camera frame
EssentialMatrix cameraE = cRb_ * E;
// Evaluate error
return Base::evaluateError(cameraE, d, boost::none, Dd);
return Base::evaluateError(cameraE, d, OptionalNone, Dd);
} else {
// Version with derivatives
Matrix D_e_cameraE, D_cameraE_E; // 2*5, 5*5
@ -372,13 +372,13 @@ class EssentialMatrixFactor4
*/
Vector evaluateError(
const EssentialMatrix& E, const CALIBRATION& K,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const override {
OptionalMatrixType H1 = OptionalNone,
OptionalMatrixType H2 = OptionalNone) const override {
// converting from pixel coordinates to normalized coordinates cA and cB
JacobianCalibration cA_H_K; // dcA/dK
JacobianCalibration cB_H_K; // dcB/dK
Point2 cA = K.calibrate(pA_, H2 ? &cA_H_K : 0, boost::none);
Point2 cB = K.calibrate(pB_, H2 ? &cB_H_K : 0, boost::none);
Point2 cA = K.calibrate(pA_, H2 ? &cA_H_K : 0, OptionalNone);
Point2 cB = K.calibrate(pB_, H2 ? &cB_H_K : 0, OptionalNone);
// convert to homogeneous coordinates
Vector3 vA = EssentialMatrix::Homogeneous(cA);

10
gtsam/slam/FrobeniusFactor.h
View File

@ -65,7 +65,7 @@ class FrobeniusPrior : public NoiseModelFactorN<Rot> {
/// Error is just Frobenius norm between Rot element and vectorized matrix M.
Vector evaluateError(const Rot& R,
boost::optional<Matrix&> H = boost::none) const override {
OptionalMatrixType H = OptionalNone) const override {
return R.vec(H) - vecM_; // Jacobian is computed only when needed.
}
};
@ -86,8 +86,8 @@ class FrobeniusFactor : public NoiseModelFactorN<Rot, Rot> {
/// Error is just Frobenius norm between rotation matrices.
Vector evaluateError(const Rot& R1, const Rot& R2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const override {
OptionalMatrixType H1 = OptionalNone,
OptionalMatrixType H2 = OptionalNone) const override {
Vector error = R2.vec(H2) - R1.vec(H1);
if (H1) *H1 = -*H1;
return error;
@ -150,8 +150,8 @@ class FrobeniusBetweenFactor : public NoiseModelFactorN<Rot, Rot> {
/// Error is Frobenius norm between R1*R12 and R2.
Vector evaluateError(const Rot& R1, const Rot& R2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const override {
OptionalMatrixType H1 = OptionalNone,
OptionalMatrixType H2 = OptionalNone) const override {
const Rot R2hat = R1.compose(R12_);
Eigen::Matrix<double, Dim, Rot::dimension> vec_H_R2hat;
Vector error = R2.vec(H2) - R2hat.vec(H1 ? &vec_H_R2hat : nullptr);

8
gtsam/slam/GeneralSFMFactor.h
View File

@ -123,7 +123,7 @@ public:
/** h(x)-z */
Vector evaluateError(const CAMERA& camera, const LANDMARK& point,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const override {
OptionalMatrixType H1=OptionalNone, OptionalMatrixType H2=OptionalNone) const override {
try {
return camera.project2(point,H1,H2) - measured_;
}
@ -258,9 +258,9 @@ public:
/** h(x)-z */
Vector evaluateError(const Pose3& pose3, const Point3& point, const CALIBRATION &calib,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none,
boost::optional<Matrix&> H3=boost::none) const override
OptionalMatrixType H1=OptionalNone,
OptionalMatrixType H2=OptionalNone,
OptionalMatrixType H3=OptionalNone) const override
{
try {
Camera camera(pose3,calib);

6
gtsam/slam/OrientedPlane3Factor.cpp
View File

@ -23,8 +23,8 @@ void OrientedPlane3Factor::print(const string& s,
//***************************************************************************
Vector OrientedPlane3Factor::evaluateError(const Pose3& pose,
const OrientedPlane3& plane, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
const OrientedPlane3& plane, OptionalMatrixType H1,
OptionalMatrixType H2) const {
Matrix36 predicted_H_pose;
Matrix33 predicted_H_plane, error_H_predicted;
@ -64,7 +64,7 @@ bool OrientedPlane3DirectionPrior::equals(const NonlinearFactor& expected,
//***************************************************************************
Vector OrientedPlane3DirectionPrior::evaluateError(
const OrientedPlane3& plane, boost::optional<Matrix&> H) const {
const OrientedPlane3& plane, OptionalMatrixType H) const {
Unit3 n_hat_p = measured_p_.normal();
Unit3 n_hat_q = plane.normal();
Matrix2 H_p;

2
gtsam/slam/PoseRotationPrior.h
View File

@ -75,7 +75,7 @@ public:
}
/** h(x)-z */
Vector evaluateError(const Pose& pose, boost::optional<Matrix&> H = boost::none) const override {
Vector evaluateError(const Pose& pose, OptionalMatrixType H = OptionalNone) const override {
const Rotation& newR = pose.rotation();
if (H) {
*H = Matrix::Zero(rDim, xDim);

2
gtsam/slam/PoseTranslationPrior.h
View File

@ -59,7 +59,7 @@ public:
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/** h(x)-z */
Vector evaluateError(const Pose& pose, boost::optional<Matrix&> H = boost::none) const override {
Vector evaluateError(const Pose& pose, OptionalMatrixType H = OptionalNone) const override {
const Translation& newTrans = pose.translation();
const Rotation& R = pose.rotation();
const int tDim = traits<Translation>::GetDimension(newTrans);

2
gtsam/slam/ProjectionFactor.h
View File

@ -133,7 +133,7 @@ namespace gtsam {
/// Evaluate error h(x)-z and optionally derivatives
Vector evaluateError(const Pose3& pose, const Point3& point,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const override {
OptionalMatrixType H1 = OptionalNone, OptionalMatrixType H2 = OptionalNone) const override {
try {
if(body_P_sensor_) {
if(H1) {

9
gtsam/slam/ReferenceFrameFactor.h
View File

@ -95,12 +95,13 @@ public:
/** Combined cost and derivative function using boost::optional */
Vector evaluateError(const Point& global, const Transform& trans, const Point& local,
boost::optional<Matrix&> Dforeign = boost::none,
boost::optional<Matrix&> Dtrans = boost::none,
boost::optional<Matrix&> Dlocal = boost::none) const override {
OptionalMatrixType Dforeign = OptionalNone,
OptionalMatrixType Dtrans = OptionalNone,
OptionalMatrixType Dlocal = OptionalNone) const override {
Point newlocal = transform_point<Transform,Point>(trans, global, Dtrans, Dforeign);
if (Dlocal)
if (Dlocal) {
*Dlocal = -1* Matrix::Identity(traits<Point>::dimension, traits<Point>::dimension);
}
return traits<Point>::Local(local,newlocal);
}

4
gtsam/slam/RotateFactor.h
View File

@ -51,7 +51,7 @@ public:
/// vector of errors returns 2D vector
Vector evaluateError(const Rot3& R,
boost::optional<Matrix&> H = boost::none) const override {
OptionalMatrixType H = OptionalNone) const override {
// predict p_ as q = R*z_, derivative H will be filled if not none
Point3 q = R.rotate(z_,H);
// error is just difference, and note derivative of that wrpt q is I3
@ -102,7 +102,7 @@ public:
}
/// vector of errors returns 2D vector
Vector evaluateError(const Rot3& iRc, boost::optional<Matrix&> H = boost::none) const override {
Vector evaluateError(const Rot3& iRc, OptionalMatrixType H = OptionalNone) const override {
Unit3 i_q = iRc * c_z_;
Vector error = i_p_.error(i_q, H);
if (H) {

2
gtsam/slam/StereoFactor.h
View File

@ -120,7 +120,7 @@ public:
/** h(x)-z */
Vector evaluateError(const Pose3& pose, const Point3& point,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const override {
OptionalMatrixType H1 = OptionalNone, OptionalMatrixType H2 = OptionalNone) const override {
try {
if(body_P_sensor_) {
if(H1) {

6
gtsam/slam/TriangulationFactor.h
View File

@ -119,10 +119,10 @@ public:
}
/// Evaluate error h(x)-z and optionally derivatives
Vector evaluateError(const Point3& point, boost::optional<Matrix&> H2 =
boost::none) const override {
Vector evaluateError(const Point3& point, OptionalMatrixType H2 =
OptionalNone) const override {
try {
return traits<Measurement>::Local(measured_, camera_.project2(point, boost::none, H2));
return traits<Measurement>::Local(measured_, camera_.project2(point, OptionalNone, H2));
} catch (CheiralityException& e) {
if (H2)
*H2 = Matrix::Zero(traits<Measurement>::dimension, 3);

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, boost::optional<std::vector<Matrix>&> H = boost::none) const override {
Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = OptionalNone) const override {
Vector a;
return a;

10
tests/simulated2D.h
View File

@ -139,7 +139,7 @@ namespace simulated2D {
}
/// Return error and optional derivative
Vector evaluateError(const Pose& x, boost::optional<Matrix&> H = boost::none) const override {
Vector evaluateError(const Pose& x, OptionalMatrixType H = OptionalNone) const override {
return (prior(x, H) - measured_);
}
@ -184,8 +184,8 @@ namespace simulated2D {
/// Evaluate error and optionally return derivatives
Vector evaluateError(const Pose& x1, const Pose& x2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const override {
OptionalMatrixType H1 = OptionalNone,
OptionalMatrixType H2 = OptionalNone) const override {
return (odo(x1, x2, H1, H2) - measured_);
}
@ -231,8 +231,8 @@ namespace simulated2D {
/// Evaluate error and optionally return derivatives
Vector evaluateError(const Pose& x1, const Landmark& x2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const override {
OptionalMatrixType H1 = OptionalNone,
OptionalMatrixType H2 = OptionalNone) const override {
return (mea(x1, x2, H1, H2) - measured_);
}

8
tests/simulated2DOriented.h
View File

@ -90,8 +90,8 @@ namespace simulated2DOriented {
}
/// Evaluate error and optionally derivative
Vector evaluateError(const Pose2& x, boost::optional<Matrix&> H =
boost::none) const {
Vector evaluateError(const Pose2& x, OptionalMatrixType H =
OptionalNone) const {
return measured_.localCoordinates(prior(x, H));
}
@ -118,8 +118,8 @@ namespace simulated2DOriented {
/// Evaluate error and optionally derivative
Vector evaluateError(const VALUE& x1, const VALUE& x2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const override {
OptionalMatrixType H1 = OptionalNone,
OptionalMatrixType H2 = OptionalNone) const override {
return measured_.localCoordinates(odo(x1, x2, H1, H2));
}

6
tests/simulated3D.h
View File

@ -89,8 +89,8 @@ struct PointPrior3D: public NoiseModelFactorN<Point3> {
* @param H is an optional Jacobian matrix (Dimension: 3x3)
* @return Vector error between prior value and x (Dimension: 3)
*/
Vector evaluateError(const Point3& x, boost::optional<Matrix&> H =
boost::none) const override {
Vector evaluateError(const Point3& x, OptionalMatrixType H =
OptionalNone) const override {
return prior(x, H) - measured_;
}
};
@ -121,7 +121,7 @@ struct Simulated3DMeasurement: public NoiseModelFactorN<Point3, Point3> {
* @return vector error between measurement and prediction (Dimension: 3)
*/
Vector evaluateError(const Point3& x1, const Point3& x2,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const override {
OptionalMatrixType H1 = OptionalNone, OptionalMatrixType H2 = OptionalNone) const override {
return mea(x1, x2, H1, H2) - measured_;
}
};

2
tests/smallExample.h
View File

@ -335,7 +335,7 @@ struct UnaryFactor: public gtsam::NoiseModelFactorN<Point2> {
gtsam::NoiseModelFactorN<Point2>(model, key), z_(z) {
}
Vector evaluateError(const Point2& x, boost::optional<Matrix&> A = boost::none) const override {
Vector evaluateError(const Point2& x, OptionalMatrixType A = OptionalNone) const override {
if (A) *A = H(x);
return (h(x) - z_);
}

6
tests/testExtendedKalmanFilter.cpp
View File

@ -213,8 +213,8 @@ public:
/** vector of errors */
Vector evaluateError(const Point2& p1, const Point2& p2,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
boost::none) const override {
OptionalMatrixType H1 = OptionalNone, OptionalMatrixType H2 =
OptionalNone) const override {
// error = p2 - f(p1)
// H1 = d error / d p1 = -d f/ d p1 = -F
@ -339,7 +339,7 @@ public:
}
/** vector of errors */
Vector evaluateError(const Point2& p, boost::optional<Matrix&> H1 = boost::none) const override {
Vector evaluateError(const Point2& p, OptionalMatrixType H1 = OptionalNone) const override {
// error = z - h(p)
// H = d error / d p = -d h/ d p = -H
Vector z_hat = h(p);

41
tests/testNonlinearFactor.cpp
View File

@ -340,8 +340,7 @@ class TestFactor1 : public NoiseModelFactor1<double> {
TestFactor1() : Base(noiseModel::Diagonal::Sigmas(Vector1(2.0)), L(1)) {}
using Base::NoiseModelFactor1; // inherit constructors
Vector evaluateError(const double& x1, boost::optional<Matrix&> H1 =
boost::none) const override {
Vector evaluateError(const double& x1, OptionalMatrixType H1 = OptionalNone) const override {
if (H1) *H1 = (Matrix(1, 1) << 1.0).finished();
return (Vector(1) << x1).finished();
}
@ -394,10 +393,10 @@ class TestFactor4 : public NoiseModelFactor4<double, double, double, double> {
Vector
evaluateError(const double& x1, const double& x2, const double& x3, const double& x4,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none,
boost::optional<Matrix&> H4 = boost::none) const override {
OptionalMatrixType H1 = OptionalNone,
OptionalMatrixType H2 = OptionalNone,
OptionalMatrixType H3 = OptionalNone,
OptionalMatrixType H4 = OptionalNone) const override {
if(H1) {
*H1 = (Matrix(1, 1) << 1.0).finished();
*H2 = (Matrix(1, 1) << 2.0).finished();
@ -484,11 +483,11 @@ public:
Vector
evaluateError(const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none,
boost::optional<Matrix&> H4 = boost::none,
boost::optional<Matrix&> H5 = boost::none) const override {
OptionalMatrixType H1 = OptionalNone,
OptionalMatrixType H2 = OptionalNone,
OptionalMatrixType H3 = OptionalNone,
OptionalMatrixType H4 = OptionalNone,
OptionalMatrixType H5 = OptionalNone) const override {
if(H1) {
*H1 = (Matrix(1, 1) << 1.0).finished();
*H2 = (Matrix(1, 1) << 2.0).finished();
@ -534,12 +533,12 @@ public:
Vector
evaluateError(const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none,
boost::optional<Matrix&> H4 = boost::none,
boost::optional<Matrix&> H5 = boost::none,
boost::optional<Matrix&> H6 = boost::none) const override {
OptionalMatrixType H1 = OptionalNone,
OptionalMatrixType H2 = OptionalNone,
OptionalMatrixType H3 = OptionalNone,
OptionalMatrixType H4 = OptionalNone,
OptionalMatrixType H5 = OptionalNone,
OptionalMatrixType H6 = OptionalNone) const override {
if(H1) {
*H1 = (Matrix(1, 1) << 1.0).finished();
*H2 = (Matrix(1, 1) << 2.0).finished();
@ -594,10 +593,10 @@ public:
Vector
evaluateError(const double& x1, const double& x2, const double& x3, const double& x4,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none,
boost::optional<Matrix&> H4 = boost::none) const override {
OptionalMatrixType H1 = OptionalNone,
OptionalMatrixType H2 = OptionalNone,
OptionalMatrixType H3 = OptionalNone,
OptionalMatrixType H4 = OptionalNone) const override {
if (H1) *H1 = (Matrix(1, 1) << 1.0).finished();
if (H2) *H2 = (Matrix(1, 1) << 2.0).finished();
if (H3) *H3 = (Matrix(1, 1) << 3.0).finished();