void PFLocalizationCore::observation(CSensoryFramePtr _sf, CObservationOdometryPtr _odometry) {

	CActionCollectionPtr action = CActionCollection::Create();
	CActionRobotMovement2D odom_move;
    odom_move.timestamp = _sf->getObservationByIndex(0)->timestamp;
    if(_odometry) {
        if(odomLastObservation_.empty()) {
            odomLastObservation_ = _odometry->odometry;
        }
        mrpt::poses::CPose2D incOdoPose = _odometry->odometry - odomLastObservation_;
        odomLastObservation_ = _odometry->odometry;
        odom_move.computeFromOdometry(incOdoPose, motion_model_options_);
        action->insert(odom_move);
        updateFilter(action, _sf);
    } else {
      if(use_motion_model_default_options_){
        log_info("No odometry at update %4i -> using dummy", update_counter_);
		odom_move.computeFromOdometry(mrpt::poses::CPose2D(0,0,0), motion_model_default_options_);
        action->insert(odom_move);
        updateFilter(action, _sf);
      } else {
        log_info("No odometry at update %4i -> skipping observation", update_counter_);
      }
    }
}
void PFLocalizationCore::updateFilter(CActionCollectionPtr _action, CSensoryFramePtr _sf) {
    if(state_ == INIT) initializeFilter();
    tictac_.Tic();
    pf_.executeOn( pdf_, _action.pointer(), _sf.pointer(), &pf_stats_ );
    timeLastUpdate_ = _sf->getObservationByIndex(0)->timestamp;
    update_counter_++;
}
예제 #3
0
파일: Main.cpp 프로젝트: DYFeng/mrpt
// ------------------------------------------------------
//					Visual Odometry
// ------------------------------------------------------
void vOdometry_lightweight()
{
	// My Local Variables
	CVisualOdometryStereo				vOdometer;
	unsigned int						step = 0;
	CTicTac								tictac;

	std::vector<CPose3DQuat>			path1;
	std::vector<CPose3DQuatPDFGaussian>	path2;

	size_t								rawlogEntry = 0;
	CFileGZInputStream					rawlogFile( RAWLOG_FILE );

	// Initial pose of the path
	path1.push_back( CPose3DQuat() );
	path2.push_back( CPose3DQuatPDFGaussian() );

	// ----------------------------------------------------------
	//						vOdometry
	// ----------------------------------------------------------
	CActionCollectionPtr	action;
	CSensoryFramePtr		observations;
	CObservationPtr			observation;

	// Load options (stereo + matching + odometry)
	vOdometer.loadOptions( INI_FILENAME );

	// Delete previous files and prepare output dir
	deleteFiles( format("%s/*.txt", OUT_DIR) );

	FILE *f_cov = os::fopen( format( "%s/cov.txt", OUT_DIR ), "wt");
	ASSERT_( f_cov != NULL );

	// Iteration counter
	int	counter = 0;

	FILE *f_log = os::fopen( format( "%s/q.txt", OUT_DIR ), "wt");
	FILE *f_log2 = os::fopen( format( "%s/path.txt", OUT_DIR ), "wt");

	unsigned int imDecimation = 5;

	// Main Loop
	tictac.Tic();
	for (;;)
	{
		if (os::kbhit())
		{
			char c = os::getch();
			if (c==27)
				break;
		}

		// Load action/observation pair from the rawlog:
		// --------------------------------------------------
		if (! CRawlog::getActionObservationPairOrObservation( rawlogFile, action, observations, observation, rawlogEntry) )
			break; // file EOF


		if ( rawlogEntry >= RAWLOG_OFFSET && 0 == ( step % DECIMATION ) )
		{
			// Execute:
			// ----------------------------------------

			// STEREO IMAGES OBSERVATION
			CObservationStereoImagesPtr sImgs;
			if( observation )
				sImgs = CObservationStereoImagesPtr( observation );
			else
				sImgs = observations->getObservationByClass<CObservationStereoImages>();

			poses::CPose3DQuatPDFGaussian outEst;
			if( sImgs )
			{
				if( counter == 0 )
				{
					// Set initial parameters
					vOdometer.stereoParams.baseline = sImgs->rightCameraPose.x();
					vOdometer.stereoParams.K = sImgs->leftCamera.intrinsicParams;
				}

				cout << "Rawlog Entry: " << rawlogEntry << " Iteration: " << counter++ << endl;

				if( step % imDecimation )
				{
					vOdometer.process_light( sImgs, outEst );
					TOdometryInfo info = vOdometer.getInfo();

					// Save to file both the quaternion and covariance matrix
					os::fprintf( f_log,"%f %f %f %f %f %f %f\n",
						outEst.mean[0], outEst.mean[1], outEst.mean[2], outEst.mean[3], outEst.mean[4], outEst.mean[5], outEst.mean[6] );
					info.m_Prev_cloud.landmarks.saveToTextFile( format( "%s/clouds%04d.txt", OUT_DIR, step ) );

					path1.push_back( path1.back() + outEst.mean );
					os::fprintf( f_log2,"%f %f %f %f %f %f %f\n",
						path1.back()[0], path1.back()[1], path1.back()[2], path1.back()[3], path1.back()[4], path1.back()[5], path1.back()[6] );

				path2.push_back( outEst );
				}
				else
					cout << "Skipped step" << endl;

			} // end if sImgs != NULL

		} // end if 'rawlogEntry >= rawlog_offset'

		step++;

		// Free memory:
		action.clear_unique();
		observations.clear_unique();

	}; // end  while !end
	cout << "*************** Tiempo: " << 1000.0f*tictac.Tac() << "************************" << endl;

	os::fclose( f_cov );
	os::fclose( f_log );
	os::fclose( f_log2 );

	// SAVE THE RESULTS
	/**/
	FILE *fPath1 = os::fopen( format("./%s/EstimatedPath.txt", OUT_DIR).c_str(), "wt");
	if( fPath1 != NULL )
	{
		std::vector<CPose3DQuat>::iterator	itPath;
		for(itPath = path1.begin(); itPath != path1.end(); ++itPath )
			os::fprintf( fPath1,"%f %f %f %f %f %f %f\n",
			itPath->x(), itPath->y(), itPath->z(),
			itPath->quat().r(), itPath->quat().x(), itPath->quat().y(), itPath->quat().z() );

		os::fclose( fPath1 );
	}
	else
		std::cout << "WARNING: The estimated path could not be saved" << std::endl;

	FILE *fPath2 = os::fopen( format("./%s/EstimatedPathPDF.txt", OUT_DIR).c_str(), "wt");
	if( fPath2 != NULL )
	{
		std::vector<CPose3DQuatPDFGaussian>::iterator	itPath;
		for(itPath = path2.begin(); itPath != path2.end(); ++itPath )
		{
			os::fprintf(fPath2,"%.3f %.3f %.3f %.3f %.3f %.3f %.3f\n",
				itPath->mean.x(), itPath->mean.y(), itPath->mean.z(),
				itPath->mean.quat().r(), itPath->mean.quat().x(), itPath->mean.quat().y(), itPath->mean.quat().z() );

			os::fprintf(fPath2,"%.3f %.3f %.3f %.3f %.3f %.3f %.3f\n", itPath->cov.get_unsafe(0,0), itPath->cov.get_unsafe(0,1), itPath->cov.get_unsafe(0,2), itPath->cov.get_unsafe(0,3), itPath->cov.get_unsafe(0,4), itPath->cov.get_unsafe(0,5), itPath->cov.get_unsafe(0,6));
			os::fprintf(fPath2,"%.3f %.3f %.3f %.3f %.3f %.3f %.3f\n", itPath->cov.get_unsafe(1,0), itPath->cov.get_unsafe(1,1), itPath->cov.get_unsafe(1,2), itPath->cov.get_unsafe(1,3), itPath->cov.get_unsafe(1,4), itPath->cov.get_unsafe(1,5), itPath->cov.get_unsafe(1,6));
			os::fprintf(fPath2,"%.3f %.3f %.3f %.3f %.3f %.3f %.3f\n", itPath->cov.get_unsafe(2,0), itPath->cov.get_unsafe(2,1), itPath->cov.get_unsafe(2,2), itPath->cov.get_unsafe(2,3), itPath->cov.get_unsafe(2,4), itPath->cov.get_unsafe(2,5), itPath->cov.get_unsafe(2,6));
			os::fprintf(fPath2,"%.3f %.3f %.3f %.3f %.3f %.3f %.3f\n", itPath->cov.get_unsafe(3,0), itPath->cov.get_unsafe(3,1), itPath->cov.get_unsafe(3,2), itPath->cov.get_unsafe(3,3), itPath->cov.get_unsafe(3,4), itPath->cov.get_unsafe(3,5), itPath->cov.get_unsafe(3,6));
			os::fprintf(fPath2,"%.3f %.3f %.3f %.3f %.3f %.3f %.3f\n", itPath->cov.get_unsafe(4,0), itPath->cov.get_unsafe(4,1), itPath->cov.get_unsafe(4,2), itPath->cov.get_unsafe(4,3), itPath->cov.get_unsafe(4,4), itPath->cov.get_unsafe(4,5), itPath->cov.get_unsafe(4,6));
			os::fprintf(fPath2,"%.3f %.3f %.3f %.3f %.3f %.3f %.3f\n", itPath->cov.get_unsafe(5,0), itPath->cov.get_unsafe(5,1), itPath->cov.get_unsafe(5,2), itPath->cov.get_unsafe(5,3), itPath->cov.get_unsafe(5,4), itPath->cov.get_unsafe(5,5), itPath->cov.get_unsafe(5,6));
			os::fprintf(fPath2,"%.3f %.3f %.3f %.3f %.3f %.3f %.3f\n", itPath->cov.get_unsafe(6,0), itPath->cov.get_unsafe(6,1), itPath->cov.get_unsafe(6,2), itPath->cov.get_unsafe(6,3), itPath->cov.get_unsafe(6,4), itPath->cov.get_unsafe(6,5), itPath->cov.get_unsafe(6,6));
		}
		os::fclose( fPath2 );
	}
	else
		std::cout << "WARNING: The estimated pdf path could not be saved" << std::endl;

	std::cout << "Saved results!" << std::endl;
	/**/

	mrpt::system::pause();

}
/*---------------------------------------------------------------

					CLSLAM_RBPF_2DLASER

	Implements a 2D local SLAM method based on a RBPF
	    over an occupancy grid map. A part of HMT-SLAM.

\param LMH   The local metric hypothesis which must be updated by this SLAM algorithm.
\param act   The action to process (or NULL).
\param sf    The observations to process (or NULL).

 WE ALREADY HAVE CONTROL OVER THE CRITICAL SECTION OF THE LMHs!

--------------------------------------------------------------- */
void CLSLAM_RBPF_2DLASER::processOneLMH(
	CLocalMetricHypothesis	*LMH,
	const CActionCollectionPtr &actions,
	const CSensoryFramePtr     &sf )
{
	MRPT_START

	// Get the current robot pose estimation:
	TPoseID		currentPoseID = LMH->m_currentRobotPose;

	// If this is the first iteration, just create a new robot pose at the origin:
	if (currentPoseID == POSEID_INVALID )
	{
		currentPoseID = CHMTSLAM::generatePoseID();
		LMH->m_currentRobotPose = currentPoseID; // Change it in the LMH

		// Create a new robot pose:
		CPose3D	initPose(0,0,0);

		ASSERT_( LMH->m_particles.size()>0 );
		for (CLocalMetricHypothesis::CParticleList::iterator  it=LMH->m_particles.begin();it!=LMH->m_particles.end();++it)
			it->d->robotPoses[ currentPoseID ] = initPose;

		ASSERT_( m_parent->m_map.nodeCount()==1 );

		m_parent->m_map_cs.enter();
		CHMHMapNodePtr firstArea = m_parent->m_map.getFirstNode();
		ASSERT_(firstArea);
		LMH->m_nodeIDmemberships[currentPoseID] = firstArea->getID();

		// Set annotation for the reference pose:
		firstArea->m_annotations.setElemental( NODE_ANNOTATION_REF_POSEID,  currentPoseID , LMH->m_ID);
		m_parent->m_map_cs.leave();
	}

	bool	insertNewRobotPose = false;
	if (sf)
	{
		if ( LMH->m_nodeIDmemberships.size()<2 )  // If there is just 1 node (the current robot pose), then there is no observation in the map yet!
		{	// Update map if this is the first observation!
			insertNewRobotPose = true;
		}
		else
		{
			// Check minimum distance from current robot pose to those in the neighborhood:
			map< TPoseID, CPose3D >				lstRobotPoses;
			LMH->getMeans( lstRobotPoses );

			CPose3D	  *currentRobotPose = & lstRobotPoses[currentPoseID];
			float		minDistLin    = 1e6f;
			float		minDistAng    = 1e6f;

			//printf("Poses in graph:\n");
			for (map< TPoseID, CPose3D >::iterator	it = lstRobotPoses.begin();it!=lstRobotPoses.end();++it)
			{
				if (it->first != currentPoseID )
				{
					float linDist = it->second.distanceTo( *currentRobotPose );
					float angDist = fabs(math::wrapToPi( it->second.yaw() - currentRobotPose->yaw() ));

					minDistLin = min( minDistLin, linDist );

					if ( linDist < m_parent->m_options.SLAM_MIN_DIST_BETWEEN_OBS  )
						minDistAng = min(minDistAng, angDist);
				}
			}

			// time to insert a new node??
			insertNewRobotPose = (minDistLin>m_parent->m_options.SLAM_MIN_DIST_BETWEEN_OBS) || ( minDistAng > m_parent->m_options.SLAM_MIN_HEADING_BETWEEN_OBS );
		}

	} // end if there are SF

	// Save data in members so PF callback "prediction_and_update_pfXXXX" have access to them:
	m_insertNewRobotPose   = insertNewRobotPose;

	// ------------------------------------------------
	//  Execute RBPF method:
	// 	1) PROCESS ACTION
	// 	2) PROCESS OBSERVATIONS
	// ------------------------------------------------
	CParticleFilter		pf;
	pf.m_options = m_parent->m_options.pf_options;
	pf.executeOn( *LMH, actions.pointer(), sf.pointer() );

	// 3) The appearance observation: update the log likelihood
	// ...


	// -----------------------------------------------------------
	//			4) UPDATE THE MAP
	// -----------------------------------------------------------
	if (insertNewRobotPose)
	{
		m_parent->printf_debug("[CLSLAM_RBPF_2DLASER] Adding new pose...\n");

		//	Leave the up-to-now "current pose" in the map, insert the SF in it, and...
		// ----------------------------------------------------------------------------
		TPoseID newCurrentPoseID = CHMTSLAM::generatePoseID();

		//	...and create a new "current pose" making a copy of the previous one:
		//     and insert the observations into the metric maps:
		// ----------------------------------------------------------------------------
		for (CLocalMetricHypothesis::CParticleList::iterator partIt = LMH->m_particles.begin();
			  partIt!=LMH->m_particles.end();
			  partIt++)
		{
			const CPose3D  *curRobotPose = &partIt->d->robotPoses[currentPoseID];
			partIt->d->robotPoses[newCurrentPoseID]= *curRobotPose;
			sf->insertObservationsInto( &partIt->d->metricMaps, curRobotPose );
		}

		// Add node membership: for now, a copy of the current pose:
		LMH->m_nodeIDmemberships[newCurrentPoseID] = LMH->m_nodeIDmemberships[currentPoseID];


		// Now, insert the SF in the just added robot pose:
		// -----------------------------------------------------
		LMH->m_SFs[ currentPoseID ] = *sf;

		// Sets the new poseID as current robot pose:
		// ----------------------------------------------------
		TPoseID		 newlyAddedPose = currentPoseID;
		currentPoseID = LMH->m_currentRobotPose = newCurrentPoseID;

		// Mark the "newlyAddedPose" as pending to reconsider in the graph-cut method
		//  (Done in the main loop @ LSLAM thread)
		// --------------------------------------------------------------------------------
		LMH->m_posesPendingAddPartitioner.push_back( newlyAddedPose );

		m_parent->printf_debug("[CLSLAM_RBPF_2DLASER] Added pose %i.\n", (int)newlyAddedPose);


		// Notice LC detectors:
		// ------------------------------
		{
			synch::CCriticalSectionLocker	lock( &m_parent->m_topLCdets_cs );

			for (std::deque<CTopLCDetectorBase*>::iterator it=m_parent->m_topLCdets.begin();it!=m_parent->m_topLCdets.end();++it)
				(*it)->OnNewPose( newlyAddedPose, sf.pointer() );
		}


	} // end of insertNewRobotPose


	MRPT_END
}
예제 #5
0
파일: test.cpp 프로젝트: KMiyawaki/mrpt
// ------------------------------------------------------
//                  TestGeometry3D
// ------------------------------------------------------
void TestLaser2Imgs()
{
	 // Set your rawlog file name
	if (!mrpt::system::fileExists(RAWLOG_FILE))
		THROW_EXCEPTION_CUSTOM_MSG1("Rawlog file does not exist: %s",RAWLOG_FILE.c_str())

	CActionCollectionPtr	action;
	CSensoryFramePtr		observations;
	size_t					rawlogEntry		= 0;
	//bool					end 			= false;
	CDisplayWindow			wind;

	// Set relative path for externally-stored images in rawlogs:
	string	rawlog_images_path = extractFileDirectory( RAWLOG_FILE );
	rawlog_images_path+="/Images";
	CImage::IMAGES_PATH_BASE = rawlog_images_path;		// Set it.

	CFileGZInputStream		rawlogFile( RAWLOG_FILE );


	for (;;)
	{
		if (os::kbhit())
		{
			char c = os::getch();
			if (c==27)
				break;
		}

		// Load action/observation pair from the rawlog:
		// --------------------------------------------------
		cout << "Reading act/oct pair from rawlog..." << endl;
		if (! CRawlog::readActionObservationPair( rawlogFile, action, observations, rawlogEntry) )
			break; // file EOF

		// CAMERA............
		// Get CObservationStereoImages
		CObservationStereoImagesPtr sImgs;
		CObservationImagePtr Img;

		sImgs = observations->getObservationByClass<CObservationStereoImages>();
		if (!sImgs)
		{
			Img = observations->getObservationByClass<CObservationImage>();
			if(!Img)
				continue;
		}

		CPose3D cameraPose;	// Get Camera Pose (B) (CPose3D)
		CMatrixDouble33 K;			// Get Calibration matrix (K)

		sImgs ? sImgs->getSensorPose( cameraPose ) : Img->getSensorPose( cameraPose );
		K = sImgs ? sImgs->leftCamera.intrinsicParams : Img->cameraParams.intrinsicParams;

		// LASER.............
		// Get CObservationRange2D
		CObservation2DRangeScanPtr laserScan = observations->getObservationByClass<CObservation2DRangeScan>();
		if (!laserScan) continue;

		// Get Laser Pose (A) (CPose3D)
		CPose3D laserPose;
		laserScan->getSensorPose( laserPose );

		if (abs(laserPose.yaw())>DEG2RAD(90)) continue; // Only front lasers

		// Get 3D Point relative to the Laser coordinate Frame (P1) (CPoint3D)
		CPoint3D point;
		CSimplePointsMap mapa;
		mapa.insertionOptions.minDistBetweenLaserPoints = 0;
		observations->insertObservationsInto( &mapa );		// <- The map contains the pose of the points (P1)

		// Get the points into the map
		vector<float>			X, Y, Z;
		vector<float>::iterator	itX, itY, itZ;
		mapa.getAllPoints(X,Y,Z);

		unsigned int imgW = sImgs? sImgs->imageLeft.getWidth() : Img->image.getWidth();
		unsigned int imgH = sImgs? sImgs->imageLeft.getHeight() : Img->image.getHeight();

		//unsigned int			idx = 0;
		vector<float>			imgX,imgY;
		vector<float>::iterator	itImgX,itImgY;
		imgX.resize( X.size() );
		imgY.resize( Y.size() );

		CImage image;
		image = sImgs ? sImgs->imageLeft : Img->image;

		// Get pixels in the image:
		// Pimg = (kx,ky,k)^T = K(I|0)*P2
		// Main loop
		for( itX = X.begin(), itY = Y.begin(), itZ = Z.begin(), itImgX = imgX.begin(), itImgY = imgY.begin();
			 itX != X.end();
			 itX++, itY++, itZ++, itImgX++, itImgY++)
		{
			// Coordinates Transformation
			CPoint3D pLaser(*itX,*itY,*itZ);
			CPoint3D pCamera(pLaser-cameraPose);

			if( pCamera.z() > 0 )
			{
				*itImgX = - K(0,0)*((pCamera.x())/(pCamera.z())) + K(0,2);
				*itImgY = - K(1,1)*((pCamera.y())/(pCamera.z())) + K(1,2);

				if( *itImgX > 0 && *itImgX < imgW && *itImgY > 0 && *itImgY < imgH )
					image.filledRectangle(*itImgX-1,*itImgY-1,*itImgX+1,*itImgY+1,TColor(255,0,0));
			} // end if
		} // end for

		action.clear_unique();
		observations.clear_unique();

		wind.showImage(image);

		mrpt::system::sleep(50);
	}; // end for

	mrpt::system::pause();
}
예제 #6
0
/* ------------------------------------------------------------
						reloadFromRawlog
   ------------------------------------------------------------ */
void CRawlogTreeView::reloadFromRawlog( int hint_rawlog_items )
{
	// Recompute the total height of the scroll area:
	//  We also compute a list for each index with:
	//    - Pointer to data
	//	  - level in the hierarchy (0,1,2)
	// --------------------------------------------------------

	if (m_rawlog)
	{
		if (hint_rawlog_items<0)
				m_tree_nodes.reserve( m_rawlog->size()+100 );
		else	m_tree_nodes.reserve( hint_rawlog_items+100 );
	}

	// Create a "tree node" for each element in the rawlog:
	// ---------------------------------------------------------
	m_tree_nodes.clear();

	m_rawlog_start = INVALID_TIMESTAMP;
	m_rawlog_last  = INVALID_TIMESTAMP;

	// Root:
	m_tree_nodes.push_back( TNodeData() );
	TNodeData  &d = m_tree_nodes.back();
	d.level = 0;

//	CVectorDouble	tims;

	if (m_rawlog)
	{
		CRawlog::iterator end_it = m_rawlog->end();
		size_t		rawlog_index = 0;
		for (CRawlog::iterator it=m_rawlog->begin();it!=end_it;it++,rawlog_index++)
		{
			m_tree_nodes.push_back( TNodeData() );
			TNodeData  &d = m_tree_nodes.back();
			d.level = 1;
			d.data = (*it);
			d.index = rawlog_index;

			// For containers, go recursively:
			if ( (*it)->GetRuntimeClass()==CLASS_ID(CSensoryFrame))
			{
				CSensoryFramePtr	sf = CSensoryFramePtr( *it );
				for (CSensoryFrame::iterator o=sf->begin();o!=sf->end();++o)
				{
					m_tree_nodes.push_back( TNodeData() );
					TNodeData  &d = m_tree_nodes.back();
					d.level = 2;
					d.data = (*o);

                    if ((*o)->timestamp!=INVALID_TIMESTAMP)
                    {
                        m_rawlog_last = (*o)->timestamp;
                        if (m_rawlog_start == INVALID_TIMESTAMP)
                            m_rawlog_start = (*o)->timestamp;
                    }
				}
			}
			else
			if ( (*it)->GetRuntimeClass()==CLASS_ID(CActionCollection))
			{
				CActionCollectionPtr	acts = CActionCollectionPtr( *it );
				for (CActionCollection::iterator a=acts->begin();a!=acts->end();++a)
				{
					m_tree_nodes.push_back( TNodeData() );
					TNodeData  &d = m_tree_nodes.back();
					d.level = 2;
					d.data = (*a);

                    if ((*a)->timestamp!=INVALID_TIMESTAMP)
                    {
                        m_rawlog_last = (*a)->timestamp;
                        if (m_rawlog_start == INVALID_TIMESTAMP)
                            m_rawlog_start = (*a)->timestamp;
                    }
				}
			}
			else
			if ( (*it)->GetRuntimeClass()->derivedFrom( CLASS_ID(CObservation) ))
			{
			    CObservationPtr o = CObservationPtr(*it);
                if (o->timestamp!=INVALID_TIMESTAMP)
                {
                    m_rawlog_last = o->timestamp;
                    if (m_rawlog_start == INVALID_TIMESTAMP)
                        m_rawlog_start = o->timestamp;

					//tims.push_back( mrpt::system::timeDifference(m_rawlog_start, o->timestamp));
                }
			}
		}
	}

//	mrpt::system::vectorToTextFile(tims,"tims.txt");

	// Set new size:
	int ly = m_tree_nodes.size();
	SetScrollbars(ROW_HEIGHT, ROW_HEIGHT, 50, ly);
}
/** This is the common function for all operations over a rawlog file ("filter" a rawlog file into a new one) or over the loaded rawlog (depending on the user selection in the GUI).
  */
void CFormChangeSensorPositions::executeOperationOnRawlog( TRawlogFilter operation, const char *endMsg )
{
    WX_START_TRY

    int   			processMax;
    bool			isInMemory;
    CStream 		*in_fil=NULL,*out_fil=NULL;


	sensorPoseReadOK = false;
	camReadIsOk = false;

    if (rbLoaded->GetValue())
    {
        // APPLY TO rawlog in memory:
        isInMemory = true;

        processMax = (int)rawlog.size();
    }
    else
    {
        // APPLY TO rawlog files:
        isInMemory = false;

        if ( !txtInputFile->GetValue().size() )
            THROW_EXCEPTION("An input rawlog file must be selected")
            if ( !txtOutputFile->GetValue().size() )
                THROW_EXCEPTION("An output rawlog file must be selected")

                string   fileName_IN( txtInputFile->GetValue().mbc_str() );
        if (!mrpt::system::fileExists(fileName_IN) )
            THROW_EXCEPTION("Input file does not exist!")

            string   fileName_OUT( txtOutputFile->GetValue().mbc_str() );

        if (!fileName_OUT.compare(fileName_IN))
            THROW_EXCEPTION("Input and output files must be different!")

		in_fil = new CFileGZInputStream(fileName_IN);
		out_fil = new CFileGZOutputStream(fileName_OUT);

        processMax = (int)in_fil->getTotalBytesCount();
    }

    wxProgressDialog    progDia(
        wxT("Modifying rawlog"),
        wxT("Processing..."),
        processMax, // range
        this, // parent
        wxPD_CAN_ABORT |
        wxPD_APP_MODAL |
        wxPD_SMOOTH |
        wxPD_AUTO_HIDE |
        wxPD_ELAPSED_TIME |
        wxPD_ESTIMATED_TIME |
        wxPD_REMAINING_TIME);

    wxTheApp->Yield();  // Let the app. process messages

    unsigned int		countLoop = 0;
    bool                keepLoading=true;
    string              errorMsg;
    wxString			auxStr;

    // Apply changes:
    int 	changes = 0;
    wxBusyCursor    cursor;

    while ((( !isInMemory && keepLoading ) ||
            (  isInMemory && countLoop < rawlog.size() ))&& !sensorPoseReadOK && !camReadIsOk )
    {
        CSerializablePtr newObj;
        try
        {
            if (isInMemory)
            {
                newObj = rawlog.getAsGeneric(countLoop);
            }
            else
            {
                (*in_fil) >> newObj;
            }

            // Check type:
            if ( newObj->GetRuntimeClass() == CLASS_ID(CSensoryFrame))
            {
                // A sensory frame:
                CSensoryFramePtr sf(newObj);

                // Process & save:
				operation(NULL,sf.pointer(),changes );

				if (!isInMemory)  (*out_fil) << *sf.pointer();
            }
            else if ( newObj->GetRuntimeClass() == CLASS_ID(CActionCollection))
            {
                // This is an action:
                CActionCollectionPtr acts =CActionCollectionPtr( newObj );

                // Process & save:
				operation( (CActionCollection*)acts.pointer(),NULL,changes);

                if (!isInMemory)  (*out_fil) << *acts;
            }
			else if ( newObj->GetRuntimeClass()->derivedFrom(CLASS_ID(CObservation)))
            {
                // A sensory frame:
                CObservationPtr o(newObj);

				static CSensoryFrame sf;
				sf.clear();
				sf.insert(o);

                // Process & save:
				operation(NULL,&sf,changes );

				if (!isInMemory)  (*out_fil) << *o;
            }
            else
            {   // Unknown class:
                THROW_EXCEPTION(format("Unexpected class found in the file: '%s'",newObj->GetRuntimeClass()->className) );
            }
        }
        catch (exception &e)
        {
            errorMsg = e.what();
            keepLoading = false;
        }
        catch (...)
        {
            keepLoading = false;
        }

        // Step counter & update progress dialog
        if (countLoop++ % 300 == 0)
        {
            auxStr.sprintf(wxT("Processing... (%u objects processed)"),countLoop);
            int curProgr =  isInMemory ? countLoop : (int)in_fil->getPosition();
            if (!progDia.Update( curProgr , auxStr ))
                keepLoading = false;
            wxTheApp->Yield();  // Let the app. process messages
        }

        // Delete only if processing file
        if (newObj && !isInMemory)
        {
            newObj.clear();
        }

    } // end while keep loading

    progDia.Update( processMax );	// Close dialog.

	if (strlen(endMsg))
	{
		char tmpStr[1000];
		os::sprintf(tmpStr,sizeof(tmpStr),"%s %i\n\nEnd message:\n%s", endMsg, changes, errorMsg.c_str() );
		wxMessageBox( _U(tmpStr), _("Result:"), wxOK,this);
	}

    if (in_fil) delete in_fil;
    if (out_fil) delete out_fil;

    WX_END_TRY
}
예제 #8
0
// ------------------------------------------------------
//				MapBuilding_ICP
// ------------------------------------------------------
void MapBuilding_ICP()
{
	MRPT_TRY_START
	  

	CTicTac								tictac,tictacGlobal,tictac_JH;
	int									step = 0;
	std::string							str;
	CSensFrameProbSequence				finalMap;
	float								t_exec;
	COccupancyGridMap2D::TEntropyInfo	entropy;

	size_t						rawlogEntry = 0;
	CFileGZInputStream					rawlogFile( RAWLOG_FILE.c_str() );

	CFileGZOutputStream sensor_data;

	// ---------------------------------
	//		Constructor
	// ---------------------------------
	CMetricMapBuilderICP mapBuilder(
		&metricMapsOpts,
		insertionLinDistance,
		insertionAngDistance,
		&icpOptions
		);

	mapBuilder.ICP_options.matchAgainstTheGrid = matchAgainstTheGrid;

	// ---------------------------------
	//   CMetricMapBuilder::TOptions
	// ---------------------------------
	mapBuilder.options.verbose					= true;
	mapBuilder.options.enableMapUpdating		= true;
    mapBuilder.options.debugForceInsertion		= false;
	mapBuilder.options.insertImagesAlways		= false;

	// Prepare output directory:
	// --------------------------------
	deleteFilesInDirectory(OUT_DIR);
	createDirectory(OUT_DIR);

	// Open log files:
	// ----------------------------------
	CFileOutputStream  f_log(format("%s/log_times.txt",OUT_DIR));
	CFileOutputStream  f_path(format("%s/log_estimated_path.txt",OUT_DIR));
	CFileOutputStream  f_pathOdo(format("%s/log_odometry_path.txt",OUT_DIR));


	// Create 3D window if requested:
	CDisplayWindow3DPtr	win3D;
#if MRPT_HAS_WXWIDGETS
	if (SHOW_PROGRESS_3D_REAL_TIME)
	{
		win3D = CDisplayWindow3DPtr( new CDisplayWindow3D("ICP-SLAM @ MRPT C++ Library (C) 2004-2008", 600, 500) );
		win3D->setCameraZoom(20);
		win3D->setCameraAzimuthDeg(-45);
	}
#endif

	if(OBS_FROM_FILE == 0){
	  sensor_data.open("sensor_data.rawlog");
	  printf("Receive From Sensor\n");
	  initLaser();
	  printf("OK\n");
	}
	

	// ----------------------------------------------------------
	//						Map Building
	// ----------------------------------------------------------
	CActionCollectionPtr	action;
	CSensoryFramePtr		observations, temp_obs;
	CSensoryFramePtr obs_set;
	CPose2D					odoPose(0,0,0);

	CSimplePointsMap	oldMap, newMap;
	CICP					ICP;

	vector_float		accum_x, accum_y, accum_z;
	
	// ICP Setting
	ICP.options.ICP_algorithm = (TICPAlgorithm)ICP_method;

	ICP.options.maxIterations			= 40;
	ICP.options.thresholdAng =			0.15;
	ICP.options.thresholdDist			= 0.75f;
	ICP.options.ALFA					= 0.30f;
	ICP.options.smallestThresholdDist	= 0.10f;
	ICP.options.doRANSAC = false;
	ICP.options.dumpToConsole();
	//
	CObservationPtr obsSick = CObservationPtr(new CObservation2DRangeScan());
	CObservationPtr obsHokuyo = CObservationPtr(new CObservation2DRangeScan());
	CSimplePointsMap hokuyoMap;
	
	bool isFirstTime = true;
	bool loop = true;

    /*
		  cout << index << "frame, Input the any key to continue" << endl;
		  getc(stdin);
		  fflush(stdin);
    */

	tictacGlobal.Tic();
	while(loop)
	{
      /*
		if(BREAK){
		  cout << index << "frame, Input the any key to continue" << endl;
		  getc(stdin);
		  fflush(stdin);
		}else{
		  if(os::kbhit())
			loop = true;
		}
      */

        if(os::kbhit())
          loop = true;
        

		if(DELAY) {
		  sleep(15);
		}
	  

		// Load action/observation pair from the rawlog:
		// --------------------------------------------------

		if(OBS_FROM_FILE == 1) {
		  if (! CRawlog::readActionObservationPair( rawlogFile, action, temp_obs, rawlogEntry) )
			break; // file EOF

		  obsSick = temp_obs->getObservationByIndex(0);
		  obsHokuyo = temp_obs->getObservationByIndex(1);
		  
		  observations = CSensoryFramePtr(new CSensoryFrame());		  
		  observations->insert((CObservationPtr)obsSick);

		  hokuyoMap.clear();
		  hokuyoMap.insertObservation(obsHokuyo.pointer());
		  
		}else{
		  rawlogEntry = rawlogEntry+2;

		  tictac.Tic();

		  obsSick = CObservationPtr(new CObservation2DRangeScan());
		  obsHokuyo = CObservationPtr(new CObservation2DRangeScan());

		  if(!receiveDataFromSensor((CObservation2DRangeScan*)obsSick.pointer(),SICK)){
			cout << " Error in receive sensor data" << endl;
			return;
		  }

		  if(!receiveDataFromSensor((CObservation2DRangeScan*)obsHokuyo.pointer(),HOKUYO)){
			cout << " Error in receive sensor data" << endl;

			return;
		  }
		  
		  cout << "Time to receive data : " << tictac.Tac()*1000.0f << endl;

		  obsSick->timestamp = mrpt::system::now();
		  obsSick->setSensorPose(CPose3D(0,0,0,DEG2RAD(0),DEG2RAD(0),DEG2RAD(0)));
		  ((CObservation2DRangeScan*)obsSick.pointer())->rightToLeft = true;
		  ((CObservation2DRangeScan*)obsSick.pointer())->stdError = 0.003f;

		  obsHokuyo->timestamp = mrpt::system::now();
		  obsHokuyo->setSensorPose(CPose3D(0,0,0.4,DEG2RAD(0),DEG2RAD(-90),DEG2RAD(0)));
		  ((CObservation2DRangeScan*)obsHokuyo.pointer())->rightToLeft = true;
		  ((CObservation2DRangeScan*)obsHokuyo.pointer())->stdError = 0.003f;
		  
		  cout << "rawlogEntry : " << rawlogEntry << endl;

		  CActionRobotMovement2D myAction;

		  newMap.clear();
		  obsSick.pointer()->insertObservationInto(&newMap);

		  if(!isFirstTime){

			static float					runningTime;
			static CICP::TReturnInfo		info;
			static CPose2D initial(0,0,0);

			CPosePDFPtr ICPPdf = ICP.AlignPDF(&oldMap, &newMap, initial, &runningTime, (void*)&info);
			CPose2D		estMean;
			CMatrixDouble33		estCov;
			ICPPdf->getCovarianceAndMean(estCov, estMean);
			printf("ICP run in %.02fms, %d iterations (%.02fms/iter), %.01f%% goodness\n -> ", runningTime*1000, info.nIterations, runningTime*1000.0f/info.nIterations, info.goodness*100 );
			cout << "ICP Odometry : " << ICPPdf->getMeanVal() << endl;

			myAction.estimationMethod = CActionRobotMovement2D::emScan2DMatching;
			myAction.poseChange = CPosePDFPtr( new CPosePDFGaussian(estMean, estCov));
		  }else{
			isFirstTime = false;
		  }

		  oldMap.clear();
		  oldMap.copyFrom(newMap);

		  observations = CSensoryFramePtr(new CSensoryFrame());
		  action = CActionCollectionPtr(new CActionCollection());		  

		  observations->insert((CObservationPtr)obsSick);

		  obs_set = CSensoryFramePtr(new CSensoryFrame());

		  obs_set->insert((CObservationPtr)obsSick);
		  obs_set->insert((CObservationPtr)obsHokuyo);
		  
		  action->insert(myAction);

		  sensor_data << action << obs_set;

		  hokuyoMap.clear();
		  hokuyoMap.insertObservation(obsHokuyo.pointer());

		}


		if (rawlogEntry>=rawlog_offset)
		{
			// Update odometry:
			{
				CActionRobotMovement2DPtr act= action->getBestMovementEstimation();
				if (act)
					odoPose = odoPose + act->poseChange->getMeanVal();
			}

			// Execute:
			// ----------------------------------------
			tictac.Tic();
				mapBuilder.processActionObservation( *action, *observations );
			t_exec = tictac.Tac();
			printf("Map building executed in %.03fms\n", 1000.0f*t_exec );


			// Info log:
			// -----------
			f_log.printf("%f %i\n",1000.0f*t_exec,mapBuilder.getCurrentlyBuiltMapSize() );

			const CMultiMetricMap* mostLikMap =  mapBuilder.getCurrentlyBuiltMetricMap();

			if (0==(step % LOG_FREQUENCY))
			{
				// Pose log:
				// -------------
				if (SAVE_POSE_LOG)
				{
					printf("Saving pose log information...");
					mapBuilder.getCurrentPoseEstimation()->saveToTextFile( format("%s/mapbuild_posepdf_%03u.txt",OUT_DIR,step) );
					printf("Ok\n");
				}
			}


			// Save a 3D scene view of the mapping process:
			if (0==(step % LOG_FREQUENCY) || (SAVE_3D_SCENE || win3D.present()))
			{
                CPose3D robotPose;
				mapBuilder.getCurrentPoseEstimation()->getMean(robotPose);

				COpenGLScenePtr		scene = COpenGLScene::Create();

                COpenGLViewportPtr view=scene->getViewport("main");
                ASSERT_(view);

                COpenGLViewportPtr view_map = scene->createViewport("mini-map");
                view_map->setBorderSize(2);
                view_map->setViewportPosition(0.01,0.01,0.35,0.35);
                view_map->setTransparent(false);

				{
					mrpt::opengl::CCamera &cam = view_map->getCamera();
					cam.setAzimuthDegrees(-90);
					cam.setElevationDegrees(90);
					cam.setPointingAt(robotPose);
					cam.setZoomDistance(20);
					cam.setOrthogonal();
				}

				// The ground:
				mrpt::opengl::CGridPlaneXYPtr groundPlane = mrpt::opengl::CGridPlaneXY::Create(-200,200,-200,200,0,5);
				groundPlane->setColor(0.4,0.4,0.4);
				view->insert( groundPlane );
				view_map->insert( CRenderizablePtr( groundPlane) ); // A copy

				// The camera pointing to the current robot pose:
				if (CAMERA_3DSCENE_FOLLOWS_ROBOT)
				{
				    scene->enableFollowCamera(true);

					mrpt::opengl::CCamera &cam = view_map->getCamera();
					cam.setAzimuthDegrees(-45);
					cam.setElevationDegrees(45);
					cam.setPointingAt(robotPose);
				}

				// The maps:
				{
					opengl::CSetOfObjectsPtr obj = opengl::CSetOfObjects::Create();
					mostLikMap->getAs3DObject( obj );
					view->insert(obj);

					// Only the point map:
					opengl::CSetOfObjectsPtr ptsMap = opengl::CSetOfObjects::Create();
					if (mostLikMap->m_pointsMaps.size())
					{
                        mostLikMap->m_pointsMaps[0]->getAs3DObject(ptsMap);
                        view_map->insert( ptsMap );
					}
				}

				// Draw the robot path:
				CPose3DPDFPtr posePDF =  mapBuilder.getCurrentPoseEstimation();
				CPose3D  curRobotPose;
				posePDF->getMean(curRobotPose);
				{
					opengl::CSetOfObjectsPtr obj = opengl::stock_objects::RobotPioneer();
					obj->setPose( curRobotPose );
					view->insert(obj);
				}
				{
					opengl::CSetOfObjectsPtr obj = opengl::stock_objects::RobotPioneer();
					obj->setPose( curRobotPose );
					view_map->insert( obj );
				}


				// Draw Hokuyo total data
				{
				  CRenderizablePtr hokuyoRender_t = scene->getByName("hokuyo_total");

				  if(!hokuyoRender_t){
					hokuyoRender_t = CPointCloud::Create();
					hokuyoRender_t->setName("hokuyo_total");
					hokuyoRender_t->setColor(0,0,1);
					hokuyoRender_t->setPose( CPose3D(0,0,0) );
					getAs<CPointCloud>(hokuyoRender_t)->setPointSize(3);
					scene->insert( hokuyoRender_t);					
				  }

				  for(size_t i =0 ; i < accum_x.size(); i++){
					getAs<CPointCloud>(hokuyoRender_t)->insertPoint(accum_x[i], accum_y[i], accum_z[i]);					
				  }
				  cout << "accum_x size : " << accum_x.size() << endl;


				}
				
				// Draw Hokuyo Current data plate
				{
				  CRenderizablePtr hokuyoRender = scene->getByName("hokuyo_cur");
				  hokuyoRender = CPointCloud::Create();
				  hokuyoRender->setName("hokuyo_cur");
				  hokuyoRender->setColor(0,1,0);
				  hokuyoRender->setPose( curRobotPose );
				  getAs<CPointCloud>(hokuyoRender)->setPointSize(0.1);
				  getAs<CPointCloud>(hokuyoRender)->loadFromPointsMap(&hokuyoMap);				  
				  scene->insert( hokuyoRender);

				  vector_float cur_x = getAs<CPointCloud>(hokuyoRender)->getArrayX();
				  vector_float cur_y = getAs<CPointCloud>(hokuyoRender)->getArrayY();
				  vector_float cur_z = getAs<CPointCloud>(hokuyoRender)->getArrayZ();


				  //				  cout << "current pose : " << curRobotPose << endl;				  
				  for(size_t i =0 ; i < cur_x.size(); i++){
					/*
					float rotate_x = cur_x[i]+ curRobotPose.y()*sin(curRobotPose.yaw()*3.14/180.0);
					float rotate_y = cur_y[i]+ curRobotPose.y()*cos(curRobotPose.yaw()*3.14/180.0);
					*/
					float rotate_x = curRobotPose.x() + cur_y[i]*sin(-curRobotPose.yaw());
					float rotate_y = curRobotPose.y() + cur_y[i]*cos(-curRobotPose.yaw());
					//					printf("cur_x, cur_y, cur_z : %f %f %f \n", rotate_x,rotate_y, cur_z[i]);

					accum_x.push_back(rotate_x);
					accum_y.push_back(rotate_y);
					accum_z.push_back(cur_z[i]);
				  }
				}

				// Save as file:
				if (0==(step % LOG_FREQUENCY) && SAVE_3D_SCENE)
				{
					CFileGZOutputStream	f( format( "%s/buildingmap_%05u.3Dscene",OUT_DIR,step ));
					f << *scene;
				}

				// Show 3D?
				if (win3D)
				{
					opengl::COpenGLScenePtr &ptrScene = win3D->get3DSceneAndLock();
					ptrScene = scene;

					win3D->unlockAccess3DScene();

					// Move camera:
					win3D->setCameraPointingToPoint( curRobotPose.x(),curRobotPose.y(),curRobotPose.z() );

					// Update:
					win3D->forceRepaint();

					sleep( SHOW_PROGRESS_3D_REAL_TIME_DELAY_MS );
				}
			}


			// Save the memory usage:
			// ------------------------------------------------------------------
			{
				printf("Saving memory usage...");
				unsigned long	memUsage = getMemoryUsage();
				FILE		*f=os::fopen( format("%s/log_MemoryUsage.txt",OUT_DIR).c_str() ,"at");
				if (f)
				{
					os::fprintf(f,"%u\t%lu\n",step,memUsage);
					os::fclose(f);
				}
				printf("Ok! (%.04fMb)\n", ((float)memUsage)/(1024*1024) );
			}

			// Save the robot estimated pose for each step:
			f_path.printf("%i %f %f %f\n",
				step,
				mapBuilder.getCurrentPoseEstimation()->getMeanVal().x(),
				mapBuilder.getCurrentPoseEstimation()->getMeanVal().y(),
				mapBuilder.getCurrentPoseEstimation()->getMeanVal().yaw() );


			f_pathOdo.printf("%i %f %f %f\n",step,odoPose.x(),odoPose.y(),odoPose.phi());

		} // end of if "rawlog_offset"...

		step++;
		printf("\n---------------- STEP %u | RAWLOG ENTRY %u ----------------\n",step, (unsigned)rawlogEntry);

		// Free memory:
		action.clear_unique();
		observations.clear_unique();
	};

	printf("\n---------------- END!! (total time: %.03f sec) ----------------\n",tictacGlobal.Tac());

	//	hokuyo.turnOff();	
	sick.stop();

	

	// Save map:
	mapBuilder.getCurrentlyBuiltMap(finalMap);

	str = format("%s/_finalmap_.simplemap",OUT_DIR);
	printf("Dumping final map in binary format to: %s\n", str.c_str() );
	mapBuilder.saveCurrentMapToFile(str);

	CMultiMetricMap  *finalPointsMap = mapBuilder.getCurrentlyBuiltMetricMap();
	str = format("%s/_finalmaps_.txt",OUT_DIR);
	printf("Dumping final metric maps to %s_XXX\n", str.c_str() );
	finalPointsMap->saveMetricMapRepresentationToFile( str );

	if (win3D)
		win3D->waitForKey();

	MRPT_TRY_END
}
예제 #9
0
void xRawLogViewerFrame::OnMenuConvertObservationOnly(wxCommandEvent& event)
{
	WX_START_TRY

	wxMessageBox( _("Select the rawlog file to convert...") );

	string 	str;
	if ( !AskForOpenRawlog( str ) ) return;

	wxMessageBox( _("Select the target file where to save the new rawlog.") );
	string 	filToSave;
	if ( !AskForSaveRawlog( filToSave ) ) return;

	wxBusyCursor        waitCursor;
	CFileGZInputStream	fil(str);
	unsigned int        filSize = (unsigned int)fil.getTotalBytesCount();

	CFileGZOutputStream	f_out(filToSave);

	wxString            auxStr;
	wxProgressDialog    progDia(
		wxT("Progress"),
		wxT("Parsing file..."),
		filSize, // range
		this, // parent
		wxPD_CAN_ABORT |
		wxPD_APP_MODAL |
		wxPD_SMOOTH |
		wxPD_AUTO_HIDE |
		wxPD_ELAPSED_TIME |
		wxPD_ESTIMATED_TIME |
		wxPD_REMAINING_TIME);

	wxTheApp->Yield();  // Let the app. process messages

	unsigned int        countLoop = 0;
	bool                keepLoading=true;
	string              errorMsg;

	CPose2D				odometry_accum;

	// We'll save here all the individual observations ordered in time:
	TListTimeAndObservations	time_ordered_list_observation;

	mrpt::system::TTimeStamp	lastValidObsTime = INVALID_TIMESTAMP;

	while (keepLoading)
	{
		if (countLoop++ % 5 == 0)
		{
			auxStr.sprintf(wxT("Parsing file... %u objects"),countLoop );
			if (!progDia.Update( (int)fil.getPosition(), auxStr ))
				keepLoading = false;
			wxTheApp->Yield();  // Let the app. process messages
		}

		try
		{
			CSerializablePtr newObj;
			fil >> newObj;

			// Check type:
			if ( newObj->GetRuntimeClass() == CLASS_ID(CSensoryFrame) )
			{
				CSensoryFramePtr SF(newObj);
				for (CSensoryFrame::iterator it=SF->begin();it!=SF->end();++it)
				{
					time_ordered_list_observation.insert( TTimeObservationPair( (*it)->timestamp, (*it) ));
					lastValidObsTime = (*it)->timestamp;
				}
			}
			else
				if ( newObj->GetRuntimeClass() == CLASS_ID(CActionCollection) )
				{
					// Replace "odometry action" with "odometry observation":
					CActionCollectionPtr	acts = CActionCollectionPtr( newObj );
					// Get odometry:
					CActionRobotMovement2DPtr actOdom = acts->getBestMovementEstimation();
					if (actOdom)
					{
						odometry_accum = odometry_accum + actOdom->poseChange->getMeanVal();

						// Generate "odometry obs":
						CObservationOdometryPtr  newO = CObservationOdometry::Create();
						newO->sensorLabel = "odometry";
						newO->timestamp   = actOdom->timestamp!=INVALID_TIMESTAMP ?  actOdom->timestamp : lastValidObsTime;
						newO->odometry    = odometry_accum;

						time_ordered_list_observation.insert( TTimeObservationPair( newO->timestamp, newO ));
					}
				}
				else
				if ( newObj->GetRuntimeClass()->derivedFrom( CLASS_ID(CObservation) ) )
				{
					CObservationPtr o = CObservationPtr( newObj );
					time_ordered_list_observation.insert( TTimeObservationPair( o->timestamp, o ));
				}


			// Dump to the new file: Only the oldest one:
			// --------------------------------------------------
			if (time_ordered_list_observation.size()>30)
			{
				// Save a few of the oldest and continue:
				for (unsigned i=0;i<15;i++)
				{
					f_out << *(time_ordered_list_observation.begin()->second);
					time_ordered_list_observation.erase( time_ordered_list_observation.begin() );
				}
			}

			// Free memory:
			newObj.clear_unique();
		}
		catch (exception &e)
		{
			errorMsg = e.what();
			keepLoading = false;
		}
		catch (...)
		{
			keepLoading = false;
		}
	} // end while keep loading

	// Save the rest to the out file:
	while (!time_ordered_list_observation.empty())
	{
		f_out << *(time_ordered_list_observation.begin()->second);
		time_ordered_list_observation.erase( time_ordered_list_observation.begin() );
	}

	progDia.Update( filSize );

	// Set error msg:
	wxMessageBox(_("Done"));

	WX_END_TRY
}
예제 #10
0
void xRawLogViewerFrame::OnMenuLossLessDecFILE(wxCommandEvent& event)
{
	WX_START_TRY

	string 	filToOpen;
	if ( !AskForOpenRawlog( filToOpen ) ) return;

	wxString strDecimation = wxGetTextFromUser(
								 _("The number of observations will be decimated (only 1 out of M will be kept). Enter the decimation ratio M:"),
								 _("Decimation"),
								 _("1") );
	long	DECIMATE_RATIO;
	strDecimation.ToLong( &DECIMATE_RATIO );

	ASSERT_(DECIMATE_RATIO>=1)

	string filToSave;
	AskForSaveRawlog(filToSave);

	CFileGZInputStream	fil(filToOpen);
	CFileGZOutputStream	f_out(filToSave);

	wxBusyCursor        waitCursor;
	unsigned int        filSize = (unsigned int)fil.getTotalBytesCount();

	wxString            auxStr;
	wxProgressDialog    progDia(
		wxT("Progress"),
		wxT("Parsing file..."),
		filSize, // range
		this, // parent
		wxPD_CAN_ABORT |
		wxPD_APP_MODAL |
		wxPD_SMOOTH |
		wxPD_AUTO_HIDE |
		wxPD_ELAPSED_TIME |
		wxPD_ESTIMATED_TIME |
		wxPD_REMAINING_TIME);

	wxTheApp->Yield();  // Let the app. process messages

	unsigned int        countLoop = 0;
	int					entryIndex = 0;
	bool                keepLoading=true;
	string              errorMsg;

	// ------------------------------------------------------------------------------
	// METHOD TO BE MEMORY EFFICIENT:
	//  To free the memory of the current rawlog entries as we create the new one,
	//  then call "clearWithoutDelete" at the end.
	// ------------------------------------------------------------------------------
	CSensoryFramePtr		accum_sf;
	CActionRobotMovement2D::TMotionModelOptions	odometryOptions;
	bool					cummMovementInit = false;
	long 					SF_counter = 0;

	// Reset cummulative pose change:
	CPose2D		accumMovement(0,0,0);

	TTimeStamp	timestamp_lastAction = INVALID_TIMESTAMP;


	while (keepLoading)
	{
		if (countLoop++ % 100 == 0)
		{
			auxStr.sprintf(wxT("Parsing file... %u objects"),entryIndex );
			if (!progDia.Update( (int)fil.getPosition(), auxStr ))
				keepLoading = false;
			wxTheApp->Yield();  // Let the app. process messages
		}

		CSerializablePtr newObj;
		try
		{
			fil >> newObj;
			entryIndex++;

			// Check type:
			if ( newObj->GetRuntimeClass() == CLASS_ID(CSensoryFrame) )
			{
				// Decimate Observations
				// ---------------------------

				// Add observations to the accum. SF:
				if (!accum_sf)
					accum_sf = CSensoryFrame::Create();

				// Copy pointers to observations only (fast):
				accum_sf->moveFrom( *CSensoryFramePtr(newObj) );

				if ( ++SF_counter >= DECIMATE_RATIO )
				{
					SF_counter = 0;

					// INSERT OBSERVATIONS:
					f_out << *accum_sf;
					accum_sf.clear_unique();

					// INSERT ACTIONS:
					CActionCollectionPtr actsCol = CActionCollection::Create();
					if (cummMovementInit)
					{
						CActionRobotMovement2D	cummMovement;
						cummMovement.computeFromOdometry(accumMovement, odometryOptions );
						cummMovement.timestamp = timestamp_lastAction;
						actsCol->insert( cummMovement );
						// Reset odometry accumulation:
						accumMovement = CPose2D(0,0,0);
					}
					f_out << *actsCol;
					actsCol.clear_unique();
				}
			}
			else if ( newObj->GetRuntimeClass() == CLASS_ID(CActionCollection) )
			{
				// Accumulate Actions
				// ----------------------
				CActionCollectionPtr curActs = CActionCollectionPtr( newObj );
				CActionRobotMovement2DPtr mov = curActs->getBestMovementEstimation();
				if (mov)
				{
					timestamp_lastAction = mov->timestamp;
					CPose2D  	incrPose = mov->poseChange->getMeanVal();

					// If we have previous observations, shift them according to this new increment:
					if (accum_sf)
					{
						CPose3D	inv_incrPose3D(  CPose3D(0,0,0) - CPose3D(incrPose) );

						for (CSensoryFrame::iterator it=accum_sf->begin();it!=accum_sf->end();++it)
						{
							CPose3D		tmpPose;

							(*it)->getSensorPose(tmpPose);
							tmpPose = inv_incrPose3D + tmpPose;
							(*it)->setSensorPose(tmpPose);
						}
					}

					// Accumulate from odometry:
					accumMovement = accumMovement + incrPose;

					// Copy the probabilistic options from the first entry we find:
					if (!cummMovementInit)
					{
						odometryOptions = mov->motionModelConfiguration;
						cummMovementInit = true;
					}
				}
			}
			else
			{
				// Unknown class:
				THROW_EXCEPTION("Unknown class found in the file!");
			}

		}
		catch (exception &e)
		{
			errorMsg = e.what();
			keepLoading = false;
		}
		catch (...)
		{
			keepLoading = false;
		}
	} // end while keep loading

	progDia.Update( filSize );


	wxTheApp->Yield();  // Let the app. process messages



	WX_END_TRY

}
예제 #11
0
void xRawLogViewerFrame::OnMenuLossLessDecimate(wxCommandEvent& event)
{
	WX_START_TRY

	CRawlog	newRawLog;
	newRawLog.setCommentText( rawlog.getCommentText() );

	wxString strDecimation = wxGetTextFromUser(
								 _("The number of observations will be decimated (only 1 out of M will be kept). Enter the decimation ratio M:"),
								 _("Decimation"),
								 _("1") );
	long	DECIMATE_RATIO;
	strDecimation.ToLong( &DECIMATE_RATIO );

	ASSERT_(DECIMATE_RATIO>=1)

	wxBusyCursor	busyCursor;
	wxTheApp->Yield();  // Let the app. process messages

	size_t	i, N = rawlog.size();

	// ------------------------------------------------------------------------------
	// METHOD TO BE MEMORY EFFICIENT:
	//  To free the memory of the current rawlog entries as we create the new one,
	//  then call "clearWithoutDelete" at the end.
	// ------------------------------------------------------------------------------
	CSensoryFramePtr		accum_sf;
	CActionRobotMovement2D::TMotionModelOptions	odometryOptions;
	bool					cummMovementInit = false;
	long 					SF_counter = 0;

	// Reset cummulative pose change:
	CPose2D		accumMovement(0,0,0);

	// For each entry:
	for (i=0;i<N;i++)
	{
		CSerializablePtr obj = rawlog.getAsGeneric(i);

		if (rawlog.getType(i)==CRawlog::etActionCollection)
		{
			// Accumulate Actions
			// ----------------------
			CActionCollectionPtr curActs = CActionCollectionPtr (  obj );
			CActionRobotMovement2DPtr mov = curActs->getBestMovementEstimation();
			if (mov)
			{
				CPose2D  	incrPose = mov->poseChange->getMeanVal();

				// If we have previous observations, shift them according to this new increment:
				if (accum_sf)
				{
					CPose3D	inv_incrPose3D(  CPose3D(0,0,0) - CPose3D(incrPose) );

					for (CSensoryFrame::iterator it=accum_sf->begin();it!=accum_sf->end();++it)
					{
						CPose3D		tmpPose;

						(*it)->getSensorPose(tmpPose);
						tmpPose = inv_incrPose3D + tmpPose;
						(*it)->setSensorPose(tmpPose);
					}
				}

				// Accumulate from odometry:
				accumMovement = accumMovement + incrPose;

				// Copy the probabilistic options from the first entry we find:
				if (!cummMovementInit)
				{
					odometryOptions = mov->motionModelConfiguration;
					cummMovementInit = true;
				}
			}
		}
		else if (rawlog.getType(i)==CRawlog::etSensoryFrame)
		{
			// Decimate Observations
			// ---------------------------

			// Add observations to the accum. SF:
			if (!accum_sf)
				accum_sf = CSensoryFrame::Create();

			// Copy pointers to observations only (fast):
			accum_sf->moveFrom( *CSensoryFramePtr(obj) );

			if ( ++SF_counter >= DECIMATE_RATIO )
			{
				SF_counter = 0;

				// INSERT OBSERVATIONS:
				newRawLog.addObservationsMemoryReference( accum_sf );
				accum_sf.clear_unique();

				// INSERT ACTIONS:
				CActionCollection	actsCol;
				if (cummMovementInit)
				{
					CActionRobotMovement2D	cummMovement;
					cummMovement.computeFromOdometry(accumMovement, odometryOptions );
					actsCol.insert( cummMovement );
					// Reset odometry accumulation:
					accumMovement = CPose2D(0,0,0);
				}
				newRawLog.addActions( actsCol );
			}
		} else
			THROW_EXCEPTION("This operation implemented for SF-based rawlogs only.");

		// Delete object
		obj.clear_unique();

	} // end for i each entry

	// Clear the list only (objects already deleted)
	rawlog.clearWithoutDelete();

	// Copy as new log:
	rawlog = newRawLog;

	rebuildTreeView();

	WX_END_TRY
}
예제 #12
0
파일: perf-icp.cpp 프로젝트: Insomnia-/mrpt
// ------------------------------------------------------
//				Benchmark: A whole ICP-SLAM run
// ------------------------------------------------------
double icp_test_1(int a1, int a2)
{
#ifdef MRPT_DATASET_DIR
	const string rawlog_file = MRPT_DATASET_DIR  "/2006-01ENE-21-SENA_Telecom Faculty_one_loop_only.rawlog";
	if (!mrpt::system::fileExists(rawlog_file))
		return 1;

	CTicTac	 tictac;

	int			step = 0;
	size_t		rawlogEntry = 0;
	CFileGZInputStream	rawlogFile( rawlog_file );

	TSetOfMetricMapInitializers  metricMapsOpts;

	const bool use_grid = a1!=0;

	if (use_grid)
	{
		COccupancyGridMap2D::TMapDefinition def;
		def.resolution = 0.05;
		metricMapsOpts.push_back( def );
	}
	else
	{
		CSimplePointsMap::TMapDefinition def;
		def.insertionOpts.minDistBetweenLaserPoints = 0.03;
		metricMapsOpts.push_back( def );
	}

	double insertionLinDistance = 0.75;
	double insertionAngDistance = DEG2RAD(30);

	CICP::TConfigParams  icpOptions;

	icpOptions.maxIterations = 40;


	// ---------------------------------
	//		Constructor
	// ---------------------------------
	CMetricMapBuilderICP mapBuilder;

	mapBuilder.ICP_options.mapInitializers = metricMapsOpts;

	mapBuilder.ICP_options.insertionLinDistance = insertionLinDistance;
	mapBuilder.ICP_options.insertionAngDistance = insertionAngDistance;

	mapBuilder.ICP_options.matchAgainstTheGrid = use_grid ;
	mapBuilder.ICP_params = icpOptions;

	// Start with an empty map:
	mapBuilder.initialize( CSimpleMap() );

	// ---------------------------------
	//   CMetricMapBuilder::TOptions
	// ---------------------------------
	mapBuilder.options.verbose					= false;
	mapBuilder.options.enableMapUpdating		= true;

	// ----------------------------------------------------------
	//						Map Building
	// ----------------------------------------------------------
	CActionCollectionPtr	action;
	CSensoryFramePtr		observations;

	for (;;)
	{
		// Load action/observation pair from the rawlog:
		// --------------------------------------------------
		if (! CRawlog::readActionObservationPair( rawlogFile, action, observations, rawlogEntry) )
			break; // file EOF

		// Execute:
		mapBuilder.processActionObservation( *action, *observations );

		step++;
//		printf("\n---------------- STEP %u | RAWLOG ENTRY %u ----------------\n",step, (unsigned)rawlogEntry);

		// Free memory:
		action.clear_unique();
		observations.clear_unique();
	}

#if 0
	mapBuilder.getCurrentlyBuiltMetricMap()->saveMetricMapRepresentationToFile( format("icp_%i_result",a1) );
#endif

	if (!step) step++;

	return tictac.Tac()/step;
#else
	return 1;
#endif
}
예제 #13
0
/*---------------------------------------------------------------
						process
  ---------------------------------------------------------------*/
void  CDetectorDoorCrossing::process(
		CActionRobotMovement2D	&in_poseChange,
		CSensoryFrame			&in_sf,
		TDoorCrossingOutParams	&out_estimation
		)
{
	// Variables for generic use:
	size_t				i;

	out_estimation.cumulativeTurning = 0;

	MRPT_START

	// 1) Add new pair to the list:
	// -----------------------------------------
	lastObs.addAction( in_poseChange );
	lastObs.addObservations( in_sf );

	// 2) Remove oldest pair:
	// -----------------------------------------
	ASSERT_( options.windowSize > 1 );
	ASSERT_( (lastObs.size() % 2) == 0 );	// Assure even size

	while (lastObs.size()>options.windowSize*2)
	{
		lastObs.remove(0);
		lastObs.remove(0);
	}

	if ( lastObs.size() < options.windowSize * 2 )
	{
		// Not enought old data yet:
		out_estimation.enoughtInformation = false;
		return;
	}

	// 3) Build an occupancy grid map with observations
	// -------------------------------------------------
	CPose2D					p, pos;

	TSetOfMetricMapInitializers			mapInitializer;

	{
		CSimplePointsMap::TMapDefinition def;
		mapInitializer.push_back( def );
	}
	{
		COccupancyGridMap2D::TMapDefinition def;
		def.resolution = options.gridResolution;
		mapInitializer.push_back( def );
	}

	CMultiMetricMap			auxMap( &mapInitializer );

	for (i=0;i<options.windowSize;i++)
	{
		CActionCollectionPtr	acts = lastObs.getAsAction( i*2+0 );
		CActionPtr				act = acts->get(0);

		ASSERT_( act->GetRuntimeClass()->derivedFrom( CLASS_ID( CActionRobotMovement2D ) ) )
		CActionRobotMovement2DPtr action = CActionRobotMovement2DPtr( act );

		action->poseChange->getMean(pos);

		out_estimation.cumulativeTurning+= fabs(pos.phi());

		// Get the cumulative pose for the current observation:
		p = p + pos;

		// Add SF to the grid map:
		CSensoryFramePtr	sf = lastObs.getAsObservations( i*2+1 );
		CPose3D				pose3D(p);
		sf->insertObservationsInto( &auxMap, &pose3D );
	}

	// 4) Compute the information differece between this
	//      "map patch" and the previous one:
	// -------------------------------------------------------
	auxMap.m_gridMaps[0]->computeEntropy( entropy );

	if (!lastEntropyValid)
	{
		out_estimation.enoughtInformation = false;
	}
	else
	{
		// 5) Fill output data
		// ---------------------------------
		out_estimation.enoughtInformation = true;


		out_estimation.informationGain = entropy.I - lastEntropy.I;
		out_estimation.pointsMap.copyFrom( *auxMap.m_pointsMaps[0] );
	}


	// For next iterations:
	lastEntropy = entropy;
	lastEntropyValid = true;

	MRPT_END

}
예제 #14
0
파일: CRawlog.cpp 프로젝트: KMiyawaki/mrpt
void  CRawlog::addAction( CAction &action )
{
	CActionCollectionPtr temp = CActionCollection::Create();
	temp->insert( action );
	m_seqOfActObs.push_back( temp );
}
예제 #15
0
// ------------------------------------------------------
//				MapBuilding_ICP
//  override_rawlog_file: If not empty, use that rawlog
//  instead of that in the config file.
// ------------------------------------------------------
void MapBuilding_ICP(const string &INI_FILENAME, const string &override_rawlog_file)
{
	MRPT_START

	CConfigFile				iniFile(INI_FILENAME);

	// ------------------------------------------
	//			Load config from file:
	// ------------------------------------------
	const string RAWLOG_FILE			 = !override_rawlog_file.empty() ? override_rawlog_file : iniFile.read_string("MappingApplication","rawlog_file","",  /*Force existence:*/ true);
	const unsigned int rawlog_offset		 = iniFile.read_int("MappingApplication","rawlog_offset",0,  /*Force existence:*/ true);
	const string OUT_DIR_STD			 = iniFile.read_string("MappingApplication","logOutput_dir","log_out",  /*Force existence:*/ true);
	const int LOG_FREQUENCY		 = iniFile.read_int("MappingApplication","LOG_FREQUENCY",5,  /*Force existence:*/ true);
	const bool  SAVE_POSE_LOG		 = iniFile.read_bool("MappingApplication","SAVE_POSE_LOG", false,  /*Force existence:*/ true);
	const bool  SAVE_3D_SCENE        = iniFile.read_bool("MappingApplication","SAVE_3D_SCENE", false,  /*Force existence:*/ true);
	const bool  CAMERA_3DSCENE_FOLLOWS_ROBOT = iniFile.read_bool("MappingApplication","CAMERA_3DSCENE_FOLLOWS_ROBOT", true,  /*Force existence:*/ true);

	bool 	SHOW_PROGRESS_3D_REAL_TIME = false;
	int		SHOW_PROGRESS_3D_REAL_TIME_DELAY_MS = 0;

	MRPT_LOAD_CONFIG_VAR( SHOW_PROGRESS_3D_REAL_TIME, bool,  iniFile, "MappingApplication");
	MRPT_LOAD_CONFIG_VAR( SHOW_PROGRESS_3D_REAL_TIME_DELAY_MS, int, iniFile, "MappingApplication");

	const char* OUT_DIR = OUT_DIR_STD.c_str();

	// ------------------------------------
	//		Constructor of ICP-SLAM object
	// ------------------------------------
	CMetricMapBuilderICP mapBuilder;

	mapBuilder.ICP_options.loadFromConfigFile( iniFile, "MappingApplication");
	mapBuilder.ICP_params.loadFromConfigFile ( iniFile, "ICP");

	// Construct the maps with the loaded configuration.
	mapBuilder.initialize();

	// ---------------------------------
	//   CMetricMapBuilder::TOptions
	// ---------------------------------
	mapBuilder.options.verbose = true;
    mapBuilder.options.alwaysInsertByClass.fromString( iniFile.read_string("MappingApplication","alwaysInsertByClass","") );


	// Print params:
	printf("Running with the following parameters:\n");
	printf(" RAWLOG file:'%s'\n", RAWLOG_FILE.c_str());
	printf(" Output directory:\t\t\t'%s'\n",OUT_DIR);
	printf(" matchAgainstTheGrid:\t\t\t%c\n", mapBuilder.ICP_options.matchAgainstTheGrid ? 'Y':'N');
	printf(" Log record freq:\t\t\t%u\n",LOG_FREQUENCY);
	printf("  SAVE_3D_SCENE:\t\t\t%c\n", SAVE_3D_SCENE ? 'Y':'N');
	printf("  SAVE_POSE_LOG:\t\t\t%c\n", SAVE_POSE_LOG ? 'Y':'N');
	printf("  CAMERA_3DSCENE_FOLLOWS_ROBOT:\t%c\n",CAMERA_3DSCENE_FOLLOWS_ROBOT ? 'Y':'N');

	printf("\n");

	mapBuilder.ICP_params.dumpToConsole();
	mapBuilder.ICP_options.dumpToConsole();


	// Checks:
	ASSERT_(RAWLOG_FILE.size()>0);
	ASSERT_(fileExists(RAWLOG_FILE));

	CTicTac								tictac,tictacGlobal,tictac_JH;
	int									step = 0;
	string							str;
	CSimpleMap				finalMap;
	float								t_exec;
	COccupancyGridMap2D::TEntropyInfo	entropy;

	size_t						rawlogEntry = 0;
	CFileGZInputStream					rawlogFile( RAWLOG_FILE.c_str() );


	// Prepare output directory:
	// --------------------------------
	deleteFilesInDirectory(OUT_DIR);
	createDirectory(OUT_DIR);

	// Open log files:
	// ----------------------------------
	CFileOutputStream  f_log(format("%s/log_times.txt",OUT_DIR));
	CFileOutputStream  f_path(format("%s/log_estimated_path.txt",OUT_DIR));
	CFileOutputStream  f_pathOdo(format("%s/log_odometry_path.txt",OUT_DIR));


	// Create 3D window if requested:
	CDisplayWindow3DPtr	win3D;
#if MRPT_HAS_WXWIDGETS
	if (SHOW_PROGRESS_3D_REAL_TIME)
	{
		win3D = CDisplayWindow3D::Create("ICP-SLAM @ MRPT C++ Library", 600, 500);
		win3D->setCameraZoom(20);
		win3D->setCameraAzimuthDeg(-45);
	}
#endif

	// ----------------------------------------------------------
	//						Map Building
	// ----------------------------------------------------------
	CPose2D					odoPose(0,0,0);

	tictacGlobal.Tic();
	for (;;)
	{
		CActionCollectionPtr	action;
		CSensoryFramePtr		observations;
		CObservationPtr			observation;

		if (os::kbhit())
		{
			char c = os::getch();
			if (c==27)
				break;
		}

		// Load action/observation pair from the rawlog:
		// --------------------------------------------------
		if (! CRawlog::getActionObservationPairOrObservation( rawlogFile, action, observations, observation, rawlogEntry) )
			break; // file EOF

		const bool isObsBasedRawlog = observation.present();

		if (rawlogEntry>=rawlog_offset)
		{
			// Update odometry:
			if (isObsBasedRawlog)
			{
				static CPose2D lastOdo;
				static bool firstOdo = true;
				if (IS_CLASS(observation,CObservationOdometry))
				{
					CObservationOdometryPtr o = CObservationOdometryPtr(observation);
					if (!firstOdo)
						odoPose = odoPose + (o->odometry - lastOdo);

					lastOdo=o->odometry;
					firstOdo=false;
				}
			}
			else
			{
				CActionRobotMovement2DPtr act= action->getBestMovementEstimation();
				if (act)
					odoPose = odoPose + act->poseChange->getMeanVal();
			}

			// Execute:
			// ----------------------------------------
			tictac.Tic();
			if (isObsBasedRawlog)
					mapBuilder.processObservation( observation );
			else	mapBuilder.processActionObservation( *action, *observations );
			t_exec = tictac.Tac();
			printf("Map building executed in %.03fms\n", 1000.0f*t_exec );


			// Info log:
			// -----------
			f_log.printf("%f %i\n",1000.0f*t_exec,mapBuilder.getCurrentlyBuiltMapSize() );

			const CMultiMetricMap* mostLikMap =  mapBuilder.getCurrentlyBuiltMetricMap();

			if (0==(step % LOG_FREQUENCY))
			{
				// Pose log:
				// -------------
				if (SAVE_POSE_LOG)
				{
					printf("Saving pose log information...");
					mapBuilder.getCurrentPoseEstimation()->saveToTextFile( format("%s/mapbuild_posepdf_%03u.txt",OUT_DIR,step) );
					printf("Ok\n");
				}
			}

			// Save a 3D scene view of the mapping process:
			if (0==(step % LOG_FREQUENCY) || (SAVE_3D_SCENE || win3D.present()))
			{
                CPose3D robotPose;
				mapBuilder.getCurrentPoseEstimation()->getMean(robotPose);

				COpenGLScenePtr		scene = COpenGLScene::Create();

                COpenGLViewportPtr view=scene->getViewport("main");
                ASSERT_(view);

                COpenGLViewportPtr view_map = scene->createViewport("mini-map");
                view_map->setBorderSize(2);
                view_map->setViewportPosition(0.01,0.01,0.35,0.35);
                view_map->setTransparent(false);

				{
					mrpt::opengl::CCamera &cam = view_map->getCamera();
					cam.setAzimuthDegrees(-90);
					cam.setElevationDegrees(90);
					cam.setPointingAt(robotPose);
					cam.setZoomDistance(20);
					cam.setOrthogonal();
				}

				// The ground:
				mrpt::opengl::CGridPlaneXYPtr groundPlane = mrpt::opengl::CGridPlaneXY::Create(-200,200,-200,200,0,5);
				groundPlane->setColor(0.4,0.4,0.4);
				view->insert( groundPlane );
				view_map->insert( CRenderizablePtr( groundPlane) ); // A copy

				// The camera pointing to the current robot pose:
				if (CAMERA_3DSCENE_FOLLOWS_ROBOT)
				{
				    scene->enableFollowCamera(true);

					mrpt::opengl::CCamera &cam = view_map->getCamera();
					cam.setAzimuthDegrees(-45);
					cam.setElevationDegrees(45);
					cam.setPointingAt(robotPose);
				}

				// The maps:
				{
					opengl::CSetOfObjectsPtr obj = opengl::CSetOfObjects::Create();
					mostLikMap->getAs3DObject( obj );
					view->insert(obj);

					// Only the point map:
					opengl::CSetOfObjectsPtr ptsMap = opengl::CSetOfObjects::Create();
					if (mostLikMap->m_pointsMaps.size())
					{
                        mostLikMap->m_pointsMaps[0]->getAs3DObject(ptsMap);
                        view_map->insert( ptsMap );
					}
				}

				// Draw the robot path:
				CPose3DPDFPtr posePDF =  mapBuilder.getCurrentPoseEstimation();
				CPose3D  curRobotPose;
				posePDF->getMean(curRobotPose);
				{
					opengl::CSetOfObjectsPtr obj = opengl::stock_objects::RobotPioneer();
					obj->setPose( curRobotPose );
					view->insert(obj);
				}
				{
					opengl::CSetOfObjectsPtr obj = opengl::stock_objects::RobotPioneer();
					obj->setPose( curRobotPose );
					view_map->insert( obj );
				}

				// Save as file:
				if (0==(step % LOG_FREQUENCY) && SAVE_3D_SCENE)
				{
					CFileGZOutputStream	f( format( "%s/buildingmap_%05u.3Dscene",OUT_DIR,step ));
					f << *scene;
				}

				// Show 3D?
				if (win3D)
				{
					opengl::COpenGLScenePtr &ptrScene = win3D->get3DSceneAndLock();
					ptrScene = scene;

					win3D->unlockAccess3DScene();

					// Move camera:
					win3D->setCameraPointingToPoint( curRobotPose.x(),curRobotPose.y(),curRobotPose.z() );

					// Update:
					win3D->forceRepaint();

					sleep( SHOW_PROGRESS_3D_REAL_TIME_DELAY_MS );
				}
			}


			// Save the memory usage:
			// ------------------------------------------------------------------
			{
				printf("Saving memory usage...");
				unsigned long	memUsage = getMemoryUsage();
				FILE		*f=os::fopen( format("%s/log_MemoryUsage.txt",OUT_DIR).c_str() ,"at");
				if (f)
				{
					os::fprintf(f,"%u\t%lu\n",step,memUsage);
					os::fclose(f);
				}
				printf("Ok! (%.04fMb)\n", ((float)memUsage)/(1024*1024) );
			}

			// Save the robot estimated pose for each step:
			f_path.printf("%i %f %f %f\n",
				step,
				mapBuilder.getCurrentPoseEstimation()->getMeanVal().x(),
				mapBuilder.getCurrentPoseEstimation()->getMeanVal().y(),
				mapBuilder.getCurrentPoseEstimation()->getMeanVal().yaw() );


			f_pathOdo.printf("%i %f %f %f\n",step,odoPose.x(),odoPose.y(),odoPose.phi());

		} // end of if "rawlog_offset"...

		step++;
		printf("\n---------------- STEP %u | RAWLOG ENTRY %u ----------------\n",step, (unsigned)rawlogEntry);
	};

	printf("\n---------------- END!! (total time: %.03f sec) ----------------\n",tictacGlobal.Tac());

	// Save map:
	mapBuilder.getCurrentlyBuiltMap(finalMap);

	str = format("%s/_finalmap_.simplemap",OUT_DIR);
	printf("Dumping final map in binary format to: %s\n", str.c_str() );
	mapBuilder.saveCurrentMapToFile(str);

	CMultiMetricMap  *finalPointsMap = mapBuilder.getCurrentlyBuiltMetricMap();
	str = format("%s/_finalmaps_.txt",OUT_DIR);
	printf("Dumping final metric maps to %s_XXX\n", str.c_str() );
	finalPointsMap->saveMetricMapRepresentationToFile( str );

	if (win3D)
		win3D->waitForKey();

	MRPT_END
}