1 // -*- Mode: c++ -*-
2 // copyright (c) 2006 by Christos Dimitrakakis <dimitrak@idiap.ch>
3 /***************************************************************************
4 * *
5 * This program is free software; you can redistribute it and/or modify *
6 * it under the terms of the GNU General Public License as published by *
7 * the Free Software Foundation; either version 2 of the License, or *
8 * (at your option) any later version. *
9 * *
10 ***************************************************************************/
11 #include <cstdio>
12 #include <cstdlib>
13 #include <cmath>
14 #include <cassert>
15 #include <list>
16 #include <vector>
17 #include "Trajectory.h"
18 #include <time.h>
19
20
21
22 /// Return a point
GetPoint(Segment & s,float w)23 Point Trajectory::GetPoint (Segment& s, float w)
24 {
25 float v = 1.0f - w;
26 return Point (w*s.left.x + v*s.right.x,
27 w*s.left.y + v*s.right.y,
28 w*s.left.z + v*s.right.z);
29 }
30
31 #define EXP_COST
32 #undef DBG_OPTIMISE
33 /// Optimise a track trajectory
Optimise(SegmentList track,int max_iter,float alpha,const char * fname,bool reset)34 void Trajectory::Optimise(SegmentList track, int max_iter, float alpha, const char* fname, bool reset)
35 {
36 int N = track.size();
37 clock_t start_time = clock();
38 int min_iter = max_iter/2; // minimum number of iterations to do
39 float time_limit = 2.0f; // if more than min_iter have been done, exit when time elapsed is larger than the time limit
40 float beta = 0.75f; // amount to reduce alpha to when it seems to be too large
41 w.resize(N);
42 dw.resize(N);
43 dw2.resize(N);
44 indices.resize(N);
45 accel.resize(N);
46
47 // initialise vectors
48 int i;
49 for (i=0; i<N; ++i) {
50 if (reset) {w[i] = 0.5f;}
51 dw2[i] = 1.0f;
52 indices[i] = i;
53 }
54
55
56 // Shuffle thoroughly
57 #if 1
58 srand(12358);
59 for (i=0; i<N-1; ++i) {
60 int z = rand()%(N-i);
61 int tmp = indices[i];
62 indices[i] = indices[z+i];
63 indices[z+i] = tmp;
64 }
65 #endif
66
67 //float prevC = 0.0f;
68 float Z = 10.0f;
69 float lambda = 0.9f;
70 float delta_C = 0.0f;
71 float prev_dCdw2 = 0.0f;
72
73 for (int iter=0; iter<max_iter; iter++) {
74
75 float C = 0.0f;
76 float P = 0.0f;
77 float dCdw2 = 0.0f;
78 float EdCdw = 0.0f;
79
80 float direction = 0.0;
81 for (int j=0; j<N-1; ++j) {
82 int i = indices[j];//rand()%(N-3) + 3;
83 int i_p3 = i - 3;
84 if (i_p3 < 0) i_p3 +=N;
85 int i_p2 = i - 2;
86 if (i_p2 < 0) i_p2 +=N;
87 int i_p1 = i - 1;
88 if (i_p1 < 0) i_p1 +=N;
89 //int i_n3 = (i + 3)%N;
90 int i_n2 = (i + 2)%N;
91 int i_n1 = (i + 1)%N;
92 //Segment s_prv3 = track[i_p3];
93 //Segment s_prv2 = track[i_p2];
94 //Segment s_prv = track[i_p1];
95 Segment s_cur = track[i];
96 //Segment s_nxt = track[i_n1];
97 //Segment s_nxt2 = track[i_n2];
98 //Point prv3 = GetPoint(track[i_p3], w[i_p3]);
99 Point prv2 = GetPoint(track[i_p2], w[i_p2]);
100 Point prv = GetPoint(track[i_p1], w[i_p1]);
101 Point cur = GetPoint(track[i], w[i]);
102 Point nxt = GetPoint(track[i_n1], w[i_n1]);
103 Point nxt2 = GetPoint(track[i_n2], w[i_n2]);
104 //Point u_prv2 = prv2 - prv3;
105 Point u_prv = prv - prv2;
106 Point u_cur = cur - prv;
107 Point u_nxt = nxt - cur;
108 Point u_nxt2 = nxt2 - nxt;
109 u_prv.Normalise();
110 u_cur.Normalise();
111 u_nxt.Normalise();
112 u_nxt2.Normalise();
113 //float l_prv2 = (prv2 - prv3).Length();
114 float l_prv = (prv - prv2).Length();
115 float l_cur = (cur - prv).Length();
116 float l_nxt = (nxt - cur).Length();
117 #if 1
118 Point a_prv = (u_cur - u_prv)/l_prv;
119 Point a_cur = (u_nxt - u_cur)/l_cur;
120 Point a_nxt = (u_nxt2 - u_nxt)/l_nxt;
121 #else
122 Point a_prv = (u_prv - u_prv2)/l_prv2;
123 Point a_cur = (u_cur - u_prv)/l_prv;
124 Point a_nxt = (u_nxt - u_cur)/l_cur;
125 #endif
126
127 float current_cost = a_prv.Length()*a_prv.Length()
128 + a_cur.Length()*a_cur.Length()
129 + a_nxt.Length()*a_nxt.Length();
130
131 //accel[i] = +a_nxt.Length();
132 accel[i] = (a_prv.Length() + a_cur.Length() + a_nxt.Length())/3.0f;
133 C += current_cost;
134
135 float dCdw = 0.0;
136
137 if (1) {
138 // Done only for a_cur, ignoring other costs.
139 {
140 Point lr = s_cur.left - s_cur.right;
141 Point d = cur - prv;
142 float dnorm2 = d.x*d.x + d.y*d.y;
143 float dnorm = sqrt(dnorm2);
144 float dxdynorm = d.x * d.y / dnorm;
145 #ifdef EXP_COST
146 float tmp = exp(a_cur.x*a_cur.x + a_cur.y*a_cur.y);
147 dCdw += tmp * a_cur.x * lr.x * (dnorm + d.x/dnorm + dxdynorm);
148 dCdw += tmp *a_cur.y * lr.y * (dnorm + d.y/dnorm + dxdynorm);
149 #else
150 dCdw += a_cur.x * lr.x * (dnorm + d.x/dnorm + dxdynorm);
151 dCdw += a_cur.y * lr.y * (dnorm + d.y/dnorm + dxdynorm);
152 #endif
153 }
154 {
155 Point lr = s_cur.left - s_cur.right;
156 Point d = nxt - cur;
157 float dnorm2 = d.x*d.x + d.y*d.y;
158 float dnorm = sqrt(dnorm2);
159 float dxdynorm = d.x * d.y / dnorm;
160 #ifdef EXP_COST
161 float tmp = exp(a_cur.x*a_cur.x + a_cur.y*a_cur.y);
162 dCdw += tmp * a_cur.x * lr.x * (dnorm + d.x/dnorm + dxdynorm);
163 dCdw += tmp *a_cur.y * lr.y * (dnorm + d.y/dnorm + dxdynorm);
164 #else
165 dCdw += a_cur.x * lr.x * (dnorm + d.x/dnorm + dxdynorm);
166 dCdw += a_cur.y * lr.y * (dnorm + d.y/dnorm + dxdynorm);
167 #endif
168 }
169 }
170
171 if (1) {
172 {
173 Point lr = s_cur.left - s_cur.right;
174 Point d = nxt - cur;
175 float dnorm2 = d.x*d.x + d.y*d.y;
176 float dnorm = sqrt(dnorm2);
177 float dxdynorm = d.x * d.y / dnorm;
178 #ifdef EXP_COST
179 float tmp = exp(a_nxt.x*a_nxt.x + a_nxt.y*a_nxt.y);
180 dCdw -= tmp * a_nxt.x * lr.x * (dnorm + d.x/dnorm + dxdynorm);
181 dCdw -= tmp * a_nxt.y * lr.y * (dnorm + d.y/dnorm + dxdynorm);
182 #else
183 dCdw -= a_nxt.x * lr.x * (dnorm + d.x/dnorm + dxdynorm);
184 dCdw -= a_nxt.y * lr.y * (dnorm + d.y/dnorm + dxdynorm);
185 #endif
186 }
187 }
188
189 if (1) {
190 {
191 Point lr = s_cur.left - s_cur.right;
192 Point d = cur - prv;
193 float dnorm2 = d.x*d.x + d.y*d.y;
194 float dnorm = sqrt(dnorm2);
195 float dxdynorm = d.x * d.y / dnorm;
196 #ifdef EXP_COST
197 float tmp = exp(a_prv.x*a_prv.x + a_prv.y*a_prv.y);
198 dCdw -= tmp*a_prv.x * lr.x * (dnorm + d.x/dnorm + dxdynorm);
199 dCdw -= tmp*a_prv.y * lr.y * (dnorm + d.y/dnorm + dxdynorm);
200 #else
201 dCdw -= a_prv.x * lr.x * (dnorm + d.x/dnorm + dxdynorm);
202 dCdw -= a_prv.y * lr.y * (dnorm + d.y/dnorm + dxdynorm);
203 #endif
204 }
205 }
206 float K = 10.0;
207 float penalty = 0.0;//K*(0.5f - w[i])*(exp(fabs(0.5-w[i]))-1);
208 if (1) {
209 float b = 0.1f;
210 if (w[i] < b) {
211 penalty += K*(b - w[i]);
212 } else if (w[i] > 1.0 -b) {
213 penalty += K*((1.0-b) - w[i]);
214 }
215 }
216 P+= K*penalty*penalty;
217 dCdw += K*penalty;
218 dw2[i] = lambda*dw2[i] + (1.0-lambda)*dCdw*dCdw;
219 direction += dCdw * dw[i];
220 float delta = dCdw/(dw2[i] + 1.0);
221 dw[i] = delta;
222 w[i] += alpha * delta;
223
224 if (1) {
225 float b = 0.0;
226 if (w[i] < b) {
227 w[i] = b;
228 } else if (w[i] > 1.0 -b) {
229 w[i] = 1.0 - b;
230 }
231 }
232
233 dCdw2 += dCdw*dCdw;
234 EdCdw += delta/(float) N;
235 } // indices
236
237
238 if (direction<0) {
239 alpha *= beta;
240 #ifdef DBG_OPTIMISE
241 fprintf (stderr, "# Reducing alpha to %f\n", alpha);
242 #endif
243 }
244 Z = (dCdw2);
245 if (Z<0.01) {
246 Z = 0.01f;
247 }
248
249
250
251 bool early_exit = false;
252 delta_C = 0.9*delta_C + 0.1*fabs(EdCdw-prev_dCdw2);
253 prev_dCdw2 = EdCdw;
254
255 if (delta_C < 0.001f) {
256 early_exit = true;
257 }
258
259 if (iter%100==0) {
260 clock_t current_time = clock();
261 float elapsed_time = (float) (current_time-start_time) / (float) CLOCKS_PER_SEC;
262 if (elapsed_time > time_limit) {
263 early_exit = true;
264 }
265
266 #ifdef DBG_OPTIMISE
267 fprintf (stderr, "%d %f %f %f %f %f %f\n",
268 iter,
269 C / (float) N,
270 P / (float) N, dCdw2, EdCdw, delta_C, elapsed_time);
271 #endif
272 }
273
274 if (iter>min_iter && early_exit) {
275 #ifdef DBG_OPTIMISE
276 fprintf (stderr, "# Time to break\n");
277 fflush (stderr);
278 #endif
279 break;
280 }
281 //prevC = C;
282 }
283
284
285
286 }
287
288
289 #if 0
290 // example
291 int main (int argc, char** argv)
292 {
293 if (argc!=3) {
294 fprintf (stderr, "usage: optimise_road iterations learning_rate\n");
295 exit(-1);
296 }
297 int iter = atoi(argv[1]);
298 float alpha = atof(argv[2]);
299 TrackData track_data;
300 SegmentList track;
301
302 track_data.setStep(10.0f);
303
304 float width_l = 9.0f;
305 float width_r = 9.0f;
306 track_data.setWidth(19.0f);
307
308 track_data.AddStraight (track, 50.0, width_l, width_r); //1
309 track_data.AddCurve (track, 90.0, 100, width_l, width_r); //2
310
311 Optimise (track, iter, alpha);
312
313 return 0;
314 }
315 #endif
316