From 0feb33482063a16af5a7fc2e4d610e5fd6622620 Mon Sep 17 00:00:00 2001 From: raulmur Date: Thu, 22 Dec 2016 12:21:05 +0100 Subject: [PATCH] AR Demo --- .gitignore | 2 - Examples/ROS/ORB_SLAM2/src/AR/ViewerAR.cc | 642 +++++++++++++++++++ Examples/ROS/ORB_SLAM2/src/AR/ViewerAR.h | 120 ++++ Examples/ROS/ORB_SLAM2/src/AR/ros_mono_ar.cc | 169 +++++ build_ros.sh | 7 + 5 files changed, 938 insertions(+), 2 deletions(-) create mode 100644 Examples/ROS/ORB_SLAM2/src/AR/ViewerAR.cc create mode 100644 Examples/ROS/ORB_SLAM2/src/AR/ViewerAR.h create mode 100644 Examples/ROS/ORB_SLAM2/src/AR/ros_mono_ar.cc create mode 100755 build_ros.sh diff --git a/.gitignore b/.gitignore index dd3e3a4eb6..611b5adb2c 100644 --- a/.gitignore +++ b/.gitignore @@ -9,7 +9,6 @@ Examples/ROS/ORB_SLAM2/MonoAR Examples/ROS/ORB_SLAM2/RGBD Examples/ROS/ORB_SLAM2/Stereo Examples/ROS/ORB_SLAM2/build/ -Examples/ROS/ORB_SLAM2/src/AR/ Examples/Stereo/stereo_euroc Examples/Stereo/stereo_kitti KeyFrameTrajectory.txt @@ -20,7 +19,6 @@ Thirdparty/g2o/config.h Thirdparty/g2o/lib/ Vocabulary/ORBvoc.txt build/ -build_ros.sh *~ lib/ diff --git a/Examples/ROS/ORB_SLAM2/src/AR/ViewerAR.cc b/Examples/ROS/ORB_SLAM2/src/AR/ViewerAR.cc new file mode 100644 index 0000000000..9c548e87d1 --- /dev/null +++ b/Examples/ROS/ORB_SLAM2/src/AR/ViewerAR.cc @@ -0,0 +1,642 @@ +/** +* This file is part of ORB-SLAM2. +* +* Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) +* For more information see +* +* ORB-SLAM2 is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* ORB-SLAM2 is distributed in the hope that it will be useful, +* but WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with ORB-SLAM2. If not, see . +*/ + +#include "ViewerAR.h" + +#include + +#include +#include +#include + +using namespace std; + +namespace ORB_SLAM2 +{ + +const float eps = 1e-4; + +cv::Mat ExpSO3(const float &x, const float &y, const float &z) +{ + cv::Mat I = cv::Mat::eye(3,3,CV_32F); + const float d2 = x*x+y*y+z*z; + const float d = sqrt(d2); + cv::Mat W = (cv::Mat_(3,3) << 0, -z, y, + z, 0, -x, + -y, x, 0); + if(d(0),v.at(1),v.at(2)); +} + +ViewerAR::ViewerAR(){} + +void ViewerAR::Run() +{ + int w,h,wui; + + cv::Mat im, Tcw; + int status; + vector vKeys; + vector vMPs; + + while(1) + { + GetImagePose(im,Tcw,status,vKeys,vMPs); + if(im.empty()) + cv::waitKey(mT); + else + { + w = im.cols; + h = im.rows; + break; + } + } + + wui=200; + + pangolin::CreateWindowAndBind("Viewer",w+wui,h); + + glEnable(GL_DEPTH_TEST); + glEnable (GL_BLEND); + + pangolin::CreatePanel("menu").SetBounds(0.0,1.0,0.0,pangolin::Attach::Pix(wui)); + pangolin::Var menu_detectplane("menu.Insert Cube",false,false); + pangolin::Var menu_clear("menu.Clear All",false,false); + pangolin::Var menu_drawim("menu.Draw Image",true,true); + pangolin::Var menu_drawcube("menu.Draw Cube",true,true); + pangolin::Var menu_cubesize("menu. Cube Size",0.05,0.01,0.3); + pangolin::Var menu_drawgrid("menu.Draw Grid",true,true); + pangolin::Var menu_ngrid("menu. Grid Elements",3,1,10); + pangolin::Var menu_sizegrid("menu. Element Size",0.05,0.01,0.3); + pangolin::Var menu_drawpoints("menu.Draw Points",false,true); + + pangolin::Var menu_LocalizationMode("menu.Localization Mode",false,true); + bool bLocalizationMode = false; + + pangolin::View& d_image = pangolin::Display("image") + .SetBounds(0,1.0f,pangolin::Attach::Pix(wui),1.0f,(float)w/h) + .SetLock(pangolin::LockLeft, pangolin::LockTop); + + pangolin::GlTexture imageTexture(w,h,GL_RGB,false,0,GL_RGB,GL_UNSIGNED_BYTE); + + pangolin::OpenGlMatrixSpec P = pangolin::ProjectionMatrixRDF_TopLeft(w,h,fx,fy,cx,cy,0.001,1000); + + vector vpPlane; + + while(1) + { + + if(menu_LocalizationMode && !bLocalizationMode) + { + mpSystem->ActivateLocalizationMode(); + bLocalizationMode = true; + } + else if(!menu_LocalizationMode && bLocalizationMode) + { + mpSystem->DeactivateLocalizationMode(); + bLocalizationMode = false; + } + + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + + // Activate camera view + d_image.Activate(); + glColor3f(1.0,1.0,1.0); + + // Get last image and its computed pose from SLAM + GetImagePose(im,Tcw,status,vKeys,vMPs); + + // Add text to image + PrintStatus(status,bLocalizationMode,im); + + if(menu_drawpoints) + DrawTrackedPoints(vKeys,vMPs,im); + + // Draw image + if(menu_drawim) + DrawImageTexture(imageTexture,im); + + glClear(GL_DEPTH_BUFFER_BIT); + + // Load camera projection + glMatrixMode(GL_PROJECTION); + P.Load(); + + glMatrixMode(GL_MODELVIEW); + + // Load camera pose + LoadCameraPose(Tcw); + + // Draw virtual things + if(status==2) + { + if(menu_clear) + { + if(!vpPlane.empty()) + { + for(size_t i=0; iMapChanged()) + { + cout << "Map changed. All virtual elements are recomputed!" << endl; + bRecompute = true; + } + } + + for(size_t i=0; iRecompute(); + } + glPushMatrix(); + pPlane->glTpw.Multiply(); + + // Draw cube + if(menu_drawcube) + { + DrawCube(menu_cubesize); + } + + // Draw grid plane + if(menu_drawgrid) + { + DrawPlane(menu_ngrid,menu_sizegrid); + } + + glPopMatrix(); + } + } + } + + + } + + pangolin::FinishFrame(); + usleep(mT*1000); + } + +} + +void ViewerAR::SetImagePose(const cv::Mat &im, const cv::Mat &Tcw, const int &status, const vector &vKeys, const vector &vMPs) +{ + unique_lock lock(mMutexPoseImage); + mImage = im.clone(); + mTcw = Tcw.clone(); + mStatus = status; + mvKeys = vKeys; + mvMPs = vMPs; +} + +void ViewerAR::GetImagePose(cv::Mat &im, cv::Mat &Tcw, int &status, std::vector &vKeys, std::vector &vMPs) +{ + unique_lock lock(mMutexPoseImage); + im = mImage.clone(); + Tcw = mTcw.clone(); + status = mStatus; + vKeys = mvKeys; + vMPs = mvMPs; +} + +void ViewerAR::LoadCameraPose(const cv::Mat &Tcw) +{ + if(!Tcw.empty()) + { + pangolin::OpenGlMatrix M; + + M.m[0] = Tcw.at(0,0); + M.m[1] = Tcw.at(1,0); + M.m[2] = Tcw.at(2,0); + M.m[3] = 0.0; + + M.m[4] = Tcw.at(0,1); + M.m[5] = Tcw.at(1,1); + M.m[6] = Tcw.at(2,1); + M.m[7] = 0.0; + + M.m[8] = Tcw.at(0,2); + M.m[9] = Tcw.at(1,2); + M.m[10] = Tcw.at(2,2); + M.m[11] = 0.0; + + M.m[12] = Tcw.at(0,3); + M.m[13] = Tcw.at(1,3); + M.m[14] = Tcw.at(2,3); + M.m[15] = 1.0; + + M.Load(); + } +} + +void ViewerAR::PrintStatus(const int &status, const bool &bLocMode, cv::Mat &im) +{ + if(!bLocMode) + { + switch(status) + { + case 1: {AddTextToImage("SLAM NOT INITIALIZED",im,255,0,0); break;} + case 2: {AddTextToImage("SLAM ON",im,0,255,0); break;} + case 3: {AddTextToImage("SLAM LOST",im,255,0,0); break;} + } + } + else + { + switch(status) + { + case 1: {AddTextToImage("SLAM NOT INITIALIZED",im,255,0,0); break;} + case 2: {AddTextToImage("LOCALIZATION ON",im,0,255,0); break;} + case 3: {AddTextToImage("LOCALIZATION LOST",im,255,0,0); break;} + } + } +} + +void ViewerAR::AddTextToImage(const string &s, cv::Mat &im, const int r, const int g, const int b) +{ + int l = 10; + //imText.rowRange(im.rows-imText.rows,imText.rows) = cv::Mat::zeros(textSize.height+10,im.cols,im.type()); + cv::putText(im,s,cv::Point(l,im.rows-l),cv::FONT_HERSHEY_PLAIN,1.5,cv::Scalar(255,255,255),2,8); + cv::putText(im,s,cv::Point(l-1,im.rows-l),cv::FONT_HERSHEY_PLAIN,1.5,cv::Scalar(255,255,255),2,8); + cv::putText(im,s,cv::Point(l+1,im.rows-l),cv::FONT_HERSHEY_PLAIN,1.5,cv::Scalar(255,255,255),2,8); + cv::putText(im,s,cv::Point(l-1,im.rows-(l-1)),cv::FONT_HERSHEY_PLAIN,1.5,cv::Scalar(255,255,255),2,8); + cv::putText(im,s,cv::Point(l,im.rows-(l-1)),cv::FONT_HERSHEY_PLAIN,1.5,cv::Scalar(255,255,255),2,8); + cv::putText(im,s,cv::Point(l+1,im.rows-(l-1)),cv::FONT_HERSHEY_PLAIN,1.5,cv::Scalar(255,255,255),2,8); + cv::putText(im,s,cv::Point(l-1,im.rows-(l+1)),cv::FONT_HERSHEY_PLAIN,1.5,cv::Scalar(255,255,255),2,8); + cv::putText(im,s,cv::Point(l,im.rows-(l+1)),cv::FONT_HERSHEY_PLAIN,1.5,cv::Scalar(255,255,255),2,8); + cv::putText(im,s,cv::Point(l+1,im.rows-(l+1)),cv::FONT_HERSHEY_PLAIN,1.5,cv::Scalar(255,255,255),2,8); + + cv::putText(im,s,cv::Point(l,im.rows-l),cv::FONT_HERSHEY_PLAIN,1.5,cv::Scalar(r,g,b),2,8); +} + +void ViewerAR::DrawImageTexture(pangolin::GlTexture &imageTexture, cv::Mat &im) +{ + if(!im.empty()) + { + imageTexture.Upload(im.data,GL_RGB,GL_UNSIGNED_BYTE); + imageTexture.RenderToViewportFlipY(); + } +} + +void ViewerAR::DrawCube(const float &size,const float x, const float y, const float z) +{ + pangolin::OpenGlMatrix M = pangolin::OpenGlMatrix::Translate(-x,-size-y,-z); + glPushMatrix(); + M.Multiply(); + pangolin::glDrawColouredCube(-size,size); + glPopMatrix(); +} + +void ViewerAR::DrawPlane(Plane *pPlane, int ndivs, float ndivsize) +{ + glPushMatrix(); + pPlane->glTpw.Multiply(); + DrawPlane(ndivs,ndivsize); + glPopMatrix(); +} + +void ViewerAR::DrawPlane(int ndivs, float ndivsize) +{ + // Plane parallel to x-z at origin with normal -y + const float minx = -ndivs*ndivsize; + const float minz = -ndivs*ndivsize; + const float maxx = ndivs*ndivsize; + const float maxz = ndivs*ndivsize; + + + glLineWidth(2); + glColor3f(0.7f,0.7f,1.0f); + glBegin(GL_LINES); + + for(int n = 0; n<=2*ndivs; n++) + { + glVertex3f(minx+ndivsize*n,0,minz); + glVertex3f(minx+ndivsize*n,0,maxz); + glVertex3f(minx,0,minz+ndivsize*n); + glVertex3f(maxx,0,minz+ndivsize*n); + } + + glEnd(); + +} + +void ViewerAR::DrawTrackedPoints(const std::vector &vKeys, const std::vector &vMPs, cv::Mat &im) +{ + const int N = vKeys.size(); + + + for(int i=0; i &vMPs, const int iterations) +{ + // Retrieve 3D points + vector vPoints; + vPoints.reserve(vMPs.size()); + vector vPointMP; + vPointMP.reserve(vMPs.size()); + + for(size_t i=0; iObservations()>5) + { + vPoints.push_back(pMP->GetWorldPos()); + vPointMP.push_back(pMP); + } + } + } + + const int N = vPoints.size(); + + if(N<50) + return NULL; + + + // Indices for minimum set selection + vector vAllIndices; + vAllIndices.reserve(N); + vector vAvailableIndices; + + for(int i=0; i bestvDist; + + //RANSAC + for(int n=0; n(3,0); + const float b = vt.at(3,1); + const float c = vt.at(3,2); + const float d = vt.at(3,3); + + vector vDistances(N,0); + + const float f = 1.0f/sqrt(a*a+b*b+c*c+d*d); + + for(int i=0; i(0)*a+vPoints[i].at(1)*b+vPoints[i].at(2)*c+d)*f; + } + + vector vSorted = vDistances; + sort(vSorted.begin(),vSorted.end()); + + int nth = max((int)(0.2*N),20); + const float medianDist = vSorted[nth]; + + if(medianDist vbInliers(N,false); + int nInliers = 0; + for(int i=0; i vInlierMPs(nInliers,NULL); + int nin = 0; + for(int i=0; i &vMPs, const cv::Mat &Tcw):mvMPs(vMPs),mTcw(Tcw.clone()) +{ + rang = -3.14f/2+((float)rand()/RAND_MAX)*3.14f; + Recompute(); +} + +void Plane::Recompute() +{ + const int N = mvMPs.size(); + + // Recompute plane with all points + cv::Mat A = cv::Mat(N,4,CV_32F); + A.col(3) = cv::Mat::ones(N,1,CV_32F); + + o = cv::Mat::zeros(3,1,CV_32F); + + int nPoints = 0; + for(int i=0; iisBad()) + { + cv::Mat Xw = pMP->GetWorldPos(); + o+=Xw; + A.row(nPoints).colRange(0,3) = Xw.t(); + nPoints++; + } + } + A.resize(nPoints); + + cv::Mat u,w,vt; + cv::SVDecomp(A,w,u,vt,cv::SVD::MODIFY_A | cv::SVD::FULL_UV); + + float a = vt.at(3,0); + float b = vt.at(3,1); + float c = vt.at(3,2); + + o = o*(1.0f/nPoints); + const float f = 1.0f/sqrt(a*a+b*b+c*c); + + // Compute XC just the first time + if(XC.empty()) + { + cv::Mat Oc = -mTcw.colRange(0,3).rowRange(0,3).t()*mTcw.rowRange(0,3).col(3); + XC = Oc-o; + } + + if((XC.at(0)*a+XC.at(1)*b+XC.at(2)*c)>0) + { + a=-a; + b=-b; + c=-c; + } + + const float nx = a*f; + const float ny = b*f; + const float nz = c*f; + + n = (cv::Mat_(3,1)<(3,1) << 0.0f, 1.0f, 0.0f); + + cv::Mat v = up.cross(n); + const float sa = cv::norm(v); + const float ca = up.dot(n); + const float ang = atan2(sa,ca); + Tpw = cv::Mat::eye(4,4,CV_32F); + + + Tpw.rowRange(0,3).colRange(0,3) = ExpSO3(v*ang/sa)*ExpSO3(up*rang); + o.copyTo(Tpw.col(3).rowRange(0,3)); + + glTpw.m[0] = Tpw.at(0,0); + glTpw.m[1] = Tpw.at(1,0); + glTpw.m[2] = Tpw.at(2,0); + glTpw.m[3] = 0.0; + + glTpw.m[4] = Tpw.at(0,1); + glTpw.m[5] = Tpw.at(1,1); + glTpw.m[6] = Tpw.at(2,1); + glTpw.m[7] = 0.0; + + glTpw.m[8] = Tpw.at(0,2); + glTpw.m[9] = Tpw.at(1,2); + glTpw.m[10] = Tpw.at(2,2); + glTpw.m[11] = 0.0; + + glTpw.m[12] = Tpw.at(0,3); + glTpw.m[13] = Tpw.at(1,3); + glTpw.m[14] = Tpw.at(2,3); + glTpw.m[15] = 1.0; + +} + +Plane::Plane(const float &nx, const float &ny, const float &nz, const float &ox, const float &oy, const float &oz) +{ + n = (cv::Mat_(3,1)<(3,1)<(3,1) << 0.0f, 1.0f, 0.0f); + + cv::Mat v = up.cross(n); + const float s = cv::norm(v); + const float c = up.dot(n); + const float a = atan2(s,c); + Tpw = cv::Mat::eye(4,4,CV_32F); + const float rang = -3.14f/2+((float)rand()/RAND_MAX)*3.14f; + cout << rang; + Tpw.rowRange(0,3).colRange(0,3) = ExpSO3(v*a/s)*ExpSO3(up*rang); + o.copyTo(Tpw.col(3).rowRange(0,3)); + + glTpw.m[0] = Tpw.at(0,0); + glTpw.m[1] = Tpw.at(1,0); + glTpw.m[2] = Tpw.at(2,0); + glTpw.m[3] = 0.0; + + glTpw.m[4] = Tpw.at(0,1); + glTpw.m[5] = Tpw.at(1,1); + glTpw.m[6] = Tpw.at(2,1); + glTpw.m[7] = 0.0; + + glTpw.m[8] = Tpw.at(0,2); + glTpw.m[9] = Tpw.at(1,2); + glTpw.m[10] = Tpw.at(2,2); + glTpw.m[11] = 0.0; + + glTpw.m[12] = Tpw.at(0,3); + glTpw.m[13] = Tpw.at(1,3); + glTpw.m[14] = Tpw.at(2,3); + glTpw.m[15] = 1.0; +} + +} diff --git a/Examples/ROS/ORB_SLAM2/src/AR/ViewerAR.h b/Examples/ROS/ORB_SLAM2/src/AR/ViewerAR.h new file mode 100644 index 0000000000..cd173ca707 --- /dev/null +++ b/Examples/ROS/ORB_SLAM2/src/AR/ViewerAR.h @@ -0,0 +1,120 @@ +/** +* This file is part of ORB-SLAM2. +* +* Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) +* For more information see +* +* ORB-SLAM2 is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* ORB-SLAM2 is distributed in the hope that it will be useful, +* but WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with ORB-SLAM2. If not, see . +*/ + + +#ifndef VIEWERAR_H +#define VIEWERAR_H + +#include +#include +#include +#include +#include"../../../include/System.h" + +namespace ORB_SLAM2 +{ + +class Plane +{ +public: + Plane(const std::vector &vMPs, const cv::Mat &Tcw); + Plane(const float &nx, const float &ny, const float &nz, const float &ox, const float &oy, const float &oz); + + void Recompute(); + + //normal + cv::Mat n; + //origin + cv::Mat o; + //arbitrary orientation along normal + float rang; + //transformation from world to the plane + cv::Mat Tpw; + pangolin::OpenGlMatrix glTpw; + //MapPoints that define the plane + std::vector mvMPs; + //camera pose when the plane was first observed (to compute normal direction) + cv::Mat mTcw, XC; +}; + +class ViewerAR +{ +public: + ViewerAR(); + + void SetFPS(const float fps){ + mFPS = fps; + mT=1e3/fps; + } + + void SetSLAM(ORB_SLAM2::System* pSystem){ + mpSystem = pSystem; + } + + // Main thread function. + void Run(); + + void SetCameraCalibration(const float &fx_, const float &fy_, const float &cx_, const float &cy_){ + fx = fx_; fy = fy_; cx = cx_; cy = cy_; + } + + void SetImagePose(const cv::Mat &im, const cv::Mat &Tcw, const int &status, + const std::vector &vKeys, const std::vector &vMPs); + + void GetImagePose(cv::Mat &im, cv::Mat &Tcw, int &status, + std::vector &vKeys, std::vector &vMPs); + +private: + + //SLAM + ORB_SLAM2::System* mpSystem; + + void PrintStatus(const int &status, const bool &bLocMode, cv::Mat &im); + void AddTextToImage(const std::string &s, cv::Mat &im, const int r=0, const int g=0, const int b=0); + void LoadCameraPose(const cv::Mat &Tcw); + void DrawImageTexture(pangolin::GlTexture &imageTexture, cv::Mat &im); + void DrawCube(const float &size, const float x=0, const float y=0, const float z=0); + void DrawPlane(int ndivs, float ndivsize); + void DrawPlane(Plane* pPlane, int ndivs, float ndivsize); + void DrawTrackedPoints(const std::vector &vKeys, const std::vector &vMPs, cv::Mat &im); + + Plane* DetectPlane(const cv::Mat Tcw, const std::vector &vMPs, const int iterations=50); + + // frame rate + float mFPS, mT; + float fx,fy,cx,cy; + + // Last processed image and computed pose by the SLAM + std::mutex mMutexPoseImage; + cv::Mat mTcw; + cv::Mat mImage; + int mStatus; + std::vector mvKeys; + std::vector mvMPs; + +}; + + +} + + +#endif // VIEWERAR_H + + diff --git a/Examples/ROS/ORB_SLAM2/src/AR/ros_mono_ar.cc b/Examples/ROS/ORB_SLAM2/src/AR/ros_mono_ar.cc new file mode 100644 index 0000000000..58d204a0f7 --- /dev/null +++ b/Examples/ROS/ORB_SLAM2/src/AR/ros_mono_ar.cc @@ -0,0 +1,169 @@ +/** +* This file is part of ORB-SLAM2. +* +* Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) +* For more information see +* +* ORB-SLAM2 is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* ORB-SLAM2 is distributed in the hope that it will be useful, +* but WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with ORB-SLAM2. If not, see . +*/ + + +#include +#include +#include +#include + +#include +#include + +#include +#include + +#include"../../../include/System.h" + +#include"ViewerAR.h" + +using namespace std; + + +ORB_SLAM2::ViewerAR viewerAR; +bool bRGB = true; + +cv::Mat K; +cv::Mat DistCoef; + + +class ImageGrabber +{ +public: + ImageGrabber(ORB_SLAM2::System* pSLAM):mpSLAM(pSLAM){} + + void GrabImage(const sensor_msgs::ImageConstPtr& msg); + + ORB_SLAM2::System* mpSLAM; +}; + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "Mono"); + ros::start(); + + if(argc != 3) + { + cerr << endl << "Usage: rosrun ORB_SLAM2 Mono path_to_vocabulary path_to_settings" << endl; + ros::shutdown(); + return 1; + } + + // Create SLAM system. It initializes all system threads and gets ready to process frames. + ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::MONOCULAR,false); + + + cout << endl << endl; + cout << "-----------------------" << endl; + cout << "Augmented Reality Demo" << endl; + cout << "1) Translate the camera to initialize SLAM." << endl; + cout << "2) Look at a planar region and translate the camera." << endl; + cout << "3) Press Insert Cube to place a virtual cube in the plane. " << endl; + cout << endl; + cout << "You can place several cubes in different planes." << endl; + cout << "-----------------------" << endl; + cout << endl; + + + viewerAR.SetSLAM(&SLAM); + + ImageGrabber igb(&SLAM); + + ros::NodeHandle nodeHandler; + ros::Subscriber sub = nodeHandler.subscribe("/camera/image_raw", 1, &ImageGrabber::GrabImage,&igb); + + + cv::FileStorage fSettings(argv[2], cv::FileStorage::READ); + bRGB = static_cast((int)fSettings["Camera.RGB"]); + float fps = fSettings["Camera.fps"]; + viewerAR.SetFPS(fps); + + float fx = fSettings["Camera.fx"]; + float fy = fSettings["Camera.fy"]; + float cx = fSettings["Camera.cx"]; + float cy = fSettings["Camera.cy"]; + + viewerAR.SetCameraCalibration(fx,fy,cx,cy); + + K = cv::Mat::eye(3,3,CV_32F); + K.at(0,0) = fx; + K.at(1,1) = fy; + K.at(0,2) = cx; + K.at(1,2) = cy; + + DistCoef = cv::Mat::zeros(4,1,CV_32F); + DistCoef.at(0) = fSettings["Camera.k1"]; + DistCoef.at(1) = fSettings["Camera.k2"]; + DistCoef.at(2) = fSettings["Camera.p1"]; + DistCoef.at(3) = fSettings["Camera.p2"]; + const float k3 = fSettings["Camera.k3"]; + if(k3!=0) + { + DistCoef.resize(5); + DistCoef.at(4) = k3; + } + + thread tViewer = thread(&ORB_SLAM2::ViewerAR::Run,&viewerAR); + + ros::spin(); + + // Stop all threads + SLAM.Shutdown(); + + // Save camera trajectory + SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt"); + + ros::shutdown(); + + return 0; +} + +void ImageGrabber::GrabImage(const sensor_msgs::ImageConstPtr& msg) +{ + // Copy the ros image message to cv::Mat. + cv_bridge::CvImageConstPtr cv_ptr; + try + { + cv_ptr = cv_bridge::toCvShare(msg); + } + catch (cv_bridge::Exception& e) + { + ROS_ERROR("cv_bridge exception: %s", e.what()); + return; + } + cv::Mat im = cv_ptr->image.clone(); + cv::Mat imu; + cv::Mat Tcw = mpSLAM->TrackMonocular(cv_ptr->image,cv_ptr->header.stamp.toSec()); + int state = mpSLAM->GetTrackingState(); + vector vMPs = mpSLAM->GetTrackedMapPoints(); + vector vKeys = mpSLAM->GetTrackedKeyPointsUn(); + + cv::undistort(im,imu,K,DistCoef); + + if(bRGB) + viewerAR.SetImagePose(imu,Tcw,state,vKeys,vMPs); + else + { + cv::cvtColor(imu,imu,CV_RGB2BGR); + viewerAR.SetImagePose(imu,Tcw,state,vKeys,vMPs); + } +} + + diff --git a/build_ros.sh b/build_ros.sh new file mode 100755 index 0000000000..d0a5edbb5f --- /dev/null +++ b/build_ros.sh @@ -0,0 +1,7 @@ +echo "Configuring and building Thirdparty/DBoW2 ..." + +cd Examples/ROS/ORB_SLAM2 +mkdir build +cd build +cmake .. -DROS_BUILD_TYPE=Release +make -j