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

FixedRef is now OptionalJacobian

release/4.3a0
dellaert 2014-11-28 17:14:26 +01:00
parent a89b4d2168
commit 6505e602d8
26 changed files with 116 additions and 118 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/geometry/Cal3Bundler.cpp
View File

@ -66,7 +66,7 @@ bool Cal3Bundler::equals(const Cal3Bundler& K, double tol) const {
/* ************************************************************************* */
Point2 Cal3Bundler::uncalibrate(const Point2& p, //
FixedRef<2, 3> Dcal, FixedRef<2, 2> Dp) const {
OptionalJacobian<2, 3> Dcal, OptionalJacobian<2, 2> Dp) const {
// r = x^2 + y^2;
// g = (1 + k(1)*r + k(2)*r^2);
// pi(:,i) = g * pn(:,i)

4
gtsam/geometry/Cal3Bundler.h
View File

@ -113,8 +113,8 @@ public:
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
* @return point in image coordinates
*/
Point2 uncalibrate(const Point2& p, FixedRef<2, 3> Dcal = boost::none,
FixedRef<2, 2> Dp = boost::none) const;
Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 3> Dcal = boost::none,
OptionalJacobian<2, 2> Dp = boost::none) const;
/// Conver a pixel coordinate to ideal coordinate
Point2 calibrate(const Point2& pi, const double tol = 1e-5) const;

2
gtsam/geometry/Cal3DS2_Base.cpp
View File

@ -91,7 +91,7 @@ static Matrix2 D2dintrinsic(double x, double y, double rr,
/* ************************************************************************* */
Point2 Cal3DS2_Base::uncalibrate(const Point2& p,
FixedRef<2,9> H1, FixedRef<2,2> H2) const {
OptionalJacobian<2,9> H1, OptionalJacobian<2,2> H2) const {
// rr = x^2 + y^2;
// g = (1 + k(1)*rr + k(2)*rr^2);

4
gtsam/geometry/Cal3DS2_Base.h
View File

@ -114,8 +114,8 @@ public:
* @return point in (distorted) image coordinates
*/
Point2 uncalibrate(const Point2& p,
FixedRef<2,9> Dcal = boost::none,
FixedRef<2,2> Dp = boost::none) const ;
OptionalJacobian<2,9> Dcal = boost::none,
OptionalJacobian<2,2> Dp = boost::none) const ;
/// Convert (distorted) image coordinates uv to intrinsic coordinates xy
Point2 calibrate(const Point2& p, const double tol=1e-5) const;

4
gtsam/geometry/Cal3_S2.cpp
View File

@ -72,8 +72,8 @@ bool Cal3_S2::equals(const Cal3_S2& K, double tol) const {
}
/* ************************************************************************* */
Point2 Cal3_S2::uncalibrate(const Point2& p, FixedRef<2, 5> Dcal,
FixedRef<2, 2> Dp) const {
Point2 Cal3_S2::uncalibrate(const Point2& p, OptionalJacobian<2, 5> Dcal,
OptionalJacobian<2, 2> Dp) const {
const double x = p.x(), y = p.y();
if (Dcal)
*Dcal << x, 0.0, y, 1.0, 0.0, 0.0, y, 0.0, 0.0, 1.0;

4
gtsam/geometry/Cal3_S2.h
View File

@ -148,8 +148,8 @@ public:
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
* @return point in image coordinates
*/
Point2 uncalibrate(const Point2& p, FixedRef<2,5> Dcal = boost::none,
FixedRef<2,2> Dp = boost::none) const;
Point2 uncalibrate(const Point2& p, OptionalJacobian<2,5> Dcal = boost::none,
OptionalJacobian<2,2> Dp = boost::none) const;
/**
* convert image coordinates uv to intrinsic coordinates xy

18
gtsam/geometry/PinholeCamera.h
View File

@ -274,7 +274,7 @@ public:
* @param Dpoint is the 2*3 Jacobian w.r.t. P
*/
static Point2 project_to_camera(const Point3& P, //
FixedRef<2, 3> Dpoint = boost::none) {
OptionalJacobian<2, 3> Dpoint = boost::none) {
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
if (P.z() <= 0)
throw CheiralityException();
@ -303,9 +303,9 @@ public:
*/
inline Point2 project(
const Point3& pw,
FixedRef<2,6> Dpose = boost::none,
FixedRef<2,3> Dpoint = boost::none,
FixedRef<2,DimK> Dcal = boost::none) const {
OptionalJacobian<2,6> Dpose = boost::none,
OptionalJacobian<2,3> Dpoint = boost::none,
OptionalJacobian<2,DimK> Dcal = boost::none) const {
// Transform to camera coordinates and check cheirality
const Point3 pc = pose_.transform_to(pw);
@ -338,9 +338,9 @@ public:
*/
inline Point2 projectPointAtInfinity(
const Point3& pw,
FixedRef<2,6> Dpose = boost::none,
FixedRef<2,2> Dpoint = boost::none,
FixedRef<2,DimK> Dcal = boost::none) const {
OptionalJacobian<2,6> Dpose = boost::none,
OptionalJacobian<2,2> Dpoint = boost::none,
OptionalJacobian<2,DimK> Dcal = boost::none) const {
if (!Dpose && !Dpoint && !Dcal) {
const Point3 pc = pose_.rotation().unrotate(pw); // get direction in camera frame (translation does not matter)
@ -378,8 +378,8 @@ public:
* @param Dpoint is the Jacobian w.r.t. point3
*/
Point2 project2(const Point3& pw, //
FixedRef<2, 6 + DimK> Dcamera = boost::none,
FixedRef<2, 3> Dpoint = boost::none) const {
OptionalJacobian<2, 6 + DimK> Dcamera = boost::none,
OptionalJacobian<2, 3> Dpoint = boost::none) const {
const Point3 pc = pose_.transform_to(pw);
const Point2 pn = project_to_camera(pc);

16
gtsam/geometry/Point2.cpp
View File

@ -38,15 +38,9 @@ bool Point2::equals(const Point2& q, double tol) const {
}
/* ************************************************************************* */
double Point2::norm() const {
return sqrt(x_ * x_ + y_ * y_);
}
/* ************************************************************************* */
double Point2::norm(boost::optional<Matrix&> H) const {
double r = norm();
double Point2::norm(OptionalJacobian<1,2> H) const {
double r = sqrt(x_ * x_ + y_ * y_);
if (H) {
H->resize(1,2);
if (fabs(r) > 1e-10)
*H << x_ / r, y_ / r;
else
@ -57,11 +51,11 @@ double Point2::norm(boost::optional<Matrix&> H) const {
/* ************************************************************************* */
static const Matrix I2 = eye(2);
double Point2::distance(const Point2& point, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
double Point2::distance(const Point2& point, OptionalJacobian<1,2> H1,
OptionalJacobian<1,2> H2) const {
Point2 d = point - *this;
if (H1 || H2) {
Matrix H;
Eigen::Matrix<double,1,2> H;
double r = d.norm(H);
if (H1) *H1 = -H;
if (H2) *H2 = H;

27
gtsam/geometry/Point2.h
View File

@ -20,7 +20,7 @@
#include <boost/serialization/nvp.hpp>
#include <gtsam/base/DerivedValue.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/OptionalJacobian.h>
#include <gtsam/base/Lie.h>
namespace gtsam {
@ -125,10 +125,10 @@ public:
/// "Compose", just adds the coordinates of two points. With optional derivatives
inline Point2 compose(const Point2& q,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const {
if(H1) *H1 = eye(2);
if(H2) *H2 = eye(2);
OptionalJacobian<2,2> H1=boost::none,
OptionalJacobian<2,2> H2=boost::none) const {
if(H1) *H1 = Eigen::Matrix2d::Identity();
if(H2) *H2 = Eigen::Matrix2d::Identity();
return *this + q;
}
@ -137,10 +137,10 @@ public:
/// "Between", subtracts point coordinates. between(p,q) == compose(inverse(p),q)
inline Point2 between(const Point2& q,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const {
if(H1) *H1 = -eye(2);
if(H2) *H2 = eye(2);
OptionalJacobian<2,2> H1=boost::none,
OptionalJacobian<2,2> H2=boost::none) const {
if(H1) *H1 = -Eigen::Matrix2d::Identity();
if(H2) *H2 = Eigen::Matrix2d::Identity();
return q - (*this);
}
@ -180,15 +180,12 @@ public:
/** creates a unit vector */
Point2 unit() const { return *this/norm(); }
/** norm of point */
double norm() const;
/** norm of point, with derivative */
double norm(boost::optional<Matrix&> H) const;
double norm(OptionalJacobian<1,2> H = boost::none) const;
/** distance between two points */
double distance(const Point2& p2, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const;
double distance(const Point2& p2, OptionalJacobian<1,2> H1 = boost::none,
OptionalJacobian<1,2> H2 = boost::none) const;
/** @deprecated The following function has been deprecated, use distance above */
inline double dist(const Point2& p2) const {

2
gtsam/geometry/Point3.cpp
View File

@ -94,7 +94,7 @@ double Point3::dot(const Point3 &q) const {
}
/* ************************************************************************* */
double Point3::norm(FixedRef<1,3> H) const {
double Point3::norm(OptionalJacobian<1,3> H) const {
double r = sqrt(x_ * x_ + y_ * y_ + z_ * z_);
if (H) {
if (fabs(r) > 1e-10)

4
gtsam/geometry/Point3.h
View File

@ -21,9 +21,9 @@
#pragma once
#include <gtsam/base/Matrix.h>
#include <gtsam/base/DerivedValue.h>
#include <gtsam/base/Lie.h>
#include <gtsam/base/OptionalJacobian.h>
#include <boost/serialization/nvp.hpp>
@ -181,7 +181,7 @@ namespace gtsam {
}
/** Distance of the point from the origin, with Jacobian */
double norm(FixedRef<1,3> H = boost::none) const;
double norm(OptionalJacobian<1,3> H = boost::none) const;
/** normalize, with optional Jacobian */
Point3 normalize(boost::optional<Matrix&> H = boost::none) const;

4
gtsam/geometry/Pose2.cpp
View File

@ -126,7 +126,7 @@ Pose2 Pose2::inverse(boost::optional<Matrix&> H1) const {
/* ************************************************************************* */
// see doc/math.lyx, SE(2) section
Point2 Pose2::transform_to(const Point2& point,
FixedRef<2, 3> H1, FixedRef<2, 2> H2) const {
OptionalJacobian<2, 3> H1, OptionalJacobian<2, 2> H2) const {
Point2 d = point - t_;
Point2 q = r_.unrotate(d);
if (!H1 && !H2) return q;
@ -150,7 +150,7 @@ Pose2 Pose2::compose(const Pose2& p2, boost::optional<Matrix&> H1,
/* ************************************************************************* */
// see doc/math.lyx, SE(2) section
Point2 Pose2::transform_from(const Point2& p,
FixedRef<2, 3> H1, FixedRef<2, 2> H2) const {
OptionalJacobian<2, 3> H1, OptionalJacobian<2, 2> H2) const {
const Point2 q = r_ * p;
if (H1 || H2) {
const Matrix R = r_.matrix();

8
gtsam/geometry/Pose2.h
View File

@ -192,13 +192,13 @@ public:
/** Return point coordinates in pose coordinate frame */
Point2 transform_to(const Point2& point,
FixedRef<2, 3> H1 = boost::none,
FixedRef<2, 2> H2 = boost::none) const;
OptionalJacobian<2, 3> H1 = boost::none,
OptionalJacobian<2, 2> H2 = boost::none) const;
/** Return point coordinates in global frame */
Point2 transform_from(const Point2& point,
FixedRef<2, 3> H1 = boost::none,
FixedRef<2, 2> H2 = boost::none) const;
OptionalJacobian<2, 3> H1 = boost::none,
OptionalJacobian<2, 2> H2 = boost::none) const;
/** syntactic sugar for transform_from */
inline Point2 operator*(const Point2& point) const { return transform_from(point);}

4
gtsam/geometry/Pose3.cpp
View File

@ -254,8 +254,8 @@ Point3 Pose3::transform_from(const Point3& p, boost::optional<Matrix&> Dpose,
}
/* ************************************************************************* */
Point3 Pose3::transform_to(const Point3& p, FixedRef<3,6> Dpose,
FixedRef<3,3> Dpoint) const {
Point3 Pose3::transform_to(const Point3& p, OptionalJacobian<3,6> Dpose,
OptionalJacobian<3,3> Dpoint) const {
// Only get transpose once, to avoid multiple allocations,
// as well as multiple conversions in the Quaternion case
const Matrix3 Rt = R_.transpose();

4
gtsam/geometry/Pose3.h
View File

@ -250,8 +250,8 @@ public:
* @return point in Pose coordinates
*/
Point3 transform_to(const Point3& p,
FixedRef<3,6> Dpose = boost::none,
FixedRef<3,3> Dpoint = boost::none) const;
OptionalJacobian<3,6> Dpose = boost::none,
OptionalJacobian<3,3> Dpoint = boost::none) const;
/// @}
/// @name Standard Interface

4
gtsam/geometry/Rot3.h
View File

@ -218,8 +218,8 @@ namespace gtsam {
Rot3 inverse(boost::optional<Matrix&> H1=boost::none) const;
/// Compose two rotations i.e., R= (*this) * R2
Rot3 compose(const Rot3& R2, FixedRef<3, 3> H1 = boost::none,
FixedRef<3, 3> H2 = boost::none) const;
Rot3 compose(const Rot3& R2, OptionalJacobian<3, 3> H1 = boost::none,
OptionalJacobian<3, 3> H2 = boost::none) const;
/** compose two rotations */
Rot3 operator*(const Rot3& R2) const;

2
gtsam/geometry/Rot3M.cpp
View File

@ -142,7 +142,7 @@ Rot3 Rot3::rodriguez(const Vector& w, double theta) {
}
/* ************************************************************************* */
Rot3 Rot3::compose(const Rot3& R2, FixedRef<3, 3> H1, FixedRef<3, 3> H2) const {
Rot3 Rot3::compose(const Rot3& R2, OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) const {
if (H1)
*H1 = R2.transpose();
if (H2)

62
gtsam/nonlinear/Expression-inl.h
View File

@ -23,6 +23,7 @@
#include <gtsam/nonlinear/Values.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/Manifold.h>
#include <gtsam/base/OptionalJacobian.h>
#include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp>
@ -42,21 +43,21 @@ namespace gtsam {
const unsigned TraceAlignment = 16;
template <typename T>
T & upAlign(T & value, unsigned requiredAlignment = TraceAlignment){
template<typename T>
T & upAlign(T & value, unsigned requiredAlignment = TraceAlignment) {
// right now only word sized types are supported.
// Easy to extend if needed,
// by somehow inferring the unsigned integer of same size
BOOST_STATIC_ASSERT(sizeof(T) == sizeof(size_t));
size_t & uiValue = reinterpret_cast<size_t &>(value);
size_t misAlignment = uiValue % requiredAlignment;
if(misAlignment) {
if (misAlignment) {
uiValue += requiredAlignment - misAlignment;
}
return value;
}
template <typename T>
T upAligned(T value, unsigned requiredAlignment = TraceAlignment){
template<typename T>
T upAligned(T value, unsigned requiredAlignment = TraceAlignment) {
return upAlign(value, requiredAlignment);
}
@ -88,19 +89,21 @@ public:
namespace internal {
template <bool UseBlock, typename Derived>
template<bool UseBlock, typename Derived>
struct UseBlockIf {
static void addToJacobian(const Eigen::MatrixBase<Derived>& dTdA,
JacobianMap& jacobians, Key key){
JacobianMap& jacobians, Key key) {
// block makes HUGE difference
jacobians(key).block<Derived::RowsAtCompileTime, Derived::ColsAtCompileTime>(0, 0) += dTdA;
};
jacobians(key).block<Derived::RowsAtCompileTime, Derived::ColsAtCompileTime>(
0, 0) += dTdA;
}
;
};
/// Handle Leaf Case for Dynamic Matrix type (slower)
template <typename Derived>
template<typename Derived>
struct UseBlockIf<false, Derived> {
static void addToJacobian(const Eigen::MatrixBase<Derived>& dTdA,
JacobianMap& jacobians, Key key) {
JacobianMap& jacobians, Key key) {
jacobians(key) += dTdA;
}
};
@ -111,10 +114,9 @@ template<typename Derived>
void handleLeafCase(const Eigen::MatrixBase<Derived>& dTdA,
JacobianMap& jacobians, Key key) {
internal::UseBlockIf<
Derived::RowsAtCompileTime != Eigen::Dynamic &&
Derived::ColsAtCompileTime != Eigen::Dynamic,
Derived>
::addToJacobian(dTdA, jacobians, key);
Derived::RowsAtCompileTime != Eigen::Dynamic
&& Derived::ColsAtCompileTime != Eigen::Dynamic, Derived>::addToJacobian(
dTdA, jacobians, key);
}
//-----------------------------------------------------------------------------
@ -265,7 +267,7 @@ public:
/// Construct an execution trace for reverse AD
virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace,
ExecutionTraceStorage* traceStorage) const = 0;
ExecutionTraceStorage* traceStorage) const = 0;
};
//-----------------------------------------------------------------------------
@ -336,7 +338,7 @@ public:
/// Construct an execution trace for reverse AD
virtual const value_type& traceExecution(const Values& values,
ExecutionTrace<value_type>& trace,
ExecutionTrace<value_type>& trace,
ExecutionTraceStorage* traceStorage) const {
trace.setLeaf(key_);
return dynamic_cast<const value_type&>(values.at(key_));
@ -454,8 +456,9 @@ struct Jacobian {
/// meta-function to generate JacobianTA optional reference
template<class T, class A>
struct OptionalJacobian {
typedef FixedRef<traits::dimension<T>::value, traits::dimension<A>::value> type;
struct MakeOptionalJacobian {
typedef OptionalJacobian<traits::dimension<T>::value,
traits::dimension<A>::value> type;
};
/**
@ -556,7 +559,7 @@ struct GenerateFunctionalNode: Argument<T, A, Base::N + 1>, Base {
};
/// Construct an execution trace for reverse AD
void trace(const Values& values, Record* record,
void trace(const Values& values, Record* record,
ExecutionTraceStorage*& traceStorage) const {
Base::trace(values, record, traceStorage); // recurse
// Write an Expression<A> execution trace in record->trace
@ -632,7 +635,7 @@ struct FunctionalNode {
};
/// Construct an execution trace for reverse AD
Record* trace(const Values& values,
Record* trace(const Values& values,
ExecutionTraceStorage* traceStorage) const {
assert(reinterpret_cast<size_t>(traceStorage) % TraceAlignment == 0);
@ -657,7 +660,8 @@ class UnaryExpression: public FunctionalNode<T, boost::mpl::vector<A1> >::type {
public:
typedef boost::function<T(const A1&, typename OptionalJacobian<T, A1>::type)> Function;
typedef boost::function<
T(const A1&, typename MakeOptionalJacobian<T, A1>::type)> Function;
typedef typename FunctionalNode<T, boost::mpl::vector<A1> >::type Base;
typedef typename Base::Record Record;
@ -702,8 +706,8 @@ class BinaryExpression: public FunctionalNode<T, boost::mpl::vector<A1, A2> >::t
public:
typedef boost::function<
T(const A1&, const A2&, typename OptionalJacobian<T, A1>::type,
typename OptionalJacobian<T, A2>::type)> Function;
T(const A1&, const A2&, typename MakeOptionalJacobian<T, A1>::type,
typename MakeOptionalJacobian<T, A2>::type)> Function;
typedef typename FunctionalNode<T, boost::mpl::vector<A1, A2> >::type Base;
typedef typename Base::Record Record;
@ -756,9 +760,10 @@ class TernaryExpression: public FunctionalNode<T, boost::mpl::vector<A1, A2, A3>
public:
typedef boost::function<
T(const A1&, const A2&, const A3&, typename OptionalJacobian<T, A1>::type,
typename OptionalJacobian<T, A2>::type,
typename OptionalJacobian<T, A3>::type)> Function;
T(const A1&, const A2&, const A3&,
typename MakeOptionalJacobian<T, A1>::type,
typename MakeOptionalJacobian<T, A2>::type,
typename MakeOptionalJacobian<T, A3>::type)> Function;
typedef typename FunctionalNode<T, boost::mpl::vector<A1, A2, A3> >::type Base;
typedef typename Base::Record Record;
@ -774,7 +779,8 @@ private:
this->template reset<A2, 2>(e2.root());
this->template reset<A3, 3>(e3.root());
ExpressionNode<T>::traceSize_ = //
upAligned(sizeof(Record)) + e1.traceSize() + e2.traceSize() + e3.traceSize();
upAligned(sizeof(Record)) + e1.traceSize() + e2.traceSize()
+ e3.traceSize();
}
friend class Expression<T> ;

12
gtsam/nonlinear/Expression.h
View File

@ -20,8 +20,8 @@
#pragma once
#include <gtsam/nonlinear/Expression-inl.h>
#include <gtsam/base/FastVector.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/base/FastVector.h>
#include <boost/bind.hpp>
#include <boost/range/adaptor/map.hpp>
@ -75,7 +75,7 @@ public:
/// Construct a nullary method expression
template<typename A>
Expression(const Expression<A>& expression,
T (A::*method)(typename OptionalJacobian<T, A>::type) const) :
T (A::*method)(typename MakeOptionalJacobian<T, A>::type) const) :
root_(new UnaryExpression<T, A>(boost::bind(method, _1, _2), expression)) {
}
@ -89,8 +89,8 @@ public:
/// Construct a unary method expression
template<typename A1, typename A2>
Expression(const Expression<A1>& expression1,
T (A1::*method)(const A2&, typename OptionalJacobian<T, A1>::type,
typename OptionalJacobian<T, A2>::type) const,
T (A1::*method)(const A2&, typename MakeOptionalJacobian<T, A1>::type,
typename MakeOptionalJacobian<T, A2>::type) const,
const Expression<A2>& expression2) :
root_(
new BinaryExpression<T, A1, A2>(boost::bind(method, _1, _2, _3, _4),
@ -224,8 +224,8 @@ template<class T>
struct apply_compose {
typedef T result_type;
static const int Dim = traits::dimension<T>::value;
T operator()(const T& x, const T& y, FixedRef<Dim, Dim> H1 = boost::none,
FixedRef<Dim, Dim> H2 = boost::none) const {
T operator()(const T& x, const T& y, OptionalJacobian<Dim, Dim> H1 =
boost::none, OptionalJacobian<Dim, Dim> H2 = boost::none) const {
return x.compose(y, H1, H2);
}
};

6
gtsam/nonlinear/NonlinearFactor.h
View File

@ -20,14 +20,14 @@
#pragma once
#include <boost/serialization/base_object.hpp>
#include <boost/assign/list_of.hpp>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/inference/Factor.h>
#include <gtsam/base/OptionalJacobian.h>
#include <boost/serialization/base_object.hpp>
#include <boost/assign/list_of.hpp>
/**
* Macro to add a standard clone function to a derived factor

14
gtsam/nonlinear/tests/testExpression.cpp
View File

@ -34,8 +34,8 @@ using namespace gtsam;
/* ************************************************************************* */
template<class CAL>
Point2 uncalibrate(const CAL& K, const Point2& p, FixedRef<2, 5> Dcal,
FixedRef<2, 2> Dp) {
Point2 uncalibrate(const CAL& K, const Point2& p, OptionalJacobian<2, 5> Dcal,
OptionalJacobian<2, 2> Dp) {
return K.uncalibrate(p, Dcal, Dp);
}
@ -75,13 +75,13 @@ TEST(Expression, Leaves) {
/* ************************************************************************* */
// Unary(Leaf)
namespace unary {
Point2 f0(const Point3& p, FixedRef<2,3> H) {
Point2 f0(const Point3& p, OptionalJacobian<2,3> H) {
return Point2();
}
LieScalar f1(const Point3& p, FixedRef<1, 3> H) {
LieScalar f1(const Point3& p, OptionalJacobian<1, 3> H) {
return LieScalar(0.0);
}
double f2(const Point3& p, FixedRef<1, 3> H) {
double f2(const Point3& p, OptionalJacobian<1, 3> H) {
return 0.0;
}
Expression<Point3> p(1);
@ -134,7 +134,7 @@ TEST(Expression, NullaryMethod) {
namespace binary {
// Create leaves
double doubleF(const Pose3& pose, //
const Point3& point, FixedRef<1, 6> H1, FixedRef<1, 3> H2) {
const Point3& point, OptionalJacobian<1, 6> H1, OptionalJacobian<1, 3> H2) {
return 0.0;
}
Expression<Pose3> x(1);
@ -243,7 +243,7 @@ TEST(Expression, compose3) {
/* ************************************************************************* */
// Test with ternary function
Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3,
FixedRef<3, 3> H1, FixedRef<3, 3> H2, FixedRef<3, 3> H3) {
OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2, OptionalJacobian<3, 3> H3) {
// return dummy derivatives (not correct, but that's ok for testing here)
if (H1)
*H1 = eye(3);

5
gtsam_unstable/nonlinear/AdaptAutoDiff.h
View File

@ -20,6 +20,7 @@
#include <gtsam_unstable/nonlinear/ceres_autodiff.h>
#include <gtsam/base/Manifold.h>
#include <gtsam/base/OptionalJacobian.h>
namespace gtsam {
@ -50,8 +51,8 @@ class AdaptAutoDiff {
public:
T operator()(const A1& a1, const A2& a2, FixedRef<N, M1> H1 = boost::none,
FixedRef<N, M2> H2 = boost::none) {
T operator()(const A1& a1, const A2& a2, OptionalJacobian<N, M1> H1 = boost::none,
OptionalJacobian<N, M2> H2 = boost::none) {
using ceres::internal::AutoDiff;

2
gtsam_unstable/nonlinear/tests/testBasisDecompositions.cpp
View File

@ -60,7 +60,7 @@ public:
}
/// Given coefficients c, predict value for x
double operator()(const Coefficients& c, FixedRef<1, N> H) {
double operator()(const Coefficients& c, OptionalJacobian<1, N> H) {
if (H)
(*H) = H_;
return H_ * c;

4
gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp
View File

@ -126,7 +126,7 @@ TEST(ExpressionFactor, Unary) {
/* ************************************************************************* */
static Point2 myUncal(const Cal3_S2& K, const Point2& p,
FixedRef<2,5> Dcal, FixedRef<2,2> Dp) {
OptionalJacobian<2,5> Dcal, OptionalJacobian<2,2> Dp) {
return K.uncalibrate(p, Dcal, Dp);
}
@ -392,7 +392,7 @@ TEST(ExpressionFactor, compose3) {
/* ************************************************************************* */
// Test compose with three arguments
Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3,
FixedRef<3, 3> H1, FixedRef<3, 3> H2, FixedRef<3, 3> H3) {
OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2, OptionalJacobian<3, 3> H3) {
// return dummy derivatives (not correct, but that's ok for testing here)
if (H1)
*H1 = eye(3);

6
gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp
View File

@ -201,7 +201,7 @@ TEST(ExpressionFactor, InvokeDerivatives) {
// Now, let's create the optional Jacobian arguments
typedef Point3 T;
typedef boost::mpl::vector<Pose3, Point3> TYPES;
typedef boost::mpl::transform<TYPES, OptionalJacobian<T, MPL::_1> >::type Optionals;
typedef boost::mpl::transform<TYPES, MakeOptionalJacobian<T, MPL::_1> >::type Optionals;
// Unfortunately this is moot: we need a pointer to a function with the
// optional derivatives; I don't see a way of calling a function that we
@ -215,8 +215,8 @@ struct proxy {
return pose.transform_to(point);
}
Point3 operator()(const Pose3& pose, const Point3& point,
FixedRef<3,6> Dpose,
FixedRef<3,3> Dpoint) const {
OptionalJacobian<3,6> Dpose,
OptionalJacobian<3,3> Dpoint) const {
return pose.transform_to(point, Dpose, Dpoint);
}
};

10
gtsam_unstable/slam/expressions.h
View File

@ -20,8 +20,8 @@ typedef Expression<Rot2> Rot2_;
typedef Expression<Pose2> Pose2_;
Point2_ transform_to(const Pose2_& x, const Point2_& p) {
Point2 (Pose2::*transform)(const Point2& p, FixedRef<2, 3> H1,
FixedRef<2, 2> H2) const = &Pose2::transform_to;
Point2 (Pose2::*transform)(const Point2& p, OptionalJacobian<2, 3> H1,
OptionalJacobian<2, 2> H2) const = &Pose2::transform_to;
return Point2_(x, transform, p);
}
@ -34,8 +34,8 @@ typedef Expression<Pose3> Pose3_;
Point3_ transform_to(const Pose3_& x, const Point3_& p) {
Point3 (Pose3::*transform)(const Point3& p, FixedRef<3, 6> Dpose,
FixedRef<3, 3> Dpoint) const = &Pose3::transform_to;
Point3 (Pose3::*transform)(const Point3& p, OptionalJacobian<3, 6> Dpose,
OptionalJacobian<3, 3> Dpoint) const = &Pose3::transform_to;
return Point3_(x, transform, p);
}
@ -49,7 +49,7 @@ Point2_ project(const Point3_& p_cam) {
}
Point2 project6(const Pose3& x, const Point3& p, const Cal3_S2& K,
FixedRef<2, 6> Dpose, FixedRef<2, 3> Dpoint, FixedRef<2, 5> Dcal) {
OptionalJacobian<2, 6> Dpose, OptionalJacobian<2, 3> Dpoint, OptionalJacobian<2, 5> Dcal) {
return PinholeCamera<Cal3_S2>(x, K).project(p, Dpose, Dpoint, Dcal);
}