meet trouble by using manif with ceres solver

[NemoLeiYANG]

Hi:

Great work. I'm working on optimization problem by using manif so I can solve the problem with Lie group.

But when I worked on autodiff in ceres solver by solving SE3 problem, I can't find the answer about write my own cost function while use your local parameterization functor, because I don't know the correct residual size according to your local parameterization functor.

Can you give me a hint or a sample ? It would be helpful.

Cheers.

[artivis]

Hi,

Have you checked the doc on the wiki pages?
manif ships with some utils and wrapper classes to interface it with Ceres (see here).
Finally you can find some example in the unit tests (e.g. a small dummy SE2 problem with autodiff).

This should be more than enough to get you started.

[NemoLeiYANG]

Hi:
Thank you for your response.
I've already see that page and I just extend that to do SE3 problem.
But I'm confusing about the local parameterization of SE3.

I guess, if I want to build a relative motion error between to poses.
I should use res = (T_ij_meas^-1 * T_ij_est).log().coeffs(),matrix() , but I
get a error about local param size from ceres. So I'm confusing the that should I
use the log map "res = xxxxx.log()" or just "res = xxxx" ?

Cheers.

[artivis]

Without seeing your code it is difficult to tell. Is it public? Do you have a link? Maybe the CeresConstraintFunctor wrapper and the different utils functions can help you API-wise.

[NemoLeiYANG]

Hi:
Here is my testing code, it's just wrap from ceres example:
cost function:

class PoseGraph3dErrorTerm {
public:
PoseGraph3dErrorTerm(const Pose3d& t_ab_measured,
const Eigen::Matrix<double, 6, 6>& sqrt_information)
: t_ab_measured_(t_ab_measured), sqrt_information_(sqrt_information) {}

template
bool operator()(const T* const t_a_ptr,
const T* const t_b_ptr,
T* residuals_ptr) const {
Eigen::Map<const manif::SE3 > t_a(t_a_ptr);
Eigen::Map<const manif::SE3 > t_b(t_b_ptr);

manif::SE3<T> t_ab = t_a.inverse()*t_b;

manif::SE3<T> res_ab = t_ab_measured_.cast<T>() * t_ab.inverse();

Eigen::Map<Eigen::Matrix<T, 6, 1> > res(residuals_ptr);
res = res_ab.log().coeffs().matrix();

return true;

}

static ceres::CostFunction* Create(
const Pose3d& t_ab_measured,
const Eigen::Matrix<double, 6, 6>& sqrt_information) {
return new ceres::AutoDiffCostFunction<PoseGraph3dErrorTerm, 6, 7, 7>(
new PoseGraph3dErrorTerm(t_ab_measured, sqrt_information));
}

EIGEN_MAKE_ALIGNED_OPERATOR_NEW

private:
// The measurement for the position of B relative to A in the A frame.
const Pose3d t_ab_measured_;
// The square root of the measurement information matrix.
const Eigen::Matrix<double, 6, 6> sqrt_information_;
};

and this is the callback function:
void BuildOptimizationProblem(const VectorOfConstraints &constraints,
MapOfPoses *poses, ceres::Problem *problem)
{
if (constraints.empty()) {
LOG(INFO) << "No constraints, no problem to optimize.";
return;
}

ceres::LossFunction *loss_function = NULL;

std::shared_ptr<ceres::LocalParameterization>
        auto_diff_local_parameterization =
        manif::make_local_parameterization_autodiff<manif::SE3d>();

for (VectorOfConstraints::const_iterator constraints_iter =
        constraints.begin();
     constraints_iter != constraints.end(); ++constraints_iter) {
    const Constraint3d &constraint = *constraints_iter;

    MapOfPoses::iterator pose_begin_iter = poses->find(constraint.id_begin);
    CHECK(pose_begin_iter != poses->end())
    << "Pose with ID: " << constraint.id_begin << " not found.";
    MapOfPoses::iterator pose_end_iter = poses->find(constraint.id_end);
    CHECK(pose_end_iter != poses->end())
    << "Pose with ID: " << constraint.id_end << " not found.";

    const Eigen::Matrix<double, 6, 6> sqrt_information =
            constraint.information.llt().matrixL();
    // Ceres will take ownership of the pointer.
    ceres::CostFunction *cost_function =
            PoseGraph3dErrorTerm::Create(constraint.t_be, sqrt_information);

    problem->AddResidualBlock(cost_function, loss_function,
                              pose_begin_iter->second.data(),
                              pose_end_iter->second.data());

    problem->SetParameterization(pose_begin_iter->second.data(), auto_diff_local_parameterization.get());
    problem->SetParameterization(pose_end_iter->second.data(),
                                 auto_diff_local_parameterization.get());
}

MapOfPoses::iterator pose_start_iter = poses->begin();
CHECK(pose_start_iter != poses->end()) << "There are no poses.";
problem->SetParameterBlockConstant(pose_start_iter->second.data());
problem->SetParameterBlockConstant(pose_start_iter->second.data());

}