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 Stereo and StereoDense base classes (with Stereo->StereoOpticalFlow and StereoDense->StereoBM) to handle easily stereo parameters. Added stereoEval tool to test Stereo/OpticalFlow=false or true. Added StereoBM parameters. Refactoring of the Preferences dialog (tree view order and some titles)

Browse Source
This commit is contained in:
matlabbe
2015-12-11 17:44:15 -05:00
parent 818eda005f
commit 9cc94ce1e6
44 changed files with 6866 additions and 4311 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/CameraModel.cpp
-147
View File
@@ -362,151 +362,4 @@ 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") && right_.load(directory, cameraName+"_right"))
{
if(ignoreStereoTransform)
{
return true;
}
//load rotation, translation
R_ = cv::Mat();
T_ = cv::Mat();
std::string filePath = directory+"/"+cameraName+"_pose.yaml";
if(UFile::exists(filePath))
{
UINFO("Reading stereo calibration file \"%s\"", filePath.c_str());
cv::FileStorage fs(filePath, cv::FileStorage::READ);
name_ = (int)fs["camera_name"];
// import from ROS calibration format
cv::FileNode n = fs["rotation_matrix"];
int rows = (int)n["rows"];
int cols = (int)n["cols"];
std::vector<double> data;
n["data"] >> data;
UASSERT(rows*cols == (int)data.size());
UASSERT(rows == 3 && cols == 3);
R_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
n = fs["translation_matrix"];
rows = (int)n["rows"];
cols = (int)n["cols"];
data.clear();
n["data"] >> data;
UASSERT(rows*cols == (int)data.size());
UASSERT(rows == 3 && cols == 1);
T_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
n = fs["essential_matrix"];
rows = (int)n["rows"];
cols = (int)n["cols"];
data.clear();
n["data"] >> data;
UASSERT(rows*cols == (int)data.size());
UASSERT(rows == 3 && cols == 3);
E_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
n = fs["fundamental_matrix"];
rows = (int)n["rows"];
cols = (int)n["cols"];
data.clear();
n["data"] >> data;
UASSERT(rows*cols == (int)data.size());
UASSERT(rows == 3 && cols == 3);
F_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
fs.release();
return true;
}
else
{
UWARN("Could not load stereo calibration file \"%s\".", filePath.c_str());
}
}
return false;
}
bool StereoCameraModel::save(const std::string & directory, bool ignoreStereoTransform) const
{
if(left_.save(directory) && right_.save(directory))
{
if(ignoreStereoTransform)
{
return true;
}
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());
cv::FileStorage fs(filePath, cv::FileStorage::WRITE);
// export in ROS calibration format
fs << "camera_name" << name_;
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 << "}";
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 << "}";
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 << "}";
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;
}
}
return false;
}
void StereoCameraModel::scale(double scale)
{
left_.scale(scale);
right_.scale(scale);
}
Transform StereoCameraModel::stereoTransform() const
{
if(!R_.empty() && !T_.empty())
{
return Transform(
R_.at<double>(0,0), R_.at<double>(0,1), R_.at<double>(0,2), T_.at<double>(0),
R_.at<double>(1,0), R_.at<double>(1,1), R_.at<double>(1,2), T_.at<double>(1),
R_.at<double>(2,0), R_.at<double>(2,1), R_.at<double>(2,2), T_.at<double>(2));
}
return Transform();
}
} /* namespace rtabmap */