Implement computeError and linearizeOplus in one function

[gitouni]

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.

[RainerKuemmerle]

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 https://github.com/RainerKuemmerle/g2o/blob/2a7ea2732c2bf4c6c0ccb9d4bc650f23b54aa647/g2o/types/slam3d/edge_se3_pointxyz_disparity.cpp#L77 https://github.com/RainerKuemmerle/g2o/blob/2a7ea2732c2bf4c6c0ccb9d4bc650f23b54aa647/g2o/types/slam3d/edge_se3_pointxyz_disparity.cpp#L117

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.

[gitouni]

Thanks for your reply. I look forward to this new feature.