--- /dev/null
+//LatLong- UTM conversion.cpp
+//Lat Long - UTM, UTM - Lat Long conversions
+
+#include <math.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include "constants.h"
+#include "LatLong-UTMconversion.h"
+
+
+/*Reference ellipsoids derived from Peter H. Dana's website-
+http://www.utexas.edu/depts/grg/gcraft/notes/datum/elist.html
+Department of Geography, University of Texas at Austin
+Internet: pdana@mail.utexas.edu
+3/22/95
+
+Source
+Defense Mapping Agency. 1987b. DMA Technical Report: Supplement to Department of Defense World Geodetic System
+1984 Technical Report. Part I and II. Washington, DC: Defense Mapping Agency
+*/
+
+
+
+void LLtoUTM(int ReferenceEllipsoid, const double Lat, const double Long,
+ double &UTMNorthing, double &UTMEasting, char* UTMZone)
+{
+//converts lat/long to UTM coords. Equations from USGS Bulletin 1532
+//East Longitudes are positive, West longitudes are negative.
+//North latitudes are positive, South latitudes are negative
+//Lat and Long are in decimal degrees
+ //Written by Chuck Gantz- chuck.gantz@globalstar.com
+
+ double a = ellipsoid[ReferenceEllipsoid].EquatorialRadius;
+ double eccSquared = ellipsoid[ReferenceEllipsoid].eccentricitySquared;
+ double k0 = 0.9996;
+
+ double LongOrigin;
+ double eccPrimeSquared;
+ double N, T, C, A, M;
+
+//Make sure the longitude is between -180.00 .. 179.9
+ double LongTemp = (Long+180)-int((Long+180)/360)*360-180; // -180.00 .. 179.9;
+
+ double LatRad = Lat*deg2rad;
+ double LongRad = LongTemp*deg2rad;
+ double LongOriginRad;
+ int ZoneNumber;
+
+ ZoneNumber = int((LongTemp + 180)/6) + 1;
+
+ if( Lat >= 56.0 && Lat < 64.0 && LongTemp >= 3.0 && LongTemp < 12.0 )
+ ZoneNumber = 32;
+
+ // Special zones for Svalbard
+ if( Lat >= 72.0 && Lat < 84.0 )
+ {
+ if( LongTemp >= 0.0 && LongTemp < 9.0 ) ZoneNumber = 31;
+ else if( LongTemp >= 9.0 && LongTemp < 21.0 ) ZoneNumber = 33;
+ else if( LongTemp >= 21.0 && LongTemp < 33.0 ) ZoneNumber = 35;
+ else if( LongTemp >= 33.0 && LongTemp < 42.0 ) ZoneNumber = 37;
+ }
+ LongOrigin = (ZoneNumber - 1)*6 - 180 + 3; //+3 puts origin in middle of zone
+ LongOriginRad = LongOrigin * deg2rad;
+
+ //compute the UTM Zone from the latitude and longitude
+ sprintf(UTMZone, "%d%c", ZoneNumber, UTMLetterDesignator(Lat));
+
+ eccPrimeSquared = (eccSquared)/(1-eccSquared);
+
+ N = a/sqrt(1-eccSquared*sin(LatRad)*sin(LatRad));
+ T = tan(LatRad)*tan(LatRad);
+ C = eccPrimeSquared*cos(LatRad)*cos(LatRad);
+ A = cos(LatRad)*(LongRad-LongOriginRad);
+
+ M = a*((1 - eccSquared/4 - 3*eccSquared*eccSquared/64 - 5*eccSquared*eccSquared*eccSquared/256)*LatRad
+ - (3*eccSquared/8 + 3*eccSquared*eccSquared/32 + 45*eccSquared*eccSquared*eccSquared/1024)*sin(2*LatRad)
+ + (15*eccSquared*eccSquared/256 + 45*eccSquared*eccSquared*eccSquared/1024)*sin(4*LatRad)
+ - (35*eccSquared*eccSquared*eccSquared/3072)*sin(6*LatRad));
+
+ UTMEasting = (double)(k0*N*(A+(1-T+C)*A*A*A/6
+ + (5-18*T+T*T+72*C-58*eccPrimeSquared)*A*A*A*A*A/120)
+ + 500000.0);
+
+ UTMNorthing = (double)(k0*(M+N*tan(LatRad)*(A*A/2+(5-T+9*C+4*C*C)*A*A*A*A/24
+ + (61-58*T+T*T+600*C-330*eccPrimeSquared)*A*A*A*A*A*A/720)));
+ if(Lat < 0)
+ UTMNorthing += 10000000.0; //10000000 meter offset for southern hemisphere
+}
+
+char UTMLetterDesignator(double Lat)
+{
+//This routine determines the correct UTM letter designator for the given latitude
+//returns 'Z' if latitude is outside the UTM limits of 84N to 80S
+ //Written by Chuck Gantz- chuck.gantz@globalstar.com
+ char LetterDesignator;
+
+ if((84 >= Lat) && (Lat >= 72)) LetterDesignator = 'X';
+ else if((72 > Lat) && (Lat >= 64)) LetterDesignator = 'W';
+ else if((64 > Lat) && (Lat >= 56)) LetterDesignator = 'V';
+ else if((56 > Lat) && (Lat >= 48)) LetterDesignator = 'U';
+ else if((48 > Lat) && (Lat >= 40)) LetterDesignator = 'T';
+ else if((40 > Lat) && (Lat >= 32)) LetterDesignator = 'S';
+ else if((32 > Lat) && (Lat >= 24)) LetterDesignator = 'R';
+ else if((24 > Lat) && (Lat >= 16)) LetterDesignator = 'Q';
+ else if((16 > Lat) && (Lat >= 8)) LetterDesignator = 'P';
+ else if(( 8 > Lat) && (Lat >= 0)) LetterDesignator = 'N';
+ else if(( 0 > Lat) && (Lat >= -8)) LetterDesignator = 'M';
+ else if((-8> Lat) && (Lat >= -16)) LetterDesignator = 'L';
+ else if((-16 > Lat) && (Lat >= -24)) LetterDesignator = 'K';
+ else if((-24 > Lat) && (Lat >= -32)) LetterDesignator = 'J';
+ else if((-32 > Lat) && (Lat >= -40)) LetterDesignator = 'H';
+ else if((-40 > Lat) && (Lat >= -48)) LetterDesignator = 'G';
+ else if((-48 > Lat) && (Lat >= -56)) LetterDesignator = 'F';
+ else if((-56 > Lat) && (Lat >= -64)) LetterDesignator = 'E';
+ else if((-64 > Lat) && (Lat >= -72)) LetterDesignator = 'D';
+ else if((-72 > Lat) && (Lat >= -80)) LetterDesignator = 'C';
+ else LetterDesignator = 'Z'; //This is here as an error flag to show that the Latitude is outside the UTM limits
+
+ return LetterDesignator;
+}
+
+
+void UTMtoLL(int ReferenceEllipsoid, const double UTMNorthing, const double UTMEasting, const char* UTMZone,
+ double& Lat, double& Long )
+{
+//converts UTM coords to lat/long. Equations from USGS Bulletin 1532
+//East Longitudes are positive, West longitudes are negative.
+//North latitudes are positive, South latitudes are negative
+//Lat and Long are in decimal degrees.
+ //Written by Chuck Gantz- chuck.gantz@globalstar.com
+
+ double k0 = 0.9996;
+ double a = ellipsoid[ReferenceEllipsoid].EquatorialRadius;
+ double eccSquared = ellipsoid[ReferenceEllipsoid].eccentricitySquared;
+ double eccPrimeSquared;
+ double e1 = (1-sqrt(1-eccSquared))/(1+sqrt(1-eccSquared));
+ double N1, T1, C1, R1, D, M;
+ double LongOrigin;
+ double mu, phi1, phi1Rad;
+ double x, y;
+ int ZoneNumber;
+ char* ZoneLetter;
+ int NorthernHemisphere; //1 for northern hemispher, 0 for southern
+
+ x = UTMEasting - 500000.0; //remove 500,000 meter offset for longitude
+ y = UTMNorthing;
+
+ ZoneNumber = strtoul(UTMZone, &ZoneLetter, 10);
+ if((*ZoneLetter - 'N') >= 0)
+ NorthernHemisphere = 1;//point is in northern hemisphere
+ else
+ {
+ NorthernHemisphere = 0;//point is in southern hemisphere
+ y -= 10000000.0;//remove 10,000,000 meter offset used for southern hemisphere
+ }
+
+ LongOrigin = (ZoneNumber - 1)*6 - 180 + 3; //+3 puts origin in middle of zone
+
+ eccPrimeSquared = (eccSquared)/(1-eccSquared);
+
+ M = y / k0;
+ mu = M/(a*(1-eccSquared/4-3*eccSquared*eccSquared/64-5*eccSquared*eccSquared*eccSquared/256));
+
+ phi1Rad = mu + (3*e1/2-27*e1*e1*e1/32)*sin(2*mu)
+ + (21*e1*e1/16-55*e1*e1*e1*e1/32)*sin(4*mu)
+ +(151*e1*e1*e1/96)*sin(6*mu);
+ phi1 = phi1Rad*rad2deg;
+
+ N1 = a/sqrt(1-eccSquared*sin(phi1Rad)*sin(phi1Rad));
+ T1 = tan(phi1Rad)*tan(phi1Rad);
+ C1 = eccPrimeSquared*cos(phi1Rad)*cos(phi1Rad);
+ R1 = a*(1-eccSquared)/pow(1-eccSquared*sin(phi1Rad)*sin(phi1Rad), 1.5);
+ D = x/(N1*k0);
+
+ Lat = phi1Rad - (N1*tan(phi1Rad)/R1)*(D*D/2-(5+3*T1+10*C1-4*C1*C1-9*eccPrimeSquared)*D*D*D*D/24
+ +(61+90*T1+298*C1+45*T1*T1-252*eccPrimeSquared-3*C1*C1)*D*D*D*D*D*D/720);
+ Lat = Lat * rad2deg;
+
+ Long = (D-(1+2*T1+C1)*D*D*D/6+(5-2*C1+28*T1-3*C1*C1+8*eccPrimeSquared+24*T1*T1)
+ *D*D*D*D*D/120)/cos(phi1Rad);
+ Long = LongOrigin + Long * rad2deg;
+
+}
--- /dev/null
+#include <stdio.h>
+#include <vector>
+#include "ceres/ceres.h"
+#include "gflags/gflags.h"
+#include "glog/logging.h"
+#include "LatLong-UTMconversion.h"
+#include "constants.h"
+
+#define GPS_SIGMA 20.0
+#define GPS_EXTRA_SIGMA_PER_SECOND 1.0
+
+#define KD_LEAF_SIZE 5
+
+using namespace ceres;
+using std::vector;
+
+struct Coord {
+ double x, y;
+};
+struct TCXPoint {
+ int time;
+ double lat, lon, alt;
+ int hr;
+
+ // computed
+ double utm_north, utm_east;
+ char utm_zone[16];
+};
+struct FootPoint {
+ int time;
+ int hr;
+ int cadence;
+ double speed;
+ double distance;
+};
+
+struct LineSegment {
+ Coord from, to;
+
+ // computed
+ double wv_x, wv_y;
+ double len_sq, inv_len_sq;
+};
+vector<LineSegment> roads;
+
+struct KDTreeNode {
+ enum Axis { LEAF, X_AXIS, Y_AXIS } split_axis;
+ double split_pos;
+ vector<int> roads_this_node;
+ KDTreeNode *left, *right;
+};
+KDTreeNode *root = NULL;
+
+double signed_distance(KDTreeNode::Axis split_axis, double split_pos, Coord c)
+{
+ if (split_axis == KDTreeNode::X_AXIS) {
+ return c.x - split_pos;
+ } else {
+ return c.y - split_pos;
+ }
+}
+
+double split_cost(const vector<int>& remaining_roads, KDTreeNode::Axis split_axis, double split_pos)
+{
+ unsigned in_left = 0, in_right = 0, in_both = 0;
+ for (int i = 0; i < remaining_roads.size(); ++i) {
+ double dist_from = signed_distance(split_axis, split_pos, roads[remaining_roads[i]].from);
+ double dist_to = signed_distance(split_axis, split_pos, roads[remaining_roads[i]].to);
+ if (dist_from > 0.0 && dist_to > 0.0) {
+ ++in_right;
+ } else if (dist_from < 0.0 && dist_to < 0.0) {
+ ++in_left;
+ } else {
+ ++in_both;
+ }
+ }
+
+ return max(in_left, in_right) + 1.1 * in_both;
+}
+
+KDTreeNode* make_kd_tree(const vector<int>& remaining_roads, KDTreeNode::Axis split_axis)
+{
+ KDTreeNode *node = new KDTreeNode;
+ if (remaining_roads.size() <= KD_LEAF_SIZE) {
+ node->split_axis = KDTreeNode::LEAF;
+ node->left = node->right = NULL;
+ node->roads_this_node = remaining_roads;
+ return node;
+ }
+
+ node->split_axis = split_axis;
+
+ // collect all points
+ vector<double> all_points;
+ KDTreeNode::Axis next_axis;
+ if (split_axis == KDTreeNode::X_AXIS) {
+ for (unsigned i = 0; i < remaining_roads.size(); ++i) {
+ all_points.push_back(roads[remaining_roads[i]].from.x - 1e-6);
+ all_points.push_back(roads[remaining_roads[i]].to.x - 1e-6);
+ all_points.push_back(roads[remaining_roads[i]].from.x + 1e-6);
+ all_points.push_back(roads[remaining_roads[i]].to.x + 1e-6);
+ }
+ next_axis = KDTreeNode::Y_AXIS;
+ } else {
+ for (unsigned i = 0; i < remaining_roads.size(); ++i) {
+ all_points.push_back(roads[remaining_roads[i]].from.y - 1e-6);
+ all_points.push_back(roads[remaining_roads[i]].to.y - 1e-6);
+ all_points.push_back(roads[remaining_roads[i]].from.y + 1e-6);
+ all_points.push_back(roads[remaining_roads[i]].to.y + 1e-6);
+ }
+ next_axis = KDTreeNode::X_AXIS;
+ }
+ sort(all_points.begin(), all_points.end());
+
+ vector<double>::iterator new_end = unique(all_points.begin(), all_points.end());
+ all_points.resize(new_end - all_points.begin());
+
+ // now try all split points
+ double best_pos = 0.0;
+ double best_cost = HUGE_VAL;
+
+ for (unsigned i = 0; i < all_points.size(); ++i) {
+ double cost = split_cost(remaining_roads, split_axis, all_points[i]);
+ if (cost < best_cost) {
+ best_pos = all_points[i];
+ best_cost = cost;
+ }
+ }
+ node->split_pos = best_pos;
+
+ vector<int> left, right;
+ for (unsigned i = 0; i < remaining_roads.size(); ++i) {
+ double dist_from = signed_distance(split_axis, best_pos, roads[remaining_roads[i]].from);
+ double dist_to = signed_distance(split_axis, best_pos, roads[remaining_roads[i]].to);
+ if (dist_from > 0.0 && dist_to > 0.0) {
+ right.push_back(remaining_roads[i]);
+ } else if (dist_from < 0.0 && dist_to < 0.0) {
+ left.push_back(remaining_roads[i]);
+ } else {
+ node->roads_this_node.push_back(remaining_roads[i]);
+ }
+ }
+
+ if (left.size() == 0) {
+ node->left = NULL;
+ } else {
+ node->left = make_kd_tree(left, next_axis);
+ }
+ if (right.size() == 0) {
+ node->right = NULL;
+ } else {
+ node->right = make_kd_tree(right, next_axis);
+ }
+ return node;
+}
+
+void plot_kd_tree(FILE *fp, KDTreeNode *node, double min_x, double min_y, double max_x, double max_y) {
+ if (node->split_axis == KDTreeNode::X_AXIS) {
+ // vertical line
+ fprintf(fp, "%f %f %f %f\n", node->split_pos, min_y, node->split_pos, max_y);
+ if (node->left != NULL) {
+ plot_kd_tree(fp, node->left, min_x, min_y, node->split_pos, max_y);
+ }
+ if (node->right != NULL) {
+ plot_kd_tree(fp, node->right, node->split_pos, min_y, max_x, max_y);
+ }
+ }
+ if (node->split_axis == KDTreeNode::Y_AXIS) {
+ // horizontal line
+ fprintf(fp, "%f %f %f %f\n", min_x, node->split_pos, max_x, node->split_pos);
+ if (node->left != NULL) {
+ plot_kd_tree(fp, node->left, min_x, min_y, max_x, node->split_pos);
+ }
+ if (node->right != NULL) {
+ plot_kd_tree(fp, node->right, min_x, node->split_pos, max_x, max_y);
+ }
+ }
+}
+
+char* write_time(time_t t)
+{
+ static char buf[256];
+ struct tm *foo = gmtime(&t);
+ strftime(buf, 256, "%Y-%m-%dT%H:%M:%SZ", foo);
+ return buf;
+}
+
+class MinimizeAccelerationCostFunction {
+ public:
+ MinimizeAccelerationCostFunction() {}
+
+ template<class T>
+ bool operator() (const T * const p0, const T * const p1, const T * const p2, T* e) const {
+ e[0] = p0[0] - T(2.0) * p1[0] + p2[0];
+ e[1] = p0[1] - T(2.0) * p1[1] + p2[1];
+ return true;
+ }
+};
+
+double sq_distance_point_point(const Coord& a, const Coord& b)
+{
+ //return hypot(b.x - a.x, b.y - a.y);
+ return (b.x - a.x) * (b.x - a.x) + (b.y - a.y) * (b.y - a.y);
+}
+
+double sq_distance_point_line_segment(const Coord& c, LineSegment &ls)
+{
+ if (ls.len_sq < 1e-6) {
+ return sq_distance_point_point(c, ls.from);
+ }
+ const double pv_x = c.x - ls.from.x;
+ const double pv_y = c.y - ls.from.y;
+ const double t = (pv_x * ls.wv_x + pv_y * ls.wv_y) * ls.inv_len_sq;
+ if (t <= 0.0) {
+ return sq_distance_point_point(c, ls.from);
+ }
+ if (t >= 1.0) {
+ return sq_distance_point_point(c, ls.to);
+ }
+
+ Coord r;
+ r.x = ls.from.x + t * ls.wv_x;
+ r.y = ls.from.y + t * ls.wv_y;
+ return sq_distance_point_point(c, r);
+}
+
+pair<int, double> find_closest_road_nonkd(Coord c)
+{
+ int best_idx = -1;
+ double sq_best_distance = HUGE_VAL;
+ for (unsigned i = 0; i < roads.size(); ++i) {
+ double sq_distance = sq_distance_point_line_segment(c, roads[i]);
+ if (sq_distance < sq_best_distance) {
+ best_idx = i;
+ sq_best_distance = sq_distance;
+ }
+ }
+ return make_pair(best_idx, std::sqrt(sq_best_distance));
+}
+
+struct KDTreeJob {
+ KDTreeJob(KDTreeNode* node, KDTreeNode* parent) : node(node), parent(parent) {}
+ KDTreeJob(const KDTreeJob& other) : node(other.node), parent(other.parent) {}
+
+ KDTreeNode* node;
+ KDTreeNode* parent;
+};
+vector<KDTreeJob> kd_todo;
+//vector<KDTreeNode*> kd_todo;
+
+pair<int, double> find_closest_road(Coord c)
+{
+ int best_idx = -1;
+ double sq_best_distance = HUGE_VAL;
+
+ kd_todo.clear();
+ kd_todo.push_back(KDTreeJob(root, NULL));
+ int kd_pos = 0;
+
+ while (kd_todo.size() > kd_pos) {
+ KDTreeJob job = kd_todo[kd_pos++];
+ KDTreeNode* node = job.node;
+
+ // Re-verify versus sq_best_distance here, since it might have changed.
+ if (job.parent != NULL) {
+ double dist = signed_distance(job.parent->split_axis, job.parent->split_pos, c);
+ if (dist*dist > sq_best_distance) {
+ // OK, later changes to best_distance has rendered this job moot.
+ continue;
+ }
+ }
+shortcut:
+
+ // Check all nodes that are interior to this.
+ for (unsigned i = 0; i < node->roads_this_node.size(); ++i) {
+ int idx = node->roads_this_node[i];
+ double sq_distance = sq_distance_point_line_segment(c, roads[idx]);
+ if (sq_distance < sq_best_distance || (sq_distance == sq_best_distance && idx < best_idx)) {
+ best_idx = idx;
+ sq_best_distance = sq_distance;
+ }
+ }
+
+ if (node->split_axis == KDTreeNode::LEAF) {
+ continue;
+ }
+
+ double dist = signed_distance(node->split_axis, node->split_pos, c);
+ if (dist * dist <= sq_best_distance) {
+ // gah! we have to put both in there.
+ // try at least the correct side first.
+ if (dist > 0.0) {
+ if (node->left != NULL) {
+ kd_todo.push_back(KDTreeJob(node->left, node));
+ }
+ node = node->right;
+ if (node != NULL) {
+ goto shortcut;
+ }
+ } else {
+ if (node->right != NULL) {
+ kd_todo.push_back(KDTreeJob(node->right, node));
+ }
+ node = node->left;
+ if (node != NULL) {
+ goto shortcut;
+ }
+ }
+ } else {
+ // woo, we can do with going on only one side
+ if (dist > 0.0) {
+ // go on to the right side
+ node = node->right;
+ if (node != NULL) {
+ goto shortcut;
+ }
+ continue;
+ }
+ if (dist < 0.0) {
+ // go on to the left side
+ node = node->left;
+ if (node != NULL) {
+ goto shortcut;
+ }
+ continue;
+ }
+ }
+ }
+
+ return make_pair(best_idx, std::sqrt(sq_best_distance));
+}
+
+class DistanceFromRoadCostFunction : public SizedCostFunction<1, 2, 2> {
+ public:
+ virtual bool Evaluate(double const* const* p,
+ double* e,
+ double** jacobians) const {
+ (void) jacobians; // Ignored; filled in by numeric differentiation.
+
+ Coord c;
+ c.x = p[0][0];
+ c.y = p[0][1];
+
+ Coord prev_c;
+ prev_c.x = p[1][0];
+ prev_c.y = p[1][1];
+
+ pair<int, double> closest_road = find_closest_road(c);
+#if 0
+ // verify kd behavior
+ pair<int, double> closest_road_nonkd = find_closest_road_nonkd(c);
+ if (closest_road.first != closest_road_nonkd.first) {
+ printf("oops! for <%f,%f> kd=(%d,%.10f) and nonkd=(%d,%.10f)\n",
+ c.x, c.y, closest_road.first, closest_road.second,
+ closest_road_nonkd.first, closest_road_nonkd.second);
+ printf("kd:\n%f %f\n%f %f\n",
+ roads[closest_road.first].from.x,
+ roads[closest_road.first].from.y,
+ roads[closest_road.first].to.x,
+ roads[closest_road.first].to.y);
+ printf("Nd:\n%f %f\n%f %f\n",
+ roads[closest_road_nonkd.first].from.x,
+ roads[closest_road_nonkd.first].from.y,
+ roads[closest_road_nonkd.first].to.x,
+ roads[closest_road_nonkd.first].to.y);
+ exit(1);
+ }
+#endif
+ double prev_distance = std::sqrt(sq_distance_point_line_segment(prev_c, roads[closest_road.first]));
+
+ // hack
+ //e[0] = 2.0 - std::exp(-(best_distance * best_distance)) - std::exp(-(prev_distance * prev_distance));
+ //e[0] = 0.2 * (best_distance + 0.5 * prev_distance);
+ //e[0] = ceres::log(best_distance);
+ e[0] = closest_road.second + 0.5 * prev_distance;
+ return true;
+ }
+};
+
+class FootDiffCostFunction {
+ public:
+ FootDiffCostFunction(double speed) : speed_(speed) {}
+
+ template<class T>
+ bool operator() (const T * const p0, const T * const p1, T* e) const {
+ T ex = p1[0] - p0[0];
+ T ey = p1[1] - p0[1];
+ T speed = 0.5 * ceres::sqrt(ex * ex + ey * ey);
+ e[0] = speed - speed_;
+ return true;
+ }
+
+ private:
+ double speed_;
+};
+
+class GPSDiffCostFunction {
+ public:
+ GPSDiffCostFunction(double x, double y) : x_(x), y_(y) {}
+
+ template<class T>
+ bool operator() (const T * const p, T* e) const {
+ T x_dist = p[0] - T(x_);
+ T y_dist = p[1] - T(y_);
+ T sq_dist = x_dist * x_dist + y_dist * y_dist;
+ e[0] = T(10.0) * (T(1.0) - ceres::exp(-sq_dist / T(GPS_SIGMA * GPS_SIGMA)));
+ //e[0] = sq_dist / GPS_SIGMA;
+ // e[0] = sq_dist;
+ return true;
+ }
+
+ private:
+ double x_, y_;
+};
+
+class MyIterationCallback : public IterationCallback {
+ public:
+ MyIterationCallback(Coord *c, int num_points) : c_(c), num_points_(num_points) {}
+
+ virtual CallbackReturnType operator()(const IterationSummary& summary)
+ {
+ char buf[256];
+ sprintf(buf, "temp%04d.plot", summary.iteration);
+ FILE *fp = fopen(buf, "w");
+ for (int i = 0; i < num_points_; ++i) {
+ fprintf(fp, "%f %f\n", c_[i].x, c_[i].y);
+ }
+ fclose(fp);
+ return SOLVER_CONTINUE;
+ }
+
+ private:
+ Coord *c_;
+ int num_points_;
+};
+
+
+int main(int argc, char** argv) {
+ google::ParseCommandLineFlags(&argc, &argv, true);
+ google::InitGoogleLogging(argv[0]);
+
+ int max_time = INT_MIN, min_time = INT_MAX;
+
+ vector<TCXPoint> tcx_points;
+ vector<FootPoint> foot_points;
+
+ FILE *fp = fopen(argv[1], "r");
+ if (fp == NULL) {
+ perror(argv[1]);
+ exit(1);
+ }
+
+ while (!feof(fp)) {
+ char buf[256];
+ if (fgets(buf, 256, fp) == NULL) {
+ break;
+ }
+ if (strncmp(buf, "tcx ", 4) == 0) {
+ TCXPoint p;
+ if (sscanf(buf, "tcx %d %lf %lf %lf %d", &p.time, &p.lat, &p.lon, &p.alt, &p.hr) != 5) {
+ fprintf(stderr, "parse error: %s\n", buf);
+ exit(1);
+ }
+
+ LLtoUTM(23, p.lat * rad2deg, p.lon * rad2deg, p.utm_north, p.utm_east, p.utm_zone);
+
+ tcx_points.push_back(p);
+ max_time = max(max_time, p.time);
+ min_time = min(min_time, p.time);
+ continue;
+ }
+ if (strncmp(buf, "footpod ", 8) == 0) {
+ FootPoint p;
+ if (sscanf(buf, "footpod %d %d %d %lf %lf", &p.time, &p.hr, &p.cadence, &p.speed, &p.distance) != 5) {
+ fprintf(stderr, "parse error: %s\n", buf);
+ exit(1);
+ }
+ foot_points.push_back(p);
+ max_time = max(max_time, p.time);
+ min_time = min(min_time, p.time);
+ continue;
+ }
+ }
+ fclose(fp);
+
+ // normalize the utm guys a bit
+ double utm_north_sum = 0.0, utm_east_sum = 0.0;
+ for (unsigned i = 0; i < tcx_points.size(); ++i) {
+ utm_north_sum += tcx_points[i].utm_north;
+ utm_east_sum += tcx_points[i].utm_east;
+ }
+ utm_north_sum /= tcx_points.size();
+ utm_east_sum /= tcx_points.size();
+ for (unsigned i = 0; i < tcx_points.size(); ++i) {
+ tcx_points[i].utm_north -= utm_north_sum;
+ tcx_points[i].utm_east -= utm_east_sum;
+ }
+
+ // Read the OSM file
+ fp = fopen(argv[2], "r");
+ if (fp == NULL) {
+ perror(argv[2]);
+ exit(1);
+ }
+
+ while (!feof(fp)) {
+ double lat1, lon1, lat2, lon2;
+ if (fscanf(fp, "%lf %lf %lf %lf\n", &lat1, &lon1, &lat2, &lon2) != 4) {
+ break;
+ }
+
+ char utm_zone[16];
+ LineSegment seg;
+ LLtoUTM(23, lat1, lon1, seg.from.y, seg.from.x, utm_zone);
+ LLtoUTM(23, lat2, lon2, seg.to.y, seg.to.x, utm_zone);
+
+ seg.from.y -= utm_north_sum;
+ seg.from.x -= utm_east_sum;
+ seg.to.y -= utm_north_sum;
+ seg.to.x -= utm_east_sum;
+
+ seg.len_sq = (seg.to.y - seg.from.y) * (seg.to.y - seg.from.y) + (seg.to.x - seg.from.x) * (seg.to.x - seg.from.x);
+ seg.inv_len_sq = 1.0 / seg.len_sq;
+ seg.wv_x = seg.to.x - seg.from.x;
+ seg.wv_y = seg.to.y - seg.from.y;
+
+ roads.push_back(seg);
+ }
+ fclose(fp);
+
+ vector<int> all_road_indexes;
+ for (unsigned i = 0; i < roads.size(); ++i) {
+ all_road_indexes.push_back(i);
+ }
+ root = make_kd_tree(all_road_indexes, KDTreeNode::X_AXIS);
+ //root = make_kd_tree(all_road_indexes, KDTreeNode::Y_AXIS);
+
+ fp = fopen("kdtree.plot", "w");
+ plot_kd_tree(fp, root, -700.0, -2100.0, 800.0, 1800.0);
+ fclose(fp);
+
+ fp = fopen("map.plot", "w");
+ for (unsigned i = 0; i < roads.size(); ++i) {
+ fprintf(fp, "%f %f %f %f\n", roads[i].from.x, roads[i].from.y, roads[i].to.x, roads[i].to.y);
+ }
+ fclose(fp);
+
+ int num_points = max_time - min_time + 1;
+ Coord c[num_points];
+
+ Problem problem;
+
+ // Seed the initial path with some guesses.
+ for (int t = min_time; t <= max_time; ++t) {
+ // find the closest TCX point (yeah, yeah, linear search...)
+ int best_idx = -1;
+ int best_distance = INT_MAX;
+ for (unsigned i = 0; i < tcx_points.size(); ++i) {
+ int distance = abs(t - tcx_points[i].time);
+ if (distance < best_distance) {
+ best_idx = i;
+ best_distance = distance;
+ }
+ }
+
+ c[t - min_time].x = tcx_points[best_idx].utm_east + (rand() % 1000) / 1000.0;
+ c[t - min_time].y = tcx_points[best_idx].utm_north + (rand() % 1000) / 1000.0;
+ }
+
+ // Create regularization constraints.
+ for (int t = min_time + 1; t <= max_time - 1; ++t) {
+ problem.AddResidualBlock(
+ new AutoDiffCostFunction<MinimizeAccelerationCostFunction, 2, 2, 2, 2>(
+ new MinimizeAccelerationCostFunction),
+ NULL,
+ reinterpret_cast<double *>(&c[(t - min_time) - 1]),
+ reinterpret_cast<double *>(&c[(t - min_time) + 0]),
+ reinterpret_cast<double *>(&c[(t - min_time) + 1]));
+ }
+
+ // Create close-to-road constraints.
+ for (int t = min_time + 1; t <= max_time; ++t) {
+ problem.AddResidualBlock(
+ new NumericDiffCostFunction<DistanceFromRoadCostFunction, FORWARD, 1, 2, 2>(
+ new DistanceFromRoadCostFunction,
+ ceres::TAKE_OWNERSHIP),
+ NULL,
+ reinterpret_cast<double *>(&c[t - min_time]),
+ reinterpret_cast<double *>(&c[t - min_time - 1]));
+ }
+
+ // Now create one contraint per GPS point.
+ for (unsigned i = 0; i < tcx_points.size(); ++i) {
+ int t = tcx_points[i].time;
+ problem.AddResidualBlock(
+ new AutoDiffCostFunction<GPSDiffCostFunction, 1, 2>(
+ new GPSDiffCostFunction(tcx_points[i].utm_east, tcx_points[i].utm_north)),
+ NULL,
+ reinterpret_cast<double *>(&c[t - min_time]));
+ }
+
+ // And per foot point.
+ for (unsigned i = 0; i < foot_points.size(); ++i) {
+ int t = foot_points[i].time;
+ if (t - 1 < min_time || t + 1 >= max_time) {
+ continue;
+ }
+ problem.AddResidualBlock(
+ new AutoDiffCostFunction<FootDiffCostFunction, 1, 2, 2>(
+ new FootDiffCostFunction(foot_points[i].speed)),
+ NULL,
+ reinterpret_cast<double *>(&c[(t - min_time) - 1]),
+ reinterpret_cast<double *>(&c[(t - min_time) + 1]));
+ }
+
+ printf("pushed %d+%d points\n", int(tcx_points.size()), int(foot_points.size()));
+ printf("time from %d to %d\n", min_time, max_time);
+
+ // Run the solver!
+ Solver::Options options;
+ options.max_num_iterations = 1000000;
+ options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
+ //options.linear_solver_type = ceres::DENSE_QR;
+ options.minimizer_progress_to_stdout = true;
+
+ // callback
+ options.update_state_every_iteration = true;
+ options.callbacks.push_back(new MyIterationCallback(c, num_points));
+
+ Solver::Summary summary;
+ Solve(options, &problem, &summary);
+ // if (summary.termination_type == NUMERICAL_FAILURE) {
+ // return 1;
+ // }
+
+ fp = fopen("out.plot", "w");
+ for (int t = min_time; t <= max_time; ++t) {
+ fprintf(fp, "%d %f %f\n", t, c[t - min_time].x, c[t - min_time].y);
+ }
+ fclose(fp);
+
+ fp = fopen("out.tcx", "w");
+ fprintf(fp, "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n");
+ fprintf(fp, "<TrainingCenterDatabase xmlns=\"http://www.garmin.com/xmlschemas/TrainingCenterDatabase/v2\" xmlns:xsi=\"http://www.w3.org/2001/XMLSchema-instance\" xsi:schemaLocation=\"http://www.garmin.com/xmlschemas/TrainingCenterDatabase/v2 http://www.garmin.com/xmlschemas/TrainingCenterDatabasev2.xsd\">\n");
+ fprintf(fp, " <Activities>\n");
+ fprintf(fp, " <Activity Sport=\"Running\">\n");
+ fprintf(fp, " <Id>smoothed</Id>\n");
+ fprintf(fp, " <Lap StartTime=\"%s\">\n", write_time(min_time));
+ fprintf(fp, " <TotalTimeSeconds>%f</TotalTimeSeconds>\n", float(max_time - min_time + 1));
+ fprintf(fp, " <DistanceMeters>3720.0</DistanceMeters>\n"); // FIXME
+ fprintf(fp, " <Intensity>Active</Intensity>\n");
+ fprintf(fp, " <TriggerMethod>Manual</TriggerMethod>\n");
+ fprintf(fp, " <Track>\n");
+
+ for (int t = min_time; t <= max_time; ++t) {
+ double lat, lon;
+ double north = c[t - min_time].y + utm_north_sum;
+ double east = c[t - min_time].x + utm_east_sum;
+ UTMtoLL(23,
+ north,
+ east,
+ tcx_points[0].utm_zone,
+ lat, lon);
+ fprintf(fp, " <TrackPoint>\n");
+ fprintf(fp, " <Time>%s</Time>\n", write_time(t));
+ fprintf(fp, " <Position>\n");
+ fprintf(fp, " <LatitudeDegrees>%.10f</LatitudeDegrees>\n", lat);
+ fprintf(fp, " <LongitudeDegrees>%.10f</LongitudeDegrees>\n", lon);
+ fprintf(fp, " </Position>\n");
+ fprintf(fp, " </TrackPoint>\n");
+ }
+ fprintf(fp, " </Track>\n");
+ fprintf(fp, " </Lap>\n");
+ fprintf(fp, " <Notes>Created by auto-smoothing points</Notes>\n");
+ fprintf(fp, " </Activity>\n");
+ fprintf(fp, " </Activities>\n");
+ fprintf(fp, "</TrainingCenterDatabase>\n");
+ fclose(fp);
+
+ std::cout << summary.BriefReport() << "\n";
+
+ return 0;
+}
projects / tcxmerge / commitdiff