Initial checkin.
authorSteinar H. Gunderson <sgunderson@bigfoot.com>
Sat, 1 Sep 2012 15:48:05 +0000 (17:48 +0200)
committerSteinar H. Gunderson <sgunderson@bigfoot.com>
Sat, 1 Sep 2012 15:48:05 +0000 (17:48 +0200)
LatLong-UTMconversion.cpp [new file with mode: 0644]
LatLong-UTMconversion.h [new file with mode: 0644]
Makefile [new file with mode: 0644]
constants.h [new file with mode: 0644]
opt.cc [new file with mode: 0644]

diff --git a/LatLong-UTMconversion.cpp b/LatLong-UTMconversion.cpp
new file mode 100644 (file)
index 0000000..235ce46
--- /dev/null
@@ -0,0 +1,183 @@
+//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;
+
+}
diff --git a/LatLong-UTMconversion.h b/LatLong-UTMconversion.h
new file mode 100644 (file)
index 0000000..478d9bb
--- /dev/null
@@ -0,0 +1,38 @@
+//LatLong- UTM conversion..h
+//definitions for lat/long to UTM and UTM to lat/lng conversions
+#include <string.h>
+
+#ifndef LATLONGCONV
+#define LATLONGCONV
+
+void LLtoUTM(int ReferenceEllipsoid, const double Lat, const double Long, 
+                        double &UTMNorthing, double &UTMEasting, char* UTMZone);
+void UTMtoLL(int ReferenceEllipsoid, const double UTMNorthing, const double UTMEasting, const char* UTMZone,
+                         double& Lat,  double& Long );
+char UTMLetterDesignator(double Lat);
+void LLtoSwissGrid(const double Lat, const double Long, 
+                        double &SwissNorthing, double &SwissEasting);
+void SwissGridtoLL(const double SwissNorthing, const double SwissEasting, 
+                                       double& Lat, double& Long);
+
+
+class Ellipsoid
+{
+public:
+       Ellipsoid(){};
+       Ellipsoid(int Id, char* name, double radius, double ecc)
+       {
+               id = Id; ellipsoidName = name; 
+               EquatorialRadius = radius; eccentricitySquared = ecc;
+       }
+
+       int id;
+       char* ellipsoidName;
+       double EquatorialRadius; 
+       double eccentricitySquared;  
+
+};
+
+
+
+#endif
diff --git a/Makefile b/Makefile
new file mode 100644 (file)
index 0000000..04b5ed7
--- /dev/null
+++ b/Makefile
@@ -0,0 +1,9 @@
+CXX=g++
+CXXFLAGS=-O2 -ffast-math -g -Wno-write-strings $(shell pkg-config eigen3 --cflags)
+LDFLAGS=-lceres_shared
+
+OBJS=opt.o LatLong-UTMconversion.o
+
+opt: $(OBJS)
+       $(CXX) -o $@ $(OBJS) $(LDFLAGS)
+
diff --git a/constants.h b/constants.h
new file mode 100644 (file)
index 0000000..1f98682
--- /dev/null
@@ -0,0 +1,41 @@
+//constants.h
+
+#ifndef CONSTANTS_H
+#define CONSTANTS_H
+
+#include "LatLong-UTMconversion.h"
+
+const double PI = 3.14159265;
+const double FOURTHPI = PI / 4;
+const double deg2rad = PI / 180;
+const double rad2deg = 180.0 / PI;
+
+static Ellipsoid ellipsoid[] = 
+{//  id, Ellipsoid name, Equatorial Radius, square of eccentricity     
+       Ellipsoid( -1, "Placeholder", 0, 0),//placeholder only, To allow array indices to match id numbers
+       Ellipsoid( 1, "Airy", 6377563, 0.00667054),
+       Ellipsoid( 2, "Australian National", 6378160, 0.006694542),
+       Ellipsoid( 3, "Bessel 1841", 6377397, 0.006674372),
+       Ellipsoid( 4, "Bessel 1841 (Nambia) ", 6377484, 0.006674372),
+       Ellipsoid( 5, "Clarke 1866", 6378206, 0.006768658),
+       Ellipsoid( 6, "Clarke 1880", 6378249, 0.006803511),
+       Ellipsoid( 7, "Everest", 6377276, 0.006637847),
+       Ellipsoid( 8, "Fischer 1960 (Mercury) ", 6378166, 0.006693422),
+       Ellipsoid( 9, "Fischer 1968", 6378150, 0.006693422),
+       Ellipsoid( 10, "GRS 1967", 6378160, 0.006694605),
+       Ellipsoid( 11, "GRS 1980", 6378137, 0.00669438),
+       Ellipsoid( 12, "Helmert 1906", 6378200, 0.006693422),
+       Ellipsoid( 13, "Hough", 6378270, 0.00672267),
+       Ellipsoid( 14, "International", 6378388, 0.00672267),
+       Ellipsoid( 15, "Krassovsky", 6378245, 0.006693422),
+       Ellipsoid( 16, "Modified Airy", 6377340, 0.00667054),
+       Ellipsoid( 17, "Modified Everest", 6377304, 0.006637847),
+       Ellipsoid( 18, "Modified Fischer 1960", 6378155, 0.006693422),
+       Ellipsoid( 19, "South American 1969", 6378160, 0.006694542),
+       Ellipsoid( 20, "WGS 60", 6378165, 0.006693422),
+       Ellipsoid( 21, "WGS 66", 6378145, 0.006694542),
+       Ellipsoid( 22, "WGS-72", 6378135, 0.006694318),
+       Ellipsoid( 23, "WGS-84", 6378137, 0.00669438)
+};
+
+#endif
diff --git a/opt.cc b/opt.cc
new file mode 100644 (file)
index 0000000..104cfec
--- /dev/null
+++ b/opt.cc
@@ -0,0 +1,683 @@
+#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;
+}