阐述区块链NFT铸造分红DAPP项目系统开发分析（成熟案例）

// 把g2o的定义放到前面
 32 typedef g2o::BlockSolver_6_3 SlamBlockSolver; 
 33 typedef g2o::LinearSolverCSparse< SlamBlockSolver::PoseMatrixType > SlamLinearSolver; 
 34 
 35 // 给定index，读取一帧数据
 36 FRAME readFrame( int index, ParameterReader& pd );
 37 // 估计一个运动的大小
 38 double normofTransform( cv::Mat rvec, cv::Mat tvec );
 39 
 40 // 检测两个帧，结果定义
 41 enum CHECK_RESULT {NOT_MATCHED=0, TOO_FAR_AWAY, TOO_CLOSE, KEYFRAME}; 
 42 // 函数声明
 43 CHECK_RESULT checkKeyframes( FRAME& f1, FRAME& f2, g2o::SparseOptimizer& opti, bool is_loops=false );
 44 // 检测近距离的回环
 45 void checkNearbyLoops( vector<FRAME>& frames, FRAME& currFrame, g2o::SparseOptimizer& opti );
 46 // 随机检测回环
 47 void checkRandomLoops( vector<FRAME>& frames, FRAME& currFrame, g2o::SparseOptimizer& opti );
 48 
 49 int main( int argc, char** argv )
 50 {
 51     // 前面部分和vo是一样的
 52     ParameterReader pd;
 53     int startIndex  =   atoi( pd.getData( "start_index" ).c_str() );
 54     int endIndex    =   atoi( pd.getData( "end_index"   ).c_str() );
 55 
 56     // 所有的关键帧都放在了这里
 57     vector< FRAME > keyframes; 
 58     // initialize
 59     cout<<"Initializing ..."<<endl;
 60     int currIndex = startIndex; // 当前索引为currIndex
 61     FRAME currFrame = readFrame( currIndex, pd ); // 当前帧数据
 62 
 63     string detector = pd.getData( "detector" );
 64     string descriptor = pd.getData( "descriptor" );
 65     CAMERA_INTRINSIC_PARAMETERS camera = getDefaultCamera();
 66     computeKeyPointsAndDesp( currFrame, detector, descriptor );
 67     PointCloud::Ptr cloud = image2PointCloud( currFrame.rgb, currFrame.depth, camera );
 68     
 69     /******************************* 
 70     // 新增:有关g2o的初始化
 71     *******************************/
 72     // 初始化求解器
 73     SlamLinearSolver* linearSolver = new SlamLinearSolver();
 74     linearSolver->setBlockOrdering( false );
 75     SlamBlockSolver* blockSolver = new SlamBlockSolver( linearSolver );
 76     g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( blockSolver );
 77 
 78     g2o::SparseOptimizer globalOptimizer;  // 最后用的就是这个东东
 79     globalOptimizer.setAlgorithm( solver ); 
 80     // 不要输出调试信息
 81     globalOptimizer.setVerbose( false );
 82     
 83 
 84     // 向globalOptimizer增加第一个顶点
 85     g2o::VertexSE3* v = new g2o::VertexSE3();
 86     v->setId( currIndex );
 87     v->setEstimate( Eigen::Isometry3d::Identity() ); //估计为单位矩阵
 88     v->setFixed( true ); //第一个顶点固定，不用优化
 89     globalOptimizer.addVertex( v );
 90     
 91     keyframes.push_back( currFrame );
 92 
 93     double keyframe_threshold = atof( pd.getData("keyframe_threshold").c_str() );
 94 
 95     bool check_loop_closure = pd.getData("check_loop_closure")==string("yes");
 96     for ( currIndex=startIndex+1; currIndex<endIndex; currIndex++ )
 97     {
 98         cout<<"Reading files "<<currIndex<<endl;
 99         FRAME currFrame = readFrame( currIndex,pd ); // 读取currFrame
100         computeKeyPointsAndDesp( currFrame, detector, descriptor ); //提取特征
101         CHECK_RESULT result = checkKeyframes( keyframes.back(), currFrame, globalOptimizer ); //匹配该帧与keyframes里最后一帧
102         switch (result) // 根据匹配结果不同采取不同策略
103         {
104         case NOT_MATCHED:
105             //没匹配上，直接跳过
106             cout<<RED"Not enough inliers."<<endl;
107             break;
108         case TOO_FAR_AWAY:
109             // 太近了，也直接跳
110             cout<<RED"Too far away, may be an error."<<endl;
111             break;
112         case TOO_CLOSE:
113             // 太远了，可能出错了
114             cout<<RESET"Too close, not a keyframe"<<endl;
115             break;
116         case KEYFRAME:
117             cout<<GREEN"This is a new keyframe"<<endl;
118             // 不远不近，刚好
119             /**
120              * This is important!!
121              * This is important!!
122              * This is important!!
123              * (very important so I've said three times!)
124              */
125             // 检测回环
126             if (check_loop_closure)
127             {
128                 checkNearbyLoops( keyframes, currFrame, globalOptimizer );
129                 checkRandomLoops( keyframes, currFrame, globalOptimizer );
130             }
131             keyframes.push_back( currFrame );
132             break;
133         default:
134             break;
135         }