i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 01:20:25 +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

28
corelib/include/rtabmap/core/CameraModel.h
View File

@@ -92,28 +92,24 @@ public:
StereoCameraModel() {}
StereoCameraModel(const std::string & name, const cv::Size & imageSize,
const cv::Mat & K1, const cv::Mat & D1, const cv::Mat & R1, const cv::Mat & P1,
const cv::Mat & K2, const cv::Mat & D2, const cv::Mat & R2, const cv::Mat & P2) :
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) :
left_(name+"_left", imageSize, K1, D1, R1, P1),
right_(name+"_right", imageSize, K2, D2, R2, P2),
name_(name)
name_(name),
R_(R),
T_(T),
E_(E),
F_(F)
{
}
virtual ~StereoCameraModel() {}
bool isValid() const {return left_.isValid() && right_.isValid();}
bool isValid() const {return left_.isValid() && right_.isValid() && !R_.empty() && !T_.empty() && !E_.empty() && !F_.empty();}
const std::string & name() const {return name_;}
bool load(const std::string & directory, const std::string & cameraName)
{
name_ = cameraName;
return left_.load(directory+"/"+cameraName+"_left.yaml") &&
right_.load(directory+"/"+cameraName+"_right.yaml");
}
bool save(const std::string & directory, const std::string & cameraName)
{
return left_.save(directory+"/"+cameraName+"_left.yaml") &&
right_.save(directory+"/"+cameraName+"_right.yaml");
}
bool load(const std::string & directory, const std::string & cameraName);
bool save(const std::string & directory, const std::string & cameraName);
double baseline() const {return -right_.Tx()/right_.fx();}
const CameraModel & left() const {return left_;}
@@ -123,6 +119,10 @@ private:
CameraModel left_;
CameraModel right_;
std::string name_;
cv::Mat R_;
cv::Mat T_;
cv::Mat E_;
cv::Mat F_;
};
} /* namespace rtabmap */

8
corelib/include/rtabmap/core/CameraRGBD.h
View File

@@ -270,9 +270,16 @@ class RTABMAP_EXP CameraFreenect2 :
public:
static bool available();
enum Type{
kTypeRGBDepthSD,
kTypeRGBDepthHD,
kTypeRGBIR
};
public:
// default local transform z in, x right, y down));
CameraFreenect2(int deviceId= 0,
Type type = kTypeRGBDepthSD,
float imageRate=0.0f,
const Transform & localTransform = Transform::getIdentity());
virtual ~CameraFreenect2();
@@ -286,6 +293,7 @@ protected:
private:
int deviceId_;
Type type_;
libfreenect2::Freenect2 * freenect2_;
libfreenect2::Freenect2Device *dev_;
libfreenect2::SyncMultiFrameListener * listener_;

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