search

HKUST-Aerial-Robotics / VINS-Mono

A Robust and Versatile Monocular Visual-Inertial State Estimator
GNU General Public License v3.0
4.96k stars 2.09k forks source link

confuse about Utility::skewSymmetric(q) used in InitialEXRotation::CalibrationExRotation #355

Closed chennuo0125-HIT closed 3 years ago

chennuo0125-HIT commented 4 years ago

mul_qua above is quaternion multiplication, but the left multiplication matrix isnt same as:

        double w = Quaterniond(Rc[i]).w();
        Vector3d q = Quaterniond(Rc[i]).vec();
        L.block<3, 3>(0, 0) = w * Matrix3d::Identity() + Utility::skewSymmetric(q);
        L.block<3, 1>(0, 3) = q;
        L.block<1, 3>(3, 0) = -q.transpose();
        L(3, 3) = w;

i think Utility::skewSymmetric(q) should be as follow:

    template <typename Derived>
    static Eigen::Matrix<typename Derived::Scalar, 3, 3> skewSymmetric(const Eigen::MatrixBase<Derived> &q)
    {
        Eigen::Matrix<typename Derived::Scalar, 3, 3> ans;
        ans << typename Derived::Scalar(0), -q(1), -q(2),
            q(1), typename Derived::Scalar(0), -q(3),
            q(2), q(3), typename Derived::Scalar(0);
        return ans;
    }

maybe i was wrong, hope for your comment