Drake gradient util

Pull request for C++ gradient utilities. I put general purpose functions like {get,set}SubMatrixGradient, transposeGrad, etc. in drakeGradientUtil, and functions that compute gradients of geometrical things in drakeGeometryUtil.
i've pushed the merge.  I believe that this discussion about auto was the
only other thing that we had left dangling.  Did you want to put a
note/cleanup issue to revisit it in other places of the code later?  Or are
we all set?


On Mon, Aug 11, 2014 at 9:17 PM, Twan Koolen <notifications@github.com>
wrote:

> In util/drakeGeometryUtil.cpp:
>
> > @@ -374,6 +374,197 @@ Eigen::Matrix<typename Derived::Scalar, 3, 3> rpy2rotmat(const Eigen::MatrixBase
> >    return R;
> >  }
> >
> > +template<typename Scalar, typename DerivedS, typename DerivedQdotToV>
> > +Eigen::Matrix<Scalar, HomogeneousTransformSize, DerivedQdotToV::ColsAtCompileTime> dHomogTrans(
> > +    const Eigen::Transform<Scalar, 3, Eigen::Isometry>& T,
> > +    const Eigen::MatrixBase<DerivedS>& S,
> > +    const Eigen::MatrixBase<DerivedQdotToV>& qdot_to_v) {
> > +  const int nq_at_compile_time = DerivedQdotToV::ColsAtCompileTime;
> > +  int nq = qdot_to_v.cols();
> > +  auto qdot_to_twist = S * qdot_to_v;
>
> This one is interesting. I have two main reasons for using auto a lot:
>
>    - the matrices could have fixed size, and I don't want to have to
>    figure out the exact dimensions of the PlainObject in which the result
>    would be stored. auto is easier to use for this reason. Another easy
>    solution is to use MatrixXd, but for small matrices, staying in
>    fixed-size land would be faster. This particular case is a border case
>    where the difference between fixed-size and dynamic size is small.
>    - I was under the impression that longer expressions should be more
>    efficient to evaluate than using a bunch of intermediate variables because
>    it allows Eigen to do lazy evaluation, combining expressions together to
>    e.g. minimize the number of times coefficients are looped through. This is
>    simulated by using auto here, since the actual return type of operator*
>    is a ProductReturnType, which doesn't do any real computation upon
>    construction. The real computation is done when a complicated expression is
>    finally assigned to a PlainObject. So I thought I was making things
>    more efficient by using auto.
>
> However, I just profiled after changing auto to MatrixXd, and computation
> time for representative inputs halved! I think what might be happening is
> that Eigen needs to evaluate the matrix to be able to use the row method
> a few lines later, and probably ends up having to evaluate the whole thing
> three times. Evidence for this is that when I use auto and get rid of the
> last row call (by setting qdot_to_omega_z equal to qdot_to_omega_x),
> computation time drops significantly, whereas computation time remains the
> same when I use MatrixXd for qdot_to_twist. So I guess the lesson here is
> to evaluate to a PlainObject before you use block methods.
>
> By the way, there is a way to get automatic compile time size inference
> while still evaluating to a PlainObject: by calling .eval(). I think this
> is what I'll do for such cases from now on.
>
> —
> Reply to this email directly or view it on GitHub
> <https://github.com/RobotLocomotion/drake/pull/283/files#r16091390>.
>