Exemple #1
0
     void
     ScreenshotManager::saveImage(const Eigen::Affine3f &camPose, PtrStepSz<const PixelRGB> rgb24)
     {

       PCL_INFO ("[o] [o] [o] [o] Saving screenshot [o] [o] [o] [o]\n");

       std::string file_extension_image = ".png";
       std::string file_extension_pose = ".txt";
       std::string filename_image = "KinFuSnapshots/";
       std::string filename_pose = "KinFuSnapshots/";

       // Get Pose
       Eigen::Matrix<float, 3, 3, Eigen::RowMajor> erreMats = camPose.linear ();
		   Eigen::Vector3f teVecs = camPose.translation ();

		   // Create filenames
		   filename_pose = filename_pose + boost::lexical_cast<std::string> (screenshot_counter) + file_extension_pose;
		   filename_image = filename_image + boost::lexical_cast<std::string> (screenshot_counter) + file_extension_image;

		   // Write files
		   writePose (filename_pose, teVecs, erreMats);
        
       // Save Image
       pcl::io::saveRgbPNGFile (filename_image, (unsigned char*)rgb24.data, 640,480);
        
       screenshot_counter++;
     }
Exemple #2
0
int Pose3D::writePose(const std::string& FileName)
{
    FILE* f = fopen(FileName.c_str(), "wb");
    
    if (!f)
        return -1;
        
    int status = writePose(f);
    
    fclose(f);
    return status;
}
Exemple #3
0
void cv::viz::writeTrajectory(InputArray _traj, const String& files_format, int start, const String& tag)
{
    if (_traj.kind() == _InputArray::STD_VECTOR_MAT)
    {
#if CV_MAJOR_VERSION < 3
        std::vector<Mat>& v = *(std::vector<Mat>*)_traj.obj;
#else
        std::vector<Mat>& v = *(std::vector<Mat>*)_traj.getObj();
#endif

        for(size_t i = 0, index = max(0, start); i < v.size(); ++i, ++index)
        {
            Affine3d affine;
            Mat pose = v[i];
            CV_Assert(pose.type() == CV_32FC(16) || pose.type() == CV_64FC(16));
            pose.copyTo(affine.matrix);
            writePose(cv::format(files_format.c_str(), index), affine, tag);
        }
        return;
    }

    if (_traj.kind() == _InputArray::STD_VECTOR || _traj.kind() == _InputArray::MAT)
    {
        CV_Assert(_traj.type() == CV_32FC(16) || _traj.type() == CV_64FC(16));

        Mat traj = _traj.getMat();

        if (traj.depth() == CV_32F)
            for(size_t i = 0, index = max(0, start); i < traj.total(); ++i, ++index)
                writePose(cv::format(files_format.c_str(), index), traj.at<Affine3f>((int)i), tag);

        if (traj.depth() == CV_64F)
            for(size_t i = 0, index = max(0, start); i < traj.total(); ++i, ++index)
                writePose(cv::format(files_format.c_str(), index), traj.at<Affine3d>((int)i), tag);
        return;
    }

    CV_Error(Error::StsError, "Unsupported array kind");
}