void GraphicEnd::generateKeyframe(Eigen::Isometry3d& T) { cout<<"generating keyframes..."<<endl; currKF.id++; currKF.frame_index=index; currKF.frame.rgb=present.frame.rgb.clone(); currKF.frame.dep=present.frame.dep.clone(); //currKF.frame.cloud=present.frame.cloud; pcl::copyPointCloud(*(present.frame.cloud),*(currKF.frame.cloud)); currKF.fullpose=present.fullpose; //keyframes.reserve(100); keyframes.push_back(currKF); //cout<<"keyframes capacity is: "<<keyframes.capacity()<<endl; SparseOptimizer& opt = _pSLAMEnd->globalOptimizer; VertexSE3* v = new VertexSE3(); v->setId( currKF.id ); v->setEstimate( Eigen::Isometry3d::Identity() ); opt.addVertex( v ); cout<<"add vertex success!!!"<<endl; //edge EdgeSE3* edge = new EdgeSE3(); edge->vertices()[0] = opt.vertex( currKF.id - 1 ); edge->vertices()[1] = opt.vertex( currKF.id ); Eigen::Matrix<double, 6,6> information = Eigen::Matrix<double, 6, 6>::Identity(); information(0, 0) = information(2,2) = 100; information(1, 1) = 100; information(3,3) = information(4,4) = information(5,5) = 100; edge->setInformation( information ); edge->setMeasurement( T ); opt.addEdge( edge ); cout<<"add edge success!!!"<<endl; }
void GraphicEnd::generateKeyFrame( Eigen::Isometry3d T ) { cout<<BOLDGREEN<<"GraphicEnd::generateKeyFrame"<<RESET<<endl; //把present中的数据存储到current中 _currKF.id ++; _currKF.planes = _present.planes; _currKF.frame_index = _index; _lastRGB = _currRGB.clone(); _kf_pos = _robot; cout<<"add key frame: "<<_currKF.id<<endl; //waitKey(0); _keyframes.push_back( _currKF ); //将current关键帧存储至全局优化器 SparseOptimizer& opt = _pSLAMEnd->globalOptimizer; //顶点 VertexSE3* v = new VertexSE3(); v->setId( _currKF.id ); //v->setEstimate( _robot ); if (_use_odometry) v->setEstimate( _odo_this ); //v->setEstimate( Eigen::Isometry3d::Identity() ); else v->setEstimate( Eigen::Isometry3d::Identity() ); opt.addVertex( v ); //边 EdgeSE3* edge = new EdgeSE3(); edge->vertices()[0] = opt.vertex( _currKF.id - 1 ); edge->vertices()[1] = opt.vertex( _currKF.id ); Eigen::Matrix<double, 6,6> information = Eigen::Matrix<double, 6, 6>::Identity(); information(0, 0) = information(2,2) = 100; information(1, 1) = 100; information(3,3) = information(4,4) = information(5,5) = 100; edge->setInformation( information ); edge->setMeasurement( T ); opt.addEdge( edge ); if (_use_odometry) { Eigen::Isometry3d To = _odo_last.inverse()*_odo_this; cout<<"odo last = "<<endl<<_odo_last.matrix()<<endl<<"odo this = "<<endl<<_odo_this.matrix()<<endl; cout<<"To = "<<endl<<To.matrix()<<endl; cout<<"Measurement = "<<endl<<T.matrix()<<endl; //waitKey( 0 ); EdgeSE3* edge = new EdgeSE3(); edge->vertices()[0] = opt.vertex( _currKF.id - 1 ); edge->vertices()[1] = opt.vertex( _currKF.id ); Eigen::Matrix<double, 6,6> information = Eigen::Matrix<double, 6, 6>::Identity(); information(0, 0) = information(1,1) = information(2,2) = information(3,3) = information(4,4) = information(5,5) = 1/(_error_odometry*_error_odometry); edge->setInformation( information ); edge->setMeasurement( To ); opt.addEdge( edge ); _odo_last = _odo_this; } }
int main() { double euc_noise = 0.01; // noise in position, m // double outlier_ratio = 0.1; SparseOptimizer optimizer; optimizer.setVerbose(false); // variable-size block solver BlockSolverX::LinearSolverType * linearSolver = new LinearSolverDense<g2o::BlockSolverX::PoseMatrixType>(); BlockSolverX * solver_ptr = new BlockSolverX(linearSolver); g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr); optimizer.setAlgorithm(solver); vector<Vector3d> true_points; for (size_t i=0;i<1000; ++i) { true_points.push_back(Vector3d((Sample::uniform()-0.5)*3, Sample::uniform()-0.5, Sample::uniform()+10)); } // set up two poses int vertex_id = 0; for (size_t i=0; i<2; ++i) { // set up rotation and translation for this node Vector3d t(0,0,i); Quaterniond q; q.setIdentity(); Eigen::Isometry3d cam; // camera pose cam = q; cam.translation() = t; // set up node VertexSE3 *vc = new VertexSE3(); vc->setEstimate(cam); vc->setId(vertex_id); // vertex id cerr << t.transpose() << " | " << q.coeffs().transpose() << endl; // set first cam pose fixed if (i==0) vc->setFixed(true); // add to optimizer optimizer.addVertex(vc); vertex_id++; } // set up point matches for (size_t i=0; i<true_points.size(); ++i) { // get two poses VertexSE3* vp0 = dynamic_cast<VertexSE3*>(optimizer.vertices().find(0)->second); VertexSE3* vp1 = dynamic_cast<VertexSE3*>(optimizer.vertices().find(1)->second); // calculate the relative 3D position of the point Vector3d pt0,pt1; pt0 = vp0->estimate().inverse() * true_points[i]; pt1 = vp1->estimate().inverse() * true_points[i]; // add in noise pt0 += Vector3d(Sample::gaussian(euc_noise ), Sample::gaussian(euc_noise ), Sample::gaussian(euc_noise )); pt1 += Vector3d(Sample::gaussian(euc_noise ), Sample::gaussian(euc_noise ), Sample::gaussian(euc_noise )); // form edge, with normals in varioius positions Vector3d nm0, nm1; nm0 << 0, i, 1; nm1 << 0, i, 1; nm0.normalize(); nm1.normalize(); Edge_V_V_GICP * e // new edge with correct cohort for caching = new Edge_V_V_GICP(); e->setVertex(0, vp0); // first viewpoint e->setVertex(1, vp1); // second viewpoint EdgeGICP meas; meas.pos0 = pt0; meas.pos1 = pt1; meas.normal0 = nm0; meas.normal1 = nm1; e->setMeasurement(meas); // e->inverseMeasurement().pos() = -kp; meas = e->measurement(); // use this for point-plane e->information() = meas.prec0(0.01); // use this for point-point // e->information().setIdentity(); // e->setRobustKernel(true); //e->setHuberWidth(0.01); optimizer.addEdge(e); } // move second cam off of its true position VertexSE3* vc = dynamic_cast<VertexSE3*>(optimizer.vertices().find(1)->second); Eigen::Isometry3d cam = vc->estimate(); cam.translation() = Vector3d(0,0,0.2); vc->setEstimate(cam); optimizer.initializeOptimization(); optimizer.computeActiveErrors(); cout << "Initial chi2 = " << FIXED(optimizer.chi2()) << endl; optimizer.setVerbose(true); optimizer.optimize(5); cout << endl << "Second vertex should be near 0,0,1" << endl; cout << dynamic_cast<VertexSE3*>(optimizer.vertices().find(0)->second) ->estimate().translation().transpose() << endl; cout << dynamic_cast<VertexSE3*>(optimizer.vertices().find(1)->second) ->estimate().translation().transpose() << endl; }
void GraphicEnd::lostRecovery() { //以present作为新的关键帧 cout<<BOLDYELLOW<<"Lost Recovery..."<<RESET<<endl; //把present中的数据存储到current中 _currKF.id ++; _currKF.planes = _present.planes; _currKF.frame_index = _index; _lastRGB = _currRGB.clone(); _kf_pos = _robot; //waitKey(0); _keyframes.push_back( _currKF ); //将current关键帧存储至全局优化器 SparseOptimizer& opt = _pSLAMEnd->globalOptimizer; //顶点 VertexSE3* v = new VertexSE3(); v->setId( _currKF.id ); if (_use_odometry) v->setEstimate( _odo_this ); else v->setEstimate( Eigen::Isometry3d::Identity() ); opt.addVertex( v ); //由于当前位置不知道,所以不添加与上一帧相关的边 if (_use_odometry) { Eigen::Isometry3d To = _odo_last.inverse()*_odo_this; EdgeSE3* edge = new EdgeSE3(); edge->vertices()[0] = opt.vertex( _currKF.id - 1 ); edge->vertices()[1] = opt.vertex( _currKF.id ); Eigen::Matrix<double, 6,6> information = Eigen::Matrix<double, 6, 6>::Identity(); information(0, 0) = information(2,2) = information(1, 1) = information(3,3) = information(4,4) = information(5,5) = 1/(_error_odometry*_error_odometry); edge->setInformation( information ); edge->setMeasurement( To ); opt.addEdge( edge ); _odo_last = _odo_this; _lost = 0; return; } //check loop closure for (int i=0; i<_keyframes.size() - 1; i++) { vector<PLANE>& p1 = _keyframes[ i ].planes; Eigen::Isometry3d T = multiPnP( p1, _currKF.planes ).T; if (T.matrix() == Eigen::Isometry3d::Identity().matrix()) //匹配不上 continue; T = T.inverse(); //若匹配上,则在两个帧之间加一条边 EdgeSE3* edge = new EdgeSE3(); edge->vertices() [0] = opt.vertex( _keyframes[i].id ); edge->vertices() [1] = opt.vertex( _currKF.id ); Eigen::Matrix<double, 6,6> information = Eigen::Matrix<double, 6, 6>::Identity(); information(0, 0) = information(1,1) = information(2,2) = 100; information(3,3) = information(4,4) = information(5,5) = 100; edge->setInformation( information ); edge->setMeasurement( T ); edge->setRobustKernel( _pSLAMEnd->_robustKernel ); opt.addEdge( edge ); } }
void GraphicEnd::init(SLAMEnd* pSLAMEnd) { cout<<"Graphic end init..."<<endl; _pSLAMEnd = pSLAMEnd; _index = atoi( g_pParaReader->GetPara("start_index").c_str() ); _rgbPath = g_pParaReader->GetPara("data_source")+string("/rgb_index/"); _depPath = g_pParaReader->GetPara("data_source")+string("/dep_index/"); _pclPath = g_pParaReader->GetPara("data_source")+string("/pcd/"); _loops = 0; _success = false; _step_time = atoi(g_pParaReader->GetPara("step_time").c_str()); _distance_threshold = atof( g_pParaReader->GetPara("distance_threshold").c_str() ); _error_threshold = atof( g_pParaReader->GetPara("error_threshold").c_str() ); _min_error_plane = atof( g_pParaReader->GetPara("min_error_plane").c_str() ); _match_min_dist = atof( g_pParaReader->GetPara("match_min_dist").c_str() ); _percent = atof( g_pParaReader->GetPara("plane_percent").c_str() ); _max_pos_change = atof( g_pParaReader->GetPara("max_pos_change").c_str()); _max_planes = atoi( g_pParaReader->GetPara("max_planes").c_str() ); _loopclosure_frames = atoi( g_pParaReader->GetPara("loopclosure_frames").c_str() ); _loop_closure_detection = (g_pParaReader->GetPara("loop_closure_detection") == string("yes"))?true:false; _loop_closure_error = atof(g_pParaReader->GetPara("loop_closure_error").c_str()); _lost_frames = atoi( g_pParaReader->GetPara("lost_frames").c_str() ); _robot = _kf_pos = Eigen::Isometry3d::Identity(); _use_odometry = g_pParaReader->GetPara("use_odometry") == string("yes"); _error_odometry = atof( g_pParaReader->GetPara("error_odometry").c_str() ); _robot2camera = Eigen::AngleAxisd(-0.5*M_PI, Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(0.5*M_PI, Eigen::Vector3d::UnitX()); if (_use_odometry)//some error happened! ty { cout<<"using odometry"<<endl; string fileaddr = g_pParaReader->GetPara("data_source")+string("/associate.txt"); cout<<"fileaddr is: "<<fileaddr<<endl; ifstream fin(fileaddr.c_str()); if (!fin) { cerr<<"cannot find associate.txt"<<endl; exit(0); } cout<<"found it"<<endl; string temp; while( !fin.eof()) { getline(fin,temp); cout<<temp<<endl; _odometry.push_back( readOdometry(fin) ); } cout<<"error??"<<endl; _odo_this = _odo_last = _odometry[ _index-1 ]; } //读取首帧图像并处理 cout<<"what happened?"<<endl; readimage(); _lastRGB = _currRGB.clone(); _currKF.id = 0; _currKF.frame_index = _index; /* _currKF.planes = extractPlanes( _currCloud ); //抓平面 generateImageOnPlane( _currRGB, _currKF.planes, _currDep); */ _currKF.planes = extractPlanesAndGenerateImage( _currCloud, _currRGB, _currDep ); for ( size_t i=0; i<_currKF.planes.size(); i++ ) { _currKF.planes[i].kp = extractKeypoints(_currKF.planes[i].image); _currKF.planes[i].desp = extractDescriptor( _currRGB, _currKF.planes[i].kp ); compute3dPosition( _currKF.planes[i], _currDep ); } _keyframes.push_back( _currKF ); //将current存储至全局优化器 SparseOptimizer& opt = _pSLAMEnd->globalOptimizer; VertexSE3* v = new VertexSE3(); v->setId( _currKF.id ); if (_use_odometry) v->setEstimate( _odo_this ); else v->setEstimate( _robot ); v->setFixed( true ); opt.addVertex( v ); _index ++; cout<<"********************"<<endl; }
int main(int argc, char** argv) { string inputFilename; string outputFilename; // command line parsing CommandArgs commandLineArguments; commandLineArguments.paramLeftOver("gm2dl-input", inputFilename, "", "gm2dl file which will be processed"); commandLineArguments.paramLeftOver("gm2dl-output", outputFilename, "", "name of the output file"); commandLineArguments.parseArgs(argc, argv); OptimizableGraph inputGraph; bool loadStatus = inputGraph.load(inputFilename.c_str()); if (! loadStatus) { cerr << "Error while loading input data" << endl; return 1; } OptimizableGraph outputGraph; // process all the vertices first double fx = -1; double baseline = -1; bool firstCam = true; for (OptimizableGraph::VertexIDMap::const_iterator it = inputGraph.vertices().begin(); it != inputGraph.vertices().end(); ++it) { if (dynamic_cast<VertexCam*>(it->second)) { VertexCam* v = static_cast<VertexCam*>(it->second); if (firstCam) { firstCam = false; g2o::ParameterCamera* camParams = new g2o::ParameterCamera; camParams->setId(0); const SBACam& c = v->estimate(); baseline = c.baseline; fx = c.Kcam(0,0); camParams->setKcam(c.Kcam(0,0), c.Kcam(1,1), c.Kcam(0,2), c.Kcam(1,2)); outputGraph.addParameter(camParams); } VertexSE3* ov = new VertexSE3; ov->setId(v->id()); Eigen::Isometry3d p; p = v->estimate().rotation(); p.translation() = v->estimate().translation(); ov->setEstimate(p); if (! outputGraph.addVertex(ov)) { assert(0 && "Failure adding camera vertex"); } } else if (dynamic_cast<VertexSBAPointXYZ*>(it->second)) { VertexSBAPointXYZ* v = static_cast<VertexSBAPointXYZ*>(it->second); VertexPointXYZ* ov = new VertexPointXYZ; ov->setId(v->id()); ov->setEstimate(v->estimate()); if (! outputGraph.addVertex(ov)) { assert(0 && "Failure adding camera vertex"); } } } for (OptimizableGraph::EdgeSet::const_iterator it = inputGraph.edges().begin(); it != inputGraph.edges().end(); ++it) { if (dynamic_cast<EdgeProjectP2SC*>(*it)) { EdgeProjectP2SC* e = static_cast<EdgeProjectP2SC*>(*it); EdgeSE3PointXYZDisparity* oe = new EdgeSE3PointXYZDisparity; oe->vertices()[0] = outputGraph.vertex(e->vertices()[1]->id()); oe->vertices()[1] = outputGraph.vertex(e->vertices()[0]->id()); double kx = e->measurement().x(); double ky = e->measurement().y(); double disparity = kx - e->measurement()(2); oe->setMeasurement(Eigen::Vector3d(kx, ky, disparity / (fx * baseline))); oe->setInformation(e->information()); // TODO convert information matrix oe->setParameterId(0,0); if (! outputGraph.addEdge(oe)) { assert(0 && "error adding edge"); } } } cout << "Vertices in/out:\t" << inputGraph.vertices().size() << " " << outputGraph.vertices().size() << endl; cout << "Edges in/out:\t" << inputGraph.edges().size() << " " << outputGraph.edges().size() << endl; cout << "Writing output ... " << flush; outputGraph.save(outputFilename.c_str()); cout << "done." << endl; return 0; }
void Optimizer::optimizeUseG2O() { // create the linear solver BlockSolverX::LinearSolverType * linearSolver = new LinearSolverCSparse<BlockSolverX::PoseMatrixType>(); // create the block solver on top of the linear solver BlockSolverX* blockSolver = new BlockSolverX(linearSolver); // create the algorithm to carry out the optimization //OptimizationAlgorithmGaussNewton* optimizationAlgorithm = new OptimizationAlgorithmGaussNewton(blockSolver); OptimizationAlgorithmLevenberg* optimizationAlgorithm = new OptimizationAlgorithmLevenberg(blockSolver); // NOTE: We skip to fix a variable here, either this is stored in the file // itself or Levenberg will handle it. // create the optimizer to load the data and carry out the optimization SparseOptimizer optimizer; SparseOptimizer::initMultiThreading(); optimizer.setVerbose(true); optimizer.setAlgorithm(optimizationAlgorithm); { pcl::ScopeTime time("G2O setup Graph vertices"); for (size_t cloud_count = 0; cloud_count < m_pointClouds.size(); ++cloud_count) { VertexSE3 *vertex = new VertexSE3; vertex->setId(cloud_count); Isometry3D affine = Isometry3D::Identity(); affine.linear() = m_pointClouds[cloud_count]->sensor_orientation_.toRotationMatrix().cast<Isometry3D::Scalar>(); affine.translation() = m_pointClouds[cloud_count]->sensor_origin_.block<3, 1>(0, 0).cast<Isometry3D::Scalar>(); vertex->setEstimate(affine); optimizer.addVertex(vertex); } optimizer.vertex(0)->setFixed(true); } { pcl::ScopeTime time("G2O setup Graph edges"); double trans_noise = 0.5, rot_noise = 0.5235; EdgeSE3::InformationType infomation = EdgeSE3::InformationType::Zero(); infomation.block<3, 3>(0, 0) << trans_noise * trans_noise, 0, 0, 0, trans_noise * trans_noise, 0, 0, 0, trans_noise * trans_noise; infomation.block<3, 3>(3, 3) << rot_noise * rot_noise, 0, 0, 0, rot_noise * rot_noise, 0, 0, 0, rot_noise * rot_noise; for (size_t pair_count = 0; pair_count < m_cloudPairs.size(); ++pair_count) { CloudPair pair = m_cloudPairs[pair_count]; int from = pair.corresIdx.first; int to = pair.corresIdx.second; EdgeSE3 *edge = new EdgeSE3; edge->vertices()[0] = optimizer.vertex(from); edge->vertices()[1] = optimizer.vertex(to); Eigen::Matrix<double, 6, 6> ATA = Eigen::Matrix<double, 6, 6>::Zero(); Eigen::Matrix<double, 6, 1> ATb = Eigen::Matrix<double, 6, 1>::Zero(); #pragma unroll 8 for (size_t point_count = 0; point_count < pair.corresPointIdx.size(); ++point_count) { int point_p = pair.corresPointIdx[point_count].first; int point_q = pair.corresPointIdx[point_count].second; PointType P = m_pointClouds[from]->points[point_p]; PointType Q = m_pointClouds[to]->points[point_q]; Eigen::Vector3d p = P.getVector3fMap().cast<double>(); Eigen::Vector3d q = Q.getVector3fMap().cast<double>(); Eigen::Vector3d Np = P.getNormalVector3fMap().cast<double>(); double b = (p - q).dot(Np); Eigen::Matrix<double, 6, 1> A_p; A_p.block<3, 1>(0, 0) = p.cross(Np); A_p.block<3, 1>(3, 0) = Np; ATA += A_p * A_p.transpose(); ATb += A_p * b; } Eigen::Matrix<double, 6, 1> X = ATA.ldlt().solve(ATb); Isometry3D measure = Isometry3D::Identity(); float beta = X[0]; float gammar = X[1]; float alpha = X[2]; measure.linear() = (Eigen::Matrix3d)Eigen::AngleAxisd(alpha, Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(gammar, Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(beta, Eigen::Vector3d::UnitX()); measure.translation() = X.block<3, 1>(3, 0); edge->setMeasurement(measure); edge->setInformation(infomation); optimizer.addEdge(edge); } } optimizer.save("debug_preOpt.g2o"); { pcl::ScopeTime time("g2o optimizing"); optimizer.initializeOptimization(); optimizer.optimize(30); } optimizer.save("debug_postOpt.g2o"); for (size_t cloud_count = 0; cloud_count < m_pointClouds.size(); ++cloud_count) { CloudTypePtr tmp(new CloudType); Isometry3D trans = ((VertexSE3 *)optimizer.vertices()[cloud_count])->estimate(); Eigen::Affine3d affine; affine.linear() = trans.rotation(); affine.translation() = trans.translation(); pcl::transformPointCloudWithNormals(*m_pointClouds[cloud_count], *tmp, affine.cast<float>()); pcl::copyPointCloud(*tmp, *m_pointClouds[cloud_count]); } PCL_WARN("Opitimization DONE!!!!\n"); if (m_params.saveDirectory.length()) { if (boost::filesystem::exists(m_params.saveDirectory) && !boost::filesystem::is_directory(m_params.saveDirectory)) { boost::filesystem::remove(m_params.saveDirectory); } boost::filesystem::create_directories(m_params.saveDirectory); char filename[1024] = { 0 }; for (size_t i = 0; i < m_pointClouds.size(); ++i) { sprintf(filename, "%s/cloud_%d.ply", m_params.saveDirectory.c_str(), i); pcl::io::savePLYFileBinary(filename, *m_pointClouds[i]); } } }
void GraphicEnd::lostRecovery() { cout<<"Lost Recovery..."<<endl; // currKF.id++; currKF.frame.rgb=present.frame.rgb.clone(); currKF.frame.dep=present.frame.dep.clone(); //currKF.frame.cloud=present.frame.cloud; pcl::copyPointCloud(*(present.frame.cloud),*(currKF.frame.cloud)); currKF.frame_index = index; //waitKey(0); keyframes.push_back( currKF ); // SparseOptimizer& opt = _pSLAMEnd->globalOptimizer; // VertexSE3* v = new VertexSE3(); v->setId( currKF.id ); v->setEstimate( Eigen::Isometry3d::Identity() ); opt.addVertex( v ); /**************************************** //check bow loop //first extract currKF desp and keys vector<cv::KeyPoint> keys; vector<vector<float> > descrip; loadFeatures(currKF.frame.rgb,descrip,keys); //add currKF into database db.add(descrip); //query, first is the image itself QueryResults ret; db.query(descrip,ret,10);//only get 3 highest score frame; //ransac check for(int j=1;j<ret.size();++j) { //extract chose image feature vector<cv::KeyPoint> keypoint; vector<vector<float> > descriptor; loadFeatures(keyframes[ret[ j ].Id].frame.rgb,descriptor,keypoint); bool ransacResult=checkFundumental(keys,descrip,keypoint,descriptor); //if pass the ransac check if(ransacResult) { Eigen::Matrix4f H; bool validTrans=calcTrans2(keyframes[ ret[ j ].Id ],currKF,H); loop_fout<<"lost frame: "<<currKF.frame_index<<" and "<<keyframes[ret[ j ].Id].frame_index<<endl; cout<<"find loop between "<<currKF.frame_index<<" and "<<keyframes[ret[ j ].Id].frame_index<<endl; if(!validTrans || H==Eigen::Matrix4f::Identity()) continue; Eigen::Isometry3d T(H.cast<double>()); T = T.inverse(); EdgeSE3* edge = new EdgeSE3(); edge->vertices() [0] = opt.vertex( keyframes[ret[ j ].Id].id ); edge->vertices() [1] = opt.vertex( currKF.id ); Matrix<double, 6,6> information = Matrix<double, 6, 6>::Identity(); information(0, 0) = information(2,2) = 100; information(1,1) = 100; information(3,3) = information(4,4) = information(5,5) = 100; edge->setInformation( information ); edge->setMeasurement( T ); edge->setRobustKernel( _pSLAMEnd->robustKernel ); opt.addEdge( edge ); } } ****************************************/ //check loop closure for (int i=0; i<keyframes.size()-1; i++) { KEYFRAME& p1 = keyframes[ i ]; //Eigen::Isometry3d T = calcTrans( p1, currKF.frame ).T; Eigen::Matrix4f H; //bool validTrans=calcTrans(p1,currKF,H); bool validTrans=calcTrans2(p1,currKF,H); if(!validTrans) continue; Eigen::Isometry3d T(H.cast<double>()); T = T.inverse(); // EdgeSE3* edge = new EdgeSE3(); edge->vertices() [0] = opt.vertex( keyframes[i].id ); edge->vertices() [1] = opt.vertex( currKF.id ); Matrix<double, 6,6> information = Matrix<double, 6, 6>::Identity(); information(0, 0) = information(1,1) = information(2,2) = 100; information(3,3) = information(4,4) = information(5,5) = 100; edge->setInformation( information ); edge->setMeasurement( T ); edge->setRobustKernel( _pSLAMEnd->robustKernel ); opt.addEdge( edge ); } lost = 0; }
void GraphicEnd2::init( SLAMEnd* pSLAMEnd) { cout<<"Graphic end 2 init.."<<endl; _pSLAMEnd = pSLAMEnd; _index = atoi( g_pParaReader->GetPara("start_index").c_str() ); _rgbPath = g_pParaReader->GetPara("data_source")+string("/rgb_index/"); _depPath = g_pParaReader->GetPara("data_source")+string("/dep_index/"); _pclPath = g_pParaReader->GetPara("data_source")+string("/pcd/"); _loops = 0; _success = false; _step_time = atoi(g_pParaReader->GetPara("step_time").c_str()); _distance_threshold = atof( g_pParaReader->GetPara("distance_threshold").c_str() ); _error_threshold = atof( g_pParaReader->GetPara("error_threshold").c_str() ); _min_error_plane = atof( g_pParaReader->GetPara("min_error_plane").c_str() ); _match_min_dist = atof( g_pParaReader->GetPara("match_min_dist").c_str() ); _percent = atof( g_pParaReader->GetPara("plane_percent").c_str() ); _max_pos_change = atof( g_pParaReader->GetPara("max_pos_change").c_str()); _max_planes = atoi( g_pParaReader->GetPara("max_planes").c_str() ); _loopclosure_frames = atoi( g_pParaReader->GetPara("loopclosure_frames").c_str() ); _loop_closure_detection = (g_pParaReader->GetPara("loop_closure_detection") == string("yes"))?true:false; _loop_closure_error = atof(g_pParaReader->GetPara("loop_closure_error").c_str()); _lost_frames = atoi( g_pParaReader->GetPara("lost_frames").c_str() ); _robot = _kf_pos = Eigen::Isometry3d::Identity(); _use_odometry = g_pParaReader->GetPara("use_odometry") == string("yes"); _error_odometry = atof( g_pParaReader->GetPara("error_odometry").c_str() ); _robot2camera = Eigen::AngleAxisd(-0.5*M_PI, Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(0.5*M_PI, Eigen::Vector3d::UnitX()); if (_use_odometry) { cout<<"using odometry"<<endl; string fileaddr = g_pParaReader->GetPara("data_source")+string("/associate.txt"); ifstream fin(fileaddr.c_str()); if (!fin) { cerr<<"cannot find associate.txt"<<endl; exit(0); } while( !fin.eof()) { _odometry.push_back( readOdometry(fin) ); } _odo_this = _odo_last = _odometry[ _index-1 ]; } //处理首帧 readimage(); _lastRGB = _currRGB.clone(); _currKF.id = 0; _currKF.frame_index = _index; _currKF.planes.push_back(extractKPandDesp(_currRGB, _currDep)); _keyframes.push_back( _currKF ); SparseOptimizer& opt = _pSLAMEnd->globalOptimizer; VertexSE3* v = new VertexSE3(); v->setId( _currKF.id ); if (_use_odometry) v->setEstimate( _odo_this ); else v->setEstimate( _robot ); v->setFixed( true ); opt.addVertex( v ); _index ++; cout<<"********************"<<endl; }