// 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; }