예제 #1
0
/*---------------------------------------------------------------
						pushObservation
  ---------------------------------------------------------------*/
void CHMTSLAM::pushObservation(const CObservation::Ptr& obs)
{
	if (m_terminateThreads)
	{  // Discard it:
		// delete obs;
		return;
	}

	// Add a CSensoryFrame with the obs:
	CSensoryFrame::Ptr sf = mrpt::make_aligned_shared<CSensoryFrame>();
	sf->insert(
		obs);  // memory will be freed when deleting the SF in other thread

	{  // Wait for critical section
		std::lock_guard<std::mutex> lock(m_inputQueue_cs);
		m_inputQueue.push(sf);
	}
}
예제 #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
}
예제 #3
0
// -----------------------------------------------
//				MAIN
// -----------------------------------------------
int main(int argc, char** argv)
{
	try
	{
		// Parse arguments:
		if (!cmd.parse(argc, argv))
			throw std::runtime_error("");  // should exit.

		const string input_log = arg_input_file.getValue();
		const string output_file = arg_output_file.getValue();
		const bool verbose = !arg_quiet.getValue();
		const bool overwrite = arg_overwrite.getValue();
		const int compress_level = arg_gz_level.getValue();

		// Check files:
		if (!mrpt::system::fileExists(input_log))
			throw runtime_error(
				format("Input file doesn't exist: '%s'", input_log.c_str()));

		if (mrpt::system::fileExists(output_file) && !overwrite)
			throw runtime_error(format(
				"Output file already exist: '%s' (Use --overwrite to "
				"override)",
				output_file.c_str()));

		VERBOSE_COUT << "Input log        : " << input_log << endl;
		VERBOSE_COUT << "Output map file  : " << output_file
					 << " (Compression level: " << compress_level << ")\n";

		// Open I/O streams:
		std::ifstream input_stream(input_log.c_str());
		if (!input_stream.is_open())
			throw runtime_error(
				format("Error opening for read: '%s'", input_log.c_str()));

		// --------------------------------
		// The main loop
		// --------------------------------
		vector<CObservation::Ptr> importedObservations;
		mrpt::maps::CSimpleMap theSimpleMap;
		const mrpt::system::TTimeStamp base_timestamp = mrpt::system::now();

		const uint64_t totalInFileSize = mrpt::system::getFileSize(input_log);
		int decimateUpdateConsole = 0;

		while (carmen_log_parse_line(
			input_stream, importedObservations, base_timestamp))
		{
			CPose2D gt_pose;
			bool has_gt_pose = false;

			for (size_t i = 0; i < importedObservations.size(); i++)
			{
				// If we have an "odometry" observation but it's not alone, it's
				// probably
				//  a "corrected" odometry from some SLAM program, so save it as
				//  ground truth:
				if (importedObservations.size() > 1 &&
					IS_CLASS(importedObservations[i], CObservationOdometry))
				{
					CObservationOdometry::Ptr odo =
						std::dynamic_pointer_cast<CObservationOdometry>(
							importedObservations[i]);
					gt_pose = odo->odometry;
					has_gt_pose = true;
					break;
				}
			}

			// Only if we have a valid pose, save it to the simple map:
			if (has_gt_pose)
			{
				CSensoryFrame::Ptr SF =
					mrpt::make_aligned_shared<CSensoryFrame>();

				for (const auto& importedObservation : importedObservations)
				{
					if (!IS_CLASS(
							importedObservation,
							CObservationOdometry))  // Odometry was already used
					// as positioning...
					{
						SF->insert(importedObservation);
					}
				}

				// Insert (observations, pose) pair:
				CPosePDFGaussian::Ptr pos =
					mrpt::make_aligned_shared<CPosePDFGaussian>();
				pos->mean = gt_pose;
				theSimpleMap.insert(pos, SF);
			}

			// Update progress in the console:
			// ----------------------------------
			if (verbose && ++decimateUpdateConsole > 10)
			{
				decimateUpdateConsole = 0;

				const std::streampos curPos = input_stream.tellg();
				const double progress_ratio =
					double(curPos) / double(totalInFileSize);
				static const int nBlocksTotal = 50;
				const int nBlocks = progress_ratio * nBlocksTotal;
				cout << "\rProgress: [" << string(nBlocks, '#')
					 << string(nBlocksTotal - nBlocks, ' ')
					 << format(
							"] %6.02f%% (%u frames)", progress_ratio * 100,
							static_cast<unsigned int>(theSimpleMap.size()));
				cout.flush();
			}
		};
		cout << "\n";

		// Save final map object:
		{
			mrpt::io::CFileGZOutputStream out_map;
			if (!out_map.open(output_file, compress_level))
				throw runtime_error(format(
					"Error opening for write: '%s'", output_file.c_str()));

			cout << "Dumping simplemap object to file...";
			cout.flush();
			mrpt::serialization::archiveFrom(out_map) << theSimpleMap;
			cout << "Done\n";
			cout.flush();
		}

		// successful end of program.
		return 0;
	}
	catch (const std::exception& e)
	{
		std::cerr << mrpt::exception_to_str(e) << std::endl;
		return -1;
	}
}