點雲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;

}