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