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 stereo option for the CalibrationDialog class

Browse Source
This commit is contained in:
Mathieu Labbe
2015-04-05 16:42:41 -04:00
parent 6a0e48a6fa
commit fee32c4e25
14 changed files with 1075 additions and 318 deletions
Download Patch File Download Diff File Expand all files Collapse all files

86
corelib/src/CameraModel.cpp
View File

@@ -34,14 +34,32 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap {
CameraModel::CameraModel() :
width_(0),
height_(0),
P_(cv::Mat::zeros(3, 4, CV_64FC1))
{
}
bool CameraModel::load(const std::string & directory, const std::string & cameraName)
CameraModel::CameraModel(const std::string & cameraName, const cv::Size & imageSize, const cv::Mat & K, const cv::Mat & D, const cv::Mat & R, const cv::Mat & P) :
name_(cameraName),
imageSize_(imageSize),
K_(K),
D_(D),
R_(R),
P_(P)
{
UASSERT(!name_.empty());
UASSERT(imageSize_.width > 0 && imageSize_.height > 0);
UASSERT(K_.rows == 3 && K_.cols == 3);
UASSERT(D_.rows == 1 && (D_.cols == 4 || D_.cols == 5 || D_.cols == 8));
UASSERT(R_.rows == 3 && R_.cols == 3);
UASSERT(P_.rows == 3 && P_.cols == 4);
// init rectification map
UINFO("Initialize rectify map");
cv::initUndistortRectifyMap(K_, D_, R_, P_, imageSize_, CV_16SC2, rectificationMap1_, rectificationMap2_);
}
bool CameraModel::load(const std::string & filePath)
{
K_ = cv::Mat();
D_ = cv::Mat();
@@ -50,14 +68,17 @@ bool CameraModel::load(const std::string & directory, const std::string & camera
rectificationMap1_ = cv::Mat();
rectificationMap2_ = cv::Mat();
std::string path = directory + UDirectory::separator() + cameraName + ".yaml";
if(UFile::exists(path))
if(UFile::exists(filePath))
{
UINFO("Reading calibration file \"%s\"", path.c_str());
cv::FileStorage fs(path, cv::FileStorage::READ);
UINFO("Reading calibration file \"%s\"", filePath.c_str());
cv::FileStorage fs(filePath, cv::FileStorage::READ);
width_ = (int)fs["image_width"];
height_ = (int)fs["image_height"];
name_ = (int)fs["camera_name"];
imageSize_.width = (int)fs["image_width"];
imageSize_.height = (int)fs["image_height"];
UASSERT(!name_.empty());
UASSERT(imageSize_.width > 0);
UASSERT(imageSize_.height > 0);
// import from ROS calibration format
cv::FileNode n = fs["camera_matrix"];
@@ -99,17 +120,56 @@ bool CameraModel::load(const std::string & directory, const std::string & camera
fs.release();
// init rectification map
cv::initUndistortRectifyMap(K_, D_, R_, P_, cv::Size(width_, height_),
CV_16SC2, rectificationMap1_, rectificationMap2_);
UINFO("Initialize rectify map");
cv::initUndistortRectifyMap(K_, D_, R_, P_, imageSize_, CV_16SC2, rectificationMap1_, rectificationMap2_);
return true;
}
return false;
}
void CameraModel::save(const std::string & directory, const std::string & cameraName)
bool CameraModel::save(const std::string & filePath)
{
UFATAL("not implemented");
if(!filePath.empty() && !name_.empty() && !K_.empty() && !D_.empty(), !R_.empty(), !P_.empty())
{
UINFO("Saving calibration to file \"%s\"", filePath.c_str());
cv::FileStorage fs(filePath, cv::FileStorage::WRITE);
// export in ROS calibration format
fs << "camera_name" << name_;
fs << "image_width" << imageSize_.width;
fs << "image_height" << imageSize_.height;
fs << "camera_matrix" << "{";
fs << "rows" << K_.rows;
fs << "cols" << K_.cols;
fs << "data" << std::vector<double>((double*)K_.data, ((double*)K_.data)+(K_.rows*K_.cols));
fs << "}";
fs << "distortion_coefficients" << "{";
fs << "rows" << D_.rows;
fs << "cols" << D_.cols;
fs << "data" << std::vector<double>((double*)D_.data, ((double*)D_.data)+(D_.rows*D_.cols));
fs << "}";
fs << "rectification_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 << "projection_matrix" << "{";
fs << "rows" << P_.rows;
fs << "cols" << P_.cols;
fs << "data" << std::vector<double>((double*)P_.data, ((double*)P_.data)+(P_.rows*P_.cols));
fs << "}";
fs.release();
return true;
}
return false;
}
cv::Mat CameraModel::rectifyImage(const cv::Mat & raw) const

100
corelib/src/CameraRGBD.cpp
View File

@@ -225,6 +225,15 @@ bool CameraOpenni::init()
return true;
}
std::string CameraOpenni::getSerial() const
{
if(interface_)
{
return interface_->getName();
}
return "";
}
void CameraOpenni::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy)
{
if(interface_ && interface_->isRunning())
@@ -637,6 +646,15 @@ bool CameraOpenNI2::init()
#endif
}
std::string CameraOpenNI2::getSerial() const
{
if(_device)
{
return _device->getDeviceInfo().getName();
}
return "";
}
void CameraOpenNI2::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy)
{
#ifdef WITH_OPENNI2
@@ -698,13 +716,41 @@ class FreenectDevice : public UThread {
if(device_ && freenect_close_device(device_) < 0){} //FN_WARNING("Device did not shutdown in a clean fashion");
}
const std::string & getSerial() const {return serial_;}
bool init()
{
if(device_)
{
this->join(true);
freenect_close_device(device_);
device_ = 0;
}
serial_.clear();
std::vector<std::string> deviceSerials;
freenect_device_attributes* attr_list;
freenect_device_attributes* item;
freenect_list_device_attributes(ctx_, &attr_list);
for (item = attr_list; item != NULL; item = item->next) {
deviceSerials.push_back(std::string(item->camera_serial));
}
freenect_free_device_attributes(attr_list);
if(freenect_open_device(ctx_, &device_, index_) < 0)
{
UERROR("FreenectDevice: Cannot open Kinect");
return false;
}
if(index_ >= 0 && index_ < (int)deviceSerials.size())
{
serial_ = deviceSerials[index_];
}
else
{
UERROR("Could not get serial for index %d", index_);
}
freenect_set_user(device_, this);
freenect_set_video_mode(device_, freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_VIDEO_RGB));
freenect_set_depth_mode(device_, freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_DEPTH_REGISTERED));
@@ -833,6 +879,7 @@ private:
private:
int index_;
std::string serial_;
freenect_context * ctx_;
freenect_device * device_;
cv::Mat depthBuffer_;
@@ -922,6 +969,17 @@ bool CameraFreenect::init()
return false;
}
std::string CameraFreenect::getSerial() const
{
#ifdef WITH_FREENECT
if(freenectDevice_)
{
return freenectDevice_->getSerial();
}
#endif
return "";
}
void CameraFreenect::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy)
{
#ifdef WITH_FREENECT
@@ -1045,6 +1103,17 @@ bool CameraFreenect2::init()
return false;
}
std::string CameraFreenect2::getSerial() const
{
#ifdef WITH_FREENECT2
if(dev_)
{
return dev_->getSerialNumber();
}
#endif
return "";
}
void CameraFreenect2::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy)
{
#ifdef WITH_FREENECT2
@@ -1353,7 +1422,7 @@ public:
//DC1394_COLOR_CODING_RAW16:
//DC1394_COLOR_FILTER_BGGR
cv::cvtColor(cv::Mat(frame->size[1], frame->size[0], CV_8UC1, capture_buffer), left, CV_BayerRG2BGR);
cv::cvtColor(cv::Mat(frame->size[1], frame->size[0], CV_8UC1, capture_buffer+image.total()), right, CV_BayerRG2BGR);
cv::cvtColor(cv::Mat(frame->size[1], frame->size[0], CV_8UC1, capture_buffer+image.total()), right, CV_BayerRG2GRAY);
dc1394_capture_enqueue(camera_, frame);
@@ -1380,8 +1449,19 @@ bool CameraDC1394::available()
#endif
}
CameraDC1394::CameraDC1394(float imageRate, const Transform & localTransform, float fx, float fy, float cx, float cy) :
CameraRGBD(imageRate, localTransform, fx, fy, cx, cy),
lookForCalibration_(false),
device_(0)
{
#ifdef WITH_DC1394
device_ = new DC1394Device();
#endif
}
CameraDC1394::CameraDC1394(const std::string & calibrationFolder, float imageRate, const Transform & localTransform, float fx, float fy, float cx, float cy) :
CameraRGBD(imageRate, localTransform, fx, fy, cx, cy),
lookForCalibration_(true),
calibrationFolder_(calibrationFolder),
device_(0)
{
@@ -1409,9 +1489,12 @@ bool CameraDC1394::init()
if(ok)
{
// look for calibration files
if(!stereoModel_.load(calibrationFolder_, device_->guid()))
if(lookForCalibration_)
{
UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!", device_->guid().c_str(), calibrationFolder_.c_str());
if(!stereoModel_.load(calibrationFolder_, device_->guid()))
{
UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!", device_->guid().c_str(), calibrationFolder_.c_str());
}
}
}
return ok;
@@ -1422,6 +1505,17 @@ bool CameraDC1394::init()
return false;
}
std::string CameraDC1394::getSerial() const
{
#ifdef WITH_DC1394
if(device_)
{
return device_->guid();
}
#endif
return "";
}
void CameraDC1394::captureImage(cv::Mat & left, cv::Mat & right, float & fx, float & baseline, float & cx, float & cy)
{
#ifdef WITH_DC1394

3
corelib/src/CameraThread.cpp
View File

@@ -125,7 +125,8 @@ void CameraThread::mainLoop()
{
if(_cameraRGBD)
{
this->post(new CameraEvent(rgb, depth, fx, fy, cx, cy, _cameraRGBD->getLocalTransform(), ++_seq, UTimer::now()));
SensorData data(rgb, depth, fx, fy, cx, cy, _cameraRGBD->getLocalTransform(), Transform(), 1, 1, ++_seq, UTimer::now());
this->post(new CameraEvent(data, _cameraRGBD->getSerial()));
}
else
{

9
corelib/src/Memory.cpp
View File

@@ -3225,6 +3225,15 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats)
UASSERT(data.depth().empty() || ((data.depth().type() == CV_16UC1 || data.depth().type() == CV_32FC1) && data.depth().rows == data.image().rows && data.depth().cols == data.image().cols));
UASSERT(data.rightImage().empty() || (data.rightImage().type() == CV_8UC1 && data.rightImage().rows == data.image().rows && data.rightImage().cols == data.image().cols));
UASSERT(data.laserScan().empty() || data.laserScan().type() == CV_32FC2);
if(!data.depthOrRightImage().empty() && (data.fx() <= 0 || data.fyOrBaseline() <= 0))
{
UERROR("Rectified images required! Calibrate your camera. (fx=%f, fy/baseline=%f, cx=%f, cy=%f)",
data.fx(), data.fyOrBaseline(), data.cx(), data.cy());
return 0;
}
UASSERT(data.depthOrRightImage().empty() || data.fx() > 0);
UASSERT(data.depthOrRightImage().empty() || data.fyOrBaseline() > 0);
UASSERT(_feature2D != 0);
PreUpdateThread preUpdateThread(_vwd);

10
corelib/src/Odometry.cpp
View File

@@ -115,6 +115,16 @@ Transform Odometry::process(const SensorData & data, OdometryInfo * info)
_pose.setIdentity(); // initialized
}
UASSERT(!data.image().empty());
UASSERT(!data.depthOrRightImage().empty());
if(data.fx() <= 0 || data.fyOrBaseline() <= 0)
{
UERROR("Rectified images required! Calibrate your camera. (fx=%f, fy/baseline=%f, cx=%f, cy=%f)",
data.fx(), data.fyOrBaseline(), data.cx(), data.cy());
return Transform();
}
UTimer time;
Transform t = this->computeTransform(data, info);

4
corelib/src/SensorData.cpp
View File

@@ -102,7 +102,7 @@ SensorData::SensorData(const cv::Mat & image,
UASSERT(depthOrRightImage.type() == CV_32FC1 || // Depth in meter
depthOrRightImage.type() == CV_16UC1 || // Depth in millimetre
depthOrRightImage.type() == CV_8U); // Right stereo image
UASSERT(!depthOrRightImage.empty() && _fx>0.0f && _fyOrBaseline>0.0f && _cx>=0.0f && _cy>=0.0f);
UASSERT(!depthOrRightImage.empty());
UASSERT(!_localTransform.isNull());
UASSERT_MSG(uIsFinite(_poseRotVariance) && _poseRotVariance>0 && uIsFinite(_poseTransVariance) && _poseTransVariance>0, "Rotational and transitional variances should not be null! (set to 1 if unknown)");
}
@@ -143,7 +143,7 @@ SensorData::SensorData(const cv::Mat & laserScan,
UASSERT(depthOrRightImage.type() == CV_32FC1 || // Depth in meter
depthOrRightImage.type() == CV_16UC1 || // Depth in millimetre
depthOrRightImage.type() == CV_8U); // Right stereo image
UASSERT(!depthOrRightImage.empty() && _fx>0.0f && _fyOrBaseline>0.0f && _cx>=0.0f && _cy>=0.0f);
UASSERT(!depthOrRightImage.empty());
UASSERT(!_localTransform.isNull());
UASSERT_MSG(uIsFinite(_poseRotVariance) && _poseRotVariance>0 && uIsFinite(_poseTransVariance) && _poseTransVariance>0, "Rotational and transitional variances should not be null! (set to 1 if unknown)");
}