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

removed some more boost optional matrix references

release/4.3a0
kartik arcot 2023-01-10 15:43:35 -08:00
parent 53d23b96ff
commit bbb997f895
9 changed files with 70 additions and 62 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/base/numericalDerivative.h
View File

@ -35,7 +35,7 @@ namespace gtsam {
*
* Usage of the boost bind version to rearrange arguments:
* for a function with one relevant param and an optional derivative:
* Foo bar(const Obj& a, boost::optional<Matrix&> H1)
* Foo bar(const Obj& a, OptionalMatrixType H1)
* Use boost.bind to restructure:
* std::bind(bar, std::placeholders::_1, boost::none)
* This syntax will fix the optional argument to boost::none, while using the first argument provided

4
gtsam/navigation/MagFactor.h
View File

@ -62,7 +62,7 @@ public:
}
static Point3 unrotate(const Rot2& R, const Point3& p,
boost::optional<Matrix&> HR = boost::none) {
OptionalMatrixType HR = OptionalNone) {
Point3 q = Rot3::Yaw(R.theta()).unrotate(p, HR, boost::none);
if (HR) {
// assign to temporary first to avoid error in Win-Debug mode
@ -115,7 +115,7 @@ public:
*/
Vector evaluateError(const Rot3& nRb, OptionalMatrixType H) const override {
// measured bM = nRb<52> * nM + b
Point3 hx = nRb.unrotate(nM_, H, boost::none) + bias_;
Point3 hx = nRb.unrotate(nM_, H, OptionalNone) + bias_;
return (hx - measured_);
}
};

29
gtsam/navigation/tests/testMagFactor.cpp
View File

@ -56,15 +56,13 @@ Unit3 dir(nM);
SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25);
} // namespace
using boost::none;
// *************************************************************************
TEST( MagFactor, unrotate ) {
Matrix H;
Point3 expected(22735.5, 314.502, 44202.5);
EXPECT( assert_equal(expected, MagFactor::unrotate(theta,nM,H),1e-1));
EXPECT( assert_equal(expected, MagFactor::unrotate(theta,nM,&H),1e-1));
EXPECT(assert_equal(numericalDerivative11<Point3, Rot2> //
(std::bind(&MagFactor::unrotate, std::placeholders::_1, nM, none), theta), H, 1e-6));
(std::bind(&MagFactor::unrotate, std::placeholders::_1, nM, OptionalNone), theta), H, 1e-6));
}
// *************************************************************************
@ -75,37 +73,32 @@ TEST( MagFactor, Factors ) {
// MagFactor
MagFactor f(1, measured, s, dir, bias, model);
EXPECT(assert_equal(Z_3x1,f.evaluateError(theta,H1),1e-5));
EXPECT(assert_equal((Matrix)numericalDerivative11<Vector, Rot2> //
(std::bind(&MagFactor::evaluateError, &f, std::placeholders::_1, none), theta), H1, 1e-7));
EXPECT(assert_equal((Matrix)numericalDerivative11<Vector, Rot2>
([&f] (const Rot2& theta) { return f.evaluateError(theta); }, theta), H1, 1e-7));
// MagFactor1
MagFactor1 f1(1, measured, s, dir, bias, model);
EXPECT(assert_equal(Z_3x1,f1.evaluateError(nRb,H1),1e-5));
EXPECT(assert_equal(numericalDerivative11<Vector, Rot3> //
(std::bind(&MagFactor1::evaluateError, &f1, std::placeholders::_1, none), nRb), H1, 1e-7));
EXPECT(assert_equal(numericalDerivative11<Vector, Rot3>
([&f1] (const Rot3& nRb) { return f1.evaluateError(nRb); }, nRb), H1, 1e-7));
// MagFactor2
MagFactor2 f2(1, 2, measured, nRb, model);
EXPECT(assert_equal(Z_3x1,f2.evaluateError(scaled,bias,H1,H2),1e-5));
EXPECT(assert_equal(numericalDerivative11<Vector, Point3> //
(std::bind(&MagFactor2::evaluateError, &f2, std::placeholders::_1, bias, none, none), scaled),//
H1, 1e-7));
([&f2] (const Point3& scaled) { return f2.evaluateError(scaled,bias); }, scaled), H1, 1e-7));
EXPECT(assert_equal(numericalDerivative11<Vector, Point3> //
(std::bind(&MagFactor2::evaluateError, &f2, scaled, std::placeholders::_1, none, none), bias),//
H2, 1e-7));
([&f2] (const Point3& bias) { return f2.evaluateError(scaled,bias); }, bias), H2, 1e-7));
// MagFactor3
MagFactor3 f3(1, 2, 3, measured, nRb, model);
EXPECT(assert_equal(Z_3x1,f3.evaluateError(s,dir,bias,H1,H2,H3),1e-5));
EXPECT(assert_equal((Matrix)numericalDerivative11<Vector,double> //
(std::bind(&MagFactor3::evaluateError, &f3, std::placeholders::_1, dir, bias, none, none, none), s),//
H1, 1e-7));
([&f3] (double s) { return f3.evaluateError(s,dir,bias); }, s), H1, 1e-7));
EXPECT(assert_equal(numericalDerivative11<Vector,Unit3> //
(std::bind(&MagFactor3::evaluateError, &f3, s, std::placeholders::_1, bias, none, none, none), dir),//
H2, 1e-7));
([&f3] (const Unit3& dir) { return f3.evaluateError(s,dir,bias); }, dir), H2, 1e-7));
EXPECT(assert_equal(numericalDerivative11<Vector,Point3> //
(std::bind(&MagFactor3::evaluateError, &f3, s, dir, std::placeholders::_1, none, none, none), bias),//
H3, 1e-7));
([&f3] (const Point3& bias) { return f3.evaluateError(s,dir,bias); }, bias), H3, 1e-7));
}
// *************************************************************************

10
gtsam/nonlinear/NonlinearFactor.h
View File

@ -381,8 +381,8 @@ struct NoiseModelFactorAliases<T1, T2, T3, T4, T5, T6, TExtra...> {
*
* Vector evaluateError(
* const Pose3& T, const Point3& p,
* boost::optional<Matrix&> H_T = boost::none,
* boost::optional<Matrix&> H_p = boost::none) const override {
* OptionalMatrixType H_T = OptionalNone,
* OptionalMatrixType H_p = OptionalNone) const override {
* Matrix36 t_H_T; // partial derivative of translation w.r.t. pose T
*
* // Only compute t_H_T if needed:
@ -449,7 +449,7 @@ protected:
/// @}
/* Like std::void_t, except produces `boost::optional<Matrix&>` instead of
/* Like std::void_t, except produces `OptionalMatrixType` instead of
* `void`. Used to expand fixed-type parameter-packs with same length as
* ValueTypes. */
@ -582,8 +582,8 @@ protected:
* ```
* Vector evaluateError(
* const Pose3& 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 { ... }
* ```
*
* If any of the optional Matrix reference arguments are specified, it should

12
gtsam/slam/BoundingConstraint.h
View File

@ -57,8 +57,8 @@ struct BoundingConstraint1: public NoiseModelFactorN<VALUE> {
* Must have optional argument for derivative with 1xN matrix, where
* N = X::dim()
*/
virtual double value(const X& x, boost::optional<Matrix&> H =
boost::none) const = 0;
virtual double value(const X& x, OptionalMatrixType H =
OptionalNone) const = 0;
/** active when constraint *NOT* met */
bool active(const Values& c) const override {
@ -69,7 +69,7 @@ struct BoundingConstraint1: public NoiseModelFactorN<VALUE> {
Vector evaluateError(const X& x, OptionalMatrixType H) const override {
Matrix D;
double error = value(x, D) - threshold_;
double error = value(x, &D) - threshold_;
if (H) {
if (isGreaterThan_) *H = D;
else *H = -1.0 * D;
@ -128,8 +128,8 @@ struct BoundingConstraint2: public NoiseModelFactorN<VALUE1, VALUE2> {
* Must have optional argument for derivatives)
*/
virtual double value(const X1& x1, const X2& x2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const = 0;
OptionalMatrixType H1 = OptionalNone,
OptionalMatrixType H2 = OptionalNone) const = 0;
/** active when constraint *NOT* met */
bool active(const Values& c) const override {
@ -141,7 +141,7 @@ struct BoundingConstraint2: public NoiseModelFactorN<VALUE1, VALUE2> {
Vector evaluateError(const X1& x1, const X2& x2,
OptionalMatrixType H1, OptionalMatrixType H2) const override {
Matrix D1, D2;
double error = value(x1, x2, D1, D2) - threshold_;
double error = value(x1, x2, &D1, &D2) - threshold_;
if (H1) {
if (isGreaterThan_) *H1 = D1;
else *H1 = -1.0 * D1;

43
gtsam_unstable/geometry/Pose3Upright.cpp
View File

@ -5,7 +5,10 @@
* @author Alex Cunningham
*/
#include <boost/mpl/assert.hpp>
#include <iostream>
#include "gtsam/base/OptionalJacobian.h"
#include "gtsam/base/Vector.h"
#include <gtsam_unstable/geometry/Pose3Upright.h>
@ -78,27 +81,34 @@ Pose3 Pose3Upright::pose() const {
}
/* ************************************************************************* */
Pose3Upright Pose3Upright::inverse(boost::optional<Matrix&> H1) const {
Pose3Upright result(T_.inverse(H1), -z_);
if (H1) {
Matrix H1_ = -I_4x4;
H1_.topLeftCorner(2,2) = H1->topLeftCorner(2,2);
H1_.topRightCorner(2, 1) = H1->topRightCorner(2, 1);
*H1 = H1_;
Pose3Upright Pose3Upright::inverse(OptionalJacobian<4, 4> H1) const {
if (!H1) {
return Pose3Upright(T_.inverse(), -z_);
}
OptionalJacobian<3, 3>::Jacobian H3x3;
// TODO: Could not use reference to a view into H1 and reuse memory
// Eigen::Ref<Eigen::Matrix<double, 3, 3>> H3x3 = H1->topLeftCorner(3,3);
Pose3Upright result(T_.inverse(H3x3), -z_);
Matrix H1_ = -I_4x4;
H1_.topLeftCorner(2, 2) = H3x3.topLeftCorner(2, 2);
H1_.topRightCorner(2, 1) = H3x3.topRightCorner(2, 1);
*H1 = H1_;
return result;
}
/* ************************************************************************* */
Pose3Upright Pose3Upright::compose(const Pose3Upright& p2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
OptionalJacobian<4,4> H1, OptionalJacobian<4,4> H2) const {
if (!H1 && !H2)
return Pose3Upright(T_.compose(p2.T_), z_ + p2.z_);
Pose3Upright result(T_.compose(p2.T_, H1), z_ + p2.z_);
// TODO: Could not use reference to a view into H1 and reuse memory
OptionalJacobian<3, 3>::Jacobian H3x3;
Pose3Upright result(T_.compose(p2.T_, H3x3), z_ + p2.z_);
if (H1) {
Matrix H1_ = I_4x4;
H1_.topLeftCorner(2,2) = H1->topLeftCorner(2,2);
H1_.topRightCorner(2, 1) = H1->topRightCorner(2, 1);
H1_.topLeftCorner(2,2) = H3x3.topLeftCorner(2,2);
H1_.topRightCorner(2, 1) = H3x3.topRightCorner(2, 1);
*H1 = H1_;
}
if (H2) *H2 = I_4x4;
@ -107,14 +117,17 @@ Pose3Upright Pose3Upright::compose(const Pose3Upright& p2,
/* ************************************************************************* */
Pose3Upright Pose3Upright::between(const Pose3Upright& p2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
OptionalJacobian<4,4> H1, OptionalJacobian<4,4> H2) const {
if (!H1 && !H2)
return Pose3Upright(T_.between(p2.T_), p2.z_ - z_);
Pose3Upright result(T_.between(p2.T_, H1, H2), p2.z_ - z_);
// TODO: Could not use reference to a view into H1 and H2 to reuse memory
OptionalJacobian<3, 3>::Jacobian H3x3_1, H3x3_2;
Pose3Upright result(T_.between(p2.T_, H3x3_1, H3x3_2), p2.z_ - z_);
if (H1) {
Matrix H1_ = -I_4x4;
H1_.topLeftCorner(2,2) = H1->topLeftCorner(2,2);
H1_.topRightCorner(2, 1) = H1->topRightCorner(2, 1);
H1_.topLeftCorner(2,2) = H3x3_1.topLeftCorner(2,2);
H1_.topRightCorner(2, 1) = H3x3_1.topRightCorner(2, 1);
*H1 = H1_;
}
if (H2) *H2 = I_4x4;

10
gtsam_unstable/geometry/Pose3Upright.h
View File

@ -98,12 +98,12 @@ public:
static Pose3Upright Identity() { return Pose3Upright(); }
/// inverse transformation with derivatives
Pose3Upright inverse(boost::optional<Matrix&> H1=boost::none) const;
Pose3Upright inverse(OptionalJacobian<4,4> H1=boost::none) const;
///compose this transformation onto another (first *this and then p2)
Pose3Upright compose(const Pose3Upright& p2,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const;
OptionalJacobian<4,4> H1=boost::none,
OptionalJacobian<4,4> H2=boost::none) const;
/// compose syntactic sugar
inline Pose3Upright operator*(const Pose3Upright& T) const { return compose(T); }
@ -113,8 +113,8 @@ public:
* as well as optionally the derivatives
*/
Pose3Upright between(const Pose3Upright& p2,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const;
OptionalJacobian<4,4> H1=boost::none,
OptionalJacobian<4,4> H2=boost::none) const;
/// @}
/// @name Lie Group

13
tests/simulated2DConstraints.h
View File

@ -25,6 +25,7 @@
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/BoundingConstraint.h>
#include <tests/simulated2D.h>
#include "gtsam/nonlinear/NonlinearFactor.h"
// \namespace
@ -87,8 +88,8 @@ namespace simulated2D {
* @param x is the estimate of the constrained variable being evaluated
* @param H is an optional Jacobian, linearized at x
*/
double value(const Point& x, boost::optional<Matrix&> H =
boost::none) const override {
double value(const Point& x, OptionalMatrixType H =
OptionalNone) const override {
if (H) {
Matrix D = Matrix::Zero(1, traits<Point>::GetDimension(x));
D(0, IDX) = 1.0;
@ -151,8 +152,8 @@ namespace simulated2D {
* @return the distance between the variables
*/
double value(const Point& x1, const Point& x2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const override {
OptionalMatrixType H1 = OptionalNone,
OptionalMatrixType H2 = OptionalNone) const override {
if (H1) *H1 = numericalDerivative21(range_trait<Point,Point>, x1, x2, 1e-5);
if (H1) *H2 = numericalDerivative22(range_trait<Point,Point>, x1, x2, 1e-5);
return range_trait(x1, x2);
@ -201,8 +202,8 @@ namespace simulated2D {
* @return the distance between the variables
*/
double value(const Pose& x1, const Point& x2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const override {
OptionalMatrixType H1 = OptionalNone,
OptionalMatrixType H2 = OptionalNone) const override {
if (H1) *H1 = numericalDerivative21(range_trait<Pose,Point>, x1, x2, 1e-5);
if (H1) *H2 = numericalDerivative22(range_trait<Pose,Point>, x1, x2, 1e-5);
return range_trait(x1, x2);

9
timing/timeRot2.cpp
View File

@ -19,6 +19,7 @@
#include <gtsam/geometry/Pose2.h>
#include <gtsam/base/timing.h>
#include <iostream>
#include "gtsam/base/OptionalJacobian.h"
using namespace std;
using namespace gtsam;
@ -43,7 +44,7 @@ Rot2 Rot2betweenOptimized(const Rot2& r1, const Rot2& r2) {
/* ************************************************************************* */
Vector Rot2BetweenFactorEvaluateErrorDefault(const Rot2& measured, const Rot2& p1, const Rot2& p2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2)
OptionalJacobian<1,1> H1, OptionalJacobian<1,1> H2)
{
Rot2 hx = p1.between(p2, H1, H2); // h(x)
// manifold equivalent of h(x)-z -> log(z,h(x))
@ -52,7 +53,7 @@ Vector Rot2BetweenFactorEvaluateErrorDefault(const Rot2& measured, const Rot2& p
/* ************************************************************************* */
Vector Rot2BetweenFactorEvaluateErrorOptimizedBetween(const Rot2& measured, const Rot2& p1, const Rot2& p2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2)
OptionalJacobian<1,1> H1, OptionalJacobian<1,1> H2)
{
Rot2 hx = Rot2betweenOptimized(p1, p2); // h(x)
if (H1) *H1 = -I_1x1;
@ -63,7 +64,7 @@ Vector Rot2BetweenFactorEvaluateErrorOptimizedBetween(const Rot2& measured, cons
/* ************************************************************************* */
Vector Rot2BetweenFactorEvaluateErrorOptimizedBetweenNoeye(const Rot2& measured, const Rot2& p1, const Rot2& p2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2)
OptionalJacobian<1,1> H1, OptionalJacobian<1,1> H2)
{
// TODO: Implement
Rot2 hx = Rot2betweenOptimized(p1, p2); // h(x)
@ -76,7 +77,7 @@ Vector Rot2BetweenFactorEvaluateErrorOptimizedBetweenNoeye(const Rot2& measured,
/* ************************************************************************* */
typedef Eigen::Matrix<double,1,1> Matrix1;
Vector Rot2BetweenFactorEvaluateErrorOptimizedBetweenFixed(const Rot2& measured, const Rot2& p1, const Rot2& p2,
boost::optional<Matrix1&> H1, boost::optional<Matrix1&> H2)
OptionalJacobian<1,1> H1, OptionalJacobian<1,1> H2)
{
// TODO: Implement
Rot2 hx = Rot2betweenOptimized(p1, p2); // h(x)