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