void CRawlog::addAction(CAction& action) { CActionCollection::Ptr temp = mrpt::make_aligned_shared<CActionCollection>(); temp->insert(action); m_seqOfActObs.push_back(temp); }
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. // ------------------------------------------------------------------------------ CSensoryFrame::Ptr 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 } CSerializable::Ptr 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 = mrpt::make_aligned_shared<CSensoryFrame>(); // Copy pointers to observations only (fast): accum_sf->moveFrom( *std::dynamic_pointer_cast<CSensoryFrame>(newObj)); if (++SF_counter >= DECIMATE_RATIO) { SF_counter = 0; // INSERT OBSERVATIONS: f_out << *accum_sf; accum_sf.reset(); // INSERT ACTIONS: CActionCollection::Ptr actsCol = mrpt::make_aligned_shared<CActionCollection>(); 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.reset(); } } else if (newObj->GetRuntimeClass() == CLASS_ID(CActionCollection)) { // Accumulate Actions // ---------------------- CActionCollection::Ptr curActs = std::dynamic_pointer_cast<CActionCollection>(newObj); CActionRobotMovement2D::Ptr 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 }