Ejemplo n.º 1
0
void fast_corner_detect(
	const IplImage* I, TSimpleFeatureList& corners, int barrier, uint8_t octave,
	std::vector<size_t>* out_feats_index_by_row)
	{
		auto ptr = reinterpret_cast<CVD::byte* >(I->imageData);
		CVD::BasicImage<CVD::byte> img(ptr, {I->width, I->height}, I->widthStep);

		std::vector<CVD::ImageRef> outputs;
		//reerve enough corners for every pixel
		outputs.reserve(I->width * I->height);
		F(img, outputs, barrier);
		for(auto & output : outputs)
		{
			corners.push_back_fast(output.x << octave, output.y << octave);
		}
		if(out_feats_index_by_row)
		{
				auto & counters = *out_feats_index_by_row;
				counters.assign(I->height, 0);
				for(auto & output : outputs)
				{
					counters[output.y]++;
				}
		}
	}
Ejemplo n.º 2
0
			inline void trackFeatures_addNewFeats<TSimpleFeatureList>(TSimpleFeatureList &featureList,const TSimpleFeatureList &new_feats, const std::vector<size_t> &sorted_indices, const size_t nNewToCheck,const size_t maxNumFeatures,const float minimum_KLT_response_to_add,const double threshold_sqr_dist_to_add_new,const size_t patchSize,const CImage &cur_gray, TFeatureID  &max_feat_ID_at_input)
			{
#if 0
				// Brute-force version:
				const int max_manhatan_dist = std::sqrt(2*threshold_sqr_dist_to_add_new);

				for (size_t i=0;i<nNewToCheck && featureList.size()<maxNumFeatures;i++)
				{
					const TSimpleFeature &feat = new_feats[ sorted_indices[i] ];
					if (feat.response<minimum_KLT_response_to_add) break; // continue;

					// Check the min-distance:
					int manh_dist = std::numeric_limits<int>::max();
					for (size_t j=0;j<featureList.size();j++)
					{
						const TSimpleFeature &existing = featureList[j];
						const int d = std::abs(existing.pt.x-feat.pt.x)+std::abs(existing.pt.y-feat.pt.y);
						mrpt::utils::keep_min(manh_dist, d);
					}

					if (manh_dist<max_manhatan_dist)
						continue; // Already occupied! skip.

					// OK: accept it
					featureList.push_back_fast(feat.pt.x,feat.pt.y);  // (x,y)
					//featureList.mark_kdtree_as_outdated();

					// Fill out the rest of data:
					TSimpleFeature &newFeat = featureList.back();

					newFeat.ID			= ++max_feat_ID_at_input;
					newFeat.response	= feat.response;
					newFeat.octave		= 0;
					newFeat.track_status = status_IDLE;  //!< Inactive: right after detection, and before being tried to track
				}
#elif 0
				// Version with an occupancy grid:
				const int grid_cell_log2 = round( std::log(std::sqrt(threshold_sqr_dist_to_add_new)*0.5)/std::log(2.0));

				int grid_lx = 1+(cur_gray.getWidth() >> grid_cell_log2);
				int grid_ly = 1+(cur_gray.getHeight()>> grid_cell_log2);

				mrpt::math::CMatrixBool & occupied_sections = featureList.getOccupiedSectionsMatrix();

				occupied_sections.setSize(grid_lx,grid_ly);  // See the comments above for an explanation.
				occupied_sections.fillAll(false);

				for (size_t i=0;i<featureList.size();i++)
				{
					const TSimpleFeature &feat = featureList[i];
					const int section_idx_x = feat.pt.x >> grid_cell_log2;
					const int section_idx_y = feat.pt.y >> grid_cell_log2;

					if (!section_idx_x || !section_idx_y || section_idx_x>=grid_lx-1 || section_idx_y>=grid_ly-1)
						continue; // This may be too radical, but speeds up the logic below...
					
					// Mark sections as occupied
					bool *ptr1 = &occupied_sections.get_unsafe(section_idx_x-1,section_idx_y-1);
					bool *ptr2 = &occupied_sections.get_unsafe(section_idx_x-1,section_idx_y  );
					bool *ptr3 = &occupied_sections.get_unsafe(section_idx_x-1,section_idx_y+1);
					ptr1[0]=ptr1[1]=ptr1[2]=true;
					ptr2[0]=ptr2[1]=ptr2[2]=true;
					ptr3[0]=ptr3[1]=ptr3[2]=true;
				}

				for (size_t i=0;i<nNewToCheck && featureList.size()<maxNumFeatures;i++)
				{
					const TSimpleFeature &feat = new_feats[ sorted_indices[i] ];
					if (feat.response<minimum_KLT_response_to_add) break; // continue;

					// Check the min-distance:
					const int section_idx_x = feat.pt.x >> grid_cell_log2;
					const int section_idx_y = feat.pt.y >> grid_cell_log2;

					if (!section_idx_x || !section_idx_y || section_idx_x>=grid_lx-2 || section_idx_y>=grid_ly-2)
						continue; // This may be too radical, but speeds up the logic below...

					if (occupied_sections(section_idx_x,section_idx_y))
						continue; // Already occupied! skip.

					// Mark section as occupied
					bool *ptr1 = &occupied_sections.get_unsafe(section_idx_x-1,section_idx_y-1);
					bool *ptr2 = &occupied_sections.get_unsafe(section_idx_x-1,section_idx_y  );
					bool *ptr3 = &occupied_sections.get_unsafe(section_idx_x-1,section_idx_y+1);

					ptr1[0]=ptr1[1]=ptr1[2]=true;
					ptr2[0]=ptr2[1]=ptr2[2]=true;
					ptr3[0]=ptr3[1]=ptr3[2]=true;

					// OK: accept it
					featureList.push_back_fast(feat.pt.x,feat.pt.y);  // (x,y)
					//featureList.mark_kdtree_as_outdated();

					// Fill out the rest of data:
					TSimpleFeature &newFeat = featureList.back();

					newFeat.ID			= ++max_feat_ID_at_input;
					newFeat.response	= feat.response;
					newFeat.octave		= 0;
					newFeat.track_status = status_IDLE;  //!< Inactive: right after detection, and before being tried to track
				}
#else
				// Version with KD-tree
				CFeatureListKDTree<TSimpleFeature>  kdtree(featureList.getVector());


				for (size_t i=0;i<nNewToCheck && featureList.size()<maxNumFeatures;i++)
				{
					const TSimpleFeature &feat = new_feats[ sorted_indices[i] ];
					if (feat.response<minimum_KLT_response_to_add) break; // continue;

					// Check the min-distance:
					double min_dist_sqr = std::numeric_limits<double>::max();

					if (!featureList.empty())
					{
						//m_timlog.enter("[CGenericFeatureTracker] add new features.kdtree");
						min_dist_sqr = kdtree.kdTreeClosestPoint2DsqrError(feat.pt.x,feat.pt.y );
						//m_timlog.leave("[CGenericFeatureTracker] add new features.kdtree");
					}

					if (min_dist_sqr>threshold_sqr_dist_to_add_new)
					{
						// OK: accept it
						featureList.push_back_fast(feat.pt.x,feat.pt.y);  // (x,y)
						kdtree.mark_as_outdated();

						// Fill out the rest of data:
						TSimpleFeature &newFeat = featureList.back();

						newFeat.ID			= ++max_feat_ID_at_input;
						newFeat.response	= feat.response;
						newFeat.octave		= 0;
						newFeat.track_status = status_IDLE;  //!< Inactive: right after detection, and before being tried to track
					}
				}

#endif
			} // end of trackFeatures_addNewFeats<>