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

Added RGB/IR calibration

Browse Source
This commit is contained in:
matlabbe
2015-04-08 12:23:10 -04:00
parent e965275fe6
commit 8ab867b802
10 changed files with 341 additions and 64 deletions
Download Patch File Download Diff File Expand all files Collapse all files

111
corelib/src/CameraModel.cpp
View File

@@ -186,4 +186,115 @@ cv::Mat CameraModel::rectifyImage(const cv::Mat & raw) const
}
}
//
//StereoCameraModel
//
bool StereoCameraModel::load(const std::string & directory, const std::string & cameraName)
{
name_ = cameraName;
if(left_.load(directory+"/"+cameraName+"_left.yaml") && right_.load(directory+"/"+cameraName+"_right.yaml"))
{
//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;
}
}
return false;
}
bool StereoCameraModel::save(const std::string & directory, const std::string & cameraName)
{
if(left_.save(directory+"/"+cameraName+"_left.yaml") && right_.save(directory+"/"+cameraName+"_right.yaml"))
{
std::string filePath = directory+"/"+cameraName+"_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 << "camera_name" << name_;
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 << "camera_name" << name_;
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;
}
} /* namespace rtabmap */

60
corelib/src/CameraRGBD.cpp
View File

@@ -1038,17 +1038,27 @@ bool CameraFreenect2::available()
#endif
}
CameraFreenect2::CameraFreenect2(int deviceId, float imageRate, const Transform & localTransform) :
CameraFreenect2::CameraFreenect2(int deviceId, Type type, float imageRate, const Transform & localTransform) :
CameraRGBD(imageRate, localTransform),
deviceId_(deviceId),
type_(type),
freenect2_(0),
dev_(0),
listener_(0)
{
#ifdef WITH_FREENECT2
freenect2_ = new libfreenect2::Freenect2();
//listener_ = new libfreenect2::SyncMultiFrameListener(libfreenect2::Frame::Color | libfreenect2::Frame::Ir | libfreenect2::Frame::Depth);
listener_ = new libfreenect2::SyncMultiFrameListener(libfreenect2::Frame::Color | libfreenect2::Frame::Depth);
switch(type_)
{
case kTypeRGBIR:
listener_ = new libfreenect2::SyncMultiFrameListener(libfreenect2::Frame::Color | libfreenect2::Frame::Ir);
break;
case kTypeRGBDepthSD:
case kTypeRGBDepthHD:
default:
listener_ = new libfreenect2::SyncMultiFrameListener(libfreenect2::Frame::Color | libfreenect2::Frame::Depth);
break;
}
UWARN("CameraFreenect2: Images are not yet registered!");
#endif
}
@@ -1141,22 +1151,42 @@ void CameraFreenect2::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, f
libfreenect2::FrameMap frames;
if(listener_->waitForNewFrame(frames, 1000))
{
libfreenect2::Frame *rgbFrame = frames[libfreenect2::Frame::Color];
//libfreenect2::Frame *ir = frames[libfreenect2::Frame::Ir];
libfreenect2::Frame *depthFrame = frames[libfreenect2::Frame::Depth];
libfreenect2::Frame *rgbFrame = 0;
libfreenect2::Frame *irFrame = 0;
libfreenect2::Frame *depthFrame = 0;
if(rgbFrame && depthFrame)
switch(type_)
{
cv::flip(cv::Mat(rgbFrame->height, rgbFrame->width, CV_8UC3, rgbFrame->data), rgb, 1);
cv::Mat(depthFrame->height, depthFrame->width, CV_32FC1, depthFrame->data).convertTo(depth, CV_16U, 1);
cv::flip(depth, depth, 1);
libfreenect2::Freenect2Device::ColorCameraParams params = dev_->getColorCameraParams();
fx = params.fx;
fy = params.fy;
cx = params.cx;
cy = params.cy;
case kTypeRGBIR:
rgbFrame = frames[libfreenect2::Frame::Color];
irFrame = frames[libfreenect2::Frame::Ir];
break;
case kTypeRGBDepthSD:
case kTypeRGBDepthHD:
default:
rgbFrame = frames[libfreenect2::Frame::Color];
depthFrame = frames[libfreenect2::Frame::Depth];
break;
}
cv::flip(cv::Mat(rgbFrame->height, rgbFrame->width, CV_8UC3, rgbFrame->data), rgb, 1);
if(irFrame)
{
cv::Mat(irFrame->height, irFrame->width, CV_32FC1, irFrame->data).convertTo(depth, CV_16U, 1);
}
else
{
cv::Mat(depthFrame->height, depthFrame->width, CV_32FC1, depthFrame->data).convertTo(depth, CV_16U, 1);
}
cv::flip(depth, depth, 1);
//libfreenect2::Freenect2Device::ColorCameraParams params = dev_->getColorCameraParams();
libfreenect2::Freenect2Device::IrCameraParams params = dev_->getIrCameraParams();
fx = params.fx;
fy = params.fy;
cx = params.cx;
cy = params.cy;
listener_->release(frames);
}
else