C++ CFeatureList::begin方法代码示例

			inline size_t trackFeatures_deleteOOB(
				CFeatureList &trackedFeats,
				const size_t img_width, const size_t img_height,
				const int MIN_DIST_MARGIN_TO_STOP_TRACKING)
			{
				CFeatureList::iterator itFeat = trackedFeats.begin();
				size_t n_removed = 0;
				while (itFeat!=trackedFeats.end())
				{
					const TFeatureTrackStatus status = (*itFeat)->track_status;
					bool eras = (status_TRACKED!=status && status_IDLE!=status);
					if (!eras)
					{
						// Also, check if it's too close to the image border:
						const float x= (*itFeat)->x;
						const float y= (*itFeat)->y;
						static const float MIN_DIST_MARGIN_TO_STOP_TRACKING = 10;
						if (x<MIN_DIST_MARGIN_TO_STOP_TRACKING  || y<MIN_DIST_MARGIN_TO_STOP_TRACKING ||
							x>(img_width-MIN_DIST_MARGIN_TO_STOP_TRACKING) ||
							y>(img_height-MIN_DIST_MARGIN_TO_STOP_TRACKING))
						{
							eras = true;
						}
					}
					if (eras)	// Erase or keep?
					{
						itFeat = trackedFeats.erase(itFeat);
						n_removed++;
					}
					else ++itFeat;
				}
				return n_removed;
			} // end of trackFeatures_deleteOOB

			inline void trackFeatures_updatePatch<CFeatureList>(CFeatureList &featureList,const CImage &cur_gray)
			{
				for (CFeatureList::iterator itFeat = featureList.begin(); itFeat != featureList.end();  ++itFeat)
				{
					CFeature* ft = itFeat->pointer();
					if (ft->track_status!=status_TRACKED)
						continue; // Skip if it's not correctly tracked.

					const size_t patch_width  = ft->patch.getWidth();
					const size_t patch_height = ft->patch.getHeight();
					if (patch_width>0 && patch_height>0)
					{
						try
						{
							const int offset = (int)patch_width/2; // + 1;
							cur_gray.extract_patch(
								ft->patch,
								round( ft->x ) - offset,
								round( ft->y ) - offset,
								patch_width,
								patch_height );
						}
						catch (std::exception &)
						{
							ft->track_status = status_OOB; // Out of bounds!
						}
					}
				}
			} // end of trackFeatures_updatePatch<>

// ------------------------------------------------------------------------------------
//								insertCvSeqInCFeatureList
// ------------------------------------------------------------------------------------
void CFeatureExtraction::insertCvSeqInCFeatureList( void* features_, CFeatureList &list, unsigned int init_ID ) const
{
#if MRPT_HAS_OPENCV && MRPT_HAS_SIFT_HESS
	CvSeq* features = reinterpret_cast<CvSeq*>( features_ );

	int n = features->total;
	ASSERT_(n > 0);

	CFeatureList::iterator itFeat;
	struct feature* thisFeat;
	int k;
	for( itFeat = list.begin(), k = 0; itFeat != list.end() && k < n; k++ )
	{
		thisFeat = (feature*)cvGetSeqElem( features, k );
		if( (*itFeat)->x == thisFeat->x && (*itFeat)->y == thisFeat->y )
		{
			(*itFeat)->ID = (TFeatureID)(k + init_ID);
			(*itFeat)->orientation = thisFeat->ori;
			(*itFeat)->scale = thisFeat->scl;
			(*itFeat)->descriptors.SIFT.resize( thisFeat->d );
			for( int i = 0; i < thisFeat->d; i++ )
				(*itFeat)->descriptors.SIFT[i] = (unsigned char)thisFeat->descr[i];

			itFeat++;
		} // end if
	} // end for
#else
	THROW_EXCEPTION("Method not available since MRPT has been compiled without OpenCV")
#endif //MRPT_HAS_OPENCV
}

/************************************************************************************************
*								extractFeaturesSIFT  									        *
************************************************************************************************/
void  CFeatureExtraction::extractFeaturesSIFT(
		const CImage			&img,
		CFeatureList			&feats,
		unsigned int			init_ID,
		unsigned int			nDesiredFeatures,
		const TImageROI			&ROI) const
{
	bool usingROI = false;
	if( ROI.xMin != 0 || ROI.xMax != 0 || ROI.yMin != 0 || ROI.yMax != 0 )
		usingROI = true;	// A ROI has been defined
	
	// ROI can not be managed properly (yet) with these method, so we extract a subimage

	// use a smart pointer so we just copy the pointer if the image is grayscale, or we'll create a new one if it was RGB:
	CImage img_grayscale(img, FAST_REF_OR_CONVERT_TO_GRAY); // Was: auxImgPtr;
	if( usingROI )
	{
		ASSERT_( ROI.xMin >= 0 && ROI.xMin < ROI.xMax && ROI.xMax < img.getWidth() && ROI.yMin >= 0 && ROI.yMax < img.getHeight() && ROI.yMin < ROI.yMax );
		CImage auximg; 
		img_grayscale.extract_patch( auximg, ROI.xMin, ROI.yMin, ROI.xMax-ROI.xMin+1, ROI.yMax-ROI.yMin+1 ); // Subimage in "auxImg"
		img_grayscale.swap(auximg);
	}

	switch( options.SIFTOptions.implementation )
	{
// --------------------------------------------------------------------------------------
//		Binary in C# -> OPTIONAL: Feature position already computed
// --------------------------------------------------------------------------------------
		
	case CSBinary:
		{
#ifdef MRPT_OS_WINDOWS
		
			char			filImg[2000],filOut[2000],filFeat[2000];
			char			paramImg[2000];

			GetTempPathA(1000,filOut);	os::strcat(filOut,1000,"temp_out.txt");			// OUTPUT FILE
			GetTempPathA(1000,filImg);	os::strcat(filImg,1000,"temp_img.bmp");			// INPUT IMAGE (BMP) FOR BINARY IN (C#)

			bool onlyDesc = feats.size() > 0 ? true : false;

			if( onlyDesc )
			{
				GetTempPathA(1000,filFeat);	os::strcat(filFeat,1000,"temp_feats.txt");		// KEYPOINTS INPUT FILE
				CMatrix		listPoints(feats.size(),2);
				for (size_t i= 0;i<feats.size();i++)
				{
					listPoints(i,0) = feats[i]->x;
					listPoints(i,1) = feats[i]->y;
				}
				listPoints.saveToTextFile( filFeat, MATRIX_FORMAT_FIXED /*Float format*/ );
			} // end if
			// -------------------------------------------
			//		CALL TO "extractSIFT.exe"
			// -------------------------------------------
			img_grayscale.saveToFile( filImg );

			// ------------------------------------
			// Version  with "CreateProcess":
			// ------------------------------------
			os::strcpy(paramImg,1000,"extractSIFT.exe -i"); os::strcat(paramImg,1000,filImg);
			os::strcat(paramImg,1000," -f"); os::strcat(paramImg,1000,filOut);
			os::strcat(paramImg,1000," -l"); os::strcat(paramImg,1000,filFeat);

			// ------------------------------------
			// Launch process
			// ------------------------------------
			bool ret = mrpt::system::launchProcess( paramImg );

			if( !ret )
				THROW_EXCEPTION( "[extractFeaturesSIFT] Could not launch external process... (extractSIFT.exe)" )

			// Process Results
			CFeatureList::iterator	itFeat = feats.begin();
			size_t	nFeats;

			CMatrix		aux;
			aux.loadFromTextFile( filOut );
			std::cout << "[computeSiftFeatures] " << aux.getRowCount() << " features." << std::endl;

			if( onlyDesc )
				nFeats = feats.size();
			else
			{
				nFeats = aux.getRowCount();
				feats.resize( nFeats );
			}

			for( size_t i = 0;
                 itFeat != feats.end();
				 i++, itFeat++)
			{
				(*itFeat)->type			= featSIFT;
				(*itFeat)->x			= usingROI ? aux(i,0) + ROI.xMin : aux(i,0);
				(*itFeat)->y			= usingROI ? aux(i,1) + ROI.yMin : aux(i,1);
				(*itFeat)->orientation	= aux(i,2);
				(*itFeat)->scale		= aux(i,3);
//.........这里部分代码省略.........

// Compute SIFT descriptors on a set of already localized points
void  CFeatureExtraction::internal_computeSiftDescriptors( const CImage	&in_img,
												  CFeatureList		&in_features) const
{

	ASSERT_( in_features.size() > 0 );
	switch( options.SIFTOptions.implementation )
	{
		case CSBinary:
		{
#ifdef MRPT_OS_WINDOWS
			char	filImg[2000],filOut[2000],filFeat[2000];
			char	paramImg[2000];

			CFeatureList::iterator		feat;

			// Save to temporary file
			GetTempPathA(1000,filImg);	os::strcat( filImg, 1000, "temp_img.bmp" );
			GetTempPathA(1000,filOut);	os::strcat( filOut,1000, "temp_feats.txt" );
			GetTempPathA(1000,filFeat);	os::strcat( filFeat, 1000, "temp_KLT_feats.txt" );

			// Fill the input file
			FILE *fout = os::fopen( filFeat,"wt");
			for(feat = in_features.begin(); feat != in_features.end(); feat++)
				os::fprintf( fout, "%.6f %.6f\n", (*feat)->x, (*feat)->y );

			os::fclose(fout);

			// -------------------------------------------
			//		CALL TO "extractSIFT.exe"
			// -------------------------------------------
			in_img.saveToFile( filImg );

			// Version  with "CreateProcess":
			// ------------------------------------
			os::strcpy(paramImg,1000,"extractSIFT.exe -i"); os::strcat(paramImg,1000,filImg);
			os::strcat(paramImg,1000," -f"); os::strcat(paramImg,1000,filOut);
			os::strcat(paramImg,1000," -l"); os::strcat(paramImg,1000,filFeat);

			bool ret = mrpt::system::launchProcess( paramImg );

			if(!ret)
				THROW_EXCEPTION("[extractFeaturesSIFT] Could not launch external process... (extractSIFT.exe)");

			MRPT_START
			// Load the results:
			CMatrix		aux;
			aux.loadFromTextFile( filOut );
			size_t nRows = aux.getRowCount();

			printf("[computeSiftFeatures1] %u features\n", nRows);

			unsigned int i;
			float lx,ly;
			lx = 0.0;
			ly = 0.0;
			feat = in_features.begin();

			for(i = 0; i < nRows; i++)
			{
				if( aux(i,0) != lx  || aux(i,1) != ly ) // Only one descriptor for feature
				{
					(*feat)->type			= featSIFT;
					(*feat)->orientation	= aux(i,2);
					(*feat)->scale			= aux(i,3);

					// The descriptor:
					aux.extractRow(i, (*feat)->descriptors.SIFT, 4);

					lx = aux(i,0);
					ly = aux(i,1);

					feat++;
				} // end if
			} // end for
			MRPT_END

			break;
#else
    THROW_EXCEPTION("TO DO @ linux OS!");
#endif
		} // end case CSBinary
		case Hess: // Implementation by Hess
		{
#if !MRPT_HAS_SIFT_HESS
			THROW_EXCEPTION("Method not available since MRPT has been compiled without Hess' SIFT library")
#elif MRPT_HAS_OPENCV	// OK, we have Hess' sift:
			IplImage* init_img;
			IplImage*** gauss_pyr, *** dog_pyr;
			CvMemStorage* storage;
			CvSeq* features;
			int octvs; //, n = 0;

			/* check arguments */
			ASSERT_(in_img.getWidth() != 0 && in_img.getHeight() != 0);

			/* build scale space pyramid; smallest dimension of top level is ~4 pixels */
			const CImage img_grayscale(in_img, FAST_REF_OR_CONVERT_TO_GRAY);
			const IplImage* ipl_im = img_grayscale.getAs<IplImage>();

//.........这里部分代码省略.........

/************************************************************************************************
*						internal_computeSurfDescriptors
************************************************************************************************/
void  CFeatureExtraction::internal_computeSurfDescriptors(
	const mrpt::utils::CImage	&inImg,
	CFeatureList		&in_features) const
{
#if MRPT_HAS_OPENCV && MRPT_OPENCV_VERSION_NUM >= 0x111

	if (in_features.empty()) return;

	const CImage img_grayscale(inImg, FAST_REF_OR_CONVERT_TO_GRAY);
	const IplImage* cGrey = img_grayscale.getAs<IplImage>();

	CvMemStorage *storage = cvCreateMemStorage(0);

	// Fill in the desired key-points:
	CvSeq *kp	=  cvCreateSeq( 0, sizeof(CvSeq), sizeof(CvSURFPoint), storage );
	for (CFeatureList::iterator	itList=in_features.begin();itList!=in_features.end();++itList)
	{
		CvSURFPoint point = cvSURFPoint(
			cvPoint2D32f((*itList)->x,(*itList)->y),
			0,  // Laplacian
			16  //sizes[layer]
			);

		cvSeqPush( kp, &point );
	}


	CvSeq *desc	=  NULL;

	// Only computes the descriptors:
	// Extract the SURF points:
	CvSURFParams surf_params = cvSURFParams(options.SURFOptions.hessianThreshold, options.SURFOptions.rotation_invariant ? 1:0);
	surf_params.nOctaves = options.SURFOptions.nOctaves;
	surf_params.nOctaveLayers = options.SURFOptions.nLayersPerOctave;

	cvExtractSURF( cGrey, NULL, &kp, &desc, storage, surf_params, 1 /* Use precomputed key-points */ );
	// *** HAVE YOU HAD A COMPILER ERROR NEAR THIS LINE?? : You need OpenCV >=1.1.0, final release or a SVN version ***

	// -----------------------------------------------------------------
	// MRPT Wrapping
	// -----------------------------------------------------------------
	CFeatureList::iterator	itList;
	int i;
	for (i=0, itList=in_features.begin();itList!=in_features.end();itList++,i++)
	{
		// Get the OpenCV SURF point
		CFeaturePtr ft = *itList;

		CvSURFPoint *point = (CvSURFPoint*)cvGetSeqElem( kp, i );

		ft->orientation = point->dir;				// Orientation
		ft->scale		= point->size*1.2/9;		// Scale

		// Get the SURF descriptor
		float* d = (float*)cvGetSeqElem( desc, i );
		ft->descriptors.SURF.resize( options.SURFOptions.rotation_invariant ? 128 : 64 );
		std::vector<float>::iterator itDesc;
		unsigned int k;
		for( k = 0, itDesc = ft->descriptors.SURF.begin(); k < ft->descriptors.SURF.size(); k++, itDesc++ )
			*itDesc = d[k];

	} // end for


	cvReleaseMemStorage(&storage); // Free memory

#else
			THROW_EXCEPTION("Method not available since either MRPT has been compiled without OpenCV or OpenCV version is incorrect (Required 1.1.0)")
#endif //MRPT_HAS_OPENCV
}  // end internal_computeSurfDescriptors

//.........这里部分代码省略.........
	map<TFeatureID, TPoint3D> lastVisibleFeats;
	std::vector<TPose3D>
		camera_key_frames_path;  // The 6D path of the Kinect camera.
	CPose3D
		currentCamPose_wrt_last;  // wrt last pose in "camera_key_frames_path"

	bool gl_keyframes_must_refresh =
		true;  // Need to update gl_keyframes from camera_key_frames_path??
	CObservation3DRangeScan::Ptr last_obs;
	string str_status, str_status2;

	while (win3D.isOpen() && !thrPar.quit)
	{
		CObservation3DRangeScan::Ptr possiblyNewObs =
			std::atomic_load(&thrPar.new_obs);
		if (possiblyNewObs && possiblyNewObs->timestamp != INVALID_TIMESTAMP &&
			(!last_obs || possiblyNewObs->timestamp != last_obs->timestamp))
		{
			// It IS a new observation:
			last_obs = possiblyNewObs;

			// Feature tracking -------------------------------------------
			ASSERT_(last_obs->hasIntensityImage);

			CImage theImg;  // The grabbed image:
			theImg = last_obs->intensityImage;

			// Do tracking:
			if (step_num > 1)  // we need "previous_image" to be valid.
			{
				tracker->trackFeatures(previous_image, theImg, trackedFeats);

				// Remove those now out of the image plane:
				CFeatureList::iterator itFeat = trackedFeats.begin();
				while (itFeat != trackedFeats.end())
				{
					const TFeatureTrackStatus status = (*itFeat)->track_status;
					bool eras =
						(status_TRACKED != status && status_IDLE != status);
					if (!eras)
					{
						// Also, check if it's too close to the image border:
						const float x = (*itFeat)->x;
						const float y = (*itFeat)->y;
						static const float MIN_DIST_MARGIN_TO_STOP_TRACKING =
							10;
						if (x < MIN_DIST_MARGIN_TO_STOP_TRACKING ||
							y < MIN_DIST_MARGIN_TO_STOP_TRACKING ||
							x > (last_obs->cameraParamsIntensity.ncols -
								 MIN_DIST_MARGIN_TO_STOP_TRACKING) ||
							y > (last_obs->cameraParamsIntensity.nrows -
								 MIN_DIST_MARGIN_TO_STOP_TRACKING))
						{
							eras = true;
						}
					}
					if (eras)  // Erase or keep?
						itFeat = trackedFeats.erase(itFeat);
					else
						++itFeat;
				}
			}

			// Create list of 3D features in space, wrt current camera pose:
			// --------------------------------------------------------------------
			map<TFeatureID, TPoint3D> curVisibleFeats;