void GraphicEnd::checkLoopClosure()
{
cout<<"**************************************************"<<endl;
cout<<"checking loop closure!!!"<<endl;
cout<<"**************************************************"<<endl;
SparseOptimizer& opt = _pSLAMEnd->globalOptimizer;
//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<<currKF.frame_index<<" and "<<keyframes[ret[ j ].Id].frame_index<<" : "<<validTrans<<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 );
}
}
}
//bow loopclosure
void GraphicEnd::Bowloopclosure()
{
SparseOptimizer& opt = _pSLAMEnd->globalOptimizer;
vector<cv::KeyPoint> keys;
vector<FSurf64::TDescriptor> descrip;
DetectionResult result;
bow_extractor(currKF.frame.rgb,keys,descrip);
bow_detector.detectLoop(keys,descrip,result);
if(result.detection())
{
cout<<"Loop found with image "<<result.match<<"!"<<endl;
/******write loop into file*******/
//ofstream loop_fout("/home/lc/workspace/paper_related/Appolo/test/result/loop.txt");
/********************************/
Eigen::Matrix4f H;
bool validTrans=calcTrans(keyframes[ result.match ],currKF,H,true);
loop_fout<<currKF.frame_index<<" and "<<keyframes[result.match].frame_index<<" : "<<validTrans<<endl;
if(!validTrans)
return;
Eigen::Isometry3d T(H.cast<double>());
//T = T.inverse();
EdgeSE3* edge = new EdgeSE3();
edge->vertices() [0] = opt.vertex( keyframes[result.match].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 );
}
}
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 );
}
}
//回环检测：在过去的帧中随机取_loopclosure_frames那么多帧进行两两比较
void GraphicEnd::loopClosure()
{
if (_keyframes.size() <= 3 ) //小于3时，回环没有意义
return;
cout<<"Checking loop closure."<<endl;
waitKey(10);
vector<int> checked;
SparseOptimizer& opt = _pSLAMEnd->globalOptimizer;
//相邻帧
for (int i=-3; i>-5; i--)
{
int n = _keyframes.size() + i;
if (n>=0)
{
vector<PLANE>& p1 = _keyframes[n].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[n].id );
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 );
edge->setRobustKernel( _pSLAMEnd->_robustKernel );
opt.addEdge( edge );
}
else
break;
}
//搜索种子帧
cout<<"checking seeds, seed.size()"<<_seed.size()<<endl;
vector<int> newseed;
for (size_t i=0; i<_seed.size(); i++)
{
vector<PLANE>& p1 = _keyframes[_seed[i]].planes;
Eigen::Isometry3d T = multiPnP( p1, _currKF.planes ).T;
if (T.matrix() == Eigen::Isometry3d::Identity().matrix()) //匹配不上
continue;
T = T.inverse();
//若匹配上，则在两个帧之间加一条边
checked.push_back( _seed[i] );
newseed.push_back( _seed[i] );
EdgeSE3* edge = new EdgeSE3();
edge->vertices() [0] = opt.vertex( _keyframes[_seed[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(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 );
}
//随机搜索
cout<<"checking random frames"<<endl;
for (int i=0; i<_loopclosure_frames; i++)
{
int frame = rand() % (_keyframes.size() -3 ); //随机在过去的帧中取一帧
if ( find(checked.begin(), checked.end(), frame) != checked.end() ) //之前已检查过
continue;
checked.push_back( frame );
vector<PLANE>& p1 = _keyframes[frame].planes;
RESULT_OF_MULTIPNP result = multiPnP( p1, _currKF.planes, true, _keyframes[frame].frame_index, 20 );
Eigen::Isometry3d T = result.T;
if (T.matrix() == Eigen::Isometry3d::Identity().matrix()) //匹配不上
continue;
T = T.inverse();
displayLC( _keyframes[frame].frame_index, _currKF.frame_index, result.norm);
newseed.push_back( frame );
//若匹配上，则在两个帧之间加一条边
cout<<BOLDBLUE<<"find a loop closure between kf "<<_currKF.id<<" and kf "<<frame<<RESET<<endl;
EdgeSE3* edge = new EdgeSE3();
edge->vertices() [0] = opt.vertex( _keyframes[frame].id );
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 );
edge->setRobustKernel( _pSLAMEnd->_robustKernel );
opt.addEdge( edge );
}
waitKey(10);
_seed = newseed;
}
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 GraphicEnd::loopClosure()
{
if (keyframes.size() >3 )
{
cout<<"Checking loop closure."<<endl;
waitKey(10);
vector<int> checked;
SparseOptimizer& opt = _pSLAMEnd->globalOptimizer;
for (int i=-2; i>-5; i--)
{
int n = keyframes.size() + i;
if (n>=0)
{
KEYFRAME& p1 = keyframes[n];
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[n].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 );
}
else
break;
}
//
cout<<"checking seeds, seed.size()"<<seed.size()<<endl;
vector<int> newseed;
for (size_t i=0; i<seed.size(); i++)
{
KEYFRAME& p1 = keyframes[seed[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();
//
checked.push_back( seed[i] );
newseed.push_back( seed[i] );
EdgeSE3* edge = new EdgeSE3();
edge->vertices() [0] = opt.vertex( keyframes[seed[i]].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 );
}
//
cout<<"checking random frames"<<endl;
for (int i=0; i<10; i++)
{
int frame = rand() % (keyframes.size() -3 ); //
if ( find(checked.begin(), checked.end(), frame) != checked.end() ) //
continue;
checked.push_back( frame );
KEYFRAME& p1 = keyframes[frame];
//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();
newseed.push_back( frame );
//
cout<<"find a loop closure between kf "<<currKF.id<<" and kf "<<frame<<endl;
EdgeSE3* edge = new EdgeSE3();
edge->vertices() [0] = opt.vertex(keyframes[frame].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 );
}
waitKey(10);
seed = newseed;
}
}