// ------------------------------------------------------------
// 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
}
