资讯详情

原创 ORB-SLAM3 单目惯导Localmap

文章目录

  • 简介
    • ORB 一些概念
    • 大体流程：
  • Run
    • 代码流程：
    • 代码注解
  • 其他函数
    • ProcessNewKeyFrame 处理新的关键帧
    • MapPointCulling 最近添加地图点的消除
    • CreateNewMapPoints 创建新的地图点
    • SearchInNeighbors 检查并整合当前关键帧和相邻帧(两级相邻)的重复MapPoints
    • ComputeF12 计算两帧基本矩阵
    • SearchInNeighbors 检查并整合当前关键帧和相邻帧的重复MapPoints
    • ScaleRefinement
    • InitializeIMU 初始化IMU
  • Atlas地图集
    • 成员变量
    • 构造
    • 其他函数
      • 创建地图
      • 改变地图
      • 设置参考地图点
      • 获得所有地图
      • 清除地图集
      • 获得关键帧数和地图点数
  • Map
    • 成员变量
    • 旋转地图

简介

ORB 一些概念

  • 关键帧是连续几帧普通帧中更具代表性的帧。
  • 作用：
    • 减少局部相邻关键帧中的信息冗余
    • 由于在SLAM该方案将普通帧的深度投影到关键帧上，因此在一定程度上，关键帧是普通帧过滤和优化的结果，防止无效和错误信息进入优化过程
    • 后端优化计算能力和精度的妥协，提高计算资源的效率
  • 选择：
    • ：画面清晰，特征点充足，特征点分布均匀等
    • ：与其他关键帧有一定的共视关系，不能重复太高，即既有约束，又减少冗余信息
    • 选择方向:(选择后按上述标准筛选)
      • 与上一帧关键帧之间的时间（帧数）间隔是否合适
      • 上一帧之间的距离够远吗？
      • 跟踪局部地图的质量(共视特征点的数量)

  • ：
    • 任拿一个关键帧，可以观察到与该关键帧相同地图点的关键帧(一级关键帧)。
    • 该关键帧的一级关键帧自身的一级关键帧称为该关键帧的二级关键帧。
    • 注：由于相同关键帧的数量可以观察到来表征。
  • ：
    • 以任何关键帧为中心，共视关键帧和联系构建的共视网。
    • 共视图中的为关键帧；如果两个关键帧共视15个以上的地图点，则使用一个连接两者表示两个关键帧可以共同观察到的点云数量。
  • 通常只使用一级相邻层次的共视关系和信息，局部地图优化采用两级相邻共视关系。
  • ：
    • 增加地图点信息，优化地图
    • 表示关键帧之间的关系和关系的紧密性
  • 具体应用：
    • 跟踪局部地图，扩大搜索范围
    • 局部建图关键帧之间的新地图点
    • 闭环检测、重定位检测
    • 优化

  • 本质图常用于父子关键帧

  • 由关键帧组成的图像。
  • 与共视图相比，本质图比较稀疏
  • 节点表示所有关键帧，但连接边缘只保留与关键帧密切相关的边缘，使结果更准确
  • 图中包含的信息包括：
    • 扩大树木的连接关系
    • 形成闭环连接关系
    • 闭环后，地图点的变化和新的连接关系
    • 良好的共视关系(至少有100个共视地图点)
  • 作用：
    • 闭环矫正时，使用相似变换（Sim3）纠正规模漂移，在本质图中均匀分摊闭环误差
  • 优势：
    • 全局BA 收敛后，局部可能不会收敛，本质图可以保证 局部收敛
    • 可显著减少运行时间
    • 本质图 局部BA 结合在一定程度上可以提高精度，但需要时间。

大体流程：

  1. 设置新的关键帧不能接受到系统，即在局部图纸（处理关键帧）过程中不允许新的关键帧集有变化
  2. 处理新的关键帧。建立新的关键帧及其属性(计算关键帧BOW向量，将关键帧的地图点添加到关键帧中，更新地图点的属性（观察地图点的关键帧、平均观测方向、最佳描述子），并添加到整体情况中map
  3. 检查当前关键帧对应的地图点是否良好(筛选条件:地图点是否好，地图点的搜索率大于0.25, 地图点的第一个关键帧(第一次观察地图点的帧id)与当前帧id相隔距离, 观察关键点的次数)
  4. 极限约束检测特征点，建立地图点加入全局map。用对极约束测量当前帧和共视关键帧(当前帧)(匹配当前帧和关键帧，然后通过三角形生成每对匹配3D点，然后检查(检查两帧中地图点的深度，两帧中的重投影误差，尺度连续性)，如果满足对极约束，建立当前帧的地图点及其属性（a.观测到该MapPoint的关键帧 b.该MapPoint的描述子 c.该MapPoint平均观测方向和深度范围)，将地图点添加到关键帧和全局map.
  5. 数据融合。每组关键帧(目前没有新的关键帧添加到一组关键帧中)处理完成后，对当前关键帧(每组最后一个关键帧)进行关键帧整合(整合当前关键帧及其共视关键帧)，整合地图点(整合当前关键帧的地图点和两级相邻关键帧(关键帧的共视关键帧和所有共视关键帧的共视关键帧)。
  6. 将关键帧交给回环检测
  7. 将当前可接受的新关键帧设置到系统中，即局部构图(处理关闭帧）过程中允许新关键帧集有变动

Run

代码流程：

代码注解

void LocalMapping::Run()
{ 
        
    mbFinished = false;
	
	// while 1 直到标记 mbFinishRequested==true`,则 break
    while(1) 
    { 
        
        // Tracking will see that Local Mapping is busy
        // 设置不在接收关键帧 ，成员变量控制：`mbAcceptKeyFrames=false`
        SetAcceptKeyFrames(false);

        // Check if there are keyframes in the queue
        // 如果有新的关键帧 且 imu_ok 时：
        if(CheckNewKeyFrames() && !mbBadImu)
        { 
        
            // BoW conversion and insertion in Map
            // 对新关键帧进行处理。建立新的关键帧及其属性并加入全局map
            ProcessNewKeyFrame();

            // Check recent MapPoints
            // <font color="blue">地图点的剔除</font>。针对 mlpRecentAddMapPoints，地图点坏的条件：
			// - 地图点本身是坏的
			// - 地图点的查找率小于0.25
			// - 第一次观察到该地图点的帧id与当前帧id相隔 ≥2 且 地图观测数小于 阈值`cnThObs`
            MapPointCulling();
            
            // Triangulate new MapPoints
            // 针对 mlpRecentAddMapPoints，相机运动过程中与相邻关键帧通过三角化恢复出一些MapPoints
            CreateNewMapPoints();

            mbAbortBA = false;
			
			// 已经处理完队列中的最后的一个关键帧
            if(!CheckNewKeyFrames())
            { 
        
                // Find more matches in neighbor keyframes and fuse point duplications
              //检查并融合当前关键帧与相邻帧（两级相邻）重复的MapPoints
              SearchInNeighbors();
            }
            
            bool b_doneLBA = false;
            int num_FixedKF_BA = 0;
            int num_OptKF_BA = 0;
            int num_MPs_BA = 0;
            int num_edges_BA = 0;
			
			// 已经处理完队列中的最后的一个关键帧，并且闭环检测没有请求停止LocalMapping
            if(!CheckNewKeyFrames() && !stopRequested())
            { 
        
                if(mpAtlas->KeyFramesInMap()>2)
                { 
        
					// 使用IMU，且imu已经初始化时：
                    if(mbInertial && mpCurrentKeyFrame->GetMap()->isImuInitialized())
                    { 
        	
                    	// 计算当前关键帧到前一关键帧+前一关键帧到前前关键帧相机中心距离之和
                        float dist = cv::norm(mpCurrentKeyFrame->mPrevKF->GetCameraCenter() - mpCurrentKeyFrame->GetCameraCenter()) +
                                cv::norm(mpCurrentKeyFrame->mPrevKF->mPrevKF->GetCameraCenter() - mpCurrentKeyFrame->mPrevKF->GetCameraCenter());
						
						// 若距离大于 5cm，则 累计 当前帧与前一帧的时间
                        if(dist>0.05)
                            mTinit += mpCurrentKeyFrame->mTimeStamp - mpCurrentKeyFrame->mPrevKF->mTimeStamp;
                        // 若当前地图做BA2不成功时，正常都会成功
                        if(!mpCurrentKeyFrame->GetMap()->GetIniertialBA2())
                        { 
        	// 若 时间小于10S 且 距离小于0.02，则打印没足够运动，重置
                            if((mTinit<10.f) && (dist<0.02))
                            { 
        
                                cout << "Not enough motion for initializing. Reseting..." << endl;
                                unique_lock<mutex> lock(mMutexReset);
                                mbResetRequestedActiveMap = true;
                                mpMapToReset = mpCurrentKeyFrame->GetMap();
                                mbBadImu = true;
                            }
                        }
                        
						// 需大范围BA标记，使用单目时需匹配内点大于75，非单目时匹配点大于100
                        bool bLarge = ((mpTracker->GetMatchesInliers()>75)&&mbMonocular)||((mpTracker->GetMatchesInliers()>100)&&!mbMonocular);
                        // 进行内部BA
                        Optimizer::LocalInertialBA(mpCurrentKeyFrame, &mbAbortBA, mpCurrentKeyFrame->GetMap(),num_FixedKF_BA,num_OptKF_BA,num_MPs_BA,num_edges_BA, bLarge, !mpCurrentKeyFrame->GetMap()->GetIniertialBA2());
                        b_doneLBA = true;
                    }
                    else // 否则直接进行BA
                    { 
        	// VI-D Local BA
                        Optimizer::LocalBundleAdjustment(mpCurrentKeyFrame,&mbAbortBA, mpCurrentKeyFrame->GetMap(),num_FixedKF_BA,num_OptKF_BA,num_MPs_BA,num_edges_BA);
                        b_doneLBA = true;
                    }

                }

                // Initialize IMU here 
                // 若IMU 未初始化时，初始化IMU，单目误差更小，imu权重更大
                if(!mpCurrentKeyFrame->GetMap()->isImuInitialized() && mbInertial)
                { 
        
                    if (mbMonocular)
                        InitializeIMU(1e2, 1e10, true);
                    else
                        InitializeIMU(1e2, 1e5, true);
                }


                // Check redundant local Keyframes 检查冗余本地关键帧
                KeyFrameCulling();
				
				// 若累计时间小于100s，且 使用imu时
                if ((mTinit<100.0f) && mbInertial)
                { 
        	
                	// imu已初始化，且跟踪ok
                    if(mpCurrentKeyFrame->GetMap()->isImuInitialized() && mpTracker->mState==Tracking::OK)
                    { 
        	
                    	// BA1 失败时，进行BA2，重新初始化IMU
                        if(!mpCurrentKeyFrame->GetMap()->GetIniertialBA1()){ 
        
                            if (mTinit>5.0f)
                            { 
        
                                cout << "start VIBA 1" << endl;
                                mpCurrentKeyFrame->GetMap()->SetIniertialBA1();
                                if (mbMonocular)
                                    InitializeIMU(1.f, 1e5, true);
                                else
                                    InitializeIMU(1.f, 1e5, true);

                                cout << "end VIBA 1" << endl;
                            }
                        }
                        else if(!mpCurrentKeyFrame->GetMap()->GetIniertialBA2()){ 
        
                            if (mTinit>15.0f){ 
        
                                cout << "start VIBA 2" << endl;
                                mpCurrentKeyFrame->GetMap()->SetIniertialBA2();
                                if (mbMonocular)
                                    InitializeIMU(0.f, 0.f, true);
                                else
                                    InitializeIMU(0.f, 0.f, true);

                                cout << "end VIBA 2" << endl;
                            }
                        }

                        // scale refinement
                        if (((mpAtlas->KeyFramesInMap())<=100) &&
                                ((mTinit>25.0f && mTinit<25.5f)||
                                (mTinit>35.0f && mTinit<35.5f)||
                                (mTinit>45.0f && mTinit<45.5f)||
                                (mTinit>55.0f && mTinit<55.5f)||
                                (mTinit>65.0f && mTinit<65.5f)||
                                (mTinit>75.0f && mTinit<75.5f))){ 
        
                            cout << "start scale ref" << endl;
                            if (mbMonocular)
                                ScaleRefinement();
                            cout << "end scale ref" << endl;
                        }
                    }
                }
            }
			
			// 闭环检测插入 当前关键帧
            mpLoopCloser->InsertKeyFrame(mpCurrentKeyFrame);
        }
        else if(Stop() && !mbBadImu)
        // Stop: `if(mbStopRequested && !mbNotStop) return true`，否则返回 false
        { 
        
            // Safe area to stop
            // while 循环，直到完全 stop，while中等待 3000us
            while(isStopped() && !CheckFinish())
            { 
        
                usleep(3000);
            }
            
            // 检测请求是否结束，`mbFinishRequested==true`,则 break
            if(CheckFinish())
                break;
        }
		
		// 如果有请求时，执行：
			// 如果有重置请求时，清空新关键帧和最近添加的地图点，不进行BA1和BA2
			// 如果有重置活跃地图时，清空新关键帧和最近添加的地图点
        ResetIfRequested();

        // Tracking will see that Local Mapping is busy
        SetAcceptKeyFrames(true); // 设置接收关键帧 
		
		// 检测请求是否结束，`mbFinishRequested==true`,则 break
        if(CheckFinish())
            break;

        usleep(3000);
    }
	// 两个状态赋值：`mbFinished=true , mbStopped=true `
    SetFinish();
}

其他函数

ProcessNewKeyFrame 对新关键帧进行处理

  • 建立新的关键帧及其属性并加入全局map
void LocalMapping::ProcessNewKeyFrame()
{ 
        	
	/// 1. 新关键帧队列中取出第一帧作为当前关键帧，然后从队列中弹出
    { 
        
        unique_lock<mutex> lock(mMutexNewKFs);
        mpCurrentKeyFrame = mlNewKeyFrames.front();
        mlNewKeyFrames.pop_front();
    }

    // Compute Bags of Words structures 
    // 当前关键帧计算词袋 `computeBoW`
    mpCurrentKeyFrame->ComputeBoW();

    /// 2.将 MapPoints 关联到新的关键帧并更新法线和描述符
    // Associate MapPoints to the new keyframe and update normal and descriptor
    // 取出 当前帧匹配的地图点
    const vector<MapPoint*> vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches();
	// 遍历 地图点
    for(size_t i=0; i<vpMapPointMatches.size(); i++)
    { 
        
        MapPoint* pMP = vpMapPointMatches[i];
        if(pMP)
        { 
        	// 地图点非空且不是坏的
            if(!pMP->isBad())
            { 
        	
            	// 若其在关键帧上则 对该地图点添加关键帧观测+法线和描述符
            	// 否则，加入：mlpRecentAddMapPoints
                if(!pMP->IsInKeyFrame(mpCurrentKeyFrame))
                { 
        
                    pMP->AddObservation(mpCurrentKeyFrame, i);
                    pMP->UpdateNormalAndDepth();
                    pMP->ComputeDistinctiveDescriptors();
                } 
                else // this can only happen for new stereo points inserted by the Tracking 
                // 这只会发生在 Tracking 插入的新立体点
                { 
        
                    mlpRecentAddedMapPoints.push_back(pMP);
                }
            }
        }
    }

    // Update links in the Covisibility Graph
    // 当前帧更新 图链接 `UpdateConnections`
    mpCurrentKeyFrame->UpdateConnections();

    // Insert Keyframe in Map
    // 将该关键帧插入地图 `mpAtlas->AddKeyFrame`
    mpAtlas->AddKeyFrame(mpCurrentKeyFrame);
}

MapPointCulling 最近添加地图点的剔除

  • 针对 mlpRecentAddMapPoints
void LocalMapping::MapPointCulling()
{ 
        
    // Check Recent Added MapPoints
    list<MapPoint*>::iterator lit = mlpRecentAddedMapPoints.begin();
    const 
        标签： 力传感器bk
 
锐单商城拥有海量元器件数据手册、
          IC替代型号，打造
          电子元器件IC百科大全！

锐单商城 - 一站式电子元器件采购平台