平面法向的角度表示以及坐标系变换

1.根据法线计算法线的垂直角sint和法线在水平投影与x轴的夹角phi

        double phi = atan2(normal(1) , normal(0));  // atan2(y,x), 计算法向在xy平面上的投影和x轴之间的夹角
        double sint = asin(normal(2));  //理论上是z轴和 该法向向量之间的夹角

2.根据角度计算法线

       Eigen::Vector3d new_normal;
        new_normal<< cos(phi)* cos(sint), sin(phi) * cos(sint), sin(sint);

3.验证代码

void  transformPlaneParams()
{
    srand(time(nullptr));
    for(int i =0;i <100000; i++)
    {

        Eigen::Vector3d normal;
        normal<< (rand()%100 - 50) , rand()%100 - 50, rand()%100 - 50;

        int randuse = rand()%4;
        int randIndex = rand() % 3;
        if(randuse == 3)
        {
            normal(randIndex) =0;
        }

      if(randuse == 2)
        {
            normal(randIndex) =0;
            normal((randIndex +1)%3) =0;
        }

        normal /= normal.norm();
        // 计算夹角
        double phi = atan2(normal(1) , normal(0));  // atan2(y,x), 计算法向在xy平面上的投影和x轴之间的夹角
        double sint = asin(normal(2));  //理论上是z轴和 该法向向量之间的夹角

        // 根据夹角计算法向
        Eigen::Vector3d new_normal;
        new_normal<< cos(phi)* cos(sint), sin(phi) * cos(sint), sin(sint);
        
        double err = (normal - new_normal).norm();
        if(abs(err) > 0.01)
        {
            std::cout<< "err: "<<err <<"   "<< normal(0)<<"    "<< normal(1)<<"    "<< normal(2)<<"    "
                    << new_normal(0)<<"    "<< new_normal(1)<<"    "<< new_normal(2)<<"\n";
        }
    }


}

4.坐标系变换

f为在当前帧坐标系下的平面参数(nx,ny,nz,d). f'为在世界坐标系下的平面参数，T表示的是当前帧在世界坐标系下的位姿。经过这个变换就可以把当前帧中平面参数变换到世界坐标系下。

struct PlaneParams
{
    int world_id; // 平面所在的全局平面ID
    int feature_id;  // 平面所在的当前帧的面ID
    Eigen::Vector4f plane;
    Eigen::Vector3f center;
};
void transformPlaneParamsToWorld(Eigen::Matrix4f& pose, const PlaneParams& planeSrc, PlaneParams& planeWorld )
{
    Eigen::Matrix4f pose_inv_T = pose.inverse().transpose();
    planeWorld.plane =pose_inv_T * planeSrc.plane;
    planeWorld.center = pose.block<3,3>(0,0) * planeSrc.center + pose.block<3,1>(0,3);
}