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

17
gtsam_unstable/nonlinear/Expression.h
View File

@ -76,8 +76,10 @@ public:
/// Construct a unary method expression /// Construct a unary method expression
template<typename A1, typename A2> template<typename A1, typename A2>
Expression(const Expression<A1>& expression1, Expression(const Expression<A1>& expression1,
T (A1::*method)(const A2&, boost::optional<Matrix&>, T (A1::*method)(const A2&,
boost::optional<Matrix&>) const, const Expression<A2>& expression2) { boost::optional<typename BinaryExpression<T, A1, A2>::JacobianTA1&>,
boost::optional<typename BinaryExpression<T, A1, A2>::JacobianTA2&>) const,
const Expression<A2>& expression2) {
root_.reset( root_.reset(
new BinaryExpression<T, A1, A2>(boost::bind(method, _1, _2, _3, _4), new BinaryExpression<T, A1, A2>(boost::bind(method, _1, _2, _3, _4),
expression1, expression2)); expression1, expression2));
@ -94,9 +96,11 @@ public:
/// Construct a ternary function expression /// Construct a ternary function expression
template<typename A1, typename A2, typename A3> template<typename A1, typename A2, typename A3>
Expression(typename TernaryExpression<T, A1, A2, A3>::Function function, 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( 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 /// Return keys that play in this expression
@ -132,8 +136,9 @@ public:
template<class T> template<class T>
struct apply_compose { struct apply_compose {
typedef T result_type; typedef T result_type;
T operator()(const T& x, const T& y, boost::optional<Matrix&> H1, typedef Eigen::Matrix<double, T::dimension, T::dimension> Jacobian;
boost::optional<Matrix&> H2) const { T operator()(const T& x, const T& y, boost::optional<Jacobian&> H1,
boost::optional<Jacobian&> H2) const {
return x.compose(y, H1, H2); 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> template<class CAL>
Point2 uncalibrate(const CAL& K, const Point2& p, boost::optional<Matrix&> Dcal, Point2 uncalibrate(const CAL& K, const Point2& p, boost::optional<Matrix25&> Dcal,
boost::optional<Matrix&> Dp) { boost::optional<Matrix2&> Dp) {
return K.uncalibrate(p, Dcal, 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()); boost::shared_ptr<Cal3_S2> fixedK(new Cal3_S2());
Point2 myProject(const Pose3& pose, const Point3& point, 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); PinholeCamera<Cal3_S2> camera(pose, *fixedK);
return camera.project(point, H1, H2); return camera.project(point, H1, H2, boost::none);
} }
int main() { int main() {