在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;
E