void show_cube(KinFu& kinfu)
{
cube_count_++;
string new_cube_name = string("cube" + to_string(cube_count_));
cv::viz::WCube cube(cv::Vec3d::all(0), cv::Vec3d(kinfu.params().volume_size), true, cv::viz::Color::apricot());
//viz.showWidget(new_cube_name, cube);
//viz.setWidgetPose(new_cube_name, viz.getWidgetPose("cube0"));
viz.setWidgetPose("cube0", kinfu.tsdf().getPose());
}
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 storePicPose(KinFu& kinfu, Affine3f pose, cv::Mat image)
{
ImgPose* imgpose = new ImgPose();
imgpose->pose = pose;
imgpose->image = image;//.clone();
//intrinsics wrong? changed rows and cols + /2
//kinfu.params().cols/2 - 0.5f
//kinfu.params().rows/2 - 0.5f
cv::Mat intrinsics = (cv::Mat_<float>(3,3) << kinfu.params()->intr.fx*2, 0, 1280/2-0.5f + 3,
0, kinfu.params()->intr.fx*2, 1024/2-0.5f,
0, 0, 1);
imgpose->intrinsics = intrinsics;
kinfu.cyclical().addImgPose(imgpose);
}
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());
}
void take_cloud(KinFu& kinfu)
{
cout << "Performing last scan" << std::endl;
/*Vec3i global_shift_;
tsdf_buffer buffer_ = kinfu.cyclical().getBuffer();
//cuda::DeviceArray<Point> cloud = kinfu.tsdf().fetchCloud(cloud_buffer, kinfu.cyclical().getBuffer());
cuda::DeviceArray<Point> cloud = kinfu.tsdf().fetchSliceAsCloud (cloud_buffer, &buffer_, buffer_.voxels_size.x - 1, buffer_.voxels_size.y - 1, buffer_.voxels_size.z - 1, global_shift_);
cv::Mat cloud_host(1, (int)cloud.size(), CV_32FC4);
cloud.download(cloud_host.ptr<Point>());
cv::Mat colors (cloud_host.size (), CV_8UC3);
for(int i = 0; i < (int)cloud_host.total(); ++i)
{
if(cloud_host.at<Point>(i).w < 0)
colors.at<cv::Vec3b>(i) = cv::Vec3b(0, 255, 0);
else
colors.at<cv::Vec3b>(i) = cv::Vec3b(255, 0, 0);
}
float voxelsize = 3.0 / 512.0;
for(int i = 0; i < (int)cloud_host.total(); ++i)
{
cloud_host.at<Point>(i).x = cloud_host.at<Point>(i).x * voxelsize;
cloud_host.at<Point>(i).y = cloud_host.at<Point>(i).y * voxelsize;
cloud_host.at<Point>(i).z = cloud_host.at<Point>(i).z * voxelsize;
}
cv::viz::WCloud cloud_widget = cv::viz::WCloud(cloud_host, colors);
cloud_widget.setRenderingProperty(cv::viz::POINT_SIZE, 2.0);
cv::viz::writeCloud("cloud.ply", cloud_host, colors);*/
//viz.showWidget("cloud", cv::viz::WPaintedCloud(cloud_host));
kinfu.performLastScan();
}