i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-10 13:30:20 +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
corelib/include/rtabmap/core/CameraEvent.h
+8 -10
View File
@@ -44,9 +44,10 @@ public:
};
public:
CameraEvent(const cv::Mat & image, int seq=0, double stamp = 0.0) :
CameraEvent(const cv::Mat & image, int seq=0, double stamp = 0.0, const std::string & cameraName = "") :
UEvent(kCodeImage),
data_(image, seq, stamp)
data_(image, seq, stamp),
cameraName_(cameraName)
{
}
@@ -55,26 +56,23 @@ public:
{
}
CameraEvent(const cv::Mat & rgb, const cv::Mat & depth, float fx, float fy, float cx, float cy, const Transform & localTransform, int id, double stamp) :
CameraEvent(const SensorData & data, const std::string & cameraName = "") :
UEvent(kCodeImageDepth),
data_(rgb, depth, fx, fy, cx, cy, localTransform, Transform(), 1.0f, 1.0f, id, stamp)
{
}
CameraEvent(const SensorData & data) :
UEvent(kCodeImageDepth),
data_(data)
data_(data),
cameraName_(cameraName)
{
}
// Image or descriptors
const SensorData & data() const {return data_;}
const std::string & cameraName() const {return cameraName_;}
virtual ~CameraEvent() {}
virtual std::string getClassName() const {return std::string("CameraEvent");}
private:
SensorData data_;
std::string cameraName_;
};
} // namespace rtabmap
corelib/include/rtabmap/core/CameraModel.h
+40 -11
View File
@@ -36,8 +36,23 @@ class CameraModel
{
public:
CameraModel();
// K is the camera intrinsic 3x3 CV_64FC1
// D is the distortion coefficients 1x5 CV_64FC1
// R is the rectification matrix 3x3 CV_64FC1 (computed from stereo or Identity)
// P is the projection matrix 3x4 CV_64FC1 (computed from stereo or equal to [K [0 0 1]'])
CameraModel(const std::string & name, const cv::Size & imageSize, const cv::Mat & K, const cv::Mat & D, const cv::Mat & R, const cv::Mat & P);
virtual ~CameraModel() {}
bool isValid() const {return !K_.empty() &&
!D_.empty() &&
!R_.empty() &&
!P_.empty() &&
imageSize_.height &&
imageSize_.width &&
!name_.empty();}
const std::string & name() const {return name_;}
double fx() const {return P_.at<double>(0,0);}
double fy() const {return P_.at<double>(1,1);}
double cx() const {return P_.at<double>(0,2);}
@@ -49,17 +64,18 @@ public:
const cv::Mat & R() const {return R_;}
const cv::Mat & P() const {return P_;}
int width() const {return width_;}
int height() const {return height_;}
const cv::Size & imageSize() const {return imageSize_;}
int imageWidth() const {return imageSize_.width;}
int imageWeight() const {return imageSize_.height;}
bool load(const std::string & directory, const std::string & cameraName);
void save(const std::string & directory, const std::string & cameraName);
bool load(const std::string & filePath);
bool save(const std::string & filePath);
cv::Mat rectifyImage(const cv::Mat & raw) const;
private:
int width_;
int height_;
std::string name_;
cv::Size imageSize_;
cv::Mat K_;
cv::Mat D_;
cv::Mat R_;
@@ -72,17 +88,29 @@ class StereoCameraModel
{
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) :
left_(name+"_left", imageSize, K1, D1, R1, P1),
right_(name+"_right", imageSize, K2, D2, R2, P2),
name_(name)
{
}
virtual ~StereoCameraModel() {}
bool isValid() const {return left_.isValid() && right_.isValid();}
const std::string & name() const {return name_;}
bool load(const std::string & directory, const std::string & cameraName)
{
return left_.load(directory, cameraName+"_left") &&
right_.load(directory, cameraName+"_right");
name_ = cameraName;
return left_.load(directory+"/"+cameraName+"_left.yaml") &&
right_.load(directory+"/"+cameraName+"_right.yaml");
}
void save(const std::string & directory, const std::string & cameraName)
bool save(const std::string & directory, const std::string & cameraName)
{
left_.save(directory, cameraName+"_left");
right_.save(directory, cameraName+"_right");
return left_.save(directory+"/"+cameraName+"_left.yaml") &&
right_.save(directory+"/"+cameraName+"_right.yaml");
}
double baseline() const {return -right_.Tx()/right_.fx();}
@@ -92,6 +120,7 @@ public:
private:
CameraModel left_;
CameraModel right_;
std::string name_;
};
} /* namespace rtabmap */
corelib/include/rtabmap/core/CameraRGBD.h
+17 -2
View File
@@ -81,6 +81,7 @@ public:
virtual ~CameraRGBD();
void takeImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy);
virtual bool init() = 0;
virtual std::string getSerial() const = 0;
//getters
float getImageRate() const {return _imageRate;}
@@ -154,7 +155,8 @@ public:
const boost::shared_ptr<openni_wrapper::DepthImage>& depth,
float constant);
bool init();
virtual bool init();
virtual std::string getSerial() const;
protected:
virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy);
@@ -191,6 +193,7 @@ public:
virtual ~CameraOpenNICV();
virtual bool init();
virtual std::string getSerial() const {return "";} // unknown with OpenCV
protected:
virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy);
@@ -223,6 +226,7 @@ public:
virtual ~CameraOpenNI2();
virtual bool init();
virtual std::string getSerial() const;
bool setAutoWhiteBalance(bool enabled);
bool setAutoExposure(bool enabled);
@@ -266,6 +270,7 @@ public:
virtual ~CameraFreenect();
bool init();
virtual std::string getSerial() const;
protected:
virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy);
@@ -298,6 +303,7 @@ public:
virtual ~CameraFreenect2();
bool init();
virtual std::string getSerial() const;
protected:
virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy);
@@ -321,7 +327,14 @@ public:
static bool available();
public:
// default local transform z in, x right, y down));
// The first constructor will not check for calibration files, so
// the images returned won't be rectified
CameraDC1394( float imageRate=0.0f,
const Transform & localTransform = Transform::getIdentity(),
float fx = 0.0f,
float fy = 0.0f,
float cx = 0.0f,
float cy = 0.0f);
CameraDC1394(const std::string & calibrationFolder,
float imageRate=0.0f,
const Transform & localTransform = Transform::getIdentity(),
@@ -332,11 +345,13 @@ public:
virtual ~CameraDC1394();
bool init();
virtual std::string getSerial() const;
protected:
virtual void captureImage(cv::Mat & left, cv::Mat & right, float & fx, float & baseline, float & cx, float & cy);
private:
bool lookForCalibration_;
std::string calibrationFolder_;
DC1394Device *device_;
StereoCameraModel stereoModel_;
corelib/src/CameraModel.cpp
+73 -13
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
corelib/src/CameraRGBD.cpp
+97 -3
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
corelib/src/CameraThread.cpp
+2 -1
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
{
corelib/src/Memory.cpp
+9
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);
corelib/src/Odometry.cpp
+10
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);
corelib/src/SensorData.cpp
+2 -2
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)");
}
guilib/include/rtabmap/gui/CalibrationDialog.h
+19 -13
View File
@@ -33,6 +33,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <QDialog>
#include <opencv2/opencv.hpp>
#include <rtabmap/core/CameraModel.h>
#include <rtabmap/utilite/UEventsHandler.h>
class Ui_calibrationDialog;
@@ -44,16 +46,16 @@ class RTABMAPGUI_EXP CalibrationDialog : public QDialog, public UEventsHandler
Q_OBJECT;
public:
CalibrationDialog(QWidget * parent = 0);
CalibrationDialog(bool stereo = false, QWidget * parent = 0);
virtual ~CalibrationDialog();
bool isCalibrated() const {return calibrated_;}
const cv::Mat & cameraMatrix() const {return cameraMatrix_;} // Matrix K
const cv::Mat & distCoeffs() const {return distCoeffs_;} // Matrix D
float fx() const {return cameraMatrix_.at<double>(0,0);} // K(0)
float fy() const {return cameraMatrix_.at<double>(1,1);} // K(4)
float cx() const {return cameraMatrix_.at<double>(0,2);} // K(2)
float cy() const {return cameraMatrix_.at<double>(1,2);} // K(5)
const cv::Mat & cameraMatrix() const {return model_.K();} // Matrix K
const cv::Mat & distCoeffs() const {return model_.D();} // Matrix D
float fx() const {return model_.fx();} // K(0)
float fy() const {return model_.fy();} // K(4)
float cx() const {return model_.cx();} // K(2)
float cy() const {return model_.cy();} // K(5)
public slots:
void setBoardWidth(int width);
@@ -61,7 +63,7 @@ public slots:
void setSquareSize(double size);
private slots:
void processImage(const cv::Mat & image);
void processImages(const cv::Mat & imageLeft, const cv::Mat & imageRight, const QString & cameraName);
void restart();
void calibrate();
void save();
@@ -85,13 +87,17 @@ private:
// parameters
cv::Size boardSize_; // innner squares
float squareSize_; // m
bool stereo_;
std::vector<std::vector<cv::Point2f> > imagePoints_;
std::vector<std::vector<float> > imageParams_;
cv::Size imageSize_;
QString cameraName_;
bool calibrated_;
cv::Mat cameraMatrix_;
cv::Mat distCoeffs_;
bool processingData_;
std::vector<std::vector<std::vector<cv::Point2f> > > imagePoints_;
std::vector<std::vector<std::vector<float> > > imageParams_;
std::vector<cv::Size > imageSize_;
rtabmap::CameraModel model_;
rtabmap::StereoCameraModel stereoModel_;
Ui_calibrationDialog * ui_;
};
guilib/src/CalibrationDialog.cpp
+470 -159
View File
@@ -40,24 +40,49 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/gui/UCv2Qt.h>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UDirectory.h>
#include <rtabmap/utilite/UFile.h>
namespace rtabmap {
#define COUNT_MIN 12
#define COUNT_MIN 40
CalibrationDialog::CalibrationDialog(QWidget * parent) :
CalibrationDialog::CalibrationDialog(bool stereo, QWidget * parent) :
QDialog(parent),
boardSize_(8,6),
squareSize_(0.033),
stereo_(stereo),
calibrated_(false),
cameraMatrix_(cv::Mat::eye(3, 3, CV_64F)),
distCoeffs_(cv::Mat::zeros(8, 1, CV_64F))
processingData_(false)
{
imagePoints_.resize(2);
imageParams_.resize(2);
imageSize_.resize(2);
qRegisterMetaType<cv::Mat>("cv::Mat");
ui_ = new Ui_calibrationDialog();
ui_->setupUi(this);
if(!stereo_)
{
ui_->progressBar_x_2->setVisible(false);
ui_->progressBar_y_2->setVisible(false);
ui_->progressBar_size_2->setVisible(false);
ui_->progressBar_skew_2->setVisible(false);
ui_->image_view_2->setVisible(false);
ui_->label_fx_2->setVisible(false);
ui_->label_fy_2->setVisible(false);
ui_->label_cx_2->setVisible(false);
ui_->label_cy_2->setVisible(false);
ui_->label_baseline->setVisible(false);
ui_->label_baseline_name->setVisible(false);
ui_->lineEdit_K_2->setVisible(false);
ui_->lineEdit_D_2->setVisible(false);
ui_->lineEdit_R_2->setVisible(false);
ui_->lineEdit_P_2->setVisible(false);
}
connect(ui_->pushButton_calibrate, SIGNAL(clicked()), this, SLOT(calibrate()));
connect(ui_->pushButton_restart, SIGNAL(clicked()), this, SLOT(restart()));
connect(ui_->pushButton_save, SIGNAL(clicked()), this, SLOT(save()));
@@ -117,96 +142,168 @@ void CalibrationDialog::closeEvent(QCloseEvent* event)
void CalibrationDialog::handleEvent(UEvent * event)
{
if(event->getClassName().compare("CameraEvent") == 0)
if(!processingData_)
{
rtabmap::CameraEvent * e = (rtabmap::CameraEvent *)event;
if(e->getCode() == rtabmap::CameraEvent::kCodeImage ||
e->getCode() == rtabmap::CameraEvent::kCodeImageDepth)
if(event->getClassName().compare("CameraEvent") == 0)
{
QMetaObject::invokeMethod(this, "processImage", Q_ARG(cv::Mat, e->data().image()));
rtabmap::CameraEvent * e = (rtabmap::CameraEvent *)event;
if(e->getCode() == rtabmap::CameraEvent::kCodeImage ||
e->getCode() == rtabmap::CameraEvent::kCodeImageDepth)
{
processingData_ = true;
QMetaObject::invokeMethod(this, "processImages",
Q_ARG(cv::Mat, e->data().image()),
Q_ARG(cv::Mat, e->data().depthOrRightImage()),
Q_ARG(QString, QString(e->cameraName().c_str())));
}
}
}
}
void CalibrationDialog::processImage(const cv::Mat & image)
void CalibrationDialog::processImages(const cv::Mat & imageLeft, const cv::Mat & imageRight, const QString & cameraName)
{
imageSize_ = image.size();
std::vector<cv::Point2f> pointBuf;
bool found = cv::findChessboardCorners( image, boardSize_, pointBuf,
CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FAST_CHECK | CV_CALIB_CB_NORMALIZE_IMAGE);
if ( found) // If done with success,
processingData_ = true;
if(cameraName_.isEmpty())
{
// improve the found corners' coordinate accuracy for chessboard
cv::Mat viewGray;
cvtColor(image, viewGray, cv::COLOR_BGR2GRAY);
int border = 8; // minimum distance from border
bool reject = false;
for(unsigned int i=0; i<pointBuf.size(); ++i)
cameraName_ = "0000";
if(!cameraName.isEmpty())
{
if(pointBuf[i].x < border || pointBuf[i].x > image.cols-border ||
pointBuf[i].y < border || pointBuf[i].y > image.rows-border)
cameraName_ = cameraName;
}
}
if(ui_->label_serial->text().isEmpty())
{
ui_->label_serial->setText(cameraName_);
}
std::vector<cv::Mat> images(2);
images[0] = imageLeft;
images[1] = imageRight;
imageSize_[0] = imageLeft.size();
imageSize_[1] = imageRight.size();
std::vector<std::vector<cv::Point2f> > pointBuf(2);
std::vector<std::vector<float> > params(2, std::vector<float>(4, 0));
bool boardAccepted[2] = {false};
for(int id=0; id<(stereo_?2:1); ++id)
{
cv::Mat viewGray;
if(!images[id].empty())
{
if(images[id].channels() == 3)
{
reject = false;
break;
cvtColor(images[id], viewGray, cv::COLOR_BGR2GRAY);
}
else
{
viewGray = images[id];
cvtColor(viewGray, images[id], cv::COLOR_GRAY2BGR); // convert to show detected points in color
}
}
else
{
UERROR("Image %d is empty!! Should not!", id);
}
bool found = false;
if(!viewGray.empty())
{
int maxScale = 1;
for( int scale = 1; scale <= maxScale; scale++ )
{
cv::Mat timg;
if( scale == 1 )
timg = viewGray;
else
cv::resize(viewGray, timg, cv::Size(), scale, scale);
found = cv::findChessboardCorners(timg, boardSize_, pointBuf[id],
CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_NORMALIZE_IMAGE);
if(found)
{
if( scale > 1 )
{
cv::Mat cornersMat(pointBuf[id]);
cornersMat *= 1./scale;
}
break;
}
}
}
if(!reject)
if(found) // If done with success,
{
// improve the found corners' coordinate accuracy for chessboard
float minSquareDistance = -1.0f;
for(unsigned int i=0; i<pointBuf.size()-1; ++i)
for(unsigned int i=0; i<pointBuf[id].size()-1; ++i)
{
float d = cv::norm(pointBuf[i] - pointBuf[i+1]);
float d = cv::norm(pointBuf[id][i] - pointBuf[id][i+1]);
if(minSquareDistance == -1.0f || minSquareDistance > d)
{
minSquareDistance = d;
}
}
float radius = minSquareDistance/2.0f +0.5f;
cv::cornerSubPix( viewGray, pointBuf, cv::Size(radius, radius), cv::Size(-1,-1),
cv::cornerSubPix( viewGray, pointBuf[id], cv::Size(radius, radius), cv::Size(-1,-1),
cv::TermCriteria( CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1 ));
// verify if view is different from any previous samples
std::vector<float> params(4, 0);
getParams(pointBuf, boardSize_, imageSize_, params[0], params[1], params[2], params[3]);
boardAccepted[id] = true;
getParams(pointBuf[id], boardSize_, imageSize_[id], params[id][0], params[id][1], params[id][2], params[id][3]);
bool add = true;
for(unsigned int i=0; i<imageParams_.size(); ++i)
// Draw the corners.
cv::drawChessboardCorners(images[id], boardSize_, cv::Mat(pointBuf[id]), found);
}
}
bool readyToCalibrate[2] = {false};
if(boardAccepted[0] && (!stereo_ || boardAccepted[1]))
{
// verify if view is different from any previous samples
bool addSample = true;
for(int id=0; id<(stereo_?2:1); ++id)
{
for(unsigned int i=0; i<imageParams_[id].size(); ++i)
{
if(fabs(params[0] - imageParams_[i].at(0)) < 0.1 && // x
fabs(params[1] - imageParams_[i].at(1)) < 0.1 && // y
fabs(params[2] - imageParams_[i].at(2)) < 0.1 && // size
fabs(params[3] - imageParams_[i].at(3)) < 0.1) // skew
if(fabs(params[id][0] - imageParams_[id][i].at(0)) < 0.1 && // x
fabs(params[id][1] - imageParams_[id][i].at(1)) < 0.1 && // y
fabs(params[id][2] - imageParams_[id][i].at(2)) < 0.05 && // size
fabs(params[id][3] - imageParams_[id][i].at(3)) < 0.1) // skew
{
add = false;
addSample = false;
}
}
if(add)
if(addSample)
{
UINFO("Added board. (x=%f, y=%f, size=%f, skew=%f)", params[0], params[1], params[2], params[3]);
imagePoints_.push_back(pointBuf);
imageParams_.push_back(params);
break;
}
}
if(addSample)
{
for(int id=0; id<(stereo_?2:1); ++id)
{
UINFO("Added board. (x=%f, y=%f, size=%f, skew=%f)", params[id][0], params[id][1], params[id][2], params[id][3]);
imagePoints_[id].push_back(pointBuf[id]);
imageParams_[id].push_back(params[id]);
// update statistics
std::vector<float> xRange(2, imageParams_[0].at(0));
std::vector<float> yRange(2, imageParams_[0].at(1));
std::vector<float> sizeRange(2, imageParams_[0].at(2));
std::vector<float> skewRange(2, imageParams_[0].at(3));
for(unsigned int i=1; i<imageParams_.size(); ++i)
std::vector<float> xRange(2, imageParams_[id][0].at(0));
std::vector<float> yRange(2, imageParams_[id][0].at(1));
std::vector<float> sizeRange(2, imageParams_[id][0].at(2));
std::vector<float> skewRange(2, imageParams_[id][0].at(3));
for(unsigned int i=1; i<imageParams_[id].size(); ++i)
{
xRange[0] = imageParams_[i].at(0) < xRange[0] ? imageParams_[i].at(0) : xRange[0];
xRange[1] = imageParams_[i].at(0) > xRange[1] ? imageParams_[i].at(0) : xRange[1];
yRange[0] = imageParams_[i].at(1) < yRange[0] ? imageParams_[i].at(1) : yRange[0];
yRange[1] = imageParams_[i].at(1) > yRange[1] ? imageParams_[i].at(1) : yRange[1];
sizeRange[0] = imageParams_[i].at(2) < sizeRange[0] ? imageParams_[i].at(2) : sizeRange[0];
sizeRange[1] = imageParams_[i].at(2) > sizeRange[1] ? imageParams_[i].at(2) : sizeRange[1];
skewRange[0] = imageParams_[i].at(3) < skewRange[0] ? imageParams_[i].at(3) : skewRange[0];
skewRange[1] = imageParams_[i].at(3) > skewRange[1] ? imageParams_[i].at(3) : skewRange[1];
xRange[0] = imageParams_[id][i].at(0) < xRange[0] ? imageParams_[id][i].at(0) : xRange[0];
xRange[1] = imageParams_[id][i].at(0) > xRange[1] ? imageParams_[id][i].at(0) : xRange[1];
yRange[0] = imageParams_[id][i].at(1) < yRange[0] ? imageParams_[id][i].at(1) : yRange[0];
yRange[1] = imageParams_[id][i].at(1) > yRange[1] ? imageParams_[id][i].at(1) : yRange[1];
sizeRange[0] = imageParams_[id][i].at(2) < sizeRange[0] ? imageParams_[id][i].at(2) : sizeRange[0];
sizeRange[1] = imageParams_[id][i].at(2) > sizeRange[1] ? imageParams_[id][i].at(2) : sizeRange[1];
skewRange[0] = imageParams_[id][i].at(3) < skewRange[0] ? imageParams_[id][i].at(3) : skewRange[0];
skewRange[1] = imageParams_[id][i].at(3) > skewRange[1] ? imageParams_[id][i].at(3) : skewRange[1];
}
UINFO("Stats:");
UINFO(" Count = %d", (int)imagePoints_.size());
UINFO(" Count = %d", (int)imagePoints_[id].size());
UINFO(" x = [%f -> %f]", xRange[0], xRange[1]);
UINFO(" y = [%f -> %f]", yRange[0], yRange[1]);
UINFO(" size = [%f -> %f]", sizeRange[0], sizeRange[1]);
@@ -217,48 +314,96 @@ void CalibrationDialog::processImage(const cv::Mat & image)
float sizeGood = sizeRange[1] - sizeRange[0];
float skewGood = skewRange[1] - skewRange[0];
if((int)imagePoints_.size() > ui_->progressBar_count->maximum())
if(id == 0)
{
ui_->progressBar_count->setMaximum((int)imagePoints_.size());
ui_->progressBar_x->setValue(xGood*100);
ui_->progressBar_y->setValue(yGood*100);
ui_->progressBar_size->setValue(sizeGood*100);
ui_->progressBar_skew->setValue(skewGood*100);
}
ui_->progressBar_count->setValue((int)imagePoints_.size());
ui_->progressBar_x->setValue(xGood*100);
ui_->progressBar_y->setValue(yGood*100);
ui_->progressBar_size->setValue(sizeGood*100);
ui_->progressBar_skew->setValue(skewGood*100);
if(imagePoints_.size() >= COUNT_MIN && xGood > 0.5 && yGood > 0.5 && sizeGood > 0.4 && skewGood > 0.5)
else
{
ui_->pushButton_calibrate->setEnabled(true);
ui_->progressBar_x_2->setValue(xGood*100);
ui_->progressBar_y_2->setValue(yGood*100);
ui_->progressBar_size_2->setValue(sizeGood*100);
ui_->progressBar_skew_2->setValue(skewGood*100);
}
if(imagePoints_[id].size() >= COUNT_MIN && xGood > 0.5 && yGood > 0.5 && sizeGood > 0.4 && skewGood > 0.5)
{
readyToCalibrate[id] = true;
}
}
}
// Draw the corners.
cv::drawChessboardCorners( image, boardSize_, cv::Mat(pointBuf), found );
if((int)imagePoints_[0].size() > ui_->progressBar_count->maximum())
{
ui_->progressBar_count->setMaximum((int)imagePoints_[0].size());
}
ui_->progressBar_count->setValue((int)imagePoints_[0].size());
}
}
if(!stereo_ && readyToCalibrate[0])
{
ui_->pushButton_calibrate->setEnabled(true);
ui_->pushButton_save->setEnabled(true);
}
else if(stereo_ && readyToCalibrate[0] && readyToCalibrate[1])
{
ui_->pushButton_calibrate->setEnabled(true);
ui_->pushButton_save->setEnabled(true);
}
if(calibrated_ && ui_->checkBox_rectified->isChecked())
{
cv::Mat temp = image.clone();
cv::undistort(temp, image, cameraMatrix_, distCoeffs_);
if(!stereo_)
{
images[0] = model_.rectifyImage(images[0]);
}
else
{
images[0] = stereoModel_.left().rectifyImage(images[0]);
images[1] = stereoModel_.right().rectifyImage(images[1]);
}
}
if(ui_->checkBox_showHorizontalLines->isChecked())
{
for(int id=0; id<(stereo_?2:1); ++id)
{
int step = imageSize_[id].height/12;
for(int i=step; i<imageSize_[id].height; i+=step)
{
cv::line(images[id], cv::Point(0, i), cv::Point(imageSize_[id].width, i), CV_RGB(0,255,0));
}
}
}
//show frame
ui_->image_view->setImage(uCvMat2QImage(image).mirrored(ui_->checkBox_mirror->isChecked(), false));
ui_->image_view->setImage(uCvMat2QImage(images[0]).mirrored(ui_->checkBox_mirror->isChecked(), false));
if(stereo_)
{
ui_->image_view_2->setImage(uCvMat2QImage(images[1]).mirrored(ui_->checkBox_mirror->isChecked(), false));
}
processingData_ = false;
}
void CalibrationDialog::restart()
{
// restart
calibrated_ = false;
imagePoints_.clear();
imageParams_.clear();
imagePoints_[0].clear();
imagePoints_[1].clear();
imageParams_[0].clear();
imageParams_[1].clear();
model_ = CameraModel();
stereoModel_ = StereoCameraModel();
cameraName_.clear();
ui_->pushButton_calibrate->setEnabled(false);
ui_->pushButton_save->setEnabled(false);
ui_->buttonBox->button(QDialogButtonBox::Ok)->setEnabled(false);
ui_->checkBox_rectified->setEnabled(false);
//ui_->pushButton_save->setEnabled(false);
ui_->progressBar_count->reset();
ui_->progressBar_count->setMaximum(COUNT_MIN);
@@ -267,116 +412,282 @@ void CalibrationDialog::restart()
ui_->progressBar_size->reset();
ui_->progressBar_skew->reset();
ui_->progressBar_x_2->reset();
ui_->progressBar_y_2->reset();
ui_->progressBar_size_2->reset();
ui_->progressBar_skew_2->reset();
ui_->label_serial->clear();
ui_->label_fx->setNum(0);
ui_->label_fy->setNum(0);
ui_->label_cx->setNum(0);
ui_->label_cy->setNum(0);
ui_->label_baseline->setNum(0);
ui_->label_error->setNum(0);
ui_->lineEdit_K->clear();
ui_->lineEdit_D->clear();
ui_->lineEdit_R->clear();
ui_->lineEdit_P->clear();
ui_->label_fx_2->setNum(0);
ui_->label_fy_2->setNum(0);
ui_->label_cx_2->setNum(0);
ui_->label_cy_2->setNum(0);
ui_->lineEdit_K_2->clear();
ui_->lineEdit_D_2->clear();
ui_->lineEdit_R_2->clear();
ui_->lineEdit_P_2->clear();
}
void CalibrationDialog::calibrate()
{
//calibrate
std::vector<cv::Mat> rvecs, tvecs;
std::vector<float> reprojErrs;
double totalAvgErr = 0;
UINFO("Calibration...");
processingData_ = true;
std::vector<std::vector<cv::Point3f> > objectPoints(1);
// compute board corner positions
for( int i = 0; i < boardSize_.height; ++i )
for( int i = 0; i < boardSize_.height; ++i )
for( int j = 0; j < boardSize_.width; ++j )
objectPoints[0].push_back(cv::Point3f(float( j*squareSize_ ), float( i*squareSize_ ), 0));
objectPoints.resize(imagePoints_.size(),objectPoints[0]);
objectPoints.resize(imagePoints_[0].size(),objectPoints[0]);
//Find intrinsic and extrinsic camera parameters
double rms = cv::calibrateCamera(objectPoints,
imagePoints_,
imageSize_,
cameraMatrix_,
distCoeffs_,
rvecs,
tvecs,
CV_CALIB_FIX_K4|CV_CALIB_FIX_K5);
std::cout << "cameraMatrix = " << cameraMatrix_ << std::endl;
std::cout << "distCoeffs = " << distCoeffs_ << std::endl;
UINFO("Re-projection error reported by calibrateCamera: %f", rms);
calibrated_ = checkRange(cameraMatrix_) && checkRange(distCoeffs_);
// compute reprojection errors
std::vector<cv::Point2f> imagePoints2;
int i, totalPoints = 0;
double totalErr = 0, err;
reprojErrs.resize(objectPoints.size());
for( i = 0; i < (int)objectPoints.size(); ++i )
if(!stereo_)
{
cv::projectPoints( cv::Mat(objectPoints[i]), rvecs[i], tvecs[i], cameraMatrix_,
distCoeffs_, imagePoints2);
err = cv::norm(cv::Mat(imagePoints_[i]), cv::Mat(imagePoints2), CV_L2);
//calibrate
std::vector<cv::Mat> rvecs, tvecs;
std::vector<float> reprojErrs;
cv::Mat K, D;
K = cv::Mat::eye(3,3,CV_64FC1);
int n = (int)objectPoints[i].size();
reprojErrs[i] = (float) std::sqrt(err*err/n);
totalErr += err*err;
totalPoints += n;
//Find intrinsic and extrinsic camera parameters
double rms = cv::calibrateCamera(objectPoints,
imagePoints_[0],
imageSize_[0],
K,
D,
rvecs,
tvecs,
CV_CALIB_FIX_K4|CV_CALIB_FIX_K5);
std::cout << "cameraMatrix = " << K << std::endl;
std::cout << "distCoeffs = " << D << std::endl;
UINFO("Re-projection error reported by calibrateCamera: %f", rms);
calibrated_ = checkRange(K) && checkRange(D);
// compute reprojection errors
std::vector<cv::Point2f> imagePoints2;
int i, totalPoints = 0;
double totalErr = 0, err;
reprojErrs.resize(objectPoints.size());
for( i = 0; i < (int)objectPoints.size(); ++i )
{
cv::projectPoints( cv::Mat(objectPoints[i]), rvecs[i], tvecs[i], K, D, imagePoints2);
err = cv::norm(cv::Mat(imagePoints_[0][i]), cv::Mat(imagePoints2), CV_L2);
int n = (int)objectPoints[i].size();
reprojErrs[i] = (float) std::sqrt(err*err/n);
totalErr += err*err;
totalPoints += n;
}
double totalAvgErr = std::sqrt(totalErr/totalPoints);
UINFO("%s. avg re projection error = %f", calibrated_ ? "Calibration succeeded" : "Calibration failed", totalAvgErr);
if(calibrated_)
{
cv::Mat P(3,4,CV_64FC1);
P.at<double>(2,3) = 1;
K.copyTo(P.colRange(0,3).rowRange(0,3));
model_ = CameraModel(cameraName_.toStdString(), imageSize_[0], K, D, cv::Mat::eye(3,3,CV_64FC1), P);
ui_->label_fx->setNum(model_.fx());
ui_->label_fy->setNum(model_.fy());
ui_->label_cx->setNum(model_.cx());
ui_->label_cy->setNum(model_.cy());
ui_->label_error->setNum(totalAvgErr);
std::stringstream strK, strD, strR, strP;
strK << model_.K();
strD << model_.D();
strR << model_.R();
strP << model_.P();
ui_->lineEdit_K->setText(strK.str().c_str());
ui_->lineEdit_D->setText(strD.str().c_str());
ui_->lineEdit_R->setText(strR.str().c_str());
ui_->lineEdit_P->setText(strP.str().c_str());
ui_->checkBox_rectified->setEnabled(true);
ui_->checkBox_rectified->setChecked(true);
ui_->buttonBox->button(QDialogButtonBox::Ok)->setEnabled(true);
ui_->pushButton_save->setEnabled(true);
}
}
else
{
UINFO("stereo calibration...");
cv::Size imageSize = imageSize_[0];
std::vector<cv::Mat> K(2), D(2);
K[0] = cv::Mat::eye(3, 3, CV_64F);
K[1] = cv::Mat::eye(3, 3, CV_64F);
cv::Mat R, T, E, F;
double rms = cv::stereoCalibrate(objectPoints, imagePoints_[0], imagePoints_[1],
K[0], D[0],
K[1], D[1],
imageSize, R, T, E, F,
cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 100, 1e-5),
cv::CALIB_FIX_ASPECT_RATIO +
cv::CALIB_ZERO_TANGENT_DIST +
cv::CALIB_SAME_FOCAL_LENGTH +
cv::CALIB_RATIONAL_MODEL +
cv::CALIB_FIX_K3 + cv::CALIB_FIX_K4 + cv::CALIB_FIX_K5);
UINFO("stereo calibration... done with RMS error=%f", rms);
calibrated_ = checkRange(K[0]) && checkRange(D[0]) && checkRange(K[1]) && checkRange(D[1]);
double err = 0;
int npoints = 0;
std::vector<cv::Vec3f> lines[2];
UINFO("Computing avg re-projection error...");
for(unsigned int i = 0; i < imagePoints_[0].size(); i++ )
{
int npt = (int)imagePoints_[0][i].size();
cv::Mat imgpt[2];
for(int k = 0; k < 2; k++ )
{
imgpt[k] = cv::Mat(imagePoints_[k][i]);
cv::undistortPoints(imgpt[k], imgpt[k], K[k], D[k], cv::Mat(), K[k]);
computeCorrespondEpilines(imgpt[k], k+1, F, lines[k]);
}
for(int j = 0; j < npt; j++ )
{
double errij = fabs(imagePoints_[0][i][j].x*lines[1][j][0] +
imagePoints_[0][i][j].y*lines[1][j][1] + lines[1][j][2]) +
fabs(imagePoints_[1][i][j].x*lines[0][j][0] +
imagePoints_[1][i][j].y*lines[0][j][1] + lines[0][j][2]);
err += errij;
}
npoints += npt;
}
double totalAvgErr = err/(double)npoints;
UINFO("%s. avg re projection error = %f", calibrated_ ? "Calibration succeeded" : "Calibration failed", totalAvgErr);
if(calibrated_)
{
cv::Mat R1, R2, P1, P2, Q;
cv::Rect validRoi[2];
cv::stereoRectify(K[0], D[0],
K[1], D[1],
imageSize, R, T, R1, R2, P1, P2, Q,
cv::CALIB_ZERO_DISPARITY, 0, imageSize, &validRoi[0], &validRoi[1]);
UINFO("Valid ROI1 = %d,%d,%d,%d ROI2 = %d,%d,%d,%d",
validRoi[0].x, validRoi[0].y, validRoi[0].width, validRoi[0].height,
validRoi[1].x, validRoi[1].y, validRoi[1].width, validRoi[1].height);
stereoModel_ = StereoCameraModel(cameraName_.toStdString(), imageSize, K[0], D[0], R1, P1, K[1], D[1], R2, P2);
ui_->label_fx->setNum(stereoModel_.left().fx());
ui_->label_fy->setNum(stereoModel_.left().fy());
ui_->label_cx->setNum(stereoModel_.left().cx());
ui_->label_cy->setNum(stereoModel_.left().cy());
ui_->label_fx_2->setNum(stereoModel_.right().fx());
ui_->label_fy_2->setNum(stereoModel_.right().fy());
ui_->label_cx_2->setNum(stereoModel_.right().cx());
ui_->label_cy_2->setNum(stereoModel_.right().cy());
ui_->label_baseline->setVisible(stereoModel_.baseline());
ui_->label_error->setNum(totalAvgErr);
std::stringstream strK, strD, strR, strP;
strK << stereoModel_.left().K();
strD << stereoModel_.left().D();
strR << stereoModel_.left().R();
strP << stereoModel_.left().P();
ui_->lineEdit_K->setText(strK.str().c_str());
ui_->lineEdit_D->setText(strD.str().c_str());
ui_->lineEdit_R->setText(strR.str().c_str());
ui_->lineEdit_P->setText(strP.str().c_str());
strK.clear();
strD.clear();
strR.clear();
strP.clear();
strK << stereoModel_.right().K();
strD << stereoModel_.right().D();
strR << stereoModel_.right().R();
strP << stereoModel_.right().P();
ui_->lineEdit_K_2->setText(strK.str().c_str());
ui_->lineEdit_D_2->setText(strD.str().c_str());
ui_->lineEdit_R_2->setText(strR.str().c_str());
ui_->lineEdit_P_2->setText(strP.str().c_str());
ui_->checkBox_rectified->setEnabled(true);
ui_->checkBox_rectified->setChecked(true);
ui_->buttonBox->button(QDialogButtonBox::Ok)->setEnabled(true);
ui_->pushButton_save->setEnabled(true);
}
}
totalAvgErr = std::sqrt(totalErr/totalPoints);
UINFO("End calibration");
UINFO("%s. avg re projection error = %f", calibrated_ ? "Calibration succeeded" : "Calibration failed", totalAvgErr);
if(calibrated_)
{
ui_->label_fx->setNum(cameraMatrix_.at<double>(0,0)); // K(0)
ui_->label_fy->setNum(cameraMatrix_.at<double>(1,1)); // K(4)
ui_->label_cx->setNum(cameraMatrix_.at<double>(0,2)); // K(2)
ui_->label_cy->setNum(cameraMatrix_.at<double>(1,2)); // K(5)
ui_->label_error->setNum(totalAvgErr);
std::stringstream strK, strD;
strK << cameraMatrix_;
strD << distCoeffs_;
ui_->lineEdit_K->setText(strK.str().c_str());
ui_->lineEdit_D->setText(strD.str().c_str());
ui_->checkBox_rectified->setEnabled(true);
ui_->checkBox_rectified->setChecked(true);
ui_->buttonBox->button(QDialogButtonBox::Ok)->setEnabled(true);
ui_->pushButton_save->setEnabled(true);
}
processingData_ = false;
}
void CalibrationDialog::save()
{
QString fileName = QFileDialog::getSaveFileName(this, tr("Export"), "calibration.yaml", "*.yaml");
if(!fileName.isEmpty())
processingData_ = true;
if(!stereo_)
{
cv::FileStorage fs(fileName.toStdString(), cv::FileStorage::WRITE);
UASSERT(model_.isValid());
QString cameraName = model_.name().c_str();
QString filePath = QFileDialog::getSaveFileName(this, tr("Export"), cameraName+".yaml", "*.yaml");
// export in ROS calibration format
fs << "camera_matrix" << "{";
fs << "rows" << cameraMatrix_.rows;
fs << "cols" << cameraMatrix_.cols;
fs << "data" << std::vector<double>((double*)cameraMatrix_.data, ((double*)cameraMatrix_.data)+(cameraMatrix_.rows*cameraMatrix_.cols));
fs << "}";
fs << "distortion_coefficients" << "{";
fs << "rows" << distCoeffs_.rows;
fs << "cols" << distCoeffs_.cols;
fs << "data" << std::vector<double>((double*)distCoeffs_.data, ((double*)distCoeffs_.data)+(distCoeffs_.rows*distCoeffs_.cols));
fs << "}";
fs.release();
QMessageBox::information(this, tr("Export"), tr("Calibration file saved to \"%1\".").arg(fileName));
UINFO("Saved \"%s\"!", fileName.toStdString().c_str());
if(!filePath.isEmpty())
{
if(model_.save(filePath.toStdString()))
{
QMessageBox::information(this, tr("Export"), tr("Calibration file saved to \"%1\".").arg(filePath));
UINFO("Saved \"%s\"!", filePath.toStdString().c_str());
}
else
{
UERROR("Error saving \"%s\"", filePath.toStdString().c_str());
}
}
}
else
{
UASSERT(stereoModel_.isValid());
QString cameraName = stereoModel_.name().c_str();
QString filePath = QFileDialog::getSaveFileName(this, tr("Export"), cameraName, "*.yaml");
std::string name = UFile::getName(filePath.toStdString());
std::string dir = UDirectory::getDir(filePath.toStdString());
if(!name.empty())
{
std::string base = (dir+UDirectory::separator()+name).c_str();
std::string leftPath = base+"_left.yaml";
std::string rightPath = base+"_right.yaml";
if(stereoModel_.save(dir, name))
{
QMessageBox::information(this, tr("Export"), tr("Calibration files saved to \"%1\" and \"%2\".").
arg(leftPath.c_str()).arg(rightPath.c_str()));
UINFO("Saved \"%s\" and \"%s\"!", leftPath.c_str(), rightPath.c_str());
}
else
{
UERROR("Error saving \"%s\" and \"%s\"", leftPath.c_str(), rightPath.c_str());
}
}
}
processingData_ = false;
}
float CalibrationDialog::getArea(const std::vector<cv::Point2f> & corners, const cv::Size & boardSize)
guilib/src/ui/calibrationDialog.ui
+269 -76
View File
@@ -6,8 +6,8 @@
<rect>
<x>0</x>
<y>0</y>
<width>1040</width>
<height>745</height>
<width>875</width>
<height>834</height>
</rect>
</property>
<property name="windowTitle">
@@ -15,11 +15,18 @@
</property>
<layout class="QVBoxLayout" name="verticalLayout_3">
<item>
<layout class="QHBoxLayout" name="horizontalLayout_3" stretch="1,0">
<layout class="QHBoxLayout" name="horizontalLayout_4" stretch="1,0">
<item>
<layout class="QVBoxLayout" name="verticalLayout_2" stretch="1,0">
<item>
<widget class="UImageView" name="image_view" native="true"/>
<layout class="QHBoxLayout" name="horizontalLayout_3">
<item>
<widget class="UImageView" name="image_view" native="true"/>
</item>
<item>
<widget class="UImageView" name="image_view_2" native="true"/>
</item>
</layout>
</item>
<item>
<layout class="QHBoxLayout" name="horizontalLayout_2">
@@ -37,6 +44,13 @@
</property>
</widget>
</item>
<item>
<widget class="QCheckBox" name="checkBox_showHorizontalLines">
<property name="text">
<string>Show horizontal lines</string>
</property>
</widget>
</item>
<item>
<spacer name="horizontalSpacer">
<property name="orientation">
@@ -122,6 +136,23 @@
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QLabel" name="label_19">
<property name="text">
<string>Serial</string>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_serial">
<property name="text">
<string>0</string>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
</layout>
</widget>
</item>
@@ -130,11 +161,39 @@
<property name="title">
<string>Progress</string>
</property>
<layout class="QFormLayout" name="formLayout">
<item row="0" column="0">
<widget class="QLabel" name="label_5">
<layout class="QGridLayout" name="gridLayout">
<item row="1" column="0">
<widget class="QLabel" name="label">
<property name="text">
<string>Count</string>
<string>X</string>
</property>
</widget>
</item>
<item row="4" column="0">
<widget class="QLabel" name="label_4">
<property name="text">
<string>Skew</string>
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QProgressBar" name="progressBar_skew">
<property name="value">
<number>24</number>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QProgressBar" name="progressBar_size">
<property name="value">
<number>24</number>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QLabel" name="label_2">
<property name="text">
<string>Y</string>
</property>
</widget>
</item>
@@ -148,13 +207,6 @@
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QLabel" name="label">
<property name="text">
<string>X</string>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QProgressBar" name="progressBar_x">
<property name="value">
@@ -165,13 +217,6 @@
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QLabel" name="label_2">
<property name="text">
<string>Y</string>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QProgressBar" name="progressBar_y">
<property name="value">
@@ -186,25 +231,57 @@
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QProgressBar" name="progressBar_size">
<property name="value">
<number>24</number>
</property>
</widget>
</item>
<item row="4" column="0">
<widget class="QLabel" name="label_4">
<item row="0" column="0">
<widget class="QLabel" name="label_5">
<property name="text">
<string>Skew</string>
<string>Count</string>
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QProgressBar" name="progressBar_skew">
<item row="1" column="2">
<widget class="QProgressBar" name="progressBar_x_2">
<property name="value">
<number>24</number>
</property>
<property name="invertedAppearance">
<bool>true</bool>
</property>
<property name="textDirection">
<enum>QProgressBar::TopToBottom</enum>
</property>
<property name="format">
<string>%p%</string>
</property>
</widget>
</item>
<item row="2" column="2">
<widget class="QProgressBar" name="progressBar_y_2">
<property name="value">
<number>24</number>
</property>
<property name="invertedAppearance">
<bool>true</bool>
</property>
</widget>
</item>
<item row="3" column="2">
<widget class="QProgressBar" name="progressBar_size_2">
<property name="value">
<number>24</number>
</property>
<property name="invertedAppearance">
<bool>true</bool>
</property>
</widget>
</item>
<item row="4" column="2">
<widget class="QProgressBar" name="progressBar_skew_2">
<property name="value">
<number>24</number>
</property>
<property name="invertedAppearance">
<bool>true</bool>
</property>
</widget>
</item>
</layout>
@@ -222,34 +299,7 @@
<property name="title">
<string>Camera intrinsic parameters</string>
</property>
<layout class="QFormLayout" name="formLayout_2">
<property name="fieldGrowthPolicy">
<enum>QFormLayout::AllNonFixedFieldsGrow</enum>
</property>
<item row="0" column="0">
<widget class="QLabel" name="label_8">
<property name="text">
<string>fx</string>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_fx">
<property name="text">
<string>0</string>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QLabel" name="label_9">
<property name="text">
<string>fy</string>
</property>
</widget>
</item>
<layout class="QGridLayout" name="gridLayout_2">
<item row="1" column="1">
<widget class="QLabel" name="label_fy">
<property name="text">
@@ -260,10 +310,10 @@
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QLabel" name="label_10">
<property name="text">
<string>cx</string>
<item row="5" column="1">
<widget class="QLineEdit" name="lineEdit_K">
<property name="readOnly">
<bool>true</bool>
</property>
</widget>
</item>
@@ -294,7 +344,7 @@
</property>
</widget>
</item>
<item row="4" column="0">
<item row="5" column="0">
<widget class="QLabel" name="label_6">
<property name="toolTip">
<string>Camera matrix</string>
@@ -304,14 +354,62 @@
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QLineEdit" name="lineEdit_K">
<item row="0" column="1">
<widget class="QLabel" name="label_fx">
<property name="text">
<string>0</string>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QLabel" name="label_9">
<property name="text">
<string>fy</string>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QLabel" name="label_10">
<property name="text">
<string>cx</string>
</property>
</widget>
</item>
<item row="10" column="0">
<widget class="QLabel" name="label_13">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="0" column="0">
<widget class="QLabel" name="label_8">
<property name="text">
<string>fx</string>
</property>
</widget>
</item>
<item row="7" column="0">
<widget class="QLabel" name="label_17">
<property name="toolTip">
<string>Distorsion coefficients</string>
</property>
<property name="text">
<string>R</string>
</property>
</widget>
</item>
<item row="6" column="2">
<widget class="QLineEdit" name="lineEdit_D_2">
<property name="readOnly">
<bool>true</bool>
</property>
</widget>
</item>
<item row="5" column="0">
<item row="6" column="0">
<widget class="QLabel" name="label_7">
<property name="toolTip">
<string>Distorsion coefficients</string>
@@ -321,14 +419,14 @@
</property>
</widget>
</item>
<item row="5" column="1">
<item row="6" column="1">
<widget class="QLineEdit" name="lineEdit_D">
<property name="readOnly">
<bool>true</bool>
</property>
</widget>
</item>
<item row="6" column="0">
<item row="9" column="0">
<widget class="QLabel" name="label_14">
<property name="toolTip">
<string>Avg. reproduction error</string>
@@ -338,17 +436,112 @@
</property>
</widget>
</item>
<item row="6" column="1">
<item row="9" column="1">
<widget class="QLabel" name="label_error">
<property name="text">
<string>0</string>
</property>
</widget>
</item>
<item row="7" column="0" colspan="2">
<widget class="QLabel" name="label_13">
<item row="0" column="2">
<widget class="QLabel" name="label_fx_2">
<property name="text">
<string/>
<string>0</string>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="2" column="2">
<widget class="QLabel" name="label_cx_2">
<property name="text">
<string>0</string>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="1" column="2">
<widget class="QLabel" name="label_fy_2">
<property name="text">
<string>0</string>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="3" column="2">
<widget class="QLabel" name="label_cy_2">
<property name="text">
<string>0</string>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="5" column="2">
<widget class="QLineEdit" name="lineEdit_K_2">
<property name="readOnly">
<bool>true</bool>
</property>
</widget>
</item>
<item row="8" column="0">
<widget class="QLabel" name="label_18">
<property name="toolTip">
<string>Distorsion coefficients</string>
</property>
<property name="text">
<string>P</string>
</property>
</widget>
</item>
<item row="7" column="1">
<widget class="QLineEdit" name="lineEdit_R">
<property name="readOnly">
<bool>true</bool>
</property>
</widget>
</item>
<item row="8" column="1">
<widget class="QLineEdit" name="lineEdit_P">
<property name="readOnly">
<bool>true</bool>
</property>
</widget>
</item>
<item row="7" column="2">
<widget class="QLineEdit" name="lineEdit_R_2">
<property name="readOnly">
<bool>true</bool>
</property>
</widget>
</item>
<item row="8" column="2">
<widget class="QLineEdit" name="lineEdit_P_2">
<property name="readOnly">
<bool>true</bool>
</property>
</widget>
</item>
<item row="4" column="0">
<widget class="QLabel" name="label_baseline_name">
<property name="text">
<string>baseline</string>
</property>
</widget>
</item>
<item row="4" column="2">
<widget class="QLabel" name="label_baseline">
<property name="text">
<string>0</string>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
guilib/src/utilite/UImageView.h
-5
View File
@@ -22,11 +22,6 @@ public:
public slots:
void setImage(const QImage & image)
{
if(pixmap_.width() != image.width() || pixmap_.height() != image.height())
{
this->setMinimumSize(image.width(), image.height());
this->setGeometry(this->geometry().x(), this->geometry().y(), image.width(), image.height());
}
pixmap_ = QPixmap::fromImage(image);
this->update();
}
tools/Calibration/main.cpp
+59 -23
View File
@@ -38,9 +38,17 @@ void showUsage()
printf("\nUsage:\n"
"rtabmap-calibration [options]\n"
"Options:\n"
" --driver # Driver number to use: 0=USB camera, 1=OpenNI-PCL, 2=OpenNI2,\n"
" 3=Freenect, 4=OpenNI-CV, 5=OpenNI-CV-ASUS\n"
" --device # Device id\n\n");
" --driver # Driver number to use:-1=USB camera\n"
" 0=OpenNI-PCL (Kinect)\n"
" 1=OpenNI2 (Kinect and Xtion PRO Live)\n"
" 2=Freenect (Kinect)\n"
" 3=OpenNI-CV (Kinect)\n"
" 4=OpenNI-CV-ASUS (Xtion PRO Live)\n"
" 5=Freenect2 (Kinect v2)\n"
" 6=DC1394 (Bumblebee2)\n"
" --device # Device id\n"
" --debug Debug log\n"
" --stereo Stereo\n\n");
exit(1);
}
@@ -49,8 +57,9 @@ int main(int argc, char * argv[])
ULogger::setType(ULogger::kTypeConsole);
ULogger::setLevel(ULogger::kInfo);
int driver = 0;
int driver = -1;
int device = 0;
bool stereo = false;
for(int i=1; i<argc; ++i)
{
if(strcmp(argv[i], "--driver") == 0)
@@ -59,7 +68,7 @@ int main(int argc, char * argv[])
if(i < argc)
{
driver = std::atoi(argv[i]);
if(driver < 0)
if(driver < -1)
{
showUsage();
}
@@ -87,6 +96,16 @@ int main(int argc, char * argv[])
}
continue;
}
if(strcmp(argv[i], "--debug") == 0)
{
ULogger::setLevel(ULogger::kDebug);
continue;
}
if(strcmp(argv[i], "--stereo") == 0)
{
stereo=true;
continue;
}
if(strcmp(argv[i], "--help") == 0)
{
showUsage();
@@ -94,44 +113,52 @@ int main(int argc, char * argv[])
printf("Unrecognized option : %s\n", argv[i]);
showUsage();
}
driver = atoi(argv[argc-1]);
if(driver < 0 || driver > 5)
if(driver < -1 || driver > 6)
{
UERROR("driver should be between 0 and 5.");
UERROR("driver should be between -1 and 6.");
showUsage();
}
UINFO("Using driver %d", driver);
UINFO("Using device %d", device);
UINFO("Stereo: %s", stereo?"true":"false");
float imageRate = 1.0f;
rtabmap::Camera * cameraUsb = 0;
rtabmap::CameraRGBD * camera = 0;
if(driver == 0)
if(driver == -1)
{
cameraUsb = new rtabmap::CameraVideo(device, imageRate);
cameraUsb = new rtabmap::CameraVideo(device);
}
else if(driver == 0)
{
camera = new rtabmap::CameraOpenni();
}
else if(driver == 1)
{
camera = new rtabmap::CameraOpenni(uNumber2Str(device), imageRate);
}
else if(driver == 2)
{
if(!rtabmap::CameraOpenNI2::available())
{
UERROR("Not built with OpenNI2 support...");
exit(-1);
}
camera = new rtabmap::CameraOpenNI2(uNumber2Str(device), imageRate);
camera = new rtabmap::CameraOpenNI2();
}
else if(driver == 3)
else if(driver == 2)
{
if(!rtabmap::CameraFreenect::available())
{
UERROR("Not built with Freenect support...");
exit(-1);
}
camera = new rtabmap::CameraFreenect(device, imageRate);
camera = new rtabmap::CameraFreenect();
}
else if(driver == 3)
{
if(!rtabmap::CameraOpenNICV::available())
{
UERROR("Not built with OpenNI from OpenCV support...");
exit(-1);
}
camera = new rtabmap::CameraOpenNICV(false);
}
else if(driver == 4)
{
@@ -140,16 +167,25 @@ int main(int argc, char * argv[])
UERROR("Not built with OpenNI from OpenCV support...");
exit(-1);
}
camera = new rtabmap::CameraOpenNICV(false, imageRate);
camera = new rtabmap::CameraOpenNICV(true);
}
else if(driver == 5)
{
if(!rtabmap::CameraOpenNICV::available())
if(!rtabmap::CameraFreenect2::available())
{
UERROR("Not built with OpenNI from OpenCV support...");
UERROR("Not built with Freenect2 support...");
exit(-1);
}
camera = new rtabmap::CameraOpenNICV(true, imageRate);
camera = new rtabmap::CameraFreenect2();
}
else if(driver == 6)
{
if(!rtabmap::CameraDC1394::available())
{
UERROR("Not built with DC1394 support...");
exit(-1);
}
camera = new rtabmap::CameraDC1394();
}
else
{
@@ -180,7 +216,7 @@ int main(int argc, char * argv[])
}
QApplication app(argc, argv);
rtabmap::CalibrationDialog dialog;
rtabmap::CalibrationDialog dialog(stereo);
dialog.registerToEventsManager();
dialog.show();