PCL入門根據向量做點雲旋轉

如果已知旋轉前某個向量n1(x1,y1,z1)和旋轉後的向量n2(x2,y2,z2)實際上已知了三個點，(0,0,0) (x1,y1,z1) (x2,y2,z2) 那麼可以求出他們構成的平面的法向量，也就是同時垂直於n1和n2的這個向量，並且經過n1和n2的交點，也就是原點。這個向量就是他們的旋轉軸。

那麼已知三點怎麼求平面的法向量呢，我們可以代入公式

其中x3,y3,z3我們設爲原點，那麼就是0,0,0.

void MatrixRotation::calculate_axis()
{
  float x1,x2,y1,y2,z1,z2;
  x1=normal_start_.normal_x,x2=normal_end_.normal_x;
  y1=normal_start_.normal_y,y2=normal_end_.normal_y;
  z1=normal_start_.normal_z,z2=normal_end_.normal_z;
  float x3,y3,z3; 
  x3=y3=z3=0;
  axis_.normal_x=(y2-y1)*(z3-z1)-(y3-y1)*(z2-z1);
  axis_.normal_y=(z2-z1)*(x3-x1)-(z3-z1)*(x2-x1);
  axis_.normal_z=(x2-x1)*(y3-y1)-(x3-x1)*(y2-y1);
  //normalize
  float square_sum=axis_.normal_x*axis_.normal_x+
                    axis_.normal_y*axis_.normal_y+
                    axis_.normal_z*axis_.normal_z;
  square_sum=sqrt(square_sum);
  axis_.normal_x/=square_sum;
  axis_.normal_y/=square_sum;
  axis_.normal_z/=square_sum;
}

求出了法向量之後帶入羅德里格斯旋轉公式，求出旋轉矩陣，

void MatrixRotation::matrix_tansform(pcl::PointCloud<PointT>::Ptr cloud)
{
  Eigen::Matrix4f transform = Eigen::Matrix4f::Identity();
  float x,y,z;
  x=axis_.normal_x;y=axis_.normal_y;z=axis_.normal_z;
  transform (0,0)=cos(theta_)+(1-cos(theta_))*x*x;
  transform (0,1)=(1-cos(theta_))*x*y-sin(theta_)*z;
  transform (0,2)=(1-cos(theta_))*x*z+sin(theta_)*y;
  transform (1,0)=(1-cos(theta_))*x*y+sin(theta_)*z;
  transform (1,1)=cos(theta_)+(1-cos(theta_))*y*y;
  transform (1,2)=(1-cos(theta_))*y*z-sin(theta_)*x;
  transform (2,0)=(1-cos(theta_))*x*z-sin(theta_)*y;
  transform (2,1)=(1-cos(theta_))*y*z+sin(theta_)*x;
  transform (2,2)=cos(theta_)+(1-cos(theta_))*z*z;
  pcl::transformPointCloud (*cloud, *cloud, transform);
}

其中，theta是需要旋轉的角度，這個公式很簡單，搜索求向量夾角就可以了，我這裏不再贅述，直接使用了pcl的方法求的，最後使用pcl的旋轉，實際上就是現有矩陣乘以旋轉矩陣。這樣就能得到旋轉後的圖像了。

完整代碼我會上傳到github上，敬請期待。。。