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;
}
HyperGraphElementAction* EdgeSE3DrawAction::operator()(HyperGraph::HyperGraphElement* element,
HyperGraphElementAction::Parameters* params_){
if (typeid(*element).name()!=_typeName)
return 0;
refreshPropertyPtrs(params_);
if (! _previousParams)
return this;
if (_show && !_show->value())
return this;
EdgeSE3* e = static_cast<EdgeSE3*>(element);
VertexSE3* fromEdge = static_cast<VertexSE3*>(e->vertices()[0]);
VertexSE3* toEdge = static_cast<VertexSE3*>(e->vertices()[1]);
if (! fromEdge || ! toEdge)
return this;
glColor3f(POSE_EDGE_COLOR);
glPushAttrib(GL_ENABLE_BIT);
glDisable(GL_LIGHTING);
glBegin(GL_LINES);
glVertex3f((float)fromEdge->estimate().translation().x(),(float)fromEdge->estimate().translation().y(),(float)fromEdge->estimate().translation().z());
glVertex3f((float)toEdge->estimate().translation().x(),(float)toEdge->estimate().translation().y(),(float)toEdge->estimate().translation().z());
glEnd();
glPopAttrib();
return this;
}
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 );
}
}
}
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;
}
}
//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 );
}
}
HyperGraphElementAction* EdgeSE3WriteGnuplotAction::operator()(HyperGraph::HyperGraphElement* element, HyperGraphElementAction::Parameters* params_) {
if (typeid(*element).name()!=_typeName)
return 0;
WriteGnuplotAction::Parameters* params=static_cast<WriteGnuplotAction::Parameters*>(params_);
if (!params->os) {
std::cerr << __PRETTY_FUNCTION__ << ": warning, on valid os specified" << std::endl;
return 0;
}
EdgeSE3* e = static_cast<EdgeSE3*>(element);
VertexSE3* fromEdge = static_cast<VertexSE3*>(e->vertices()[0]);
VertexSE3* toEdge = static_cast<VertexSE3*>(e->vertices()[1]);
Vector6d fromV, toV;
fromV=toVectorMQT(fromEdge->estimate());
toV=toVectorMQT(toEdge->estimate());
for (int i=0; i<6; i++) {
*(params->os) << fromV[i] << " ";
}
for (int i=0; i<6; i++) {
*(params->os) << toV[i] << " ";
}
*(params->os) << std::endl;
return this;
}
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 writeQueue() {
SensorData* data;
std::ofstream ofG2O(&filename[0]);
geometry_msgs::TransformStamped msg;
int num = 0;
// this is the vertex where we are packing the data
g2o::VertexSE3* activeVertex = 0;
// this is the timestamp of the first measurement added to the vertex
double activeVertexTime=0;
// this is the previous vertex
g2o::VertexSE3* previousVertex = 0;
// this is the timestamp of the first measurement added to the previous vertex
double previousVertexTime=0;
// the last position of the robot (not of the vertex, needed to measure the distances)
Eigen::Isometry3d lastRobotPose;
// set of sensors we packed in the current data.
// We do not want to put 10 camera images of the same camera in the same vertex.
std::set<Sensor*> addedSensors;
Eigen::Vector2d distances(0.,0);
while (true)
{
if(! _queue.empty())
{
data = (SensorData*)_queue.front();
double timeNow = _queue.lastElementTime();
conditionalPrint(annoyingLevel) << "size=" << _queue.size() << " lastTime=" << FIXED(timeNow) << endl;
if (timeNow - data->timeStamp()> initialDelay)
{ // we have enough stuff in the queue
_queue.pop_front();
if (! nptr->ok())
continue;
tf::StampedTransform transform;
bool we_got_transf = false;
try
{
ros::Time timeStamp;
// Get transformation
(*tfListener).lookupTransform("/odom", "/base_link", timeStamp.fromSec(data->timeStamp()), transform);
we_got_transf = true;
}
catch (tf::TransformException & ex)
{
ROS_ERROR("%s", ex.what());
}
if (! we_got_transf)
continue;
Eigen::Isometry3d currentRobotPose = fromStampedTransform(transform);
double currentDataTime = data->timeStamp();
distances += isometry2distance(lastRobotPose.inverse()*currentRobotPose);
double passedTime = currentDataTime-previousVertexTime;
lastRobotPose = currentRobotPose;
conditionalPrint(annoyingLevel) << "distances: " << distances[0] << " " << distances[1] << " " << passedTime << endl;
if (distances[0] < minDistances[0] &&
distances[1] < minDistances[1] &&
passedTime < minTime){
conditionalPrint(annoyingLevel) << "reject: (time/distance)" << endl;
// SKIP THE FRAME
delete data;
data = 0;
continue;
}
if (!activeVertex) {
activeVertex = new g2o::VertexSE3();
activeVertex->setId(num);
activeVertex->setEstimate(fromStampedTransform(transform));
activeVertexTime = currentDataTime;
}
Sensor* sensor = data->sensor();
assert (sensor && "!");
// check if we already packed the data for this kind of sensor
if (addedSensors.count(sensor)){
conditionalPrint(annoyingLevel) << "reject: (sensor) "<< endl;
delete data;
} else {
addedSensors.insert(sensor);
Parameter* parameter = sensor->parameter();
assert (parameter && "!@#");
//data->writeOut(filename);
if (! graph->parameters().getParameter(parameter->id())){
graph->parameters().addParameter(parameter);
graph->saveParameter(ofG2O, parameter);
}
activeVertex->addUserData(data);
data->setDataContainer(activeVertex);
}
// detach the data from the thing
data = 0;
if (currentDataTime - activeVertexTime > vertexTimeWindow) {
conditionalPrint(annoyingLevel) << "flush" << endl;
graph->addVertex(activeVertex);
graph->saveVertex(ofG2O, activeVertex);
if (previousVertex) {
EdgeSE3* e = new EdgeSE3();
e->setVertex(0, previousVertex);
e->setVertex(1, activeVertex);
e->setMeasurementFromState();
Eigen::Matrix<double, 6,6> m;
m.setIdentity();
e->setInformation(m);
graph->addEdge(e);
graph->saveEdge(ofG2O, e);
// JACP: do not do the remove, scan the data list and do a release() of the images which are big. The rest can stay in memory
g2o::HyperGraph::Data* d = previousVertex->userData();
while (d) {
RGBDData* rgbd = dynamic_cast<RGBDData*> (d);
if (rgbd)
rgbd->release();
d = d->next();
}
vertecesQueue.push_back(previousVertex);
}
previousVertex = activeVertex;
previousVertexTime = activeVertexTime;
addedSensors.clear();
activeVertex = 0;
distances.setZero();
num++;
conditionalPrint(defaultLevel) << ".";
}
}
}
usleep(20e3); // queue is emp-ty
}
}
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 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;
}
}
