From 08ddff41302224f28fedccd49e0a1a44b74f38f9 Mon Sep 17 00:00:00 2001 From: Jerryaa98 Date: Thu, 31 Aug 2023 17:39:48 +0300 Subject: [PATCH] removed reddundant variables and menus --- slam/include/FrameDrawer.h | 2 + slam/src/FrameDrawer.cc | 48 +++--- slam/src/Viewer.cc | 301 ++----------------------------------- 3 files changed, 40 insertions(+), 311 deletions(-) diff --git a/slam/include/FrameDrawer.h b/slam/include/FrameDrawer.h index 872768ba..ad8069c8 100644 --- a/slam/include/FrameDrawer.h +++ b/slam/include/FrameDrawer.h @@ -52,6 +52,8 @@ class FrameDrawer void DrawTextInfo(cv::Mat &im, int nState, cv::Mat &imText); + void DrawRectangleAndCircle(float x, float y, float width); + // Info of the frame to be drawn cv::Mat mIm; int N; diff --git a/slam/src/FrameDrawer.cc b/slam/src/FrameDrawer.cc index e1c368ab..e974aca8 100644 --- a/slam/src/FrameDrawer.cc +++ b/slam/src/FrameDrawer.cc @@ -124,35 +124,12 @@ namespace ORB_SLAM2 { // This is a match to a MapPoint in the map if (vbMap[i]) { - - glColor3f(0, 1.0f, 0); - glBegin(GL_LINE_LOOP); - glVertex2f(x1 - width, y1 - width); - glVertex2f(x1 - width, y1 + width); - glVertex2f(x1 + width, y1 + width); - glVertex2f(x1 + width, y1 - width); - glEnd(); - - glBegin(GL_POINTS); - glVertex2f(x1, y1); - glEnd(); - + DrawRectangleAndCircle(x1, y1, width); mnTracked++; } else // This is match to a "visual odometry" MapPoint created in the last frame { - glColor3f(0, 1.0f, 0); - glBegin(GL_LINE_LOOP); - glVertex2f(x1 - width, y1 - width); - glVertex2f(x1 - width, y1 + width); - glVertex2f(x1 + width, y1 + width); - glVertex2f(x1 + width, y1 - width); - glEnd(); - - glBegin(GL_POINTS); - glVertex2f(x1, y1); - glEnd(); - + DrawRectangleAndCircle(x1, y1, width); mnTrackedVO++; } } @@ -160,6 +137,20 @@ namespace ORB_SLAM2 { } } + void FrameDrawer::DrawRectangleAndCircle(float x, float y, float width) { + glColor3f(0, 1.0f, 0); + glBegin(GL_LINE_LOOP); + glVertex2f(x - width, y - width); + glVertex2f(x - width, y + width); + glVertex2f(x + width, y + width); + glVertex2f(x + width, y - width); + glEnd(); + + glBegin(GL_POINTS); + glVertex2f(x, y); + glEnd(); + } + void FrameDrawer::DrawTextInfo(cv::Mat &im, int nState, cv::Mat &imText) { stringstream s; @@ -191,6 +182,13 @@ namespace ORB_SLAM2 { imText.rowRange(im.rows, imText.rows) = cv::Mat::zeros(textSize.height + 10, im.cols, im.type()); cv::putText(imText, s.str(), cv::Point(5, imText.rows - 5), cv::FONT_HERSHEY_PLAIN, 1, cv::Scalar(255, 255, 255), 1, 8); + /*glColor3f(1.0f, 1.0f, 1.0f); + glRasterPos2f(5, imText.rows - 5); + int len, i; + len = s.str().size(); + for (i = 0; i < len; i++) { + glutBitmapCharacter(GLUT_BITMAP_HELVETICA_18, s.str()[i]); + }*/ } diff --git a/slam/src/Viewer.cc b/slam/src/Viewer.cc index b6e853d6..3ee4789e 100644 --- a/slam/src/Viewer.cc +++ b/slam/src/Viewer.cc @@ -95,30 +95,6 @@ namespace ORB_SLAM2 // Issue specific OpenGl we might need glEnable(GL_BLEND); glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); - if (isPangolinExists) - { - pangolin::Panel("menu").SetBounds(0.0, 1.0, 0.0, pangolin::Attach::Pix(175)); - } - else - { - pangolin::CreatePanel("menu").SetBounds(0.0, 1.0, 0.0, pangolin::Attach::Pix(175)); - } - pangolin::Var menuFollowCamera("menu.Follow Camera", true, true); - pangolin::Var menuShowPoints("menu.Show Points", true, true); - pangolin::Var menuShowKeyFrames("menu.Show KeyFrames", true, true); - pangolin::Var menuShowGraph("menu.Show Graph", true, true); - pangolin::Var menuLocalizationMode("menu.Localization Mode", mbReuse, true); - pangolin::Var menuOpenSimulator("menu.Open Simulator", false, true); - pangolin::Var menuReset("menu.Reset", false, false); - pangolin::Var menuShutDown("menu.ShutDown", false, false); - pangolin::Var menuMoveLeft("menu.Move Left", false, false); - pangolin::Var menuMoveRight("menu.Move Right", false, false); - pangolin::Var menuMoveDown("menu.Move Down", false, false); - pangolin::Var menuMoveUp("menu.Move Up", false, false); - pangolin::Var menuRotateLeft("menu.Rotate Left", false, false); - pangolin::Var menuRotateRight("menu.Rotate Right", false, false); - pangolin::Var menuRotateDown("menu.Rotate Down", false, false); - pangolin::Var menuRotateUp("menu.Rotate Up", false, false); // Define Camera Render Object (for view / scene browsing) pangolin::OpenGlRenderState s_cam( @@ -149,288 +125,41 @@ namespace ORB_SLAM2 { glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); - if (!menuOpenSimulator) - { - mpMapDrawer->GetCurrentOpenGLCameraMatrix(Twc); - } - else - { - Twc.m[0] = (float)mTwc.at(0); - Twc.m[1] = (float)mTwc.at(1); - Twc.m[2] = (float)mTwc.at(2); - Twc.m[4] = (float)mTwc.at(4); - Twc.m[5] = (float)mTwc.at(5); - Twc.m[6] = (float)mTwc.at(6); - Twc.m[8] = (float)mTwc.at(8); - Twc.m[9] = (float)mTwc.at(9); - Twc.m[10] = (float)mTwc.at(10); - Twc.m[12] = (float)mTwc.at(12); - Twc.m[13] = (float)mTwc.at(13); - Twc.m[14] = (float)mTwc.at(14); - } - if (menuFollowCamera && bFollow) + mpMapDrawer->GetCurrentOpenGLCameraMatrix(Twc); + + if (bFollow) { s_cam.Follow(Twc); } - else if (menuFollowCamera && !bFollow) + else if (!bFollow) { s_cam.SetModelViewMatrix( pangolin::ModelViewLookAt(mViewpointX, mViewpointY, mViewpointZ, 0, 0, 0, 0.0, -1.0, 0.0)); s_cam.Follow(Twc); bFollow = true; } - else if (!menuFollowCamera && bFollow) - { - bFollow = false; - } - if (menuLocalizationMode && !bLocalizationMode && !menuOpenSimulator) + if (!bLocalizationMode) { mpSystem->ActivateLocalizationMode(); bLocalizationMode = true; } - else if (!menuLocalizationMode && bLocalizationMode && !menuOpenSimulator) - { - mpSystem->DeactivateLocalizationMode(); - bLocalizationMode = false; - } - d_cam.Activate(s_cam); - // mpMapDrawer->DrawCurrentCamera(Twc); - if (!menuOpenSimulator && (menuShowKeyFrames || menuShowGraph)) - mpMapDrawer->DrawKeyFrames(menuShowKeyFrames, menuShowGraph); - if (menuShowPoints) - { - if (!menuOpenSimulator) - { - mpMapDrawer->DrawMapPoints(); - } - else - { - mpMapDrawer->DrawMapPoints(true, mPointsSeen, mNewPointsSeen); - } - } + d_cam.Activate(s_cam); - if (!menuOpenSimulator) + mpMapDrawer->DrawKeyFrames(true, true); + + mpMapDrawer->DrawMapPoints(); + + if (mpFrameDrawer != nullptr) { - if (mpFrameDrawer != nullptr) - { - view1.Activate(); + view1.Activate(); - mpFrameDrawer->DrawFrame(); - - } + mpFrameDrawer->DrawFrame(); } - + pangolin::FinishFrame(); - - if (menuMoveLeft) - { - mPointsSeen.insert(mPointsSeen.end(), mNewPointsSeen.begin(), mNewPointsSeen.end()); - - mCurrentPosition.x -= mMovingScale; - - mNewPointsSeen = Auxiliary::getPointsFromPos(mCloudPoints, mCurrentPosition, mCurrentYaw, mCurrentPitch, mCurrentRoll, mTwc); - std::vector::iterator it; - for (it = mNewPointsSeen.begin(); it != mNewPointsSeen.end();) - { - if (std::find(mPointsSeen.begin(), mPointsSeen.end(), *it) != mPointsSeen.end()) - { - it = mNewPointsSeen.erase(it); - } - else - { - ++it; - } - } - menuMoveLeft = false; - } - - if (menuMoveRight) - { - mPointsSeen.insert(mPointsSeen.end(), mNewPointsSeen.begin(), mNewPointsSeen.end()); - - mCurrentPosition.x += mMovingScale; - - mNewPointsSeen = Auxiliary::getPointsFromPos(mCloudPoints, mCurrentPosition, mCurrentYaw, mCurrentPitch, mCurrentRoll, mTwc); - std::vector::iterator it; - for (it = mNewPointsSeen.begin(); it != mNewPointsSeen.end();) - { - if (std::find(mPointsSeen.begin(), mPointsSeen.end(), *it) != mPointsSeen.end()) - { - it = mNewPointsSeen.erase(it); - } - else - { - ++it; - } - } - menuMoveRight = false; - } - - if (menuMoveDown) - { - mPointsSeen.insert(mPointsSeen.end(), mNewPointsSeen.begin(), mNewPointsSeen.end()); - - mCurrentPosition.y -= mMovingScale; - - mNewPointsSeen = Auxiliary::getPointsFromPos(mCloudPoints, mCurrentPosition, mCurrentYaw, mCurrentPitch, mCurrentRoll, mTwc); - std::vector::iterator it; - for (it = mNewPointsSeen.begin(); it != mNewPointsSeen.end();) - { - if (std::find(mPointsSeen.begin(), mPointsSeen.end(), *it) != mPointsSeen.end()) - { - it = mNewPointsSeen.erase(it); - } - else - { - ++it; - } - } - menuMoveDown = false; - } - - if (menuMoveUp) - { - mPointsSeen.insert(mPointsSeen.end(), mNewPointsSeen.begin(), mNewPointsSeen.end()); - - mCurrentPosition.y += mMovingScale; - - mNewPointsSeen = Auxiliary::getPointsFromPos(mCloudPoints, mCurrentPosition, mCurrentYaw, mCurrentPitch, mCurrentRoll, mTwc); - std::vector::iterator it; - for (it = mNewPointsSeen.begin(); it != mNewPointsSeen.end();) - { - if (std::find(mPointsSeen.begin(), mPointsSeen.end(), *it) != mPointsSeen.end()) - { - it = mNewPointsSeen.erase(it); - } - else - { - ++it; - } - } - menuMoveUp = false; - } - - if (menuRotateLeft) - { - mPointsSeen.insert(mPointsSeen.end(), mNewPointsSeen.begin(), mNewPointsSeen.end()); - - mCurrentYaw -= mRotateScale; - - mNewPointsSeen = Auxiliary::getPointsFromPos(mCloudPoints, mCurrentPosition, mCurrentYaw, mCurrentPitch, mCurrentRoll, mTwc); - std::cout << "Current Pos: " << mCurrentPosition << ", yaw: " << mCurrentYaw << ", pitch: " << mCurrentPitch << ", roll: " << mCurrentRoll << std::endl; - std::vector::iterator it; - for (it = mNewPointsSeen.begin(); it != mNewPointsSeen.end();) - { - if (std::find(mPointsSeen.begin(), mPointsSeen.end(), *it) != mPointsSeen.end()) - { - it = mNewPointsSeen.erase(it); - } - else - { - ++it; - } - } - menuRotateLeft = false; - } - - if (menuRotateRight) - { - mPointsSeen.insert(mPointsSeen.end(), mNewPointsSeen.begin(), mNewPointsSeen.end()); - - mCurrentYaw += mRotateScale; - - mNewPointsSeen = Auxiliary::getPointsFromPos(mCloudPoints, mCurrentPosition, mCurrentYaw, mCurrentPitch, mCurrentRoll, mTwc); - std::vector::iterator it; - for (it = mNewPointsSeen.begin(); it != mNewPointsSeen.end();) - { - if (std::find(mPointsSeen.begin(), mPointsSeen.end(), *it) != mPointsSeen.end()) - { - it = mNewPointsSeen.erase(it); - } - else - { - ++it; - } - } - menuRotateRight = false; - } - - if (menuRotateDown) - { - mPointsSeen.insert(mPointsSeen.end(), mNewPointsSeen.begin(), mNewPointsSeen.end()); - - mCurrentPitch -= mRotateScale; - - mNewPointsSeen = Auxiliary::getPointsFromPos(mCloudPoints, mCurrentPosition, mCurrentYaw, mCurrentPitch, mCurrentRoll, mTwc); - std::vector::iterator it; - for (it = mNewPointsSeen.begin(); it != mNewPointsSeen.end();) - { - if (std::find(mPointsSeen.begin(), mPointsSeen.end(), *it) != mPointsSeen.end()) - { - it = mNewPointsSeen.erase(it); - } - else - { - ++it; - } - } - menuRotateDown = false; - } - - if (menuRotateUp) - { - mPointsSeen.insert(mPointsSeen.end(), mNewPointsSeen.begin(), mNewPointsSeen.end()); - - mCurrentPitch += mRotateScale; - - mNewPointsSeen = Auxiliary::getPointsFromPos(mCloudPoints, mCurrentPosition, mCurrentYaw, mCurrentPitch, mCurrentRoll, mTwc); - std::vector::iterator it; - for (it = mNewPointsSeen.begin(); it != mNewPointsSeen.end();) - { - if (std::find(mPointsSeen.begin(), mPointsSeen.end(), *it) != mPointsSeen.end()) - { - it = mNewPointsSeen.erase(it); - } - else - { - ++it; - } - } - menuRotateUp = false; - } - - if (menuReset) - { - menuShowGraph = true; - menuShowKeyFrames = true; - menuShowPoints = true; - menuLocalizationMode = false; - menuOpenSimulator = false; - if (bLocalizationMode) - mpSystem->DeactivateLocalizationMode(); - bLocalizationMode = false; - bFollow = true; - menuFollowCamera = true; - mpSystem->Reset(); - menuReset = false; - - mCurrentPosition = cv::Point3d(0, 0, 0); - mCurrentYaw = 0; - mCurrentPitch = 0; - mCurrentRoll = 0; - - mNewPointsSeen = Auxiliary::getPointsFromPos(mCloudPoints, mCurrentPosition, mCurrentYaw, mCurrentPitch, mCurrentRoll, mTwc); - mPointsSeen = std::vector(); - } - - if (menuShutDown) - { - mpSystem->shutdown_requested = true; - } - if (Stop()) { while (isStopped()) @@ -506,4 +235,4 @@ namespace ORB_SLAM2 mbStopped = false; } -} +} \ No newline at end of file