error: static assertion failed: INVALID_VECTOR_VECTOR_PRODUCT__IF_YOU_WANTED_A_DOT_OR_COEFF_WISE_PRODUCT_YOU_MUST_USE_THE_EXPLICIT_FUNCTIONS
     #define EIGEN_STATIC_ASSERT(X,MSG) static_assert(X,#MSG);

矩阵维度不匹配导致的无法计算问题

error: static assertion failed: YOU_MIXED_MATRICES_OF_DIFFERENT_SIZES

把不同的siee矩阵赋值，一般是向量与矩阵混搭了，两个向量表达式类型不兼容（相同的固定大小或动态大小）

Sophus ensure failed in function 'Sophus::SO3<Scalar_, Options>::SO3(const Transformation&) [with Scalar_ = double; int Options = 0; Sophus::SO3<Scalar_, Options>::Transformation = Eigen::Matrix<double, 3, 3>]', file '/usr/local/include/sophus/so3.hpp', line 471.
R is not orthogonal:
           1      -5e-23 9.73925e-23
     -5e-23    0.999999           0
9.73925e-23           0    0.999999
已放弃 (核心已转储)

/*
    Hello SLAM!
            our first camera pose is :
    6.12323e-17            1            0           10
    0.5 -3.06162e-17    -0.866025            0
                                             -0.866025  5.30288e-17         -0.5           10
    0            0            0            1
    our second camera pose is :
    4.32978e-17            1  4.32978e-17           10
    0.965926 -3.06162e-17    -0.258819           10
                                                 -0.258819  5.30288e-17    -0.965926           10
    0            0            0            1
*/

创建的旋转矩阵不科学？

 Eigen::DenseCoeffsBase<Derived, 1>::Scalar& Eigen::DenseCoeffsBase<Derived, 1>::operator()(Eigen::Index) [with Derived = Eigen::Matrix<double, 4, 1>; Eigen::DenseCoeffsBase<Derived, 1>::Scalar = double; Eigen::Index = long int]: Assertion `index >= 0 && index < size()' failed.

一个愚蠢的问题，这里主要是因为向量或者矩阵的大小超界，排查错误是索引的问题超界了，HessePlane.head(3) = normal; HessePlane(3) = dis;，四维向量们把索引写成4了，排查好久

四元数相关错误

• 四元数的学习使用，Eigen库里面赋值形式实数在前，存储形式实数在后。注意顺序

    Eigen::Quaterniond q(1,0,0,0);
    std::cout<<q.coeffs()<<std::endl;//打印结果 0，0，0，1

• 四元数可视化
• 四元数的表示形式Hamilton & JPL定义，不同定义是影响计算的，混淆有问题。
• 四元数乘法左右有区别。

    Eigen::Quaterniond q(1,0,0,0);
    std::cout<<q.coeffs().transpose()<<std::endl;
    std::cout<<q<<std::endl;

显示结果为：

Hello SLAM!
Using homo para......
plane normal from Tcw_  is -0.965926 -0.258819         0
plane distance from Tcw_ is 6.86491
plane normal is -0.965926 -0.258819        -0
plane distance is 6.86491
0 0 0 1
0i + 0j + 0k + 1

学习一种保护措施

const double SMALL_EPS = 1e-5;

    if (n < SMALL_EPS)
    {
      // If quaternion is normalized and n=1, then w should be 1;
      // w=0 should never happen here!
      assert(fabs(w)>SMALL_EPS);

      two_atan_nbyw_by_n = 2./w - 2.*(n*n)/(w*squared_w);
    }
    else
    {
      if (fabs(w)<SMALL_EPS)
      {
        if (w>0)
        {
          two_atan_nbyw_by_n = M_PI/n;
        }
        else
        {
          two_atan_nbyw_by_n = -M_PI/n;
        }
      }
      two_atan_nbyw_by_n = 2*atan(n/w)/n;
    }

    *theta = two_atan_nbyw_by_n*n;
    return two_atan_nbyw_by_n * other.unit_quaternion_.vec();
}