search

xdspacelab / openvslam

OpenVSLAM: A Versatile Visual SLAM Framework
https://openvslam.readthedocs.io/
2.97k stars 868 forks source link

Jacobian computation #521

Open IaroslavS opened 3 years ago

IaroslavS commented 3 years ago

Hi, this is not an issue, I'm just wondering the meaning of the Jacobian computation, implemented in OpenVSLAM in file https://github.com/xdspacelab/openvslam/blob/master/src/openvslam/optimize/g2o/se3/perspective_pose_opt_edge.cc There are several variants and I understand why Jacobian matrix for mono differs from stereo (there's no x_right for mono). I don't understand where the formulas for Jacobian elements were taken. Could you, please, explain, the origin of these formulas:

    _jacobianOplusXi(0, 0) = x * y / z_sq * fx_;
    _jacobianOplusXi(0, 1) = -(1.0 + (x * x / z_sq)) * fx_;
    _jacobianOplusXi(0, 2) = y / z * fx_;
    _jacobianOplusXi(0, 3) = -1.0 / z * fx_;
    _jacobianOplusXi(0, 4) = 0.0;
    _jacobianOplusXi(0, 5) = x / z_sq * fx_;

    _jacobianOplusXi(1, 0) = (1.0 + y * y / z_sq) * fy_;
    _jacobianOplusXi(1, 1) = -x * y / z_sq * fy_;
    _jacobianOplusXi(1, 2) = -x / z * fy_;
    _jacobianOplusXi(1, 3) = 0.0;
    _jacobianOplusXi(1, 4) = -1.0 / z * fy_;
    _jacobianOplusXi(1, 5) = y / z_sq * fy_;

    _jacobianOplusXi(2, 0) = _jacobianOplusXi(0, 0) - focal_x_baseline_ * y / z_sq;
    _jacobianOplusXi(2, 1) = _jacobianOplusXi(0, 1) + focal_x_baseline_ * x / z_sq;
    _jacobianOplusXi(2, 2) = _jacobianOplusXi(0, 2);
    _jacobianOplusXi(2, 3) = _jacobianOplusXi(0, 3);
    _jacobianOplusXi(2, 4) = 0.0;
    _jacobianOplusXi(2, 5) = _jacobianOplusXi(0, 5) - focal_x_baseline_ / z_sq;

Particularly, I don't see if these formulas reflect projection errors (I see absolute values). Are the last 3 colomns ( Jacobian[:,3:] ) reflect yaw, pitch, roll ?