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

Binary functions now take fixed Jacobians

release/4.3a0
dellaert 2014-10-07 01:01:46 +02:00
parent 921b79f446
commit 613cb0bb12
4 changed files with 23 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
gtsam_unstable/nonlinear/Expression-inl.h
View File

@ -386,8 +386,8 @@ public:
typedef Eigen::Matrix<double,T::dimension,A1::dimension> JacobianTA1;
typedef Eigen::Matrix<double,T::dimension,A2::dimension> JacobianTA2;
typedef boost::function<
T(const A1&, const A2&, boost::optional<Matrix&>,
boost::optional<Matrix&>)> Function;
T(const A1&, const A2&, boost::optional<JacobianTA1&>,
boost::optional<JacobianTA2&>)> Function;
private:
@ -429,10 +429,11 @@ public:
using boost::none;
Augmented<A1> a1 = this->expressionA1_->forward(values);
Augmented<A2> a2 = this->expressionA2_->forward(values);
Matrix dTdA1, dTdA2;
JacobianTA1 dTdA1;
JacobianTA2 dTdA2;
T t = function_(a1.value(), a2.value(),
a1.constant() ? none : boost::optional<Matrix&>(dTdA1),
a2.constant() ? none : boost::optional<Matrix&>(dTdA2));
a1.constant() ? none : boost::optional<JacobianTA1&>(dTdA1),
a2.constant() ? none : boost::optional<JacobianTA2&>(dTdA2));
return Augmented<T>(t, dTdA1, a1.jacobians(), dTdA2, a2.jacobians());
}
@ -440,7 +441,8 @@ public:
struct Trace: public JacobianTrace<T> {
boost::shared_ptr<JacobianTrace<A1> > trace1;
boost::shared_ptr<JacobianTrace<A2> > trace2;
Matrix dTdA1, dTdA2;
JacobianTA1 dTdA1;
JacobianTA2 dTdA2;
/// Start the reverse AD process
virtual void reverseAD(JacobianMap& jacobians) const {
trace1->reverseAD(dTdA1, jacobians);

17
gtsam_unstable/nonlinear/Expression.h
View File

@ -76,8 +76,10 @@ public:
/// Construct a unary method expression
template<typename A1, typename A2>
Expression(const Expression<A1>& expression1,
T (A1::*method)(const A2&, boost::optional<Matrix&>,
boost::optional<Matrix&>) const, const Expression<A2>& expression2) {
T (A1::*method)(const A2&,
boost::optional<typename BinaryExpression<T, A1, A2>::JacobianTA1&>,
boost::optional<typename BinaryExpression<T, A1, A2>::JacobianTA2&>) const,
const Expression<A2>& expression2) {
root_.reset(
new BinaryExpression<T, A1, A2>(boost::bind(method, _1, _2, _3, _4),
expression1, expression2));
@ -94,9 +96,11 @@ public:
/// Construct a ternary function expression
template<typename A1, typename A2, typename A3>
Expression(typename TernaryExpression<T, A1, A2, A3>::Function function,
const Expression<A1>& expression1, const Expression<A2>& expression2, const Expression<A3>& expression3) {
const Expression<A1>& expression1, const Expression<A2>& expression2,
const Expression<A3>& expression3) {
root_.reset(
new TernaryExpression<T, A1, A2, A3>(function, expression1, expression2, expression3));
new TernaryExpression<T, A1, A2, A3>(function, expression1, expression2,
expression3));
}
/// Return keys that play in this expression
@ -132,8 +136,9 @@ public:
template<class T>
struct apply_compose {
typedef T result_type;
T operator()(const T& x, const T& y, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
typedef Eigen::Matrix<double, T::dimension, T::dimension> Jacobian;
T operator()(const T& x, const T& y, boost::optional<Jacobian&> H1,
boost::optional<Jacobian&> H2) const {
return x.compose(y, H1, H2);
}
};

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

@ -32,8 +32,8 @@ using namespace gtsam;
/* ************************************************************************* */
template<class CAL>
Point2 uncalibrate(const CAL& K, const Point2& p, boost::optional<Matrix&> Dcal,
boost::optional<Matrix&> Dp) {
Point2 uncalibrate(const CAL& K, const Point2& p, boost::optional<Matrix25&> Dcal,
boost::optional<Matrix2&> Dp) {
return K.uncalibrate(p, Dcal, Dp);
}

4
gtsam_unstable/timing/timeCameraExpression.cpp
View File

@ -47,9 +47,9 @@ void time(const NonlinearFactor& f, const Values& values) {
boost::shared_ptr<Cal3_S2> fixedK(new Cal3_S2());
Point2 myProject(const Pose3& pose, const Point3& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
boost::optional<Matrix26&> H1, boost::optional<Matrix23&> H2) {
PinholeCamera<Cal3_S2> camera(pose, *fixedK);
return camera.project(point, H1, H2);
return camera.project(point, H1, H2, boost::none);
}
int main() {