点云cell的特征向量和特征值分析

在ndt中会定义一个ndtcell的尺寸，初始化时会计算每个cell的中的均值mean和方差cov。

有了均值和方差以后就可以计算特征向量和特征值。

这儿获得的33的特征向量和31的特征值，分别代表旋转和每个轴的分布离散情况。

错！
这儿的3*3的特征向量很有可能不是旋转矩阵。因为是直接通过对cov进行分解得到。
例如
0 0 1
0 1 0
1 0 0

虽然它乘以他的转置是单位阵，但是他不是右手坐标系下的旋转矩阵。将它转化为四元数是 0 0 0 0.707 不是归一化的！

其中特征向量的每一列和特征值是一一对应的，每一列代表过零点和该点的连线所指的方向（问题所在，这这儿可以是相反的方向。）

解决思路：
1.将特征向量的第一列叉乘第二列，获得右手坐标系下的z轴，重新赋值。

      Eigen::Matrix3d evecs_;
      Eigen::Vector3d evals_;
      evecs_ = leaf.getEvecs();
      evals_ = leaf.getEvals();

      Eigen::Matrix3d evecs_reset;
      Eigen::Vector3d evals_reset = evals_;

      Eigen::Vector3d tempCross = evecs_.block<3,1>(0,0).cross(evecs_.block<3,1>(0,1));
      evecs_reset.block<3,2>(0,0) = evecs_.block<3,2>(0,0);
      evecs_reset.block<3,1>(0,2) = tempCross;


      Eigen::Quaterniond q(evecs_reset);

2.（错误思路）
从显示角度考虑，只要对应的交换特征向量的列和特征值的顺序，保证交换好的特征向量是符合右手坐标的就可以了。
这个会出现再怎么交换也无法满足右手定律。

bool resetEigenValueAndVecor(const Eigen::Matrix3d cov, const Eigen::Vector3d vals,
                             Eigen::Matrix3d &cov_out, Eigen::Vector3d &vals_out)
{
  //未经验证的方法。通过四元数是否归一化来判断。
  Eigen::Quaterniond tempQ(cov);

  if(tempQ.norm() != 1)
  {
    //尝试换列

    // 1换2
    cov_out << cov(0,1),  cov(0,0),  cov(0,2),
                cov(1,1),  cov(1,0),  cov(1,2),
                cov(2,1),  cov(2,0),  cov(2,2);
    vals_out << vals(1), vals(0), vals(2);
    {
      Eigen::Quaterniond tempQ(cov_out);
      if(tempQ.norm() == 1)
      {
        return true;
      }
    }

    // 1换3
    cov_out << cov(0,2),  cov(0,1),  cov(0,0),
                cov(1,2),  cov(1,1),  cov(1,0),
                cov(2,2),  cov(2,1),  cov(2,0);
    vals_out << vals(2), vals(1), vals(0);
    {
      Eigen::Quaterniond tempQ(cov_out);
      if(tempQ.norm() == 1)
      {
        return true;
      }
    }

    // 2换3
    cov_out << cov(0,0),  cov(0,2),  cov(0,1),
                cov(1,0),  cov(1,2),  cov(1,1),
                cov(2,0),  cov(2,2),  cov(2,1);
    vals_out << vals(0), vals(2), vals(1);
    {
      Eigen::Quaterniond tempQ(cov_out);
      if(tempQ.norm() == 1)
      {
        return true;
      }
    }
  }
  else
  {
    cov_out = cov;
    vals_out = vals;
    return true;
  }
  return false;

}