generic-library/opencv
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

reading/writing trajectories

Browse Source
This commit is contained in:
Anatoly Baksheev 2013-12-08 19:31:23 +04:00
parent e048df51ce
commit e990d5b999
2 changed files with 98 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
modules/viz/include/opencv2/viz.hpp
View File

@ -91,6 +91,22 @@ namespace cv
template<typename _Tp> inline bool isNan(const Point3_<_Tp>& p)
{ return isNan(p.x) || isNan(p.y) || isNan(p.z); }
///////////////////////////////////////////////////////////////////////////////////////////////
/// Read/write poses and trajectories
CV_EXPORTS bool readPose(const String& file, Affine3f& pose, const String& tag = "pose");
CV_EXPORTS bool readPose(const String& file, Affine3d& pose, const String& tag = "pose");
CV_EXPORTS void writePose(const String& file, const Affine3f& pose, const String& tag = "pose");
CV_EXPORTS void writePose(const String& file, const Affine3d& pose, const String& tag = "pose");
CV_EXPORTS void writeTrajectory(const std::vector<Affine3f>& traj, const String& files_format = "pose%05d.xml", int start = 0, const String& tag = "pose");
CV_EXPORTS void writeTrajectory(const std::vector<Affine3d>& traj, const String& files_format = "pose%05d.xml", int start = 0, const String& tag = "pose");
CV_EXPORTS void readTrajectory(std::vector<Affine3f>& traj, const String& files_format = "pose%05d.xml", int start = 0, int end = INT_MAX, const String& tag = "pose");
CV_EXPORTS void readTrajectory(std::vector<Affine3d>& traj, const String& files_format = "pose%05d.xml", int start = 0, int end = INT_MAX, const String& tag = "pose");
} /* namespace viz */
} /* namespace cv */

82
modules/viz/src/viz.cpp → modules/viz/src/vizcore.cpp
View File

@ -160,3 +160,85 @@ cv::String cv::viz::VizStorage::generateWindowName(const String &window_name)
cv::viz::Viz3d cv::viz::get(const String &window_name) { return Viz3d (window_name); }
void cv::viz::unregisterAllWindows() { VizStorage::unregisterAll(); }
///////////////////////////////////////////////////////////////////////////////////////////////
/// Read/write poses and trajectories
namespace cv { namespace viz { namespace impl
{
template <typename _Tp>
bool readPose(const String& file, Affine3<_Tp>& pose, const String& tag)
{
FileStorage fs(file, FileStorage::READ);
if (!fs.isOpened())
return false;
Mat hdr(pose.matrix, false);
fs[tag] >> hdr;
return !hdr.empty() && hdr.depth() == DataDepth<_Tp>::value;
}
template <typename _Tp>
void writePose(const String& file, const Affine3<_Tp>& pose, const String& tag)
{
FileStorage fs(file, FileStorage::WRITE);
fs << tag << Mat(pose.matrix, false);
}
template <typename _Tp>
void readTrajectory(std::vector<Affine3<_Tp> >& traj, const String& files_format, int start, int end, const String& tag)
{
start = max(0, std::min(start, end));
end = std::max(start, end);
std::vector< Affine3<_Tp> > temp;
for(int i = start; i < end; ++i)
{
Affine3<_Tp> affine;
bool ok = readPose(cv::format(files_format.c_str(), i),affine, tag);
if (!ok)
break;
temp.push_back(affine);
}
traj.swap(temp);
}
template <typename _Tp>
void writeTrajectory(const std::vector<Affine3<_Tp> >& traj, const String& files_format, int start, const String& tag)
{
for(size_t i = 0, index = max(0, start); i < traj.size(); ++i, ++index)
writePose(cv::format(files_format.c_str(), index), traj[i], tag);
}
}}}
bool cv::viz::readPose(const String& file, Affine3f& pose, const String& tag) { return impl::readPose(file, pose, tag); }
bool cv::viz::readPose(const String& file, Affine3d& pose, const String& tag) { return impl::readPose(file, pose, tag); }
void cv::viz::writePose(const String& file, const Affine3f& pose, const String& tag) { impl::writePose(file, pose, tag); }
void cv::viz::writePose(const String& file, const Affine3d& pose, const String& tag) { impl::writePose(file, pose, tag); }
void cv::viz::readTrajectory(std::vector<Affine3f>& traj, const String& files_format, int start, int end, const String& tag)
{ impl::readTrajectory(traj, files_format, start, end, tag); }
void cv::viz::readTrajectory(std::vector<Affine3d>& traj, const String& files_format, int start, int end, const String& tag)
{ impl::readTrajectory(traj, files_format, start, end, tag); }
void cv::viz::writeTrajectory(const std::vector<Affine3f>& traj, const String& files_format, int start, const String& tag)
{ impl::writeTrajectory(traj, files_format, start, tag); }
void cv::viz::writeTrajectory(const std::vector<Affine3d>& traj, const String& files_format, int start, const String& tag)
{ impl::writeTrajectory(traj, files_format, start, tag); }