/**
 * @brief IMLSICPMatcher::ComputeNormal
 * 计算法向量
 * @param nearPoints    某个点周围的所有激光点
 * @return
 */
Eigen::Vector2d IMLSICPMatcher::ComputeNormal(std::vector<Eigen::Vector2d> &nearPoints)
{
    Eigen::Vector2d normal;

    //根据周围的激光点计算法向量，参考ppt中NICP计算法向量的方法
    // 计算周围点的几何中心
    Eigen::Vector2d meanPoints = Eigen::Vector2d::Zero();
    for (auto p: nearPoints) {
        meanPoints += p;
    }
    meanPoints /= nearPoints.size();

    // 计算方差
    Eigen::Matrix2d sigma = Eigen::Matrix2d::Zero();
    for (auto p: nearPoints) {
        Eigen::Vector2d diff = p - meanPoints;
        sigma += diff * diff.transpose();
    }
    sigma /= nearPoints.size();

    // 特征分解
    Eigen::EigenSolver<Eigen::Matrix2d> solver(sigma);
    // 取出特征值和特征向量
    Eigen::MatrixXd eigenvalue = solver.eigenvalues().real();
    Eigen::MatrixXd eigenvector = solver.eigenvectors().real();
    Eigen::Index evalsMin;
    // 找到最小特征值的位置
    eigenvalue.rowwise().sum().minCoeff(&evalsMin);
    // 取出该列
    normal << eigenvector(0, evalsMin), eigenvector(1, evalsMin);

    return normal;
}

//TODO
//根据函数进行投影．计算height，即ppt中的I(x)
double sum1 = 0.0, sum2 = 0.0;
for (int i = 0; i < nearPoints.size(); i++) {
    auto p_i = nearPoints[i];
    auto normal_i = nearNormals[i];
    auto diff = x - p_i;
    double wi = exp(- (diff.squaredNorm()) / (m_h * m_h));
    sum1 += (wi * (diff.dot(normal_i)));
    sum2 += wi;
}
height = sum1 / sum2;
//end of TODO

//TODO
//计算yi．
yi = xi - height * nearNormal;
//end of TODO

// csm 进行 pl-icp 需要的变量
bool run_pl_icp = true;
LDP m_prevLDP;
sm_params m_PIICPParams;
sm_result m_OutputResult;

// 进行pl-icp的相关函数
    void SetPIICPParams() {...}

    void LaserScanToLDP(const champion_nav_msgs::ChampionNavLaserScanConstPtr& pScan,
                        LDP& ldp) {
        int nPts = pScan->ranges.size(); // intensity 在 ChampionNavLaserScan 没用
        ...
        ldp->theta[i] = pScan->angles[i]; // ChampionNavLaserScan 不规定统一角度间隔，所以可以直接取
        ...
    }

    Eigen::Vector3d  PIICPBetweenTwoFrames(LDP& currentLDPScan,
                                           Eigen::Vector3d tmprPose) {...}

imlsDebug()
    {
        ...

        // 进行 pl-icp 相关设置
        m_prevLDP = NULL;
        SetPIICPParams();

        // 读取 rosbag 信息
        rosbag::Bag bag;
        bag.open(bagfile, rosbag::bagmode::Read);

        ...
    }

if(m_isFirstFrame == true)
        {
            std::cout <<"First Frame"<<std::endl;
            m_isFirstFrame = false;
            m_prevLaserPose = Eigen::Vector3d(0, 0, 0);
            pubPath(m_prevLaserPose, m_imlsPath, m_imlsPathPub);
            // 针对不同匹配算法做不一样的设置
            if (!run_pl_icp) {
                ConvertChampionLaserScanToEigenPointCloud(msg, m_prevPointCloud);
            } else {
                LaserScanToLDP(msg, m_prevLDP);
            }
            return ;
        }

        if (!run_pl_icp) { // 使用 imls-icp 匹配

            ...

        } else { // 用 csm 进行 pl-icp

            //把当前的激光数据转换为 pl-icp能识别的数据 & 进行矫正
            //d_point_scan就是用激光计算得到的两帧数据之间的旋转 & 平移
            LDP currentLDP;
            LaserScanToLDP(msg, currentLDP);
            Eigen::Vector3d d_point_scan = PIICPBetweenTwoFrames(currentLDP, Eigen::Vector3d::Zero());
            Eigen::Matrix3d lastPose, rPose;

            lastPose << cos(m_prevLaserPose(2)), -sin(m_prevLaserPose(2)), m_prevLaserPose(0),
                        sin(m_prevLaserPose(2)),  cos(m_prevLaserPose(2)), m_prevLaserPose(1),
                        0,  0,  1;

            rPose << cos(d_point_scan(2)), -sin(d_point_scan(2)), d_point_scan(0),
                    sin(d_point_scan(2)),  cos(d_point_scan(2)), d_point_scan(1),
                    0,  0,  1;

            Eigen::Matrix3d nowPose = lastPose * rPose;
            m_prevLaserPose << nowPose(0, 2) , nowPose(1, 2), atan2(nowPose(1, 0), nowPose(0, 0));
            pubPath(m_prevLaserPose, m_imlsPath, m_imlsPathPub);

        }
    }