i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 17:40:23 +08:00
Code Issues Packages Projects Releases Wiki Activity

Add ANMS (SSC method) (#1276)

Browse Source
This commit is contained in:
Borong Yuan
2024-05-20 09:21:00 +08:00
committed by GitHub
parent 0d4e4730c7
commit fbeabf0751
12 changed files with 321 additions and 157 deletions
Download Patch File Download Diff File Expand all files Collapse all files

178
corelib/src/Features2d.cpp
View File

@@ -268,70 +268,111 @@ void Feature2D::filterKeypointsByDisparity(
}
}
void Feature2D::limitKeypoints(std::vector<cv::KeyPoint> & keypoints, int maxKeypoints)
void Feature2D::limitKeypoints(std::vector<cv::KeyPoint> & keypoints, int maxKeypoints, const cv::Size & imageSize, bool ssc)
{
cv::Mat descriptors;
limitKeypoints(keypoints, descriptors, maxKeypoints);
limitKeypoints(keypoints, descriptors, maxKeypoints, imageSize, ssc);
}
void Feature2D::limitKeypoints(std::vector<cv::KeyPoint> & keypoints, cv::Mat & descriptors, int maxKeypoints)
void Feature2D::limitKeypoints(std::vector<cv::KeyPoint> & keypoints, cv::Mat & descriptors, int maxKeypoints, const cv::Size & imageSize, bool ssc)
{
std::vector<cv::Point3f> keypoints3D;
limitKeypoints(keypoints, keypoints3D, descriptors, maxKeypoints);
limitKeypoints(keypoints, keypoints3D, descriptors, maxKeypoints, imageSize, ssc);
}
void Feature2D::limitKeypoints(std::vector<cv::KeyPoint> & keypoints, std::vector<cv::Point3f> & keypoints3D, cv::Mat & descriptors, int maxKeypoints)
void Feature2D::limitKeypoints(std::vector<cv::KeyPoint> & keypoints, std::vector<cv::Point3f> & keypoints3D, cv::Mat & descriptors, int maxKeypoints, const cv::Size & imageSize, bool ssc)
{
UASSERT_MSG((int)keypoints.size() == descriptors.rows || descriptors.rows == 0, uFormat("keypoints=%d descriptors=%d", (int)keypoints.size(), descriptors.rows).c_str());
UASSERT_MSG(keypoints.size() == keypoints3D.size() || keypoints3D.size() == 0, uFormat("keypoints=%d keypoints3D=%d", (int)keypoints.size(), (int)keypoints3D.size()).c_str());
if(maxKeypoints > 0 && (int)keypoints.size() > maxKeypoints)
{
UTimer timer;
ULOGGER_DEBUG("too much words (%d), removing words with the hessian threshold", keypoints.size());
// Remove words under the new hessian threshold
// Sort words by hessian
std::multimap<float, int> hessianMap; // <hessian,id>
for(unsigned int i = 0; i <keypoints.size(); ++i)
{
//Keep track of the data, to be easier to manage the data in the next step
hessianMap.insert(std::pair<float, int>(fabs(keypoints[i].response), i));
}
// Remove them from the signature
int removed = (int)hessianMap.size()-maxKeypoints;
std::multimap<float, int>::reverse_iterator iter = hessianMap.rbegin();
std::vector<cv::KeyPoint> kptsTmp(maxKeypoints);
int removed;
std::vector<cv::KeyPoint> kptsTmp;
std::vector<cv::Point3f> kpts3DTmp;
if(!keypoints3D.empty())
{
kpts3DTmp.resize(maxKeypoints);
}
cv::Mat descriptorsTmp;
if(descriptors.rows)
if(ssc)
{
descriptorsTmp = cv::Mat(maxKeypoints, descriptors.cols, descriptors.type());
}
for(unsigned int k=0; k < kptsTmp.size() && iter!=hessianMap.rend(); ++k, ++iter)
{
kptsTmp[k] = keypoints[iter->second];
if(keypoints3D.size())
ULOGGER_DEBUG("too much words (%d), removing words with SSC", keypoints.size());
static constexpr float tolerance = 0.1;
auto ResultVec = util2d::SSC(keypoints, maxKeypoints, tolerance, imageSize.width, imageSize.height);
removed = keypoints.size()-ResultVec.size();
// retrieve final keypoints
kptsTmp.resize(ResultVec.size());
if(!keypoints3D.empty())
{
kpts3DTmp[k] = keypoints3D[iter->second];
kpts3DTmp.resize(ResultVec.size());
}
if(descriptors.rows)
{
if(descriptors.type() == CV_32FC1)
descriptorsTmp = cv::Mat(ResultVec.size(), descriptors.cols, descriptors.type());
}
for(unsigned int k=0; k<ResultVec.size(); ++k)
{
kptsTmp[k] = keypoints[ResultVec[k]];
if(keypoints3D.size())
{
memcpy(descriptorsTmp.ptr<float>(k), descriptors.ptr<float>(iter->second), descriptors.cols*sizeof(float));
kpts3DTmp[k] = keypoints3D[ResultVec[k]];
}
else
if(descriptors.rows)
{
memcpy(descriptorsTmp.ptr<char>(k), descriptors.ptr<char>(iter->second), descriptors.cols*sizeof(char));
if(descriptors.type() == CV_32FC1)
{
memcpy(descriptorsTmp.ptr<float>(k), descriptors.ptr<float>(ResultVec[k]), descriptors.cols*sizeof(float));
}
else
{
memcpy(descriptorsTmp.ptr<char>(k), descriptors.ptr<char>(ResultVec[k]), descriptors.cols*sizeof(char));
}
}
}
}
ULOGGER_DEBUG("%d keypoints removed, (kept %d), minimum response=%f", removed, (int)kptsTmp.size(), kptsTmp.size()?kptsTmp.back().response:0.0f);
else
{
ULOGGER_DEBUG("too much words (%d), removing words with the hessian threshold", keypoints.size());
// Remove words under the new hessian threshold
// Sort words by hessian
std::multimap<float, int> hessianMap; // <hessian,id>
for(unsigned int i = 0; i <keypoints.size(); ++i)
{
//Keep track of the data, to be easier to manage the data in the next step
hessianMap.insert(std::pair<float, int>(fabs(keypoints[i].response), i));
}
// Remove them from the signature
removed = (int)hessianMap.size()-maxKeypoints;
std::multimap<float, int>::reverse_iterator iter = hessianMap.rbegin();
kptsTmp.resize(maxKeypoints);
if(!keypoints3D.empty())
{
kpts3DTmp.resize(maxKeypoints);
}
if(descriptors.rows)
{
descriptorsTmp = cv::Mat(maxKeypoints, descriptors.cols, descriptors.type());
}
for(unsigned int k=0; k<kptsTmp.size() && iter!=hessianMap.rend(); ++k, ++iter)
{
kptsTmp[k] = keypoints[iter->second];
if(keypoints3D.size())
{
kpts3DTmp[k] = keypoints3D[iter->second];
}
if(descriptors.rows)
{
if(descriptors.type() == CV_32FC1)
{
memcpy(descriptorsTmp.ptr<float>(k), descriptors.ptr<float>(iter->second), descriptors.cols*sizeof(float));
}
else
{
memcpy(descriptorsTmp.ptr<char>(k), descriptors.ptr<char>(iter->second), descriptors.cols*sizeof(char));
}
}
}
}
ULOGGER_DEBUG("%d keypoints removed, (kept %d), minimum response=%f", removed, (int)kptsTmp.size(), !ssc&&kptsTmp.size()?kptsTmp.back().response:0.0f);
ULOGGER_DEBUG("removing words time = %f s", timer.ticks());
keypoints = kptsTmp;
keypoints3D = kpts3DTmp;
@@ -342,31 +383,46 @@ void Feature2D::limitKeypoints(std::vector<cv::KeyPoint> & keypoints, std::vecto
}
}
void Feature2D::limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std::vector<bool> & inliers, int maxKeypoints)
void Feature2D::limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std::vector<bool> & inliers, int maxKeypoints, const cv::Size & imageSize, bool ssc)
{
if(maxKeypoints > 0 && (int)keypoints.size() > maxKeypoints)
{
UTimer timer;
ULOGGER_DEBUG("too much words (%d), removing words with the hessian threshold", (int)keypoints.size());
// Remove words under the new hessian threshold
// Sort words by hessian
std::multimap<float, int> hessianMap; // <hessian,id>
for(unsigned int i = 0; i <keypoints.size(); ++i)
{
//Keep track of the data, to be easier to manage the data in the next step
hessianMap.insert(std::pair<float, int>(fabs(keypoints[i].response), i));
}
// Keep keypoints with highest response
int removed = (int)hessianMap.size()-maxKeypoints;
std::multimap<float, int>::reverse_iterator iter = hessianMap.rbegin();
inliers.resize(keypoints.size(), false);
float minimumHessian = 0.0f;
for(int k=0; k < maxKeypoints && iter!=hessianMap.rend(); ++k, ++iter)
int removed;
inliers.resize(keypoints.size(), false);
if(ssc)
{
inliers[iter->second] = true;
minimumHessian = iter->first;
ULOGGER_DEBUG("too much words (%d), removing words with SSC", keypoints.size());
static constexpr float tolerance = 0.1;
auto ResultVec = util2d::SSC(keypoints, maxKeypoints, tolerance, imageSize.width, imageSize.height);
removed = keypoints.size()-ResultVec.size();
for(unsigned int k=0; k<ResultVec.size(); ++k)
{
inliers[ResultVec[k]] = true;
}
}
else
{
ULOGGER_DEBUG("too much words (%d), removing words with the hessian threshold", keypoints.size());
// Remove words under the new hessian threshold
// Sort words by hessian
std::multimap<float, int> hessianMap; // <hessian,id>
for(unsigned int i = 0; i<keypoints.size(); ++i)
{
//Keep track of the data, to be easier to manage the data in the next step
hessianMap.insert(std::pair<float, int>(fabs(keypoints[i].response), i));
}
// Keep keypoints with highest response
removed = (int)hessianMap.size()-maxKeypoints;
std::multimap<float, int>::reverse_iterator iter = hessianMap.rbegin();
for(int k=0; k<maxKeypoints && iter!=hessianMap.rend(); ++k, ++iter)
{
inliers[iter->second] = true;
minimumHessian = iter->first;
}
}
ULOGGER_DEBUG("%d keypoints removed, (kept %d), minimum response=%f", removed, maxKeypoints, minimumHessian);
ULOGGER_DEBUG("filter keypoints time = %f s", timer.ticks());
@@ -378,7 +434,7 @@ void Feature2D::limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std:
}
}
void Feature2D::limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std::vector<bool> & inliers, int maxKeypoints, const cv::Size & imageSize, int gridRows, int gridCols)
void Feature2D::limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std::vector<bool> & inliers, int maxKeypoints, const cv::Size & imageSize, int gridRows, int gridCols, bool ssc)
{
if(maxKeypoints <= 0 || (int)keypoints.size() <= maxKeypoints)
{
@@ -406,7 +462,7 @@ void Feature2D::limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std:
for(size_t i=0; i<keypointsPerCell.size(); ++i)
{
std::vector<bool> inliersCell;
limitKeypoints(keypointsPerCell[i], inliersCell, maxKeypointsPerCell);
limitKeypoints(keypointsPerCell[i], inliersCell, maxKeypointsPerCell, cv::Size(colSize, rowSize), ssc);
for(size_t j=0; j<inliersCell.size(); ++j)
{
if(inliersCell[j])
@@ -432,6 +488,7 @@ cv::Rect Feature2D::computeRoi(const cv::Mat & image, const std::vector<float> &
/////////////////////
Feature2D::Feature2D(const ParametersMap & parameters) :
maxFeatures_(Parameters::defaultKpMaxFeatures()),
SSC_(Parameters::defaultKpSSC()),
_maxDepth(Parameters::defaultKpMaxDepth()),
_minDepth(Parameters::defaultKpMinDepth()),
_roiRatios(std::vector<float>(4, 0.0f)),
@@ -453,6 +510,7 @@ void Feature2D::parseParameters(const ParametersMap & parameters)
uInsert(parameters_, parameters);
Parameters::parse(parameters, Parameters::kKpMaxFeatures(), maxFeatures_);
Parameters::parse(parameters, Parameters::kKpSSC(), SSC_);
Parameters::parse(parameters, Parameters::kKpMaxDepth(), _maxDepth);
Parameters::parse(parameters, Parameters::kKpMinDepth(), _minDepth);
Parameters::parse(parameters, Parameters::kKpSubPixWinSize(), _subPixWinSize);
@@ -736,7 +794,7 @@ std::vector<cv::KeyPoint> Feature2D::generateKeypoints(const cv::Mat & image, co
subKeypoints = this->generateKeypointsImpl(image, roi, mask);
if (this->getType() != Feature2D::Type::kFeaturePyDetector)
{
limitKeypoints(subKeypoints, maxFeatures);
limitKeypoints(subKeypoints, maxFeatures, roi.size(), this->getSSC());
}
if(roi.x || roi.y)
{
@@ -2142,7 +2200,7 @@ std::vector<cv::KeyPoint> ORBOctree::generateKeypointsImpl(const cv::Mat & image
if((int)keypoints.size() > this->getMaxFeatures())
{
limitKeypoints(keypoints, descriptors_, this->getMaxFeatures());
limitKeypoints(keypoints, descriptors_, this->getMaxFeatures(), roi.size(), this->getSSC());
}
#else
UWARN("RTAB-Map is not built with ORB OcTree option enabled so ORB OcTree feature cannot be used!");

49
corelib/src/Memory.cpp
View File

@@ -111,6 +111,7 @@ Memory::Memory(const ParametersMap & parameters) :
_rotateImagesUpsideUp(Parameters::defaultMemRotateImagesUpsideUp()),
_createOccupancyGrid(Parameters::defaultRGBDCreateOccupancyGrid()),
_visMaxFeatures(Parameters::defaultVisMaxFeatures()),
_visSSC(Parameters::defaultVisSSC()),
_imagesAlreadyRectified(Parameters::defaultRtabmapImagesAlreadyRectified()),
_rectifyOnlyFeatures(Parameters::defaultRtabmapRectifyOnlyFeatures()),
_covOffDiagonalIgnored(Parameters::defaultMemCovOffDiagIgnored()),
@@ -603,6 +604,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
Parameters::parse(params, Parameters::kMemRotateImagesUpsideUp(), _rotateImagesUpsideUp);
Parameters::parse(params, Parameters::kRGBDCreateOccupancyGrid(), _createOccupancyGrid);
Parameters::parse(params, Parameters::kVisMaxFeatures(), _visMaxFeatures);
Parameters::parse(params, Parameters::kVisSSC(), _visSSC);
Parameters::parse(params, Parameters::kRtabmapImagesAlreadyRectified(), _imagesAlreadyRectified);
Parameters::parse(params, Parameters::kRtabmapRectifyOnlyFeatures(), _rectifyOnlyFeatures);
Parameters::parse(params, Parameters::kMemCovOffDiagIgnored(), _covOffDiagonalIgnored);
@@ -5131,9 +5133,13 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
UASSERT(keypoints3D.empty() || keypoints3D.size() == keypoints.size());
int maxFeatures = _rawDescriptorsKept&&!pose.isNull()&&_feature2D->getMaxFeatures()>0&&_feature2D->getMaxFeatures()<_visMaxFeatures?_visMaxFeatures:_feature2D->getMaxFeatures();
bool ssc = _rawDescriptorsKept&&!pose.isNull()&&_feature2D->getMaxFeatures()>0&&_feature2D->getMaxFeatures()<_visMaxFeatures?_visSSC:_feature2D->getSSC();
if((int)keypoints.size() > maxFeatures)
{
_feature2D->limitKeypoints(keypoints, keypoints3D, descriptors, maxFeatures);
if(data.cameraModels().size()==1 || data.stereoCameraModels().size()==1)
_feature2D->limitKeypoints(keypoints, keypoints3D, descriptors, maxFeatures, data.cameraModels().size()?data.cameraModels()[0].imageSize():data.stereoCameraModels()[0].left().imageSize(), ssc);
else
_feature2D->limitKeypoints(keypoints, keypoints3D, descriptors, maxFeatures);
}
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_detection(), t*1000.0f);
@@ -5349,22 +5355,13 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
int inliersCount = 0;
if((_feature2D->getGridRows() > 1 || _feature2D->getGridCols() > 1) &&
(decimatedData.cameraModels().size()==1 || decimatedData.stereoCameraModels().size()==1 ||
data.cameraModels().size()==1 || data.stereoCameraModels().size()==1))
data.cameraModels().size()==1 || data.stereoCameraModels().size()==1))
{
Feature2D::limitKeypoints(keypoints,
inliers,
_feature2D->getMaxFeatures(),
decimatedData.cameraModels().size()?decimatedData.cameraModels()[0].imageSize():
decimatedData.stereoCameraModels().size()?decimatedData.stereoCameraModels()[0].left().imageSize():
data.cameraModels().size()?data.cameraModels()[0].imageSize():data.stereoCameraModels()[0].left().imageSize(),
_feature2D->getGridRows(), _feature2D->getGridCols());
for(size_t i=0; i<inliers.size(); ++i)
{
if(inliers[i])
{
++inliersCount;
}
}
Feature2D::limitKeypoints(keypoints, inliers, _feature2D->getMaxFeatures(),
decimatedData.cameraModels().size()?decimatedData.cameraModels()[0].imageSize():
decimatedData.stereoCameraModels().size()?decimatedData.stereoCameraModels()[0].left().imageSize():
data.cameraModels().size()?data.cameraModels()[0].imageSize():data.stereoCameraModels()[0].left().imageSize(),
_feature2D->getGridRows(), _feature2D->getGridCols(), _feature2D->getSSC());
}
else
{
@@ -5373,8 +5370,24 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
UWARN("Ignored %s and %s parameters as they cannot be used for multi-cameras setup or uncalibrated camera.",
Parameters::kKpGridCols().c_str(), Parameters::kKpGridRows().c_str());
}
Feature2D::limitKeypoints(keypoints, inliers, _feature2D->getMaxFeatures());
inliersCount = _feature2D->getMaxFeatures();
if(decimatedData.cameraModels().size()==1 || decimatedData.stereoCameraModels().size()==1 ||
data.cameraModels().size()==1 || data.stereoCameraModels().size()==1)
{
Feature2D::limitKeypoints(keypoints, inliers, _feature2D->getMaxFeatures(),
decimatedData.cameraModels().size()?decimatedData.cameraModels()[0].imageSize():
decimatedData.stereoCameraModels().size()?decimatedData.stereoCameraModels()[0].left().imageSize():
data.cameraModels().size()?data.cameraModels()[0].imageSize():data.stereoCameraModels()[0].left().imageSize(),
_feature2D->getSSC());
}
else
{
Feature2D::limitKeypoints(keypoints, inliers, _feature2D->getMaxFeatures());
}
}
for(size_t i=0; i<inliers.size(); ++i)
{
if(inliers[i])
++inliersCount;
}
descriptorsForQuantization = cv::Mat(inliersCount, descriptors.cols, descriptors.type());

5
corelib/src/RegistrationVis.cpp
View File

@@ -102,6 +102,7 @@ RegistrationVis::RegistrationVis(const ParametersMap & parameters, Registration
uInsert(_featureParameters, ParametersPair(Parameters::kKpNndrRatio(), _featureParameters.at(Parameters::kVisCorNNDR())));
uInsert(_featureParameters, ParametersPair(Parameters::kKpDetectorStrategy(), _featureParameters.at(Parameters::kVisFeatureType())));
uInsert(_featureParameters, ParametersPair(Parameters::kKpMaxFeatures(), _featureParameters.at(Parameters::kVisMaxFeatures())));
uInsert(_featureParameters, ParametersPair(Parameters::kKpSSC(), _featureParameters.at(Parameters::kVisSSC())));
uInsert(_featureParameters, ParametersPair(Parameters::kKpMaxDepth(), _featureParameters.at(Parameters::kVisMaxDepth())));
uInsert(_featureParameters, ParametersPair(Parameters::kKpMinDepth(), _featureParameters.at(Parameters::kVisMinDepth())));
uInsert(_featureParameters, ParametersPair(Parameters::kKpRoiRatios(), _featureParameters.at(Parameters::kVisRoiRatios())));
@@ -234,6 +235,10 @@ void RegistrationVis::parseParameters(const ParametersMap & parameters)
{
uInsert(_featureParameters, ParametersPair(Parameters::kKpMaxFeatures(), parameters.at(Parameters::kVisMaxFeatures())));
}
if(uContains(parameters, Parameters::kVisSSC()))
{
uInsert(_featureParameters, ParametersPair(Parameters::kKpSSC(), parameters.at(Parameters::kVisSSC())));
}
if(uContains(parameters, Parameters::kVisMaxDepth()))
{
uInsert(_featureParameters, ParametersPair(Parameters::kKpMaxDepth(), parameters.at(Parameters::kVisMaxDepth())));

1
corelib/src/SensorCaptureThread.cpp
View File

@@ -241,6 +241,7 @@ void SensorCaptureThread::enableFeatureDetection(const ParametersMap & parameter
ParametersMap defaultParams = Parameters::getDefaultParameters("Vis");
uInsert(params, ParametersPair(Parameters::kKpDetectorStrategy(), uValue(params, Parameters::kVisFeatureType(), defaultParams.at(Parameters::kVisFeatureType()))));
uInsert(params, ParametersPair(Parameters::kKpMaxFeatures(), uValue(params, Parameters::kVisMaxFeatures(), defaultParams.at(Parameters::kVisMaxFeatures()))));
uInsert(params, ParametersPair(Parameters::kKpSSC(), uValue(params, Parameters::kVisSSC(), defaultParams.at(Parameters::kVisSSC()))));
uInsert(params, ParametersPair(Parameters::kKpMaxDepth(), uValue(params, Parameters::kVisMaxDepth(), defaultParams.at(Parameters::kVisMaxDepth()))));
uInsert(params, ParametersPair(Parameters::kKpMinDepth(), uValue(params, Parameters::kVisMinDepth(), defaultParams.at(Parameters::kVisMinDepth()))));
uInsert(params, ParametersPair(Parameters::kKpRoiRatios(), uValue(params, Parameters::kVisRoiRatios(), defaultParams.at(Parameters::kVisRoiRatios()))));

77
corelib/src/util2d.cpp
View File

@@ -2202,6 +2202,83 @@ void NMS(
}
}
std::vector<int> SSC(
const std::vector<cv::KeyPoint> & keypoints, int maxKeypoints, float tolerance, int cols, int rows)
{
// several temp expression variables to simplify solution equation
int exp1 = rows + cols + 2*maxKeypoints;
long long exp2 = ((long long)4*cols + (long long)4*maxKeypoints + (long long)4*rows*maxKeypoints + (long long)rows*rows + (long long)cols*cols - (long long)2*rows*cols + (long long)4*rows*cols*maxKeypoints);
double exp3 = sqrt(exp2);
double exp4 = maxKeypoints - 1;
double sol1 = -round((exp1 + exp3) / exp4); // first solution
double sol2 = -round((exp1 - exp3) / exp4); // second solution
// binary search range initialization with positive solution
int high = (sol1 > sol2) ? sol1 : sol2;
int low = floor(sqrt((double)keypoints.size() / maxKeypoints));
low = std::max(1, low);
int width;
int prevWidth = -1;
unsigned int Kmin = round(maxKeypoints - (maxKeypoints * tolerance));
unsigned int Kmax = round(maxKeypoints + (maxKeypoints * tolerance));
std::vector<int> ResultVec, result;
result.reserve(keypoints.size());
bool complete = false;
while(!complete)
{
width = low + (high - low) / 2;
if(width==prevWidth || low>high) // needed to reassure the same radius is not repeated again
{
ResultVec = result; // return the keypoints from the previous iteration
break;
}
result.clear();
double c = (double)width / 2.0; // initializing Grid
int numCellCols = floor(cols / c);
int numCellRows = floor(rows / c);
std::vector<std::vector<bool>> coveredVec(numCellRows+1, std::vector<bool>(numCellCols+1, false));
for(unsigned int i=0; i<keypoints.size(); ++i)
{
int row = floor(keypoints[i].pt.y / c); // get position of the cell current point is located at
int col = floor(keypoints[i].pt.x / c);
if(coveredVec[row][col] == false) // if the cell is not covered
{
result.push_back(i);
int rowMin = ((row - floor(width / c)) >= 0) ? (row - floor(width / c)) : 0; // get range which current radius is covering
int rowMax = ((row + floor(width / c)) <= numCellRows) ? (row + floor(width / c)) : numCellRows;
int colMin = ((col - floor(width / c)) >= 0) ? (col - floor(width / c)) : 0;
int colMax = ((col + floor(width / c)) <= numCellCols) ? (col + floor(width / c)) : numCellCols;
for(int rowToCov=rowMin; rowToCov<=rowMax; ++rowToCov)
{
for(int colToCov=colMin; colToCov<=colMax; ++colToCov)
{
if(!coveredVec[rowToCov][colToCov])
coveredVec[rowToCov][colToCov] = true; // cover cells within the square bounding box with width
}
}
}
}
if(result.size() >= Kmin && result.size() <= Kmax) // solution found
{
ResultVec = result;
complete = true;
}
else if(result.size() < Kmin)
high = width - 1; // update binary search range
else
low = width + 1;
prevWidth = width;
}
return ResultVec;
}
void rotateImagesUpsideUpIfNecessary(
CameraModel & model,
cv::Mat & rgb,