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() { 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 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; }