Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Rebased Feature: Implement keypoint filtering with cv::Mat #65

Open
wants to merge 5 commits into
base: master
Choose a base branch
from
+122 −43
Open

Rebased Feature: Implement keypoint filtering with cv::Mat #65

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
e432594
basic attempt
DavidPetkovsek Jun 29, 2022
d0b1b4f
WARNING: WILL BREAK EXAMPLES
DavidPetkovsek Jul 4, 2022
b1ce94f
fix up some rebase issues
Soldann Jul 13, 2022
e6c490c
added overloads to fix examples
Soldann Jul 15, 2022
1d51a48
fix overload definition of TrackMonocular
Soldann Jul 15, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
10 changes: 5 additions & 5 deletions include/Frame.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<Frame*>(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<Frame*>(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<Frame*>(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<Frame*>(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<Frame*>(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<Frame*>(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();
Expand Down Expand Up @@ -343,7 +343,7 @@ class Frame
//Grid for the right image
std::vector<std::size_t> 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<Frame*>(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<Frame*>(NULL), const IMU::Calib &ImuCalib = IMU::Calib());

//Stereo fisheye
void ComputeStereoFishEyeMatches();
Expand Down
2 changes: 1 addition & 1 deletion include/ORBextractor.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<cv::KeyPoint> &_keypoints,
cv::OutputArray _descriptors, std::vector<int> &vLappingArea);

Expand Down
3 changes: 3 additions & 0 deletions include/System.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -107,17 +107,20 @@ 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 cv::Mat *mask, const double &timestamp, const vector<IMU::Point>& vImuMeas = vector<IMU::Point>(), string filename="");
Sophus::SE3f TrackStereo(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timestamp, const vector<IMU::Point>& vImuMeas = vector<IMU::Point>(), 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 &timestamp, const vector<IMU::Point>& vImuMeas = vector<IMU::Point>(), string filename="");
Sophus::SE3f TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const double &timestamp, const vector<IMU::Point>& vImuMeas = vector<IMU::Point>(), 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 &timestamp, const vector<IMU::Point>& vImuMeas = vector<IMU::Point>(), string filename="");
Sophus::SE3f TrackMonocular(const cv::Mat &im, const double &timestamp, const vector<IMU::Point>& vImuMeas = vector<IMU::Point>(), string filename="");


Expand Down
6 changes: 4 additions & 2 deletions include/Tracking.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);

Expand Down
28 changes: 15 additions & 13 deletions src/Frame.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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,
Expand Down Expand Up @@ -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();
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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();
Expand Down Expand Up @@ -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<int> vLapping = {x0, x1};
if (flag == 0)
monoLeft =
(*mpORBextractorLeft)(im, cv::Mat(), mvKeys, mDescriptors, vLapping);
(*mpORBextractorLeft)(im, mask, mvKeys, mDescriptors, vLapping);
else
monoRight = (*mpORBextractorRight)(im, cv::Mat(), mvKeysRight,
monoRight = (*mpORBextractorRight)(im, mask, mvKeysRight,
mDescriptorsRight, vLapping);
}

Expand Down Expand Up @@ -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,
Expand Down Expand Up @@ -1139,11 +1139,13 @@ Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight,
#endif
thread threadLeft(&Frame::ExtractORB, this, 0, imLeft,
static_cast<KannalaBrandt8 *>(mpCamera)->mvLappingArea[0],
static_cast<KannalaBrandt8 *>(mpCamera)->mvLappingArea[1]);
static_cast<KannalaBrandt8 *>(mpCamera)->mvLappingArea[1],
mask);
thread threadRight(
&Frame::ExtractORB, this, 1, imRight,
static_cast<KannalaBrandt8 *>(mpCamera2)->mvLappingArea[0],
static_cast<KannalaBrandt8 *>(mpCamera2)->mvLappingArea[1]);
static_cast<KannalaBrandt8 *>(mpCamera2)->mvLappingArea[1],
mask);
threadLeft.join();
threadRight.join();
#ifdef REGISTER_TIMES
Expand Down
43 changes: 36 additions & 7 deletions src/ORBextractor.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1003,7 +1003,7 @@ static void computeDescriptors(const Mat& image, vector<KeyPoint>& keypoints,
descriptors.ptr((int)i));
}

int ORBextractor::operator()(InputArray _image, InputArray _mask,
int ORBextractor::operator()(InputArray _image, const cv::Mat *_mask,
vector<KeyPoint>& _keypoints,
OutputArray _descriptors,
std::vector<int>& vLappingArea) {
Expand All @@ -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;
assert(image.size() == mask.size());
assert(mask.type() == CV_8UC1 );
}

// Pre-compute the scale pyramid
ComputePyramid(image);

Expand All @@ -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<KeyPoint>& keypoints = allKeypoints[level];
int nkeypointsLevel = (int)keypoints.size();
Expand All @@ -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<cv::KeyPoint> 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<uint8_t>(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<KeyPoint>::iterator keypoint = keypoints.begin(),
keypointEnd = keypoints.end();
Expand All @@ -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));
Expand Down
47 changes: 44 additions & 3 deletions src/System.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -261,6 +261,13 @@ Sophus::SE3f System::TrackStereo(const cv::Mat& imLeft, const cv::Mat& imRight,
const double& timestamp,
const vector<IMU::Point>& 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<IMU::Point>& vImuMeas,
string filename) {
if (mSensor != STEREO && mSensor != IMU_STEREO) {
cerr << "ERROR: you called TrackStereo but input sensor was not set to "
"Stereo nor Stereo-Inertial."
Expand All @@ -269,6 +276,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();
Expand All @@ -280,6 +291,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();
Expand Down Expand Up @@ -324,7 +338,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;
Expand All @@ -341,6 +355,13 @@ Sophus::SE3f System::TrackRGBD(const cv::Mat& im, const cv::Mat& depthmap,
const double& timestamp,
const vector<IMU::Point>& 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<IMU::Point>& vImuMeas,
string filename) {
if (mSensor != RGBD && mSensor != IMU_RGBD) {
cerr << "ERROR: you called TrackRGBD but input sensor was not set to RGBD."
<< endl;
Expand All @@ -349,12 +370,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
Expand Down Expand Up @@ -396,7 +423,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<mutex> lock2(mMutexState);
mTrackingState = mpTracker->mState;
Expand All @@ -408,6 +435,12 @@ Sophus::SE3f System::TrackRGBD(const cv::Mat& im, const cv::Mat& depthmap,
Sophus::SE3f System::TrackMonocular(const cv::Mat& im, const double& timestamp,
const vector<IMU::Point>& 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<IMU::Point>& vImuMeas,
string filename) {
{
unique_lock<mutex> lock(mMutexReset);
if (mbShutDown) return Sophus::SE3f();
Expand All @@ -421,10 +454,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
Expand Down Expand Up @@ -467,7 +508,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<mutex> lock2(mMutexState);
mTrackingState = mpTracker->mState;
Expand Down
Loading