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 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 }