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_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());
}