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++;
}
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;
}
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");
}