3 #include "ceres/ceres.h"
4 #include "gflags/gflags.h"
5 #include "glog/logging.h"
6 #include "LatLong-UTMconversion.h"
10 #define GPS_EXTRA_SIGMA_PER_SECOND 1.0
12 #define KD_LEAF_SIZE 5
14 using namespace ceres;
26 double utm_north, utm_east;
42 double len_sq, inv_len_sq;
44 vector<LineSegment> roads;
48 double a, b, d; // split plane
49 vector<int> roads_this_node;
50 BSPTreeNode *left, *right;
52 BSPTreeNode *root = NULL;
54 double signed_distance(double a, double b, double d, Coord c)
56 return a * c.x + b * c.y + d;
59 double split_cost(const vector<int>& remaining_roads, double a, double b, double d)
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) {
67 } else if (dist_from < 0.0 && dist_to < 0.0) {
74 return max(in_left, in_right) + 1.1 * in_both;
77 BSPTreeNode* make_bsp_tree(const vector<int>& remaining_roads)
79 BSPTreeNode *node = new BSPTreeNode;
80 if (remaining_roads.size() <= KD_LEAF_SIZE) {
82 node->left = node->right = NULL;
83 node->roads_this_node = remaining_roads;
87 node->is_leaf = false;
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]];
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;
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;
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;
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]);
127 node->roads_this_node.push_back(remaining_roads[i]);
131 if (left.empty() || right.empty()) {
132 node->is_leaf = true;
133 node->left = node->right = NULL;
134 node->roads_this_node = remaining_roads;
138 if (left.size() == 0) {
141 node->left = make_bsp_tree(left);
143 if (right.size() == 0) {
146 node->right = make_bsp_tree(right);
151 void intersect_line_line_x(double a1, double b1, double d1, double a2, double b2, double d2, double *min_x, double *max_x)
153 // find somewhere (x,y) where:
155 // A1x + B1y + D1 = 0 [I]
156 // A2x + B2y + D2 = 0 [II]
158 // [I] gives: y = -((A1/B1) x + D1/B1)
161 // A2x - B2 ((A1/B1) x + D1/B1) + D2 = 0
162 // A2x - A1 B2 / B1 x - D1 B2 / B1 + D2 = 0
163 // ((A2 B1 - A1 B2) / B1) x = D1 B2 / B1 - D2 [A2 B1 - A1 B2 = det]
164 // x = D1 B2 / det - D2 B1 / det = (D1 B2 - D2 B1) / det
165 double det = a2 * b1 - a1 * b2;
166 if (fabs(det) <= 1e-6) {
169 double intersect_x = (d1 * b2 - d2 * b1) / det;
171 if (-det * (*max_x - *min_x) < 0) {
172 *min_x = min(*min_x, intersect_x);
173 *max_x = min(*max_x, intersect_x);
175 *min_x = max(*min_x, intersect_x);
176 *max_x = max(*max_x, intersect_x);
180 void intersect_line_line_y(double a1, double b1, double d1, double a2, double b2, double d2, double *min_y, double *max_y)
182 // find somewhere (x,y) where:
184 // A1x + B1y + D1 = 0 [I]
185 // A2x + B2y + D2 = 0 [II]
187 // [I] gives: y = -((B1/A1) x + D1/A1)
190 // B2y - A2 ((B1/A1) x + D1/A1) + D2 = 0
191 // B2y - B1 A2 / A1 x - D1 A2 / A1 + D2 = 0
192 // ((A1 B2 - B1 A2) / A1) y = D1 A2 / A1 - D2 [A1 B2 - B1 A2 = det]
193 // y = D1 A2 / det - D2 A1 / det = (D1 B2 - D2 B1) / det
194 double det = a1 * b2 - a2 * b1;
195 if (fabs(det) <= 1e-6) {
198 double intersect_y = (d1 * a2 - d2 * a1) / det;
199 if (det * (*max_y - *min_y) < 0) {
200 *min_y = min(*min_y, intersect_y);
201 *max_y = min(*max_y, intersect_y);
203 *min_y = max(*min_y, intersect_y);
204 *max_y = max(*max_y, intersect_y);
208 void plot_line_based_on_x(FILE *fp, double a, double b, double d, double x1, double x2)
210 // see intersect_line_line()
211 double y1 = - (a * x1 + d) / b;
212 double y2 = - (a * x2 + d) / b;
213 fprintf(fp, "%f %f %f %f\n", x1, y1, x2, y2);
216 void plot_line_based_on_y(FILE *fp, double a, double b, double d, double y1, double y2)
218 // see intersect_line_line()
219 double x1 = - (b * y1 + d) / a;
220 double x2 = - (b * y2 + d) / a;
221 fprintf(fp, "%f %f %f %f\n", x1, y1, x2, y2);
224 void plot_line(double a, double b, double d)
226 if (fabs(a) > fabs(b)) {
227 double x1 = -2000.0, x2 = 2000.0;
228 if (b > 0) swap(x1, x2);
229 intersect_line_line_x(a, b, d, 0, 1.0, 2000, &x1, &x2);
230 intersect_line_line_x(a, b, d, 0, -1.0, 2000, &x1, &x2);
231 plot_line_based_on_x(stdout, a, b, d, x1, x2);
233 double y1 = -2000.0, y2 = 2000.0;
234 if (a < 0) swap(y1, y2);
235 intersect_line_line_y(a, b, d, 1.0, 0.0, 2000, &y1, &y2);
236 intersect_line_line_y(a, b, d, -1.0, 0.0, 2000, &y1, &y2);
237 plot_line_based_on_y(stdout, a, b, d, y1, y2);
241 void plot_bsp_tree(FILE *fp, BSPTreeNode *node, vector<pair<BSPTreeNode*, bool> > previous_visited)
243 double a = node->a, b = node->b, d = node->d;
245 double x1, x2, y1, y2;
251 if (fabs(a) > fabs(b)) {
252 x1 = -2000.0, x2 = 2000.0;
253 if (b > 0) swap(x1, x2);
254 intersect_line_line_x(a, b, d, 0, 1.0, 2000, &x1, &x2);
255 intersect_line_line_x(a, b, d, 0, -1.0, 2000, &x1, &x2);
257 // clip against all previous lines
258 for (unsigned i = 0; i < previous_visited.size(); ++i) {
259 BSPTreeNode* o = previous_visited[i].first;
260 if (previous_visited[i].second) {
261 intersect_line_line_x(a, b, d, o->a, o->b, o->d, &x1, &x2);
263 intersect_line_line_x(a, b, d, -o->a, -o->b, -o->d, &x1, &x2);
267 if (fabs(x1 - x2) > 1e-6) {
268 plot_line_based_on_x(fp, a, b, d, x1, x2);
271 // copy of the stuff above
272 y1 = -2000.0, y2 = 2000.0;
273 if (a < 0) swap(y1, y2);
274 intersect_line_line_y(a, b, d, 1.0, 0.0, 2000, &y1, &y2);
275 intersect_line_line_y(a, b, d, -1.0, 0.0, 2000, &y1, &y2);
277 // clip against all previous lines
278 for (unsigned i = 0; i < previous_visited.size(); ++i) {
279 BSPTreeNode* o = previous_visited[i].first;
280 if (previous_visited[i].second) {
281 intersect_line_line_y(a, b, d, o->a, o->b, o->d, &y1, &y2);
283 intersect_line_line_y(a, b, d, -o->a, -o->b, -o->d, &y1, &y2);
287 if (fabs(y1 - y2) > 1e-6) {
288 plot_line_based_on_y(fp, a, b, d, y1, y2);
292 previous_visited.push_back(make_pair(node, false));
293 if (node->left != NULL) {
294 plot_bsp_tree(fp, node->left, previous_visited);
296 if (node->right != NULL) {
297 previous_visited.back().second = true;
298 plot_bsp_tree(fp, node->right, previous_visited);
302 char* write_time(time_t t)
304 static char buf[256];
305 struct tm *foo = gmtime(&t);
306 strftime(buf, 256, "%Y-%m-%dT%H:%M:%SZ", foo);
310 class MinimizeAccelerationCostFunction {
312 MinimizeAccelerationCostFunction() {}
315 bool operator() (const T * const p0, const T * const p1, const T * const p2, T* e) const {
316 e[0] = p0[0] - T(2.0) * p1[0] + p2[0];
317 e[1] = p0[1] - T(2.0) * p1[1] + p2[1];
322 double sq_distance_point_point(const Coord& a, const Coord& b)
324 //return hypot(b.x - a.x, b.y - a.y);
325 return (b.x - a.x) * (b.x - a.x) + (b.y - a.y) * (b.y - a.y);
328 double sq_distance_point_line_segment(const Coord& c, LineSegment &ls)
330 if (ls.len_sq < 1e-6) {
331 return sq_distance_point_point(c, ls.from);
333 const double pv_x = c.x - ls.from.x;
334 const double pv_y = c.y - ls.from.y;
335 const double t = (pv_x * ls.wv_x + pv_y * ls.wv_y) * ls.inv_len_sq;
337 return sq_distance_point_point(c, ls.from);
340 return sq_distance_point_point(c, ls.to);
344 r.x = ls.from.x + t * ls.wv_x;
345 r.y = ls.from.y + t * ls.wv_y;
346 return sq_distance_point_point(c, r);
349 pair<int, double> find_closest_road_nonbsp(Coord c)
352 double sq_best_distance = HUGE_VAL;
353 for (unsigned i = 0; i < roads.size(); ++i) {
354 double sq_distance = sq_distance_point_line_segment(c, roads[i]);
355 if (sq_distance < sq_best_distance) {
357 sq_best_distance = sq_distance;
360 return make_pair(best_idx, std::sqrt(sq_best_distance));
364 BSPTreeJob(BSPTreeNode* node, BSPTreeNode* parent) : node(node), parent(parent) {}
365 BSPTreeJob(const BSPTreeJob& other) : node(other.node), parent(other.parent) {}
370 vector<BSPTreeJob> bsp_todo;
371 //vector<BSPTreeNode*> bsp_todo;
373 pair<int, double> find_closest_road(Coord c)
376 double sq_best_distance = HUGE_VAL;
379 bsp_todo.push_back(BSPTreeJob(root, NULL));
382 while (bsp_todo.size() > bsp_pos) {
383 BSPTreeJob job = bsp_todo[bsp_pos++];
384 BSPTreeNode* node = job.node;
386 // Re-verify versus sq_best_distance here, since it might have changed.
387 if (job.parent != NULL) {
388 double dist = signed_distance(job.parent->a, job.parent->b, job.parent->d, c);
389 if (dist*dist > sq_best_distance) {
390 // OK, later changes to best_distance has rendered this job moot.
396 // Check all nodes that are interior to this.
397 for (unsigned i = 0; i < node->roads_this_node.size(); ++i) {
398 int idx = node->roads_this_node[i];
399 double sq_distance = sq_distance_point_line_segment(c, roads[idx]);
400 if (sq_distance < sq_best_distance || (sq_distance == sq_best_distance && idx < best_idx)) {
402 sq_best_distance = sq_distance;
410 double dist = signed_distance(node->a, node->b, node->d, c);
411 if (dist * dist <= sq_best_distance) {
412 // gah! we have to put both in there.
413 // try at least the correct side first.
415 if (node->left != NULL) {
416 bsp_todo.push_back(BSPTreeJob(node->left, node));
423 if (node->right != NULL) {
424 bsp_todo.push_back(BSPTreeJob(node->right, node));
432 // woo, we can do with going on only one side
434 // go on to the right side
442 // go on to the left side
452 return make_pair(best_idx, std::sqrt(sq_best_distance));
455 class DistanceFromRoadCostFunction : public SizedCostFunction<1, 2, 2> {
457 virtual bool Evaluate(double const* const* p,
459 double** jacobians) const {
460 (void) jacobians; // Ignored; filled in by numeric differentiation.
470 pair<int, double> closest_road = find_closest_road(c);
472 // verify bsp behavior
473 pair<int, double> closest_road_nonbsp = find_closest_road_nonbsp(c);
474 if (closest_road.first != closest_road_nonbsp.first) {
475 printf("oops! for <%f,%f> bsp=(%d,%.10f) and nonbsp=(%d,%.10f)\n",
476 c.x, c.y, closest_road.first, closest_road.second,
477 closest_road_nonbsp.first, closest_road_nonbsp.second);
478 printf("bsp:\n%f %f\n%f %f\n",
479 roads[closest_road.first].from.x,
480 roads[closest_road.first].from.y,
481 roads[closest_road.first].to.x,
482 roads[closest_road.first].to.y);
483 printf("Nd:\n%f %f\n%f %f\n",
484 roads[closest_road_nonbsp.first].from.x,
485 roads[closest_road_nonbsp.first].from.y,
486 roads[closest_road_nonbsp.first].to.x,
487 roads[closest_road_nonbsp.first].to.y);
491 double prev_distance = std::sqrt(sq_distance_point_line_segment(prev_c, roads[closest_road.first]));
494 //e[0] = 2.0 - std::exp(-(best_distance * best_distance)) - std::exp(-(prev_distance * prev_distance));
495 //e[0] = 0.2 * (best_distance + 0.5 * prev_distance);
496 //e[0] = ceres::log(best_distance);
497 e[0] = closest_road.second + 0.5 * prev_distance;
502 class FootDiffCostFunction {
504 FootDiffCostFunction(double speed) : speed_(speed) {}
507 bool operator() (const T * const p0, const T * const p1, T* e) const {
508 T ex = p1[0] - p0[0];
509 T ey = p1[1] - p0[1];
510 T speed = 0.5 * ceres::sqrt(ex * ex + ey * ey);
511 e[0] = speed - speed_;
519 class GPSDiffCostFunction {
521 GPSDiffCostFunction(double x, double y) : x_(x), y_(y) {}
524 bool operator() (const T * const p, T* e) const {
525 T x_dist = p[0] - T(x_);
526 T y_dist = p[1] - T(y_);
527 T sq_dist = x_dist * x_dist + y_dist * y_dist;
528 e[0] = T(10.0) * (T(1.0) - ceres::exp(-sq_dist / T(GPS_SIGMA * GPS_SIGMA)));
529 //e[0] = sq_dist / GPS_SIGMA;
538 class MyIterationCallback : public IterationCallback {
540 MyIterationCallback(Coord *c, int num_points) : c_(c), num_points_(num_points) {}
542 virtual CallbackReturnType operator()(const IterationSummary& summary)
545 sprintf(buf, "temp%04d.plot", summary.iteration);
546 FILE *fp = fopen(buf, "w");
547 for (int i = 0; i < num_points_; ++i) {
548 fprintf(fp, "%f %f\n", c_[i].x, c_[i].y);
551 return SOLVER_CONTINUE;
560 int main(int argc, char** argv) {
561 google::ParseCommandLineFlags(&argc, &argv, true);
562 google::InitGoogleLogging(argv[0]);
564 int max_time = INT_MIN, min_time = INT_MAX;
566 vector<TCXPoint> tcx_points;
567 vector<FootPoint> foot_points;
569 FILE *fp = fopen(argv[1], "r");
577 if (fgets(buf, 256, fp) == NULL) {
580 if (strncmp(buf, "tcx ", 4) == 0) {
582 if (sscanf(buf, "tcx %d %lf %lf %lf %d", &p.time, &p.lat, &p.lon, &p.alt, &p.hr) != 5) {
583 fprintf(stderr, "parse error: %s\n", buf);
587 LLtoUTM(23, p.lat * rad2deg, p.lon * rad2deg, p.utm_north, p.utm_east, p.utm_zone);
589 tcx_points.push_back(p);
590 max_time = max(max_time, p.time);
591 min_time = min(min_time, p.time);
594 if (strncmp(buf, "footpod ", 8) == 0) {
596 if (sscanf(buf, "footpod %d %d %d %lf %lf", &p.time, &p.hr, &p.cadence, &p.speed, &p.distance) != 5) {
597 fprintf(stderr, "parse error: %s\n", buf);
600 foot_points.push_back(p);
601 max_time = max(max_time, p.time);
602 min_time = min(min_time, p.time);
608 // normalize the utm guys a bit
609 double utm_north_sum = 0.0, utm_east_sum = 0.0;
610 for (unsigned i = 0; i < tcx_points.size(); ++i) {
611 utm_north_sum += tcx_points[i].utm_north;
612 utm_east_sum += tcx_points[i].utm_east;
614 utm_north_sum /= tcx_points.size();
615 utm_east_sum /= tcx_points.size();
616 for (unsigned i = 0; i < tcx_points.size(); ++i) {
617 tcx_points[i].utm_north -= utm_north_sum;
618 tcx_points[i].utm_east -= utm_east_sum;
622 fp = fopen(argv[2], "r");
629 double lat1, lon1, lat2, lon2;
630 if (fscanf(fp, "%lf %lf %lf %lf\n", &lat1, &lon1, &lat2, &lon2) != 4) {
636 LLtoUTM(23, lat1, lon1, seg.from.y, seg.from.x, utm_zone);
637 LLtoUTM(23, lat2, lon2, seg.to.y, seg.to.x, utm_zone);
639 seg.from.y -= utm_north_sum;
640 seg.from.x -= utm_east_sum;
641 seg.to.y -= utm_north_sum;
642 seg.to.x -= utm_east_sum;
644 seg.len_sq = (seg.to.y - seg.from.y) * (seg.to.y - seg.from.y) + (seg.to.x - seg.from.x) * (seg.to.x - seg.from.x);
645 seg.inv_len_sq = 1.0 / seg.len_sq;
646 seg.wv_x = seg.to.x - seg.from.x;
647 seg.wv_y = seg.to.y - seg.from.y;
649 roads.push_back(seg);
653 vector<int> all_road_indexes;
654 for (unsigned i = 0; i < roads.size(); ++i) {
655 all_road_indexes.push_back(i);
657 root = make_bsp_tree(all_road_indexes);
659 fp = fopen("bsptree.plot", "w");
660 plot_bsp_tree(fp, root, vector<pair<BSPTreeNode*, bool> >());
663 fp = fopen("map.plot", "w");
664 for (unsigned i = 0; i < roads.size(); ++i) {
665 fprintf(fp, "%f %f %f %f\n", roads[i].from.x, roads[i].from.y, roads[i].to.x, roads[i].to.y);
669 int num_points = max_time - min_time + 1;
674 // Seed the initial path with some guesses.
675 for (int t = min_time; t <= max_time; ++t) {
676 // find the closest TCX point (yeah, yeah, linear search...)
678 int best_distance = INT_MAX;
679 for (unsigned i = 0; i < tcx_points.size(); ++i) {
680 int distance = abs(t - tcx_points[i].time);
681 if (distance < best_distance) {
683 best_distance = distance;
687 c[t - min_time].x = tcx_points[best_idx].utm_east + (rand() % 1000) / 1000.0;
688 c[t - min_time].y = tcx_points[best_idx].utm_north + (rand() % 1000) / 1000.0;
691 // Create regularization constraints.
692 for (int t = min_time + 1; t <= max_time - 1; ++t) {
693 problem.AddResidualBlock(
694 new AutoDiffCostFunction<MinimizeAccelerationCostFunction, 2, 2, 2, 2>(
695 new MinimizeAccelerationCostFunction),
697 reinterpret_cast<double *>(&c[(t - min_time) - 1]),
698 reinterpret_cast<double *>(&c[(t - min_time) + 0]),
699 reinterpret_cast<double *>(&c[(t - min_time) + 1]));
702 // Create close-to-road constraints.
703 for (int t = min_time + 1; t <= max_time; ++t) {
704 problem.AddResidualBlock(
705 new NumericDiffCostFunction<DistanceFromRoadCostFunction, FORWARD, 1, 2, 2>(
706 new DistanceFromRoadCostFunction,
707 ceres::TAKE_OWNERSHIP),
709 reinterpret_cast<double *>(&c[t - min_time]),
710 reinterpret_cast<double *>(&c[t - min_time - 1]));
713 // Now create one contraint per GPS point.
714 for (unsigned i = 0; i < tcx_points.size(); ++i) {
715 int t = tcx_points[i].time;
716 problem.AddResidualBlock(
717 new AutoDiffCostFunction<GPSDiffCostFunction, 1, 2>(
718 new GPSDiffCostFunction(tcx_points[i].utm_east, tcx_points[i].utm_north)),
720 reinterpret_cast<double *>(&c[t - min_time]));
723 // And per foot point.
724 for (unsigned i = 0; i < foot_points.size(); ++i) {
725 int t = foot_points[i].time;
726 if (t - 1 < min_time || t + 1 >= max_time) {
729 problem.AddResidualBlock(
730 new AutoDiffCostFunction<FootDiffCostFunction, 1, 2, 2>(
731 new FootDiffCostFunction(foot_points[i].speed)),
733 reinterpret_cast<double *>(&c[(t - min_time) - 1]),
734 reinterpret_cast<double *>(&c[(t - min_time) + 1]));
737 printf("pushed %d+%d points\n", int(tcx_points.size()), int(foot_points.size()));
738 printf("time from %d to %d\n", min_time, max_time);
741 Solver::Options options;
742 options.max_num_iterations = 1000000;
743 options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
744 //options.linear_solver_type = ceres::DENSE_QR;
745 options.minimizer_progress_to_stdout = true;
748 options.update_state_every_iteration = true;
749 options.callbacks.push_back(new MyIterationCallback(c, num_points));
751 Solver::Summary summary;
752 Solve(options, &problem, &summary);
753 // if (summary.termination_type == NUMERICAL_FAILURE) {
757 fp = fopen("out.plot", "w");
758 for (int t = min_time; t <= max_time; ++t) {
759 fprintf(fp, "%d %f %f\n", t, c[t - min_time].x, c[t - min_time].y);
763 fp = fopen("out.tcx", "w");
764 fprintf(fp, "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n");
765 fprintf(fp, "<TrainingCenterDatabase xmlns=\"http://www.garmin.com/xmlschemas/TrainingCenterDatabase/v2\" xmlns:xsi=\"http://www.w3.org/2001/XMLSchema-instance\" xsi:schemaLocation=\"http://www.garmin.com/xmlschemas/TrainingCenterDatabase/v2 http://www.garmin.com/xmlschemas/TrainingCenterDatabasev2.xsd\">\n");
766 fprintf(fp, " <Activities>\n");
767 fprintf(fp, " <Activity Sport=\"Running\">\n");
768 fprintf(fp, " <Id>smoothed</Id>\n");
769 fprintf(fp, " <Lap StartTime=\"%s\">\n", write_time(min_time));
770 fprintf(fp, " <TotalTimeSeconds>%f</TotalTimeSeconds>\n", float(max_time - min_time + 1));
771 fprintf(fp, " <DistanceMeters>3720.0</DistanceMeters>\n"); // FIXME
772 fprintf(fp, " <Intensity>Active</Intensity>\n");
773 fprintf(fp, " <TriggerMethod>Manual</TriggerMethod>\n");
774 fprintf(fp, " <Track>\n");
776 for (int t = min_time; t <= max_time; ++t) {
778 double north = c[t - min_time].y + utm_north_sum;
779 double east = c[t - min_time].x + utm_east_sum;
783 tcx_points[0].utm_zone,
785 fprintf(fp, " <TrackPoint>\n");
786 fprintf(fp, " <Time>%s</Time>\n", write_time(t));
787 fprintf(fp, " <Position>\n");
788 fprintf(fp, " <LatitudeDegrees>%.10f</LatitudeDegrees>\n", lat);
789 fprintf(fp, " <LongitudeDegrees>%.10f</LongitudeDegrees>\n", lon);
790 fprintf(fp, " </Position>\n");
791 fprintf(fp, " </TrackPoint>\n");
793 fprintf(fp, " </Track>\n");
794 fprintf(fp, " </Lap>\n");
795 fprintf(fp, " <Notes>Created by auto-smoothing points</Notes>\n");
796 fprintf(fp, " </Activity>\n");
797 fprintf(fp, " </Activities>\n");
798 fprintf(fp, "</TrainingCenterDatabase>\n");
801 std::cout << summary.BriefReport() << "\n";