4 #include <opencv2/highgui/highgui.hpp> 5 #include <opencv2/imgproc/imgproc.hpp> 6 #include <opencv2/viz/vizcore.hpp> 8 #include <lvr/geometry/HalfEdgeVertex.hpp> 13 typedef lvr::ColorVertex<float, unsigned char>
cVertex;
14 typedef lvr::HalfEdgeVertex<cVertex, lvr::Normal<float> >*
VertexPtr;
22 if(event.action != cv::viz::KeyboardEvent::KEY_DOWN)
25 if(event.code ==
't' || event.code ==
'T')
28 if(event.code ==
'i' || event.code ==
'I')
31 if(event.code ==
'r' || event.code ==
'R')
33 kinfu.
kinfu_->triggerRecord();
37 if(event.code ==
'g' || event.code ==
'G')
40 if(event.code ==
'c' || event.code ==
'C')
43 if(event.code ==
'd' || event.code ==
'D')
51 kinfu.
kinfu_->params().distance_camera_target += 0.1;
52 kinfu.
kinfu_->performShift();
56 kinfu.
kinfu_->params().distance_camera_target -= 0.1;
57 kinfu.
kinfu_->performShift();
59 if(!event.symbol.compare(
"Escape"))
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)
76 capture_.setRegistration(
true);
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());
88 cv::Vec2d fov(0.64,0.48);
89 cv::viz::WCameraPosition arrow(fov, 0.5, cv::viz::Color::silver());
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);
97 viz.showWidget(
"coor", cv::viz::WCoordinateSystem(0.3));
99 viz.setWidgetPose(
"cube0", kinfu_->tsdf().getPose());
100 viz.registerKeyboardCallback(KeyboardCallback,
this);
101 viz.setWindowSize(cv::Size(1800,480));
104 cv::namedWindow(
"Scene", 0 );
105 cv::resizeWindow(
"Scene",800,500);
106 cv::moveWindow(
"Scene", 800, 500);
108 cv::namedWindow(
"Image", 0 );
109 cv::resizeWindow(
"Image",800, 500);
110 cv::moveWindow(
"Image", 0, 500);
111 timer_start_ = (double)cv::getTickCount();
113 sample_poses_.push_back(kinfu_->getCameraPose());
114 viz.showWidget(
"path", cv::viz::WTrajectory(sample_poses_));
119 unordered_map<VertexPtr, size_t> index_map;
120 size_t verts_size = 0;
121 size_t faces_size = 0;
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;
133 cv_mesh =
new cv::viz::Mesh();
134 cv_mesh->cloud.create(1, slice_size, CV_32FC3);
139 verts.create(1, slice_size, CV_32FC3);
143 for(
size_t k = 0; k < slice_size; k++)
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]);
150 cv::hconcat(mesh_->cloud, verts, mesh_->cloud);
156 cv_mesh->polygons.create(1, lvr_mesh->getFaces().size() * 4, CV_32SC1);
157 poly_ptr = cv_mesh->polygons.ptr<
int>();
161 faces.create(1, slice_face_size * 4, CV_32SC1);
162 poly_ptr = faces.ptr<
int>();
164 for(
size_t k = 0; k < slice_face_size; k++)
166 auto face = lvr_mesh->getFaces()[k + faces_size];
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()];
173 cv::hconcat(mesh_->polygons, faces, mesh_->polygons);
181 auto fused_map = lvr_mesh->m_fused_verts;
186 cv_mesh->colors.create(1, slice_size, CV_64FC(3));
187 cptr = cv_mesh->colors.ptr<cv::Vec3d>();
191 buffer.create(1, slice_size, CV_64FC(3));
192 cptr = buffer.ptr<cv::Vec3d>();
196 for(
size_t i = 0; i < slice_size; ++i)
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);
203 *cptr++ = cv::Vec3d(0.0, 255.0, 0.0);
208 buffer.convertTo(buffer, CV_8U, 255.0);
209 cv::hconcat(mesh_->colors, buffer, mesh_->colors);
213 cv_mesh->colors.convertTo(cv_mesh->colors, CV_8U, 255.0);
216 verts_size = lvr_mesh->getVertices().size();
217 faces_size = lvr_mesh->getFaces().size();
224 kinfu_->triggerCheckForShift();
231 depth.convertTo(display, CV_8U, 255.0/4000);
232 cv::imshow(
"Depth", display);
243 view_host_.create(view_device_.rows(), view_device_.cols(), CV_8UC4);
244 view_device_.download(view_host_.ptr<
void>(), view_host_.step);
246 cv::imshow(
"Scene", view_host_);
255 viz.setWidgetPose(
"cube0", kinfu.
tsdf().
getPose());
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)));
271 cout <<
"Performing last scan" << std::endl;
278 imgpose->
pose = pose;
279 imgpose->
image = image;
283 cv::Mat intrinsics = (cv::Mat_<float>(3,3) << kinfu.
params().
intr.
fx*2, 0, 1280/2-0.5f + 3,
295 imwrite(
string(
"Pic" + std::to_string(pic_count_) +
".png"), image );
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;
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;
308 pose_file <<
"Camera Intrinsics: " << endl;
316 KinFu& kinfu = *kinfu_;
317 cv::Mat depth, image, image_copy;
320 std::thread mesh_viz;
322 std::vector<Affine3f> posen;
323 std::vector<cv::Mat> rvecs;
326 cv::Mat best_rvec,best_image;
333 for (
int i = 0; !exit_ && !viz.wasStopped(); ++i)
337 int has_frame = capture_.grab(depth, image);
338 cv::flip(depth, depth, 1);
339 cv::flip(image, image, 1);
342 return std::cout <<
"Can't grab" << std::endl,
false;
346 depth_device_.upload(depth.data, depth.step, depth.rows, depth.cols);
350 has_image = kinfu(depth_device_);
354 if ((!pause_ || !capture_.isRecord()) && !(kinfu.
hasShifted() && kinfu.
isLastScan()) && has_image)
358 double ref_timer = cv::getTickCount();
359 double time =
abs((timer_start_ - ref_timer))/ cv::getTickFrequency();
362 image.copyTo(image_copy);
375 for(
size_t z=0;z<rvecs.size();z++){
376 dist +=
norm(mom_rvec-rvecs[z]);
378 if(dist > best_dist){
382 best_rvec = mom_rvec.clone();
383 best_image = image.clone();
391 rvecs.push_back(best_rvec);
392 posen.push_back(best_pose);
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;
403 show_raycasted(kinfu);
408 if(garbageMesh_ !=
NULL)
410 viz.removeWidget(
"mesh");
413 viz.showWidget(
"mesh", cv::viz::WMesh(*mesh_));
414 garbageMesh_ = mesh_;
421 cv::imshow(
"Image", image);
423 if (!iteractive_mode_)
428 int key = cv::waitKey(3);
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;
440 case 27:
case 32: exit_ =
true;
break;
443 if(frame_count==20) frame_count=0;
444 viz.spinOnce(2,
true);
452 unsigned int image_count=0;
453 unsigned int frame_count=0;
454 bool exit_, iteractive_mode_,
pause_, meshRender_, no_viz_;
lvr::HalfEdgeVertex< cVertex, lvr::Normal< float > > * VertexPtr
cuda::Depth depth_device_
const kaboom::Options * options
lvr::ColorVertex< float, unsigned char > cVertex
void storePicPose(KinFu &kinfu, Affine3f pose, cv::Mat image)
const cuda::CyclicalBuffer & cyclical() const
float getCameraOffset() const
void take_cloud(KinFu &kinfu)
A class to parse the program options for the reconstruction executable.
void extractImage(KinFu &kinfu, cv::Mat &image)
void storePicPose(KinFu &kinfu, Affine3f pose, cv::Mat image)
static KinFuParams default_params()
Affine3f getCameraPose(int time=-1) const
double distance_camera_target
void show_depth(const cv::Mat &depth)
void addImgPose(ImgPose *imgPose)
cuda::DeviceArray< Point > cloud_buffer
cv::viz::Mesh * garbageMesh_
const cuda::TsdfVolume & tsdf() const
vector< Affine3f > sample_poses_
void show_raycasted(KinFu &kinfu)
KinFuApp(OpenNISource &source, Options *options)
static void KeyboardCallback(const cv::viz::KeyboardEvent &event, void *pthis)
__kf_device__ float norm(const float3 &v)
void show_cube(KinFu &kinfu)
void renderImage(cuda::Image &image, int flags=0)
float getShiftingDistance() const
const KinFuParams & params() const