From e432594c9061305184c8c2163a450c1821febc5f Mon Sep 17 00:00:00 2001 From: DavidPetkovsek Date: Wed, 29 Jun 2022 14:14:10 -0400 Subject: [PATCH 1/5] basic attempt --- include/ORBextractor.h | 2 +- src/ORBextractor.cc | 43 +++++++++++++++++++++++++++++++++++------- src/System.cc | 33 ++++++++++++++++++++++++++------ 3 files changed, 64 insertions(+), 14 deletions(-) diff --git a/include/ORBextractor.h b/include/ORBextractor.h index aa08b9b0..e2c16d6b 100644 --- a/include/ORBextractor.h +++ b/include/ORBextractor.h @@ -53,7 +53,7 @@ class ORBextractor { // Compute the ORB features and descriptors on an image. // ORB are dispersed on the image using an octree. // Mask is ignored in the current implementation. - int operator()(cv::InputArray _image, cv::InputArray _mask, + int operator()(cv::InputArray _image, cv::InputArray * _mask, std::vector &_keypoints, cv::OutputArray _descriptors, std::vector &vLappingArea); diff --git a/src/ORBextractor.cc b/src/ORBextractor.cc index 340a7afa..aa3d8c65 100644 --- a/src/ORBextractor.cc +++ b/src/ORBextractor.cc @@ -1003,7 +1003,7 @@ static void computeDescriptors(const Mat& image, vector& keypoints, descriptors.ptr((int)i)); } -int ORBextractor::operator()(InputArray _image, InputArray _mask, +int ORBextractor::operator()(InputArray _image, InputArray *_mask, vector& _keypoints, OutputArray _descriptors, std::vector& vLappingArea) { @@ -1013,6 +1013,13 @@ int ORBextractor::operator()(InputArray _image, InputArray _mask, Mat image = _image.getMat(); assert(image.type() == CV_8UC1); + Mat mask; + if (_mask != nullptr){ + mask = _mask->getMat(); + assert(image.size() == mask.size()); + assert(mask.type() == CV_8UC1 ); + } + // Pre-compute the scale pyramid ComputePyramid(image); @@ -1038,7 +1045,7 @@ int ORBextractor::operator()(InputArray _image, InputArray _mask, int offset = 0; // Modified for speeding up stereo fisheye matching - int monoIndex = 0, stereoIndex = nkeypoints - 1; + int monoIndex = 0, stereoIndexFromBack = 1; for (int level = 0; level < nlevels; ++level) { vector& keypoints = allKeypoints[level]; int nkeypointsLevel = (int)keypoints.size(); @@ -1056,8 +1063,30 @@ int ORBextractor::operator()(InputArray _image, InputArray _mask, offset += nkeypointsLevel; - float scale = - mvScaleFactor[level]; // getScale(level, firstLevel, scaleFactor); + // filter keypoints based on mask + + // segfault because keypoints are outside of the image.... but why >:( + float scale = mvScaleFactor[level]; + + if (_mask != nullptr){ + vector newPoints; + std::copy_if(keypoints.begin(), keypoints.end(), std::back_inserter(newPoints), [&mask, &scale, &nkeypoints, &level](cv::KeyPoint p){ + KeyPoint newPoint = p; + newPoint.pt *= scale; + + bool ret = mask.at(newPoint.pt.y,newPoint.pt.x) == 255; + //std::cout << ret << std::endl; + --nkeypoints; + return ret; + }); + + //std::cout << "old size " << keypoints.size() << " new size " << newPoints.size() << std::endl; + keypoints = newPoints; + } + + // end filter + + //float scale = mvScaleFactor[level]; //getScale(level, firstLevel, scaleFactor); int i = 0; for (vector::iterator keypoint = keypoints.begin(), keypointEnd = keypoints.end(); @@ -1069,9 +1098,9 @@ int ORBextractor::operator()(InputArray _image, InputArray _mask, if (keypoint->pt.x >= vLappingArea[0] && keypoint->pt.x <= vLappingArea[1]) { - _keypoints.at(stereoIndex) = (*keypoint); - desc.row(i).copyTo(descriptors.row(stereoIndex)); - stereoIndex--; + _keypoints.at(nkeypoints - stereoIndexFromBack) = (*keypoint); + desc.row(i).copyTo(descriptors.row(nkeypoints - stereoIndexFromBack)); + ++stereoIndexFromBack; } else { _keypoints.at(monoIndex) = (*keypoint); desc.row(i).copyTo(descriptors.row(monoIndex)); diff --git a/src/System.cc b/src/System.cc index 10e32306..02c04810 100644 --- a/src/System.cc +++ b/src/System.cc @@ -258,7 +258,7 @@ System::System(const string& strVocFile, const string& strSettingsFile, } Sophus::SE3f System::TrackStereo(const cv::Mat& imLeft, const cv::Mat& imRight, - const double& timestamp, + const cv::Mat *mask, const double& timestamp, const vector& vImuMeas, string filename) { if (mSensor != STEREO && mSensor != IMU_STEREO) { @@ -269,6 +269,10 @@ Sophus::SE3f System::TrackStereo(const cv::Mat& imLeft, const cv::Mat& imRight, } cv::Mat imLeftToFeed, imRightToFeed; + cv::Mat maskToFeed; + if (mask != nullptr){ + maskToFeed = mask->clone(); + } if (settings_ && settings_->needToRectify()) { cv::Mat M1l = settings_->M1l(); cv::Mat M2l = settings_->M2l(); @@ -280,6 +284,9 @@ Sophus::SE3f System::TrackStereo(const cv::Mat& imLeft, const cv::Mat& imRight, } else if (settings_ && settings_->needToResize()) { cv::resize(imLeft, imLeftToFeed, settings_->newImSize()); cv::resize(imRight, imRightToFeed, settings_->newImSize()); + if (mask != nullptr){ + cv::resize(*mask,maskToFeed,settings_->newImSize()); + } } else { imLeftToFeed = imLeft.clone(); imRightToFeed = imRight.clone(); @@ -324,7 +331,7 @@ Sophus::SE3f System::TrackStereo(const cv::Mat& imLeft, const cv::Mat& imRight, mpTracker->GrabImuData(vImuMeas[i_imu]); // std::cout << "start GrabImageStereo" << std::endl; - Sophus::SE3f Tcw = mpTracker->GrabImageStereo(imLeftToFeed, imRightToFeed, + Sophus::SE3f Tcw = mpTracker->GrabImageStereo(imLeftToFeed, imRightToFeed, mask == nullptr ? nullptr : &maskToFeed, timestamp, filename); // std::cout << "out grabber" << std::endl; @@ -338,7 +345,7 @@ Sophus::SE3f System::TrackStereo(const cv::Mat& imLeft, const cv::Mat& imRight, } Sophus::SE3f System::TrackRGBD(const cv::Mat& im, const cv::Mat& depthmap, - const double& timestamp, + const cv::Mat *mask, const double& timestamp, const vector& vImuMeas, string filename) { if (mSensor != RGBD && mSensor != IMU_RGBD) { @@ -349,12 +356,18 @@ Sophus::SE3f System::TrackRGBD(const cv::Mat& im, const cv::Mat& depthmap, cv::Mat imToFeed = im.clone(); cv::Mat imDepthToFeed = depthmap.clone(); + cv::Mat maskToFeed; + if (mask != nullptr){ + maskToFeed = mask->clone(); + } if (settings_ && settings_->needToResize()) { cv::Mat resizedIm; cv::resize(im, resizedIm, settings_->newImSize()); imToFeed = resizedIm; cv::resize(depthmap, imDepthToFeed, settings_->newImSize()); + if (mask != nullptr) + cv::resize(*mask, maskToFeed, settings_->newImSize()); } // Check mode change @@ -396,7 +409,7 @@ Sophus::SE3f System::TrackRGBD(const cv::Mat& im, const cv::Mat& depthmap, mpTracker->GrabImuData(vImuMeas[i_imu]); Sophus::SE3f Tcw = - mpTracker->GrabImageRGBD(imToFeed, imDepthToFeed, timestamp, filename); + mpTracker->GrabImageRGBD(imToFeed, imDepthToFeed, mask == nullptr ? nullptr : &maskToFeed, timestamp, filename); unique_lock lock2(mMutexState); mTrackingState = mpTracker->mState; @@ -405,7 +418,7 @@ Sophus::SE3f System::TrackRGBD(const cv::Mat& im, const cv::Mat& depthmap, return Tcw; } -Sophus::SE3f System::TrackMonocular(const cv::Mat& im, const double& timestamp, +Sophus::SE3f System::TrackMonocular(const cv::Mat& im, const cv::Mat *mask, const double& timestamp, const vector& vImuMeas, string filename) { { @@ -421,10 +434,18 @@ Sophus::SE3f System::TrackMonocular(const cv::Mat& im, const double& timestamp, } cv::Mat imToFeed = im.clone(); + cv::Mat maskToFeed; + if (mask != nullptr) { + maskToFeed = mask->clone(); + } if (settings_ && settings_->needToResize()) { cv::Mat resizedIm; cv::resize(im, resizedIm, settings_->newImSize()); imToFeed = resizedIm; + + if (mask != nullptr) { + cv::resize(*mask, maskToFeed, settings_->newImSize()); + } } // Check mode change @@ -467,7 +488,7 @@ Sophus::SE3f System::TrackMonocular(const cv::Mat& im, const double& timestamp, mpTracker->GrabImuData(vImuMeas[i_imu]); Sophus::SE3f Tcw = - mpTracker->GrabImageMonocular(imToFeed, timestamp, filename); + mpTracker->GrabImageMonocular(imToFeed, mask == nullptr ? nullptr : &maskToFeed, timestamp, filename); unique_lock lock2(mMutexState); mTrackingState = mpTracker->mState; From d0b1b4fa63f65ea805e1e5fba8d2833fcd5bc05b Mon Sep 17 00:00:00 2001 From: DavidPetkovsek Date: Mon, 4 Jul 2022 17:00:12 -0400 Subject: [PATCH 2/5] WARNING: WILL BREAK EXAMPLES move mask out to top level System call --- include/Frame.h | 10 +++++----- include/System.h | 6 +++--- include/Tracking.h | 6 ++++-- src/Frame.cc | 28 +++++++++++++++------------- src/Tracking.cc | 26 ++++++++++++++------------ 5 files changed, 41 insertions(+), 35 deletions(-) diff --git a/include/Frame.h b/include/Frame.h index d143a74e..8ea275c6 100644 --- a/include/Frame.h +++ b/include/Frame.h @@ -59,19 +59,19 @@ class Frame Frame(const Frame &frame); // Constructor for stereo cameras. - Frame(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeStamp, ORBextractor* extractorLeft, ORBextractor* extractorRight, ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, GeometricCamera* pCamera,Frame* pPrevF = static_cast(NULL), const IMU::Calib &ImuCalib = IMU::Calib()); + Frame(const cv::Mat &imLeft, const cv::Mat &imRight, const cv::Mat *mask, const double &timeStamp, ORBextractor* extractorLeft, ORBextractor* extractorRight, ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, GeometricCamera* pCamera,Frame* pPrevF = static_cast(NULL), const IMU::Calib &ImuCalib = IMU::Calib()); // Constructor for RGB-D cameras. - Frame(const cv::Mat &imGray, const cv::Mat &imDepth, const double &timeStamp, ORBextractor* extractor,ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, GeometricCamera* pCamera,Frame* pPrevF = static_cast(NULL), const IMU::Calib &ImuCalib = IMU::Calib()); + Frame(const cv::Mat &imGray, const cv::Mat &imDepth, const cv::Mat *mask, const double &timeStamp, ORBextractor* extractor,ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, GeometricCamera* pCamera,Frame* pPrevF = static_cast(NULL), const IMU::Calib &ImuCalib = IMU::Calib()); // 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()); + 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()); // Destructor // ~Frame(); // Extract ORB on the image. 0 for left image and 1 for right image. - void ExtractORB(int flag, const cv::Mat &im, const int x0, const int x1); + void ExtractORB(int flag, const cv::Mat &im, const int x0, const int x1, const cv::Mat * mask = nullptr); // Compute Bag of Words representation. void ComputeBoW(); @@ -343,7 +343,7 @@ class Frame //Grid for the right image std::vector mGridRight[FRAME_GRID_COLS][FRAME_GRID_ROWS]; - Frame(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeStamp, ORBextractor* extractorLeft, ORBextractor* extractorRight, ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, GeometricCamera* pCamera, GeometricCamera* pCamera2, Sophus::SE3f& Tlr,Frame* pPrevF = static_cast(NULL), const IMU::Calib &ImuCalib = IMU::Calib()); + Frame(const cv::Mat &imLeft, const cv::Mat &imRight, const cv::Mat *mask, const double &timeStamp, ORBextractor* extractorLeft, ORBextractor* extractorRight, ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, GeometricCamera* pCamera, GeometricCamera* pCamera2, Sophus::SE3f& Tlr,Frame* pPrevF = static_cast(NULL), const IMU::Calib &ImuCalib = IMU::Calib()); //Stereo fisheye void ComputeStereoFishEyeMatches(); diff --git a/include/System.h b/include/System.h index f79863bf..2a8789f5 100644 --- a/include/System.h +++ b/include/System.h @@ -107,18 +107,18 @@ class System // Proccess the given stereo frame. Images must be synchronized and rectified. // Input images: RGB (CV_8UC3) or grayscale (CV_8U). RGB is converted to grayscale. // Returns the camera pose (empty if tracking fails). - Sophus::SE3f TrackStereo(const cv::Mat &imLeft, const cv::Mat &imRight, const double ×tamp, const vector& vImuMeas = vector(), string filename=""); + Sophus::SE3f TrackStereo(const cv::Mat &imLeft, const cv::Mat &imRight, const cv::Mat *mask, const double ×tamp, const vector& vImuMeas = vector(), string filename=""); // Process the given rgbd frame. Depthmap must be registered to the RGB frame. // Input image: RGB (CV_8UC3) or grayscale (CV_8U). RGB is converted to grayscale. // Input depthmap: Float (CV_32F). // Returns the camera pose (empty if tracking fails). - Sophus::SE3f TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const double ×tamp, const vector& vImuMeas = vector(), string filename=""); + Sophus::SE3f TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const cv::Mat *mask, const double ×tamp, const vector& vImuMeas = vector(), string filename=""); // Proccess the given monocular frame and optionally imu data // Input images: RGB (CV_8UC3) or grayscale (CV_8U). RGB is converted to grayscale. // Returns the camera pose (empty if tracking fails). - Sophus::SE3f TrackMonocular(const cv::Mat &im, const double ×tamp, const vector& vImuMeas = vector(), string filename=""); + Sophus::SE3f TrackMonocular(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. diff --git a/include/Tracking.h b/include/Tracking.h index 9b9a0a2c..7ce46120 100644 --- a/include/Tracking.h +++ b/include/Tracking.h @@ -71,11 +71,13 @@ class Tracking { // matching. Sophus::SE3f GrabImageStereo(const cv::Mat& imRectLeft, const cv::Mat& imRectRight, + const cv::Mat *mask, const double& timestamp, string filename); Sophus::SE3f GrabImageRGBD(const cv::Mat& imRGB, const cv::Mat& imD, + const cv::Mat *mask, const double& timestamp, string filename); - Sophus::SE3f GrabImageMonocular(const cv::Mat& im, const double& timestamp, - string filename); + Sophus::SE3f GrabImageMonocular(const cv::Mat& im, const cv::Mat *mask, + const double& timestamp, string filename); void GrabImuData(const IMU::Point& imuMeasurement); diff --git a/src/Frame.cc b/src/Frame.cc index 64750d0a..ee795de0 100644 --- a/src/Frame.cc +++ b/src/Frame.cc @@ -146,7 +146,7 @@ Frame::Frame(const Frame &frame) #endif } -Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight, +Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight, const cv::Mat *mask, const double &timeStamp, ORBextractor *extractorLeft, ORBextractor *extractorRight, ORBVocabulary *voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, @@ -190,8 +190,8 @@ Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight, std::chrono::steady_clock::time_point time_StartExtORB = std::chrono::steady_clock::now(); #endif - thread threadLeft(&Frame::ExtractORB, this, 0, imLeft, 0, 0); - thread threadRight(&Frame::ExtractORB, this, 1, imRight, 0, 0); + thread threadLeft(&Frame::ExtractORB, this, 0, imLeft, 0, 0, mask); + thread threadRight(&Frame::ExtractORB, this, 1, imRight, 0, 0, mask); threadLeft.join(); threadRight.join(); #ifdef REGISTER_TIMES @@ -271,7 +271,7 @@ Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight, AssignFeaturesToGrid(); } -Frame::Frame(const cv::Mat &imGray, const cv::Mat &imDepth, +Frame::Frame(const cv::Mat &imGray, const cv::Mat &imDepth, const cv::Mat *mask, const double &timeStamp, ORBextractor *extractor, ORBVocabulary *voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, GeometricCamera *pCamera, Frame *pPrevF, @@ -314,7 +314,7 @@ Frame::Frame(const cv::Mat &imGray, const cv::Mat &imDepth, std::chrono::steady_clock::time_point time_StartExtORB = std::chrono::steady_clock::now(); #endif - ExtractORB(0, imGray, 0, 0); + ExtractORB(0, imGray, 0, 0, mask); #ifdef REGISTER_TIMES std::chrono::steady_clock::time_point time_EndExtORB = std::chrono::steady_clock::now(); @@ -382,7 +382,7 @@ Frame::Frame(const cv::Mat &imGray, const cv::Mat &imDepth, AssignFeaturesToGrid(); } -Frame::Frame(const cv::Mat &imGray, const double &timeStamp, +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) @@ -424,7 +424,7 @@ Frame::Frame(const cv::Mat &imGray, const double &timeStamp, std::chrono::steady_clock::time_point time_StartExtORB = std::chrono::steady_clock::now(); #endif - ExtractORB(0, imGray, 0, 1000); + ExtractORB(0, imGray, 0, 1000, mask); #ifdef REGISTER_TIMES std::chrono::steady_clock::time_point time_EndExtORB = std::chrono::steady_clock::now(); @@ -527,13 +527,13 @@ void Frame::AssignFeaturesToGrid() { } void Frame::ExtractORB(int flag, const cv::Mat &im, const int x0, - const int x1) { + const int x1, const cv::Mat *mask) { vector vLapping = {x0, x1}; if (flag == 0) monoLeft = - (*mpORBextractorLeft)(im, cv::Mat(), mvKeys, mDescriptors, vLapping); + (*mpORBextractorLeft)(im, cv::Mat(), mask, mvKeys, mDescriptors, vLapping); else - monoRight = (*mpORBextractorRight)(im, cv::Mat(), mvKeysRight, + monoRight = (*mpORBextractorRight)(im, cv::Mat(), mask, mvKeysRight, mDescriptorsRight, vLapping); } @@ -1089,7 +1089,7 @@ void Frame::setIntegrated() { mbImuPreintegrated = true; } -Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight, +Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight, const cv::Mat *mask, const double &timeStamp, ORBextractor *extractorLeft, ORBextractor *extractorRight, ORBVocabulary *voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, @@ -1139,11 +1139,13 @@ Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight, #endif thread threadLeft(&Frame::ExtractORB, this, 0, imLeft, static_cast(mpCamera)->mvLappingArea[0], - static_cast(mpCamera)->mvLappingArea[1]); + static_cast(mpCamera)->mvLappingArea[1], + mask); thread threadRight( &Frame::ExtractORB, this, 1, imRight, static_cast(mpCamera2)->mvLappingArea[0], - static_cast(mpCamera2)->mvLappingArea[1]); + static_cast(mpCamera2)->mvLappingArea[1], + mask); threadLeft.join(); threadRight.join(); #ifdef REGISTER_TIMES diff --git a/src/Tracking.cc b/src/Tracking.cc index 5eefd9ee..51b37e00 100644 --- a/src/Tracking.cc +++ b/src/Tracking.cc @@ -1377,6 +1377,7 @@ bool Tracking::GetStepByStep() { return bStepByStep; } Sophus::SE3f Tracking::GrabImageStereo(const cv::Mat& imRectLeft, const cv::Mat& imRectRight, + const cv::Mat *mask, const double& timestamp, string filename) { // cout << "GrabImageStereo" << endl; @@ -1408,20 +1409,20 @@ Sophus::SE3f Tracking::GrabImageStereo(const cv::Mat& imRectLeft, // cout << "Incoming frame creation" << endl; if (mSensor == System::STEREO && !mpCamera2) - mCurrentFrame = Frame(mImGray, imGrayRight, timestamp, mpORBextractorLeft, + mCurrentFrame = Frame(mImGray, imGrayRight, mask, timestamp, mpORBextractorLeft, mpORBextractorRight, mpORBVocabulary, mK, mDistCoef, mbf, mThDepth, mpCamera); else if (mSensor == System::STEREO && mpCamera2) - mCurrentFrame = Frame(mImGray, imGrayRight, timestamp, mpORBextractorLeft, + mCurrentFrame = Frame(mImGray, imGrayRight, mask, timestamp, mpORBextractorLeft, mpORBextractorRight, mpORBVocabulary, mK, mDistCoef, mbf, mThDepth, mpCamera, mpCamera2, mTlr); else if (mSensor == System::IMU_STEREO && !mpCamera2) - mCurrentFrame = Frame(mImGray, imGrayRight, timestamp, mpORBextractorLeft, + mCurrentFrame = Frame(mImGray, imGrayRight, mask, timestamp, mpORBextractorLeft, mpORBextractorRight, mpORBVocabulary, mK, mDistCoef, mbf, mThDepth, mpCamera, &mLastFrame, *mpImuCalib); else if (mSensor == System::IMU_STEREO && mpCamera2) mCurrentFrame = - Frame(mImGray, imGrayRight, timestamp, mpORBextractorLeft, + Frame(mImGray, imGrayRight, mask, timestamp, mpORBextractorLeft, mpORBextractorRight, mpORBVocabulary, mK, mDistCoef, mbf, mThDepth, mpCamera, mpCamera2, mTlr, &mLastFrame, *mpImuCalib); @@ -1443,7 +1444,8 @@ Sophus::SE3f Tracking::GrabImageStereo(const cv::Mat& imRectLeft, } Sophus::SE3f Tracking::GrabImageRGBD(const cv::Mat& imRGB, const cv::Mat& imD, - const double& timestamp, string filename) { + const cv::Mat *mask, const double& timestamp, + string filename) { mImGray = imRGB; cv::Mat imDepth = imD; @@ -1464,11 +1466,11 @@ Sophus::SE3f Tracking::GrabImageRGBD(const cv::Mat& imRGB, const cv::Mat& imD, if (mSensor == System::RGBD) mCurrentFrame = - Frame(mImGray, imDepth, timestamp, mpORBextractorLeft, mpORBVocabulary, + Frame(mImGray, imDepth, mask, timestamp, mpORBextractorLeft, mpORBVocabulary, mK, mDistCoef, mbf, mThDepth, mpCamera); else if (mSensor == System::IMU_RGBD) mCurrentFrame = - Frame(mImGray, imDepth, timestamp, mpORBextractorLeft, mpORBVocabulary, + Frame(mImGray, imDepth, mask, timestamp, mpORBextractorLeft, mpORBVocabulary, mK, mDistCoef, mbf, mThDepth, mpCamera, &mLastFrame, *mpImuCalib); mCurrentFrame.mNameFile = filename; @@ -1483,7 +1485,7 @@ Sophus::SE3f Tracking::GrabImageRGBD(const cv::Mat& imRGB, const cv::Mat& imD, return mCurrentFrame.GetPose(); } -Sophus::SE3f Tracking::GrabImageMonocular(const cv::Mat& im, +Sophus::SE3f Tracking::GrabImageMonocular(const cv::Mat& im, const cv::Mat *mask, const double& timestamp, string filename) { mImGray = im; @@ -1503,20 +1505,20 @@ Sophus::SE3f Tracking::GrabImageMonocular(const cv::Mat& im, if (mState == NOT_INITIALIZED || mState == NO_IMAGES_YET || (lastID - initID) < mMaxFrames) mCurrentFrame = - Frame(mImGray, timestamp, mpIniORBextractor, mpORBVocabulary, + Frame(mImGray, mask, timestamp, mpIniORBextractor, mpORBVocabulary, mpCamera, mDistCoef, mbf, mThDepth); else mCurrentFrame = - Frame(mImGray, timestamp, mpORBextractorLeft, mpORBVocabulary, + Frame(mImGray, mask, timestamp, mpORBextractorLeft, mpORBVocabulary, mpCamera, mDistCoef, mbf, mThDepth); } else if (mSensor == System::IMU_MONOCULAR) { if (mState == NOT_INITIALIZED || mState == NO_IMAGES_YET) { mCurrentFrame = - Frame(mImGray, timestamp, mpIniORBextractor, mpORBVocabulary, + Frame(mImGray, mask, timestamp, mpIniORBextractor, mpORBVocabulary, mpCamera, mDistCoef, mbf, mThDepth, &mLastFrame, *mpImuCalib); } else mCurrentFrame = - Frame(mImGray, timestamp, mpORBextractorLeft, mpORBVocabulary, + Frame(mImGray, mask, timestamp, mpORBextractorLeft, mpORBVocabulary, mpCamera, mDistCoef, mbf, mThDepth, &mLastFrame, *mpImuCalib); } From b1ce94f0f6959306b70aeb2cb02a7413d9326474 Mon Sep 17 00:00:00 2001 From: Soldann Date: Wed, 13 Jul 2022 17:05:11 -0400 Subject: [PATCH 3/5] fix up some rebase issues --- include/ORBextractor.h | 2 +- src/Frame.cc | 4 ++-- src/ORBextractor.cc | 4 ++-- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/include/ORBextractor.h b/include/ORBextractor.h index e2c16d6b..dacd8f44 100644 --- a/include/ORBextractor.h +++ b/include/ORBextractor.h @@ -53,7 +53,7 @@ class ORBextractor { // Compute the ORB features and descriptors on an image. // ORB are dispersed on the image using an octree. // Mask is ignored in the current implementation. - int operator()(cv::InputArray _image, cv::InputArray * _mask, + int operator()(cv::InputArray _image, const cv::Mat *_mask, std::vector &_keypoints, cv::OutputArray _descriptors, std::vector &vLappingArea); diff --git a/src/Frame.cc b/src/Frame.cc index ee795de0..f50b89f2 100644 --- a/src/Frame.cc +++ b/src/Frame.cc @@ -531,9 +531,9 @@ void Frame::ExtractORB(int flag, const cv::Mat &im, const int x0, vector vLapping = {x0, x1}; if (flag == 0) monoLeft = - (*mpORBextractorLeft)(im, cv::Mat(), mask, mvKeys, mDescriptors, vLapping); + (*mpORBextractorLeft)(im, mask, mvKeys, mDescriptors, vLapping); else - monoRight = (*mpORBextractorRight)(im, cv::Mat(), mask, mvKeysRight, + monoRight = (*mpORBextractorRight)(im, mask, mvKeysRight, mDescriptorsRight, vLapping); } diff --git a/src/ORBextractor.cc b/src/ORBextractor.cc index aa3d8c65..f1d134dd 100644 --- a/src/ORBextractor.cc +++ b/src/ORBextractor.cc @@ -1003,7 +1003,7 @@ static void computeDescriptors(const Mat& image, vector& keypoints, descriptors.ptr((int)i)); } -int ORBextractor::operator()(InputArray _image, InputArray *_mask, +int ORBextractor::operator()(InputArray _image, const cv::Mat *_mask, vector& _keypoints, OutputArray _descriptors, std::vector& vLappingArea) { @@ -1015,7 +1015,7 @@ int ORBextractor::operator()(InputArray _image, InputArray *_mask, Mat mask; if (_mask != nullptr){ - mask = _mask->getMat(); + mask = *_mask; assert(image.size() == mask.size()); assert(mask.type() == CV_8UC1 ); } From e6c490c67fd3a1275ffe6cf139b2ca9aaca35da9 Mon Sep 17 00:00:00 2001 From: Soldann Date: Fri, 15 Jul 2022 04:53:04 -1000 Subject: [PATCH 4/5] added overloads to fix examples --- include/System.h | 3 +++ src/System.cc | 20 ++++++++++++++++++++ 2 files changed, 23 insertions(+) diff --git a/include/System.h b/include/System.h index 2a8789f5..42c1625f 100644 --- a/include/System.h +++ b/include/System.h @@ -108,17 +108,20 @@ class System // Input images: RGB (CV_8UC3) or grayscale (CV_8U). RGB is converted to grayscale. // Returns the camera pose (empty if tracking fails). Sophus::SE3f TrackStereo(const cv::Mat &imLeft, const cv::Mat &imRight, const cv::Mat *mask, const double ×tamp, const vector& vImuMeas = vector(), string filename=""); + Sophus::SE3f TrackStereo(const cv::Mat &imLeft, const cv::Mat &imRight, const double ×tamp, const vector& vImuMeas = vector(), string filename=""); // Process the given rgbd frame. Depthmap must be registered to the RGB frame. // Input image: RGB (CV_8UC3) or grayscale (CV_8U). RGB is converted to grayscale. // Input depthmap: Float (CV_32F). // Returns the camera pose (empty if tracking fails). Sophus::SE3f TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const cv::Mat *mask, const double ×tamp, const vector& vImuMeas = vector(), string filename=""); + Sophus::SE3f TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const double ×tamp, const vector& vImuMeas = vector(), string filename=""); // Proccess the given monocular frame and optionally imu data // Input images: RGB (CV_8UC3) or grayscale (CV_8U). RGB is converted to grayscale. // Returns the camera pose (empty if tracking fails). Sophus::SE3f TrackMonocular(const cv::Mat &im, const cv::Mat *mask, const double ×tamp, const vector& vImuMeas = vector(), string filename=""); + Sophus::SE3f TrackMonocular(const cv::Mat &im, const double ×tamp, const vector& vImuMeas = vector(), string filename=""); // This stops local mapping thread (map building) and performs only camera tracking. diff --git a/src/System.cc b/src/System.cc index 02c04810..fad122ce 100644 --- a/src/System.cc +++ b/src/System.cc @@ -257,6 +257,13 @@ System::System(const string& strVocFile, const string& strSettingsFile, Verbose::SetTh(Verbose::VERBOSITY_QUIET); } +Sophus::SE3f System::TrackStereo(const cv::Mat& imLeft, const cv::Mat& imRight, + const double& timestamp, + const vector& vImuMeas, + string filename) { + return TrackStereo(imLeft, imRight, timestamp, vImuMeas, filename); +} + Sophus::SE3f System::TrackStereo(const cv::Mat& imLeft, const cv::Mat& imRight, const cv::Mat *mask, const double& timestamp, const vector& vImuMeas, @@ -344,6 +351,13 @@ Sophus::SE3f System::TrackStereo(const cv::Mat& imLeft, const cv::Mat& imRight, return Tcw; } +Sophus::SE3f System::TrackRGBD(const cv::Mat& im, const cv::Mat& depthmap, + const double& timestamp, + const vector& vImuMeas, + string filename) { + return TrackRGBD(im, depthmap, nullptr, timestamp, vImuMeas, filename); +} + Sophus::SE3f System::TrackRGBD(const cv::Mat& im, const cv::Mat& depthmap, const cv::Mat *mask, const double& timestamp, const vector& vImuMeas, @@ -418,6 +432,12 @@ Sophus::SE3f System::TrackRGBD(const cv::Mat& im, const cv::Mat& depthmap, return Tcw; } +Sophus::SE3f System::TrackMonocular(const cv::Mat& im, const cv::Mat *mask, const double& timestamp, + const vector& vImuMeas, + string filename) { + return TrackMonocular(im, nullptr, timestamp, vImuMeas, filename); +} + Sophus::SE3f System::TrackMonocular(const cv::Mat& im, const cv::Mat *mask, const double& timestamp, const vector& vImuMeas, string filename) { From 1d51a485b41a1cd2edd5029383682f7f16dd27f6 Mon Sep 17 00:00:00 2001 From: Soldann Date: Fri, 15 Jul 2022 05:25:34 -1000 Subject: [PATCH 5/5] fix overload definition of TrackMonocular --- src/System.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/System.cc b/src/System.cc index fad122ce..c5ca1e2a 100644 --- a/src/System.cc +++ b/src/System.cc @@ -432,7 +432,7 @@ Sophus::SE3f System::TrackRGBD(const cv::Mat& im, const cv::Mat& depthmap, return Tcw; } -Sophus::SE3f System::TrackMonocular(const cv::Mat& im, const cv::Mat *mask, const double& timestamp, +Sophus::SE3f System::TrackMonocular(const cv::Mat& im, const double& timestamp, const vector& vImuMeas, string filename) { return TrackMonocular(im, nullptr, timestamp, vImuMeas, filename);