From aa0f95439ba40545322a00b9307ad9efb29d7c6f Mon Sep 17 00:00:00 2001 From: WanCai-ac <837867565@qq.com> Date: Mon, 11 Nov 2024 12:54:29 +0800 Subject: [PATCH 1/2] Modified obtaining feature points outside the mask --- Examples/Monocular-Inertial/gopro_slam.cc | 15 +-- include/Frame.h | 2 + include/System.h | 2 + include/Tracking.h | 1 + src/Frame.cc | 111 ++++++++++++++++++++++ src/System.cc | 80 ++++++++++++++++ src/Tracking.cc | 50 ++++++++++ 7 files changed, 254 insertions(+), 7 deletions(-) diff --git a/Examples/Monocular-Inertial/gopro_slam.cc b/Examples/Monocular-Inertial/gopro_slam.cc index f9a0f767d11..3bd5793abc2 100644 --- a/Examples/Monocular-Inertial/gopro_slam.cc +++ b/Examples/Monocular-Inertial/gopro_slam.cc @@ -124,7 +124,7 @@ int main(int argc, char **argv) { std::string save_map; app.add_option("--save_map", save_map); - bool enable_gui = false; + bool enable_gui = true; app.add_flag("-g,--enable_gui", enable_gui); int num_threads = 4; @@ -216,7 +216,8 @@ int main(int argc, char **argv) { bool success = cap.read(im); if (!success) { cout << "cap.read failed!" << endl; - break; + //break; + continue; } // resize image and draw gripper mask @@ -226,9 +227,9 @@ int main(int argc, char **argv) { } // apply mask image if loaded - if (!mask_img.empty()) { - im_track.setTo(cv::Scalar(0,0,0), mask_img); - } + // if (!mask_img.empty()) { + // im_track.setTo(cv::Scalar(0,0,0), mask_img); + // } // gather imu measurements between frames // Load imu measurements from previous frame @@ -245,8 +246,8 @@ int main(int argc, char **argv) { std::chrono::steady_clock::now(); // Pass the image to the SLAM system - auto result = SLAM.LocalizeMonocular(im_track, tframe, vImuMeas); - + // auto result = SLAM.LocalizeMonocular(im_track, tframe, vImuMeas); + auto result = SLAM.LocalizeMonocular(im_track, mask_img, tframe, vImuMeas); // check lost frames if (! result.second){ n_lost_frames += 1; diff --git a/include/Frame.h b/include/Frame.h index 880d553bbfe..5dafad65bbb 100644 --- a/include/Frame.h +++ b/include/Frame.h @@ -68,6 +68,8 @@ class Frame // Constructor for Monocular cameras. Frame(const cv::Mat &imGray, const double &timeStamp, ORBextractor* extractor,ORBVocabulary* voc, GeometricCamera* pCamera, cv::Mat &distCoef, const float &bf, const float &thDepth, Frame* pPrevF = static_cast(NULL), const IMU::Calib &ImuCalib = IMU::Calib(), cv::Ptr aruco_dict=cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_50)); + Frame(const cv::Mat &imGray,const cv::Mat& mask, const double &timeStamp, ORBextractor* extractor,ORBVocabulary* voc, GeometricCamera* pCamera, cv::Mat &distCoef, const float &bf, const float &thDepth, Frame* pPrevF = static_cast(NULL), const IMU::Calib &ImuCalib = IMU::Calib(), cv::Ptr aruco_dict=cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_50)); + // Destructor // ~Frame(); diff --git a/include/System.h b/include/System.h index 58c245dffb5..2ebb8945cdb 100644 --- a/include/System.h +++ b/include/System.h @@ -132,6 +132,8 @@ class System // Returns the camera pose (empty if tracking fails). std::pair LocalizeMonocular(const cv::Mat &im, const double ×tamp, const vector& vImuMeas = vector(), string filename=""); + std::pair LocalizeMonocular(const cv::Mat &im,const cv::Mat &mask, const double ×tamp, const vector& vImuMeas= vector(), string filename=""); + // This stops local mapping thread (map building) and performs only camera tracking. void ActivateLocalizationMode(); // This resumes local mapping thread and performs SLAM again. diff --git a/include/Tracking.h b/include/Tracking.h index 6a7ce8fec3c..c85ecfdfb53 100644 --- a/include/Tracking.h +++ b/include/Tracking.h @@ -75,6 +75,7 @@ class Tracking Sophus::SE3f GrabImageStereo(const cv::Mat &imRectLeft,const cv::Mat &imRectRight, const double ×tamp, string filename); Sophus::SE3f GrabImageRGBD(const cv::Mat &imRGB,const cv::Mat &imD, const double ×tamp, string filename); Sophus::SE3f GrabImageMonocular(const cv::Mat &im, const double ×tamp, string filename); + Sophus::SE3f GrabImageMonocular(const cv::Mat &im, const cv::Mat &mask, const double ×tamp, string filename); void GrabImuData(const IMU::Point &imuMeasurement); diff --git a/src/Frame.cc b/src/Frame.cc index 393054a0539..1f544fda641 100644 --- a/src/Frame.cc +++ b/src/Frame.cc @@ -342,6 +342,117 @@ Frame::Frame(const cv::Mat &imGray, const double &timeStamp, ORBextractor* extra } +Frame::Frame(const cv::Mat &imGray,const cv::Mat& mask, const double &timeStamp, ORBextractor* extractor,ORBVocabulary* voc, GeometricCamera* pCamera, cv::Mat &distCoef, const float &bf, const float &thDepth, Frame* pPrevF, const IMU::Calib &ImuCalib, cv::Ptr aruco_dict) + :mpcpi(NULL),mpORBvocabulary(voc),mpORBextractorLeft(extractor),mpORBextractorRight(static_cast(NULL)), + mTimeStamp(timeStamp), mK(static_cast(pCamera)->toK()), mK_(static_cast(pCamera)->toK_()), mDistCoef(distCoef.clone()), mbf(bf), mThDepth(thDepth), + mImuCalib(ImuCalib), mpImuPreintegrated(NULL),mpPrevFrame(pPrevF),mpImuPreintegratedFrame(NULL), mpReferenceKF(static_cast(NULL)), mbIsSet(false), mbImuPreintegrated(false), mpCamera(pCamera), + mpCamera2(nullptr), mbHasPose(false), mbHasVelocity(false) +{ + // Frame ID + mnId=nNextId++; + + // Scale Level Info + mnScaleLevels = mpORBextractorLeft->GetLevels(); + mfScaleFactor = mpORBextractorLeft->GetScaleFactor(); + mfLogScaleFactor = log(mfScaleFactor); + mvScaleFactors = mpORBextractorLeft->GetScaleFactors(); + mvInvScaleFactors = mpORBextractorLeft->GetInverseScaleFactors(); + mvLevelSigma2 = mpORBextractorLeft->GetScaleSigmaSquares(); + mvInvLevelSigma2 = mpORBextractorLeft->GetInverseScaleSigmaSquares(); + + // ArUcO detection + cv::Ptr parameters = cv::aruco::DetectorParameters::create(); + cv::aruco::detectMarkers(imGray, aruco_dict, markerCorners, markerIds, parameters, rejectedCandidates); + + // ORB extraction + ExtractORB(0,imGray,0,1000); + + N = mvKeys.size(); + if(mvKeys.empty()) + return; + + std::vector _mvKeys; + cv::Mat _mDescriptors; + + for (size_t i(0); i < mvKeys.size(); ++i) + { + if((int)mask.at(mvKeys[i].pt.y,mvKeys[i].pt.x)!=255) + { + _mvKeys.push_back(mvKeys[i]); + _mDescriptors.push_back(mDescriptors.row(i)); + } + + } + + mvKeys = _mvKeys; + mDescriptors = _mDescriptors; + + N = mvKeys.size(); + + if(mvKeys.empty()) + return; + + UndistortKeyPoints(); + + // Set no stereo information + mvuRight = vector(N,-1); + mvDepth = vector(N,-1); + mnCloseMPs = 0; + + mvpMapPoints = vector(N,static_cast(NULL)); + + mmProjectPoints.clear();// = map(N, static_cast(NULL)); + mmMatchedInImage.clear(); + + mvbOutlier = vector(N,false); + + // This is done only for the first Frame (or after a change in the calibration) + if(mbInitialComputations) + { + ComputeImageBounds(imGray); + + mfGridElementWidthInv=static_cast(FRAME_GRID_COLS)/static_cast(mnMaxX-mnMinX); + mfGridElementHeightInv=static_cast(FRAME_GRID_ROWS)/static_cast(mnMaxY-mnMinY); + + fx = static_cast(mpCamera)->toK().at(0,0); + fy = static_cast(mpCamera)->toK().at(1,1); + cx = static_cast(mpCamera)->toK().at(0,2); + cy = static_cast(mpCamera)->toK().at(1,2); + invfx = 1.0f/fx; + invfy = 1.0f/fy; + + mbInitialComputations=false; + } + + + mb = mbf/fx; + + //Set no stereo fisheye information + Nleft = -1; + Nright = -1; + mvLeftToRightMatch = vector(0); + mvRightToLeftMatch = vector(0); + mvStereo3Dpoints = vector(0); + monoLeft = -1; + monoRight = -1; + + AssignFeaturesToGrid(); + + if(pPrevF) + { + if(pPrevF->HasVelocity()) + { + SetVelocity(pPrevF->GetVelocity()); + } + } + else + { + mVw.setZero(); + } + + mpMutexImu = new std::mutex(); +} + void Frame::AssignFeaturesToGrid() { // Fill matrix with points diff --git a/src/System.cc b/src/System.cc index 15c28e26368..0785e2de565 100644 --- a/src/System.cc +++ b/src/System.cc @@ -518,6 +518,86 @@ std::pair System::LocalizeMonocular(const cv::Mat &im, const return std::make_pair(Tcw, has_tracking); } +std::pair System::LocalizeMonocular(const cv::Mat &im,const cv::Mat &mask, const double ×tamp, const vector& vImuMeas, string filename) +{ + + { + unique_lock lock(mMutexReset); + if(mbShutDown) + return std::make_pair(Sophus::SE3f(), false); + } + + if(mSensor!=MONOCULAR && mSensor!=IMU_MONOCULAR) + { + cerr << "ERROR: you called TrackMonocular but input sensor was not set to Monocular nor Monocular-Inertial." << endl; + exit(-1); + } + + cv::Mat imToFeed = im.clone(); + if(settings_ && settings_->needToResize()){ + cv::Mat resizedIm; + cv::resize(im,resizedIm,settings_->newImSize()); + imToFeed = resizedIm; + } + + // Check mode change + { + unique_lock lock(mMutexMode); + if(mbActivateLocalizationMode) + { + mpLocalMapper->RequestStop(); + + // Wait until Local Mapping has effectively stopped + while(!mpLocalMapper->isStopped()) + { + usleep(1000); + } + + mpTracker->InformOnlyTracking(true); + mbActivateLocalizationMode = false; + } + if(mbDeactivateLocalizationMode) + { + mpTracker->InformOnlyTracking(false); + mpLocalMapper->Release(); + mbDeactivateLocalizationMode = false; + } + } + + // Check reset + { + unique_lock lock(mMutexReset); + if(mbReset) + { + mpTracker->Reset(); + mbReset = false; + mbResetActiveMap = false; + } + else if(mbResetActiveMap) + { + cout << "SYSTEM-> Reseting active map in monocular case" << endl; + mpTracker->ResetActiveMap(); + mbResetActiveMap = false; + } + } + + if (mSensor == System::IMU_MONOCULAR) + for(size_t i_imu = 0; i_imu < vImuMeas.size(); i_imu++) + mpTracker->GrabImuData(vImuMeas[i_imu]); + + Sophus::SE3f Tcw = mpTracker->GrabImageMonocular(imToFeed,mask,timestamp,filename); + int trackingState = mpTracker->mState; + // has tracking for OK and RECENTLY_LOST state. + bool has_tracking = (trackingState == 2) || (trackingState == 3); + // bool has_tracking = trackingState == 2; + + unique_lock lock2(mMutexState); + mTrackingState = mpTracker->mState; + mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints; + mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn; + + return std::make_pair(Tcw, has_tracking); +} void System::ActivateLocalizationMode() { diff --git a/src/Tracking.cc b/src/Tracking.cc index 2c2b4b7851e..521aae2f388 100644 --- a/src/Tracking.cc +++ b/src/Tracking.cc @@ -1202,6 +1202,56 @@ Sophus::SE3f Tracking::GrabImageMonocular(const cv::Mat &im, const double × return mCurrentFrame.GetPose(); } +Sophus::SE3f Tracking::GrabImageMonocular(const cv::Mat &im,const cv::Mat &mask, const double ×tamp, string filename) +{ + mImGray = im; + if(mImGray.channels()==3) + { + if(mbRGB) + cvtColor(mImGray,mImGray,cv::COLOR_RGB2GRAY); + else + cvtColor(mImGray,mImGray,cv::COLOR_BGR2GRAY); + } + else if(mImGray.channels()==4) + { + if(mbRGB) + cvtColor(mImGray,mImGray,cv::COLOR_RGBA2GRAY); + else + cvtColor(mImGray,mImGray,cv::COLOR_BGRA2GRAY); + } + + if (mSensor == System::MONOCULAR) + { + if(mState==NOT_INITIALIZED || mState==NO_IMAGES_YET ||(lastID - initID) < mMaxFrames) + mCurrentFrame = Frame(mImGray,timestamp,mpIniORBextractor,mpORBVocabulary,mpCamera,mDistCoef,mbf,mThDepth); + else + mCurrentFrame = Frame(mImGray,timestamp,mpORBextractorLeft,mpORBVocabulary,mpCamera,mDistCoef,mbf,mThDepth); + } + else if(mSensor == System::IMU_MONOCULAR) + { + // std::cout<< "mState: " << mState << std::endl; + if(mState==NOT_INITIALIZED || mState==NO_IMAGES_YET) + { + mCurrentFrame = Frame(mImGray,mask,timestamp,mpIniORBextractor,mpORBVocabulary,mpCamera,mDistCoef,mbf,mThDepth,&mLastFrame,*mpImuCalib, maruco_dict); + } + else{ + mCurrentFrame = Frame(mImGray,mask,timestamp,mpORBextractorLeft,mpORBVocabulary,mpCamera,mDistCoef,mbf,mThDepth,&mLastFrame,*mpImuCalib, maruco_dict); + } + } + + + if (mState==NO_IMAGES_YET) + t0=timestamp; + + mCurrentFrame.mNameFile = filename; + mCurrentFrame.mnDataset = mnNumDataset; + + lastID = mCurrentFrame.mnId; + + Track(); + + return mCurrentFrame.GetPose(); +} void Tracking::GrabImuData(const IMU::Point &imuMeasurement) { From 0c60ab4e4a481c61fab07a6db756ee0c937ee3c3 Mon Sep 17 00:00:00 2001 From: WanCai-ac <837867565@qq.com> Date: Mon, 11 Nov 2024 13:32:07 +0800 Subject: [PATCH 2/2] Modified obtaining feature points outside the mask --- Examples/Monocular-Inertial/gopro_slam.cc | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/Examples/Monocular-Inertial/gopro_slam.cc b/Examples/Monocular-Inertial/gopro_slam.cc index 3bd5793abc2..9f3c018ea97 100644 --- a/Examples/Monocular-Inertial/gopro_slam.cc +++ b/Examples/Monocular-Inertial/gopro_slam.cc @@ -124,7 +124,7 @@ int main(int argc, char **argv) { std::string save_map; app.add_option("--save_map", save_map); - bool enable_gui = true; + bool enable_gui = false; app.add_flag("-g,--enable_gui", enable_gui); int num_threads = 4; @@ -216,8 +216,7 @@ int main(int argc, char **argv) { bool success = cap.read(im); if (!success) { cout << "cap.read failed!" << endl; - //break; - continue; + break; } // resize image and draw gripper mask