// SE3 plus operation for Ceres
//
// T * exp(x)
//
virtual bool Plus(double const* T_raw, double const* delta_raw,
double* T_plus_delta_raw) const {
Eigen::Map<SE3d const> const T(T_raw);
Eigen::Map<Vector6d const> const delta(delta_raw);
Eigen::Map<SE3d> T_plus_delta(T_plus_delta_raw);
T_plus_delta = T * SE3d::exp(delta);
return true;
}
/**
* \brief SE3 plus operation for Ceres
*
* \f$ T\cdot\exp(\widehat{\delta}) \f$
*/
virtual bool Plus(const double* T_raw, const double* delta_raw,
double* T_plus_delta_raw) const {
const Eigen::Map<const Sophus::SE3d> T(T_raw);
const Eigen::Map<const Eigen::Matrix<double, 6, 1> > delta(delta_raw);
Eigen::Map<Sophus::SE3d> T_plus_delta(T_plus_delta_raw);
T_plus_delta = T * Sophus::SE3d::exp(delta);
return true;
}
/**
* \brief SE3 plus operation for Ceres
*
* \f$ T\cdot\exp(\widehat{\delta}) \f$
*/
virtual bool Plus(const double * T_raw, const double * delta_raw,
double * T_plus_delta_raw) const {
const Eigen::Map<const Sophus::SE3d> T(T_raw);
const Eigen::Map<const Eigen::Matrix<double,6,1> > delta(delta_raw);
Eigen::Map<Sophus::SE3d> T_plus_delta(T_plus_delta_raw);
T_plus_delta = T * Sophus::SE3d::exp(delta);
const Eigen::IOFormat CommaInitFmt(StreamPrecision, DontAlignCols, ", ", ", ", "", "", " << ", ";");
//cout << "X " << T.matrix().format(CommaInitFmt) << endl;
//cout << "Y " << T_plus_delta.matrix().format(CommaInitFmt) << endl;
const Eigen::Map<const Eigen::Matrix<double, 7, 1>> T_param(T_raw);
const Eigen::Map<const Eigen::Matrix<double, 7, 1>> T_out(T_plus_delta_raw);
//cout << "\n" << (T_param - T_out).lpNorm<Eigen::Infinity>() << "\n";
//cout << T_param.format(CommaInitFmt) << " # " << T_out.format(CommaInitFmt) << endl;
return true;
}
