C++ CSerializablePtr::clear_unique方法代码示例

/* -----------------------------------------------------------------------
	Used to pass CORBA-like object into a MRPT object.
		See doc about "Integration with BABEL".
   ----------------------------------------------------------------------- */
void utils::StringToObject(const std::string &str, CSerializablePtr &obj)
{
	MRPT_START

	obj.clear_unique();
	if (str.empty()) return;

	CMemoryStream	tmp;
	size_t			n;
	size_t			i,lastIdx;

	obj.clear_unique();

	n = str.size();

	// Scan the string to decode it:
	// ----------------------------------
	lastIdx = 0;
	const char *data = str.c_str();
	unsigned char c;
	for (i=0;i<n && (c=data[i])!=0;i++)
	{
		// Search for first "0x01" byte:
		if ( c == 0x01 )
		{
			// Copy all till now:
			tmp.WriteBuffer( &data[lastIdx], i - lastIdx + 1 );
			i+=1; // +1 from "for" loop
			lastIdx = i+1;

			// And decode:
			//   0x01 0x01 --> 0x01
			//   0x01 0x02 --> 0x00
			if (data[i]==0x01)
					((unsigned char*)tmp.getRawBufferData())[tmp.getTotalBytesCount()-1] = (unsigned char)0x01;
			else 	((unsigned char*)tmp.getRawBufferData())[tmp.getTotalBytesCount()-1] = (unsigned char)0x00;
		}
	} // end for i

	// Copy the rest:
	if ( (n-lastIdx) > 0)
		tmp.WriteBuffer( &data[lastIdx], n - lastIdx );

	// And the '\0' char:
	char dummy = '\0';
	tmp.WriteBuffer( &dummy, sizeof(char) );

	tmp.Seek(0,CStream::sFromBeginning);
	obj = tmp.ReadObject();

	MRPT_END

}

/*---------------------------------------------------------------
				deserializeIntoNewObject
 ---------------------------------------------------------------*/
void  CMessage::deserializeIntoNewObject( CSerializablePtr &obj )
{
	MRPT_START
	CMemoryStream		auxStream;

	// Copy data into the stream:
	if (!content.empty())
	{
		auxStream.WriteBuffer( &content[0], content.size() );
		auxStream.Seek(0);

		// Try to parse data into a new object:
		obj = auxStream.ReadObject( );
	}
	else obj.clear_unique();


	MRPT_END
}

/* -----------------------------------------------------------------------
				RawStringToObject
   ----------------------------------------------------------------------- */
void utils::RawStringToObject(const std::string & in_data, CSerializablePtr &obj)
{
	try
	{
		obj.clear_unique();

		if (in_data.empty()) return;

		CMemoryStream	tmp( &in_data[0], in_data.size());
		obj = tmp.ReadObject();
	}
	catch (std::bad_alloc &e)
	{
		throw e;
	}
	catch(std::exception &e)
	{
		std::cerr << "[OctetVectorToObject] Exception: " << e.what() << std::endl;
	}
	catch(...)
	{
		std::cerr << "[OctetVectorToObject] Unknown exception" << std::endl;
	}
}

//.........这里部分代码省略.........
	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
}

//.........这里部分代码省略.........
		wxPD_ESTIMATED_TIME |
		wxPD_REMAINING_TIME);

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

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

	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 (unsigned k=0;k<SF->size();k++)
				{
					if (SF->getObservationByIndex(k)->GetRuntimeClass()==CLASS_ID(CObservationStereoImages ) )
					{
						CObservationStereoImagesPtr obsSt = SF->getObservationByIndexAs<CObservationStereoImagesPtr>(k);

						// save image to file & convert into external storage:
						string fileName = format("img_stereo_%u_left_%05u.%s",k,imgSaved,imgFileExtension.c_str() );
						obsSt->imageLeft.saveToFile( outDir + fileName );
						obsSt->imageLeft.setExternalStorage( fileName );

						imgSaved++;

						// save image to file & convert into external storage:
						fileName = format("img_stereo_%u_right_%05u.%s",k,imgSaved,imgFileExtension.c_str() );
						obsSt->imageRight.saveToFile( outDir + fileName );
						obsSt->imageRight.setExternalStorage( fileName );

						imgSaved++;
					}
					if (SF->getObservationByIndex(k)->GetRuntimeClass()==CLASS_ID(CObservationImage ) )
					{
						CObservationImagePtr obsIm = SF->getObservationByIndexAs<CObservationImagePtr>(k);

						// save image to file & convert into external storage:
						string fileName = format("img_monocular_%u_%05u.%s",k,imgSaved,imgFileExtension.c_str() );
						obsIm->image.saveToFile( outDir + fileName );
						obsIm->image.setExternalStorage( fileName );

						imgSaved++;
					}
				}

			}
			else
				if ( newObj->GetRuntimeClass() == CLASS_ID(CActionCollection) ||
						newObj->GetRuntimeClass() == CLASS_ID(CPose2D) )
				{
				}
				else
				{
					// Unknown class:
					THROW_EXCEPTION("Unknown class found in the file!");
				}

			// Dump to the new file:
			f_out << *newObj;

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

	progDia.Update( filSize );

	// Set error msg:
	wxMessageBox(_U( format("Images saved: %i",imgSaved).c_str() ),_("Done"),wxOK,this);

	WX_END_TRY

}

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;
//.........这里部分代码省略.........