// ------------------------------------------------------------ // Set the sensor pose // ------------------------------------------------------------ void exec_setPoseByIdx( mrpt::slam::CActionCollection *acts, mrpt::slam::CSensoryFrame *SF, int &changesCount ) { if (SF) if (SF->size()>idxToProcess) { CObservationPtr obs = SF->getObservationByIndex(idxToProcess); if (changeOnlyXYZ) { CPose3D tmpPose; obs->getSensorPose(tmpPose); tmpPose.setFromValues( sensorPoseToSet.x, sensorPoseToSet.y, sensorPoseToSet.z, tmpPose.yaw(), tmpPose.pitch(), tmpPose.roll() ); obs->setSensorPose( tmpPose ); } else { obs->setSensorPose( sensorPoseToSet ); } changesCount++; } }
void exec_setPoseByLabel( mrpt::slam::CActionCollection *acts, mrpt::slam::CSensoryFrame *SF, int &changesCount ) { if (SF) { for (CSensoryFrame::iterator it= SF->begin();it!=SF->end();++it) { CObservationPtr obs = *it; if ( obs->sensorLabel == labelToProcess) { if (changeOnlyXYZ) { CPose3D tmpPose; obs->getSensorPose(tmpPose); tmpPose.setFromValues( sensorPoseToSet.x, sensorPoseToSet.y, sensorPoseToSet.z, tmpPose.yaw(), tmpPose.pitch(), tmpPose.roll() ); obs->setSensorPose( tmpPose ); } else { obs->setSensorPose( sensorPoseToSet ); } changesCount++; } } // end for each obs. } }
void xRawLogViewerFrame::OnMenuResortByTimestamp(wxCommandEvent& event) { WX_START_TRY bool useSensorTimestamp = (wxYES==wxMessageBox( _("Yes: use sensor-based UTC timestamp. No: use computer-based timestamp, when available."), _("Which timestamp to use?"),wxYES_NO, this )); wxBusyCursor waitCursor; // First, build an ordered list of "times"->"indexes": // ------------------------------------------------------ std::multimap<TTimeStamp,size_t> ordered_times; size_t i, n = rawlog.size(); for (i=0;i<n;i++) { switch ( rawlog.getType(i) ) { default: wxMessageBox(_("Error: this command is for rawlogs without sensory frames.")); return; break; case CRawlog::etObservation: { CObservationPtr o = rawlog.getAsObservation(i); TTimeStamp tim = useSensorTimestamp ? o->getTimeStamp() : o->getOriginalReceivedTimeStamp(); if (o->timestamp == INVALID_TIMESTAMP) { wxMessageBox( wxString::Format(_("Error: Element %u does not have a valid timestamp."),(unsigned int)i) ); return; } ordered_times.insert( multimap<TTimeStamp,size_t>::value_type(o->timestamp,i)); } break; } // end switch type } // end for i // Now create the new ordered rawlog // ------------------------------------------------------ CRawlog temp_rawlog; temp_rawlog.setCommentText( rawlog.getCommentText() ); for (std::multimap<TTimeStamp,size_t>::iterator it=ordered_times.begin();it!=ordered_times.end();++it) { size_t idx = it->second; temp_rawlog.addObservationMemoryReference( rawlog.getAsObservation(idx) ); } rawlog.moveFrom(temp_rawlog); // Update the views: rebuildTreeView(); tree_view->Refresh(); WX_END_TRY }
void exec_getCurrentPoseByLabel( mrpt::slam::CActionCollection *acts, mrpt::slam::CSensoryFrame *SF, int &changesCount ) { if (SF && SF->size()) { CObservationPtr o = SF->getObservationBySensorLabel(labelToProcess); if (o) { o->getSensorPose( sensorPoseToRead ); sensorPoseReadOK = true; } } }
/** Iterate */ bool CImageGrabberApp::Iterate() { if( !m_initialized_ok || !m_start_grabbing ) return true; m_counter++; // grab image from video input CObservationPtr obs; obs = m_camera.getNextFrame(); if(obs) { VERBOSE_LEVEL(2) << "[pImageGrabber -- INFO] Object grabbed." << endl; // serialize observation and send message mrpt::vector_byte bObs; mrpt::utils::ObjectToOctetVector(obs.pointer(), bObs); //! @moos_publish DETECTED_FACES m_Comms.Notify( "GRABBED_IMAGE", bObs ); // update counter ++m_grabbed_images_counter; // check if we are recording a mono or a stereo observation if( IS_CLASS(obs,CObservationImage) ) { VERBOSE_LEVEL(2) << "[pImageGrabber -- INFO] Single image observation." << endl; CObservationImagePtr imgptr = static_cast<CObservationImagePtr>(obs); std::string filename = imgptr->image.getExternalStorageFileAbsolutePath(); VERBOSE_LEVEL(2) << "[pImageGrabber -- INFO] Image file name: " << filename << endl; m_grabbed_files.push_back(filename); VERBOSE_LEVEL(2) << "[pImageGrabber -- INFO] Files in list: " << m_grabbed_files.size() << endl; // save file path for later deletion if desired (clear buffer after N images) if( m_delete_old_files && (m_grabbed_images_th > 0) && (m_grabbed_files.size() > m_grabbed_images_th) ) { mrpt::system::deleteFile( m_grabbed_files.front() ); m_grabbed_files.pop_front(); } // end-if } // end-if-mono else if( IS_CLASS(obs,CObservationStereoImages) ) { VERBOSE_LEVEL(2) << "[pImageGrabber -- INFO] Stereo image observation." << endl; CObservationStereoImagesPtr imgptr = static_cast<CObservationStereoImagesPtr>(obs); std::string filename1 = imgptr->imageLeft.getExternalStorageFileAbsolutePath(); std::string filename2 = imgptr->imageRight.getExternalStorageFileAbsolutePath(); VERBOSE_LEVEL(2) << "[pImageGrabber -- INFO] Image file names: " << endl << filename1 << endl << filename2 << endl; m_grabbed_stereo_files.push_back( make_pair(filename1,filename2)); VERBOSE_LEVEL(2) << "[pImageGrabber -- INFO] Stereo files in list: " << m_grabbed_stereo_files.size() << endl; // save file path for later deletion if desired (clear buffer after N images) if( m_delete_old_files && (m_grabbed_images_th > 0) && (m_grabbed_stereo_files.size() > m_grabbed_images_th) ) { mrpt::system::deleteFile( m_grabbed_stereo_files.front().first ); mrpt::system::deleteFile( m_grabbed_stereo_files.front().second ); m_grabbed_stereo_files.pop_front(); } // end-if } // end-if-stereo return true; } return false; } // end-Iterate
/*--------------------------------------------------------------- path_from_rtk_gps ---------------------------------------------------------------*/ void mrpt::topography::path_from_rtk_gps( mrpt::poses::CPose3DInterpolator &robot_path, const mrpt::slam::CRawlog &rawlog, size_t first, size_t last, bool isGUI, bool disableGPSInterp, int PATH_SMOOTH_FILTER , TPathFromRTKInfo *outInfo ) { MRPT_START #if MRPT_HAS_WXWIDGETS // Use a smart pointer so we are safe against exceptions: stlplus::smart_ptr<wxBusyCursor> waitCursorPtr; if (isGUI) waitCursorPtr = stlplus::smart_ptr<wxBusyCursor>( new wxBusyCursor() ); #endif // Go: generate the map: size_t i; ASSERT_(first<=last); ASSERT_(last<=rawlog.size()-1); set<string> lstGPSLabels; size_t count = 0; robot_path.clear(); robot_path.setMaxTimeInterpolation(3.0); // Max. seconds of GPS blackout not to interpolate. robot_path.setInterpolationMethod( CPose3DInterpolator::imSSLSLL ); TPathFromRTKInfo outInfoTemp; if (outInfo) *outInfo = outInfoTemp; map<string, map<TTimeStamp,TPoint3D> > gps_paths; // label -> (time -> 3D local coords) bool abort = false; bool ref_valid = false; // Load configuration block: CConfigFileMemory memFil; rawlog.getCommentTextAsConfigFile(memFil); TGeodeticCoords ref( memFil.read_double("GPS_ORIGIN","lat_deg",0), memFil.read_double("GPS_ORIGIN","lon_deg",0), memFil.read_double("GPS_ORIGIN","height",0) ); ref_valid = !ref.isClear(); // Do we have info for the consistency test? const double std_0 = memFil.read_double("CONSISTENCY_TEST","std_0",0); bool doConsistencyCheck = std_0>0; // Do we have the "reference uncertainty" matrix W^\star ?? memFil.read_matrix("UNCERTAINTY","W_star",outInfoTemp.W_star); const bool doUncertaintyCovs = size(outInfoTemp.W_star,1)!=0; if (doUncertaintyCovs && (size(outInfoTemp.W_star,1)!=6 || size(outInfoTemp.W_star,2)!=6)) THROW_EXCEPTION("ERROR: W_star matrix for uncertainty estimation is provided but it's not a 6x6 matrix."); // ------------------------------------------ // Look for the 2 observations: // ------------------------------------------ #if MRPT_HAS_WXWIDGETS wxProgressDialog *progDia=NULL; if (isGUI) { progDia = new wxProgressDialog( wxT("Building map"), wxT("Getting GPS observations..."), (int)(last-first+1), // range NULL, // parent wxPD_CAN_ABORT | wxPD_APP_MODAL | wxPD_SMOOTH | wxPD_AUTO_HIDE | wxPD_ELAPSED_TIME | wxPD_ESTIMATED_TIME | wxPD_REMAINING_TIME); } #endif // The list with all time ordered gps's in valid RTK mode typedef std::map< mrpt::system::TTimeStamp, std::map<std::string,CObservationGPSPtr> > TListGPSs; TListGPSs list_gps_obs; map<string,size_t> GPS_RTK_reads; // label-># of RTK readings map<string,TPoint3D> GPS_local_coords_on_vehicle; // label -> local pose on the vehicle for (i=first;!abort && i<=last;i++) { switch ( rawlog.getType(i) ) { default: break; case CRawlog::etObservation: { CObservationPtr o = rawlog.getAsObservation(i); if (o->GetRuntimeClass()==CLASS_ID(CObservationGPS)) { CObservationGPSPtr obs = CObservationGPSPtr(o); if (obs->has_GGA_datum && obs->GGA_datum.fix_quality==4) { // Add to the list: list_gps_obs[obs->timestamp][obs->sensorLabel] = obs; lstGPSLabels.insert(obs->sensorLabel); } // Save to GPS paths: if (obs->has_GGA_datum && (obs->GGA_datum.fix_quality==4 || obs->GGA_datum.fix_quality==5)) { GPS_RTK_reads[obs->sensorLabel]++; // map<string,TPoint3D> GPS_local_coords_on_vehicle; // label -> local pose on the vehicle if (GPS_local_coords_on_vehicle.find(obs->sensorLabel)==GPS_local_coords_on_vehicle.end()) GPS_local_coords_on_vehicle[obs->sensorLabel] = TPoint3D( obs->sensorPose ); //map<string, map<TTimeStamp,TPoint3D> > gps_paths; // label -> (time -> 3D local coords) gps_paths[obs->sensorLabel][obs->timestamp] = TPoint3D( obs->GGA_datum.longitude_degrees, obs->GGA_datum.latitude_degrees, obs->GGA_datum.altitude_meters ); } } } break; } // end switch type // Show progress: if ((count++ % 100)==0) { #if MRPT_HAS_WXWIDGETS if (progDia) { if (!progDia->Update((int)(i-first))) abort = true; wxTheApp->Yield(); } #endif } } // end for i #if MRPT_HAS_WXWIDGETS if (progDia) { delete progDia; progDia=NULL; } #endif // ----------------------------------------------------------- // At this point we already have the sensor positions, thus // we can estimate the covariance matrix D: // // TODO: Generalize equations for # of GPS > 3 // ----------------------------------------------------------- map< set<string>, double > Ad_ij; // InterGPS distances in 2D map< set<string>, double > phi_ij; // Directions on XY of the lines between i-j map< string, size_t> D_cov_indexes; // Sensor label-> index in the matrix (first=0, ...) map< size_t, string> D_cov_rev_indexes; // Reverse of D_cov_indexes CMatrixDouble D_cov; // square distances cov CMatrixDouble D_cov_1; // square distances cov (inverse) vector_double D_mean; // square distances mean if (doConsistencyCheck && GPS_local_coords_on_vehicle.size()==3) { unsigned int cnt = 0; for(map<string,TPoint3D>::iterator i=GPS_local_coords_on_vehicle.begin();i!=GPS_local_coords_on_vehicle.end();++i) { // Index tables: D_cov_indexes[i->first] = cnt; D_cov_rev_indexes[cnt] = i->first; cnt++; for(map<string,TPoint3D>::iterator j=i;j!=GPS_local_coords_on_vehicle.end();++j) { if (i!=j) { const TPoint3D &pi = i->second; const TPoint3D &pj = j->second; Ad_ij[ make_set(i->first,j->first) ] = pi.distanceTo( pj ); phi_ij[ make_set(i->first,j->first) ] = atan2( pj.y-pi.y, pj.x-pi.x ); } } } ASSERT_( D_cov_indexes.size()==3 && D_cov_rev_indexes.size()==3 ); D_cov.setSize( D_cov_indexes.size(), D_cov_indexes.size() ); D_mean.resize( D_cov_indexes.size() ); // See paper for the formulas! // TODO: generalize for N>3 D_cov(0,0) = 2*square( Ad_ij[ make_set(D_cov_rev_indexes[0],D_cov_rev_indexes[1])] ); D_cov(1,1) = 2*square( Ad_ij[ make_set(D_cov_rev_indexes[0],D_cov_rev_indexes[2])] ); D_cov(2,2) = 2*square( Ad_ij[ make_set(D_cov_rev_indexes[1],D_cov_rev_indexes[2])] ); D_cov(1,0) = Ad_ij[ make_set(D_cov_rev_indexes[0],D_cov_rev_indexes[1])] * Ad_ij[ make_set(D_cov_rev_indexes[0],D_cov_rev_indexes[2])] * cos( phi_ij[ make_set(D_cov_rev_indexes[0],D_cov_rev_indexes[1])] - phi_ij[ make_set(D_cov_rev_indexes[0],D_cov_rev_indexes[2])] ); D_cov(0,1) = D_cov(1,0); D_cov(2,0) =-Ad_ij[ make_set(D_cov_rev_indexes[0],D_cov_rev_indexes[1])] * Ad_ij[ make_set(D_cov_rev_indexes[1],D_cov_rev_indexes[2])] * cos( phi_ij[ make_set(D_cov_rev_indexes[0],D_cov_rev_indexes[1])] - phi_ij[ make_set(D_cov_rev_indexes[1],D_cov_rev_indexes[2])] ); D_cov(0,2) = D_cov(2,0); D_cov(2,1) = Ad_ij[ make_set(D_cov_rev_indexes[0],D_cov_rev_indexes[2])] * Ad_ij[ make_set(D_cov_rev_indexes[1],D_cov_rev_indexes[2])] * cos( phi_ij[ make_set(D_cov_rev_indexes[0],D_cov_rev_indexes[2])] - phi_ij[ make_set(D_cov_rev_indexes[1],D_cov_rev_indexes[2])] ); D_cov(1,2) = D_cov(2,1); D_cov *= 4*square(std_0); D_cov_1 = D_cov.inv(); //cout << D_cov.inMatlabFormat() << endl; D_mean[0] = square( Ad_ij[ make_set(D_cov_rev_indexes[0],D_cov_rev_indexes[1])] ); D_mean[1] = square( Ad_ij[ make_set(D_cov_rev_indexes[0],D_cov_rev_indexes[2])] ); D_mean[2] = square( Ad_ij[ make_set(D_cov_rev_indexes[1],D_cov_rev_indexes[2])] ); } else doConsistencyCheck =false; // ------------------------------------------ // Look for the 2 observations: // ------------------------------------------ int N_GPSs = list_gps_obs.size(); if (N_GPSs) { // loop interpolate 1-out-of-5: this solves the issue with JAVAD GPSs // that skip some readings at some times .0 .2 .4 .6 .8 if (list_gps_obs.size()>4) { TListGPSs::iterator F = list_gps_obs.begin(); ++F; ++F; TListGPSs::iterator E = list_gps_obs.end(); --E; --E; for (TListGPSs::iterator i=F;i!=E;++i) { // Now check if we have 3 gps with the same time stamp: //const size_t N = i->second.size(); std::map<std::string, CObservationGPSPtr> & GPS = i->second; // Check if any in "lstGPSLabels" is missing here: for (set<string>::iterator l=lstGPSLabels.begin();l!=lstGPSLabels.end();++l) { // For each GPS in the current timestamp: bool fnd = ( GPS.find(*l)!=GPS.end() ); if (fnd) continue; // this one is present. // Ok, we have "*l" missing in the set "*i". // Try to interpolate from neighbors: TListGPSs::iterator i_b1 = i; --i_b1; TListGPSs::iterator i_a1 = i; ++i_a1; CObservationGPSPtr GPS_b1, GPS_a1; if (i_b1->second.find( *l )!=i_b1->second.end()) GPS_b1 = i_b1->second.find( *l )->second; if (i_a1->second.find( *l )!=i_a1->second.end()) GPS_a1 = i_a1->second.find( *l )->second; if (!disableGPSInterp && GPS_a1 && GPS_b1) { if ( mrpt::system::timeDifference( GPS_b1->timestamp, GPS_a1->timestamp ) < 0.5 ) { CObservationGPSPtr new_gps = CObservationGPSPtr( new CObservationGPS(*GPS_a1) ); new_gps->sensorLabel = *l; //cout << mrpt::system::timeLocalToString(GPS_b1->timestamp) << " " << mrpt::system::timeLocalToString(GPS_a1->timestamp) << " " << *l; //cout << endl; new_gps->GGA_datum.longitude_degrees = 0.5 * ( GPS_a1->GGA_datum.longitude_degrees + GPS_b1->GGA_datum.longitude_degrees ); new_gps->GGA_datum.latitude_degrees = 0.5 * ( GPS_a1->GGA_datum.latitude_degrees + GPS_b1->GGA_datum.latitude_degrees ); new_gps->GGA_datum.altitude_meters = 0.5 * ( GPS_a1->GGA_datum.altitude_meters + GPS_b1->GGA_datum.altitude_meters ); new_gps->timestamp = (GPS_a1->timestamp + GPS_b1->timestamp) / 2; i->second[new_gps->sensorLabel] = new_gps; } } } } // end loop interpolate 1-out-of-5 } #if MRPT_HAS_WXWIDGETS wxProgressDialog *progDia3=NULL; if (isGUI) { progDia3 = new wxProgressDialog( wxT("Building map"), wxT("Estimating 6D path..."), N_GPSs, // range NULL, // parent wxPD_CAN_ABORT | wxPD_APP_MODAL | wxPD_SMOOTH | wxPD_AUTO_HIDE | wxPD_ELAPSED_TIME | wxPD_ESTIMATED_TIME | wxPD_REMAINING_TIME); } #endif int idx_in_GPSs = 0; for (TListGPSs::iterator i=list_gps_obs.begin();i!=list_gps_obs.end();++i, idx_in_GPSs++) { // Now check if we have 3 gps with the same time stamp: if (i->second.size()>=3) { const size_t N = i->second.size(); std::map<std::string, CObservationGPSPtr> & GPS = i->second; vector_double X(N),Y(N),Z(N); // Global XYZ coordinates std::map<string,size_t> XYZidxs; // Sensor label -> indices in X Y Z if (!ref_valid) // get the reference lat/lon, if it's not set from rawlog configuration block. { ref_valid = true; ref = GPS.begin()->second->GGA_datum.getAsStruct<TGeodeticCoords>(); } // Compute the XYZ coordinates of all sensors: TMatchingPairList corrs; unsigned int k; std::map<std::string, CObservationGPSPtr>::iterator g_it; for (k=0,g_it=GPS.begin();g_it!=GPS.end();g_it++,k++) { TPoint3D P; mrpt::topography::geodeticToENU_WGS84( g_it->second->GGA_datum.getAsStruct<TGeodeticCoords>(), P, ref ); // Correction of offsets: const string sect = string("OFFSET_")+g_it->second->sensorLabel; P.x += memFil.read_double( sect, "x", 0 ); P.y += memFil.read_double( sect, "y", 0 ); P.z += memFil.read_double( sect, "z", 0 ); XYZidxs[g_it->second->sensorLabel] = k; // Save index correspondence // Create the correspondence: corrs.push_back( TMatchingPair( k,k, // Indices P.x,P.y,P.z, // "This"/Global coords g_it->second->sensorPose.x(),g_it->second->sensorPose.y(),g_it->second->sensorPose.z() // "other"/local coordinates )); X[k] = P.x; Y[k] = P.y; Z[k] = P.z; } if (doConsistencyCheck && GPS.size()==3) { // XYZ[k] have the k'd final coordinates of each GPS // GPS[k] are the CObservations: // Compute the inter-GPS square distances: vector_double iGPSdist2(3); // [0]: sq dist between: D_cov_rev_indexes[0],D_cov_rev_indexes[1] TPoint3D P0( X[XYZidxs[D_cov_rev_indexes[0]]], Y[XYZidxs[D_cov_rev_indexes[0]]], Z[XYZidxs[D_cov_rev_indexes[0]]] ); TPoint3D P1( X[XYZidxs[D_cov_rev_indexes[1]]], Y[XYZidxs[D_cov_rev_indexes[1]]], Z[XYZidxs[D_cov_rev_indexes[1]]] ); TPoint3D P2( X[XYZidxs[D_cov_rev_indexes[2]]], Y[XYZidxs[D_cov_rev_indexes[2]]], Z[XYZidxs[D_cov_rev_indexes[2]]] ); iGPSdist2[0] = P0.sqrDistanceTo(P1); iGPSdist2[1] = P0.sqrDistanceTo(P2); iGPSdist2[2] = P1.sqrDistanceTo(P2); double mahaD = mrpt::math::mahalanobisDistance( iGPSdist2, D_mean, D_cov_1 ); outInfoTemp.mahalabis_quality_measure[i->first] = mahaD; //cout << "x: " << iGPSdist2 << " MU: " << D_mean << " -> " << mahaD << endl; } // end consistency // Use a 6D matching method to estimate the location of the vehicle: CPose3D optimal_pose; double optimal_scale; // "this" (reference map) -> GPS global coordinates // "other" -> GPS local coordinates on the vehicle mrpt::scanmatching::leastSquareErrorRigidTransformation6D( corrs,optimal_pose,optimal_scale, true ); // Force scale=1 //cout << "optimal pose: " << optimal_pose << " " << optimal_scale << endl; // Final vehicle pose: CPose3D &veh_pose= optimal_pose; // Add to the interpolator: MRPT_CHECK_NORMAL_NUMBER( veh_pose.x() ); MRPT_CHECK_NORMAL_NUMBER( veh_pose.y() ); MRPT_CHECK_NORMAL_NUMBER( veh_pose.z() ); MRPT_CHECK_NORMAL_NUMBER( veh_pose.yaw() ); MRPT_CHECK_NORMAL_NUMBER( veh_pose.pitch() ); MRPT_CHECK_NORMAL_NUMBER( veh_pose.roll() ); robot_path.insert( i->first, veh_pose ); // If we have W_star, compute the pose uncertainty: if (doUncertaintyCovs) { CPose3DPDFGaussian final_veh_uncert; final_veh_uncert.mean.setFromValues(0,0,0,0,0,0); final_veh_uncert.cov = outInfoTemp.W_star; // Rotate the covariance according to the real vehicle pose: final_veh_uncert.changeCoordinatesReference(veh_pose); outInfoTemp.vehicle_uncertainty[ i->first ] = final_veh_uncert.cov; } } // Show progress: if ((count++ % 100)==0) { #if MRPT_HAS_WXWIDGETS if (progDia3) { if (!progDia3->Update(idx_in_GPSs)) abort = true; wxTheApp->Yield(); } #endif } } // end for i #if MRPT_HAS_WXWIDGETS if (progDia3) { delete progDia3; progDia3=NULL; } #endif if (PATH_SMOOTH_FILTER>0 && robot_path.size()>1) { CPose3DInterpolator filtered_robot_path = robot_path; // Do Angles smoother filter of the path: // --------------------------------------------- const double MAX_DIST_TO_FILTER = 4.0; for (CPose3DInterpolator::iterator i=robot_path.begin();i!=robot_path.end();++i) { CPose3D p = i->second; vector_double pitchs, rolls; // The elements to average pitchs.push_back(p.pitch()); rolls.push_back(p.roll()); CPose3DInterpolator::iterator q=i; for (int k=0;k<PATH_SMOOTH_FILTER && q!=robot_path.begin();k++) { --q; if (abs( mrpt::system::timeDifference(q->first,i->first))<MAX_DIST_TO_FILTER ) { pitchs.push_back( q->second.pitch() ); rolls.push_back( q->second.roll() ); } } q=i; for (int k=0;k<PATH_SMOOTH_FILTER && q!=(--robot_path.end()) ;k++) { ++q; if (abs( mrpt::system::timeDifference(q->first,i->first))<MAX_DIST_TO_FILTER ) { pitchs.push_back( q->second.pitch() ); rolls.push_back( q->second.roll() ); } } p.setYawPitchRoll(p.yaw(), mrpt::math::averageWrap2Pi(pitchs), mrpt::math::averageWrap2Pi(rolls) ); // save in filtered path: filtered_robot_path.insert( i->first, p ); } // Replace: robot_path = filtered_robot_path; } // end PATH_SMOOTH_FILTER } // end step generate 6D path // Here we can set best_gps_path (that with the max. number of RTK fixed/foat readings): if (outInfo) { string bestLabel; size_t bestNum = 0; for (map<string,size_t>::iterator i=GPS_RTK_reads.begin();i!=GPS_RTK_reads.end();++i) { if (i->second>bestNum) { bestNum = i->second; bestLabel = i->first; } } outInfoTemp.best_gps_path = gps_paths[bestLabel]; // and transform to XYZ: // Correction of offsets: const string sect = string("OFFSET_")+bestLabel; const double off_X = memFil.read_double( sect, "x", 0 ); const double off_Y = memFil.read_double( sect, "y", 0 ); const double off_Z = memFil.read_double( sect, "z", 0 ); // map<TTimeStamp,TPoint3D> best_gps_path; // time -> 3D local coords for (map<TTimeStamp,TPoint3D>::iterator i=outInfoTemp.best_gps_path.begin();i!=outInfoTemp.best_gps_path.end();++i) { TPoint3D P; mrpt::topography::geodeticToENU_WGS84( TGeodeticCoords(i->second.x,i->second.y,i->second.z), // i->second.x,i->second.y,i->second.z, // lat, lon, heigh P, // X Y Z ref ); i->second.x = P.x + off_X; i->second.y = P.y + off_Y; i->second.z = P.z + off_Z; } } // end best_gps_path if (outInfo) *outInfo = outInfoTemp; MRPT_END }
// ------------------------------------------------------ // MapBuilding_ICP // ------------------------------------------------------ void MapBuilding_ICP() { MRPT_TRY_START CTicTac tictac,tictacGlobal,tictac_JH; int step = 0; std::string str; CSensFrameProbSequence finalMap; float t_exec; COccupancyGridMap2D::TEntropyInfo entropy; size_t rawlogEntry = 0; CFileGZInputStream rawlogFile( RAWLOG_FILE.c_str() ); CFileGZOutputStream sensor_data; // --------------------------------- // Constructor // --------------------------------- CMetricMapBuilderICP mapBuilder( &metricMapsOpts, insertionLinDistance, insertionAngDistance, &icpOptions ); mapBuilder.ICP_options.matchAgainstTheGrid = matchAgainstTheGrid; // --------------------------------- // CMetricMapBuilder::TOptions // --------------------------------- mapBuilder.options.verbose = true; mapBuilder.options.enableMapUpdating = true; mapBuilder.options.debugForceInsertion = false; mapBuilder.options.insertImagesAlways = false; // Prepare output directory: // -------------------------------- deleteFilesInDirectory(OUT_DIR); createDirectory(OUT_DIR); // Open log files: // ---------------------------------- CFileOutputStream f_log(format("%s/log_times.txt",OUT_DIR)); CFileOutputStream f_path(format("%s/log_estimated_path.txt",OUT_DIR)); CFileOutputStream f_pathOdo(format("%s/log_odometry_path.txt",OUT_DIR)); // Create 3D window if requested: CDisplayWindow3DPtr win3D; #if MRPT_HAS_WXWIDGETS if (SHOW_PROGRESS_3D_REAL_TIME) { win3D = CDisplayWindow3DPtr( new CDisplayWindow3D("ICP-SLAM @ MRPT C++ Library (C) 2004-2008", 600, 500) ); win3D->setCameraZoom(20); win3D->setCameraAzimuthDeg(-45); } #endif if(OBS_FROM_FILE == 0){ sensor_data.open("sensor_data.rawlog"); printf("Receive From Sensor\n"); initLaser(); printf("OK\n"); } // ---------------------------------------------------------- // Map Building // ---------------------------------------------------------- CActionCollectionPtr action; CSensoryFramePtr observations, temp_obs; CSensoryFramePtr obs_set; CPose2D odoPose(0,0,0); CSimplePointsMap oldMap, newMap; CICP ICP; vector_float accum_x, accum_y, accum_z; // ICP Setting ICP.options.ICP_algorithm = (TICPAlgorithm)ICP_method; ICP.options.maxIterations = 40; ICP.options.thresholdAng = 0.15; ICP.options.thresholdDist = 0.75f; ICP.options.ALFA = 0.30f; ICP.options.smallestThresholdDist = 0.10f; ICP.options.doRANSAC = false; ICP.options.dumpToConsole(); // CObservationPtr obsSick = CObservationPtr(new CObservation2DRangeScan()); CObservationPtr obsHokuyo = CObservationPtr(new CObservation2DRangeScan()); CSimplePointsMap hokuyoMap; bool isFirstTime = true; bool loop = true; /* cout << index << "frame, Input the any key to continue" << endl; getc(stdin); fflush(stdin); */ tictacGlobal.Tic(); while(loop) { /* if(BREAK){ cout << index << "frame, Input the any key to continue" << endl; getc(stdin); fflush(stdin); }else{ if(os::kbhit()) loop = true; } */ if(os::kbhit()) loop = true; if(DELAY) { sleep(15); } // Load action/observation pair from the rawlog: // -------------------------------------------------- if(OBS_FROM_FILE == 1) { if (! CRawlog::readActionObservationPair( rawlogFile, action, temp_obs, rawlogEntry) ) break; // file EOF obsSick = temp_obs->getObservationByIndex(0); obsHokuyo = temp_obs->getObservationByIndex(1); observations = CSensoryFramePtr(new CSensoryFrame()); observations->insert((CObservationPtr)obsSick); hokuyoMap.clear(); hokuyoMap.insertObservation(obsHokuyo.pointer()); }else{ rawlogEntry = rawlogEntry+2; tictac.Tic(); obsSick = CObservationPtr(new CObservation2DRangeScan()); obsHokuyo = CObservationPtr(new CObservation2DRangeScan()); if(!receiveDataFromSensor((CObservation2DRangeScan*)obsSick.pointer(),SICK)){ cout << " Error in receive sensor data" << endl; return; } if(!receiveDataFromSensor((CObservation2DRangeScan*)obsHokuyo.pointer(),HOKUYO)){ cout << " Error in receive sensor data" << endl; return; } cout << "Time to receive data : " << tictac.Tac()*1000.0f << endl; obsSick->timestamp = mrpt::system::now(); obsSick->setSensorPose(CPose3D(0,0,0,DEG2RAD(0),DEG2RAD(0),DEG2RAD(0))); ((CObservation2DRangeScan*)obsSick.pointer())->rightToLeft = true; ((CObservation2DRangeScan*)obsSick.pointer())->stdError = 0.003f; obsHokuyo->timestamp = mrpt::system::now(); obsHokuyo->setSensorPose(CPose3D(0,0,0.4,DEG2RAD(0),DEG2RAD(-90),DEG2RAD(0))); ((CObservation2DRangeScan*)obsHokuyo.pointer())->rightToLeft = true; ((CObservation2DRangeScan*)obsHokuyo.pointer())->stdError = 0.003f; cout << "rawlogEntry : " << rawlogEntry << endl; CActionRobotMovement2D myAction; newMap.clear(); obsSick.pointer()->insertObservationInto(&newMap); if(!isFirstTime){ static float runningTime; static CICP::TReturnInfo info; static CPose2D initial(0,0,0); CPosePDFPtr ICPPdf = ICP.AlignPDF(&oldMap, &newMap, initial, &runningTime, (void*)&info); CPose2D estMean; CMatrixDouble33 estCov; ICPPdf->getCovarianceAndMean(estCov, estMean); printf("ICP run in %.02fms, %d iterations (%.02fms/iter), %.01f%% goodness\n -> ", runningTime*1000, info.nIterations, runningTime*1000.0f/info.nIterations, info.goodness*100 ); cout << "ICP Odometry : " << ICPPdf->getMeanVal() << endl; myAction.estimationMethod = CActionRobotMovement2D::emScan2DMatching; myAction.poseChange = CPosePDFPtr( new CPosePDFGaussian(estMean, estCov)); }else{ isFirstTime = false; } oldMap.clear(); oldMap.copyFrom(newMap); observations = CSensoryFramePtr(new CSensoryFrame()); action = CActionCollectionPtr(new CActionCollection()); observations->insert((CObservationPtr)obsSick); obs_set = CSensoryFramePtr(new CSensoryFrame()); obs_set->insert((CObservationPtr)obsSick); obs_set->insert((CObservationPtr)obsHokuyo); action->insert(myAction); sensor_data << action << obs_set; hokuyoMap.clear(); hokuyoMap.insertObservation(obsHokuyo.pointer()); } if (rawlogEntry>=rawlog_offset) { // Update odometry: { CActionRobotMovement2DPtr act= action->getBestMovementEstimation(); if (act) odoPose = odoPose + act->poseChange->getMeanVal(); } // Execute: // ---------------------------------------- tictac.Tic(); mapBuilder.processActionObservation( *action, *observations ); t_exec = tictac.Tac(); printf("Map building executed in %.03fms\n", 1000.0f*t_exec ); // Info log: // ----------- f_log.printf("%f %i\n",1000.0f*t_exec,mapBuilder.getCurrentlyBuiltMapSize() ); const CMultiMetricMap* mostLikMap = mapBuilder.getCurrentlyBuiltMetricMap(); if (0==(step % LOG_FREQUENCY)) { // Pose log: // ------------- if (SAVE_POSE_LOG) { printf("Saving pose log information..."); mapBuilder.getCurrentPoseEstimation()->saveToTextFile( format("%s/mapbuild_posepdf_%03u.txt",OUT_DIR,step) ); printf("Ok\n"); } } // Save a 3D scene view of the mapping process: if (0==(step % LOG_FREQUENCY) || (SAVE_3D_SCENE || win3D.present())) { CPose3D robotPose; mapBuilder.getCurrentPoseEstimation()->getMean(robotPose); COpenGLScenePtr scene = COpenGLScene::Create(); COpenGLViewportPtr view=scene->getViewport("main"); ASSERT_(view); COpenGLViewportPtr view_map = scene->createViewport("mini-map"); view_map->setBorderSize(2); view_map->setViewportPosition(0.01,0.01,0.35,0.35); view_map->setTransparent(false); { mrpt::opengl::CCamera &cam = view_map->getCamera(); cam.setAzimuthDegrees(-90); cam.setElevationDegrees(90); cam.setPointingAt(robotPose); cam.setZoomDistance(20); cam.setOrthogonal(); } // The ground: mrpt::opengl::CGridPlaneXYPtr groundPlane = mrpt::opengl::CGridPlaneXY::Create(-200,200,-200,200,0,5); groundPlane->setColor(0.4,0.4,0.4); view->insert( groundPlane ); view_map->insert( CRenderizablePtr( groundPlane) ); // A copy // The camera pointing to the current robot pose: if (CAMERA_3DSCENE_FOLLOWS_ROBOT) { scene->enableFollowCamera(true); mrpt::opengl::CCamera &cam = view_map->getCamera(); cam.setAzimuthDegrees(-45); cam.setElevationDegrees(45); cam.setPointingAt(robotPose); } // The maps: { opengl::CSetOfObjectsPtr obj = opengl::CSetOfObjects::Create(); mostLikMap->getAs3DObject( obj ); view->insert(obj); // Only the point map: opengl::CSetOfObjectsPtr ptsMap = opengl::CSetOfObjects::Create(); if (mostLikMap->m_pointsMaps.size()) { mostLikMap->m_pointsMaps[0]->getAs3DObject(ptsMap); view_map->insert( ptsMap ); } } // Draw the robot path: CPose3DPDFPtr posePDF = mapBuilder.getCurrentPoseEstimation(); CPose3D curRobotPose; posePDF->getMean(curRobotPose); { opengl::CSetOfObjectsPtr obj = opengl::stock_objects::RobotPioneer(); obj->setPose( curRobotPose ); view->insert(obj); } { opengl::CSetOfObjectsPtr obj = opengl::stock_objects::RobotPioneer(); obj->setPose( curRobotPose ); view_map->insert( obj ); } // Draw Hokuyo total data { CRenderizablePtr hokuyoRender_t = scene->getByName("hokuyo_total"); if(!hokuyoRender_t){ hokuyoRender_t = CPointCloud::Create(); hokuyoRender_t->setName("hokuyo_total"); hokuyoRender_t->setColor(0,0,1); hokuyoRender_t->setPose( CPose3D(0,0,0) ); getAs<CPointCloud>(hokuyoRender_t)->setPointSize(3); scene->insert( hokuyoRender_t); } for(size_t i =0 ; i < accum_x.size(); i++){ getAs<CPointCloud>(hokuyoRender_t)->insertPoint(accum_x[i], accum_y[i], accum_z[i]); } cout << "accum_x size : " << accum_x.size() << endl; } // Draw Hokuyo Current data plate { CRenderizablePtr hokuyoRender = scene->getByName("hokuyo_cur"); hokuyoRender = CPointCloud::Create(); hokuyoRender->setName("hokuyo_cur"); hokuyoRender->setColor(0,1,0); hokuyoRender->setPose( curRobotPose ); getAs<CPointCloud>(hokuyoRender)->setPointSize(0.1); getAs<CPointCloud>(hokuyoRender)->loadFromPointsMap(&hokuyoMap); scene->insert( hokuyoRender); vector_float cur_x = getAs<CPointCloud>(hokuyoRender)->getArrayX(); vector_float cur_y = getAs<CPointCloud>(hokuyoRender)->getArrayY(); vector_float cur_z = getAs<CPointCloud>(hokuyoRender)->getArrayZ(); // cout << "current pose : " << curRobotPose << endl; for(size_t i =0 ; i < cur_x.size(); i++){ /* float rotate_x = cur_x[i]+ curRobotPose.y()*sin(curRobotPose.yaw()*3.14/180.0); float rotate_y = cur_y[i]+ curRobotPose.y()*cos(curRobotPose.yaw()*3.14/180.0); */ float rotate_x = curRobotPose.x() + cur_y[i]*sin(-curRobotPose.yaw()); float rotate_y = curRobotPose.y() + cur_y[i]*cos(-curRobotPose.yaw()); // printf("cur_x, cur_y, cur_z : %f %f %f \n", rotate_x,rotate_y, cur_z[i]); accum_x.push_back(rotate_x); accum_y.push_back(rotate_y); accum_z.push_back(cur_z[i]); } } // Save as file: if (0==(step % LOG_FREQUENCY) && SAVE_3D_SCENE) { CFileGZOutputStream f( format( "%s/buildingmap_%05u.3Dscene",OUT_DIR,step )); f << *scene; } // Show 3D? if (win3D) { opengl::COpenGLScenePtr &ptrScene = win3D->get3DSceneAndLock(); ptrScene = scene; win3D->unlockAccess3DScene(); // Move camera: win3D->setCameraPointingToPoint( curRobotPose.x(),curRobotPose.y(),curRobotPose.z() ); // Update: win3D->forceRepaint(); sleep( SHOW_PROGRESS_3D_REAL_TIME_DELAY_MS ); } } // Save the memory usage: // ------------------------------------------------------------------ { printf("Saving memory usage..."); unsigned long memUsage = getMemoryUsage(); FILE *f=os::fopen( format("%s/log_MemoryUsage.txt",OUT_DIR).c_str() ,"at"); if (f) { os::fprintf(f,"%u\t%lu\n",step,memUsage); os::fclose(f); } printf("Ok! (%.04fMb)\n", ((float)memUsage)/(1024*1024) ); } // Save the robot estimated pose for each step: f_path.printf("%i %f %f %f\n", step, mapBuilder.getCurrentPoseEstimation()->getMeanVal().x(), mapBuilder.getCurrentPoseEstimation()->getMeanVal().y(), mapBuilder.getCurrentPoseEstimation()->getMeanVal().yaw() ); f_pathOdo.printf("%i %f %f %f\n",step,odoPose.x(),odoPose.y(),odoPose.phi()); } // end of if "rawlog_offset"... step++; printf("\n---------------- STEP %u | RAWLOG ENTRY %u ----------------\n",step, (unsigned)rawlogEntry); // Free memory: action.clear_unique(); observations.clear_unique(); }; printf("\n---------------- END!! (total time: %.03f sec) ----------------\n",tictacGlobal.Tac()); // hokuyo.turnOff(); sick.stop(); // Save map: mapBuilder.getCurrentlyBuiltMap(finalMap); str = format("%s/_finalmap_.simplemap",OUT_DIR); printf("Dumping final map in binary format to: %s\n", str.c_str() ); mapBuilder.saveCurrentMapToFile(str); CMultiMetricMap *finalPointsMap = mapBuilder.getCurrentlyBuiltMetricMap(); str = format("%s/_finalmaps_.txt",OUT_DIR); printf("Dumping final metric maps to %s_XXX\n", str.c_str() ); finalPointsMap->saveMetricMapRepresentationToFile( str ); if (win3D) win3D->waitForKey(); MRPT_TRY_END }
// Convert from observations only to actions-SF format: void xRawLogViewerFrame::OnMenuConvertSF(wxCommandEvent& event) { WX_START_TRY bool onlyOnePerLabel = (wxYES==wxMessageBox( _("Keep only one observation of each label within each sensoryframe?"), _("Convert to sensoryframe's"),wxYES_NO, this )); wxString strMaxL= wxGetTextFromUser( _("Maximum length of each sensoryframe (seconds):"), _("Convert to sensoryframe's"), _("1.0") ); double maxLengthSF; strMaxL.ToDouble(&maxLengthSF); // Process: CRawlog new_rawlog; new_rawlog.setCommentText( rawlog.getCommentText() ); wxBusyCursor waitCursor; unsigned int nEntries = (unsigned int)rawlog.size(); wxProgressDialog progDia( wxT("Progress"), wxT("Parsing rawlog..."), nEntries, // 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 CSensoryFrame SF_new; set<string> SF_new_labels; TTimeStamp SF_new_first_t = INVALID_TIMESTAMP; CObservationOdometryPtr last_sf_odo, cur_sf_odo; for (unsigned int countLoop=0;countLoop<nEntries;countLoop++) { if (countLoop % 20 == 0) { if (!progDia.Update( countLoop, wxString::Format(wxT("Parsing rawlog... %u objects"),countLoop ) )) { return; } wxTheApp->Yield(); // Let the app. process messages } switch (rawlog.getType(countLoop)) { case CRawlog::etSensoryFrame: case CRawlog::etActionCollection: { THROW_EXCEPTION("The rawlog already has sensory frames and/or actions!"); } break; case CRawlog::etObservation: { CObservationPtr o = rawlog.getAsObservation(countLoop); // Update stats: bool label_existed = SF_new_labels.find(o->sensorLabel)!=SF_new_labels.end(); double SF_len = SF_new_first_t == INVALID_TIMESTAMP ? 0 : timeDifference(SF_new_first_t, o->timestamp); // Decide: // End SF and start a new one? if ( SF_len>maxLengthSF && SF_new.size()!=0) { new_rawlog.addObservations(SF_new); // Odometry increments: CActionCollection acts; if (last_sf_odo && cur_sf_odo) { CActionRobotMovement2D act; act.timestamp = cur_sf_odo->timestamp; CActionRobotMovement2D::TMotionModelOptions opts; act.computeFromOdometry( cur_sf_odo->odometry - last_sf_odo->odometry,opts); acts.insert(act); } new_rawlog.addActions(acts); last_sf_odo = cur_sf_odo; cur_sf_odo.clear_unique(); SF_new.clear(); SF_new_labels.clear(); SF_new_first_t = INVALID_TIMESTAMP; } // Insert into SF: if (!onlyOnePerLabel || !label_existed) { SF_new.insert(o); SF_new_labels.insert(o->sensorLabel); } if (SF_new_first_t == INVALID_TIMESTAMP) SF_new_first_t = o->timestamp; if (o->GetRuntimeClass() ==CLASS_ID(CObservationOdometry) ) { cur_sf_odo = CObservationOdometryPtr(o); } } break; default: break; } // end for each entry } // end while keep loading // Remaining obs: if (SF_new.size()) { new_rawlog.addObservations(SF_new); SF_new.clear(); } progDia.Update( nEntries ); // Update: rawlog.moveFrom(new_rawlog); rebuildTreeView(); WX_END_TRY }
// ---------------------------------------------------- // The online/offline grabbing thread // ---------------------------------------------------- void thread_grabbing(TThreadParam &p) { try { #if MRPT_HAS_CXX11 typedef std::unique_ptr<CKinect> CKinectPtr; // This assures automatic destruction #else typedef std::auto_ptr<CKinect> CKinectPtr; // This assures automatic destruction #endif // Only one of these will be actually used: CKinectPtr kinect; CFileGZInputStream dataset; mrpt::system::TTimeStamp dataset_prev_tim = INVALID_TIMESTAMP; mrpt::system::TTimeStamp my_last_read_obs_tim = INVALID_TIMESTAMP; if (p.is_online) { // Online: // --------------------- kinect = CKinectPtr(new CKinect()); // Set params: kinect->enableGrabRGB(true); kinect->enableGrabDepth(true); kinect->enableGrabAccelerometers(false); kinect->enableGrab3DPoints(p.generate_3D_pointcloud_in_this_thread); // Open: cout << "Calling CKinect::initialize()..."; kinect->initialize(); cout << "OK\n"; } else { // Offline: // --------------------- if (!dataset.open(p.rawlog_file)) throw std::runtime_error("Couldn't open rawlog dataset file for input..."); // Set external images directory: CImage::IMAGES_PATH_BASE = CRawlog::detectImagesDirectory(p.rawlog_file); } CTicTac tictac; int nImgs = 0; while (!p.quit) { if (p.is_online) { // Grab new observation from the camera: bool there_is_obs=true, hard_error=false; CObservation3DRangeScanPtr obs = CObservation3DRangeScan::Create(); // Smart pointers to observations. Memory pooling is used internally kinect->getNextObservation(*obs,there_is_obs,hard_error); if(hard_error) throw std::runtime_error("Sensor returned 'hardware error'"); else if (there_is_obs) { // Send object to the main thread: p.new_obs.set(obs); } } else { // Offline: CObservationPtr obs; do { try { dataset >> obs; } catch (std::exception &e) { throw std::runtime_error( string("\nError reading from dataset file (EOF?):\n")+string(e.what()) ); } ASSERT_(obs.present()) } while (!IS_CLASS(obs,CObservation3DRangeScan)); // We have one observation: CObservation3DRangeScanPtr obs3D = CObservation3DRangeScanPtr(obs); obs3D->load(); // *Important* This is needed to load the range image if stored as a separate file. // Do we have to wait to emulate real-time behavior? const mrpt::system::TTimeStamp cur_tim = obs3D->timestamp; const mrpt::system::TTimeStamp now_tim = mrpt::system::now(); if (dataset_prev_tim!=INVALID_TIMESTAMP && my_last_read_obs_tim!=INVALID_TIMESTAMP) { #ifndef FAKE_KINECT_FPS_RATE const double At_dataset = mrpt::system::timeDifference( dataset_prev_tim, cur_tim ); #else const double At_dataset = 1.0/FAKE_KINECT_FPS_RATE; #endif const double At_actual = mrpt::system::timeDifference( my_last_read_obs_tim, now_tim ); const double need_to_wait_ms = 1000.*( At_dataset-At_actual ); //cout << "[Kinect grab thread] Need to wait (ms): " << need_to_wait_ms << endl; if (need_to_wait_ms>0) mrpt::system::sleep( need_to_wait_ms ); } // Send observation to main thread: p.new_obs.set(obs3D); dataset_prev_tim = cur_tim; my_last_read_obs_tim = mrpt::system::now(); } // Update Hz rate estimate nImgs++; if (nImgs>10) { p.Hz = nImgs / tictac.Tac(); nImgs=0; tictac.Tic(); } } } catch(std::exception &e) { cout << "Exception in Kinect thread: " << e.what() << endl; p.quit = true; } }
// ------------------------------------------------------ // MapBuilding_ICP // override_rawlog_file: If not empty, use that rawlog // instead of that in the config file. // ------------------------------------------------------ void MapBuilding_ICP(const string &INI_FILENAME, const string &override_rawlog_file) { MRPT_START CConfigFile iniFile(INI_FILENAME); // ------------------------------------------ // Load config from file: // ------------------------------------------ const string RAWLOG_FILE = !override_rawlog_file.empty() ? override_rawlog_file : iniFile.read_string("MappingApplication","rawlog_file","", /*Force existence:*/ true); const unsigned int rawlog_offset = iniFile.read_int("MappingApplication","rawlog_offset",0, /*Force existence:*/ true); const string OUT_DIR_STD = iniFile.read_string("MappingApplication","logOutput_dir","log_out", /*Force existence:*/ true); const int LOG_FREQUENCY = iniFile.read_int("MappingApplication","LOG_FREQUENCY",5, /*Force existence:*/ true); const bool SAVE_POSE_LOG = iniFile.read_bool("MappingApplication","SAVE_POSE_LOG", false, /*Force existence:*/ true); const bool SAVE_3D_SCENE = iniFile.read_bool("MappingApplication","SAVE_3D_SCENE", false, /*Force existence:*/ true); const bool CAMERA_3DSCENE_FOLLOWS_ROBOT = iniFile.read_bool("MappingApplication","CAMERA_3DSCENE_FOLLOWS_ROBOT", true, /*Force existence:*/ true); bool SHOW_PROGRESS_3D_REAL_TIME = false; int SHOW_PROGRESS_3D_REAL_TIME_DELAY_MS = 0; MRPT_LOAD_CONFIG_VAR( SHOW_PROGRESS_3D_REAL_TIME, bool, iniFile, "MappingApplication"); MRPT_LOAD_CONFIG_VAR( SHOW_PROGRESS_3D_REAL_TIME_DELAY_MS, int, iniFile, "MappingApplication"); const char* OUT_DIR = OUT_DIR_STD.c_str(); // ------------------------------------ // Constructor of ICP-SLAM object // ------------------------------------ CMetricMapBuilderICP mapBuilder; mapBuilder.ICP_options.loadFromConfigFile( iniFile, "MappingApplication"); mapBuilder.ICP_params.loadFromConfigFile ( iniFile, "ICP"); // Construct the maps with the loaded configuration. mapBuilder.initialize(); // --------------------------------- // CMetricMapBuilder::TOptions // --------------------------------- mapBuilder.options.verbose = true; mapBuilder.options.alwaysInsertByClass.fromString( iniFile.read_string("MappingApplication","alwaysInsertByClass","") ); // Print params: printf("Running with the following parameters:\n"); printf(" RAWLOG file:'%s'\n", RAWLOG_FILE.c_str()); printf(" Output directory:\t\t\t'%s'\n",OUT_DIR); printf(" matchAgainstTheGrid:\t\t\t%c\n", mapBuilder.ICP_options.matchAgainstTheGrid ? 'Y':'N'); printf(" Log record freq:\t\t\t%u\n",LOG_FREQUENCY); printf(" SAVE_3D_SCENE:\t\t\t%c\n", SAVE_3D_SCENE ? 'Y':'N'); printf(" SAVE_POSE_LOG:\t\t\t%c\n", SAVE_POSE_LOG ? 'Y':'N'); printf(" CAMERA_3DSCENE_FOLLOWS_ROBOT:\t%c\n",CAMERA_3DSCENE_FOLLOWS_ROBOT ? 'Y':'N'); printf("\n"); mapBuilder.ICP_params.dumpToConsole(); mapBuilder.ICP_options.dumpToConsole(); // Checks: ASSERT_(RAWLOG_FILE.size()>0); ASSERT_(fileExists(RAWLOG_FILE)); CTicTac tictac,tictacGlobal,tictac_JH; int step = 0; string str; CSimpleMap finalMap; float t_exec; COccupancyGridMap2D::TEntropyInfo entropy; size_t rawlogEntry = 0; CFileGZInputStream rawlogFile( RAWLOG_FILE.c_str() ); // Prepare output directory: // -------------------------------- deleteFilesInDirectory(OUT_DIR); createDirectory(OUT_DIR); // Open log files: // ---------------------------------- CFileOutputStream f_log(format("%s/log_times.txt",OUT_DIR)); CFileOutputStream f_path(format("%s/log_estimated_path.txt",OUT_DIR)); CFileOutputStream f_pathOdo(format("%s/log_odometry_path.txt",OUT_DIR)); // Create 3D window if requested: CDisplayWindow3DPtr win3D; #if MRPT_HAS_WXWIDGETS if (SHOW_PROGRESS_3D_REAL_TIME) { win3D = CDisplayWindow3D::Create("ICP-SLAM @ MRPT C++ Library", 600, 500); win3D->setCameraZoom(20); win3D->setCameraAzimuthDeg(-45); } #endif // ---------------------------------------------------------- // Map Building // ---------------------------------------------------------- CPose2D odoPose(0,0,0); tictacGlobal.Tic(); for (;;) { CActionCollectionPtr action; CSensoryFramePtr observations; CObservationPtr observation; if (os::kbhit()) { char c = os::getch(); if (c==27) break; } // Load action/observation pair from the rawlog: // -------------------------------------------------- if (! CRawlog::getActionObservationPairOrObservation( rawlogFile, action, observations, observation, rawlogEntry) ) break; // file EOF const bool isObsBasedRawlog = observation.present(); if (rawlogEntry>=rawlog_offset) { // Update odometry: if (isObsBasedRawlog) { static CPose2D lastOdo; static bool firstOdo = true; if (IS_CLASS(observation,CObservationOdometry)) { CObservationOdometryPtr o = CObservationOdometryPtr(observation); if (!firstOdo) odoPose = odoPose + (o->odometry - lastOdo); lastOdo=o->odometry; firstOdo=false; } } else { CActionRobotMovement2DPtr act= action->getBestMovementEstimation(); if (act) odoPose = odoPose + act->poseChange->getMeanVal(); } // Execute: // ---------------------------------------- tictac.Tic(); if (isObsBasedRawlog) mapBuilder.processObservation( observation ); else mapBuilder.processActionObservation( *action, *observations ); t_exec = tictac.Tac(); printf("Map building executed in %.03fms\n", 1000.0f*t_exec ); // Info log: // ----------- f_log.printf("%f %i\n",1000.0f*t_exec,mapBuilder.getCurrentlyBuiltMapSize() ); const CMultiMetricMap* mostLikMap = mapBuilder.getCurrentlyBuiltMetricMap(); if (0==(step % LOG_FREQUENCY)) { // Pose log: // ------------- if (SAVE_POSE_LOG) { printf("Saving pose log information..."); mapBuilder.getCurrentPoseEstimation()->saveToTextFile( format("%s/mapbuild_posepdf_%03u.txt",OUT_DIR,step) ); printf("Ok\n"); } } // Save a 3D scene view of the mapping process: if (0==(step % LOG_FREQUENCY) || (SAVE_3D_SCENE || win3D.present())) { CPose3D robotPose; mapBuilder.getCurrentPoseEstimation()->getMean(robotPose); COpenGLScenePtr scene = COpenGLScene::Create(); COpenGLViewportPtr view=scene->getViewport("main"); ASSERT_(view); COpenGLViewportPtr view_map = scene->createViewport("mini-map"); view_map->setBorderSize(2); view_map->setViewportPosition(0.01,0.01,0.35,0.35); view_map->setTransparent(false); { mrpt::opengl::CCamera &cam = view_map->getCamera(); cam.setAzimuthDegrees(-90); cam.setElevationDegrees(90); cam.setPointingAt(robotPose); cam.setZoomDistance(20); cam.setOrthogonal(); } // The ground: mrpt::opengl::CGridPlaneXYPtr groundPlane = mrpt::opengl::CGridPlaneXY::Create(-200,200,-200,200,0,5); groundPlane->setColor(0.4,0.4,0.4); view->insert( groundPlane ); view_map->insert( CRenderizablePtr( groundPlane) ); // A copy // The camera pointing to the current robot pose: if (CAMERA_3DSCENE_FOLLOWS_ROBOT) { scene->enableFollowCamera(true); mrpt::opengl::CCamera &cam = view_map->getCamera(); cam.setAzimuthDegrees(-45); cam.setElevationDegrees(45); cam.setPointingAt(robotPose); } // The maps: { opengl::CSetOfObjectsPtr obj = opengl::CSetOfObjects::Create(); mostLikMap->getAs3DObject( obj ); view->insert(obj); // Only the point map: opengl::CSetOfObjectsPtr ptsMap = opengl::CSetOfObjects::Create(); if (mostLikMap->m_pointsMaps.size()) { mostLikMap->m_pointsMaps[0]->getAs3DObject(ptsMap); view_map->insert( ptsMap ); } } // Draw the robot path: CPose3DPDFPtr posePDF = mapBuilder.getCurrentPoseEstimation(); CPose3D curRobotPose; posePDF->getMean(curRobotPose); { opengl::CSetOfObjectsPtr obj = opengl::stock_objects::RobotPioneer(); obj->setPose( curRobotPose ); view->insert(obj); } { opengl::CSetOfObjectsPtr obj = opengl::stock_objects::RobotPioneer(); obj->setPose( curRobotPose ); view_map->insert( obj ); } // Save as file: if (0==(step % LOG_FREQUENCY) && SAVE_3D_SCENE) { CFileGZOutputStream f( format( "%s/buildingmap_%05u.3Dscene",OUT_DIR,step )); f << *scene; } // Show 3D? if (win3D) { opengl::COpenGLScenePtr &ptrScene = win3D->get3DSceneAndLock(); ptrScene = scene; win3D->unlockAccess3DScene(); // Move camera: win3D->setCameraPointingToPoint( curRobotPose.x(),curRobotPose.y(),curRobotPose.z() ); // Update: win3D->forceRepaint(); sleep( SHOW_PROGRESS_3D_REAL_TIME_DELAY_MS ); } } // Save the memory usage: // ------------------------------------------------------------------ { printf("Saving memory usage..."); unsigned long memUsage = getMemoryUsage(); FILE *f=os::fopen( format("%s/log_MemoryUsage.txt",OUT_DIR).c_str() ,"at"); if (f) { os::fprintf(f,"%u\t%lu\n",step,memUsage); os::fclose(f); } printf("Ok! (%.04fMb)\n", ((float)memUsage)/(1024*1024) ); } // Save the robot estimated pose for each step: f_path.printf("%i %f %f %f\n", step, mapBuilder.getCurrentPoseEstimation()->getMeanVal().x(), mapBuilder.getCurrentPoseEstimation()->getMeanVal().y(), mapBuilder.getCurrentPoseEstimation()->getMeanVal().yaw() ); f_pathOdo.printf("%i %f %f %f\n",step,odoPose.x(),odoPose.y(),odoPose.phi()); } // end of if "rawlog_offset"... step++; printf("\n---------------- STEP %u | RAWLOG ENTRY %u ----------------\n",step, (unsigned)rawlogEntry); }; printf("\n---------------- END!! (total time: %.03f sec) ----------------\n",tictacGlobal.Tac()); // Save map: mapBuilder.getCurrentlyBuiltMap(finalMap); str = format("%s/_finalmap_.simplemap",OUT_DIR); printf("Dumping final map in binary format to: %s\n", str.c_str() ); mapBuilder.saveCurrentMapToFile(str); CMultiMetricMap *finalPointsMap = mapBuilder.getCurrentlyBuiltMetricMap(); str = format("%s/_finalmaps_.txt",OUT_DIR); printf("Dumping final metric maps to %s_XXX\n", str.c_str() ); finalPointsMap->saveMetricMapRepresentationToFile( str ); if (win3D) win3D->waitForKey(); MRPT_END }