From: Steinar H. Gunderson Date: Sat, 1 Sep 2012 15:48:05 +0000 (+0200) Subject: Initial checkin. X-Git-Url: https://git.sesse.net/?p=tcxmerge;a=commitdiff_plain;h=4f2445c0fb6acc4a2c268e207d605251f1c802f1 Initial checkin. --- 4f2445c0fb6acc4a2c268e207d605251f1c802f1 diff --git a/LatLong-UTMconversion.cpp b/LatLong-UTMconversion.cpp new file mode 100644 index 0000000..235ce46 --- /dev/null +++ b/LatLong-UTMconversion.cpp @@ -0,0 +1,183 @@ +//LatLong- UTM conversion.cpp +//Lat Long - UTM, UTM - Lat Long conversions + +#include +#include +#include +#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 index 0000000..478d9bb --- /dev/null +++ b/LatLong-UTMconversion.h @@ -0,0 +1,38 @@ +//LatLong- UTM conversion..h +//definitions for lat/long to UTM and UTM to lat/lng conversions +#include + +#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 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 index 0000000..1f98682 --- /dev/null +++ b/constants.h @@ -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 index 0000000..104cfec --- /dev/null +++ b/opt.cc @@ -0,0 +1,683 @@ +#include +#include +#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 roads; + +struct KDTreeNode { + enum Axis { LEAF, X_AXIS, Y_AXIS } split_axis; + double split_pos; + vector 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& 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& 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 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::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 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 + 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 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 kd_todo; +//vector kd_todo; + +pair 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 closest_road = find_closest_road(c); +#if 0 + // verify kd behavior + pair 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 + 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 + 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 tcx_points; + vector 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 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( + new MinimizeAccelerationCostFunction), + NULL, + reinterpret_cast(&c[(t - min_time) - 1]), + reinterpret_cast(&c[(t - min_time) + 0]), + reinterpret_cast(&c[(t - min_time) + 1])); + } + + // Create close-to-road constraints. + for (int t = min_time + 1; t <= max_time; ++t) { + problem.AddResidualBlock( + new NumericDiffCostFunction( + new DistanceFromRoadCostFunction, + ceres::TAKE_OWNERSHIP), + NULL, + reinterpret_cast(&c[t - min_time]), + reinterpret_cast(&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( + new GPSDiffCostFunction(tcx_points[i].utm_east, tcx_points[i].utm_north)), + NULL, + reinterpret_cast(&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( + new FootDiffCostFunction(foot_points[i].speed)), + NULL, + reinterpret_cast(&c[(t - min_time) - 1]), + reinterpret_cast(&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, "\n"); + fprintf(fp, "\n"); + fprintf(fp, " \n"); + fprintf(fp, " \n"); + fprintf(fp, " smoothed\n"); + fprintf(fp, " \n", write_time(min_time)); + fprintf(fp, " %f\n", float(max_time - min_time + 1)); + fprintf(fp, " 3720.0\n"); // FIXME + fprintf(fp, " Active\n"); + fprintf(fp, " Manual\n"); + fprintf(fp, " \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, " \n"); + fprintf(fp, " \n", write_time(t)); + fprintf(fp, " \n"); + fprintf(fp, " %.10f\n", lat); + fprintf(fp, " %.10f\n", lon); + fprintf(fp, " \n"); + fprintf(fp, " \n"); + } + fprintf(fp, " \n"); + fprintf(fp, " \n"); + fprintf(fp, " Created by auto-smoothing points\n"); + fprintf(fp, " \n"); + fprintf(fp, " \n"); + fprintf(fp, "\n"); + fclose(fp); + + std::cout << summary.BriefReport() << "\n"; + + return 0; +}