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;
}
