Implement computeError and linearizeOplus in one function
gitouni opened this issue · comments
Hi, @RainerKuemmerle, I'm wondering how to compute Error and Jacobian in one function, since there are many same intermediate steps betweeen them. For example, when I use compute the Error
and Jacobian
with respect to a SE3 Vertex, I need to convert the SE3 matrix to rotation and translation vectors twice in computeError
and linearizeOplus
respectively. How can I implement them together and avoid redundant steps? Below is an example program and std::tie(Rab, tab, s) = v->toPose();
is a redundent step.
#include <Eigen/Dense>
#include <vector>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/solver.h>
namespace Eigen{
Eigen::Matrix3d skew(Eigen::Vector3d vec){
return (Eigen::Matrix3d() << 0, vec(2), -vec(1), -vec(2), 0, vec(0), vec(1), -vec(0), 0).finished();
}
}
class VertexPose : public g2o::BaseVertex<7, g2o::Vector7> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
virtual void setToOriginImpl() override {
_estimate = (g2o::Vector7() << 0, 0, 0, 0, 0, 0, 1.0).finished();
}
/// left multiplication on SE3
virtual void oplusImpl(const double *update) override {
g2o::Vector7::ConstMapType updateVec(update);
_estimate += updateVec;
}
virtual bool read(std::istream &in) override {return false;}
virtual bool write(std::ostream &out) const override {return false;}
std::tuple<Eigen::Matrix3d, Eigen::Vector3d, double> toPose() const{
Eigen::Vector3d rotvec(_estimate.head<3>());
Eigen::Vector3d translation(_estimate.segment<3>(3, 3));
double scale = _estimate(6);
double rotvec_norm = rotvec.norm();
Eigen::AngleAxisd ax(rotvec_norm, rotvec/rotvec_norm);
return {ax.toRotationMatrix(), translation, scale};
}
};
class EdgeHE : public g2o::BaseUnaryEdge<3, g2o::Vector3, VertexPose> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
EdgeHE(const Eigen::Isometry3d &Ta, const Eigen::Isometry3d &Tb, const double weight): _Ta(Ta), _Tb(Tb), _weight(weight){}
virtual void computeError() override {
const VertexPose *v = static_cast<VertexPose *> (_vertices[0]);
Eigen::Matrix3d Rab;
Eigen::Vector3d tab;
double s;
std::tie(Rab, tab, s) = v->toPose(); // Redundant Steps
Eigen::Vector4d tsab; // [tab, s]
tsab.block<3, 1>(0, 0) = tab;
tsab(3) = s;
Eigen::AngleAxisd axisTa(_Ta.rotation()), axisTb(_Tb.rotation());
Eigen::Vector3d errRotVec = Rab * (axisTb.angle() * axisTb.axis()) - (axisTa.angle() * axisTa.axis());
Eigen::Matrix<double, 3, 4> A;
Eigen::Vector3d b;
A.block<3, 3>(0, 0) = _Ta.rotation() - Eigen::Matrix3d::Identity();
A.block<3, 1>(0, 3) = _Ta.translation();
b = Rab * _Tb.translation();
Eigen::Vector3d errTran = A * tsab - b;
_error << _weight * (errRotVec + errTran);
}
virtual void linearizeOplus() override {
const VertexPose *v = static_cast<VertexPose *> (_vertices[0]);
Eigen::Matrix3d Rab;
Eigen::Vector3d tab;
double s;
std::tie(Rab, tab, s) = v->toPose(); // Redundant Steps
Eigen::Vector4d tsab; // [tab, s]
tsab.block<3, 1>(0, 0) = tab;
tsab(3) = s;
Eigen::AngleAxisd axisTb(_Tb.rotation());
Eigen::Matrix3d JacobianRotVec = -1.0 * Eigen::skew(Rab * (axisTb.angle() * axisTb.axis())); // -(Rp)^ or (-Rp)^
Eigen::Matrix<double, 3, 4> JacobianTran;
JacobianTran.block<3, 3>(0, 0) = _Ta.rotation() - Eigen::Matrix3d::Identity();
JacobianTran.block<3, 1>(0, 3) = _Ta.translation();
_jacobianOplusXi << _weight * JacobianRotVec, _weight * JacobianTran; // (3,3) (3,4) -> concat -> (3,7)
}
void updateWeight(double weight){
_weight = weight;
}
double returnSquaredWeight(){
return _weight * _weight;
}
virtual bool read(std::istream &in) override {return false;}
virtual bool write(std::ostream &out) const override {return false;}
private:
Eigen::Isometry3d _Ta, _Tb;
double _weight;
};
Another example can be found in floam_g2o. Compared with the original FLOAM implemented by ceres, it cannot avoid compute variables lp
, nu
and ne
repeatedly both in computeError
and linearizeOplus
functions.
I gratefully appreciate your reply.
The g2o framework to this end allows to add a Cache
to edges which can be used to cache intermediate operations, such as a transformation matrix. See for example
The cache is a different concept and computation of error and Jacobian in one call is currently not addressed but might be an interesting feature to implement.
Thanks for your reply. I look forward to this new feature.