15 #include <mrpt/otherlibs/do_opencv_includes.h>
30 const unsigned int init_ID,
const unsigned int nDesiredFeatures,
37 #if MRPT_OPENCV_VERSION_NUM < 0x240
43 vector<KeyPoint> cv_feats;
46 const bool use_precomputed_feats = feats.
size() > 0;
48 if (use_precomputed_feats)
50 cv_feats.resize(feats.
size());
51 for (
size_t k = 0; k < cv_feats.size(); ++k)
53 cv_feats[k].pt.x = feats[k]->x;
54 cv_feats[k].pt.y = feats[k]->y;
60 const Mat cvImg = cv::cvarrToMat(inImg_gray.
getAs<IplImage>());
63 #if MRPT_OPENCV_VERSION_NUM < 0x300
64 Ptr<Feature2D> orb = Algorithm::create<Feature2D>(
"Feature2D.ORB");
65 orb->operator()(cvImg, Mat(), cv_feats, cv_descs, use_precomputed_feats);
67 const size_t n_feats_2_extract =
68 nDesiredFeatures == 0 ? 1000 : 3 * nDesiredFeatures;
69 Ptr<cv::ORB> orb = cv::ORB::create(
70 n_feats_2_extract, options.ORBOptions.scale_factor,
71 options.ORBOptions.n_levels);
72 orb->detectAndCompute(
73 cvImg, Mat(), cv_feats, cv_descs, use_precomputed_feats);
76 const size_t n_feats = cv_feats.size();
79 const unsigned int patch_size_2 = options.patchSize / 2;
80 unsigned int f_id = init_ID;
81 if (use_precomputed_feats)
83 for (
size_t k = 0; k < n_feats; ++k)
85 feats[k]->descriptors.ORB.
resize(cv_descs.cols);
86 for (
int m = 0; m < cv_descs.cols; ++m)
87 feats[k]->descriptors.ORB[m] =
88 cv_descs.at<uchar>(k, m);
98 if (options.ORBOptions.extract_patch && options.patchSize > 0)
101 feats[k]->patch,
round(feats[k]->
x) - patch_size_2,
102 round(feats[k]->
y) - patch_size_2, options.patchSize,
113 std::vector<size_t> sorted_indices(n_feats);
114 for (
size_t i = 0; i < n_feats; i++) sorted_indices[i] = i;
116 sorted_indices.begin(), sorted_indices.end(),
131 const bool do_filter_min_dist = options.ORBOptions.min_distance > 1;
132 const unsigned int occupied_grid_cell_size =
133 options.ORBOptions.min_distance / 2.0;
134 const float occupied_grid_cell_size_inv = 1.0f / occupied_grid_cell_size;
136 unsigned int grid_lx =
139 : (
unsigned int)(1 + inImg.
getWidth() * occupied_grid_cell_size_inv);
140 unsigned int grid_ly =
143 : (
unsigned int)(1 + inImg.
getHeight() * occupied_grid_cell_size_inv);
147 occupied_sections.
fillAll(
false);
149 const size_t n_max_feats = nDesiredFeatures > 0
150 ?
std::min(
size_t(nDesiredFeatures), n_feats)
153 if (!options.addNewFeatures) feats.
clear();
157 const size_t imgW = inImg.
getWidth();
160 while (c_feats < n_max_feats && k < n_feats)
162 const size_t idx = sorted_indices[k++];
163 const KeyPoint& kp = cv_feats[idx];
164 if (options.ORBOptions.extract_patch && options.patchSize > 0)
167 const int xBorderInf = (int)floor(kp.pt.x - patch_size_2);
168 const int xBorderSup = (int)floor(kp.pt.x + patch_size_2);
169 const int yBorderInf = (int)floor(kp.pt.y - patch_size_2);
170 const int yBorderSup = (int)floor(kp.pt.y + patch_size_2);
172 if (!(xBorderSup < (
int)imgW && xBorderInf > 0 &&
173 yBorderSup < (int)imgH && yBorderInf > 0))
177 if (do_filter_min_dist)
180 const size_t section_idx_x =
181 size_t(kp.pt.x * occupied_grid_cell_size_inv);
182 const size_t section_idx_y =
183 size_t(kp.pt.y * occupied_grid_cell_size_inv);
185 if (occupied_sections(section_idx_x, section_idx_y))
189 occupied_sections.
set_unsafe(section_idx_x, section_idx_y,
true);
190 if (section_idx_x > 0)
192 section_idx_x - 1, section_idx_y,
true);
193 if (section_idx_y > 0)
195 section_idx_x, section_idx_y - 1,
true);
196 if (section_idx_x < grid_lx - 1)
198 section_idx_x + 1, section_idx_y,
true);
199 if (section_idx_y < grid_ly - 1)
201 section_idx_x, section_idx_y + 1,
true);
210 ft->response = kp.response;
211 ft->orientation = kp.angle;
212 ft->scale = kp.octave;
216 ft->descriptors.ORB.resize(cv_descs.cols);
217 for (
int m = 0; m < cv_descs.cols; ++m)
218 ft->descriptors.ORB[m] = cv_descs.at<uchar>(idx, m);
220 if (options.ORBOptions.extract_patch && options.patchSize > 0)
222 ft->patchSize = options.patchSize;
225 ft->patch,
round(ft->x) - patch_size_2,
226 round(ft->y) - patch_size_2, options.patchSize,
242 #if MRPT_OPENCV_VERSION_NUM < 0x240
247 const size_t n_feats = in_features.
size();
251 vector<KeyPoint> cv_feats(n_feats);
252 for (
size_t k = 0; k < n_feats; ++k)
254 KeyPoint& kp = cv_feats[k];
255 kp.pt.x = in_features[k]->x;
256 kp.pt.y = in_features[k]->y;
257 kp.angle = in_features[k]->orientation;
258 kp.
size = in_features[k]->scale;
261 Mat cvImg(cv::cvarrToMat(inImg_gray.
getAs<IplImage>()));
264 #if MRPT_OPENCV_VERSION_NUM < 0x300
265 Ptr<Feature2D> orb = Algorithm::create<Feature2D>(
"Feature2D.ORB");
267 cvImg, Mat(), cv_feats, cv_descs,
true );
269 Ptr<cv::ORB> orb = cv::ORB::create(
270 n_feats, options.ORBOptions.scale_factor, options.ORBOptions.n_levels);
271 orb->detectAndCompute(
272 cvImg, Mat(), cv_feats, cv_descs,
true );
276 for (
size_t k = 0; k < n_feats; ++k)
278 in_features[k]->descriptors.ORB.
resize(cv_descs.cols);
279 for (
int i = 0; i < cv_descs.cols; ++i)
280 in_features[k]->descriptors.ORB[i] = cv_descs.at<uchar>(k, i);