// Show GPS path in a window:
void xRawLogViewerFrame::OnMenuDrawGPSPath(wxCommandEvent& event)
{
WX_START_TRY
string the_label = AskForObservationByLabel("Choose the GPS to use:");
size_t i, M = 0, n = rawlog.size();
TGeodeticCoords ref;
bool ref_valid = false;
// Ask the user for the reference?
if (wxYES!=wxMessageBox(_("Do you want to take the GPS reference automatically from the first found entry?"),_("GPS path"),wxYES_NO ))
{
wxString s = wxGetTextFromUser(
_("Reference Latitude (degrees):"),
_("GPS reference"),
_("0.0"), this );
if (s.IsEmpty()) return;
if (!s.ToDouble(&ref.lat.decimal_value)) { wxMessageBox(_("Invalid number")); return; }
s = wxGetTextFromUser(
_("Reference Longitude (degrees):"),
_("GPS reference"),
_("0.0"), this );
if (s.IsEmpty()) return;
if (!s.ToDouble(&ref.lon.decimal_value)) { wxMessageBox(_("Invalid number")); return; }
s = wxGetTextFromUser(
_("Reference Height (meters):"),
_("GPS reference"),
_("0.0"), this );
if (s.IsEmpty()) return;
if (!s.ToDouble(&ref.height)) { wxMessageBox(_("Invalid number")); return; }
ref_valid=true;
}
// Only RTK fixed?
bool only_rtk = wxYES==wxMessageBox(_("Take into account 'rtk' (modes 4-5) readings only?"),_("GPS path"),wxYES_NO );
vector<float> xs,ys,zs;
double overall_distance = 0;
for (i=0;i<n;i++)
{
CObservationGPSPtr obs;
switch ( rawlog.getType(i) )
{
case CRawlog::etSensoryFrame:
{
CSensoryFramePtr sf = rawlog.getAsObservations(i);
CObservationPtr o= sf->getObservationBySensorLabel(the_label);
if (o && IS_CLASS(o,CObservationGPS))
{
obs = CObservationGPSPtr(o);
}
}
break;
case CRawlog::etObservation:
{
CObservationPtr o = rawlog.getAsObservation(i);
if ( !os::_strcmpi(o->sensorLabel.c_str(), the_label.c_str()) && IS_CLASS(o,CObservationGPS))
{
obs = CObservationGPSPtr(o);
}
}
break;
default: break;
}
// If we had a GPS obs, process it:
if (obs && obs->has_GGA_datum && (!only_rtk || obs->GGA_datum.fix_quality==4 || obs->GGA_datum.fix_quality==5) )
{
TPoint3D X_ENU; // Transformed coordinates
const TGeodeticCoords obsCoords = obs->GGA_datum.getAsStruct<TGeodeticCoords>();
// The first gps datum?
if (!ref_valid)
{
ref_valid=true;
ref = obsCoords;
}
// Local XYZ coordinates transform:
geodeticToENU_WGS84( obsCoords, X_ENU, ref );
// Geocentric XYZ:
TPoint3D X_geo;
geodeticToGeocentric_WGS84( obsCoords, X_geo);
if (!xs.empty())
overall_distance+=sqrt( square(X_ENU.x-*xs.rbegin())+square(X_ENU.y-*ys.rbegin())+square(X_ENU.z-*zs.rbegin()) );
xs.push_back(X_ENU.x);
ys.push_back(X_ENU.y);
zs.push_back(X_ENU.z);
M++;
}
}
// Window 3d:
winGPSPath = CDisplayWindow3D::Create(format("GPS path, %i points (%s) %.03f meters length", int(M), the_label.c_str(), overall_distance ) );
COpenGLScene scene;
CPointCloudPtr gl_path = CPointCloud::Create();
gl_path->setAllPoints(xs,ys,zs);
gl_path->setColor(0,0,1);
gl_path->setPointSize(3);
scene.insert( gl_path );
scene.insert( CGridPlaneXYPtr( CGridPlaneXY::Create(-300,300,-300,300,0,10)));
scene.insert( CAxisPtr( CAxis::Create(-300,-300,-50, 300,300,50, 1.0, 3, true ) ) );
COpenGLScenePtr the_scene = winGPSPath->get3DSceneAndLock();
*the_scene = scene;
winGPSPath->unlockAccess3DScene();
winGPSPath->repaint();
// 2D wins:
winGPSPath2D_xy = CDisplayWindowPlots::Create( format("GPS path - XY (%s)", the_label.c_str() ) );
winGPSPath2D_xy->plot(xs,ys,"b");
winGPSPath2D_xy->axis_fit(true);
winGPSPath2D_xz = CDisplayWindowPlots::Create( format("GPS path - XZ (%s)", the_label.c_str() ) );
winGPSPath2D_xz->plot(xs,zs,"b");
winGPSPath2D_xz->axis_fit(true);
WX_END_TRY
}
void xRawLogViewerFrame::OnMenuShiftTimestampsByLabel(wxCommandEvent& event)
{
WX_START_TRY
wxMessageBox(_("The timestamps of all the observations of a given sensor label will be shifted a given number of seconds. Press OK to continue."));
vector_string the_labels = AskForObservationByLabelMultiple("Choose the sensor(s):");
if (the_labels.empty()) return;
wxString s = wxGetTextFromUser(
_("Enter the number of seconds to shift (can have fractions, be positive or negative)"),
_("Timestamp shift"),
_("0.0"), this );
if (s.IsEmpty()) return;
double delta_time_secs;
if (!s.ToDouble(&delta_time_secs))
{
wxMessageBox(_("Invalid number"));
return;
}
size_t i,n = rawlog.size();
unsigned int nChanges = 0;
TTimeStamp DeltaTime = mrpt::system::secondsToTimestamp( delta_time_secs );
for (i=0;i<n;i++)
{
switch ( rawlog.getType(i) )
{
case CRawlog::etSensoryFrame:
{
CSensoryFramePtr sf = rawlog.getAsObservations(i);
CObservationPtr o;
for (size_t k=0;k<the_labels.size();k++)
{
size_t idx = 0;
while ( (o=sf->getObservationBySensorLabel(the_labels[k],idx++)).present() )
{
o->timestamp += DeltaTime;
nChanges++;
}
}
}
break;
case CRawlog::etObservation:
{
CObservationPtr o = rawlog.getAsObservation(i);
for (size_t k=0;k<the_labels.size();k++)
if (o->sensorLabel==the_labels[k])
{
o->timestamp += DeltaTime;
nChanges++;
}
}
break;
default:
break;
} // end switch type
} // end for
if (wxYES==wxMessageBox( wxString::Format(_("%u changes. Do you want to re-order by timestamp?"),nChanges), _("Done"),wxYES_NO, this ))
{
OnMenuResortByTimestamp(event);
}
WX_END_TRY
}
void xRawLogViewerFrame::OnMenuDistanceBtwGPSs(wxCommandEvent& event)
{
WX_START_TRY
wxMessageBox(_("It will be measured the distance between two GPSs, assuming they are fixed on the vehicle,\n and using only RTK fixed observations."));
if (listOfSensorLabels.empty())
{
wxMessageBox(_("No sensors were found with proper sensor labels. Labels are required for this operation."));
return;
}
// List of labels:
wxArrayString lstLabels;
for (std::map<std::string,TInfoPerSensorLabel>::iterator i=listOfSensorLabels.begin();i!=listOfSensorLabels.end();++i)
lstLabels.Add( _U( i->first.c_str() ) );
wxString ret = wxGetSingleChoice(
_("Choose the first GPS:"),
_("Sensor Labels"),
lstLabels,
this );
if (ret.IsEmpty()) return;
string gps1 = string(ret.mb_str());
ret = wxGetSingleChoice(
_("Choose the second GPS:"),
_("Sensor Labels"),
lstLabels,
this );
if (ret.IsEmpty()) return;
string gps2 = string(ret.mb_str());
size_t i, n = rawlog.size();
// Look for the 2 observations:
CObservationGPSPtr last_GPS1, last_GPS2;
vector_double dists;
TGeodeticCoords refCoords(0,0,0);
// Load configuration block:
CConfigFileMemory memFil;
rawlog.getCommentTextAsConfigFile(memFil);
refCoords.lat = memFil.read_double("GPS_ORIGIN","lat_deg",0);
refCoords.lon = memFil.read_double("GPS_ORIGIN","lon_deg",0);
refCoords.height = memFil.read_double("GPS_ORIGIN","height",0);
bool ref_valid = !refCoords.isClear();
for (i=0;i<n;i++)
{
switch ( rawlog.getType(i) )
{
case CRawlog::etSensoryFrame:
{
CSensoryFramePtr sf = rawlog.getAsObservations(i);
if (!ref_valid)
{
CObservationGPSPtr o = sf->getObservationByClass<CObservationGPS>();
if (o && o->has_GGA_datum)
{
refCoords = o->GGA_datum.getAsStruct<TGeodeticCoords>();
ref_valid = true;
}
}
CObservationPtr o1 = sf->getObservationBySensorLabel(gps1);
CObservationPtr o2 = sf->getObservationBySensorLabel(gps2);
if (o1)
{
ASSERT_(o1->GetRuntimeClass()==CLASS_ID(CObservationGPS));
CObservationGPSPtr obs = CObservationGPSPtr(o1);
if (obs->has_GGA_datum && obs->GGA_datum.fix_quality==4)
last_GPS1 = obs;
}
if (o2)
{
ASSERT_(o2->GetRuntimeClass()==CLASS_ID(CObservationGPS));
CObservationGPSPtr obs = CObservationGPSPtr(o2);
if (obs->has_GGA_datum && obs->GGA_datum.fix_quality==4)
last_GPS2 = obs;
}
}
break;
case CRawlog::etObservation:
{
CObservationPtr o = rawlog.getAsObservation(i);
if (!ref_valid && IS_CLASS(o,CObservationGPS))
{
CObservationGPSPtr ob = CObservationGPSPtr(o);
if (ob && ob->has_GGA_datum)
{
refCoords = ob->GGA_datum.getAsStruct<TGeodeticCoords>();
ref_valid = true;
}
}
if (o->sensorLabel == gps1)
{
ASSERT_(IS_CLASS(o,CObservationGPS));
CObservationGPSPtr obs = CObservationGPSPtr(o);
if (obs->has_GGA_datum && obs->GGA_datum.fix_quality==4)
last_GPS1 = obs;
}
if (o->sensorLabel == gps2)
{
ASSERT_(IS_CLASS(o,CObservationGPS));
CObservationGPSPtr obs = CObservationGPSPtr(o);
if (obs->has_GGA_datum && obs->GGA_datum.fix_quality==4)
last_GPS2 = obs;
}
}
break;
default:
break;
} // end switch type
// Now check if we have 2 gps with the same time stamp:
if (last_GPS1 && last_GPS2)
{
if (last_GPS1->GGA_datum.UTCTime == last_GPS2->GGA_datum.UTCTime)
{
// Compute distance:
TPoint3D p1;
mrpt::topography::geodeticToENU_WGS84(
last_GPS1->GGA_datum.getAsStruct<TGeodeticCoords>(),
p1,
refCoords);
TPoint3D p2;
mrpt::topography::geodeticToENU_WGS84(
last_GPS2->GGA_datum.getAsStruct<TGeodeticCoords>(),
p2,
refCoords);
// Fix offset:
p1.x += memFil.read_double( string("OFFSET_")+last_GPS1->sensorLabel, "x", 0 );
p1.y += memFil.read_double( string("OFFSET_")+last_GPS1->sensorLabel, "y", 0 );
p1.z += memFil.read_double( string("OFFSET_")+last_GPS1->sensorLabel, "z", 0 );
p2.x += memFil.read_double( string("OFFSET_")+last_GPS2->sensorLabel, "x", 0 );
p2.y += memFil.read_double( string("OFFSET_")+last_GPS2->sensorLabel, "y", 0 );
p2.z += memFil.read_double( string("OFFSET_")+last_GPS2->sensorLabel, "z", 0 );
double d = mrpt::math::distance(p1,p2);
dists.push_back(d);
last_GPS1.clear_unique();
last_GPS2.clear_unique();
}
}
} // end for
if (dists.empty())
{
wxMessageBox(_("No valid GPS observations were found."),_("Done"),wxOK,this);
}
else
{
double d_mean,d_std;
mrpt::math::meanAndStd(dists,d_mean,d_std);
wxMessageBox(_U(
format("The distance between GPS sensors is %.04fm, with\n a sigma=%.04fm, average from %u entries.",
d_mean,d_std, (unsigned)dists.size()).c_str() ),_("Done"),wxOK,this);
}
WX_END_TRY
}