i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-08 12:30:20 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added CameraRGBDImages class (read RGB-D images from a folder)

Browse Source
This commit is contained in:
matlabbe
2015-07-30 14:17:29 -04:00
parent 38807bf12e
commit 2877a14360
24 changed files with 1315 additions and 237 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/CameraModel.cpp
+55 -11
View File
@@ -97,7 +97,38 @@ CameraModel::CameraModel(
K_.at<double>(1,2) = cy;
}
bool CameraModel::load(const std::string & filePath)
CameraModel::CameraModel(
const std::string & name,
double fx,
double fy,
double cx,
double cy,
const Transform & localTransform,
double Tx) :
name_(name),
K_(cv::Mat::eye(3, 3, CV_64FC1)),
D_(cv::Mat::zeros(1, 5, CV_64FC1)),
R_(cv::Mat::eye(3, 3, CV_64FC1)),
P_(cv::Mat::eye(3, 4, CV_64FC1)),
localTransform_(localTransform)
{
UASSERT_MSG(fx >= 0.0, uFormat("fx=%f", fx).c_str());
UASSERT_MSG(fy >= 0.0, uFormat("fy=%f", fy).c_str());
UASSERT_MSG(cx >= 0.0, uFormat("cx=%f", cx).c_str());
UASSERT_MSG(cy >= 0.0, uFormat("cy=%f", cy).c_str());
P_.at<double>(0,0) = fx;
P_.at<double>(1,1) = fy;
P_.at<double>(0,2) = cx;
P_.at<double>(1,2) = cy;
P_.at<double>(0,3) = Tx;
K_.at<double>(0,0) = fx;
K_.at<double>(1,1) = fy;
K_.at<double>(0,2) = cx;
K_.at<double>(1,2) = cy;
}
bool CameraModel::load(const std::string & directory, const std::string & cameraName)
{
K_ = cv::Mat();
D_ = cv::Mat();
@@ -106,6 +137,7 @@ bool CameraModel::load(const std::string & filePath)
mapX_ = cv::Mat();
mapY_ = cv::Mat();
std::string filePath = directory+"/"+cameraName+".yaml";
if(UFile::exists(filePath))
{
UINFO("Reading calibration file \"%s\"", filePath.c_str());
@@ -115,8 +147,8 @@ bool CameraModel::load(const std::string & filePath)
imageSize_.width = (int)fs["image_width"];
imageSize_.height = (int)fs["image_height"];
UASSERT(!name_.empty());
UASSERT(imageSize_.width > 0);
UASSERT(imageSize_.height > 0);
//UASSERT(imageSize_.width > 0);
//UASSERT(imageSize_.height > 0);
// import from ROS calibration format
cv::FileNode n = fs["camera_matrix"];
@@ -157,9 +189,12 @@ bool CameraModel::load(const std::string & filePath)
fs.release();
// init rectification map
UINFO("Initialize rectify map");
cv::initUndistortRectifyMap(K_, D_, R_, P_, imageSize_, CV_32FC1, mapX_, mapY_);
if(imageSize_.height > 0 && imageSize_.width > 0)
{
// init rectification map
UINFO("Initialize rectify map");
cv::initUndistortRectifyMap(K_, D_, R_, P_, imageSize_, CV_32FC1, mapX_, mapY_);
}
return true;
}
@@ -170,8 +205,9 @@ bool CameraModel::load(const std::string & filePath)
return false;
}
bool CameraModel::save(const std::string & filePath) const
bool CameraModel::save(const std::string & directory) const
{
std::string filePath = directory+"/"+name_+".yaml";
if(!filePath.empty() && !name_.empty() && !K_.empty() && !D_.empty() && !R_.empty() && !P_.empty())
{
UINFO("Saving calibration to file \"%s\"", filePath.c_str());
@@ -240,6 +276,7 @@ cv::Mat CameraModel::rectifyImage(const cv::Mat & raw, int interpolation) const
}
else
{
UERROR("Cannot rectify image because the rectify map is not initialized.");
return raw.clone();
}
}
@@ -299,10 +336,17 @@ cv::Mat CameraModel::rectifyDepth(const cv::Mat & raw) const
//
//StereoCameraModel
//
void StereoCameraModel::setName(const std::string & name)
{
name_=name;
left_.setName(name_+"_left");
right_.setName(name_+"_right");
}
bool StereoCameraModel::load(const std::string & directory, const std::string & cameraName, bool ignoreStereoTransform)
{
name_ = cameraName;
if(left_.load(directory+"/"+cameraName+"_left.yaml") && right_.load(directory+"/"+cameraName+"_right.yaml"))
if(left_.load(directory, cameraName+"_left") && right_.load(directory, cameraName+"_right"))
{
if(ignoreStereoTransform)
{
@@ -368,15 +412,15 @@ bool StereoCameraModel::load(const std::string & directory, const std::string &
}
return false;
}
bool StereoCameraModel::save(const std::string & directory, const std::string & cameraName, bool ignoreStereoTransform) const
bool StereoCameraModel::save(const std::string & directory, bool ignoreStereoTransform) const
{
if(left_.save(directory+"/"+cameraName+"_left.yaml") && right_.save(directory+"/"+cameraName+"_right.yaml"))
if(left_.save(directory) && right_.save(directory))
{
if(ignoreStereoTransform)
{
return true;
}
std::string filePath = directory+"/"+cameraName+"_pose.yaml";
std::string filePath = directory+"/"+name_+"_pose.yaml";
if(!filePath.empty() && !name_.empty() && !R_.empty() && !T_.empty())
{
UINFO("Saving stereo calibration to file \"%s\"", filePath.c_str());