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.