source: pacpussensors/trunk/Vislab/20150310_lib3dv-1.2.0/src/playback.cc

Last change on this file was 136, checked in by ldecherf, 8 years ago

Doc

File size: 23.9 KB
Line 
1/* 3dv-client/playback.cc
2 *
3 * Copyright (C) 2013 VisLab
4 *
5 * This file is part of lib3dv; you can redistribute it and/or modify
6 * it under the terms of the GNU Lesser General Public License as published by
7 * the Free Software Foundation; either version 3 of the License, or (at
8 * your option) any later version.
9 *
10 * This program is distributed in the hope that it will be useful, but
11 * WITHOUT ANY WARRANTY; without even the implied warranty of
12 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
13 * General Public License for more details.
14 *
15 * You should have received a copy of the GNU Lesser General Public License
16 * along with this program; if not, see <http://www.gnu.org/licenses/>.
17 */
18
19#include "playback.h"
20
21#ifdef _MSC_VER
22#include <conio.h>
23#else
24#include "3dv_conio.h"
25#endif
26
27
28uint8_t playback(std::vector<std::string> playback_value,uint8_t log_level)
29{
30 if(playback_value.size() > 0)
31 {
32 std::stringstream ss;
33 uint32_t framenumber;
34 uint32_t begin_framenumber;
35 uint32_t first=999999;
36 uint32_t last=0;
37 uint32_t myframe=0;
38 std::string path = playback_value[0]+"/";
39 boost::filesystem::path path_filename =path;
40
41
42 if(boost::filesystem::exists(path_filename)==1)
43 {
44 if(boost::filesystem::is_directory(path_filename))
45 {
46 typedef std::vector<boost::filesystem::path> vec;
47 vec v;
48 copy(boost::filesystem::directory_iterator(path_filename), boost::filesystem::directory_iterator(), back_inserter(v));
49 sort(v.begin(), v.end());
50 for(vec::const_iterator it (v.begin()); it != v.end(); ++it)
51 {
52 std::vector<std::string> tokens;
53 boost::filesystem::path linep=*it;
54 std::string line=linep.stem().string();
55 boost::split(tokens, line, boost::is_any_of("-"));
56 if(tokens[0]=="dsi")
57 {
58 std::string stringnumber=tokens[1];
59 uint32_t number=atoi(stringnumber.c_str());
60 if(number<first)
61 first=number;
62 if(number>last)
63 last=number;
64 }
65 }
66 }
67 else
68 {
69 std::cerr<<"[EE] 3dv-client:"<< path_filename <<" exists, but is neither a regular file nor a directory\n";
70 return lib3dv::error::NONE;
71 }
72
73 framenumber=first;
74 if(playback_value.size() > 1)
75 {
76 ss << playback_value[1];
77 ss >> begin_framenumber;
78 framenumber= begin_framenumber;
79 }
80
81 std::cout << "[II] 3dv-client: playback " << path <<" from frame " <<framenumber<<std::endl;
82
83 if(begin_framenumber>last)
84 begin_framenumber=last;
85 else if(begin_framenumber<first)
86 begin_framenumber=first;
87
88
89 boost::thread display_thread;
90 boost::shared_ptr<display> display;
91 device_params params;
92
93 std::string filenameini = path+"params.ini";
94 std::cout << "[II] 3dv-client: loading INI file: " << filenameini << std::endl;
95 std::ifstream file (filenameini.c_str(), std::ios::in);
96 std::string line;
97
98 if (file.is_open())
99 {
100 while( getline (file,line))
101 {
102 std::vector<std::string> tokens;
103 boost::split(tokens, line, boost::is_any_of("="));
104
105 if(tokens[0] == "calibration.u0")
106 params.intrinsics.m_u0 = boost::lexical_cast<float>(tokens[1].c_str());
107 else if(tokens[0] == "calibration.v0")
108 params.intrinsics.m_v0 = boost::lexical_cast<float>(tokens[1].c_str());
109 else if(tokens[0] == "calibration.ku")
110 params.intrinsics.m_ku = boost::lexical_cast<float>(tokens[1].c_str());
111 else if(tokens[0] == "calibration.kv")
112 params.intrinsics.m_kv = boost::lexical_cast<float>(tokens[1].c_str());
113 else if(tokens[0] == "calibration.x")
114 params.position.m_x = boost::lexical_cast<float>(tokens[1].c_str());
115 else if(tokens[0] == "calibration.y")
116 params.position.m_y = boost::lexical_cast<float>(tokens[1].c_str());
117 else if(tokens[0] == "calibration.z")
118 params.position.m_z = boost::lexical_cast<float>(tokens[1].c_str());
119 else if(tokens[0] == "calibration.yaw")
120 params.orientation.m_yaw = boost::lexical_cast<float>(tokens[1].c_str());
121 else if(tokens[0] == "calibration.pitch")
122 params.orientation.m_pitch = boost::lexical_cast<float>(tokens[1].c_str());
123 else if(tokens[0] == "calibration.roll")
124 params.orientation.m_roll = boost::lexical_cast<float>(tokens[1].c_str());
125 else if(tokens[0] == "calibration.baseline")
126 params.baseline = boost::lexical_cast<float>(tokens[1].c_str());
127 else if(tokens[0] == "depth mapping.downsample ratio")
128 params.downsample = boost::lexical_cast<uint32_t>(tokens[1].c_str());
129 else if(tokens[0] =="advanced.detection.area.step")
130 params.area_step = boost::lexical_cast<double>(tokens[1].c_str());
131 }
132 file.close();
133 }
134 else
135 std::cout << "[WW] 3dv-client::Unable to open INI file\n";
136
137 display = boost::shared_ptr< ::display>(new ::display(params.intrinsics, params.position, params.orientation, params.baseline, params.downsample, params.area_step,log_level));
138 display_thread = boost::thread(boost::bind(&::display::run, display.get()));
139
140 std::string filename;
141
142 boost::posix_time::ptime last_timestamp;
143 boost::posix_time::ptime now_timestamp;
144 boost::posix_time::ptime old_timestamp;
145
146
147 #ifdef _MSC_VER
148 #else
149 set_conio_terminal_mode();
150 #endif
151
152 char p='h';
153 int play=1;
154 float speed=1;
155 int skip=1;
156 int change=1;
157 bool loop=false;
158 bool enable_classification=true;
159 std::cout.precision(2);
160
161 while(1)
162 {
163 while(p!='g')
164 {
165 while (!kbhit())
166 {
167 if (p=='h')
168 {
169 std::cout <<"\n\r[II] 3dv-client: playback menu:\n\n\r"
170 " h : display the help\n\r"
171 " q : quit playback\n\r"
172 "\n\r"
173 " up-arrow, s : start playback\n\r"
174 " down-arrow, r : start reverse playback\n\r"
175 " right-arrow : go to the next frame\n\r"
176 " left-arrow : go to the previous frame\n\r"
177 " p : pause playback\n\r"
178 " l : enable/disable loop playback\n\r"
179 " c : enable/disable classification output\n\r"
180 "\n\r"
181 " home : go to the first frame\n\r"
182 " end : go to the last frame\n\r"
183 " g [nframe] : go to frame nframe\n\r"
184 "\n\r"
185 " +/- : increment/decrement playback speed \n\r"
186 " [1|2|3|4|5|6|7|8|9|0] : set speed [1x|2x|3x|4x|5x|0.1x|0.25x|0.5x|0.75x|max] \n\r"
187 " [F1|F2|F3|F4] : set skip frame to [1|2|5|10] (NOTE: you may want to increase the speed as well)\n\n\r";
188 "\n\r";
189 }
190 else if (p=='s')
191 {play=1;}
192 else if (p=='p')
193 {play=0;}
194 else if (p=='r')
195 {play=-1;}
196 else if (p=='l')
197 {loop=!loop;}
198 else if (p=='c')
199 {enable_classification=!enable_classification;}
200 else if (p=='0')
201 {speed=0;}
202 else if (p=='1')
203 {speed=1;}
204 else if (p=='2')
205 {speed=2;}
206 else if (p=='3')
207 {speed=3;}
208 else if (p=='4')
209 {speed=4;}
210 else if (p=='5')
211 {speed=5;}
212 else if (p=='6')
213 {speed=0.10;}
214 else if (p=='7')
215 {speed=0.25;}
216 else if (p=='8')
217 {speed=0.50;}
218 else if (p=='9')
219 {speed=0.75;}
220 else if (p=='+')
221 {
222 if (speed<=20) speed*=2;
223 else if(speed==0) speed=1;
224 }
225 else if (p=='-')
226 {
227 if (speed>=0.1) speed/=2;
228 else if(speed==0) speed=1;
229 }
230 #ifdef _MSC_VER
231 else if (p=='H')
232 {play=1;}
233 else if (p=='P')
234 {play=-1;}
235 else if (p=='M')
236 {framenumber+=skip;play=2;}
237 else if (p=='K')
238 {framenumber-=skip;play=-2;}
239 else if (p=='G')
240 {framenumber=first;play=0;}
241 else if (p=='O')
242 {framenumber=last;play=0;}
243 else if (p==';')
244 {skip=1;}
245 else if (p=='<')
246 {skip=2;}
247 else if (p=='=')
248 {skip=5;}
249 else if (p=='>')
250 {skip=10;}
251 #else
252 else if (p=='A')
253 {play=1;}
254 else if (p=='B')
255 {play=-1;}
256 else if (p=='C')
257 {framenumber+=skip;play=2;}
258 else if (p=='D')
259 {framenumber-=skip;play=-2;}
260 else if (p=='H')
261 {framenumber=first;play=0;}
262 else if (p=='F')
263 {framenumber=last;play=0;}
264 else if (p=='P')
265 {skip=1;}
266 else if (p=='Q')
267 {skip=2;}
268 else if (p=='R')
269 {skip=5;}
270 else if (p=='S')
271 {skip=10;}
272 #endif
273 else if (p=='q')
274 {
275 if(display)
276 {
277 display->stop();
278 display_thread.join();
279 }
280 std::cout << "\n\r[II] 3dv-client: operation completed\n\r";
281 return lib3dv::error::NONE;
282 }
283 else
284 {
285 change=0;
286 }
287
288 if(loop)
289 {
290 if(framenumber>last)
291 {
292 framenumber=first;
293 std::cout<<"\r\n";
294 change=1;
295 }
296 else if(framenumber<first)
297 {
298 framenumber=last;
299 std::cout<<"\r\n";
300 change=1;
301 }
302 }
303 else
304 {
305 if(framenumber>last)
306 framenumber=last;
307 else if(framenumber<first)
308 framenumber=first;
309 }
310
311 if(change)
312 {
313 if(speed==0.0)
314 {
315 std::cout<<"\r\t\t\t\t\t\tskip="<<skip<<" \r";
316 std::cout<<"\r\t\t\t\tspeed=max \r";
317 }
318 else
319 {
320 std::cout<<"\r\t\t\t\t\t\tskip="<<skip<<" \r";
321 std::cout<<"\r\t\t\t\tspeed="<<speed<<"x \r";
322 }
323 }
324
325 if(play==1 )
326 std::cout<<"\r > ";
327 else if(play==-1 )
328 std::cout<<"\r < ";
329 else if(play==-2 )
330 std::cout<<"\r<--";
331 else if(play==2 )
332 std::cout<<"\r-->";
333 else if(play==0 )
334 std::cout<<"\r = ";
335
336 if(framenumber==last)
337 std::cout<<"E";
338 else if(framenumber==first)
339 std::cout<<"B";
340 else
341 std::cout<<" ";
342
343 std::string str_nframe=(boost::format("%06i") %framenumber).str();
344 if(myframe!=framenumber || change)
345 std::cout<<"\t"<<str_nframe<<":";
346 filename=path+"dsi-"+str_nframe+".pgm";
347 if (boost::filesystem::exists(filename)==1)
348 {
349 boost::shared_ptr<lib3dv::image> dsi_image = boost::shared_ptr<lib3dv::image>(new lib3dv::image());
350 pnm_load<uint16_t>(dsi_image,filename.c_str(),lib3dv::image::type::DSI);
351 boost::posix_time::time_duration steptime;
352 if(play==1)
353 steptime= dsi_image->m_timestamp-last_timestamp;
354 else if(play==-1)
355 steptime=last_timestamp- dsi_image->m_timestamp;
356
357 last_timestamp= dsi_image->m_timestamp;
358 now_timestamp=boost::posix_time::microsec_clock::local_time();
359 boost::posix_time::time_duration codetime=now_timestamp-old_timestamp;
360 if((play==1 || play==-1) && speed!=0 && framenumber!=first && framenumber!=begin_framenumber)
361 {
362 int32_t sleep=steptime.total_milliseconds()/speed-codetime.total_milliseconds();
363 if (sleep>0)
364 {
365 boost::this_thread::sleep(boost::posix_time::milliseconds(sleep));
366 }
367 }
368 old_timestamp=boost::posix_time::microsec_clock::local_time();
369
370 if (myframe!=framenumber)
371 display->update(dsi_image);
372 if(myframe!=framenumber || change)
373 std::cout<<"d";
374 }
375
376 for(int i=0;i<6;i++)
377 {
378 std::string image_type;
379 if (i<2)
380 image_type="left_rectified-";
381 else if (i<4)
382 image_type="right_rectified-";
383 if (i%2==0)
384 filename=path+image_type+str_nframe+".ppm";
385 else
386 filename=path+image_type+str_nframe+".pgm";
387 if (i==4)
388 filename=path+"left_raw-"+str_nframe+".pgm";
389 if (i==5)
390 filename=path+"right_raw-"+str_nframe+".pgm";
391 if (boost::filesystem::exists(filename)==1)
392 {
393 boost::shared_ptr<lib3dv::image> output_image = boost::shared_ptr<lib3dv::image>(new lib3dv::image());
394 pnm_load<uint8_t>(output_image,filename.c_str(),(lib3dv::image::type::types)(i/2+1+(i/5)));
395 if(myframe!=framenumber || change)
396 std::cout<<"i";
397 if (myframe!=framenumber)
398 display->update(output_image);
399 }
400 }
401
402 filename=path+"terrain-"+str_nframe+".txt";
403 boost::shared_ptr<lib3dv::terrain> output_terrain= boost::shared_ptr<lib3dv::terrain>(new lib3dv::terrain());
404 if (boost::filesystem::exists(filename)==1)
405 {
406 std::ifstream file(filename.c_str(), std::ios::in);
407 if(file)
408 {
409 boost::archive::text_iarchive archive(file);
410 archive >> *output_terrain;
411 }
412
413 display->update(output_terrain);
414 if (myframe!=framenumber || change)
415 std::cout<<"t";
416 }
417 else
418 display->update(output_terrain);
419
420 filename=path+"obstacles-"+str_nframe+".txt";
421 boost::shared_ptr< std::vector<lib3dv::obstacle> > output_obstacles=boost::shared_ptr< std::vector<lib3dv::obstacle> >(new std::vector<lib3dv::obstacle>);
422 if (boost::filesystem::exists(filename)==1)
423 {
424 boost::shared_ptr< std::vector<lib3dv::obstacle> > output_obstacles=boost::shared_ptr< std::vector<lib3dv::obstacle> >(new std::vector<lib3dv::obstacle>);
425 std::ifstream fileo(filename.c_str(), std::ios::in);
426 if(fileo)
427 {
428 boost::archive::text_iarchive archive(fileo);
429 archive >> *output_obstacles;
430 }
431 display->update( output_obstacles);
432 if (myframe!=framenumber || change)
433 std::cout<<"o";
434 }
435 else
436 display->update( output_obstacles);
437
438 filename=path+"motion-"+str_nframe+".txt";
439 boost::shared_ptr<lib3dv::motion> output_motion=boost::shared_ptr<lib3dv::motion>(new lib3dv::motion);
440 if (boost::filesystem::exists(filename)==1 )
441 {
442 boost::shared_ptr<lib3dv::motion> output_motion=boost::shared_ptr<lib3dv::motion>(new lib3dv::motion);
443 std::ifstream filem(filename.c_str(), std::ios::in);
444 if(filem)
445 {
446 boost::archive::text_iarchive archive(filem);
447 archive >> *output_motion;
448 }
449 if (myframe!=framenumber)
450 display->update(output_motion);
451 if (myframe!=framenumber || change)
452 std::cout<<"m";
453 }
454 else
455 {
456 display->update(output_motion);
457 }
458
459 filename=path+"classification-"+str_nframe+".txt";
460 boost::shared_ptr<lib3dv::classification> output_classificat=boost::shared_ptr<lib3dv::classification>(new lib3dv::classification);
461 if (boost::filesystem::exists(filename)==1 )
462 {
463
464 std::ifstream filec(filename.c_str(), std::ios::in);
465 if(filec && enable_classification)
466 {
467 boost::archive::text_iarchive archive(filec);
468 archive >> *output_classificat;
469 }
470 if (myframe!=framenumber)
471 display->update(output_classificat);
472 if (myframe!=framenumber || change)
473 std::cout<<"c";
474 }
475 else
476 display->update(output_classificat);
477
478 if (myframe!=framenumber || change)
479 {
480 std::cout<<" \r";
481 }
482 std::cout.flush();
483 change=1;
484 myframe=framenumber;
485 if(play==1)
486 framenumber+=skip;
487 if(play==-1)
488 framenumber-=skip;
489 p='a';
490 }
491 p=getch();
492 }
493 std::cout<<"\n\rInsert the framenumber and press enter:";
494
495 #ifdef _MSC_VER
496 std::cin>>framenumber;
497 #else
498 reset_terminal_mode();
499 std::cin>>framenumber;
500 set_conio_terminal_mode();
501 #endif
502
503 play=0;
504 std::cout<<"GO->\t"<<(boost::format("%06i") %framenumber).str();
505 change=1;
506 p='a';
507 }//end while(1)
508
509 }
510 else
511 {
512 std::cerr << "[EE] 3dv-client: the path :"<< path<<" doesn't exist"<< std::endl;
513 return lib3dv::error::NONE;
514 }
515
516 }
517 else
518 {
519 std::cerr << "[EE] 3dv-client: at least 1 argument must be provided to --playback" << std::endl;
520 return lib3dv::error::NONE;
521 }
522}
Note: See TracBrowser for help on using the repository browser.