diff --git a/ROS2/ORB_SLAM3/CMakeLists.txt b/ROS2/ORB_SLAM3/CMakeLists.txt index 1f456463a1..3f04eb7377 100644 --- a/ROS2/ORB_SLAM3/CMakeLists.txt +++ b/ROS2/ORB_SLAM3/CMakeLists.txt @@ -30,12 +30,14 @@ find_package(Pangolin REQUIRED) find_package(rclcpp REQUIRED) find_package(cv_bridge REQUIRED) find_package(sensor_msgs REQUIRED) +find_package(message_filters REQUIRED) set(ROS_DEPS rclcpp ament_cmake cv_bridge sensor_msgs + message_filters ) include_directories( @@ -59,15 +61,31 @@ ${PROJECT_SOURCE_DIR}/../../lib/libORB_SLAM3.so ) # Node for monocular camera -add_executable(Mono -src/ros_mono.cc -) -target_link_libraries(Mono ${LIBS} ) +add_executable(Mono src/ros_mono.cc) +target_link_libraries(Mono ${LIBS}) ament_target_dependencies(Mono rclcpp ${ROS_DEPS}) # Node for monocular inrtial camera -add_executable(MonoInertial -src/ros_mono_inertial.cc -) +add_executable(MonoInertial src/ros_mono_inertial.cc) target_link_libraries(MonoInertial ${LIBS} ) ament_target_dependencies(MonoInertial rclcpp ${ROS_DEPS}) + +# Node for monocular inrtial camera +add_executable(RGBD src/ros_rgbd.cc) +target_link_libraries(RGBD ${LIBS} ) +ament_target_dependencies(RGBD rclcpp ${ROS_DEPS}) + +# Node for monocular inrtial camera +add_executable(Stereo src/ros_stereo.cc) +target_link_libraries(Stereo ${LIBS} ) +ament_target_dependencies(Stereo rclcpp ${ROS_DEPS}) + +# Node for monocular inrtial camera +add_executable(StereoInertial src/ros_stereo_inertial.cc) +target_link_libraries(StereoInertial ${LIBS} ) +ament_target_dependencies(StereoInertial rclcpp ${ROS_DEPS}) + +# # Node for monocular camera +# add_executable(AR src/AR/ros_mono_ar.cc src/AR/ViewerAR.cc src/AR/ViewerAR.h) +# target_link_libraries(AR ${LIBS}) +# ament_target_dependencies(AR rclcpp ${ROS_DEPS}) \ No newline at end of file diff --git a/ROS2/ORB_SLAM3/package.xml b/ROS2/ORB_SLAM3/package.xml index ca7c99253b..df19fcea76 100644 --- a/ROS2/ORB_SLAM3/package.xml +++ b/ROS2/ORB_SLAM3/package.xml @@ -16,5 +16,6 @@ tf image_transport cv_bridge + message_filters \ No newline at end of file diff --git a/ROS2/ORB_SLAM3/src/AR/ViewerAR.cc b/ROS2/ORB_SLAM3/src/AR/ViewerAR.cc new file mode 100644 index 0000000000..dc9f18b7e3 --- /dev/null +++ b/ROS2/ORB_SLAM3/src/AR/ViewerAR.cc @@ -0,0 +1,640 @@ +/** +* This file is part of ORB-SLAM3 +* +* Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* +* ORB-SLAM3 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-SLAM3 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-SLAM3. +* If not, see . +*/ + +#include "ViewerAR.h" + +#include + +#include +#include +#include + +using namespace std; + +namespace ORB_SLAM3 +{ + +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/ROS2/ORB_SLAM3/src/AR/ViewerAR.h b/ROS2/ORB_SLAM3/src/AR/ViewerAR.h new file mode 100644 index 0000000000..f2c91ec1ed --- /dev/null +++ b/ROS2/ORB_SLAM3/src/AR/ViewerAR.h @@ -0,0 +1,116 @@ +/** +* This file is part of ORB-SLAM3 +* +* Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* +* ORB-SLAM3 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-SLAM3 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-SLAM3. +* If not, see . +*/ + + +#ifndef VIEWERAR_H +#define VIEWERAR_H + +#include +#include +#include +#include +#include"../../../../include/System.h" + +namespace ORB_SLAM3 +{ + +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_SLAM3::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_SLAM3::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/ROS2/ORB_SLAM3/src/AR/ros_mono_ar.cc b/ROS2/ORB_SLAM3/src/AR/ros_mono_ar.cc new file mode 100644 index 0000000000..036b17f26c --- /dev/null +++ b/ROS2/ORB_SLAM3/src/AR/ros_mono_ar.cc @@ -0,0 +1,167 @@ +/** +* This file is part of ORB-SLAM3 +* +* Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* +* ORB-SLAM3 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-SLAM3 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-SLAM3. +* If not, see . +*/ + + +#include +#include +#include +#include + +#include +#include + +#include +#include + +#include"../../../../include/System.h" + +#include"ViewerAR.h" + +using namespace std; + + +ORB_SLAM3::ViewerAR viewerAR; +bool bRGB = true; + +cv::Mat K; +cv::Mat DistCoef; + + +class ImageGrabber: public rclcpp::Node +{ +public: + ImageGrabber(ORB_SLAM3::System* pSLAM): + Node("Mono-AR"), + mpSLAM(pSLAM) + { + sub_ = this->create_subscription("/camera/image_raw", 1, std::bind(&ImageGrabber::GrabImage, this, std::placeholders::_1)); + } + + void GrabImage(const sensor_msgs::msg::Image::ConstPtr& msg); + + ORB_SLAM3::System* mpSLAM; +private: + rclcpp::Subscription::SharedPtr sub_; +}; + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + + if(argc != 3) + { + cerr << endl << "Usage: rosrun ORB_SLAM3 Mono path_to_vocabulary path_to_settings" << endl; + rclcpp::shutdown(); + return 1; + } + + // Create SLAM system. It initializes all system threads and gets ready to process frames. + ORB_SLAM3::System SLAM(argv[1],argv[2],ORB_SLAM3::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); + + 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_SLAM3::ViewerAR::Run,&viewerAR); + + rclcpp::spin(std::make_shared(&SLAM)); + + // Stop all threads + SLAM.Shutdown(); + + // Save camera trajectory + SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt"); + + rclcpp::shutdown(); + + return 0; +} + +void ImageGrabber::GrabImage(const sensor_msgs::msg::Image::ConstPtr& 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) + { + RCLCPP_ERROR(this->get_logger(), "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.sec); + 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/ROS2/ORB_SLAM3/src/ros_mono_inertial.cc b/ROS2/ORB_SLAM3/src/ros_mono_inertial.cc index 3a0a2a558b..fbfb5d3ec3 100644 --- a/ROS2/ORB_SLAM3/src/ros_mono_inertial.cc +++ b/ROS2/ORB_SLAM3/src/ros_mono_inertial.cc @@ -70,7 +70,6 @@ class ImageGrabber: public ImuGrabber std::mutex ImageBufMutex; ORB_SLAM3::System* mpSLAM; - ImuGrabber ImuGb; const bool mbClahe; cv::Ptr mClahe = cv::createCLAHE(3.0, cv::Size(8, 8)); @@ -89,7 +88,6 @@ int main(int argc, char **argv) return 1; } - if(argc==4) { std::string sbEqual(argv[3]); @@ -186,7 +184,7 @@ void ImageGrabber::SyncWithImu() } } -void ImuGrabber::GrabImu(const sensor_msgs::msg::Imu::ConstPtr &imu_msg) +void ImuGrabber::GrabImu(const sensor_msgs::msg::Imu::ConstPtr &imu_msg) { ImuBufMutex.lock(); imuBuf.push(imu_msg); diff --git a/ROS2/ORB_SLAM3/src/ros_rgbd.cc b/ROS2/ORB_SLAM3/src/ros_rgbd.cc new file mode 100644 index 0000000000..b8b953a221 --- /dev/null +++ b/ROS2/ORB_SLAM3/src/ros_rgbd.cc @@ -0,0 +1,112 @@ +/** +* This file is part of ORB-SLAM3 +* +* Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* +* ORB-SLAM3 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-SLAM3 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-SLAM3. +* If not, see . +*/ + + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include + +#include"../../../include/System.h" + +using namespace std; + +class ImageGrabber: public rclcpp::Node +{ +public: + ImageGrabber(ORB_SLAM3::System* pSLAM): + mpSLAM(pSLAM), + Node("RGBD") + { + message_filters::Subscriber rgb_sub(this, "/camera/rgb/image_raw"); + message_filters::Subscriber depth_sub(this, "camera/depth_registered/image_raw"); + typedef message_filters::sync_policies::ApproximateTime sync_pol; + message_filters::Synchronizer sync(sync_pol(10), rgb_sub,depth_sub); + sync.registerCallback(std::bind(&ImageGrabber::GrabRGBD,this, std::placeholders::_1, std::placeholders::_2)); + } + + void GrabRGBD(const sensor_msgs::msg::Image::ConstPtr& msgRGB,const sensor_msgs::msg::Image::ConstPtr& msgD); + + ORB_SLAM3::System* mpSLAM; +}; + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + + if(argc != 3) + { + cerr << endl << "Usage: rosrun ORB_SLAM3 RGBD path_to_vocabulary path_to_settings" << endl; + rclcpp::shutdown(); + return 1; + } + + // Create SLAM system. It initializes all system threads and gets ready to process frames. + ORB_SLAM3::System SLAM(argv[1],argv[2],ORB_SLAM3::System::RGBD,true); + + // ImageGrabber igb(&SLAM); + + rclcpp::spin(std::make_shared(&SLAM)); + + // Stop all threads + SLAM.Shutdown(); + + // Save camera trajectory + SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt"); + + rclcpp::shutdown(); + + return 0; +} + +void ImageGrabber::GrabRGBD(const sensor_msgs::msg::Image::ConstPtr& msgRGB,const sensor_msgs::msg::Image::ConstPtr& msgD) +{ + // Copy the ros image message to cv::Mat. + cv_bridge::CvImageConstPtr cv_ptrRGB; + try + { + cv_ptrRGB = cv_bridge::toCvShare(msgRGB); + } + catch (cv_bridge::Exception& e) + { + RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what()); + return; + } + + cv_bridge::CvImageConstPtr cv_ptrD; + try + { + cv_ptrD = cv_bridge::toCvShare(msgD); + } + catch (cv_bridge::Exception& e) + { + RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what()); + return; + } + mpSLAM->TrackRGBD(cv_ptrRGB->image,cv_ptrD->image,cv_ptrRGB->header.stamp.sec); +} + + diff --git a/ROS2/ORB_SLAM3/src/ros_stereo.cc b/ROS2/ORB_SLAM3/src/ros_stereo.cc new file mode 100644 index 0000000000..ba5e92af3e --- /dev/null +++ b/ROS2/ORB_SLAM3/src/ros_stereo.cc @@ -0,0 +1,170 @@ +/** +* This file is part of ORB-SLAM3 +* +* Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* +* ORB-SLAM3 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-SLAM3 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-SLAM3. +* If not, see . +*/ + + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include + +#include"../../../include/System.h" + +using namespace std; + +class ImageGrabber: public rclcpp::Node +{ +public: + ImageGrabber(ORB_SLAM3::System* pSLAM): + mpSLAM(pSLAM), + Node("RGBD") + { + message_filters::Subscriber left_sub(this, "/camera/left/image_raw"); + message_filters::Subscriber right_sub(this, "/camera/right/image_raw"); + typedef message_filters::sync_policies::ApproximateTime sync_pol; + message_filters::Synchronizer sync(sync_pol(10), left_sub,right_sub); + sync.registerCallback(std::bind(&ImageGrabber::GrabStereo,this,std::placeholders::_1,std::placeholders::_2)); + } + + void GrabStereo(const sensor_msgs::msg::Image::ConstPtr& msgLeft,const sensor_msgs::msg::Image::ConstPtr& msgRight); + + ORB_SLAM3::System* mpSLAM; + bool do_rectify; + cv::Mat M1l,M2l,M1r,M2r; +}; + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + + if(argc != 4) + { + cerr << endl << "Usage: rosrun ORB_SLAM3 Stereo path_to_vocabulary path_to_settings do_rectify" << endl; + rclcpp::shutdown(); + return 1; + } + + // Create SLAM system. It initializes all system threads and gets ready to process frames. + ORB_SLAM3::System SLAM(argv[1],argv[2],ORB_SLAM3::System::STEREO,true); + + // ImageGrabber igb(&SLAM); + auto node = std::make_shared(&SLAM); + + stringstream ss(argv[3]); + ss >> boolalpha >> node->do_rectify; + + if(node->do_rectify) + { + // Load settings related to stereo calibration + cv::FileStorage fsSettings(argv[2], cv::FileStorage::READ); + if(!fsSettings.isOpened()) + { + cerr << "ERROR: Wrong path to settings" << endl; + return -1; + } + + cv::Mat K_l, K_r, P_l, P_r, R_l, R_r, D_l, D_r; + fsSettings["LEFT.K"] >> K_l; + fsSettings["RIGHT.K"] >> K_r; + + fsSettings["LEFT.P"] >> P_l; + fsSettings["RIGHT.P"] >> P_r; + + fsSettings["LEFT.R"] >> R_l; + fsSettings["RIGHT.R"] >> R_r; + + fsSettings["LEFT.D"] >> D_l; + fsSettings["RIGHT.D"] >> D_r; + + int rows_l = fsSettings["LEFT.height"]; + int cols_l = fsSettings["LEFT.width"]; + int rows_r = fsSettings["RIGHT.height"]; + int cols_r = fsSettings["RIGHT.width"]; + + if(K_l.empty() || K_r.empty() || P_l.empty() || P_r.empty() || R_l.empty() || R_r.empty() || D_l.empty() || D_r.empty() || + rows_l==0 || rows_r==0 || cols_l==0 || cols_r==0) + { + cerr << "ERROR: Calibration parameters to rectify stereo are missing!" << endl; + return -1; + } + cv::initUndistortRectifyMap(K_l,D_l,R_l,P_l.rowRange(0,3).colRange(0,3),cv::Size(cols_l,rows_l),CV_32F,node->M1l,node->M2l); + cv::initUndistortRectifyMap(K_r,D_r,R_r,P_r.rowRange(0,3).colRange(0,3),cv::Size(cols_r,rows_r),CV_32F,node->M1r,node->M2r); + } + + rclcpp::spin(node); + + // Stop all threads + SLAM.Shutdown(); + + // Save camera trajectory + SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory_TUM_Format.txt"); + SLAM.SaveTrajectoryTUM("FrameTrajectory_TUM_Format.txt"); + SLAM.SaveTrajectoryKITTI("FrameTrajectory_KITTI_Format.txt"); + + rclcpp::shutdown(); + + return 0; +} + +void ImageGrabber::GrabStereo(const sensor_msgs::msg::Image::ConstPtr& msgLeft,const sensor_msgs::msg::Image::ConstPtr& msgRight) +{ + // Copy the ros image message to cv::Mat. + cv_bridge::CvImageConstPtr cv_ptrLeft; + try + { + cv_ptrLeft = cv_bridge::toCvShare(msgLeft); + } + catch (cv_bridge::Exception& e) + { + RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what()); + return; + } + + cv_bridge::CvImageConstPtr cv_ptrRight; + try + { + cv_ptrRight = cv_bridge::toCvShare(msgRight); + } + catch (cv_bridge::Exception& e) + { + RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what()); + return; + } + + if(do_rectify) + { + cv::Mat imLeft, imRight; + cv::remap(cv_ptrLeft->image,imLeft,M1l,M2l,cv::INTER_LINEAR); + cv::remap(cv_ptrRight->image,imRight,M1r,M2r,cv::INTER_LINEAR); + mpSLAM->TrackStereo(imLeft,imRight,cv_ptrLeft->header.stamp.sec); + } + else + { + mpSLAM->TrackStereo(cv_ptrLeft->image,cv_ptrRight->image,cv_ptrLeft->header.stamp.sec); + } + +} + + diff --git a/ROS2/ORB_SLAM3/src/ros_stereo_inertial.cc b/ROS2/ORB_SLAM3/src/ros_stereo_inertial.cc new file mode 100644 index 0000000000..598e508d6f --- /dev/null +++ b/ROS2/ORB_SLAM3/src/ros_stereo_inertial.cc @@ -0,0 +1,296 @@ +/** +* This file is part of ORB-SLAM3 +* +* Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* +* ORB-SLAM3 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-SLAM3 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-SLAM3. +* If not, see . +*/ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include + +#include "../../../include/System.h" +#include "../include/ImuTypes.h" + +using namespace std; + +class ImuGrabber: public rclcpp::Node +{ +public: + ImuGrabber():Node("Stero_Inertial") + { + sub_imu_ = this->create_subscription("/imu", 1000, std::bind(&ImuGrabber::GrabImu, this, std::placeholders::_1)); + }; + void GrabImu(const sensor_msgs::msg::Imu::ConstPtr &imu_msg); + + queue imuBuf; + std::mutex ImuBufMutex; + private: + rclcpp::Subscription::SharedPtr sub_imu_; + +}; + +class ImageGrabber: public ImuGrabber +{ +public: + ImageGrabber(ORB_SLAM3::System* pSLAM, const bool bRect, const bool bClahe): + mpSLAM(pSLAM), + do_rectify(bRect), + mbClahe(bClahe) + { + // Maximum delay, 5 seconds + sub_img_left = this->create_subscription("/camera/left/image_raw", 100, std::bind(&ImageGrabber::GrabImageLeft, this, std::placeholders::_1)); + sub_img_right = this->create_subscription("/camera/right/image_raw", 100, std::bind(&ImageGrabber::GrabImageRight, this, std::placeholders::_1)); + } + + void GrabImageLeft(const sensor_msgs::msg::Image::ConstPtr& msg); + void GrabImageRight(const sensor_msgs::msg::Image::ConstPtr& msg); + cv::Mat GetImage(const sensor_msgs::msg::Image::ConstPtr &img_msg); + void SyncWithImu(); + + queue imgLeftBuf, imgRightBuf; + std::mutex mBufMutexLeft,mBufMutexRight; + + ORB_SLAM3::System* mpSLAM; + ImuGrabber mpImuGb; + + const bool do_rectify; + cv::Mat M1l,M2l,M1r,M2r; + + const bool mbClahe; + cv::Ptr mClahe = cv::createCLAHE(3.0, cv::Size(8, 8)); + private: + rclcpp::Subscription::SharedPtr sub_img_left; + rclcpp::Subscription::SharedPtr sub_img_right; +}; + + + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + bool bEqual = false; + if(argc < 4 || argc > 5) + { + cerr << endl << "Usage: rosrun ORB_SLAM3 Stereo_Inertial path_to_vocabulary path_to_settings do_rectify [do_equalize]" << endl; + rclcpp::shutdown(); + return 1; + } + + std::string sbRect(argv[3]); + if(argc==5) + { + std::string sbEqual(argv[4]); + if(sbEqual == "true") + bEqual = true; + } + + // Create SLAM system. It initializes all system threads and gets ready to process frames. + ORB_SLAM3::System SLAM(argv[1],argv[2],ORB_SLAM3::System::IMU_STEREO,true); + + auto node = std::make_shared(&SLAM,sbRect == "true", &bEqual); + + if(node->do_rectify) + { + // Load settings related to stereo calibration + cv::FileStorage fsSettings(argv[2], cv::FileStorage::READ); + if(!fsSettings.isOpened()) + { + cerr << "ERROR: Wrong path to settings" << endl; + return -1; + } + + cv::Mat K_l, K_r, P_l, P_r, R_l, R_r, D_l, D_r; + fsSettings["LEFT.K"] >> K_l; + fsSettings["RIGHT.K"] >> K_r; + + fsSettings["LEFT.P"] >> P_l; + fsSettings["RIGHT.P"] >> P_r; + + fsSettings["LEFT.R"] >> R_l; + fsSettings["RIGHT.R"] >> R_r; + + fsSettings["LEFT.D"] >> D_l; + fsSettings["RIGHT.D"] >> D_r; + + int rows_l = fsSettings["LEFT.height"]; + int cols_l = fsSettings["LEFT.width"]; + int rows_r = fsSettings["RIGHT.height"]; + int cols_r = fsSettings["RIGHT.width"]; + + if(K_l.empty() || K_r.empty() || P_l.empty() || P_r.empty() || R_l.empty() || R_r.empty() || D_l.empty() || D_r.empty() || + rows_l==0 || rows_r==0 || cols_l==0 || cols_r==0) + { + cerr << "ERROR: Calibration parameters to rectify stereo are missing!" << endl; + return -1; + } + + cv::initUndistortRectifyMap(K_l,D_l,R_l,P_l.rowRange(0,3).colRange(0,3),cv::Size(cols_l,rows_l),CV_32F,node->M1l,node->M2l); + cv::initUndistortRectifyMap(K_r,D_r,R_r,P_r.rowRange(0,3).colRange(0,3),cv::Size(cols_r,rows_r),CV_32F,node->M1r,node->M2r); + } + + + std::thread sync_thread(&ImageGrabber::SyncWithImu, node); + + rclcpp::spin(node); + + return 0; +} + + + +void ImageGrabber::GrabImageLeft(const sensor_msgs::msg::Image::ConstPtr &img_msg) +{ + mBufMutexLeft.lock(); + if (!imgLeftBuf.empty()) + imgLeftBuf.pop(); + imgLeftBuf.push(img_msg); + mBufMutexLeft.unlock(); +} + +void ImageGrabber::GrabImageRight(const sensor_msgs::msg::Image::ConstPtr &img_msg) +{ + mBufMutexRight.lock(); + if (!imgRightBuf.empty()) + imgRightBuf.pop(); + imgRightBuf.push(img_msg); + mBufMutexRight.unlock(); +} + +cv::Mat ImageGrabber::GetImage(const sensor_msgs::msg::Image::ConstPtr &img_msg) +{ + // Copy the ros image message to cv::Mat. + cv_bridge::CvImageConstPtr cv_ptr; + try + { + cv_ptr = cv_bridge::toCvShare(img_msg, sensor_msgs::image_encodings::MONO8); + } + catch (cv_bridge::Exception& e) + { + RCLCPP_ERROR(this->get_logger(),"cv_bridge exception: %s", e.what()); + } + + if(cv_ptr->image.type()==0) + { + return cv_ptr->image.clone(); + } + else + { + std::cout << "Error type" << std::endl; + return cv_ptr->image.clone(); + } +} + +void ImageGrabber::SyncWithImu() +{ + const double maxTimeDiff = 0.01; + while(1) + { + cv::Mat imLeft, imRight; + double tImLeft = 0, tImRight = 0; + if (!imgLeftBuf.empty()&&!imgRightBuf.empty()&&!imuBuf.empty()) + { + tImLeft = imgLeftBuf.front()->header.stamp.sec; + tImRight = imgRightBuf.front()->header.stamp.sec; + + this->mBufMutexRight.lock(); + while((tImLeft-tImRight)>maxTimeDiff && imgRightBuf.size()>1) + { + imgRightBuf.pop(); + tImRight = imgRightBuf.front()->header.stamp.sec; + } + this->mBufMutexRight.unlock(); + + this->mBufMutexLeft.lock(); + while((tImRight-tImLeft)>maxTimeDiff && imgLeftBuf.size()>1) + { + imgLeftBuf.pop(); + tImLeft = imgLeftBuf.front()->header.stamp.sec; + } + this->mBufMutexLeft.unlock(); + + if((tImLeft-tImRight)>maxTimeDiff || (tImRight-tImLeft)>maxTimeDiff) + { + // std::cout << "big time difference" << std::endl; + continue; + } + if(tImLeft>imuBuf.back()->header.stamp.sec) + continue; + + this->mBufMutexLeft.lock(); + imLeft = GetImage(imgLeftBuf.front()); + imgLeftBuf.pop(); + this->mBufMutexLeft.unlock(); + + this->mBufMutexRight.lock(); + imRight = GetImage(imgRightBuf.front()); + imgRightBuf.pop(); + this->mBufMutexRight.unlock(); + + vector vImuMeas; + ImuBufMutex.lock(); + if(!imuBuf.empty()) + { + // Load imu measurements from buffer + vImuMeas.clear(); + while(!imuBuf.empty() && imuBuf.front()->header.stamp.sec<=tImLeft) + { + double t = imuBuf.front()->header.stamp.sec; + cv::Point3f acc(imuBuf.front()->linear_acceleration.x, imuBuf.front()->linear_acceleration.y, imuBuf.front()->linear_acceleration.z); + cv::Point3f gyr(imuBuf.front()->angular_velocity.x, imuBuf.front()->angular_velocity.y, imuBuf.front()->angular_velocity.z); + vImuMeas.push_back(ORB_SLAM3::IMU::Point(acc,gyr,t)); + imuBuf.pop(); + } + } + ImuBufMutex.unlock(); + if(mbClahe) + { + mClahe->apply(imLeft,imLeft); + mClahe->apply(imRight,imRight); + } + + if(do_rectify) + { + cv::remap(imLeft,imLeft,M1l,M2l,cv::INTER_LINEAR); + cv::remap(imRight,imRight,M1r,M2r,cv::INTER_LINEAR); + } + + mpSLAM->TrackStereo(imLeft,imRight,tImLeft,vImuMeas); + + std::chrono::milliseconds tSleep(1); + std::this_thread::sleep_for(tSleep); + } + } +} + +void ImuGrabber::GrabImu(const sensor_msgs::msg::Imu::ConstPtr &imu_msg) +{ + ImuBufMutex.lock(); + imuBuf.push(imu_msg); + ImuBufMutex.unlock(); + return; +} + +