主要内容

trimLoopClosures

优化姿态图，消除坏循环封闭

折叠所有页面

语法

poseGraphUpdated = trimLoopClosures (poseGraphObj trimParams solverOptions)
[poseGraphUpdated, trimInfo] = trimLoopClosures (poseGraphObj、trimParams solverOptions)

描述

poseGraphUpdated= trimLoopClosures (poseGraphObj，trimParams，solverOptions）对位姿图进行优化，使其最能满足边缘约束，并根据中指定的残差参数去除任何坏环闭合边trimParams．创建solverOptions输入用poseGraphSolverOptions函数。

该函数结合非最小位姿图解算器实现了带有截断最小二乘鲁棒代价的梯度非凸性(GNC)方法［1］．

例子

［poseGraphUpdated，trimInfo) = trimLoopClosures (poseGraphObj，trimParams，solverOptions）返回与修剪过程相关的附加信息。

例子

全部折叠

打开生活的脚本

基于节点和边约束的位姿图优化。修剪环闭合基于他们的边缘残差。

加载包含二维姿态图的数据集。检查poseGraph对象，以查看节点和循环闭包的数量。

负载grid-2d-posegraph.matpgdisp (pg)
poseGraph with properties: NumNodes: 120 nummedges: 193 NumLoopClosureEdges: 74 LoopClosureEdgeIDs:[120 121 122 123 124 126 127 128 129 130…LandmarkNodeIDs: [1x0双]

在没有id的情况下绘制姿态图。红线表示在数据集中标识的循环闭包。图中的姿势应该遵循网格模式，但显示出随时间推移的漂移迹象。

显示(pg,“id”，“关闭”）;标题(“原来的姿势图”）

图中包含一个轴对象。标题为Original Pose Graph的轴对象包含3个类型为line的对象。

优化姿势握拍使用optimizePoseGraph函数。默认情况下，该函数使用“builtin-trust-region”解算器。由于位姿图包含一些坏循环闭包，因此得到的位姿图实际上是不理想的。

pgOptim = optimizePoseGraph (pg);图;显示(pgOptim);

图中包含一个轴对象。axis对象包含225个类型为line, text的对象。

看看原始姿态图的边缘残差。较大的离群错误值在末尾表示坏循环闭包。

resErrorVec = edgeResidualErrors (pg);情节(resErrorVec);标题('边缘残余误差由边缘ID'）

图中包含一个轴对象。标题为Edge Residual Errors by Edge ID的轴对象包含一个类型为line的对象。

在姿态图中，应根据残差对某些环闭包进行修剪。使用trimLoopClosures函数修剪这些坏循环闭包。设置修剪器参数的最大值和截断阈值。这个阈值是根据测量精度设置的，应该针对您的系统进行调整。

trimParams。MaxIterations = 100;trimParams。TruncationThreshold = 25;solverOptions = poseGraphSolverOptions;

使用trimLoopClosures功能与修剪参数和求解器选项。

[pgNew, trimInfo, debugInfo] = trimLoopClosures(pg,trimParams,solverOptions);

从trimInfo输出，绘制从优化姿态图中移除的循环闭包。通过绘制残差图之前，您可以看到大的错误循环闭包被删除。

removedLCs = trimInfo.LoopClosuresToRemove;持有在情节(removedLCs 0(长度(removedLCs)),”或“)标题(“边缘残余误差和被移除的循环闭包”)传说(“剩余错误”，“删除循环闭包”)包含(“边缘id”) ylabel (“边缘残留误差”)举行从

图中包含一个轴对象。标题为Edge Residual Errors和Removed Loop Closures的axis对象包含45个类型为line的对象。这些对象表示残余错误，已删除的循环闭包。


                   

                  

                 

                 
显示新的姿势图与坏循环闭包修剪。

                 

                  

                   

                    
显示(pgNew“id”，“关闭”）;

                   

                  

                 

                 

                  

                   
图中包含一个轴对象。轴对象包含3个类型为line的对象。

                  

                 

                 

                

               

              

             

             

              
              
             

            

           

           

            
输入参数

            

             
全部折叠

             

              

               

                

                 
                 

                  

                   
姿态图，指定为poseGraph或poseGraph3D对象。

                  

                 

                

               

              

              

               

                

                 
                 

                  

                   
修边环闭包残余误差参数，指定为带字段的结构:

                   

                    
    
                     
  • MaxIterations—允许环路闭合修剪的最大迭代次数，指定为正整数。在一次裁剪迭代中，基于求解器选项和任意边对位姿图进行优化TruncationThreshold修剪。
    • 
                     
  • TruncationThreshold—边缘允许的最大残差。这个值很大程度上取决于你指定的姿态图poseGraphObj．要根据所有错误找到合适的阈值，请使用edgeResidualErrors函数用于姿态图。
    • 
                    

                   

                   

                   
例子:结构(“TruncationThreshold”“MaxIterations”,10日,20)

                   
数据类型:结构体

                  

                 

                

               

              

              

               

                

                 
                 

                  

                   
位姿图形求解器选项，指定为通过调用poseGraphSolverOptions函数。该函数为指定的姿态图形求解器类型生成一组具有默认值的求解器选项:

                   

                    

                     

                      
pgSolverTrustRegion = poseGraphSolverOptions (“builtin-trust-region”）

                     

                    

                   

                   

                    

                     

                      
pgSolverTrustRegion = TrustRegion (builin -trust-region-dogleg)选项:MaxIterations: 300 MaxTime: 10 FunctionTolerance: 1.0000e-08 GradientTolerance: 5.0000e-09 StepTolerance: 1.0000e-12 InitialTrustRegionRadius: 100 VerboseOutput: 'off'

                     

                    

                   

                   

                    

                     

                      
pgSolverG2o = poseGraphSolverOptions (“g2o-levenberg-marquardt”）

                     

                    

                   

                   

                    

                     

                      
pgSolverG2o = G2oLevenbergMarquardt (g2o-levenberg-marquardt)选项:MaxIterations: 300 MaxTime: 10 FunctionTolerance: 1.0000e-09 VerboseOutput: 'off'

                     

                    

                   

                   

                   
修改选项以使用点表示法调优求解器参数。

                   

                    

                     

                      
pgSolverG2o。MaxIterations = 200;

                     

                    

                   

                  

                 

                

               

              

             

            

           

           

            
输出参数

            

             
全部折叠

             

              

               

                
                

                 

                  
带有修剪过的环形闭包的姿态图，指定为poseGraph或poseGraph3D对象。

                 

                

               

              

              

               

                
                

                 

                  
来自修剪过程的信息，作为带有字段的结构返回:

                  

                   
    
                    
  • LoopClosuresToRemove-要从输入中删除的循环闭包边缘idposeGraphObj．这些循环闭包在输出中被删除poseGraphUpdated．
    • 
                    
  • 迭代-执行微调迭代的次数。
    • 
                   

                  

                  

                 

                

               

              

             

            

           

           

            
参考文献

            

             
[1] Yang, Heng, et al.“用于鲁棒空间感知的梯度非凸性:从非最小解到全局异常值拒绝”。IEEE机器人与自动化通讯，第5卷，第5期。2、2020年4月，第1127-34页。DOI.org (Crossref), doi: 10.1109 / LRA.2020.2965893。

            

           

           

            
另请参阅

            
功能

            
            
对象

            
           

           

            介绍了R2020b
           

          

         

         
         

         

          
         

        

        
        
        
       

      

     

    

    
    

     

      

       

        

         
        

       

      

     

    

    
    
    

     

      

       

        

         

          
导航工具的文档

          
         

         

          
金宝app