HyperGraphElementAction* VertexSE3DrawAction::operator()(HyperGraph::HyperGraphElement* element,
HyperGraphElementAction::Parameters* params_){
if (typeid(*element).name()!=_typeName)
return 0;
initializeDrawActionsCache();
refreshPropertyPtrs(params_);
if (! _previousParams)
return this;
if (_show && !_show->value())
return this;
VertexSE3* that = static_cast<VertexSE3*>(element);
glColor3f(POSE_VERTEX_COLOR);
glPushMatrix();
glMultMatrixd(that->estimate().matrix().data());
opengl::drawArrow2D(_triangleX->value(), _triangleY->value(), _triangleX->value()*.3f);
drawCache(that->cacheContainer(), params_);
drawUserData(that->userData(), params_);
if(_showId && _showId->value()){
float scale = _idSize ? _idSize->value() : 1.f;
drawId(std::to_string(that->id()), scale);
}
glPopMatrix();
return this;
}
bool EdgeSE3::setMeasurementFromState() {
VertexSE3 *from = static_cast<VertexSE3*>(_vertices[0]);
VertexSE3 *to = static_cast<VertexSE3*>(_vertices[1]);
Eigen::Isometry3d delta = from->estimate().inverse() * to->estimate();
setMeasurement(delta);
return true;
}
bool extractAbsolutePrior(Eigen::Isometry3f& priorMean, Matrix6f& priorInfo,
const DrawableFrame* current){
VertexSE3* currentVertex =current->_vertex;
ImuData* imuData = 0;
OptimizableGraph::Data* d = currentVertex->userData();
while(d) {
ImuData* imuData_ = dynamic_cast<ImuData*>(d);
if (imuData_){
imuData = imuData_;
}
d=d->next();
}
if (imuData){
Eigen::Matrix3d R=imuData->getOrientation().matrix();
Eigen::Matrix3d Omega = imuData->getOrientationCovariance().inverse();
priorMean.setIdentity();
priorInfo.setZero();
for (int c = 0; c<3; c++)
for (int r = 0; r<3; r++)
priorMean.linear()(r,c)=R(r,c);
for (int c = 0; c<3; c++)
for (int r = 0; r<3; r++)
priorInfo(r+3,c+3)=Omega(r,c);
return true;
}
return false;
}
HyperGraphElementAction* EdgeProjectDisparityDrawAction::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;
EdgeSE3PointXYZDisparity* e = static_cast<EdgeSE3PointXYZDisparity*>(element);
VertexSE3* fromEdge = static_cast<VertexSE3*>(e->vertices()[0]);
VertexPointXYZ* toEdge = static_cast<VertexPointXYZ*>(e->vertices()[1]);
if (! fromEdge || ! toEdge)
return this;
Eigen::Isometry3d fromTransform=fromEdge->estimate() * e->cameraParameter()->offset();
glColor3f(LANDMARK_EDGE_COLOR);
glPushAttrib(GL_ENABLE_BIT);
glDisable(GL_LIGHTING);
glBegin(GL_LINES);
glVertex3f((float)fromTransform.translation().x(),(float)fromTransform.translation().y(),(float)fromTransform.translation().z());
glVertex3f((float)toEdge->estimate().x(),(float)toEdge->estimate().y(),(float)toEdge->estimate().z());
glEnd();
glPopAttrib();
return this;
}
HyperGraphElementAction* VertexSE3DrawAction::operator()(HyperGraph::HyperGraphElement* element,
HyperGraphElementAction::Parameters* params_){
if (typeid(*element).name()!=_typeName)
return 0;
if (! _cacheDrawActions){
_cacheDrawActions = HyperGraphActionLibrary::instance()->actionByName("draw");
}
refreshPropertyPtrs(params_);
if (! _previousParams)
return this;
if (_show && !_show->value())
return this;
VertexSE3* that = static_cast<VertexSE3*>(element);
glColor3f(0.5,0.5,0.8);
glPushMatrix();
glTranslatef(that->estimate().translation().x(),that->estimate().translation().y(),that->estimate().translation().z());
AngleAxisd aa(that->estimate().rotation());
glRotatef(RAD2DEG(aa.angle()),aa.axis().x(),aa.axis().y(),aa.axis().z());
if (_triangleX && _triangleY){
drawTriangle(_triangleX->value(), _triangleY->value());
}
CacheContainer* caches=that->cacheContainer();
if (caches){
for (CacheContainer::iterator it=caches->begin(); it!=caches->end(); it++){
Cache* c = it->second;
(*_cacheDrawActions)(c, params_);
}
}
glPopMatrix();
return this;
}
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::savepcd()
{
cout<<"save final pointcloud"<<endl;
SLAMEnd slam;
slam.init(NULL);
SparseOptimizer& opt = slam.globalOptimizer;
opt.load("/home/lc/workspace/paper_related/Appolo/test/result/final_after.g2o");
ifstream fin("/home/lc/workspace/paper_related/Appolo/test/result/key_frame.txt");
PointCloud::Ptr output(new PointCloud());
PointCloud::Ptr curr( new PointCloud());
pcl::VoxelGrid<PointT> voxel;
voxel.setLeafSize(0.01f, 0.01f, 0.01f );
string pclPath ="/media/新加卷/dataset/dataset1/pcd/";
pcl::PassThrough<PointT> pass;
pass.setFilterFieldName("z");
double z = 5.0;
pass.setFilterLimits(0.0, z);
while( !fin.eof() )
{
int frame, id;
fin>>id>>frame;
ss<<pclPath<<frame<<".pcd";
string str;
ss>>str;
cout<<"loading "<<str<<endl;
ss.clear();
pcl::io::loadPCDFile( str.c_str(), *curr );
//cout<<"curr cloud size is: "<<curr->points.size()<<endl;
VertexSE3* pv = dynamic_cast<VertexSE3*> (opt.vertex( id ));
if (pv == NULL)
break;
Eigen::Isometry3d pos = pv->estimate();
cout<<pos.matrix()<<endl;
voxel.setInputCloud( curr );
PointCloud::Ptr tmp( new PointCloud());
voxel.filter( *tmp );
curr.swap( tmp );
pass.setInputCloud( curr );
pass.filter(*tmp);
curr->swap( *tmp );
//cout<<"tmp: "<<tmp->points.size()<<endl;
pcl::transformPointCloud( *curr, *tmp, pos.matrix());
*output += *tmp;
//cout<<"output: "<<output->points.size()<<endl;
}
voxel.setInputCloud( output );
PointCloud::Ptr output_filtered( new PointCloud );
voxel.filter( *output_filtered );
output->swap( *output_filtered );
//cout<<output->points.size()<<endl;
pcl::io::savePCDFile( "/home/lc/workspace/paper_related/Appolo/test/result/result.pcd", *output);
cout<<"final result saved."<<endl;
}
void EdgeSE3PointXYZCov::initialEstimate(const g2o::OptimizableGraph::VertexSet& from_, g2o::OptimizableGraph::Vertex* /*to_*/)
{
VertexSE3* from = static_cast<VertexSE3*>(_vertices[0]);
VertexPointXYZCov* to = static_cast<VertexPointXYZCov*>(_vertices[1]);
if (from_.count(from) > 0)
to->setEstimate(from->estimate() * measurement());
else
std::cerr << __PRETTY_FUNCTION__ << std::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;
}
}
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);
}
void EdgeSE3Prior::linearizeOplus(){
VertexSE3 *from = static_cast<VertexSE3*>(_vertices[0]);
Isometry3 E;
Isometry3 Z, X, P;
X=from->estimate().rotation().toRotationMatrix();
X.translation()=from->estimate().translation();
P=_cache->offsetParam()->offset().rotation().toRotationMatrix();
P.translation()=_cache->offsetParam()->offset().translation();
Z=_measurement.rotation().toRotationMatrix();
Z.translation()=_measurement.translation();
internal::computeEdgeSE3PriorGradient(E, _jacobianOplusXi, Z, X, P);
}
void EdgeSE3LotsOfXYZ::computeError(){
VertexSE3 * pose = static_cast<VertexSE3 *> (_vertices[0]);
for(unsigned int i=0; i<_observedPoints; i++){
VertexPointXYZ * xyz = static_cast<VertexPointXYZ *> (_vertices[1+i]);
Vector3D m = pose->estimate().inverse() * xyz->estimate();
unsigned int index = 3*i;
_error[index] = m[0] - _measurement[index];
_error[index+1] = m[1] - _measurement[index+1];
_error[index+2] = m[2] - _measurement[index+2];
}
}
void EdgeSE3::linearizeOplus() {
// BaseBinaryEdge<6, Eigen::Isometry3d, VertexSE3, VertexSE3>::linearizeOplus();
// return;
VertexSE3 *from = static_cast<VertexSE3*>(_vertices[0]);
VertexSE3 *to = static_cast<VertexSE3*>(_vertices[1]);
Eigen::Isometry3d E;
const Eigen::Isometry3d& Xi=from->estimate();
const Eigen::Isometry3d& Xj=to->estimate();
const Eigen::Isometry3d& Z=_measurement;
computeEdgeSE3Gradient(E, _jacobianOplusXi , _jacobianOplusXj, Z, Xi, Xj);
}
void EdgeSE3PointXYZDepth::initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* /*to_*/)
{
(void) from;
assert(from.size() == 1 && from.count(_vertices[0]) == 1 && "Can not initialize VertexDepthCam position by VertexTrackXYZ");
VertexSE3 *cam = dynamic_cast<VertexSE3*>(_vertices[0]);
VertexPointXYZ *point = dynamic_cast<VertexPointXYZ*>(_vertices[1]);
const Eigen::Matrix<double, 3, 3>& invKcam = params->invKcam();
Eigen::Vector3d p;
p(2) = _measurement(2);
p.head<2>() = _measurement.head<2>()*p(2);
p=invKcam*p;
point->setEstimate(cam->estimate() * (params->offsetMatrix() * p));
}
void EdgeSE3PointXYZ::initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* /*to_*/)
{
(void) from;
assert(from.size() == 1 && from.count(_vertices[0]) == 1 && "Can not initialize VertexDepthCam position by VertexTrackXYZ");
VertexSE3 *cam = dynamic_cast<VertexSE3*>(_vertices[0]);
VertexPointXYZ *point = dynamic_cast<VertexPointXYZ*>(_vertices[1]);
// SE3OffsetCache* vcache = (SE3OffsetCache* ) cam->getCache(_cacheIds[0]);
// if (! vcache){
// cerr << "fatal error in retrieving cache" << endl;
// }
// SE3OffsetParameters* params=vcache->params;
Eigen::Vector3d p=_measurement;
point->setEstimate(cam->estimate() * (offsetParam->offset() * p));
}
HyperGraphElementAction* VertexSE3WriteGnuplotAction::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;
}
VertexSE3* v = static_cast<VertexSE3*>(element);
Vector6d est=toVectorMQT(v->estimate());
for (int i=0; i<6; i++)
*(params->os) << est[i] << " ";
*(params->os) << std::endl;
return this;
}
bool EdgeSE3LotsOfXYZ::setMeasurementFromState(){
VertexSE3 * pose = static_cast<VertexSE3 *> (_vertices[0]);
Eigen::Transform<double, 3, 1> poseinv = pose->estimate().inverse();
for(unsigned int i=0; i<_observedPoints; i++){
VertexPointXYZ * xyz = static_cast<VertexPointXYZ *> (_vertices[1+i]);
// const Vector3D &pt = xyz->estimate();
Vector3D m = poseinv * xyz->estimate();
unsigned int index = 3*i;
_measurement[index] = m[0];
_measurement[index+1] = m[1];
_measurement[index+2] = m[2];
}
return true;
}
HyperGraphElementAction* VertexSE3WriteGnuplotAction::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, no valid os specified" << std::endl;
return 0;
}
VertexSE3* v = static_cast<VertexSE3*>(element);
*(params->os) << v->estimate().translation().x() << " "
<< v->estimate().translation().y() << " "
<< v->estimate().translation().z() << " ";
*(params->os) << v->estimate().rotation().x() << " "
<< v->estimate().rotation().y() << " "
<< v->estimate().rotation().z() << " " << std::endl;
return this;
}
void ViewerState::addCloudSelected(){
if(listItem) {
cerr << "adding a frame" << endl;
int index = pwnGMW->listWidget->row(listItem);
cerr << "index: " << endl;
std::string fname = listAssociations[index]->baseFilename()+"_depth.pgm";
DepthImage depthImage;
if (!depthImage.load(fname.c_str(), true)){
cerr << " skipping " << fname << endl;
newCloudAdded = false;
*addCloud = 0;
return;
}
DepthImage scaledDepthImage;
DepthImage::scale(scaledDepthImage, depthImage, reduction);
imageRows = scaledDepthImage.rows();
imageCols = scaledDepthImage.cols();
correspondenceFinder->setSize(imageRows, imageCols);
Frame * frame=new Frame();
converter->compute(*frame, scaledDepthImage, sensorOffset);
OptimizableGraph::DataContainer* dc =listAssociations[index]->dataContainer();
cerr << "datacontainer: " << dc << endl;
VertexSE3* v = dynamic_cast<VertexSE3*>(dc);
cerr << "vertex of the frame: " << v->id() << endl;
DrawableFrame* drawableFrame = new DrawableFrame(globalT, drawableFrameParameters, frame);
drawableFrame->_vertex = v;
Eigen::Isometry3f priorMean;
Matrix6f priorInfo;
bool hasImu =extractAbsolutePrior(priorMean, priorInfo, drawableFrame);
if (hasImu && drawableFrameVector.size()==0){
globalT.linear() = priorMean.linear();
cerr << "!!!! i found an imu for the first vertex, me happy" << endl;
drawableFrame->setTransformation(globalT);
}
drawableFrameVector.push_back(drawableFrame);
pwnGMW->viewer_3d->addDrawable(drawableFrame);
}
newCloudAdded = true;
*addCloud = 0;
_meHasNewFrame = true;
}
void EdgeSE3Offset::linearizeOplus(){
//BaseBinaryEdge<6, SE3Quat, VertexSE3, VertexSE3>::linearizeOplus();
VertexSE3 *from = static_cast<VertexSE3*>(_vertices[0]);
VertexSE3 *to = static_cast<VertexSE3*>(_vertices[1]);
Eigen::Isometry3d E;
const Eigen::Isometry3d& Xi=from->estimate();
const Eigen::Isometry3d& Xj=to->estimate();
const Eigen::Isometry3d& Pi=_cacheFrom->offsetParam()->offset();
const Eigen::Isometry3d& Pj=_cacheTo->offsetParam()->offset();
const Eigen::Isometry3d& Z=_measurement;
// Matrix6d Ji, Jj;
// computeSE3Gradient(E, Ji , Jj,
// Z, Pi, Xi, Pj, Xj);
// cerr << "Ji:" << endl;
// cerr << Ji-_jacobianOplusXi << endl;
// cerr << "Jj:" << endl;
// cerr << Jj-_jacobianOplusXj << endl;
internal::computeEdgeSE3Gradient(E, _jacobianOplusXi , _jacobianOplusXj, Z, Xi, Xj, Pi, Pj);
}
void EdgeSE3PointXYZDisparity::initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* /*to*/)
{
(void) from;
assert(from.size() == 1 && from.count(_vertices[0]) == 1 && "Can not initialize VertexDepthCam position by VertexTrackXYZ");
VertexSE3 *cam = dynamic_cast<VertexSE3*>(_vertices[0]);
VertexPointXYZ *point = dynamic_cast<VertexPointXYZ*>(_vertices[1]);
// VertexCameraCache* vcache = (VertexCameraCache* ) cam->getCache(_cacheIds[0]);
// if (! vcache){
// cerr << "fatal error in retrieving cache" << endl;
// }
//ParameterCamera* params=vcache->params;
const Eigen::Matrix<double, 3, 3>& invKcam = params->invKcam();
Eigen::Vector3d p;
double w=1./_measurement(2);
p.head<2>() = _measurement.head<2>()*w;
p(2) = w;
p = invKcam * p;
p = cam->estimate() * (params->offset() * p);
point->setEstimate(p);
}
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;
}
HyperGraphElementAction* VertexSE3DrawAction::operator()(HyperGraph::HyperGraphElement* element,
HyperGraphElementAction::Parameters* params_){
if (typeid(*element).name()!=_typeName)
return 0;
if (! _cacheDrawActions){
_cacheDrawActions = HyperGraphActionLibrary::instance()->actionByName("draw");
}
refreshPropertyPtrs(params_);
if (! _previousParams)
return this;
if (_show && !_show->value())
return this;
VertexSE3* that = static_cast<VertexSE3*>(element);
glColor3f(0.5f,0.5f,0.8f);
glPushMatrix();
glMultMatrixd(that->estimate().matrix().data());
if (_triangleX && _triangleY){
drawTriangle(_triangleX->value(), _triangleY->value());
}
CacheContainer* caches=that->cacheContainer();
if (caches){
for (CacheContainer::iterator it=caches->begin(); it!=caches->end(); it++){
Cache* c = it->second;
(*_cacheDrawActions)(c, params_);
}
}
OptimizableGraph::Data* d=that->userData();
while (d && _cacheDrawActions ){
(*_cacheDrawActions)(d, params_);
d=d->next();
}
glPopMatrix();
return this;
}
bool extractRelativePrior(Eigen::Isometry3f& priorMean, Matrix6f& priorInfo,
const DrawableFrame* reference, const DrawableFrame* current){
VertexSE3* referenceVertex =reference->_vertex;
VertexSE3* currentVertex =current->_vertex;
bool priorFound = false;
priorInfo.setZero();
for (HyperGraph::EdgeSet::const_iterator it=referenceVertex->edges().begin();
it!= referenceVertex->edges().end(); it++){
const EdgeSE3* e = dynamic_cast<const EdgeSE3*>(*it);
if (e->vertex(0)==referenceVertex && e->vertex(1) == currentVertex){
priorFound=true;
for (int c=0; c<6; c++)
for (int r=0; r<6; r++)
priorInfo(r,c) = e->information()(r,c);
for(int c=0; c<4; c++)
for(int r=0; r<3; r++)
priorMean.matrix()(r,c) = e->measurement().matrix()(r,c);
priorMean.matrix().row(3) << 0,0,0,1;
}
}
return priorFound;
}
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 EdgeSE3::initialEstimate(const OptimizableGraph::VertexSet& from_, OptimizableGraph::Vertex* /*to_*/) {
VertexSE3 *from = static_cast<VertexSE3*>(_vertices[0]);
VertexSE3 *to = static_cast<VertexSE3*>(_vertices[1]);
if (from_.count(from) > 0) {
to->setEstimate(from->estimate() * _measurement);
} else
from->setEstimate(to->estimate() * _measurement.inverse());
//cerr << "IE" << endl;
}
void EdgeSE3Offset::initialEstimate(const OptimizableGraph::VertexSet& from_, OptimizableGraph::Vertex* /*to_*/) {
VertexSE3 *from = static_cast<VertexSE3*>(_vertices[0]);
VertexSE3 *to = static_cast<VertexSE3*>(_vertices[1]);
Eigen::Isometry3d virtualMeasurement = _cacheFrom->offsetParam()->offset() * measurement() * _cacheTo->offsetParam()->offset().inverse();
if (from_.count(from) > 0) {
to->setEstimate(from->estimate() * virtualMeasurement);
} else
from->setEstimate(to->estimate() * virtualMeasurement.inverse());
}
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;
}
