Ceres: LocalParametrization -> Manifold

133 Views Asked by At

I am new to c++, ceres and SLAM applications in general. I am currently working on the following paper: https://www.semanticscholar.org/reader/1a7f9f1624a06d438258e6c6aaa3e375c5ab8453. I am currently trying to get the program to run on my own hardware. But since I want to work with the new version of Ceres, I have to port the code from LocalParametization to Manifold. Here is the previous code::

double parameters[7] = {0, 0, 0, 1, 0, 0, 0};
problem.AddParameterBlock(parameters,7,new PoseSE3Parameterization);
...
//calculating costs
...

ceres::CostFunction *cost_function =new SurfNormAnalyticCostFunction(curr_point, last_point, norm, residual_weight);
problem.AddResidualBlock(cost_function, loss_function, parameters);

SurfNormAnalyticCostFunction::SurfNormAnalyticCostFunction(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_,
                                                           Eigen::Vector3d plane_unit_norm_, double weight_)
        : curr_point(curr_point_), last_point(last_point_), plane_unit_norm(plane_unit_norm_), weight(weight_) {
}
bool SurfNormAnalyticCostFunction::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
{
    Eigen::Map<const Eigen::Quaterniond> q_w_curr(parameters[0]);
    Eigen::Map<const Eigen::Vector3d> t_w_curr(parameters[0] + 4);

    Eigen::Vector3d point_w = q_w_curr * curr_point + t_w_curr;

    residuals[0] = weight*plane_unit_norm.dot(point_w - last_point);

    if(jacobians != nullptr)
    {
        if(jacobians[0] != nullptr)
        {
            Eigen::Matrix3d skew_point_w = skew(point_w);

            Eigen::Matrix<double, 3, 6> dp_by_so3;
            dp_by_so3.block<3,3>(0,0) = -skew_point_w;
            (dp_by_so3.block<3,3>(0, 3)).setIdentity();
            Eigen::Map<Eigen::Matrix<double, 1, 7, Eigen::RowMajor> > J_se3(jacobians[0]);
            J_se3.setZero();
            J_se3.block<1,6>(0,0) = weight*plane_unit_norm.transpose() * dp_by_so3;

        }
    }
    return true;

}

bool PoseSE3Parameterization::Plus(const double *x, const double *delta, double *x_plus_delta) const
{
    Eigen::Map<const Eigen::Vector3d> trans(x + 4);

    Eigen::Quaterniond delta_q;
    Eigen::Vector3d delta_t;
    getTransformFromSe3(Eigen::Map<const Eigen::Matrix<double,6,1>>(delta), delta_q, delta_t);
    Eigen::Map<const Eigen::Quaterniond> quater(x);
    Eigen::Map<Eigen::Quaterniond> quater_plus(x_plus_delta);
    Eigen::Map<Eigen::Vector3d> trans_plus(x_plus_delta + 4);

    quater_plus = delta_q * quater;
    trans_plus = delta_q * trans + delta_t;

    return true;
}
bool PoseSE3Parameterization::ComputeJacobian(const double *x, double *jacobian) const
{
    Eigen::Map<Eigen::Matrix<double, 7, 6, Eigen::RowMajor>> j(jacobian);
    (j.topRows(6)).setIdentity();
    (j.bottomRows(1)).setZero();

    return true;
}

My approach was that I can use the Ceres internal manifolds.

ceres::ProductManifold<ceres::EigenQuaternionManifold,ceres::EuclideanManifold<3>> se3;
problem.AddParameterBlock(parameters, 7, &se3);

But I don't get the same results as with the previous implementation. Any advice would be greatly appreciated.

0

There are 0 best solutions below