示例#1
0
void CFormMotionModel::applyToLoadedRawlog()
{
	WX_START_TRY

	// Apply changes:
	size_t changes = 0;
	{
		wxBusyCursor cursor;

		size_t first = 0;
		size_t last = rawlog.size() - 1;

		if (!cbAll->GetValue())
		{
			first = atoi(string(edRangeFrom->GetValue().mb_str()).c_str());
			last = atoi(string(edRangeTo->GetValue().mb_str()).c_str());
		}

		for (size_t i = first; i <= last; i++)
		{
			// Check type:
			if (rawlog.getType(i) == CRawlog::etActionCollection)
			{
				// This is an action:
				CActionCollection::Ptr acts = rawlog.getAsAction(i);

				CActionRobotMovement2D::Ptr firstActionMov =
					acts->getActionByClass<CActionRobotMovement2D>();

				if (firstActionMov)
				{
					if (firstActionMov->estimationMethod ==
						CActionRobotMovement2D::emOdometry)
					{
						// Use the kinematics motion model to estimate a pose
						// change gaussian approximation:
						firstActionMov->computeFromOdometry(
							firstActionMov->rawOdometryIncrementReading,
							options);
					}
					else
					{
						// Take the mean value
						firstActionMov->computeFromOdometry(
							firstActionMov->poseChange->getMeanVal(), options);
					}
					changes++;
				}
			}  // end is action
		}  // end for i
	}

	wxString str;
	str.sprintf(_("Objects changed: %d"), changes);
	wxMessageBox(str, _("Result:"), wxOK, this);

	WX_END_TRY
}
示例#2
0
void xRawLogViewerFrame::OnMenuCompactRawlog(wxCommandEvent& event)
{
	WX_START_TRY

	bool onlyOnePerLabel =
		(wxYES == wxMessageBox(
					  _("Keep only one observation of each label within each "
						"sensoryframe?"),
					  _("Compact rawlog"), wxYES_NO, this));

	int progress_N = static_cast<int>(rawlog.size());
	int progress_i = progress_N;

	wxProgressDialog progDia(
		wxT("Compacting rawlog"), wxT("Processing..."),
		progress_N,  // 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

	CActionRobotMovement2D::Ptr lastAct;
	CSensoryFrame::Ptr lastSF;  //  = nullptr;

	unsigned counter_loops = 0;
	unsigned nActionsDel = 0;
	unsigned nEmptySFDel = 0;

	CRawlog::iterator it = rawlog.begin();
	for (progress_i = 0; it != rawlog.end(); progress_i--)
	{
		if (counter_loops++ % 50 == 0)
		{
			if (!progDia.Update(progress_N - progress_i)) break;
			wxTheApp->Yield();  // Let the app. process messages
		}

		bool deleteThis = false;

		if (it.getType() == CRawlog::etActionCollection)
		{
			// Is this a part of multiple actions?
			if (lastAct)
			{
				// Remove this one and add it to the first in the series:
				CActionRobotMovement2D::Ptr act =
					std::dynamic_pointer_cast<CActionCollection>(*it)
						->getMovementEstimationByType(
							CActionRobotMovement2D::emOdometry);
				ASSERT_(act);
				lastAct->computeFromOdometry(
					lastAct->rawOdometryIncrementReading +
						act->rawOdometryIncrementReading,
					lastAct->motionModelConfiguration);

				deleteThis = true;
				nActionsDel++;
			}
			else
			{
				// This is the first one:
				lastAct = std::dynamic_pointer_cast<CActionCollection>(*it)
							  ->getMovementEstimationByType(
								  CActionRobotMovement2D::emOdometry);
				ASSERT_(lastAct);

				// Before leaving the last SF, leave only one observation for
				// each sensorLabel:
				if (onlyOnePerLabel && lastSF)
				{
					CSensoryFrame::Ptr newSF =
						mrpt::make_aligned_shared<CSensoryFrame>();
					set<string> knownLabels;

					for (CSensoryFrame::const_iterator o = lastSF->begin();
						 o != lastSF->end(); ++o)
					{
						if (knownLabels.find((*o)->sensorLabel) ==
							knownLabels.end())
							newSF->insert(*o);
						knownLabels.insert((*o)->sensorLabel);
					}
					*lastSF = *newSF;
				}
				// Ok, done:
				lastSF.reset();
			}
		}
		else if (it.getType() == CRawlog::etSensoryFrame)
		{
			// Is this a part of a series?
			if (lastSF)
			{
				// remove this one and accumulate in the first in the serie:
				lastSF->moveFrom(
					*std::dynamic_pointer_cast<CSensoryFrame>(*it));

				deleteThis = true;
				nEmptySFDel++;
			}
			else
			{
				// This is the first SF:
				CSensoryFrame::Ptr sf =
					std::dynamic_pointer_cast<CSensoryFrame>(*it);

				// Only take into account if not empty!
				if (sf->size())
				{
					lastSF = sf;
					ASSERT_(lastSF);
					lastAct.reset();
				}
				else
				{
					deleteThis = true;
					nEmptySFDel++;
				}
			}
		}
		else
			THROW_EXCEPTION("Unexpected class found!")

		if (deleteThis)
		{
			it = rawlog.erase(it);
			progress_i--;  // Extra count
		}
		else
			it++;
	}

	progDia.Update(progress_N);

	string str = format(
		"%u actions deleted\n%u sensory frames deleted", nActionsDel,
		nEmptySFDel);
	::wxMessageBox(_U(str.c_str()));

	rebuildTreeView();

	WX_END_TRY
}