Hi. Bundle with AutoDiffCostFunction

[Hyon Lim]

Hi.

I'm working on bundle adjustment of monocular image sequences.

So far, it quite works well with reprojection error metric with angle-axis representation.

What basically ceres does is that pass updated pose vector (6D) in every interation to my evaluation function below

bool operator()(const T *const pose6d, const T *const point3d, T *residual) const

and returns residual.  

I'm using ceres::AngleAxisRotatePoint() to rotate my world point with given pose vector.

The problem I have is that I have other pose representation, but not with T data type.

I think I couldn't access the value of T directly (Jet type?)

It utilizes some function in the Eigen library.

What's the best way to do this?

In summary, I would like to use T as double.

-Hyon

[Sameer Agarwal]

Hi Hyon,

The problem I have is that I have other pose representation, but not with T data type.

Two ways around this.

1. Write your own templated implementation of the functionality you are using.

2. Eigen allows you to instantiate vectors and matrices with a templated type T, so for example you could do the following

bool operator()(const T *const pose6d, const T *const point3d, T *residual) const {

  Eigen::Vector<T, 3> angle_axis_rotation;

  angle_axis_rotation(0) = pose6d[0];

  angle_axis_rotation(1) = pose6d[1];

  angle_axis_rotation(2) = pose6d[2];

}

and then use angle_axis_rotation as an eigen vector.

In summary, I would like to use T as double.

T is not a double. T is a template parameter. Your template function is instantiated with Jet and double depending on whether Ceres is evaluating just residuals or residuals and derivatives. If you treat T as a double, then you will break the automatic differentiation code.

BTW what pose representation are you trying to use?

Sameer

 

-Hyon

--
--
----------------------------------------
Ceres Solver Google Group
http://groups.google.com/group/ceres-solver?hl=en?hl=en
 
---
You received this message because you are subscribed to the Google Groups "Ceres Solver" group.
To unsubscribe from this group and stop receiving emails from it, send an email to ceres-solver...@googlegroups.com.
For more options, visit https://groups.google.com/groups/opt_out.
 
 

[Hyon Lim]

Hi. This is Hyon.

I'm thinking to use Sim(3) which was proposed by H. Strasdat.

Have you implemented Sim(3)?

-Hyon

[Sameer Agarwal]

Hyon,

I have no idea what Sim(3) is.

Sameer

[Sameer Agarwal]

Hyon,

Sim(3) is just a scale + a rigid transformation. You could easily represent it as 7 scalars. 1 for scale, three for angle_axis and three for translation. This is easily implemented using the functions in rotation.h

Sameer

[Steven Lovegrove]

Hi Hyon,

I don't know if you're familiar with Hauke Strasdat's library for working with Lie Groups (Sophus: https://github.com/strasdat/Sophus), but I have been using it successfully with Ceres and like the abstraction it offers.

I've recently added some unit tests to my own development branch of Sophus which demonstrate using Sophus::SE3 with auto-diff, though Sophus::Sim3 should work similarly:

https://github.com/stevenlovegrove/Sophus/blob/develop/test/ceres/test_ceres_se3.cpp

The Sophus library supports use of Eigen::Map for wrapping the Scalar (double / Jet) arrays within Ceres. The file local_parameterization_se3.hpp also implements the appropriate update step for SE3 parameters (internally stored as quaternion+translation). A Sim(3) local parameterization would look similar. 

Steve

[Sameer Agarwal]

Hi Steven,

The Sophus library looks very cool. 

I took a quick look and a couple of things stood out to me.

1. It would help if Ceres had support for AutoDiffLocalParameterization. Keir already has some code for this  in blender, we should pull it into Ceres. It will make the Jacobian computation SE3 much simpler.

2. In the code fragment

    problem.AddParameterBlock(T_wr.data(), 7, new LocalParameterizationSe3);

    CostFunction* cost_function =

        new AutoDiffCostFunction<TestCostFunctor, 6, 7>(
            new TestCostFunctor(T_w_targ.inverse())
        );
    problem.AddResidualBlock(cost_function, NULL, T_wr.data());


and then

template<typename T>
    bool operator()( const T* const sT_wa, T* sResiduals ) const
    {
        const Eigen::Map<const Sophus::SE3Group<T> > T_wa(sT_wa);
        Eigen::Map<Eigen::Matrix<T,6,1> > residuals(sResiduals);
        

        residuals = (T_aw.cast<T>() * T_wa).log();
        return true;
    }



There is still two things which are error prone. One is the specification of dimensions. The user needs to know the size of the SE3 internal parameterization. I think the SE3Group class should expose that. Also the local parameterization for SE3 should also come from the group itself. 

I was also wondering if you had any ideas about eliminating/automating the construction of T_wa from sT_wa and making it transparent to the user?

I'd be curious to hear what other things in the Ceres API can be improved/added to make geometrical problems like SLAM easier to handle.

Sameer

[Steven Lovegrove]

Hi Sameer,

It would be great to talk about this further. I think it would be quite possible to do this for types that support Eigen::Map and support the Eigen data() convention. Perhaps the biggest issue is communicating generally the number of parameters of the underlying representation and the local parameterisation as you say. Maybe the most general way would be to query these via some Ceres traits class that allow other as yet unknown types to added.

Steve

[Sameer Agarwal]

Hi Steven,

One of the patterns that Jim Roseborough has been using for constructing automatically differentiated cost functions is this. Suppose you have

struct Foo {

  template <typename T>

  bool operator()(const T* x, const T* y, T* residuals) const;

} 

and then

Problem problem;

problem.AddResidualBlock(

  new AutoDiffCostFunctor<Foo, kNumResiduals, kParam1, kParam2>(new Foo(...)),

  loss_function, 

  param_1, 

  param_2);

where when the user is trying to use the functor Foo they need to remember its sizing information. Instead do 

struct Foo {

  template <typename T>

  bool operator()(const T* x, const T* y, T* residuals) const;

  static CostFunction* Create(...) {

    return 

     new AutoDiffCostFunctor<Foo, kNumResiduals, kParam1, kParam2>(new Foo(...));

  }

}

and then

Problem problem;

problem.AddResidualBlock(Foo::Create(...), loss_function, param_1, param_2);

The user still needs to make sure that he/she is passing the right pointers, but the static sizing information about Foo is co-located with Foo.

I think a similar pattern could be used with various manifolds, where you could have something like

Manifold m;

problem.AddResidualBlock(m.data(), m.size(), Manifold:CreateLocalParameterization());

and similarly for autodiff functors, have a wrapper which does automatic serialization and de-serialization of the raw arrays of T into the corresponding manifold and back. To make it backward compatible, we could have a Euclidean manifold which just puts T* into a bounds checked array.

But this is all API stuff. The reason to do all this would be if we can 

1. leverage existing code or write better code to work with points on these manifolds -- along the lines of Sophus.

2. Have either analytical or automatic derivatives for the exponential and logarithm maps.

Sameer

[Adrian Haarbach]

Hi all,

What is the current status of Sophus SE3 integration into Ceres?

In Steven's github code from above (moved to https://github.com/stevenlovegrove/Sophus/blob/master/test/ceres/local_parameterization_se3.hpp and https://github.com/stevenlovegrove/Sophus/blob/master/test/ceres/local_parameterization_se3.hpp), the Jacobian is computed analytically, but isn't it also possible to use it with AutoDiffLocalParameterization? It seems that Steven even contributed a lot to Sophus, as his fork https://github.com/stevenlovegrove/Sophus/ is 30 commits ahead of https://github.com/strasdat/Sophus, so which version of Sophus to use with Ceres?

Is it possible to autodiff SE3 like this?:

struct SophusSE3Plus{

    template<typename T>

    bool operator()(const T* x_raw, const T* delta_raw, T* x_plus_delta_raw) const {

        const Eigen::Map< const Sophus::SE3Group<T> > x(x_raw);

        const Eigen::Map< const Eigen::Matrix<T,6,1> > delta(delta_raw);

        Eigen::Map< Sophus::SE3Group<T> > x_plus_delta(x_plus_delta_raw);

        x_plus_delta = x * Sophus::SE3Group<T>::exp(delta);

        return true;

      }

};

LocalParameterization *se3_parameterization = new AutoDiffLocalParameterization<SophusSE3Plus,Sophus::SE3::num_parameters, Sophus::SE3::DoF>;

I am currently investigating different libraries (ceres, g2o), local parameterizations (Lie-algebra twists, quaternions, angle-axis) (using ceres' rotation.h, <Eigen/Geometry>, Sophus) as well as different ways to calculate derivatives (analytic, numeric, automatic :-) ) for the problem of minimizing 3D Alignment errors in multi-view LM-ICP pose graph optimization.

I have previously been using the g2o library (https://github.com/RainerKuemmerle/g2o) and GICP edges (https://github.com/RainerKuemmerle/g2o/blob/master/g2o/types/icp/types_icp.cpp) to model the point-to-point as well as point-to-plane ICP distance metrics and minimizing them on a graph, but I was not convinced that g2o is actually computing the right thing and using the correct derivatives, because my global error increases first before it gets decreased.

Then I heard about automatic derivatives in Ceres, I am even happier to hear that autodiff works for Local Parameterizations as well. I would be glad if anyone could share his experience

Adrian

[Sameer Agarwal]

Hi Adrian,

My replies are inline.

On Thu, Jun 11, 2015 at 9:39 AM Adrian Haarbach <adre...@gmail.com> wrote:

Hi all,

What is the current status of Sophus SE3 integration into Ceres?

There is no official integration, but we are happy to work with you on it.

 

In Steven's github code from above (moved to https://github.com/stevenlovegrove/Sophus/blob/master/test/ceres/local_parameterization_se3.hpp and https://github.com/stevenlovegrove/Sophus/blob/master/test/ceres/local_parameterization_se3.hpp), the Jacobian is computed analytically, but isn't it also possible to use it with AutoDiffLocalParameterization?

Yes it is. I believe that code predates the availability of AutoDiffLocalParameterization. Though you will get a bit of efficiency with analytical derivatives, not sure if it is worth the code complexity though.

 

It seems that Steven even contributed a lot to Sophus, as his fork https://github.com/stevenlovegrove/Sophus/ is 30 commits ahead of https://github.com/strasdat/Sophus, so which version of Sophus to use with Ceres?

That is a question for Hauke and Steven, or others on this list that have experience with the code, I have no experience with Sophus.

 

Is it possible to autodiff SE3 like this?:

struct SophusSE3Plus{

    template<typename T>

    bool operator()(const T* x_raw, const T* delta_raw, T* x_plus_delta_raw) const {

        const Eigen::Map< const Sophus::SE3Group<T> > x(x_raw);

        const Eigen::Map< const Eigen::Matrix<T,6,1> > delta(delta_raw);

        Eigen::Map< Sophus::SE3Group<T> > x_plus_delta(x_plus_delta_raw);

        x_plus_delta = x * Sophus::SE3Group<T>::exp(delta);

        return true;

      }

};

That sounds about right. Assuming Sophus is doing the right things underneath.

I am currently investigating different libraries (ceres, g2o), local parameterizations (Lie-algebra twists, quaternions, angle-axis) (using ceres' rotation.h, <Eigen/Geometry>, Sophus) as well as different ways to calculate derivatives (analytic, numeric, automatic :-) ) for the problem of minimizing 3D Alignment errors in multi-view LM-ICP pose graph optimization.

I have previously been using the g2o library (https://github.com/RainerKuemmerle/g2o) and GICP edges (https://github.com/RainerKuemmerle/g2o/blob/master/g2o/types/icp/types_icp.cpp) to model the point-to-point as well as point-to-plane ICP distance metrics and minimizing them on a graph, but I was not convinced that g2o is actually computing the right thing and using the correct derivatives, because my global error increases first before it gets decreased. 

Then I heard about automatic derivatives in Ceres, I am even happier to hear that autodiff works for Local Parameterizations as well. I would be glad if anyone could share his experience

I am not sure what kind of experience you are looking for here. It is well tested code that works. If you find a bug or something that does not work well I am happy to look at it.

Sameer

 

To view this discussion on the web visit https://groups.google.com/d/msgid/ceres-solver/d0965180-4c23-4588-a2cc-dff36c865c90%40googlegroups.com.
For more options, visit https://groups.google.com/d/optout.

[Steven Lovegrove]

Hi Adrian,

I spent some time ensuring that autodiff works well through Hauke's Sophus library, but I haven't been keeping up with Ceres to know if it is still working well, but I don't have any particular reason to believe that it shouldn't. As you can see from this simple example, you can use Sophus types directly with AutoDiff using Eigen::Map.

https://github.com/stevenlovegrove/Sophus/blob/master/test/ceres/test_ceres_se3.cpp

A number of my changes for Ceres compatibility are in Hauke's Devel branch, not Master. The Sophus library is not very actively maintained right now, so you should be aware that you may need to get your hands dirty if you encounter any issues, particularly regarding use with Ceres.

Best,

Steve

...

[Jon Zubizarreta Gorostidi]

Hi!

I am also interested in optimizing with lie algebra parameterization.

I work with sophus too, so my question is:

¿Did the proposed solution with auto differentiation worked?

¿Have you tested if there is a loss in performance between AutoDiffParameterization and the solution proposed in sophus library?

Thanks in advance,

Jon

[Alex Stewart]

Jon,

1) Yes, Sophus does work with Ceres auto-diff and you can use AutoDiffLocalParameterization.

2) I have tried both approaches, although I would generally use the analytic result from Sophus as it is available.

-Alex

To view this discussion on the web visit https://groups.google.com/d/msgid/ceres-solver/38d858fe-a41f-4b8b-8b32-267a9a569a40%40googlegroups.com.
For more options, visit https://groups.google.com/d/optout.

[Jon Zubizarreta Gorostidi]

Thanks!

Another question comes to my mind. What about if I want to work only in the tangent space without quaternions nor rotation matrices?

I mean, x is a 6-vector of se3 and delta_x too.

¿Do I have to specify the LocalParameterization?

If yes, I am thinking of this possibility:

The plus function: x_plus_delta = log(exp(x)*exp(x_delta)); because I need to do the composition (the addition x_plus_delta = x + x_delta is not correct)

Then the jacobian should be computed differentiating the above expression.

¿Am I in the right way?

Jon

And the jacobian should be the deriva

[Alex Stewart]

So, it's a fundamental property of SO(3) that all 3-dimensional representations of it must have a singularity (page 33), the question is just where that occurs.  So although the optimiser should work in the tangent space to compute the offset (x_delta) to apply to the transform (x) as that is the minimal representation, you should represent the transform (x) as an over-parameterised form that does not have a singularity - i.e. either a quaternion + translation or a homogeneous 4x4 matrix.  Internally Sophus uses the former.  Strictly speaking if you were *sure* you were never going to go near the singularity then you could use the tangent space as the representation for the transform (x) as well, but in practice that is not a good idea.

You probably want something like the following if you want to use auto-diff:

// Construct with:

//

// ceres::LocalParameterization* local_parameterization =

//     new ceres::AutoDiffLocalParameterization<

//           LiePostmultiplyLocalParameterization<Sophus::SE<X>Group>,

//           Sophus::SE<X>d::num_parameters,

//           Sophus::SE<X>d::DoF>();

template<template<class,int> class SEGroupTypeBase>

struct LiePostmultiplyLocalParameterization {

  template<typename T>

  bool operator()(const T* x, const T* delta, T* x_plus_delta) const {

    // Performs a RIGHT multiplicative update in SE(3), s.t:

    //

    //   G_plus_delta = G * G_delta

    const Eigen::Map<const SEGroupTypeBase<T, 0> > G(x);

    const Eigen::Map<const Eigen::Matrix<T, SEGroupTypeBase<T, 0>::DoF, 1> > lie_delta(delta);

    Eigen::Map<SEGroupTypeBase<T, 0> > G_plus_delta(x_plus_delta);

    G_plus_delta = G * SEGroupTypeBase<T, 0>::exp(lie_delta);

    return true;

  }

};

To view this discussion on the web visit https://groups.google.com/d/msgid/ceres-solver/7e1308ee-6950-403d-8317-3d0ddcc2482a%40googlegroups.com.

[Jon Zubizarreta Gorostidi]

Yes I was thinking of taking the exponential map of (x) to work with 4x4 matrices in the cost function.

However, after your reply I am thinking of storing directly the quaternion + translation or a 4x4 matrix as you mentioned.

It has no sense to store the parameters of the tangent space in the parameter block and use the exponential map whenever I want to use it.

Thanks for everything.

Jon