KinFuApp.hpp
Go to the documentation of this file.
1 #pragma once
2 
3 #include <iostream>
4 #include <opencv2/highgui/highgui.hpp>
5 #include <opencv2/imgproc/imgproc.hpp>
6 #include <opencv2/viz/vizcore.hpp>
7 #include <kfusion/kinfu.hpp>
8 #include <lvr/geometry/HalfEdgeVertex.hpp>
9 
10 #include <io/capture.hpp>
11 
12 using namespace kfusion;
13 typedef lvr::ColorVertex<float, unsigned char> cVertex;
14 typedef lvr::HalfEdgeVertex<cVertex, lvr::Normal<float> >* VertexPtr;
15 
16 struct KinFuApp
17 {
18  static void KeyboardCallback(const cv::viz::KeyboardEvent& event, void* pthis)
19  {
20  KinFuApp& kinfu = *static_cast<KinFuApp*>(pthis);
21 
22  if(event.action != cv::viz::KeyboardEvent::KEY_DOWN)
23  return;
24 
25  if(event.code == 't' || event.code == 'T')
26  kinfu.take_cloud(*kinfu.kinfu_);
27 
28  if(event.code == 'i' || event.code == 'I')
29  kinfu.set_interactive();
30 
31  if(event.code == 'r' || event.code == 'R')
32  {
33  kinfu.kinfu_->triggerRecord();
34  kinfu.capture_.triggerRecord();
35  }
36 
37  if(event.code == 'g' || event.code == 'G')
38  kinfu.extractImage(*kinfu.kinfu_, *kinfu.image_);
39 
40  if(event.code == 'c' || event.code == 'C')
41  kinfu.checkForShift();
42 
43  if(event.code == 'd' || event.code == 'D')
44  {
45  kinfu.capture_.triggerPause();
46  kinfu.pause_ = !kinfu.pause_;
47  }
48 
49  if(event.code == '+')
50  {
51  kinfu.kinfu_->params().distance_camera_target += 0.1;
52  kinfu.kinfu_->performShift();
53  }
54  if(event.code == '-')
55  {
56  kinfu.kinfu_->params().distance_camera_target -= 0.1;
57  kinfu.kinfu_->performShift();
58  }
59  if(!event.symbol.compare("Escape"))
60  {
61  kinfu.exit_ = true;
62  }
63  }
65  exit_ (false), iteractive_mode_(false), pause_(false), meshRender_(false), no_viz_(options->noVizualisation()),
66  capture_ (source), cube_count_(0), pic_count_(0), mesh_(NULL), garbageMesh_(NULL)
67  {
69  params.shifting_distance = options->getShiftingDistance();
70  params.distance_camera_target = options->getCameraOffset();
71  params.volume_pose = params.volume_pose.translate(Vec3f(0, 0, options->getCameraOffset()));
72  params.cmd_options = options;
73 
74  kinfu_ = KinFu::Ptr( new KinFu(params) );
75 
76  capture_.setRegistration(true);
77 
78  viz.showWidget("legend", cv::viz::WText("Controls", cv::Point(5, 205), 30, cv::viz::Color::red()));
79  viz.showWidget("r", cv::viz::WText("r Trigger record", cv::Point(5, 175), 20, cv::viz::Color::green()));
80  viz.showWidget("d", cv::viz::WText("d Trigger pause", cv::Point(5, 150), 20, cv::viz::Color::green()));
81  viz.showWidget("t", cv::viz::WText("t Finish scan", cv::Point(5, 125), 20, cv::viz::Color::green()));
82  viz.showWidget("g", cv::viz::WText("g Export image & pose", cv::Point(5, 100), 20, cv::viz::Color::green()));
83  viz.showWidget("i", cv::viz::WText("i Interactive mode", cv::Point(5, 75), 20, cv::viz::Color::green()));
84  viz.showWidget("+", cv::viz::WText("+/- Change cam distance", cv::Point(5, 50), 20, cv::viz::Color::green()));
85  viz.showWidget("esc", cv::viz::WText("ESC Quit", cv::Point(5, 25), 20, cv::viz::Color::green()));
86  cv::viz::WCube cube(cv::Vec3d::all(0), cv::Vec3d(params.volume_size), true, cv::viz::Color::red());
87  //cv::viz::WArrow arrow(cv::Point3f(0, 0, 0), cv::Point3f(0, 0, options->getCameraOffset()), 0.03, cv::viz::Color::green());
88  cv::Vec2d fov(0.64,0.48);
89  cv::viz::WCameraPosition arrow(fov, 0.5, cv::viz::Color::silver());
90  cv::viz::WSphere sphere(cv::Point3f(0, 0, 0), params.shifting_distance);
91  sphere.setRenderingProperty(cv::viz::OPACITY, 0.2);
92  cube.setRenderingProperty(cv::viz::LINE_WIDTH, 2.0);
93  arrow.setRenderingProperty(cv::viz::LINE_WIDTH, 2.0);
94  viz.showWidget("cube0", cube);
95  viz.showWidget("arrow", arrow);
96  //viz.showWidget("center", sphere);
97  viz.showWidget("coor", cv::viz::WCoordinateSystem(0.3));
98  //show_cube(*kinfu_);
99  viz.setWidgetPose("cube0", kinfu_->tsdf().getPose());
100  viz.registerKeyboardCallback(KeyboardCallback, this);
101  viz.setWindowSize(cv::Size(1800,480));
102  viz.setWindowPosition(cv::Point(0,0));
103 
104  cv::namedWindow("Scene", 0 );
105  cv::resizeWindow("Scene",800,500);
106  cv::moveWindow("Scene", 800, 500);
107 
108  cv::namedWindow("Image", 0 );
109  cv::resizeWindow("Image",800, 500);
110  cv::moveWindow("Image", 0, 500);
111  timer_start_ = (double)cv::getTickCount();
112  set_interactive();
113  sample_poses_.push_back(kinfu_->getCameraPose());
114  viz.showWidget("path", cv::viz::WTrajectory(sample_poses_));
115  }
116 
117  void show_mesh()
118  {
119  unordered_map<VertexPtr, size_t> index_map;
120  size_t verts_size = 0;
121  size_t faces_size = 0;
122  while(true)
123  {
124  auto lvr_mesh = kinfu_->cyclical().getMesh();
125  size_t slice_size = lvr_mesh->getVertices().size() - verts_size;
126  size_t slice_face_size = lvr_mesh->getFaces().size() - faces_size;
127  cv::viz::Mesh* cv_mesh;
128  //fill cloud
129  cv::Mat verts;
130  cv::Vec3f *ddata;
131  if(mesh_ == NULL)
132  {
133  cv_mesh = new cv::viz::Mesh();
134  cv_mesh->cloud.create(1, slice_size, CV_32FC3);
135  ddata = cv_mesh->cloud.ptr<cv::Vec3f>();
136  }
137  else
138  {
139  verts.create(1, slice_size, CV_32FC3);
140  ddata = verts.ptr<cv::Vec3f>();
141  }
142 
143  for(size_t k = 0; k < slice_size; k++)
144  {
145  auto vertex = lvr_mesh->getVertices()[k + verts_size];
146  index_map[vertex] = k + verts_size;
147  *ddata++ = cv::Vec3f(vertex->m_position[0], vertex->m_position[1], vertex->m_position[2]);
148  }
149  if(mesh_ != NULL)
150  cv::hconcat(mesh_->cloud, verts, mesh_->cloud);
151  //fill polygons
152  cv::Mat faces;
153  int* poly_ptr;
154  if(mesh_ == NULL)
155  {
156  cv_mesh->polygons.create(1, lvr_mesh->getFaces().size() * 4, CV_32SC1);
157  poly_ptr = cv_mesh->polygons.ptr<int>();
158  }
159  else
160  {
161  faces.create(1, slice_face_size * 4, CV_32SC1);
162  poly_ptr = faces.ptr<int>();
163  }
164  for(size_t k = 0; k < slice_face_size; k++)
165  {
166  auto face = lvr_mesh->getFaces()[k + faces_size];
167  *poly_ptr++ = 3;
168  *poly_ptr++ = index_map[face->m_edge->end()];
169  *poly_ptr++ = index_map[face->m_edge->next()->end()];
170  *poly_ptr++ = index_map[face->m_edge->next()->next()->end()];
171  }
172  if(mesh_ != NULL)
173  cv::hconcat(mesh_->polygons, faces, mesh_->polygons);
174 
175  //fill color
176  cv::Mat colors;
177  cv::Mat buffer;
178  cv::Vec3d *cptr;
179  size_t size;
180  //auto cBuffer = lvr_mesh->meshBuffer()->getVertexColorArray(size);
181  auto fused_map = lvr_mesh->m_fused_verts;
182  //cout << "slcie size " << slice_size << endl;
183  //cout << "color size " << size << endl;
184  if(mesh_ == NULL)
185  {
186  cv_mesh->colors.create(1, slice_size, CV_64FC(3));
187  cptr = cv_mesh->colors.ptr<cv::Vec3d>();
188  }
189  else
190  {
191  buffer.create(1, slice_size, CV_64FC(3));
192  cptr = buffer.ptr<cv::Vec3d>();
193  //buffer.convertTo(colors, CV_8U, 255.0);
194 
195  }
196  for(size_t i = 0; i < slice_size; ++i)
197  {
198  if(lvr_mesh->getVertices()[i + verts_size]->m_fused && !lvr_mesh->getVertices()[i + verts_size]->m_fusedNeighbor)
199  *cptr++ = cv::Vec3d(0.0, 0.0, 255.0);
200  else if(lvr_mesh->getVertices()[i + verts_size]->m_fusedNeighbor)
201  *cptr++ = cv::Vec3d(255.0, 0.0, 0.0);
202  else
203  *cptr++ = cv::Vec3d(0.0, 255.0, 0.0);
204  }
205  //*cptr++ = cv::Vec3d(3 * cBuffer[fused_map[i]], 3 * cBuffer[fused_map[i] + 1], 3 * cBuffer[fused_map[i] + 2]);
206  if(mesh_ != NULL)
207  {
208  buffer.convertTo(buffer, CV_8U, 255.0);
209  cv::hconcat(mesh_->colors, buffer, mesh_->colors);
210  }
211  else
212  {
213  cv_mesh->colors.convertTo(cv_mesh->colors, CV_8U, 255.0);
214  mesh_ = cv_mesh;
215  }
216  verts_size = lvr_mesh->getVertices().size();
217  faces_size = lvr_mesh->getFaces().size();
218  meshRender_ = true;
219  }
220  }
221 
223  {
224  kinfu_->triggerCheckForShift();
225  }
226 
227  void show_depth(const cv::Mat& depth)
228  {
229  cv::Mat display;
230  //cv::normalize(depth, display, 0, 255, cv::NORM_MINMAX, CV_8U);
231  depth.convertTo(display, CV_8U, 255.0/4000);
232  cv::imshow("Depth", display);
233  }
234 
235  void show_raycasted(KinFu& kinfu)
236  {
237  const int mode = 4;
238  //if (iteractive_mode_)
239  //kinfu.renderImage(view_device_, viz.getViewerPose(), mode);
240  //else
241  kinfu.renderImage(view_device_, mode);
242 
243  view_host_.create(view_device_.rows(), view_device_.cols(), CV_8UC4);
244  view_device_.download(view_host_.ptr<void>(), view_host_.step);
245 
246  cv::imshow("Scene", view_host_);
247 
248  if(iteractive_mode_)
249  viz.setWidgetPose("arrow", kinfu.getCameraPose());
250  }
251 
252  void show_cube(KinFu& kinfu)
253  {
254  cube_count_++;
255  viz.setWidgetPose("cube0", kinfu.tsdf().getPose());
256  //cv::Affine3f center = kinfu.tsdf().getPose().translate(cv::Vec3f(1.5, 1.5, 1.5));
257  //viz.setWidgetPose("center", center);
258  }
259 
261  {
262  iteractive_mode_ = !iteractive_mode_;
263  Affine3f eagle_view = Affine3f::Identity();
264  eagle_view.translate(kinfu_->getCameraPose().translation());
265  viz.setViewerPose(eagle_view.translate(Vec3f(0,0,-1)));
266  }
267 
268 
269  void take_cloud(KinFu& kinfu)
270  {
271  cout << "Performing last scan" << std::endl;
272  kinfu.performLastScan();
273  }
274 
275  void storePicPose(KinFu& kinfu, Affine3f pose, cv::Mat image)
276  {
277  ImgPose* imgpose = new ImgPose();
278  imgpose->pose = pose;
279  imgpose->image = image;//.clone();
280  //intrinsics wrong? changed rows and cols + /2
281  //kinfu.params().cols/2 - 0.5f
282  //kinfu.params().rows/2 - 0.5f
283  cv::Mat intrinsics = (cv::Mat_<float>(3,3) << kinfu.params().intr.fx*2, 0, 1280/2-0.5f + 3,
284  0, kinfu.params().intr.fx*2, 1024/2-0.5f,
285  0, 0, 1);
286  imgpose->intrinsics = intrinsics;
287  kinfu.cyclical().addImgPose(imgpose);
288  }
289 
290  void extractImage(KinFu& kinfu, cv::Mat& image)
291  {
292  pic_count_++;
293  auto trans = kinfu.getCameraPose().translation();
294  auto rot = kinfu.getCameraPose().rotation();
295  imwrite( string("Pic" + std::to_string(pic_count_) + ".png"), image );
296  ofstream pose_file;
297  pose_file.open (string("Pic" + std::to_string(pic_count_) + ".txt"));
298  pose_file << "Translation: " << endl;
299  pose_file << trans[0] << endl;
300  pose_file << trans[1] << endl;
301  pose_file << trans[2] << endl;
302  pose_file << endl;
303  pose_file << "Rotation: " << endl;
304  pose_file << rot(0,0) << " " << rot(0,1) << " " << rot(0,2) << endl;
305  pose_file << rot(1,0) << " " << rot(1,1) << " " << rot(1,2) << endl;
306  pose_file << rot(2,0) << " " << rot(2,1) << " " << rot(2,2) << endl;
307  pose_file << endl;
308  pose_file << "Camera Intrinsics: " << endl;
309  pose_file << kinfu.params().intr.fx << " " << kinfu.params().rows << " " << kinfu.params().cols << endl;
310  pose_file.close();
311  }
312 
313 
314  bool execute()
315  {
316  KinFu& kinfu = *kinfu_;
317  cv::Mat depth, image, image_copy;
318  double time_ms = 0;
319  int has_image = 0;
320  std::thread mesh_viz;
321 
322  std::vector<Affine3f> posen;
323  std::vector<cv::Mat> rvecs;
324 
325  Affine3f best_pose;
326  cv::Mat best_rvec,best_image;
327  float best_dist=0.0;
328 
329  if(!no_viz_)
330  {
331  mesh_viz = thread(&KinFuApp::show_mesh,this);
332  }
333  for (int i = 0; !exit_ && !viz.wasStopped(); ++i)
334  {
335  if((!pause_ || !capture_.isRecord()) && !(kinfu.hasShifted() && kinfu.isLastScan()))
336  {
337  int has_frame = capture_.grab(depth, image);
338  cv::flip(depth, depth, 1);
339  cv::flip(image, image, 1);
340  image_ = &image;
341  if (has_frame == 0)
342  return std::cout << "Can't grab" << std::endl, false;
343  // check if oni file ended
344  if (has_frame == 2)
345  take_cloud(kinfu);
346  depth_device_.upload(depth.data, depth.step, depth.rows, depth.cols);
347 
348  {
349  SampledScopeTime fps(time_ms); (void)fps;
350  has_image = kinfu(depth_device_);
351  }
352  }
353 
354  if ((!pause_ || !capture_.isRecord()) && !(kinfu.hasShifted() && kinfu.isLastScan()) && has_image)
355  {
356  //biggest rvec difference -> new pic
357  //
358  double ref_timer = cv::getTickCount();
359  double time = abs((timer_start_ - ref_timer))/ cv::getTickFrequency();
360 
361  if(rvecs.size()<1){
362  image.copyTo(image_copy);
363 
364  //buffer of all imgposes
365  rvecs.push_back(cv::Mat(kinfu.getCameraPose().rvec()));
366  posen.push_back(kinfu.getCameraPose());
367 
368  //storePicPose(kinfu, image_copy);
369  //extractImage(kinfu, image_copy);
370  }
371  else
372  {
373  float dist = 0.0;
374  cv::Mat mom_rvec(kinfu.getCameraPose().rvec());
375  for(size_t z=0;z<rvecs.size();z++){
376  dist += norm(mom_rvec-rvecs[z]);
377  }
378  if(dist > best_dist){
379  best_dist = dist;
380  //mom_rvec.copyTo(best_rvec);
381  //image.copyTo(best_image);
382  best_rvec = mom_rvec.clone();
383  best_image = image.clone();
384  best_pose = kinfu.getCameraPose();
385  //std::cout << "better image found, sum rvec distances: " << best_dist << std::endl;
386  }
387  //if(time - 3.0 > 0)
388  if(kinfu.params().cmd_options->textures() && frame_count==7)
389  {
390 
391  rvecs.push_back(best_rvec);
392  posen.push_back(best_pose);
393 
394  storePicPose(kinfu, best_pose, best_image);
395  //extractImage(kinfu, best_image);
396  sample_poses_.push_back(kinfu_->getCameraPose());
397  viz.showWidget("path", cv::viz::WTrajectory(sample_poses_));
398  std::cout << "image taken "<< image_count++ << ", time: "<< time << std::endl;
399  timer_start_ = ref_timer;
400 
401  }
402  }
403  show_raycasted(kinfu);
404  }
405 
406  if(meshRender_)
407  {
408  if(garbageMesh_ != NULL)
409  {
410  viz.removeWidget("mesh");
411  //delete garbageMesh_;
412  }
413  viz.showWidget("mesh", cv::viz::WMesh(*mesh_));
414  garbageMesh_ = mesh_;
415  meshRender_ = false;
416  }
417 
418  if(kinfu.hasShifted())
419  show_cube(kinfu);
420 
421  cv::imshow("Image", image);
422 
423  if (!iteractive_mode_)
424  {
425  viz.setViewerPose(kinfu.getCameraPose());
426  }
427 
428  int key = cv::waitKey(3);
429 
430  switch(key)
431  {
432  case 't': case 'T' : take_cloud(kinfu); break;
433  case 'i': case 'I' : set_interactive(); break;
434  case 'd': case 'D' : capture_.triggerPause();pause_ = !pause_; break;
435  case 'r': case 'R' : kinfu.triggerRecord(); capture_.triggerRecord(); break;
436  case 'g': case 'G' : extractImage(kinfu, image); break;
437  case 'c': case 'C' : checkForShift(); break;
438  case '+': kinfu.params().distance_camera_target += 0.1; kinfu.performShift(); break;
439  case '-': kinfu.params().distance_camera_target -= 0.1; kinfu.performShift(); break;
440  case 27: case 32: exit_ = true; break;
441  }
442  frame_count++;
443  if(frame_count==20) frame_count=0;
444  viz.spinOnce(2, true);
445  //exit_ = exit_ || ( kinfu.hasShifted() && kinfu.isLastScan() );
446  //if(kinfu.cyclical().getSliceCount() == 4 && !(kinfu.hasShifted() && kinfu.isLastScan()))
447  //take_cloud(kinfu);
448  }
449  return true;
450  }
451 
452  unsigned int image_count=0;
453  unsigned int frame_count=0;
454  bool exit_, iteractive_mode_, pause_, meshRender_, no_viz_;
457  vector<Affine3f> sample_poses_;
458  cv::viz::Viz3d viz;
459  cv::viz::Mesh* mesh_;
460  cv::viz::Mesh* garbageMesh_;
461  size_t cube_count_, pic_count_;
462  double timer_start_;
463  cv::Mat view_host_;
464  cv::Mat* image_;
468 };
KinFu::Ptr kinfu_
Definition: KinFuApp.hpp:456
cv::Mat image
Definition: types.hpp:31
lvr::HalfEdgeVertex< cVertex, lvr::Normal< float > > * VertexPtr
Definition: KinFuApp.hpp:14
cv::Vec3f Vec3f
Definition: types.hpp:16
void set_interactive()
Definition: KinFuApp.hpp:260
cuda::Depth depth_device_
Definition: KinFuApp.hpp:466
OpenNISource & capture_
Definition: KinFuApp.hpp:455
const kaboom::Options * options
cv::Mat view_host_
Definition: KinFuApp.hpp:463
lvr::ColorVertex< float, unsigned char > cVertex
Definition: KinFuApp.hpp:13
cv::Mat intrinsics
Definition: types.hpp:33
void storePicPose(KinFu &kinfu, Affine3f pose, cv::Mat image)
Definition: MainWindow.cpp:27
cv::Affine3f Affine3f
Definition: types.hpp:18
size_t pic_count_
Definition: KinFuApp.hpp:461
bool execute()
Definition: KinFuApp.hpp:314
void performLastScan()
Definition: kinfu.cpp:41
const cuda::CyclicalBuffer & cyclical() const
Definition: kinfu.cpp:52
cv::Ptr< KinFu > Ptr
Definition: kinfu.hpp:25
void take_cloud(KinFu &kinfu)
Definition: KinFuApp.hpp:269
cv::viz::Mesh * mesh_
Definition: KinFuApp.hpp:459
A class to parse the program options for the reconstruction executable.
bool exit_
Definition: KinFuApp.hpp:454
void extractImage(KinFu &kinfu, cv::Mat &image)
Definition: KinFuApp.hpp:290
float fx
Definition: types.hpp:22
void storePicPose(KinFu &kinfu, Affine3f pose, cv::Mat image)
Definition: KinFuApp.hpp:275
bool hasShifted()
Definition: kinfu.hpp:43
static KinFuParams default_params()
Definition: core.cpp:238
Affine3f getCameraPose(int time=-1) const
Definition: kinfu.cpp:117
double distance_camera_target
Definition: types.hpp:142
void show_depth(const cv::Mat &depth)
Definition: KinFuApp.hpp:227
bool pause_
Definition: KinFuApp.hpp:454
Options * cmd_options
Definition: types.hpp:162
void show_mesh()
Definition: KinFuApp.hpp:117
Affine3f getPose() const
Definition: tsdf_volume.cpp:64
DeviceArray2D class
T abs(T x)
Definition: nanoflann.hpp:237
void addImgPose(ImgPose *imgPose)
float shifting_distance
Definition: types.hpp:141
cuda::DeviceArray< Point > cloud_buffer
Definition: KinFuApp.hpp:467
cv::viz::Mesh * garbageMesh_
Definition: KinFuApp.hpp:460
cv::Mat * image_
Definition: KinFuApp.hpp:464
const cuda::TsdfVolume & tsdf() const
Definition: kinfu.cpp:46
Affine3f volume_pose
Definition: types.hpp:139
vector< Affine3f > sample_poses_
Definition: KinFuApp.hpp:457
bool isLastScan()
Definition: kinfu.hpp:52
void performShift()
Definition: kinfu.hpp:41
void show_raycasted(KinFu &kinfu)
Definition: KinFuApp.hpp:235
cv::viz::Viz3d viz
Definition: KinFuApp.hpp:458
KinFuApp(OpenNISource &source, Options *options)
Definition: KinFuApp.hpp:64
double timer_start_
Definition: KinFuApp.hpp:462
cuda::Image view_device_
Definition: KinFuApp.hpp:465
void checkForShift()
Definition: KinFuApp.hpp:222
Affine3f pose
Definition: types.hpp:32
static void KeyboardCallback(const cv::viz::KeyboardEvent &event, void *pthis)
Definition: KinFuApp.hpp:18
__kf_device__ float norm(const float3 &v)
Definition: temp_utils.hpp:87
Utility.
Definition: capture.hpp:8
void triggerRecord()
Definition: kinfu.hpp:49
void show_cube(KinFu &kinfu)
Definition: KinFuApp.hpp:252
#define NULL
Definition: mydefs.hpp:141
void renderImage(cuda::Image &image, int flags=0)
Definition: kinfu.cpp:214
const KinFuParams & params() const
Definition: kinfu.cpp:35


lvr2
Author(s): Thomas Wiemann , Sebastian Pütz , Alexander Mock , Lars Kiesow , Lukas Kalbertodt , Tristan Igelbrink , Johan M. von Behren , Dominik Feldschnieders , Alexander Löhr
autogenerated on Mon Feb 28 2022 22:46:06