可视化两个点云的特征匹配点对+语义筛除错误对应点

一、3.28日尝试了利用语义信息筛除语义标签不匹配的对应点。

具体做法是：
根据source和target点云每个点的fpfh特征，匹配得到对应点，用变量result表示。result中包含transformation matrix，利用result.transformation计算出information matrix。

删除result对应点对中语义标签不一致的点对(这里用颜色表示)，对剩下的点对 new_result重新进行transformation matrix的估计，得到new_result.transformation。

根据new_result.transformation重新计算information矩阵。

部分代码如下：

    if(result is not None):
        correspondence_set_new=[]      #用于筛选颜色(语义)标签正确的对应点对
         #correspondence:一对对应点的序号     result.correspondence_set存储所有的对应点对的序号
        for correspondence in np.asarray(result.correspondence_set) :
             #保留颜色(语义)标签相同的对应点对
            if abs(np.asarray(source.colors)[correspondence[0]][0]-np.asarray(target.colors)[correspondence[1]][0])<0.01 and\
            abs(np.asarray(source.colors)[correspondence[0]][1]-np.asarray(target.colors)[correspondence[1]][1])<0.01 and\
                abs(np.asarray(source.colors)[correspondence[0]][2]-np.asarray(target.colors)[correspondence[1]][2])<0.01: 
                correspondence_set_new.append([correspondence[0],correspondence[1]])        
        
        #输入语义筛选后的点对correspondence_set_new，重新计算transformation
        correspondence_set_new=o3d.utility.Vector2iVector(correspondence_set_new
        new_result=o3d.pipelines.registration.registration_ransac_based_on_correspondence(           
            source, target, correspondence_set_new, distance_threshold,
            o3d.pipelines.registration.TransformationEstimationPointToPoint(
                False), 3,
            [
                o3d.pipelines.registration.
                CorrespondenceCheckerBasedOnEdgeLength(0.9),
                o3d.pipelines.registration.CorrespondenceCheckerBasedOnDistance(
                    distance_threshold)
            ],
            o3d.pipelines.registration.RANSACConvergenceCriteria(100000, 0.999))
        
        #对比一下语义筛选对应点对前后的transformation matrix
        print('old transformation is :',result.transformation)
        print('new transformation is :',new_result.transformation)
        print('num of second ransac corres point',len(new_result.correspondence_set))
    #通过transformation 对source和target点云进行变换，变换后通过最近邻搜索得到对应点对，由这些对应点对的坐标构成information               输入new_reslut,得到新的information
    information = o3d.pipelines.registration.get_information_matrix_from_point_clouds(            
        source, target, distance_threshold/3, new_result.transformation)

二、可视化两个点云的特征匹配点对

    #用于显示对应点对，显示source和target点云的同时，将两个对应点对连接成线显示
    if(result is not None):
        points = []
        lines=[]
               #correspondence:一对对应点的序号     result.correspondence_set存储所有的对应点对的序号
        for correspondence in np.asarray(result.correspondence_set) :
            points.append(np.asarray(source.points)[correspondence[0]])
            points.append(np.asarray(target.points)[correspondence[1]])           
        for i in range(0,len(np.asarray(result.correspondence_set))-1):
            lines.append([2*i,2*i+1])
         # 选取对应点连线为   黑   色
        colors = [[0, 0, 0] for i in range(len(lines))]
    
        line_set = o3d.geometry.LineSet(
            points=o3d.utility.Vector3dVector(points),
            lines=o3d.utility.Vector2iVector(lines),
        )
        line_set.colors = o3d.utility.Vector3dVector(colors)      
    o3d.visualization.draw_geometries([source,target,line_set])

显示效果如下图：