bow3d代码记录与学习

简要记录:

分120扇区，每一个区里面进行聚类，聚类完了合并临近聚类，聚类用一个中心点代表
特征构建：
1.聚类中心最邻近三个点，用聚类中心和其中一个构建一个主方向，其他点和当前聚类中心点连线和 主方向构成夹角，夹角投票到对应bin中（划分180份），bin中存储点对距离，选择距离最小的点对作为该特征点对应第j（bin）的特征描述。
    特征具有旋转不变性，感觉具有一定的平移不变性，xy尾插差异太大感觉可能最后聚类中心会有一定变动
2.构建单词数据库：
    map（描述距离，列数j） = (frameid, 特征i） 
3.回环探测：
    假设当前帧上有n个特征，用当前第i个特征（描述距离，列数j）作为单词，查询(frmaeid,i)，理论上如果两帧中有同名特征，则用该特征查找相似特征，如果没有0特征秒描述，则frameid，类书从0-179.
    找到当前特征对应的最佳frameid后，加入到(score,max_proper_framid)中，score是单词重合次数，max_proper_framid是最佳帧id，每一个特征都对应m个（只要大于thr）就可以，
    使用的map<score,max_proper_framid>具有天然的排序功能。

    使用当前帧和m)+m1+m2 + m...个最佳帧id逐一匹配，
    计算帧描述行于行的最佳匹配（其实就是聚类特征点与聚类特征点的最佳匹配），得到聚类均值点---聚类中值点 对应后，从聚类中筛选出来最大曲率值的点，行对应---点对对应起来， svd分解求解相对旋转和平移矩阵。
    如果相对旋转和平移矩阵的 矢量模场小于一致，则认为查找到了闭环帧。

LinK3D_Extractor.cpp

namespace BoW3D
{
    LinK3D_Extractor::LinK3D_Extractor(
            int nScans_,   // n线激光
            float scanPeriod_,  // 扫描周期
            float minimumRange_,   // 一处距离激光雷达一定半径内的点
            float distanceTh_,    // 聚类距离       
            int matchTh_):  // 匹配点得分阈值
            nScans(nScans_), 
            scanPeriod(scanPeriod_), 
            minimumRange(minimumRange_),  
            distanceTh(distanceTh_),          
            matchTh(matchTh_)
            {
                scanNumTh = ceil(nScans / 6);  // 聚类点所在的行数 阈值
                ptNumTh = ceil(1.5 * scanNumTh);  // 一个聚类中最小的点个数               
            }
    /*
       移除距离激光雷达中心一定距离的点
    */
    void LinK3D_Extractor::removeClosedPointCloud(
            const pcl::PointCloud<pcl::PointXYZ> &cloud_in,
            pcl::PointCloud<pcl::PointXYZ> &cloud_out)
    {
        if (&cloud_in != &cloud_out)
        {
            cloud_out.header = cloud_in.header;
            cloud_out.points.resize(cloud_in.points.size());
        }

        size_t j = 0;

        for (size_t i = 0; i < cloud_in.points.size(); ++i)
        {
            if (cloud_in.points[i].x * cloud_in.points[i].x 
                + cloud_in.points[i].y * cloud_in.points[i].y 
                + cloud_in.points[i].z * cloud_in.points[i].z 
                < minimumRange * minimumRange) // 计算距离的平方 小于设定的范围值
            {
                continue;
            }
                
            cloud_out.points[j] = cloud_in.points[i];
            j++;
        }

        if (j != cloud_in.points.size())
        {
            cloud_out.points.resize(j);
        }

        cloud_out.height = 1;
        cloud_out.width = static_cast<uint32_t>(j);
        cloud_out.is_dense = true;
    }

    /*
        loam系列的常规提取方法
    */

    void LinK3D_Extractor::extractEdgePoint(
            pcl::PointCloud<pcl::PointXYZ>::Ptr pLaserCloudIn, 
            ScanEdgePoints &edgePoints)
    {
        vector<int> scanStartInd(nScans, 0);
        vector<int> scanEndInd(nScans, 0);

        pcl::PointCloud<pcl::PointXYZ> laserCloudIn;
        laserCloudIn = *pLaserCloudIn;
        vector<int> indices;

        pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn, indices);
        removeClosedPointCloud(laserCloudIn, laserCloudIn);

        int cloudSize = laserCloudIn.points.size();
        float startOri = -atan2(laserCloudIn.points[0].y, laserCloudIn.points[0].x);
        float endOri = -atan2(laserCloudIn.points[cloudSize - 1].y, laserCloudIn.points[cloudSize - 1].x) + 2 * M_PI;
    
        if (endOri - startOri > 3 * M_PI)
        {
            endOri -= 2 * M_PI;
        }
        else if (endOri - startOri < M_PI)
        {
            endOri += 2 * M_PI;
        }
        
        bool halfPassed = false;
        int count = cloudSize;
        pcl::PointXYZI point;
        vector<pcl::PointCloud<pcl::PointXYZI>> laserCloudScans(nScans);
        
        for (int i = 0; i < cloudSize; i++)
        {
            point.x = laserCloudIn.points[i].x;
            point.y = laserCloudIn.points[i].y;
            point.z = laserCloudIn.points[i].z;
            
            float angle = atan(point.z / sqrt(point.x * point.x + point.y * point.y)) * 180 / M_PI;
            int scanID = 0;

            if (nScans == 16)
            {
                scanID = int((angle + 15) / 2 + 0.5);
                if (scanID > (nScans - 1) || scanID < 0)
                {
                    count--;
                    continue;
                }
            }
            else if (nScans == 32)
            {
                scanID = int((angle + 92.0/3.0) * 3.0 / 4.0);
                if (scanID > (nScans - 1) || scanID < 0)
                {
                    count--;
                    continue;
                }
            }
            else if (nScans == 64)
            {   
                if (angle >= -8.83)
                    scanID = int((2 - angle) * 3.0 + 0.5);
                else
                    scanID = nScans / 2 + int((-8.83 - angle) * 2.0 + 0.5);

                if (angle > 2 || angle < -24.33 || scanID > 50 || scanID < 0)
                {
                    count--;
                    continue;
                }
            }
            else
            {
                printf("wrong scan number\n");
            }
            
            float ori = -atan2(point.y, point.x);
            if (!halfPassed)
            { 
                if (ori < startOri - M_PI / 2)
                {
                    ori += 2 * M_PI;
                }
                else if (ori > startOri + M_PI * 3 / 2)
                {
                    ori -= 2 * M_PI;
                }

                if (ori - startOri > M_PI)
                {
                    halfPassed = true;
                }
            }
            else
            {
                ori += 2 * M_PI;
                if (ori < endOri - M_PI * 3 / 2)
                {
                    ori += 2 * M_PI;
                }
                else if (ori > endOri + M_PI / 2)
                {
                    ori -= 2 * M_PI;
                }
            }

            point.intensity = ori;  // 水平角度
            laserCloudScans[scanID].points.push_back(point);  // scanid线上加入该点       
        }

        size_t scanSize = laserCloudScans.size();
        edgePoints.resize(scanSize);
        cloudSize = count;
                
        for(int i = 0; i < nScans; i++)
        {
            int laserCloudScansSize = laserCloudScans[i].size();
            if(laserCloudScansSize >= 15)
            {
                for(int j = 5; j < laserCloudScansSize - 5; j++)
                {
                    float diffX = laserCloudScans[i].points[j - 5].x + laserCloudScans[i].points[j - 4].x
                                + laserCloudScans[i].points[j - 3].x + laserCloudScans[i].points[j - 2].x
                                + laserCloudScans[i].points[j - 1].x - 10 * laserCloudScans[i].points[j].x
                                + laserCloudScans[i].points[j + 1].x + laserCloudScans[i].points[j + 2].x
                                + laserCloudScans[i].points[j + 3].x + laserCloudScans[i].points[j + 4].x
                                + laserCloudScans[i].points[j + 5].x;
                    float diffY = laserCloudScans[i].points[j - 5].y + laserCloudScans[i].points[j - 4].y
                                + laserCloudScans[i].points[j - 3].y + laserCloudScans[i].points[j - 2].y
                                + laserCloudScans[i].points[j - 1].y - 10 * laserCloudScans[i].points[j].y
                                + laserCloudScans[i].points[j + 1].y + laserCloudScans[i].points[j + 2].y
                                + laserCloudScans[i].points[j + 3].y + laserCloudScans[i].points[j + 4].y
                                + laserCloudScans[i].points[j + 5].y;
                    float diffZ = laserCloudScans[i].points[j - 5].z + laserCloudScans[i].points[j - 4].z
                                + laserCloudScans[i].points[j - 3].z + laserCloudScans[i].points[j - 2].z
                                + laserCloudScans[i].points[j - 1].z - 10 * laserCloudScans[i].points[j].z
                                + laserCloudScans[i].points[j + 1].z + laserCloudScans[i].points[j + 2].z
                                + laserCloudScans[i].points[j + 3].z + laserCloudScans[i].points[j + 4].z
                                + laserCloudScans[i].points[j + 5].z;

                    float curv = diffX * diffX + diffY * diffY + diffZ * diffZ;
                    if(curv > 10 && curv < 20000)
                    {
                        float ori = laserCloudScans[i].points[j].intensity;  //表示 所代表的角度
                        float relTime = (ori - startOri) / (endOri - startOri);

                        PointXYZSCA tmpPt;
                        tmpPt.x = laserCloudScans[i].points[j].x;
                        tmpPt.y = laserCloudScans[i].points[j].y;
                        tmpPt.z = laserCloudScans[i].points[j].z;
                        tmpPt.scan_position = i + scanPeriod * relTime; // 行数 . 时间
                        tmpPt.curvature = curv;  // 曲率
                        tmpPt.angle = ori;  // 起始角度
                        edgePoints[i].emplace_back(tmpPt);  
                    }
                }
            }
        }            
    }    

    //Roughly divide the areas to save time for clustering.
    void LinK3D_Extractor::divideArea(ScanEdgePoints &scanCloud, ScanEdgePoints &sectorAreaCloud)
    {
        sectorAreaCloud.resize(120); //The horizontal plane is divided into 120 sector area centered on LiDAR coordinate.
        int numScansPt = scanCloud.size();
        if(numScansPt == 0)
        {
            return;
        }
            
        for(int i = 0; i < numScansPt; i++) 
        {
            int numAScanPt = scanCloud[i].size();  // 第i条线的点数
            for(int j = 0; j < numAScanPt; j++)
            {                
                int areaID = 0;
                float angle = scanCloud[i][j].angle;  // 水平角度
                
                if(angle > 0 && angle < 2 * M_PI)
                {
                    areaID = std::floor((angle / (2 * M_PI)) * 120);
                }   
                else if(angle > 2 * M_PI)
                {
                    areaID = std::floor(((angle - 2 * M_PI) / (2 * M_PI)) * 120);
                }
                else if(angle < 0)
                {
                    areaID = std::floor(((angle + 2 * M_PI) / (2 * M_PI)) * 120);
                }

                sectorAreaCloud[areaID].push_back(scanCloud[i][j]);//根据角度划分成120sector，把点都放到这个里面
            }
        }
    }


    float LinK3D_Extractor::computeClusterMean(vector<PointXYZSCA> &cluster)
    {        
        float distSum = 0;
        int numPt = cluster.size();

        for(int i = 0; i < numPt; i++)
        {
            distSum += distXY(cluster[i]);
        }

        return (distSum/numPt);
    }

    void LinK3D_Extractor::computeXYMean(vector<PointXYZSCA> &cluster, std::pair<float, float> &xyMeans)
    {         
        int numPt = cluster.size();
        float xSum = 0;
        float ySum = 0;

        for(int i = 0; i < numPt; i++)
        {
            xSum += cluster[i].x;
            ySum += cluster[i].y;
        }

        float xMean = xSum/numPt;
        float yMean = ySum/numPt;
        xyMeans = std::make_pair(xMean, yMean);
    }

    /*
        每一个扇区中的点首先进行聚类，然后聚类和聚类之间进行合并，得到结果
    */
    void LinK3D_Extractor::getCluster(const ScanEdgePoints &sectorAreaCloud, ScanEdgePoints &clusters)
    {    
        ScanEdgePoints tmpclusters;
        PointXYZSCA curvPt;
        vector<PointXYZSCA> dummy(1, curvPt); 

        int numArea = sectorAreaCloud.size();

        //Cluster for each sector area.
        for(int i = 0; i < numArea; i++)
        {
            if(sectorAreaCloud[i].size() < 6)
                continue;

            int numPt = sectorAreaCloud[i].size();        
            ScanEdgePoints curAreaCluster(1, dummy);
            curAreaCluster[0][0] = sectorAreaCloud[i][0];  // 该区域的第一个点

            /*
               遍历该扇区的每一个点，求该点距离每一个聚类中心的距离，如果遇到距离近的聚类，则直接加入进去，如果没有一个聚类中心距离近，则直接用该点新生成一个聚类
            */
            for(int j = 1; j < numPt; j++)
            {
                int numCluster = curAreaCluster.size();

                for(int k = 0; k < numCluster; k++)
                {
                    float mean = computeClusterMean(curAreaCluster[k]);  // 计算该扇区中第k个聚类的xy长度均值
                    std::pair<float, float> xyMean;
                    computeXYMean(curAreaCluster[k], xyMean);  // 计算该扇区中第k个聚类的xy坐标点均值
                    
                    PointXYZSCA tmpPt = sectorAreaCloud[i][j];  // 第i个扇区，第j个点
                                        
                    if(abs(distXY(tmpPt) - mean) < distanceTh 
                        && abs(xyMean.first - tmpPt.x) < distanceTh 
                        && abs(xyMean.second - tmpPt.y) < distanceTh)  //如果该点距离第k个扇区距离小于给定的阈值，则把该点放入到该扇区
                    {
                        curAreaCluster[k].emplace_back(tmpPt);
                        break;
                    }
                    else if(abs(distXY(tmpPt) - mean) >= distanceTh && k == numCluster-1)  // 如果该点距离均值大于所有聚类的距离，则新加入一聚类
                    {
                        curAreaCluster.emplace_back(dummy);
                        curAreaCluster[numCluster][0] = tmpPt;
                    }
                    else
                    { 
                        continue; 
                    }                    
                }
            }

            //筛出掉点数比较少的聚类，其他的聚类加入到tmpclusters
            int numCluster = curAreaCluster.size();
            for(int j = 0; j < numCluster; j++)
            {
                int numPt = curAreaCluster[j].size();

                if(numPt < ptNumTh)
                {
                    continue;
                }
                tmpclusters.emplace_back(curAreaCluster[j]);//扇区内聚类，聚类点数大于一定点数的存储出来
            }
        }
        //
        int numCluster = tmpclusters.size();  // 所有扇区的聚类个数
        
        vector<bool> toBeMerge(numCluster, false);
        multimap<int, int> mToBeMergeInd;
        set<int> sNeedMergeInd;

        //如果toBeMerge[j] == true 则continue，如果有三个聚类相邻的，会不会把三个聚类相邻的分成了两类，第三类再也找不到了
        //Merge the neighbor clusters.  感觉处理的稍有点问题
        for(int i = 0; i < numCluster; i++)
        {
            if(toBeMerge[i]){
                continue;
            }
            float means1 = computeClusterMean(tmpclusters[i]);  // 计算当前聚类中心
            std::pair<float, float> xyMeans1;
            computeXYMean(tmpclusters[i], xyMeans1);  // 计算当前聚类的xy中心

            for(int j = 1; j < numCluster; j++)  // 从第1个到最后一个聚类中心，进行合并
            {
                if(toBeMerge[j])
                {
                    continue;
                }

                float means2 = computeClusterMean(tmpclusters[j]);  // 计算第二个聚类中心的平均距离
                std::pair<float, float> xyMeans2;
                computeXYMean(tmpclusters[j], xyMeans2);  // 计算第二个聚类中心的xy平均

                if(abs(means1 - means2) < 2*distanceTh 
                    && abs(xyMeans1.first - xyMeans2.first) < 2*distanceTh 
                    && abs(xyMeans1.second - xyMeans2.second) < 2*distanceTh)
                {
                    mToBeMergeInd.insert(std::make_pair(i, j));  // 如果聚类中心小于距离阈值，则存储该合并点对
                    sNeedMergeInd.insert(i);
                    toBeMerge[i] = true;
                    toBeMerge[j] = true;
                }
            }

        }

        if(sNeedMergeInd.empty())
        {
            for(int i = 0; i < numCluster; i++)
            {
                clusters.emplace_back(tmpclusters[i]);  // 不需要merge的时候，直接加入到聚类中
            }
        }
        else
        {
            for(int i = 0; i < numCluster; i++)
            {
                if(toBeMerge[i] == false)
                {
                    clusters.emplace_back(tmpclusters[i]);  // 1. 单独聚类的，周围没有可以合并的聚类，直接添加
                }
            }
            
            //2. 需要合并的，都加入到needMergeInd聚类中
            for(auto setIt = sNeedMergeInd.begin(); setIt != sNeedMergeInd.end(); ++setIt)
            {
                int needMergeInd = *setIt;
                auto entries = mToBeMergeInd.count(needMergeInd);
                auto iter = mToBeMergeInd.find(needMergeInd);
                vector<int> vInd;

                while(entries)
                {
                    int ind = iter->second;
                    vInd.emplace_back(ind);
                    ++iter;
                    --entries;
                }

                clusters.emplace_back(tmpclusters[needMergeInd]);
                size_t numCluster = clusters.size();

                for(size_t j = 0; j < vInd.size(); j++)
                {
                    for(size_t ptNum = 0; ptNum < tmpclusters[vInd[j]].size(); ptNum++)
                    {
                        clusters[numCluster - 1].emplace_back(tmpclusters[vInd[j]][ptNum]);//把点加入到这个类别中
                    }
                }
            }
        }       
    }

    void LinK3D_Extractor::computeDirection(pcl::PointXYZI ptFrom, pcl::PointXYZI ptTo, Eigen::Vector2f &direction)
    {
        direction(0, 0) = ptTo.x - ptFrom.x;
        direction(1, 0) = ptTo.y - ptFrom.y;
    }

    // 得到聚类点，相同的均值距离上，只选择一个聚类，舍弃掉其他聚类
    vector<pcl::PointXYZI> LinK3D_Extractor::getMeanKeyPoint(const ScanEdgePoints &clusters, ScanEdgePoints &validCluster)
    {        
        int count = 0;
        int numCluster = clusters.size();
        vector<pcl::PointXYZI> keyPoints;
        vector<pcl::PointXYZI> tmpKeyPoints;
        ScanEdgePoints tmpEdgePoints;
        map<float, int> distanceOrder;

        for(int i = 0; i < numCluster; i++)
        {
            int ptCnt = clusters[i].size();      
            if(ptCnt < ptNumTh)
            {
                continue;
            }

            vector<PointXYZSCA> tmpCluster;
            set<int> scans;
            float x = 0, y = 0, z = 0, intensity = 0;
            for(int ptNum = 0; ptNum < ptCnt; ptNum++)  // 计算一个聚类中点的xyz均值和强度均值
            {
                PointXYZSCA pt = clusters[i][ptNum];          
                int scan = int(pt.scan_position);
                scans.insert(scan);

                x += pt.x;
                y += pt.y;
                z += pt.z;
                intensity += pt.scan_position;
            }

            if(scans.size() < (size_t)scanNumTh)
            {
                continue;
            }

            pcl::PointXYZI pt;  // 均值点
            pt.x = x/ptCnt;
            pt.y = y/ptCnt;
            pt.z = z/ptCnt;
            pt.intensity = intensity/ptCnt;

            float distance = pt.x * pt.x + pt.y * pt.y + pt.z * pt.z;  // 计算均值点的模长

            auto iter = distanceOrder.find(distance);
            if(iter != distanceOrder.end())  // 如果已经有这个长度了，则直接continue，相当于是这个聚类也不要，相同的距离上只要一个聚类
            {
                continue;
            }

            distanceOrder[distance] = count;  // <distance, count>  count相当于是序号
            count++;
            
            tmpKeyPoints.emplace_back(pt);  // 加入均值点
            tmpEdgePoints.emplace_back(clusters[i]);  // 加入聚类中的点       
        }

        for(auto iter = distanceOrder.begin(); iter != distanceOrder.end(); iter++)
        {
            int index = (*iter).second;  // 第index个聚类
            pcl::PointXYZI tmpPt = tmpKeyPoints[index];  // 得到这个聚类中的所有点
            
            keyPoints.emplace_back(tmpPt);  // 关键点中加入该均值点
            validCluster.emplace_back(tmpEdgePoints[index]);  // 加入该聚类中的所有点 tmpEdgePoints[index] 表示一个聚类
        }
                
        return keyPoints;
    }

    float LinK3D_Extractor::fRound(float in)
    {
        float f;
        int temp = std::round(in * 10);
        f = temp/10.0;
        
        return f;
    }
    /*
    * 
       0. 传进去的点都是聚类的均值点，其他的点都不再要了
       1.每一点找最邻近点三个点
       2.该点和最邻近的一个点作为主方向，该点和其他除了（该点和该最邻近点）所构成的方向为otherdir，计算otherdir于主方向之间的夹角。
       夹角空间划分为180份，计算的夹角投影到这180份中，每一份存储了 该点和其他点所构成的距离
       3.遍历这180份，如果一份上存储距离数量为0，则跳过，如果不为0，则对距离进行排序，选择最短距离作为描述存储到descriptors的第i行第该份中
      
    */
    void LinK3D_Extractor::getDescriptors(const vector<pcl::PointXYZI> &keyPoints, 
                                          cv::Mat &descriptors)
    {
        if(keyPoints.empty())
        {
            return;
        }

        int ptSize = keyPoints.size();  // 聚类的个数 （均值点个数）

        descriptors = cv::Mat::zeros(ptSize, 180, CV_32FC1); 

        vector<vector<float>> distanceTab;
        vector<float> oneRowDis(ptSize, 0);
        distanceTab.resize(ptSize, oneRowDis);  // ptsize方阵

        vector<vector<Eigen::Vector2f>> directionTab;
        Eigen::Vector2f direct(0, 0);
        vector<Eigen::Vector2f> oneRowDirect(ptSize, direct);
        directionTab.resize(ptSize, oneRowDirect);

        // 构建距离表和方向表
        //Build distance and direction tables for fast descriptor generation.
        for(size_t i = 0; i < keyPoints.size(); i++)
        {
            for(size_t j = i+1; j < keyPoints.size(); j++)
            {
                float dist = distPt2Pt(keyPoints[i], keyPoints[j]);
                distanceTab[i][j] = fRound(dist);  // 第i个聚类和第j个聚类之间的距离
                distanceTab[j][i] = distanceTab[i][j];  // 对称矩阵i->j的距离和j->i的距离相等

                Eigen::Vector2f tmpDirection;
                                
                tmpDirection(0, 0) = keyPoints[j].x - keyPoints[i].x;  // 第i个聚类和第j个聚类之间的方向
                tmpDirection(1, 0) = keyPoints[j].y - keyPoints[i].y;

                directionTab[i][j] = tmpDirection;  // 方向表
                directionTab[j][i] = -tmpDirection;
            }
        }

        for(size_t i = 0; i < keyPoints.size(); i++)
        {
            vector<float> tempRow(distanceTab[i]);  // 存储了距离的排序
            std::sort(tempRow.begin(), tempRow.end());
            int Index[3];
           
            //Get the closest three keypoints of current keypoint.
            for(int k = 0; k < 3; k++)
            {                
                vector<float>::iterator it1 = find(distanceTab[i].begin(), distanceTab[i].end(), tempRow[k+1]); //计算最小距离对应的迭代器
                if(it1 == distanceTab[i].end())
                {
                    continue;
                }
                else
                {
                    Index[k] = std::distance(distanceTab[i].begin(), it1);  // 计算最小距离对应的索引
                }
            }

            //Generate the descriptor for each closest keypoint. 
            //The final descriptor is based on the priority of the three closest keypoint.
            for(int indNum = 0; indNum < 3; indNum++)
            {
                int index = Index[indNum];
                Eigen::Vector2f mainDirection;
                mainDirection = directionTab[i][index]; // 第i个点和第index个点构成主方向
                
                vector<vector<float>> areaDis(180);  //第i点和 最近的一点 统计 areaDis
                areaDis[0].emplace_back(distanceTab[i][index]);
                          
                for(size_t j = 0; j < keyPoints.size(); j++)  // 非第i个点，也非第index个点
                {
                    if(j == i || (int)j == index)
                    {
                        continue;
                    }
                    
                    Eigen::Vector2f otherDirection = directionTab[i][j];
                
                    Eigen::Matrix2f matrixDirect;
                    matrixDirect << mainDirection(0, 0), mainDirection(1, 0), otherDirection(0, 0), otherDirection(1, 0);
                    float deter = matrixDirect.determinant();

                    // 其他方向和 第i点于第indNum最短距离均值点 之间的方向夹角
                    int areaNum = 0;
                    double cosAng = (double)mainDirection.dot(otherDirection) / (double)(mainDirection.norm() * otherDirection.norm());                                 
                    if(abs(cosAng) - 1 > 0)
                    {   
                        continue;
                    }
                                       
                    float angle = acos(cosAng) * 180 / M_PI;
                    
                    if(angle < 0 || angle > 180)
                    {
                        continue;
                    }
                    
                    if(deter > 0)
                    {
                        areaNum = ceil((angle - 1) / 2);  // 还是分成180份                         
                    }
                    else
                    {
                        if(angle - 2 < 0)
                        { 
                            areaNum = 0;
                        }
                        else
                        {
                            angle = 360 - angle;
                            areaNum = ceil((angle - 1) / 2); 
                        }   
                    }

                    if(areaNum != 0)
                    {
                        areaDis[areaNum].emplace_back(distanceTab[i][j]);  // 该角度下加入ij距离
                    }
                }
                
                float *descriptor = descriptors.ptr<float>(i);                                

                for(int areaNum = 0; areaNum < 180; areaNum++) 
                {
                    if(areaDis[areaNum].size() == 0)//存储距离数为0的话，则跳过
                    {
                        continue;
                    }
                    else
                    {
                        std::sort(areaDis[areaNum].begin(), areaDis[areaNum].end());//选择存储距离数排序

                        if(descriptor[areaNum] == 0)
                        {  // 形象的来说就是 descriptor[areaNum] 中存储的就是该角度扇区下的聚类中心到 其它的聚类中心的距离
                            descriptor[areaNum] = areaDis[areaNum][0]; //如果sort是从小到达排序，则descriptor[areaNum]中存储了 areanum角度中存储的最小距离， areaNum表示角度
                        }                        
                    }
                }                
            }            
        }
    }
    /*
      1.对比两个特征点的descriptor，保留点对和得分
      2. 筛掉一对多的情况
    */
    void LinK3D_Extractor::match(
            vector<pcl::PointXYZI> &curAggregationKeyPt, 
            vector<pcl::PointXYZI> &toBeMatchedKeyPt,
            cv::Mat &curDescriptors, 
            cv::Mat &toBeMatchedDescriptors, 
            vector<pair<int, int>> &vMatchedIndex)
    {        
        int curKeypointNum = curAggregationKeyPt.size();
        int toBeMatchedKeyPtNum = toBeMatchedKeyPt.size();
        
        multimap<int, int> matchedIndexScore;      
        multimap<int, int> mMatchedIndex;
        set<int> sIndex;
       
        //查找每一个点和对应点的最高得分
        for(int i = 0; i < curKeypointNum; i++)
        {
            std::pair<int, int> highestIndexScore(0, 0);
            float* pDes1 = curDescriptors.ptr<float>(i);
            
            for(int j = 0; j < toBeMatchedKeyPtNum; j++)
            {
                int sameDimScore = 0;
                float* pDes2 = toBeMatchedDescriptors.ptr<float>(j); 
                
                for(int bitNum = 0; bitNum < 180; bitNum++)
                {                    
                    if(pDes1[bitNum] != 0 && pDes2[bitNum] != 0 && abs(pDes1[bitNum] - pDes2[bitNum]) <= 0.2){  //该bin上最短距离小于0.2
                        sameDimScore += 1;
                    }
                    
                    if(bitNum > 90 && sameDimScore < 3){ // 如果遍历数量大于90还没找到3个相似的bin，则直接break，认为是这两帧不匹配
                        break;                        
                    }                    
                }
               
                if(sameDimScore > highestIndexScore.second)
                {
                    highestIndexScore.first = j;
                    highestIndexScore.second = sameDimScore;
                }
            }
            
            //得到 当前帧第i个点 对应的 闭环帧第j个点，的得分索引
            //Used for removing the repeated matches.
            matchedIndexScore.insert(std::make_pair(i, highestIndexScore.second)); //Record i and its corresponding score.
            mMatchedIndex.insert(std::make_pair(highestIndexScore.first, i)); //Record the corresponding match between j and i.
            sIndex.insert(highestIndexScore.first); //Record the index that may be repeated matches.
        }
        /*
           有可能闭环帧一个特征是 当前帧上n个特征的最佳匹配特征，如果闭环帧一个特征点只和当前镇上一个特征点一一对应，则直接vMatchedIndex.emplace_back， 否则选择匹配得分最高的匹配对加入vMatchedIndex
        */
        //Remove the repeated matches.
        for(set<int>::iterator setIt = sIndex.begin(); setIt != sIndex.end(); ++setIt)
        {
            int indexJ = *setIt;
            auto entries = mMatchedIndex.count(indexJ);
            if(entries == 1)
            {
                auto iterI = mMatchedIndex.find(indexJ);
                auto iterScore = matchedIndexScore.find(iterI->second);
                if(iterScore->second >= matchTh)  //如果匹配只有一个，且匹配highestIndexScore.second满足数值，则直接加入匹配对中
                {                    
                    vMatchedIndex.emplace_back(std::make_pair(iterI->second, indexJ));
                }           
            }
            else
            { 
                auto iter1 = mMatchedIndex.find(indexJ);
                int highestScore = 0;
                int highestScoreIndex = -1;

                while(entries)
                {
                    int indexI = iter1->second;
                    auto iterScore = matchedIndexScore.find(indexI);
                    if(iterScore->second > highestScore){
                        highestScore = iterScore->second;
                        highestScoreIndex = indexI;
                    }                
                    ++iter1;
                    --entries;
                }

                if(highestScore >= matchTh)  //找对最高的匹配对，只留下来最高得分的匹配点
                {                                       
                    vMatchedIndex.emplace_back(std::make_pair(highestScoreIndex, indexJ));                    
                }            
            }
        }
    }


    /*
       把每一个聚类的点按照行存起来，然后把没一个聚类中的每一行只保留一个点存起来
    */
    //Remove the edge keypoints with low curvature for further edge keypoints matching.
    void LinK3D_Extractor::filterLowCurv(ScanEdgePoints &clusters, ScanEdgePoints &filtered)
    {
        int numCluster = clusters.size();
        filtered.resize(numCluster);
        for(int i = 0; i < numCluster; i++)
        {
            int numPt = clusters[i].size();
            ScanEdgePoints tmpCluster;
            vector<int> vScanID;

            //每一个聚类 按照行索引进行排序
            for(int j = 0; j < numPt; j++)
            {
                PointXYZSCA pt = clusters[i][j];
                int scan = int(pt.scan_position);
                auto it = std::find(vScanID.begin(), vScanID.end(), scan);

                if(it == vScanID.end())
                {
                    vScanID.emplace_back(scan); // 第i个聚类中的第j点的行id加入到vScanID
                    vector<PointXYZSCA> vPt(1, pt);
                    tmpCluster.emplace_back(vPt);
                }
                else
                {
                    int filteredInd = std::distance(vScanID.begin(), it);//如果该行已经有点存进去了，则查找行号
                    tmpCluster[filteredInd].emplace_back(pt);  // 然后把点存入到vector中
                }
            }

            /*
               每一行上只保留一个点，如果有多个点，则只保留点曲率最大的点
            */

            for(size_t scanID = 0; scanID < tmpCluster.size(); scanID++)
            {
                if(tmpCluster[scanID].size() == 1)  // 该行上聚类点个数是一个
                {
                    filtered[i].emplace_back(tmpCluster[scanID][0]);  // 则filtered 直接push该点
                }
                else
                {
                    float maxCurv = 0;
                    PointXYZSCA maxCurvPt;
                    for(size_t j = 0; j < tmpCluster[scanID].size(); j++)
                    {
                        if(tmpCluster[scanID][j].curvature > maxCurv)  // 有多个点的时候，只保留最大曲率的点
                        {
                            maxCurv = tmpCluster[scanID][j].curvature;
                            maxCurvPt = tmpCluster[scanID][j];
                        }
                    }

                    filtered[i].emplace_back(maxCurvPt);  // filtered 直接push该点
                }
            }  
        }
    }

    /*
    
       根据vMatched 得到匹配的聚类index，根据聚类索引计算对应的行，每一行自动成为匹配点
    */

    //Get the edge keypoint matches based on the matching results of aggregation keypoints.
    void LinK3D_Extractor::findEdgeKeypointMatch(
            ScanEdgePoints &filtered1, 
            ScanEdgePoints &filtered2, 
            vector<std::pair<int, int>> &vMatched, 
            vector<std::pair<PointXYZSCA, PointXYZSCA>> &matchPoints)
    {
        int numMatched = vMatched.size();
        for(int i = 0; i < numMatched; i++)
        {
            pair<int, int> matchedInd = vMatched[i];  //匹配聚类的索引
                        
            int numPt1 = filtered1[matchedInd.first].size();  //过滤点集合1的点个数 
            int numPt2 = filtered2[matchedInd.second].size(); //过滤点集合2的点个数 

            map<int, int> mScanID_Index1;
            map<int, int> mScanID_Index2;

            for(int i = 0; i < numPt1; i++)
            {
                int scanID1 = int(filtered1[matchedInd.first][i].scan_position);
                pair<int, int> scanID_Ind(scanID1, i);
                mScanID_Index1.insert(scanID_Ind);  //得到（行，点号）
            }

            for(int i = 0; i < numPt2; i++)
            {
                int scanID2 = int(filtered2[matchedInd.second][i].scan_position);
                pair<int, int> scanID_Ind(scanID2, i);
                mScanID_Index2.insert(scanID_Ind);  //得到（行，点号）
            }

            for(auto it1 = mScanID_Index1.begin(); it1 != mScanID_Index1.end(); it1++)
            {
                int scanID1 = (*it1).first;
                auto it2 = mScanID_Index2.find(scanID1);  // 找到相同行的点
                if(it2 == mScanID_Index2.end()){
                    continue;
                }
                else
                {
                    vector<PointXYZSCA> tmpMatchPt;
                    PointXYZSCA pt1 = filtered1[matchedInd.first][(*it1).second];  //得到点1
                    PointXYZSCA pt2 = filtered2[matchedInd.second][(*it2).second];  // 得到点2
                    
                    pair<PointXYZSCA, PointXYZSCA> matchPt(pt1, pt2);
                    matchPoints.emplace_back(matchPt);  // 构建匹配点
                }
            }
        }
    }

    void LinK3D_Extractor::operator()(pcl::PointCloud<pcl::PointXYZ>::Ptr pLaserCloudIn, vector<pcl::PointXYZI> &keyPoints, cv::Mat &descriptors, ScanEdgePoints &validCluster)
    {
        ScanEdgePoints edgePoints;
        extractEdgePoint(pLaserCloudIn, edgePoints);  // 计算了每一个点的曲率值

        ScanEdgePoints sectorAreaCloud;
        divideArea(edgePoints, sectorAreaCloud);   // 把360度的角度分成120份，根据点的角度，计算点所落在的扇区，并把点存储在该区域中

        ScanEdgePoints clusters;
        getCluster(sectorAreaCloud, clusters); //扇区内点聚类，聚类完成后，把类别相邻的进行合并
        
        vector<int> index;
        keyPoints = getMeanKeyPoint(clusters, validCluster);  // 相同的距离上只保留一个聚类，
          
        getDescriptors(keyPoints, descriptors); 
    }

}

Bow3d.cpp

namespace BoW3D
{
    BoW3D::BoW3D(LinK3D_Extractor* pLinK3D_Extractor, float thr_, int thf_, int num_add_retrieve_features_): 
            mpLinK3D_Extractor(pLinK3D_Extractor), 
            thr(thr_), 
            thf(thf_), 
            num_add_retrieve_features(num_add_retrieve_features_)
    {
       N_nw_ofRatio = std::make_pair(0, 0); //first存储的唯一单词个数，second存储了单词的出现次数
    }
    
    /*
    
      当前帧的每一个聚类均值点 的描述 列像素中，如果不为空（列像素的值存储了该角度下与该均值点最小的距离），则转换成一个单词，map[descriptor(i,j)， i] = {[frameId, i]},更新N_nw_ofRatio
    */

    void BoW3D::update(Frame* pCurrentFrame)
    {
        mvFrames.emplace_back(pCurrentFrame);

        cv::Mat descriptors = pCurrentFrame->mDescriptors;
        long unsigned int frameId = pCurrentFrame->mnId;

        size_t numFeature = descriptors.rows;

        if(numFeature < (size_t)num_add_retrieve_features) 
        {
            for(size_t i = 0; i < numFeature; i++)
            {
                float *p = descriptors.ptr<float>(i);
                for(size_t j = 0; j < (size_t)descriptors.cols; j++)
                {
                    if(p[j] != 0)
                    {
                        unordered_map<pair<float, int>, unordered_set<pair<int, int>, pair_hash>, pair_hash>::iterator it; 

                        pair<float, int> word= make_pair(p[j], j);  // descriptor(i,j)， i表示第i个特征点，j表示第j个单词
                        it = this->find(word);

                        if(it == this->end())
                        {
                            unordered_set<pair<int,int>, pair_hash> place;
                            place.insert(make_pair(frameId, i));  // 帧id 第i个特征
                            (*this)[word] = place;

                            N_nw_ofRatio.first++;
                            N_nw_ofRatio.second++;
                        }
                        else
                        {
                            (*it).second.insert(make_pair(frameId, i));
                            N_nw_ofRatio.second++;
                        }

                    }
                }
            }
        }
        else
        {
            for(size_t i = 0; i < (size_t)num_add_retrieve_features; i++)
            {
                float *p = descriptors.ptr<float>(i);
                for(size_t j = 0; j < (size_t)descriptors.cols; j++)
                {
                    if(p[j] != 0)
                    {
                        unordered_map<pair<float, int>, unordered_set<pair<int, int>, pair_hash>, pair_hash>::iterator it; 

                        pair<float, int> word= make_pair(p[j], j);
                        it = this->find(word);

                        if(it == this->end())
                        {
                            unordered_set<pair<int,int>, pair_hash> place;
                            place.insert(make_pair(frameId, i));
                            (*this)[word] = place;

                            N_nw_ofRatio.first++;  // 首次加入单词的时候，first和second都加1
                            N_nw_ofRatio.second++;
                        }
                        else
                        {
                            (*it).second.insert(make_pair(frameId, i));

                            N_nw_ofRatio.second++;  // 再次加入单词的时候，second加1，相除的话表示一个单词被平均统计了几次
                        }
                    }
                }
            }
        }
    }
       

    void BoW3D::retrieve(Frame* pCurrentFrame, int &loopFrameId, Eigen::Matrix3d &loopRelR, Eigen::Vector3d &loopRelt)
    {        
        int frameId = pCurrentFrame->mnId;

        cv::Mat descriptors = pCurrentFrame->mDescriptors;      
        size_t rowSize = descriptors.rows;       
        
        map<int, int>mScoreFrameID;

        if(rowSize < (size_t)num_add_retrieve_features) 
        {
            for(size_t i = 0; i < rowSize; i++) // 当前帧聚类数（聚类均值点个数）
            {
                unordered_map<pair<int, int>, int, pair_hash> mPlaceScore;                
                                
                float *p = descriptors.ptr<float>(i);

                int countValue = 0;

                for(size_t j = 0; j < (size_t)descriptors.cols; j++)
                {
                    countValue++;

                    if(p[j] != 0)
                    {                   
                        pair<float, int> word = make_pair(p[j], j);  // （描述，列矢量）
                        auto wordPlacesIter = this->find(word);

                        if(wordPlacesIter == this->end())
                        {
                            continue;
                        }
                        else
                        {
                            double averNumInPlaceSet = N_nw_ofRatio.second / N_nw_ofRatio.first;//粗糙词频
                            int curNumOfPlaces = (wordPlacesIter->second).size(); // 当前单词数量
                            
                            double ratio = curNumOfPlaces / averNumInPlaceSet; // 当前单词的辨识度

                            if(ratio > thr) // 当前单词不具有啥辨识度
                            {
                                continue;
                            }

                            /*
                               统计和当前帧帧差数量大于300的帧
                               2. 理论上如果当前帧第i个点和 某一帧第placesIter.second个点相似，descriptor180份中没有0，则单词数量会存储180份。 也就是越相似，两帧的相同点单词重合数量约束
                            */
                            for(auto placesIter = (wordPlacesIter->second).begin(); placesIter != (wordPlacesIter->second).end(); placesIter++)
                            {
                                //The interval between the loop and the current frame should be at least 300.
                                if(frameId - (*placesIter).first < 300)  // 帧id有要求 placesIter（frameid，i）
                                {
                                    continue;
                                }

                                auto placeNumIt = mPlaceScore.find(*placesIter);          //placesIter 表示（帧，第i个特征）           
                                
                                if(placeNumIt == mPlaceScore.end())
                                {                                
                                    mPlaceScore[*placesIter] = 1;  // 该单词统计次数
                                }
                                else
                                {
                                    mPlaceScore[*placesIter]++;    //该单词统计次数加1                                
                                }                                                              
                            }                       
                        }                            
                    }                    
                }

                for(auto placeScoreIter = mPlaceScore.begin(); placeScoreIter != mPlaceScore.end(); placeScoreIter++)
                {
                    if((*placeScoreIter).second > thf)   // 统计次数大于thf阈值
                    {
                       mScoreFrameID[(*placeScoreIter).second] = ((*placeScoreIter).first).first;  //score, frameid
                    }
                }                                   
            }                  
        }
        else
        {
            for(size_t i = 0; i < (size_t)num_add_retrieve_features; i++) 
            {
                unordered_map<pair<int, int>, int, pair_hash> mPlaceScore;
                
                float *p = descriptors.ptr<float>(i);

                int countValue = 0;

                for(size_t j = 0; j < (size_t)descriptors.cols; j++)
                {
                    countValue++;

                    if(p[j] != 0)
                    {                   
                        pair<float, int> word = make_pair(p[j], j);    

                        auto wordPlacesIter = this->find(word);

                        if(wordPlacesIter == this->end())
                        {
                            continue;
                        }
                        else
                        {
                            double averNumInPlaceSet = (double) N_nw_ofRatio.second / N_nw_ofRatio.first;
                            int curNumOfPlaces = (wordPlacesIter->second).size();

                            double ratio = curNumOfPlaces / averNumInPlaceSet;

                            if(ratio > thr)
                            {
                                continue;
                            }
                            
                            for(auto placesIter = (wordPlacesIter->second).begin(); placesIter != (wordPlacesIter->second).end(); placesIter++)
                            {
                                //The interval between the loop and the current frame should be at least 300.
                                if(frameId - (*placesIter).first < 300) 
                                {
                                    continue;
                                }

                                auto placeNumIt = mPlaceScore.find(*placesIter);                    
                                
                                if(placeNumIt == mPlaceScore.end())
                                {                                
                                    mPlaceScore[*placesIter] = 1;
                                }
                                else
                                {
                                    mPlaceScore[*placesIter]++;                                    
                                }                                                              
                            }                       
                        }                            
                    }
                }

                for(auto placeScoreIter = mPlaceScore.begin(); placeScoreIter != mPlaceScore.end(); placeScoreIter++)
                {
                    if((*placeScoreIter).second > thf) 
                    {
                       mScoreFrameID[(*placeScoreIter).second] = ((*placeScoreIter).first).first;  // 这个就是聚类中心对应起来了
                    }
                }                                   
            }                           
        }     

        if(mScoreFrameID.size() == 0)
        {
            return;
        }

        for(auto it = mScoreFrameID.rbegin(); it != mScoreFrameID.rend(); it++)
        {          
            int loopId = (*it).second;

            Frame* pLoopFrame = mvFrames[loopId];  // 闭环帧
            vector<pair<int, int>> vMatchedIndex;  
            
            mpLinK3D_Extractor->match(pCurrentFrame->mvAggregationKeypoints, pLoopFrame->mvAggregationKeypoints, pCurrentFrame->mDescriptors, pLoopFrame->mDescriptors, vMatchedIndex);               

            int returnValue = 0;
            Eigen::Matrix3d loopRelativeR;
            Eigen::Vector3d loopRelativet;
                                
            returnValue = loopCorrection(pCurrentFrame, pLoopFrame, vMatchedIndex, loopRelativeR, loopRelativet);  // 得到匹配相对变换

            //The distance between the loop and the current should less than 3m.                  
            if(returnValue != -1 && loopRelativet.norm() < 3 && loopRelativet.norm() > 0)  // 得到闭环
            {
                loopFrameId = (*it).second;
                loopRelR = loopRelativeR;
                loopRelt = loopRelativet;                         
                
                return;
            }     
        } 
    }


    int BoW3D::loopCorrection(Frame* currentFrame, Frame* matchedFrame, vector<pair<int, int>> &vMatchedIndex, Eigen::Matrix3d &R, Eigen::Vector3d &t)
    {
        if(vMatchedIndex.size() <= 30)
        {
            return -1;
        }

        ScanEdgePoints currentFiltered;
        ScanEdgePoints matchedFiltered;
        mpLinK3D_Extractor->filterLowCurv(currentFrame->mClusterEdgeKeypoints, currentFiltered);//每一个聚类的每一行中选择一个曲率最大的点
        mpLinK3D_Extractor->filterLowCurv(matchedFrame->mClusterEdgeKeypoints, matchedFiltered);

        vector<std::pair<PointXYZSCA, PointXYZSCA>> matchedEdgePt;
        mpLinK3D_Extractor->findEdgeKeypointMatch(currentFiltered, matchedFiltered, vMatchedIndex, matchedEdgePt); // 根据vMatchedIndex所存储的匹配的聚类，计算匹配点
        
        /*
            下面的代码是根据匹配点计算相对变换矩阵
        */

        pcl::PointCloud<pcl::PointXYZ>::Ptr source(new pcl::PointCloud<pcl::PointXYZ>());
        pcl::PointCloud<pcl::PointXYZ>::Ptr target(new pcl::PointCloud<pcl::PointXYZ>());

        pcl::CorrespondencesPtr corrsPtr (new pcl::Correspondences()); 

        for(int i = 0; i < (int)matchedEdgePt.size(); i++)
        {
            std::pair<PointXYZSCA, PointXYZSCA> matchPoint = matchedEdgePt[i];

            pcl::PointXYZ sourcePt(matchPoint.first.x, matchPoint.first.y, matchPoint.first.z);            
            pcl::PointXYZ targetPt(matchPoint.second.x, matchPoint.second.y, matchPoint.second.z);
            
            source->push_back(sourcePt);
            target->push_back(targetPt);

            pcl::Correspondence correspondence(i, i, 0);
            corrsPtr->push_back(correspondence);
        }
        // 移除了错误匹配点
        pcl::Correspondences corrs;
        pcl::registration::CorrespondenceRejectorSampleConsensus<pcl::PointXYZ> Ransac_based_Rejection;
        Ransac_based_Rejection.setInputSource(source);
        Ransac_based_Rejection.setInputTarget(target);
        double sac_threshold = 0.4;
        Ransac_based_Rejection.setInlierThreshold(sac_threshold);
        Ransac_based_Rejection.getRemainingCorrespondences(*corrsPtr, corrs);

        if(corrs.size() <= 100)
        {
            return -1;
        }       
                
        Eigen::Vector3d p1 = Eigen::Vector3d::Zero();
        Eigen::Vector3d p2 = p1;
        int corrSize = (int)corrs.size();
        for(int i = 0; i < corrSize; i++)
        {  
            pcl::Correspondence corr = corrs[i];         
            p1(0) += source->points[corr.index_query].x;
            p1(1) += source->points[corr.index_query].y;
            p1(2) += source->points[corr.index_query].z; 

            p2(0) += target->points[corr.index_match].x;
            p2(1) += target->points[corr.index_match].y;
            p2(2) += target->points[corr.index_match].z;
        }

        Eigen::Vector3d center1 = Eigen::Vector3d(p1(0)/corrSize, p1(1)/corrSize, p1(2)/corrSize);
        Eigen::Vector3d center2 = Eigen::Vector3d(p2(0)/corrSize, p2(1)/corrSize, p2(2)/corrSize);
       
        // 加入移除了重心的点
        vector<Eigen::Vector3d> vRemoveCenterPt1, vRemoveCenterPt2; 
        for(int i = 0; i < corrSize; i++)
        {
            pcl::Correspondence corr = corrs[i];
            pcl::PointXYZ sourcePt = source->points[corr.index_query];
            pcl::PointXYZ targetPt = target->points[corr.index_match];

            Eigen::Vector3d removeCenterPt1 = Eigen::Vector3d(sourcePt.x - center1(0), sourcePt.y - center1(1), sourcePt.z - center1(2));
            Eigen::Vector3d removeCenterPt2 = Eigen::Vector3d(targetPt.x - center2(0), targetPt.y - center2(1), targetPt.z - center2(2));
        
            vRemoveCenterPt1.emplace_back(removeCenterPt1);
            vRemoveCenterPt2.emplace_back(removeCenterPt2);
        }

        Eigen::Matrix3d w = Eigen::Matrix3d::Zero();

        for(int i = 0; i < corrSize; i++)
        {
            w += vRemoveCenterPt1[i] * vRemoveCenterPt2[i].transpose();
        }      
        //计算旋转和平移矩阵
        Eigen::JacobiSVD<Eigen::Matrix3d> svd(w, Eigen::ComputeFullU|Eigen::ComputeFullV);
        Eigen::Matrix3d U = svd.matrixU();
        Eigen::Matrix3d V = svd.matrixV();
        
        R = V * U.transpose();
        t = center2 - R * center1;

        return 1;
    }

}