void extractImage(KinFu& kinfu, cv::Mat& image)
{
pic_count_++;
auto trans = kinfu.getCameraPose().translation();
trans += Vec3f(150,150,150);
imwrite( string("Pic" + std::to_string(pic_count_) + ".jpg"), image );
ofstream pose_file;
pose_file.open (string("Pic" + std::to_string(pic_count_) + ".pose"));
pose_file << "Rotation: " << kinfu.getCameraPose().rotation();
pose_file << endl;
pose_file << "Translation: " << trans;
pose_file.close();
}
void show_raycasted(KinFu& kinfu)
{
const int mode = 4;
//if (iteractive_mode_)
//kinfu.renderImage(view_device_, viz.getViewerPose(), mode);
//else
kinfu.renderImage(view_device_, mode);
view_host_.create(view_device_.rows(), view_device_.cols(), CV_8UC4);
view_device_.download(view_host_.ptr<void>(), view_host_.step);
cv::imshow("Scene", view_host_);
viz.setWidgetPose("arrow", kinfu.getCameraPose());
}