104cfecb8080833f566fd7b8d1b07d402dc5973e
[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 KDTreeNode {
47         enum Axis { LEAF, X_AXIS, Y_AXIS } split_axis;
48         double split_pos;
49         vector<int> roads_this_node;
50         KDTreeNode *left, *right;
51 };
52 KDTreeNode *root = NULL;
53
54 double signed_distance(KDTreeNode::Axis split_axis, double split_pos, Coord c)
55 {
56         if (split_axis == KDTreeNode::X_AXIS) {
57                 return c.x - split_pos;
58         } else {
59                 return c.y - split_pos;
60         }
61 }
62
63 double split_cost(const vector<int>& remaining_roads, KDTreeNode::Axis split_axis, double split_pos)
64 {
65         unsigned in_left = 0, in_right = 0, in_both = 0;
66         for (int i = 0; i < remaining_roads.size(); ++i) {
67                 double dist_from = signed_distance(split_axis, split_pos, roads[remaining_roads[i]].from);
68                 double dist_to = signed_distance(split_axis, split_pos, roads[remaining_roads[i]].to);
69                 if (dist_from > 0.0 && dist_to > 0.0) {
70                         ++in_right;
71                 } else if (dist_from < 0.0 && dist_to < 0.0) {
72                         ++in_left;
73                 } else {
74                         ++in_both;
75                 }
76         }
77
78         return max(in_left, in_right) + 1.1 * in_both;
79 }
80
81 KDTreeNode* make_kd_tree(const vector<int>& remaining_roads, KDTreeNode::Axis split_axis)
82 {
83         KDTreeNode *node = new KDTreeNode;
84         if (remaining_roads.size() <= KD_LEAF_SIZE) {
85                 node->split_axis = KDTreeNode::LEAF;
86                 node->left = node->right = NULL;
87                 node->roads_this_node = remaining_roads;
88                 return node;
89         }       
90
91         node->split_axis = split_axis;
92
93         // collect all points
94         vector<double> all_points;
95         KDTreeNode::Axis next_axis;
96         if (split_axis == KDTreeNode::X_AXIS) {
97                 for (unsigned i = 0; i < remaining_roads.size(); ++i) {
98                         all_points.push_back(roads[remaining_roads[i]].from.x - 1e-6);
99                         all_points.push_back(roads[remaining_roads[i]].to.x - 1e-6);
100                         all_points.push_back(roads[remaining_roads[i]].from.x + 1e-6);
101                         all_points.push_back(roads[remaining_roads[i]].to.x + 1e-6);
102                 }
103                 next_axis = KDTreeNode::Y_AXIS;
104         } else {
105                 for (unsigned i = 0; i < remaining_roads.size(); ++i) {
106                         all_points.push_back(roads[remaining_roads[i]].from.y - 1e-6);
107                         all_points.push_back(roads[remaining_roads[i]].to.y - 1e-6);
108                         all_points.push_back(roads[remaining_roads[i]].from.y + 1e-6);
109                         all_points.push_back(roads[remaining_roads[i]].to.y + 1e-6);
110                 }
111                 next_axis = KDTreeNode::X_AXIS;
112         }
113         sort(all_points.begin(), all_points.end());
114
115         vector<double>::iterator new_end = unique(all_points.begin(), all_points.end());
116         all_points.resize(new_end - all_points.begin());
117         
118         // now try all split points
119         double best_pos = 0.0;
120         double best_cost = HUGE_VAL;
121
122         for (unsigned i = 0; i < all_points.size(); ++i) {
123                 double cost = split_cost(remaining_roads, split_axis, all_points[i]);
124                 if (cost < best_cost) {
125                         best_pos = all_points[i];
126                         best_cost = cost;
127                 }
128         }
129         node->split_pos = best_pos;
130
131         vector<int> left, right;
132         for (unsigned i = 0; i < remaining_roads.size(); ++i) {
133                 double dist_from = signed_distance(split_axis, best_pos, roads[remaining_roads[i]].from);
134                 double dist_to = signed_distance(split_axis, best_pos, roads[remaining_roads[i]].to);
135                 if (dist_from > 0.0 && dist_to > 0.0) {
136                         right.push_back(remaining_roads[i]);
137                 } else if (dist_from < 0.0 && dist_to < 0.0) {
138                         left.push_back(remaining_roads[i]);
139                 } else {
140                         node->roads_this_node.push_back(remaining_roads[i]);
141                 }
142         }
143
144         if (left.size() == 0) {
145                 node->left = NULL;
146         } else {
147                 node->left = make_kd_tree(left, next_axis);
148         }
149         if (right.size() == 0) {
150                 node->right = NULL;
151         } else {
152                 node->right = make_kd_tree(right, next_axis);
153         }
154         return node;
155 }
156
157 void plot_kd_tree(FILE *fp, KDTreeNode *node, double min_x, double min_y, double max_x, double max_y) {
158         if (node->split_axis == KDTreeNode::X_AXIS) {
159                 // vertical line
160                 fprintf(fp, "%f %f %f %f\n", node->split_pos, min_y, node->split_pos, max_y);
161                 if (node->left != NULL) {
162                         plot_kd_tree(fp, node->left, min_x, min_y, node->split_pos, max_y);
163                 }
164                 if (node->right != NULL) {
165                         plot_kd_tree(fp, node->right, node->split_pos, min_y, max_x, max_y);
166                 }
167         }
168         if (node->split_axis == KDTreeNode::Y_AXIS) {
169                 // horizontal line
170                 fprintf(fp, "%f %f %f %f\n", min_x, node->split_pos, max_x, node->split_pos);
171                 if (node->left != NULL) {
172                         plot_kd_tree(fp, node->left, min_x, min_y, max_x, node->split_pos);
173                 }
174                 if (node->right != NULL) {
175                         plot_kd_tree(fp, node->right, min_x, node->split_pos, max_x, max_y);
176                 }
177         }
178 }
179
180 char* write_time(time_t t)
181 {
182         static char buf[256];
183         struct tm *foo = gmtime(&t);
184         strftime(buf, 256, "%Y-%m-%dT%H:%M:%SZ", foo);
185         return buf;
186 }
187
188 class MinimizeAccelerationCostFunction {
189  public:
190         MinimizeAccelerationCostFunction() {}
191         
192         template<class T>
193         bool operator() (const T * const p0, const T * const p1, const T * const p2, T* e) const {
194                 e[0] = p0[0] - T(2.0) * p1[0] + p2[0];
195                 e[1] = p0[1] - T(2.0) * p1[1] + p2[1];
196                 return true;
197         }
198 };
199
200 double sq_distance_point_point(const Coord& a, const Coord& b)
201 {
202         //return hypot(b.x - a.x, b.y - a.y);
203         return (b.x - a.x) * (b.x - a.x) + (b.y - a.y) * (b.y - a.y);
204 }
205                         
206 double sq_distance_point_line_segment(const Coord& c, LineSegment &ls)
207 {
208         if (ls.len_sq < 1e-6) {
209                 return sq_distance_point_point(c, ls.from);
210         }
211         const double pv_x = c.x - ls.from.x;
212         const double pv_y = c.y - ls.from.y;
213         const double t = (pv_x * ls.wv_x + pv_y * ls.wv_y) * ls.inv_len_sq;
214         if (t <= 0.0) {
215                 return sq_distance_point_point(c, ls.from);
216         }
217         if (t >= 1.0) {
218                 return sq_distance_point_point(c, ls.to);
219         }
220
221         Coord r;
222         r.x = ls.from.x + t * ls.wv_x;
223         r.y = ls.from.y + t * ls.wv_y;
224         return sq_distance_point_point(c, r);
225 }
226
227 pair<int, double> find_closest_road_nonkd(Coord c)
228 {
229         int best_idx = -1;
230         double sq_best_distance = HUGE_VAL;
231         for (unsigned i = 0; i < roads.size(); ++i) {
232                 double sq_distance = sq_distance_point_line_segment(c, roads[i]);
233                 if (sq_distance < sq_best_distance) {
234                         best_idx = i;
235                         sq_best_distance = sq_distance;
236                 }
237         }
238         return make_pair(best_idx, std::sqrt(sq_best_distance));
239 }
240
241 struct KDTreeJob {
242         KDTreeJob(KDTreeNode* node, KDTreeNode* parent) : node(node), parent(parent) {}
243         KDTreeJob(const KDTreeJob& other) : node(other.node), parent(other.parent) {}
244
245         KDTreeNode* node;
246         KDTreeNode* parent;
247 };
248 vector<KDTreeJob> kd_todo;
249 //vector<KDTreeNode*> kd_todo;
250
251 pair<int, double> find_closest_road(Coord c)
252 {
253         int best_idx = -1;
254         double sq_best_distance = HUGE_VAL;
255
256         kd_todo.clear();
257         kd_todo.push_back(KDTreeJob(root, NULL));
258         int kd_pos = 0;
259
260         while (kd_todo.size() > kd_pos) {
261                 KDTreeJob job = kd_todo[kd_pos++];
262                 KDTreeNode* node = job.node;
263
264                 // Re-verify versus sq_best_distance here, since it might have changed.
265                 if (job.parent != NULL) {
266                         double dist = signed_distance(job.parent->split_axis, job.parent->split_pos, c);
267                         if (dist*dist > sq_best_distance) {
268                                 // OK, later changes to best_distance has rendered this job moot.
269                                 continue;
270                         }
271                 }
272 shortcut:
273
274                 // Check all nodes that are interior to this.
275                 for (unsigned i = 0; i < node->roads_this_node.size(); ++i) {
276                         int idx = node->roads_this_node[i];
277                         double sq_distance = sq_distance_point_line_segment(c, roads[idx]);
278                         if (sq_distance < sq_best_distance || (sq_distance == sq_best_distance && idx < best_idx)) {
279                                 best_idx = idx;
280                                 sq_best_distance = sq_distance;
281                         }
282                 }
283
284                 if (node->split_axis == KDTreeNode::LEAF) {
285                         continue;
286                 }
287
288                 double dist = signed_distance(node->split_axis, node->split_pos, c);
289                 if (dist * dist <= sq_best_distance) {
290                         // gah! we have to put both in there.
291                         // try at least the correct side first.
292                         if (dist > 0.0) {
293                                 if (node->left != NULL) {
294                                         kd_todo.push_back(KDTreeJob(node->left, node));
295                                 }
296                                 node = node->right;
297                                 if (node != NULL) {
298                                         goto shortcut;
299                                 }
300                         } else {
301                                 if (node->right != NULL) {
302                                         kd_todo.push_back(KDTreeJob(node->right, node));
303                                 }
304                                 node = node->left;
305                                 if (node != NULL) {
306                                         goto shortcut;
307                                 }
308                         }
309                 } else {
310                         // woo, we can do with going on only one side
311                         if (dist > 0.0) {
312                                 // go on to the right side
313                                 node = node->right;
314                                 if (node != NULL) {
315                                         goto shortcut;
316                                 }
317                                 continue;
318                         }
319                         if (dist < 0.0) {
320                                 // go on to the left side
321                                 node = node->left;
322                                 if (node != NULL) {
323                                         goto shortcut;
324                                 }
325                                 continue;
326                         }
327                 }
328         }
329
330         return make_pair(best_idx, std::sqrt(sq_best_distance));
331 }
332
333 class DistanceFromRoadCostFunction : public SizedCostFunction<1, 2, 2> {
334  public:
335          virtual bool Evaluate(double const* const* p,
336                                double* e,
337                                double** jacobians) const {
338                 (void) jacobians;  // Ignored; filled in by numeric differentiation.
339
340                 Coord c;
341                 c.x = p[0][0];
342                 c.y = p[0][1];
343
344                 Coord prev_c;
345                 prev_c.x = p[1][0];
346                 prev_c.y = p[1][1];
347
348                 pair<int, double> closest_road = find_closest_road(c);
349 #if 0
350                 // verify kd behavior
351                 pair<int, double> closest_road_nonkd = find_closest_road_nonkd(c);
352                 if (closest_road.first != closest_road_nonkd.first) {
353                         printf("oops! for <%f,%f> kd=(%d,%.10f) and nonkd=(%d,%.10f)\n",
354                                 c.x, c.y, closest_road.first, closest_road.second,
355                                 closest_road_nonkd.first, closest_road_nonkd.second);
356                         printf("kd:\n%f %f\n%f %f\n",
357                                 roads[closest_road.first].from.x,
358                                 roads[closest_road.first].from.y,
359                                 roads[closest_road.first].to.x,
360                                 roads[closest_road.first].to.y);
361                         printf("Nd:\n%f %f\n%f %f\n",
362                                 roads[closest_road_nonkd.first].from.x,
363                                 roads[closest_road_nonkd.first].from.y,
364                                 roads[closest_road_nonkd.first].to.x,
365                                 roads[closest_road_nonkd.first].to.y);
366                         exit(1);
367                 }
368 #endif
369                 double prev_distance = std::sqrt(sq_distance_point_line_segment(prev_c, roads[closest_road.first]));
370
371                 // hack
372                 //e[0] = 2.0 - std::exp(-(best_distance * best_distance)) - std::exp(-(prev_distance * prev_distance));
373                 //e[0] = 0.2 * (best_distance + 0.5 * prev_distance);
374                 //e[0] = ceres::log(best_distance);
375                 e[0] = closest_road.second + 0.5 * prev_distance;
376                 return true;
377          }
378 };
379
380 class FootDiffCostFunction {
381  public:
382         FootDiffCostFunction(double speed) : speed_(speed) {}
383         
384         template<class T>
385         bool operator() (const T * const p0, const T * const p1, T* e) const {
386                 T ex = p1[0] - p0[0];
387                 T ey = p1[1] - p0[1];
388                 T speed = 0.5 * ceres::sqrt(ex * ex + ey * ey);
389                 e[0] = speed - speed_;
390                 return true;
391         }
392
393  private:
394         double speed_;
395 };
396
397 class GPSDiffCostFunction {
398  public:
399         GPSDiffCostFunction(double x, double y) : x_(x), y_(y) {}
400         
401         template<class T>
402         bool operator() (const T * const p, T* e) const {
403                 T x_dist = p[0] - T(x_);
404                 T y_dist = p[1] - T(y_);
405                 T sq_dist = x_dist * x_dist + y_dist * y_dist;
406                 e[0] = T(10.0) * (T(1.0) - ceres::exp(-sq_dist / T(GPS_SIGMA * GPS_SIGMA)));
407                 //e[0] = sq_dist / GPS_SIGMA;
408                 // e[0] = sq_dist;
409                 return true;
410         }
411
412  private:
413         double x_, y_;
414 };
415
416 class MyIterationCallback : public IterationCallback {
417  public:
418         MyIterationCallback(Coord *c, int num_points) : c_(c), num_points_(num_points) {}
419
420         virtual CallbackReturnType operator()(const IterationSummary& summary)
421         {
422                 char buf[256];
423                 sprintf(buf, "temp%04d.plot", summary.iteration);
424                 FILE *fp = fopen(buf, "w");
425                 for (int i = 0; i < num_points_; ++i) {
426                         fprintf(fp, "%f %f\n", c_[i].x, c_[i].y);
427                 }
428                 fclose(fp);
429                 return SOLVER_CONTINUE;
430         }
431
432  private:
433         Coord *c_;
434         int num_points_;
435 };
436
437
438 int main(int argc, char** argv) {
439         google::ParseCommandLineFlags(&argc, &argv, true);
440         google::InitGoogleLogging(argv[0]);
441
442         int max_time = INT_MIN, min_time = INT_MAX;
443
444         vector<TCXPoint> tcx_points;
445         vector<FootPoint> foot_points;
446
447         FILE *fp = fopen(argv[1], "r");
448         if (fp == NULL) {
449                 perror(argv[1]);
450                 exit(1);
451         }
452
453         while (!feof(fp)) {
454                 char buf[256];
455                 if (fgets(buf, 256, fp) == NULL) {
456                         break;
457                 }
458                 if (strncmp(buf, "tcx ", 4) == 0) {
459                         TCXPoint p;
460                         if (sscanf(buf, "tcx %d %lf %lf %lf %d", &p.time, &p.lat, &p.lon, &p.alt, &p.hr) != 5) {
461                                 fprintf(stderr, "parse error: %s\n", buf);
462                                 exit(1);
463                         }
464
465                         LLtoUTM(23, p.lat * rad2deg, p.lon * rad2deg, p.utm_north, p.utm_east, p.utm_zone);
466
467                         tcx_points.push_back(p);
468                         max_time = max(max_time, p.time);
469                         min_time = min(min_time, p.time);
470                         continue;
471                 }
472                 if (strncmp(buf, "footpod ", 8) == 0) {
473                         FootPoint p;
474                         if (sscanf(buf, "footpod %d %d %d %lf %lf", &p.time, &p.hr, &p.cadence, &p.speed, &p.distance) != 5) {
475                                 fprintf(stderr, "parse error: %s\n", buf);
476                                 exit(1);
477                         }
478                         foot_points.push_back(p);
479                         max_time = max(max_time, p.time);
480                         min_time = min(min_time, p.time);
481                         continue;
482                 }
483         }
484         fclose(fp);
485
486         // normalize the utm guys a bit
487         double utm_north_sum = 0.0, utm_east_sum = 0.0;
488         for (unsigned i = 0; i < tcx_points.size(); ++i) {
489                 utm_north_sum += tcx_points[i].utm_north;
490                 utm_east_sum += tcx_points[i].utm_east;
491         }
492         utm_north_sum /= tcx_points.size();
493         utm_east_sum /= tcx_points.size();
494         for (unsigned i = 0; i < tcx_points.size(); ++i) {
495                 tcx_points[i].utm_north -= utm_north_sum;
496                 tcx_points[i].utm_east -= utm_east_sum;
497         }
498
499         // Read the OSM file
500         fp = fopen(argv[2], "r");
501         if (fp == NULL) {
502                 perror(argv[2]);
503                 exit(1);
504         }
505
506         while (!feof(fp)) {
507                 double lat1, lon1, lat2, lon2;
508                 if (fscanf(fp, "%lf %lf %lf %lf\n", &lat1, &lon1, &lat2, &lon2) != 4) {
509                         break;
510                 }
511
512                 char utm_zone[16];
513                 LineSegment seg;
514                 LLtoUTM(23, lat1, lon1, seg.from.y, seg.from.x, utm_zone);
515                 LLtoUTM(23, lat2, lon2, seg.to.y, seg.to.x, utm_zone);
516
517                 seg.from.y -= utm_north_sum;
518                 seg.from.x -= utm_east_sum;
519                 seg.to.y -= utm_north_sum;
520                 seg.to.x -= utm_east_sum;
521
522                 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);
523                 seg.inv_len_sq = 1.0 / seg.len_sq;
524                 seg.wv_x = seg.to.x - seg.from.x;
525                 seg.wv_y = seg.to.y - seg.from.y;
526
527                 roads.push_back(seg);
528         }
529         fclose(fp);
530
531         vector<int> all_road_indexes;
532         for (unsigned i = 0; i < roads.size(); ++i) {
533                 all_road_indexes.push_back(i);
534         }
535         root = make_kd_tree(all_road_indexes, KDTreeNode::X_AXIS);
536         //root = make_kd_tree(all_road_indexes, KDTreeNode::Y_AXIS);
537
538         fp = fopen("kdtree.plot", "w");
539         plot_kd_tree(fp, root, -700.0, -2100.0, 800.0, 1800.0);
540         fclose(fp);
541         
542         fp = fopen("map.plot", "w");
543         for (unsigned i = 0; i < roads.size(); ++i) {
544                 fprintf(fp, "%f %f %f %f\n", roads[i].from.x, roads[i].from.y, roads[i].to.x, roads[i].to.y);
545         }
546         fclose(fp);
547
548         int num_points = max_time - min_time + 1;
549         Coord c[num_points];
550
551         Problem problem;
552
553         // Seed the initial path with some guesses.
554         for (int t = min_time; t <= max_time; ++t) {
555                 // find the closest TCX point (yeah, yeah, linear search...)
556                 int best_idx = -1;
557                 int best_distance = INT_MAX;
558                 for (unsigned i = 0; i < tcx_points.size(); ++i) {
559                         int distance = abs(t - tcx_points[i].time);
560                         if (distance < best_distance) {
561                                 best_idx = i;
562                                 best_distance = distance;
563                         }
564                 }
565
566                 c[t - min_time].x = tcx_points[best_idx].utm_east + (rand() % 1000) / 1000.0;
567                 c[t - min_time].y = tcx_points[best_idx].utm_north + (rand() % 1000) / 1000.0;
568         }
569
570         // Create regularization constraints.
571         for (int t = min_time + 1; t <= max_time - 1; ++t) {
572                 problem.AddResidualBlock(
573                     new AutoDiffCostFunction<MinimizeAccelerationCostFunction, 2, 2, 2, 2>(
574                         new MinimizeAccelerationCostFunction),
575                     NULL,
576                     reinterpret_cast<double *>(&c[(t - min_time) - 1]),
577                     reinterpret_cast<double *>(&c[(t - min_time) + 0]),
578                     reinterpret_cast<double *>(&c[(t - min_time) + 1]));
579         }
580         
581         // Create close-to-road constraints.
582         for (int t = min_time + 1; t <= max_time; ++t) {
583                 problem.AddResidualBlock(
584                     new NumericDiffCostFunction<DistanceFromRoadCostFunction, FORWARD, 1, 2, 2>(
585                         new DistanceFromRoadCostFunction,
586                         ceres::TAKE_OWNERSHIP),
587                     NULL,
588                     reinterpret_cast<double *>(&c[t - min_time]),
589                     reinterpret_cast<double *>(&c[t - min_time - 1]));
590         }
591
592         // Now create one contraint per GPS point.
593         for (unsigned i = 0; i < tcx_points.size(); ++i) {
594                 int t = tcx_points[i].time;
595                 problem.AddResidualBlock(
596                     new AutoDiffCostFunction<GPSDiffCostFunction, 1, 2>(
597                         new GPSDiffCostFunction(tcx_points[i].utm_east, tcx_points[i].utm_north)),
598                     NULL,
599                     reinterpret_cast<double *>(&c[t - min_time]));
600         }
601
602         // And per foot point.
603         for (unsigned i = 0; i < foot_points.size(); ++i) {
604                 int t = foot_points[i].time;
605                 if (t - 1 < min_time || t + 1 >= max_time) {
606                         continue;
607                 }
608                 problem.AddResidualBlock(
609                     new AutoDiffCostFunction<FootDiffCostFunction, 1, 2, 2>(
610                         new FootDiffCostFunction(foot_points[i].speed)),
611                     NULL,
612                     reinterpret_cast<double *>(&c[(t - min_time) - 1]),
613                     reinterpret_cast<double *>(&c[(t - min_time) + 1]));
614         }
615
616         printf("pushed %d+%d points\n", int(tcx_points.size()), int(foot_points.size()));
617         printf("time from %d to %d\n", min_time, max_time);
618
619         // Run the solver!
620         Solver::Options options;
621         options.max_num_iterations = 1000000;
622         options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
623         //options.linear_solver_type = ceres::DENSE_QR;
624         options.minimizer_progress_to_stdout = true;
625
626         // callback
627         options.update_state_every_iteration = true;
628         options.callbacks.push_back(new MyIterationCallback(c, num_points));
629
630         Solver::Summary summary;
631         Solve(options, &problem, &summary);
632         // if (summary.termination_type == NUMERICAL_FAILURE) {
633         //      return 1;
634         // }
635
636         fp = fopen("out.plot", "w");
637         for (int t = min_time; t <= max_time; ++t) {
638                 fprintf(fp, "%d %f %f\n", t, c[t - min_time].x, c[t - min_time].y);
639         }
640         fclose(fp);
641
642         fp = fopen("out.tcx", "w");
643         fprintf(fp, "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n");
644         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");
645         fprintf(fp, "  <Activities>\n");
646         fprintf(fp, "    <Activity Sport=\"Running\">\n");
647         fprintf(fp, "      <Id>smoothed</Id>\n");
648         fprintf(fp, "      <Lap StartTime=\"%s\">\n", write_time(min_time));
649         fprintf(fp, "        <TotalTimeSeconds>%f</TotalTimeSeconds>\n", float(max_time - min_time + 1));
650         fprintf(fp, "        <DistanceMeters>3720.0</DistanceMeters>\n");  // FIXME
651         fprintf(fp, "        <Intensity>Active</Intensity>\n");
652         fprintf(fp, "        <TriggerMethod>Manual</TriggerMethod>\n");
653         fprintf(fp, "        <Track>\n");
654
655         for (int t = min_time; t <= max_time; ++t) {
656                 double lat, lon;
657                 double north = c[t - min_time].y + utm_north_sum;
658                 double east = c[t - min_time].x + utm_east_sum;
659                 UTMtoLL(23,
660                         north,
661                         east,
662                         tcx_points[0].utm_zone,
663                         lat, lon);
664                 fprintf(fp, "          <TrackPoint>\n");
665                 fprintf(fp, "            <Time>%s</Time>\n", write_time(t));
666                 fprintf(fp, "            <Position>\n");        
667                 fprintf(fp, "              <LatitudeDegrees>%.10f</LatitudeDegrees>\n", lat);
668                 fprintf(fp, "              <LongitudeDegrees>%.10f</LongitudeDegrees>\n", lon);
669                 fprintf(fp, "            </Position>\n");
670                 fprintf(fp, "          </TrackPoint>\n");
671         }
672         fprintf(fp, "        </Track>\n");
673         fprintf(fp, "      </Lap>\n");
674         fprintf(fp, "      <Notes>Created by auto-smoothing points</Notes>\n");
675         fprintf(fp, "    </Activity>\n");
676         fprintf(fp, "  </Activities>\n");
677         fprintf(fp, "</TrainingCenterDatabase>\n");
678         fclose(fp);
679
680         std::cout << summary.BriefReport() << "\n";
681         
682         return 0;
683 }