void getGrayScaleRegMatrix(const Cloud &cloud_target, const Cloud &cloud_source, Matrix &matrix);

//Surf_Threshold 是限定提取特征阈值的常量；ax_Remove_Threshold 是限定最大距离阈值的常量。详见下面的代码。
const double Max_Remove_Threshold = 750;
const int Surf_Threshold = 750;

//获取点云对应的二维点阵图信息。
IplImage* getIplImage(const CloudPtr &input_cloud)
{
    //创建图像。
    IplImage *image = cvCreateImage(cvSize((*input_cloud).width, (*input_cloud).height), IPL_DEPTH_8U, 3);
    //填充图像。
    cv::Scalar pixel;
    for (int i = 0; i < (*image).height; i++)
    {
        for (int j = 0; j < (*image).width; j++)
        {
            pixel.val[0] = (*input_cloud).points[i*(*image).width + j].b;
            pixel.val[1] = (*input_cloud).points[i*(*image).width + j].g;
            pixel.val[2] = (*input_cloud).points[i*(*image).width + j].r;
            cvSet2D(image, i, j, pixel);
        }
    }
    //返回创建的图像。
    return image;
}

//配准主函数。
void getGrayScaleRegMatrix(const CloudPtr &cloud_target, const CloudPtr &cloud_source, Matrix &matrix)
{
    //获取原始点云对应的二维点阵图信息。参考 getIplImage 函数。
    IplImage *oriImageTarget = getIplImage(cloud_target);
    IplImage *oriImageSource = getIplImage(cloud_source);

    //将原始图像转换为灰度图像，备 SURF 特征点提取使用。
    IplImage *grayImageTarget = cvCreateImage(cvGetSize(oriImageTarget), IPL_DEPTH_8U, 1);
    cvCvtColor(oriImageTarget, grayImageTarget, CV_RGB2GRAY);
    IplImage *grayImageSource = cvCreateImage(cvGetSize(oriImageSource), IPL_DEPTH_8U, 1);
    cvCvtColor(oriImageSource, grayImageSource, CV_RGB2GRAY);

    //提取 SURF 特征点，并存入 keypointsTarget 和 keypointsSource 向量。Surf_Threshold 是限定提取特征阈值的常量。
    cv::SurfFeatureDetector detector(Surf_Threshold);
    std::vector<cv::KeyPoint> keypointsTarget;
    detector.detect(grayImageTarget, keypointsTarget);
    std::vector<cv::KeyPoint> keypointsSource;
    detector.detect(grayImageSource, keypointsSource);

    //计算 SURF 特征点的描述符。
    cv::SurfDescriptorExtractor extractor;
    cv::Mat descriptorsSource, descriptorsTarget;
    extractor.compute(grayImageTarget, keypointsTarget, descriptorsTarget);
    extractor.compute(grayImageSource, keypointsSource, descriptorsSource);

    //将两幅点云的描述符通过 Flann 算法进行特征点的关系对应。
    cv::FlannBasedMatcher matcher;
    std::vector<cv::DMatch> matches;
    matcher.match(descriptorsSource, descriptorsTarget, matches);

    //获得对应关系列表。
    std::vector<cv::Point2f> sourcePoints;
    std::vector<cv::Point2f> targetPoints;
    for (int i = 0; i < matches.size(); i++)
    {
        sourcePoints.push_back(keypointsSource[matches[i].queryIdx].pt);
        targetPoints.push_back(keypointsTarget[matches[i].trainIdx].pt);
    }

    //使用 RANSAC 算法进行错误对应关系的去除。特征点对的保留与丢弃结果存储在 m_RANSACStatus 中。
    std::vector<uchar> m_RANSACStatus;
    cv::findHomography(sourcePoints, targetPoints, CV_RANSAC, 3.0, m_RANSACStatus);

    //创建 PCL 适用的关系列表。
    pcl::Correspondences correspondences;
    correspondences.empty();

    int temp_x_target, temp_y_target, temp_index_target, temp_x_source, temp_y_source, temp_index_source;
    for (int i = 0; i < matches.size(); i++)
    {
        if (m_RANSACStatus[i] == 1) //保留的对应点对。
        {
            //四舍五入计算X、Y坐标，并计算其在点云中存储的位置。
            temp_x_target = (int)(targetPoints[i].x + 0.5);
            temp_y_target = (int)(targetPoints[i].y + 0.5);
            temp_index_target = temp_y_target * (*grayImageTarget).width + temp_x_target;

            temp_x_source = (int)(sourcePoints[i].x + 0.5);
            temp_y_source = (int)(sourcePoints[i].y + 0.5);
            temp_index_source = temp_y_source * (*grayImageSource).width + temp_x_source;

            //若特征点对在三维点云中没有对应的空间点（没有深度信息）则不考虑该对应关系。
            if ((*cloud_source).points[temp_index_source].z != (*cloud_source).points[temp_index_source].z)
                continue;
            if ((*cloud_target).points[temp_index_target].z != (*cloud_target).points[temp_index_target].z)
                continue;

            //存储对应关系。
            pcl::Correspondence correspondence;
            correspondence.index_query = temp_index_source;
            correspondence.index_match = temp_index_target;
            correspondences.push_back(correspondence);
        }
    }

    //利用点云的空间特点，如果对应关系的相差距离过大，则去除该关系。Max_Remove_Threshold 是限定最大距离阈值的常量。
    for (int i = 0;;)
    {
        if (i == correspondences.size())
            break;
        if (Utils::getDistance((*cloud_target).points[correspondences[i].index_match], (*cloud_source).points[correspondences[i].index_query]) > Max_Remove_Threshold)
        {
            correspondences.erase(correspondences.begin() + i);
        }
        else
            i++;
    }
    
    //根据对应关系计算变换矩阵。
    pcl::registration::TransformationEstimationLM<Point, Point> estimation;
    estimation.estimateRigidTransformation((*cloud_source), (*cloud_target), correspondences, matrix);
}