/*---------------------------------------------------------------
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);
}
}
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
}
// -----------------------------------------------
// 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;
}
}
