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