本文整理汇总了C++中VertexSE3::setEstimate方法的典型用法代码示例。如果您正苦于以下问题:C++ VertexSE3::setEstimate方法的具体用法?C++ VertexSE3::setEstimate怎么用?C++ VertexSE3::setEstimate使用的例子?那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类VertexSE3
的用法示例。
在下文中一共展示了VertexSE3::setEstimate方法的9个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: measurement
void EdgeSE3Prior::initialEstimate(const OptimizableGraph::VertexSet& /*from_*/, OptimizableGraph::Vertex* /*to_*/) {
VertexSE3 *v = static_cast<VertexSE3*>(_vertices[0]);
SE3Quat newEstimate = _offsetParam->offset().inverse() * measurement();
if (_information.block<3,3>(0,0).squaredNorm()==0){ // do not set translation
newEstimate.setTranslation(v->estimate().translation());
}
if (_information.block<3,3>(3,3).squaredNorm()==0){ // do not set rotation
newEstimate.setRotation(v->estimate().rotation());
}
v->setEstimate(newEstimate);
}
示例2: main
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);
//.........这里部分代码省略.........
示例3: lostRecovery
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 );
}
}
示例4: init
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;
}
示例5: main
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;
}
示例6: main
int main(int argc, char **argv)
{
int num_points = 0;
// check for arg, # of points to use in projection SBA
if (argc > 1)
num_points = atoi(argv[1]);
double euc_noise = 0.1; // noise in position, m
double pix_noise = 1.0; // pixel noise
// double outlier_ratio = 0.1;
SparseOptimizer optimizer;
optimizer.setVerbose(false);
// variable-size block solver
BlockSolverX::LinearSolverType * linearSolver
= new LinearSolverCSparse<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 camera params
Vector2d focal_length(500,500); // pixels
Vector2d principal_point(320,240); // 640x480 image
double baseline = 0.075; // 7.5 cm baseline
// set up camera params and projection matrices on vertices
g2o::VertexSCam::setKcam(focal_length[0],focal_length[1],
principal_point[0],principal_point[1],
baseline);
// 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
VertexSCam *vc = new VertexSCam();
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);
// make sure projection matrices are set
vc->setAll();
// add to optimizer
optimizer.addVertex(vc);
vertex_id++;
}
// set up point matches for GICP
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 ));
//.........这里部分代码省略.........
示例7: time
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");
{
//.........这里部分代码省略.........
示例8: lostRecovery
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 );
//.........这里部分代码省略.........
示例9: fin
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;
}