TrackPoint RouteInfo::nearestPoint(const QDateTime& p_time)
{
int idx = nearestIndex(p_time);
if(idx > 0 &&
idx < m_trackpts.count() &&
abs(p_time.secsTo(m_trackpts.at(idx).time)) < 60)
{
return m_trackpts.at(idx);
}
return TrackPoint();
}
bool VisionPipeLine::stage_localization()
{
_trackpoints.clear();
for (int pos =0; pos<_bloblist.size() && pos<MAX_TRACKED_BLOB_CNT; ++pos)
{
CvPoint2D32f destPos = _posEstimator(_bloblist[pos]._center);
CvPoint2D32f destPosR = _posEstimator(cvPoint2D32f(_bloblist[pos]._center.x, _bloblist[pos]._center.y + sqrt(_bloblist[pos]._area)));
float pressure = abs(destPosR.y - destPos.y);
if (pressure < 5) continue;
_trackpoints.push_back( TrackPoint(destPos.x, destPos.y, pressure));
}
return true;
}
TrackList::TrackList(QFile &input)
{
if (!input.exists()) {
throw new Exception(QString("File not found: <%1>").arg(input.fileName()));
}
QDomDocument d;
d.setContent(&input);
QDomElement root = d.documentElement();
QDomElement pointNode, segmentNode, trackNode;
Track t;
// <gpx
// <trk><name>АВГ-30-09 14:15:47</name>
// <trkseg>
// <trkpt lat="54.11111" lon="37.11111"><ele>112.13</ele><time>2009-08-29T15:36:42Z</time></trkpt>
tracks.clear();
trackNode = root.firstChildElement("trk");
for (; !trackNode.isNull(); trackNode = trackNode.nextSiblingElement("trk")) {
t.name = trackNode.firstChildElement("name").text();
segmentNode = trackNode.firstChildElement("trkseg");
for (; !segmentNode.isNull(); segmentNode = segmentNode.nextSiblingElement("trkseg")) {
t.points.clear();
pointNode = segmentNode.firstChildElement("trkpt");
for (; !pointNode.isNull(); pointNode = pointNode.nextSiblingElement("trkpt")) {
t.points.append(TrackPoint(
pointNode.attribute("lat").toFloat(),
pointNode.attribute("lon").toFloat(),
QDateTime::fromString(pointNode.firstChildElement("time").text(), Qt::ISODate)
));
}
qSort(t.points);
if (t.points.length()) {
tracks.append(t);
}
}
}
}
bool
GpxHandler::parse(QFile *gpxFile)
{
QXmlStreamReader xml(gpxFile);
while (!xml.atEnd() && !xml.hasError()) {
xml.readNext();
QString name = xml.name().toString();
if (xml.isStartElement()) {
if (name == "trk") {
if (_depth != 0) {
qWarning() << "Nested tracks at line " << xml.lineNumber()
<< " column " << xml.columnNumber();;
}
_currentTrack = new Track();
_currentTrack->sport = "Unknown (gpx)";
_tracks->append(_currentTrack);
} else if (name == "name" && _currentTrack) {
_currentTrack->name = xml.readElementText();
} else if (name == "trkseg") {
_depth += 1;
} else if (name == "trkpt") {
_depth += 1;
_foundLat = false;
_foundLon = false;
_inPoint = true;
bool ok = false;
QXmlStreamAttributes attrs = xml.attributes();
if (attrs.hasAttribute("lat")) {
QString lat = attrs.value("lat").toString();
_currentPoint.pos.y = lat.toDouble(&ok);
if (ok) {
_foundLat = true;
} else {
qWarning() << "Invalid trk element latitude: '"
<< lat << "' at line " << xml.lineNumber() << " column "
<< xml.columnNumber();
}
} else {
qWarning() << "trk element at line " << xml.lineNumber() <<
"column " << xml.columnNumber() <<
"was missing a latitude attribute";
}
if (attrs.hasAttribute("lon")) {
QString lon = attrs.value("lon").toString();
_currentPoint.pos.x = lon.toDouble(&ok);
if (ok) {
_foundLon = true;
} else {
qWarning() << "Invalid trk element longitude: '"
<< lon << "' at line " << xml.lineNumber() << " column "
<< xml.columnNumber();
}
} else {
qWarning() << "trk element at line " << xml.lineNumber() <<
"column " << xml.columnNumber() <<
"was missing a longitude attribute";
}
} else if (name == "time") {
if (_inPoint) {
QByteArray theTime = xml.readElementText().toLocal8Bit();
_currentPoint.time = Util::parseTime(theTime.constData());
_foundTime = true;
}
}
} else if (xml.isEndElement()) {
if (name == "trk") {
_depth -= 1;
_currentTrack = NULL;
} else if (name == "trkseg") {
_depth -= 1;
} else if (name == "trkpt") {
_depth -= 1;
if (_currentTrack && _inPoint && _foundLat && _foundLon
&& _foundTime)
{
/* push a copy of our point into the list */
_currentTrack->points.push_back(TrackPoint(_currentPoint));
}
_inPoint = false;
}
}
}
if (xml.hasError()) {
_error = QString("Got an error at line %0 column %1: '%2'").arg(
xml.lineNumber()).arg(xml.columnNumber())
.arg(xml.errorString());
qWarning() << _error;
return false;
}
return true;
}
