git.sesse.net Git - tcxmerge/blob - opt.cc

   1 #include <stdio.h>
   2 #include <vector>
   3 #include "ceres/ceres.h"
   4 #include "gflags/gflags.h"
   5 #include "glog/logging.h"
   6 #include "LatLong-UTMconversion.h"
   7 #include "constants.h"
   8
   9 #define GPS_SIGMA 20.0
  10 #define GPS_EXTRA_SIGMA_PER_SECOND 1.0
  11
  12 #define KD_LEAF_SIZE 5
  13
  14 using namespace ceres;
  15 using std::vector;
  16
  17 struct Coord {
  18         double x, y;
  19 };
  20 struct TCXPoint {
  21         int time;
  22         double lat, lon, alt;
  23         int hr;
  24
  25         // computed
  26         double utm_north, utm_east;
  27         char utm_zone[16];
  28 };
  29 struct FootPoint {
  30         int time;
  31         int hr;
  32         int cadence;
  33         double speed;
  34         double distance;
  35 };
  36
  37 struct LineSegment {
  38         Coord from, to;
  39
  40         // computed
  41         double wv_x, wv_y;
  42         double len_sq, inv_len_sq;
  43 };
  44 vector<LineSegment> roads;
  45
  46 struct BSPTreeNode {
  47         bool is_leaf;
  48         double a, b, d;  // split plane
  49         vector<int> roads_this_node;
  50         BSPTreeNode *left, *right;
  51 };
  52 BSPTreeNode *root = NULL;
  53
  54 double signed_distance(double a, double b, double d, Coord c)
  55 {
  56         return a * c.x + b * c.y + d;
  57 }
  58
  59 double split_cost(const vector<int>& remaining_roads, double a, double b, double d)
  60 {
  61         unsigned in_left = 0, in_right = 0, in_both = 0;
  62         for (int i = 0; i < remaining_roads.size(); ++i) {
  63                 double dist_from = signed_distance(a, b, d, roads[remaining_roads[i]].from);
  64                 double dist_to = signed_distance(a, b, d, roads[remaining_roads[i]].to);
  65                 if (dist_from > 0.0 && dist_to > 0.0) {
  66                         ++in_right;
  67                 } else if (dist_from < 0.0 && dist_to < 0.0) {
  68                         ++in_left;
  69                 } else {
  70                         ++in_both;
  71                 }
  72         }
  73
  74         return max(in_left, in_right) + 1.1 * in_both;
  75 }
  76
  77 BSPTreeNode* make_bsp_tree(const vector<int>& remaining_roads)
  78 {
  79         BSPTreeNode *node = new BSPTreeNode;
  80         if (remaining_roads.size() <= KD_LEAF_SIZE) {
  81                 node->is_leaf = true;
  82                 node->left = node->right = NULL;
  83                 node->roads_this_node = remaining_roads;
  84                 return node;
  85         }
  86
  87         node->is_leaf = false;
  88
  89         // try all possible split planes (TODO: +/- epsilon)
  90         double best_a = 0.0, best_b = 0.0, best_d = 0.0;
  91         double best_cost = HUGE_VAL;
  92         for (unsigned i = 0; i < remaining_roads.size(); ++i) {
  93                 const LineSegment& ls = roads[remaining_roads[i]];
  94
  95                 // find the normal
  96                 double a =   ls.to.y - ls.from.y;
  97                 double b = -(ls.to.x - ls.from.x);
  98                 double invlen = 1.0 / hypot(a, b);
  99                 a *= invlen, b *= invlen;
 100
 101                 double d = -(a * ls.from.x + b * ls.from.y);
 102                 double cost = split_cost(remaining_roads, a, b, d - 1e-6);
 103                 if (cost < best_cost) {
 104                         best_a = a, best_b = b, best_d = d - 1e-6;
 105                         best_cost = cost;
 106                 }
 107
 108                 cost = split_cost(remaining_roads, a, b, d + 1e+6);
 109                 if (cost < best_cost) {
 110                         best_a = a, best_b = b, best_d = d + 1e+6;
 111                         best_cost = cost;
 112                 }
 113         }
 114         node->a = best_a;
 115         node->b = best_b;
 116         node->d = best_d;
 117
 118         vector<int> left, right;
 119         for (unsigned i = 0; i < remaining_roads.size(); ++i) {
 120                 double dist_from = signed_distance(best_a, best_b, best_d, roads[remaining_roads[i]].from);
 121                 double dist_to = signed_distance(best_a, best_b, best_d, roads[remaining_roads[i]].to);
 122                 if (dist_from > 0.0 && dist_to > 0.0) {
 123                         right.push_back(remaining_roads[i]);
 124                 } else if (dist_from < 0.0 && dist_to < 0.0) {
 125                         left.push_back(remaining_roads[i]);
 126                 } else {
 127                         node->roads_this_node.push_back(remaining_roads[i]);
 128                 }
 129         }
 130
 131         if (left.empty() || right.empty()) {
 132                 node->is_leaf = true;
 133                 node->left = node->right = NULL;
 134                 node->roads_this_node = remaining_roads;
 135                 return node;
 136         }
 137
 138         if (left.size() == 0) {
 139                 node->left = NULL;
 140         } else {
 141                 node->left = make_bsp_tree(left);
 142         }
 143         if (right.size() == 0) {
 144                 node->right = NULL;
 145         } else {
 146                 node->right = make_bsp_tree(right);
 147         }
 148         return node;
 149 }
 150
 151 void intersect_line_line_x(double a1, double b1, double d1, double a2, double b2, double d2, double *min_x, double *max_x)
 152 {
 153         // find somewhere (x,y) where:
 154         //
 155         //   A1x + B1y + D1 = 0  [I]
 156         //   A2x + B2y + D2 = 0  [II]
 157         //
 158         // [I] gives: y = -((A1/B1) x + D1/B1)
 159         // Insert into [II]:
 160         //
 161         //   A2x - B2 ((A1/B1) x + D1/B1) + D2 = 0
 162         //   A2x - A1 B2 / B1 x - D1 B2 / B1 + D2 = 0
 163         //   ((A2 B1 - A1 B2) / B1) x = D1 B2 / B1 - D2   [A2 B1 - A1 B2 = det]
 164         //   x = D1 B2 / det - D2 B1 / det = (D1 B2 - D2 B1) / det
 165         double det = a2 * b1 - a1 * b2;
 166         if (fabs(det) <= 1e-6) {
 167                 return;
 168         }
 169         double intersect_x = (d1 * b2 - d2 * b1) / det;
 170
 171         if (-det * (*max_x - *min_x) < 0) {
 172                 *min_x = min(*min_x, intersect_x);
 173                 *max_x = min(*max_x, intersect_x);
 174         } else {
 175                 *min_x = max(*min_x, intersect_x);
 176                 *max_x = max(*max_x, intersect_x);
 177         }
 178 }
 179
 180 void intersect_line_line_y(double a1, double b1, double d1, double a2, double b2, double d2, double *min_y, double *max_y)
 181 {
 182         // find somewhere (x,y) where:
 183         //
 184         //   A1x + B1y + D1 = 0  [I]
 185         //   A2x + B2y + D2 = 0  [II]
 186         //
 187         // [I] gives: y = -((B1/A1) x + D1/A1)
 188         // Insert into [II]:
 189         //
 190         //   B2y - A2 ((B1/A1) x + D1/A1) + D2 = 0
 191         //   B2y - B1 A2 / A1 x - D1 A2 / A1 + D2 = 0
 192         //   ((A1 B2 - B1 A2) / A1) y = D1 A2 / A1 - D2   [A1 B2 - B1 A2 = det]
 193         //   y = D1 A2 / det - D2 A1 / det = (D1 B2 - D2 B1) / det
 194         double det = a1 * b2 - a2 * b1;
 195         if (fabs(det) <= 1e-6) {
 196                 return;
 197         }
 198         double intersect_y = (d1 * a2 - d2 * a1) / det;
 199         if (det * (*max_y - *min_y) < 0) {
 200                 *min_y = min(*min_y, intersect_y);
 201                 *max_y = min(*max_y, intersect_y);
 202         } else {
 203                 *min_y = max(*min_y, intersect_y);
 204                 *max_y = max(*max_y, intersect_y);
 205         }
 206 }
 207
 208 void plot_line_based_on_x(FILE *fp, double a, double b, double d, double x1, double x2)
 209 {
 210         // see intersect_line_line()
 211         double y1 = - (a * x1 + d) / b;
 212         double y2 = - (a * x2 + d) / b;
 213         fprintf(fp, "%f %f %f %f\n", x1, y1, x2, y2);
 214 }
 215
 216 void plot_line_based_on_y(FILE *fp, double a, double b, double d, double y1, double y2)
 217 {
 218         // see intersect_line_line()
 219         double x1 = - (b * y1 + d) / a;
 220         double x2 = - (b * y2 + d) / a;
 221         fprintf(fp, "%f %f %f %f\n", x1, y1, x2, y2);
 222 }
 223
 224 void plot_line(double a, double b, double d)
 225 {
 226         if (fabs(a) > fabs(b)) {
 227                 double x1 = -2000.0, x2 = 2000.0;
 228                 if (b > 0) swap(x1, x2);
 229                 intersect_line_line_x(a, b, d, 0,  1.0, 2000, &x1, &x2);
 230                 intersect_line_line_x(a, b, d, 0, -1.0, 2000, &x1, &x2);
 231                 plot_line_based_on_x(stdout, a, b, d, x1, x2);
 232         } else {
 233                 double y1 = -2000.0, y2 = 2000.0;
 234                 if (a < 0) swap(y1, y2);
 235                 intersect_line_line_y(a, b, d,  1.0, 0.0, 2000, &y1, &y2);
 236                 intersect_line_line_y(a, b, d, -1.0, 0.0, 2000, &y1, &y2);
 237                 plot_line_based_on_y(stdout, a, b, d, y1, y2);
 238         }
 239 }
 240
 241 void plot_bsp_tree(FILE *fp, BSPTreeNode *node, vector<pair<BSPTreeNode*, bool> > previous_visited)
 242 {
 243         double a = node->a, b = node->b, d = node->d;
 244
 245         double x1, x2, y1, y2;
 246
 247         if (node->is_leaf) {
 248                 return;
 249         }
 250
 251         if (fabs(a) > fabs(b)) {
 252                 x1 = -2000.0, x2 = 2000.0;
 253                 if (b > 0) swap(x1, x2);
 254                 intersect_line_line_x(a, b, d, 0,  1.0, 2000, &x1, &x2);
 255                 intersect_line_line_x(a, b, d, 0, -1.0, 2000, &x1, &x2);
 256
 257                 // clip against all previous lines
 258                 for (unsigned i = 0; i < previous_visited.size(); ++i) {
 259                         BSPTreeNode* o = previous_visited[i].first;
 260                         if (previous_visited[i].second) {
 261                                 intersect_line_line_x(a, b, d, o->a, o->b, o->d, &x1, &x2);
 262                         } else {
 263                                 intersect_line_line_x(a, b, d, -o->a, -o->b, -o->d, &x1, &x2);
 264                         }
 265                 }
 266
 267                 if (fabs(x1 - x2) > 1e-6) {
 268                         plot_line_based_on_x(fp, a, b, d, x1, x2);
 269                 }
 270         } else {
 271                 // copy of the stuff above
 272                 y1 = -2000.0, y2 = 2000.0;
 273                 if (a < 0) swap(y1, y2);
 274                 intersect_line_line_y(a, b, d,  1.0, 0.0, 2000, &y1, &y2);
 275                 intersect_line_line_y(a, b, d, -1.0, 0.0, 2000, &y1, &y2);
 276
 277                 // clip against all previous lines
 278                 for (unsigned i = 0; i < previous_visited.size(); ++i) {
 279                         BSPTreeNode* o = previous_visited[i].first;
 280                         if (previous_visited[i].second) {
 281                                 intersect_line_line_y(a, b, d, o->a, o->b, o->d, &y1, &y2);
 282                         } else {
 283                                 intersect_line_line_y(a, b, d, -o->a, -o->b, -o->d, &y1, &y2);
 284                         }
 285                 }
 286
 287                 if (fabs(y1 - y2) > 1e-6) {
 288                         plot_line_based_on_y(fp, a, b, d, y1, y2);
 289                 }
 290         }
 291
 292         previous_visited.push_back(make_pair(node, false));
 293         if (node->left != NULL) {
 294                 plot_bsp_tree(fp, node->left, previous_visited);
 295         }
 296         if (node->right != NULL) {
 297                 previous_visited.back().second = true;
 298                 plot_bsp_tree(fp, node->right, previous_visited);
 299         }
 300 }
 301
 302 char* write_time(time_t t)
 303 {
 304         static char buf[256];
 305         struct tm *foo = gmtime(&t);
 306         strftime(buf, 256, "%Y-%m-%dT%H:%M:%SZ", foo);
 307         return buf;
 308 }
 309
 310 class MinimizeAccelerationCostFunction {
 311  public:
 312         MinimizeAccelerationCostFunction() {}
 313
 314         template<class T>
 315         bool operator() (const T * const p0, const T * const p1, const T * const p2, T* e) const {
 316                 e[0] = p0[0] - T(2.0) * p1[0] + p2[0];
 317                 e[1] = p0[1] - T(2.0) * p1[1] + p2[1];
 318                 return true;
 319         }
 320 };
 321
 322 double sq_distance_point_point(const Coord& a, const Coord& b)
 323 {
 324         //return hypot(b.x - a.x, b.y - a.y);
 325         return (b.x - a.x) * (b.x - a.x) + (b.y - a.y) * (b.y - a.y);
 326 }
 327
 328 double sq_distance_point_line_segment(const Coord& c, LineSegment &ls)
 329 {
 330         if (ls.len_sq < 1e-6) {
 331                 return sq_distance_point_point(c, ls.from);
 332         }
 333         const double pv_x = c.x - ls.from.x;
 334         const double pv_y = c.y - ls.from.y;
 335         const double t = (pv_x * ls.wv_x + pv_y * ls.wv_y) * ls.inv_len_sq;
 336         if (t <= 0.0) {
 337                 return sq_distance_point_point(c, ls.from);
 338         }
 339         if (t >= 1.0) {
 340                 return sq_distance_point_point(c, ls.to);
 341         }
 342
 343         Coord r;
 344         r.x = ls.from.x + t * ls.wv_x;
 345         r.y = ls.from.y + t * ls.wv_y;
 346         return sq_distance_point_point(c, r);
 347 }
 348
 349 pair<int, double> find_closest_road_nonbsp(Coord c)
 350 {
 351         int best_idx = -1;
 352         double sq_best_distance = HUGE_VAL;
 353         for (unsigned i = 0; i < roads.size(); ++i) {
 354                 double sq_distance = sq_distance_point_line_segment(c, roads[i]);
 355                 if (sq_distance < sq_best_distance) {
 356                         best_idx = i;
 357                         sq_best_distance = sq_distance;
 358                 }
 359         }
 360         return make_pair(best_idx, std::sqrt(sq_best_distance));
 361 }
 362
 363 struct BSPTreeJob {
 364         BSPTreeJob(BSPTreeNode* node, BSPTreeNode* parent) : node(node), parent(parent) {}
 365         BSPTreeJob(const BSPTreeJob& other) : node(other.node), parent(other.parent) {}
 366
 367         BSPTreeNode* node;
 368         BSPTreeNode* parent;
 369 };
 370 vector<BSPTreeJob> bsp_todo;
 371 //vector<BSPTreeNode*> bsp_todo;
 372
 373 pair<int, double> find_closest_road(Coord c)
 374 {
 375         int best_idx = -1;
 376         double sq_best_distance = HUGE_VAL;
 377
 378         bsp_todo.clear();
 379         bsp_todo.push_back(BSPTreeJob(root, NULL));
 380         int bsp_pos = 0;
 381
 382         while (bsp_todo.size() > bsp_pos) {
 383                 BSPTreeJob job = bsp_todo[bsp_pos++];
 384                 BSPTreeNode* node = job.node;
 385
 386                 // Re-verify versus sq_best_distance here, since it might have changed.
 387                 if (job.parent != NULL) {
 388                         double dist = signed_distance(job.parent->a, job.parent->b, job.parent->d, c);
 389                         if (dist*dist > sq_best_distance) {
 390                                 // OK, later changes to best_distance has rendered this job moot.
 391                                 continue;
 392                         }
 393                 }
 394 shortcut:
 395
 396                 // Check all nodes that are interior to this.
 397                 for (unsigned i = 0; i < node->roads_this_node.size(); ++i) {
 398                         int idx = node->roads_this_node[i];
 399                         double sq_distance = sq_distance_point_line_segment(c, roads[idx]);
 400                         if (sq_distance < sq_best_distance || (sq_distance == sq_best_distance && idx < best_idx)) {
 401                                 best_idx = idx;
 402                                 sq_best_distance = sq_distance;
 403                         }
 404                 }
 405
 406                 if (node->is_leaf) {
 407                         continue;
 408                 }
 409
 410                 double dist = signed_distance(node->a, node->b, node->d, c);
 411                 if (dist * dist <= sq_best_distance) {
 412                         // gah! we have to put both in there.
 413                         // try at least the correct side first.
 414                         if (dist > 0.0) {
 415                                 if (node->left != NULL) {
 416                                         bsp_todo.push_back(BSPTreeJob(node->left, node));
 417                                 }
 418                                 node = node->right;
 419                                 if (node != NULL) {
 420                                         goto shortcut;
 421                                 }
 422                         } else {
 423                                 if (node->right != NULL) {
 424                                         bsp_todo.push_back(BSPTreeJob(node->right, node));
 425                                 }
 426                                 node = node->left;
 427                                 if (node != NULL) {
 428                                         goto shortcut;
 429                                 }
 430                         }
 431                 } else {
 432                         // woo, we can do with going on only one side
 433                         if (dist > 0.0) {
 434                                 // go on to the right side
 435                                 node = node->right;
 436                                 if (node != NULL) {
 437                                         goto shortcut;
 438                                 }
 439                                 continue;
 440                         }
 441                         if (dist < 0.0) {
 442                                 // go on to the left side
 443                                 node = node->left;
 444                                 if (node != NULL) {
 445                                         goto shortcut;
 446                                 }
 447                                 continue;
 448                         }
 449                 }
 450         }
 451
 452         return make_pair(best_idx, std::sqrt(sq_best_distance));
 453 }
 454
 455 class DistanceFromRoadCostFunction : public SizedCostFunction<1, 2, 2> {
 456  public:
 457          virtual bool Evaluate(double const* const* p,
 458                                double* e,
 459                                double** jacobians) const {
 460                 (void) jacobians;  // Ignored; filled in by numeric differentiation.
 461
 462                 Coord c;
 463                 c.x = p[0][0];
 464                 c.y = p[0][1];
 465
 466                 Coord prev_c;
 467                 prev_c.x = p[1][0];
 468                 prev_c.y = p[1][1];
 469
 470                 pair<int, double> closest_road = find_closest_road(c);
 471 #if 0
 472                 // verify bsp behavior
 473                 pair<int, double> closest_road_nonbsp = find_closest_road_nonbsp(c);
 474                 if (closest_road.first != closest_road_nonbsp.first) {
 475                         printf("oops! for <%f,%f> bsp=(%d,%.10f) and nonbsp=(%d,%.10f)\n",
 476                                 c.x, c.y, closest_road.first, closest_road.second,
 477                                 closest_road_nonbsp.first, closest_road_nonbsp.second);
 478                         printf("bsp:\n%f %f\n%f %f\n",
 479                                 roads[closest_road.first].from.x,
 480                                 roads[closest_road.first].from.y,
 481                                 roads[closest_road.first].to.x,
 482                                 roads[closest_road.first].to.y);
 483                         printf("Nd:\n%f %f\n%f %f\n",
 484                                 roads[closest_road_nonbsp.first].from.x,
 485                                 roads[closest_road_nonbsp.first].from.y,
 486                                 roads[closest_road_nonbsp.first].to.x,
 487                                 roads[closest_road_nonbsp.first].to.y);
 488                         exit(1);
 489                 }
 490 #endif
 491                 double prev_distance = std::sqrt(sq_distance_point_line_segment(prev_c, roads[closest_road.first]));
 492
 493                 // hack
 494                 //e[0] = 2.0 - std::exp(-(best_distance * best_distance)) - std::exp(-(prev_distance * prev_distance));
 495                 //e[0] = 0.2 * (best_distance + 0.5 * prev_distance);
 496                 //e[0] = ceres::log(best_distance);
 497                 e[0] = closest_road.second + 0.5 * prev_distance;
 498                 return true;
 499          }
 500 };
 501
 502 class FootDiffCostFunction {
 503  public:
 504         FootDiffCostFunction(double speed) : speed_(speed) {}
 505
 506         template<class T>
 507         bool operator() (const T * const p0, const T * const p1, T* e) const {
 508                 T ex = p1[0] - p0[0];
 509                 T ey = p1[1] - p0[1];
 510                 T speed = 0.5 * ceres::sqrt(ex * ex + ey * ey);
 511                 e[0] = speed - speed_;
 512                 return true;
 513         }
 514
 515  private:
 516         double speed_;
 517 };
 518
 519 class GPSDiffCostFunction {
 520  public:
 521         GPSDiffCostFunction(double x, double y) : x_(x), y_(y) {}
 522
 523         template<class T>
 524         bool operator() (const T * const p, T* e) const {
 525                 T x_dist = p[0] - T(x_);
 526                 T y_dist = p[1] - T(y_);
 527                 T sq_dist = x_dist * x_dist + y_dist * y_dist;
 528                 e[0] = T(10.0) * (T(1.0) - ceres::exp(-sq_dist / T(GPS_SIGMA * GPS_SIGMA)));
 529                 //e[0] = sq_dist / GPS_SIGMA;
 530                 // e[0] = sq_dist;
 531                 return true;
 532         }
 533
 534  private:
 535         double x_, y_;
 536 };
 537
 538 class MyIterationCallback : public IterationCallback {
 539  public:
 540         MyIterationCallback(Coord *c, int num_points) : c_(c), num_points_(num_points) {}
 541
 542         virtual CallbackReturnType operator()(const IterationSummary& summary)
 543         {
 544                 char buf[256];
 545                 sprintf(buf, "temp%04d.plot", summary.iteration);
 546                 FILE *fp = fopen(buf, "w");
 547                 for (int i = 0; i < num_points_; ++i) {
 548                         fprintf(fp, "%f %f\n", c_[i].x, c_[i].y);
 549                 }
 550                 fclose(fp);
 551                 return SOLVER_CONTINUE;
 552         }
 553
 554  private:
 555         Coord *c_;
 556         int num_points_;
 557 };
 558
 559
 560 int main(int argc, char** argv) {
 561         google::ParseCommandLineFlags(&argc, &argv, true);
 562         google::InitGoogleLogging(argv[0]);
 563
 564         int max_time = INT_MIN, min_time = INT_MAX;
 565
 566         vector<TCXPoint> tcx_points;
 567         vector<FootPoint> foot_points;
 568
 569         FILE *fp = fopen(argv[1], "r");
 570         if (fp == NULL) {
 571                 perror(argv[1]);
 572                 exit(1);
 573         }
 574
 575         while (!feof(fp)) {
 576                 char buf[256];
 577                 if (fgets(buf, 256, fp) == NULL) {
 578                         break;
 579                 }
 580                 if (strncmp(buf, "tcx ", 4) == 0) {
 581                         TCXPoint p;
 582                         if (sscanf(buf, "tcx %d %lf %lf %lf %d", &p.time, &p.lat, &p.lon, &p.alt, &p.hr) != 5) {
 583                                 fprintf(stderr, "parse error: %s\n", buf);
 584                                 exit(1);
 585                         }
 586
 587                         LLtoUTM(23, p.lat * rad2deg, p.lon * rad2deg, p.utm_north, p.utm_east, p.utm_zone);
 588
 589                         tcx_points.push_back(p);
 590                         max_time = max(max_time, p.time);
 591                         min_time = min(min_time, p.time);
 592                         continue;
 593                 }
 594                 if (strncmp(buf, "footpod ", 8) == 0) {
 595                         FootPoint p;
 596                         if (sscanf(buf, "footpod %d %d %d %lf %lf", &p.time, &p.hr, &p.cadence, &p.speed, &p.distance) != 5) {
 597                                 fprintf(stderr, "parse error: %s\n", buf);
 598                                 exit(1);
 599                         }
 600                         foot_points.push_back(p);
 601                         max_time = max(max_time, p.time);
 602                         min_time = min(min_time, p.time);
 603                         continue;
 604                 }
 605         }
 606         fclose(fp);
 607
 608         // normalize the utm guys a bit
 609         double utm_north_sum = 0.0, utm_east_sum = 0.0;
 610         for (unsigned i = 0; i < tcx_points.size(); ++i) {
 611                 utm_north_sum += tcx_points[i].utm_north;
 612                 utm_east_sum += tcx_points[i].utm_east;
 613         }
 614         utm_north_sum /= tcx_points.size();
 615         utm_east_sum /= tcx_points.size();
 616         for (unsigned i = 0; i < tcx_points.size(); ++i) {
 617                 tcx_points[i].utm_north -= utm_north_sum;
 618                 tcx_points[i].utm_east -= utm_east_sum;
 619         }
 620
 621         // Read the OSM file
 622         fp = fopen(argv[2], "r");
 623         if (fp == NULL) {
 624                 perror(argv[2]);
 625                 exit(1);
 626         }
 627
 628         while (!feof(fp)) {
 629                 double lat1, lon1, lat2, lon2;
 630                 if (fscanf(fp, "%lf %lf %lf %lf\n", &lat1, &lon1, &lat2, &lon2) != 4) {
 631                         break;
 632                 }
 633
 634                 char utm_zone[16];
 635                 LineSegment seg;
 636                 LLtoUTM(23, lat1, lon1, seg.from.y, seg.from.x, utm_zone);
 637                 LLtoUTM(23, lat2, lon2, seg.to.y, seg.to.x, utm_zone);
 638
 639                 seg.from.y -= utm_north_sum;
 640                 seg.from.x -= utm_east_sum;
 641                 seg.to.y -= utm_north_sum;
 642                 seg.to.x -= utm_east_sum;
 643
 644                 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);
 645                 seg.inv_len_sq = 1.0 / seg.len_sq;
 646                 seg.wv_x = seg.to.x - seg.from.x;
 647                 seg.wv_y = seg.to.y - seg.from.y;
 648
 649                 roads.push_back(seg);
 650         }
 651         fclose(fp);
 652
 653         vector<int> all_road_indexes;
 654         for (unsigned i = 0; i < roads.size(); ++i) {
 655                 all_road_indexes.push_back(i);
 656         }
 657         root = make_bsp_tree(all_road_indexes);
 658
 659         fp = fopen("bsptree.plot", "w");
 660         plot_bsp_tree(fp, root, vector<pair<BSPTreeNode*, bool> >());
 661         fclose(fp);
 662
 663         fp = fopen("map.plot", "w");
 664         for (unsigned i = 0; i < roads.size(); ++i) {
 665                 fprintf(fp, "%f %f %f %f\n", roads[i].from.x, roads[i].from.y, roads[i].to.x, roads[i].to.y);
 666         }
 667         fclose(fp);
 668
 669         int num_points = max_time - min_time + 1;
 670         Coord c[num_points];
 671
 672         Problem problem;
 673
 674         // Seed the initial path with some guesses.
 675         for (int t = min_time; t <= max_time; ++t) {
 676                 // find the closest TCX point (yeah, yeah, linear search...)
 677                 int best_idx = -1;
 678                 int best_distance = INT_MAX;
 679                 for (unsigned i = 0; i < tcx_points.size(); ++i) {
 680                         int distance = abs(t - tcx_points[i].time);
 681                         if (distance < best_distance) {
 682                                 best_idx = i;
 683                                 best_distance = distance;
 684                         }
 685                 }
 686
 687                 c[t - min_time].x = tcx_points[best_idx].utm_east + (rand() % 1000) / 1000.0;
 688                 c[t - min_time].y = tcx_points[best_idx].utm_north + (rand() % 1000) / 1000.0;
 689         }
 690
 691         // Create regularization constraints.
 692         for (int t = min_time + 1; t <= max_time - 1; ++t) {
 693                 problem.AddResidualBlock(
 694                     new AutoDiffCostFunction<MinimizeAccelerationCostFunction, 2, 2, 2, 2>(
 695                         new MinimizeAccelerationCostFunction),
 696                     NULL,
 697                     reinterpret_cast<double *>(&c[(t - min_time) - 1]),
 698                     reinterpret_cast<double *>(&c[(t - min_time) + 0]),
 699                     reinterpret_cast<double *>(&c[(t - min_time) + 1]));
 700         }
 701
 702         // Create close-to-road constraints.
 703         for (int t = min_time + 1; t <= max_time; ++t) {
 704                 problem.AddResidualBlock(
 705                     new NumericDiffCostFunction<DistanceFromRoadCostFunction, FORWARD, 1, 2, 2>(
 706                         new DistanceFromRoadCostFunction,
 707                         ceres::TAKE_OWNERSHIP),
 708                     NULL,
 709                     reinterpret_cast<double *>(&c[t - min_time]),
 710                     reinterpret_cast<double *>(&c[t - min_time - 1]));
 711         }
 712
 713         // Now create one contraint per GPS point.
 714         for (unsigned i = 0; i < tcx_points.size(); ++i) {
 715                 int t = tcx_points[i].time;
 716                 problem.AddResidualBlock(
 717                     new AutoDiffCostFunction<GPSDiffCostFunction, 1, 2>(
 718                         new GPSDiffCostFunction(tcx_points[i].utm_east, tcx_points[i].utm_north)),
 719                     NULL,
 720                     reinterpret_cast<double *>(&c[t - min_time]));
 721         }
 722
 723         // And per foot point.
 724         for (unsigned i = 0; i < foot_points.size(); ++i) {
 725                 int t = foot_points[i].time;
 726                 if (t - 1 < min_time || t + 1 >= max_time) {
 727                         continue;
 728                 }
 729                 problem.AddResidualBlock(
 730                     new AutoDiffCostFunction<FootDiffCostFunction, 1, 2, 2>(
 731                         new FootDiffCostFunction(foot_points[i].speed)),
 732                     NULL,
 733                     reinterpret_cast<double *>(&c[(t - min_time) - 1]),
 734                     reinterpret_cast<double *>(&c[(t - min_time) + 1]));
 735         }
 736
 737         printf("pushed %d+%d points\n", int(tcx_points.size()), int(foot_points.size()));
 738         printf("time from %d to %d\n", min_time, max_time);
 739
 740         // Run the solver!
 741         Solver::Options options;
 742         options.max_num_iterations = 1000000;
 743         options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
 744         //options.linear_solver_type = ceres::DENSE_QR;
 745         options.minimizer_progress_to_stdout = true;
 746
 747         // callback
 748         options.update_state_every_iteration = true;
 749         options.callbacks.push_back(new MyIterationCallback(c, num_points));
 750
 751         Solver::Summary summary;
 752         Solve(options, &problem, &summary);
 753         // if (summary.termination_type == NUMERICAL_FAILURE) {
 754         //      return 1;
 755         // }
 756
 757         fp = fopen("out.plot", "w");
 758         for (int t = min_time; t <= max_time; ++t) {
 759                 fprintf(fp, "%d %f %f\n", t, c[t - min_time].x, c[t - min_time].y);
 760         }
 761         fclose(fp);
 762
 763         fp = fopen("out.tcx", "w");
 764         fprintf(fp, "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n");
 765         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");
 766         fprintf(fp, "  <Activities>\n");
 767         fprintf(fp, "    <Activity Sport=\"Running\">\n");
 768         fprintf(fp, "      <Id>smoothed</Id>\n");
 769         fprintf(fp, "      <Lap StartTime=\"%s\">\n", write_time(min_time));
 770         fprintf(fp, "        <TotalTimeSeconds>%f</TotalTimeSeconds>\n", float(max_time - min_time + 1));
 771         fprintf(fp, "        <DistanceMeters>3720.0</DistanceMeters>\n");  // FIXME
 772         fprintf(fp, "        <Intensity>Active</Intensity>\n");
 773         fprintf(fp, "        <TriggerMethod>Manual</TriggerMethod>\n");
 774         fprintf(fp, "        <Track>\n");
 775
 776         for (int t = min_time; t <= max_time; ++t) {
 777                 double lat, lon;
 778                 double north = c[t - min_time].y + utm_north_sum;
 779                 double east = c[t - min_time].x + utm_east_sum;
 780                 UTMtoLL(23,
 781                         north,
 782                         east,
 783                         tcx_points[0].utm_zone,
 784                         lat, lon);
 785                 fprintf(fp, "          <TrackPoint>\n");
 786                 fprintf(fp, "            <Time>%s</Time>\n", write_time(t));
 787                 fprintf(fp, "            <Position>\n");
 788                 fprintf(fp, "              <LatitudeDegrees>%.10f</LatitudeDegrees>\n", lat);
 789                 fprintf(fp, "              <LongitudeDegrees>%.10f</LongitudeDegrees>\n", lon);
 790                 fprintf(fp, "            </Position>\n");
 791                 fprintf(fp, "          </TrackPoint>\n");
 792         }
 793         fprintf(fp, "        </Track>\n");
 794         fprintf(fp, "      </Lap>\n");
 795         fprintf(fp, "      <Notes>Created by auto-smoothing points</Notes>\n");
 796         fprintf(fp, "    </Activity>\n");
 797         fprintf(fp, "  </Activities>\n");
 798         fprintf(fp, "</TrainingCenterDatabase>\n");
 799         fclose(fp);
 800
 801         std::cout << summary.BriefReport() << "\n";
 802
 803         return 0;
 804 }