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

Added OpenNI2 and Freenect calibration #115

Browse Source
This commit is contained in:
matlabbe
2016-09-13 18:26:18 -04:00
parent e5977d6157
commit acb700bd27
14 changed files with 1248 additions and 753 deletions
Download Patch File Download Diff File Expand all files Collapse all files

166
corelib/src/StereoCameraModel.cpp
View File

@@ -42,13 +42,15 @@ StereoCameraModel::StereoCameraModel(
const cv::Mat & K2, const cv::Mat & D2, const cv::Mat & R2, const cv::Mat & P2,
const cv::Mat & R, const cv::Mat & T, const cv::Mat & E, const cv::Mat & F,
const Transform & localTransform) :
left_(name+"_left", imageSize1, K1, D1, R1, P1, localTransform),
right_(name+"_right", imageSize2, K2, D2, R2, P2, localTransform),
name_(name),
R_(R),
T_(T),
E_(E),
F_(F)
leftSuffix_("left"),
rightSuffix_("right"),
left_(name+"_"+leftSuffix_, imageSize1, K1, D1, R1, P1, localTransform),
right_(name+"_"+rightSuffix_, imageSize2, K2, D2, R2, P2, localTransform),
name_(name),
R_(R),
T_(T),
E_(E),
F_(F)
{
UASSERT(R_.empty() || (R_.rows == 3 && R_.cols == 3 && R_.type() == CV_64FC1));
UASSERT(T_.empty() || (T_.rows == 3 && T_.cols == 1 && T_.type() == CV_64FC1));
@@ -64,6 +66,8 @@ StereoCameraModel::StereoCameraModel(
const cv::Mat & T,
const cv::Mat & E,
const cv::Mat & F) :
leftSuffix_("left"),
rightSuffix_("right"),
left_(leftCameraModel),
right_(rightCameraModel),
name_(name),
@@ -72,8 +76,8 @@ StereoCameraModel::StereoCameraModel(
E_(E),
F_(F)
{
left_.setName(name+"_left");
right_.setName(name+"_right");
left_.setName(name+"_"+getLeftSuffix());
right_.setName(name+"_"+getRightSuffix());
UASSERT(R_.empty() || (R_.rows == 3 && R_.cols == 3 && R_.type() == CV_64FC1));
UASSERT(T_.empty() || (T_.rows == 3 && T_.cols == 1 && T_.type() == CV_64FC1));
UASSERT(E_.empty() || (E_.rows == 3 && E_.cols == 3 && E_.type() == CV_64FC1));
@@ -99,12 +103,14 @@ StereoCameraModel::StereoCameraModel(
const CameraModel & leftCameraModel,
const CameraModel & rightCameraModel,
const Transform & extrinsics) :
leftSuffix_("left"),
rightSuffix_("right"),
left_(leftCameraModel),
right_(rightCameraModel),
name_(name)
{
left_.setName(name+"_left");
right_.setName(name+"_right");
left_.setName(name+"_"+getLeftSuffix());
right_.setName(name+"_"+getRightSuffix());
if(!extrinsics.isNull())
{
@@ -132,8 +138,10 @@ StereoCameraModel::StereoCameraModel(
double baseline,
const Transform & localTransform,
const cv::Size & imageSize) :
left_(fx, fy, cx, cy, localTransform, 0, imageSize),
right_(fx, fy, cx, cy, localTransform, baseline*-fx, imageSize)
leftSuffix_("left"),
rightSuffix_("right"),
left_(fx, fy, cx, cy, localTransform, 0, imageSize),
right_(fx, fy, cx, cy, localTransform, baseline*-fx, imageSize)
{
}
@@ -147,23 +155,29 @@ StereoCameraModel::StereoCameraModel(
double baseline,
const Transform & localTransform,
const cv::Size & imageSize) :
left_(name+"_left", fx, fy, cx, cy, localTransform, 0, imageSize),
right_(name+"_right", fx, fy, cx, cy, localTransform, baseline*-fx, imageSize),
name_(name)
leftSuffix_("left"),
rightSuffix_("right"),
left_(name+"_"+getLeftSuffix(), fx, fy, cx, cy, localTransform, 0, imageSize),
right_(name+"_"+getRightSuffix(), fx, fy, cx, cy, localTransform, baseline*-fx, imageSize),
name_(name)
{
}
void StereoCameraModel::setName(const std::string & name)
void StereoCameraModel::setName(const std::string & name, const std::string & leftSuffix, const std::string & rightSuffix)
{
name_=name;
left_.setName(name_+"_left");
right_.setName(name_+"_right");
leftSuffix_ = leftSuffix;
rightSuffix_ = rightSuffix;
left_.setName(name_+"_"+getLeftSuffix());
right_.setName(name_+"_"+getRightSuffix());
}
bool StereoCameraModel::load(const std::string & directory, const std::string & cameraName, bool ignoreStereoTransform)
{
name_ = cameraName;
if(left_.load(directory, cameraName+"_left") && right_.load(directory, cameraName+"_right"))
bool leftLoaded = left_.load(directory, cameraName+"_"+getLeftSuffix());
bool rightLoaded = right_.load(directory, cameraName+"_"+getRightSuffix());
if(leftLoaded && rightLoaded)
{
if(ignoreStereoTransform)
{
@@ -277,63 +291,69 @@ bool StereoCameraModel::save(const std::string & directory, bool ignoreStereoTra
{
return true;
}
std::string filePath = directory+"/"+name_+"_pose.yaml";
if(!filePath.empty() && (!R_.empty() && !T_.empty()))
return saveStereoTransform(directory);
}
return false;
}
bool StereoCameraModel::saveStereoTransform(const std::string & directory) const
{
std::string filePath = directory+"/"+name_+"_pose.yaml";
if(!filePath.empty() && (!R_.empty() && !T_.empty()))
{
UINFO("Saving stereo calibration to file \"%s\"", filePath.c_str());
cv::FileStorage fs(filePath, cv::FileStorage::WRITE);
// export in ROS calibration format
if(!name_.empty())
{
UINFO("Saving stereo calibration to file \"%s\"", filePath.c_str());
cv::FileStorage fs(filePath, cv::FileStorage::WRITE);
// export in ROS calibration format
if(!name_.empty())
{
fs << "camera_name" << name_;
}
if(!R_.empty())
{
fs << "rotation_matrix" << "{";
fs << "rows" << R_.rows;
fs << "cols" << R_.cols;
fs << "data" << std::vector<double>((double*)R_.data, ((double*)R_.data)+(R_.rows*R_.cols));
fs << "}";
}
if(!T_.empty())
{
fs << "translation_matrix" << "{";
fs << "rows" << T_.rows;
fs << "cols" << T_.cols;
fs << "data" << std::vector<double>((double*)T_.data, ((double*)T_.data)+(T_.rows*T_.cols));
fs << "}";
}
if(!E_.empty())
{
fs << "essential_matrix" << "{";
fs << "rows" << E_.rows;
fs << "cols" << E_.cols;
fs << "data" << std::vector<double>((double*)E_.data, ((double*)E_.data)+(E_.rows*E_.cols));
fs << "}";
}
if(!F_.empty())
{
fs << "fundamental_matrix" << "{";
fs << "rows" << F_.rows;
fs << "cols" << F_.cols;
fs << "data" << std::vector<double>((double*)F_.data, ((double*)F_.data)+(F_.rows*F_.cols));
fs << "}";
}
fs.release();
return true;
fs << "camera_name" << name_;
}
else
if(!R_.empty())
{
UERROR("Failed saving stereo extrinsics (they are null).");
fs << "rotation_matrix" << "{";
fs << "rows" << R_.rows;
fs << "cols" << R_.cols;
fs << "data" << std::vector<double>((double*)R_.data, ((double*)R_.data)+(R_.rows*R_.cols));
fs << "}";
}
if(!T_.empty())
{
fs << "translation_matrix" << "{";
fs << "rows" << T_.rows;
fs << "cols" << T_.cols;
fs << "data" << std::vector<double>((double*)T_.data, ((double*)T_.data)+(T_.rows*T_.cols));
fs << "}";
}
if(!E_.empty())
{
fs << "essential_matrix" << "{";
fs << "rows" << E_.rows;
fs << "cols" << E_.cols;
fs << "data" << std::vector<double>((double*)E_.data, ((double*)E_.data)+(E_.rows*E_.cols));
fs << "}";
}
if(!F_.empty())
{
fs << "fundamental_matrix" << "{";
fs << "rows" << F_.rows;
fs << "cols" << F_.cols;
fs << "data" << std::vector<double>((double*)F_.data, ((double*)F_.data)+(F_.rows*F_.cols));
fs << "}";
}
fs.release();
return true;
}
else
{
UERROR("Failed saving stereo extrinsics (they are null).");
}
return false;
}