i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 00:37:46 +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: 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), 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), UEvent(kCodeImageDepth),
data_(rgb, depth, fx, fy, cx, cy, localTransform, Transform(), 1.0f, 1.0f, id, stamp) data_(data),
{ cameraName_(cameraName)
}
CameraEvent(const SensorData & data) :
UEvent(kCodeImageDepth),
data_(data)
{ {
} }
// Image or descriptors // Image or descriptors
const SensorData & data() const {return data_;} const SensorData & data() const {return data_;}
const std::string & cameraName() const {return cameraName_;}
virtual ~CameraEvent() {} virtual ~CameraEvent() {}
virtual std::string getClassName() const {return std::string("CameraEvent");} virtual std::string getClassName() const {return std::string("CameraEvent");}
private: private:
SensorData data_; SensorData data_;
std::string cameraName_;
}; };
} // namespace rtabmap } // namespace rtabmap
corelib/include/rtabmap/core/CameraModel.h
+40 -11
View File
@@ -36,8 +36,23 @@ class CameraModel
{ {
public: public:
CameraModel(); 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() {} 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 fx() const {return P_.at<double>(0,0);}
double fy() const {return P_.at<double>(1,1);} double fy() const {return P_.at<double>(1,1);}
double cx() const {return P_.at<double>(0,2);} double cx() const {return P_.at<double>(0,2);}
@@ -49,17 +64,18 @@ public:
const cv::Mat & R() const {return R_;} const cv::Mat & R() const {return R_;}
const cv::Mat & P() const {return P_;} const cv::Mat & P() const {return P_;}
int width() const {return width_;} const cv::Size & imageSize() const {return imageSize_;}
int height() const {return height_;} int imageWidth() const {return imageSize_.width;}
int imageWeight() const {return imageSize_.height;}
bool load(const std::string & directory, const std::string & cameraName); bool load(const std::string & filePath);
void save(const std::string & directory, const std::string & cameraName); bool save(const std::string & filePath);
cv::Mat rectifyImage(const cv::Mat & raw) const; cv::Mat rectifyImage(const cv::Mat & raw) const;
private: private:
int width_; std::string name_;
int height_; cv::Size imageSize_;
cv::Mat K_; cv::Mat K_;
cv::Mat D_; cv::Mat D_;
cv::Mat R_; cv::Mat R_;
@@ -72,17 +88,29 @@ class StereoCameraModel
{ {
public: public:
StereoCameraModel() {} 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() {} 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) bool load(const std::string & directory, const std::string & cameraName)
{ {
return left_.load(directory, cameraName+"_left") && name_ = cameraName;
right_.load(directory, cameraName+"_right"); 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"); return left_.save(directory+"/"+cameraName+"_left.yaml") &&
right_.save(directory, cameraName+"_right"); right_.save(directory+"/"+cameraName+"_right.yaml");
} }
double baseline() const {return -right_.Tx()/right_.fx();} double baseline() const {return -right_.Tx()/right_.fx();}
@@ -92,6 +120,7 @@ public:
private: private:
CameraModel left_; CameraModel left_;
CameraModel right_; CameraModel right_;
std::string name_;
}; };
} /* namespace rtabmap */ } /* namespace rtabmap */
corelib/include/rtabmap/core/CameraRGBD.h
+17 -2
View File
@@ -81,6 +81,7 @@ public:
virtual ~CameraRGBD(); virtual ~CameraRGBD();
void takeImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy); void takeImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy);
virtual bool init() = 0; virtual bool init() = 0;
virtual std::string getSerial() const = 0;
//getters //getters
float getImageRate() const {return _imageRate;} float getImageRate() const {return _imageRate;}
@@ -154,7 +155,8 @@ public:
const boost::shared_ptr<openni_wrapper::DepthImage>& depth, const boost::shared_ptr<openni_wrapper::DepthImage>& depth,
float constant); float constant);
bool init(); virtual bool init();
virtual std::string getSerial() const;
protected: protected:
virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy); 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 ~CameraOpenNICV();
virtual bool init(); virtual bool init();
virtual std::string getSerial() const {return "";} // unknown with OpenCV
protected: protected:
virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy); 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 ~CameraOpenNI2();
virtual bool init(); virtual bool init();
virtual std::string getSerial() const;
bool setAutoWhiteBalance(bool enabled); bool setAutoWhiteBalance(bool enabled);
bool setAutoExposure(bool enabled); bool setAutoExposure(bool enabled);
@@ -266,6 +270,7 @@ public:
virtual ~CameraFreenect(); virtual ~CameraFreenect();
bool init(); bool init();
virtual std::string getSerial() const;
protected: protected:
virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy); virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy);
@@ -298,6 +303,7 @@ public:
virtual ~CameraFreenect2(); virtual ~CameraFreenect2();
bool init(); bool init();
virtual std::string getSerial() const;
protected: protected:
virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy); 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(); static bool available();
public: 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, CameraDC1394(const std::string & calibrationFolder,
float imageRate=0.0f, float imageRate=0.0f,
const Transform & localTransform = Transform::getIdentity(), const Transform & localTransform = Transform::getIdentity(),
@@ -332,11 +345,13 @@ public:
virtual ~CameraDC1394(); virtual ~CameraDC1394();
bool init(); bool init();
virtual std::string getSerial() const;
protected: protected:
virtual void captureImage(cv::Mat & left, cv::Mat & right, float & fx, float & baseline, float & cx, float & cy); virtual void captureImage(cv::Mat & left, cv::Mat & right, float & fx, float & baseline, float & cx, float & cy);
private: private:
bool lookForCalibration_;
std::string calibrationFolder_; std::string calibrationFolder_;
DC1394Device *device_; DC1394Device *device_;
StereoCameraModel stereoModel_; 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 { namespace rtabmap {
CameraModel::CameraModel() : CameraModel::CameraModel() :
width_(0),
height_(0),
P_(cv::Mat::zeros(3, 4, CV_64FC1)) 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(); K_ = cv::Mat();
D_ = cv::Mat(); D_ = cv::Mat();
@@ -50,14 +68,17 @@ bool CameraModel::load(const std::string & directory, const std::string & camera
rectificationMap1_ = cv::Mat(); rectificationMap1_ = cv::Mat();
rectificationMap2_ = cv::Mat(); rectificationMap2_ = cv::Mat();
std::string path = directory + UDirectory::separator() + cameraName + ".yaml"; if(UFile::exists(filePath))
if(UFile::exists(path))
{ {
UINFO("Reading calibration file \"%s\"", path.c_str()); UINFO("Reading calibration file \"%s\"", filePath.c_str());
cv::FileStorage fs(path, cv::FileStorage::READ); cv::FileStorage fs(filePath, cv::FileStorage::READ);
width_ = (int)fs["image_width"]; name_ = (int)fs["camera_name"];
height_ = (int)fs["image_height"]; 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 // import from ROS calibration format
cv::FileNode n = fs["camera_matrix"]; cv::FileNode n = fs["camera_matrix"];
@@ -99,17 +120,56 @@ bool CameraModel::load(const std::string & directory, const std::string & camera
fs.release(); fs.release();
// init rectification map // init rectification map
cv::initUndistortRectifyMap(K_, D_, R_, P_, cv::Size(width_, height_), UINFO("Initialize rectify map");
CV_16SC2, rectificationMap1_, rectificationMap2_); cv::initUndistortRectifyMap(K_, D_, R_, P_, imageSize_, CV_16SC2, rectificationMap1_, rectificationMap2_);
return true; return true;
} }
return false; 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 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; 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) void CameraOpenni::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy)
{ {
if(interface_ && interface_->isRunning()) if(interface_ && interface_->isRunning())
@@ -637,6 +646,15 @@ bool CameraOpenNI2::init()
#endif #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) void CameraOpenNI2::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy)
{ {
#ifdef WITH_OPENNI2 #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"); 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() 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) if(freenect_open_device(ctx_, &device_, index_) < 0)
{ {
UERROR("FreenectDevice: Cannot open Kinect"); UERROR("FreenectDevice: Cannot open Kinect");
return false; 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_user(device_, this);
freenect_set_video_mode(device_, freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_VIDEO_RGB)); 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)); freenect_set_depth_mode(device_, freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_DEPTH_REGISTERED));
@@ -833,6 +879,7 @@ private:
private: private:
int index_; int index_;
std::string serial_;
freenect_context * ctx_; freenect_context * ctx_;
freenect_device * device_; freenect_device * device_;
cv::Mat depthBuffer_; cv::Mat depthBuffer_;
@@ -922,6 +969,17 @@ bool CameraFreenect::init()
return false; 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) void CameraFreenect::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy)
{ {
#ifdef WITH_FREENECT #ifdef WITH_FREENECT
@@ -1045,6 +1103,17 @@ bool CameraFreenect2::init()
return false; 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) void CameraFreenect2::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy)
{ {
#ifdef WITH_FREENECT2 #ifdef WITH_FREENECT2
@@ -1353,7 +1422,7 @@ public:
//DC1394_COLOR_CODING_RAW16: //DC1394_COLOR_CODING_RAW16:
//DC1394_COLOR_FILTER_BGGR //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), 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); dc1394_capture_enqueue(camera_, frame);
@@ -1380,8 +1449,19 @@ bool CameraDC1394::available()
#endif #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) : 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), CameraRGBD(imageRate, localTransform, fx, fy, cx, cy),
lookForCalibration_(true),
calibrationFolder_(calibrationFolder), calibrationFolder_(calibrationFolder),
device_(0) device_(0)
{ {
@@ -1409,9 +1489,12 @@ bool CameraDC1394::init()
if(ok) if(ok)
{ {
// look for calibration files // 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; return ok;
@@ -1422,6 +1505,17 @@ bool CameraDC1394::init()
return false; 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) void CameraDC1394::captureImage(cv::Mat & left, cv::Mat & right, float & fx, float & baseline, float & cx, float & cy)
{ {
#ifdef WITH_DC1394 #ifdef WITH_DC1394
corelib/src/CameraThread.cpp
+2 -1
View File
@@ -125,7 +125,8 @@ void CameraThread::mainLoop()
{ {
if(_cameraRGBD) 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 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.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.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); 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); UASSERT(_feature2D != 0);
PreUpdateThread preUpdateThread(_vwd); 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 _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; UTimer time;
Transform t = this->computeTransform(data, info); 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 UASSERT(depthOrRightImage.type() == CV_32FC1 || // Depth in meter
depthOrRightImage.type() == CV_16UC1 || // Depth in millimetre depthOrRightImage.type() == CV_16UC1 || // Depth in millimetre
depthOrRightImage.type() == CV_8U); // Right stereo image 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(!_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)"); 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 UASSERT(depthOrRightImage.type() == CV_32FC1 || // Depth in meter
depthOrRightImage.type() == CV_16UC1 || // Depth in millimetre depthOrRightImage.type() == CV_16UC1 || // Depth in millimetre
depthOrRightImage.type() == CV_8U); // Right stereo image 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(!_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)"); 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 <QDialog>
#include <opencv2/opencv.hpp> #include <opencv2/opencv.hpp>
#include <rtabmap/core/CameraModel.h>
#include <rtabmap/utilite/UEventsHandler.h> #include <rtabmap/utilite/UEventsHandler.h>
class Ui_calibrationDialog; class Ui_calibrationDialog;
@@ -44,16 +46,16 @@ class RTABMAPGUI_EXP CalibrationDialog : public QDialog, public UEventsHandler
Q_OBJECT; Q_OBJECT;
public: public:
CalibrationDialog(QWidget * parent = 0); CalibrationDialog(bool stereo = false, QWidget * parent = 0);
virtual ~CalibrationDialog(); virtual ~CalibrationDialog();
bool isCalibrated() const {return calibrated_;} bool isCalibrated() const {return calibrated_;}
const cv::Mat & cameraMatrix() const {return cameraMatrix_;} // Matrix K const cv::Mat & cameraMatrix() const {return model_.K();} // Matrix K
const cv::Mat & distCoeffs() const {return distCoeffs_;} // Matrix D const cv::Mat & distCoeffs() const {return model_.D();} // Matrix D
float fx() const {return cameraMatrix_.at<double>(0,0);} // K(0) float fx() const {return model_.fx();} // K(0)
float fy() const {return cameraMatrix_.at<double>(1,1);} // K(4) float fy() const {return model_.fy();} // K(4)
float cx() const {return cameraMatrix_.at<double>(0,2);} // K(2) float cx() const {return model_.cx();} // K(2)
float cy() const {return cameraMatrix_.at<double>(1,2);} // K(5) float cy() const {return model_.cy();} // K(5)
public slots: public slots:
void setBoardWidth(int width); void setBoardWidth(int width);
@@ -61,7 +63,7 @@ public slots:
void setSquareSize(double size); void setSquareSize(double size);
private slots: private slots:
void processImage(const cv::Mat & image); void processImages(const cv::Mat & imageLeft, const cv::Mat & imageRight, const QString & cameraName);
void restart(); void restart();
void calibrate(); void calibrate();
void save(); void save();
@@ -85,13 +87,17 @@ private:
// parameters // parameters
cv::Size boardSize_; // innner squares cv::Size boardSize_; // innner squares
float squareSize_; // m float squareSize_; // m
bool stereo_;
std::vector<std::vector<cv::Point2f> > imagePoints_; QString cameraName_;
std::vector<std::vector<float> > imageParams_;
cv::Size imageSize_;
bool calibrated_; bool calibrated_;
cv::Mat cameraMatrix_; bool processingData_;
cv::Mat distCoeffs_;
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_; 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/gui/UCv2Qt.h>
#include <rtabmap/utilite/ULogger.h> #include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UDirectory.h>
#include <rtabmap/utilite/UFile.h>
namespace rtabmap { namespace rtabmap {
#define COUNT_MIN 12 #define COUNT_MIN 40
CalibrationDialog::CalibrationDialog(QWidget * parent) : CalibrationDialog::CalibrationDialog(bool stereo, QWidget * parent) :
QDialog(parent), QDialog(parent),
boardSize_(8,6), boardSize_(8,6),
squareSize_(0.033), squareSize_(0.033),
stereo_(stereo),
calibrated_(false), calibrated_(false),
cameraMatrix_(cv::Mat::eye(3, 3, CV_64F)), processingData_(false)
distCoeffs_(cv::Mat::zeros(8, 1, CV_64F))
{ {
imagePoints_.resize(2);
imageParams_.resize(2);
imageSize_.resize(2);
qRegisterMetaType<cv::Mat>("cv::Mat"); qRegisterMetaType<cv::Mat>("cv::Mat");
ui_ = new Ui_calibrationDialog(); ui_ = new Ui_calibrationDialog();
ui_->setupUi(this); 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_calibrate, SIGNAL(clicked()), this, SLOT(calibrate()));
connect(ui_->pushButton_restart, SIGNAL(clicked()), this, SLOT(restart())); connect(ui_->pushButton_restart, SIGNAL(clicked()), this, SLOT(restart()));
connect(ui_->pushButton_save, SIGNAL(clicked()), this, SLOT(save())); connect(ui_->pushButton_save, SIGNAL(clicked()), this, SLOT(save()));
@@ -117,96 +142,168 @@ void CalibrationDialog::closeEvent(QCloseEvent* event)
void CalibrationDialog::handleEvent(UEvent * event) void CalibrationDialog::handleEvent(UEvent * event)
{ {
if(event->getClassName().compare("CameraEvent") == 0) if(!processingData_)
{ {
rtabmap::CameraEvent * e = (rtabmap::CameraEvent *)event; if(event->getClassName().compare("CameraEvent") == 0)
if(e->getCode() == rtabmap::CameraEvent::kCodeImage ||
e->getCode() == rtabmap::CameraEvent::kCodeImageDepth)
{ {
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(); processingData_ = true;
std::vector<cv::Point2f> pointBuf; if(cameraName_.isEmpty())
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,
{ {
// improve the found corners' coordinate accuracy for chessboard cameraName_ = "0000";
cv::Mat viewGray; if(!cameraName.isEmpty())
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)
{ {
if(pointBuf[i].x < border || pointBuf[i].x > image.cols-border || cameraName_ = cameraName;
pointBuf[i].y < border || pointBuf[i].y > image.rows-border) }
}
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; cvtColor(images[id], viewGray, cv::COLOR_BGR2GRAY);
break; }
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; 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) if(minSquareDistance == -1.0f || minSquareDistance > d)
{ {
minSquareDistance = d; minSquareDistance = d;
} }
} }
float radius = minSquareDistance/2.0f +0.5f; 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 )); cv::TermCriteria( CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1 ));
// verify if view is different from any previous samples boardAccepted[id] = true;
std::vector<float> params(4, 0); getParams(pointBuf[id], boardSize_, imageSize_[id], params[id][0], params[id][1], params[id][2], params[id][3]);
getParams(pointBuf, boardSize_, imageSize_, params[0], params[1], params[2], params[3]);
bool add = true; // Draw the corners.
for(unsigned int i=0; i<imageParams_.size(); ++i) 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 if(fabs(params[id][0] - imageParams_[id][i].at(0)) < 0.1 && // x
fabs(params[1] - imageParams_[i].at(1)) < 0.1 && // y fabs(params[id][1] - imageParams_[id][i].at(1)) < 0.1 && // y
fabs(params[2] - imageParams_[i].at(2)) < 0.1 && // size fabs(params[id][2] - imageParams_[id][i].at(2)) < 0.05 && // size
fabs(params[3] - imageParams_[i].at(3)) < 0.1) // skew 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]); break;
imagePoints_.push_back(pointBuf); }
imageParams_.push_back(params); }
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 // update statistics
std::vector<float> xRange(2, imageParams_[0].at(0)); std::vector<float> xRange(2, imageParams_[id][0].at(0));
std::vector<float> yRange(2, imageParams_[0].at(1)); std::vector<float> yRange(2, imageParams_[id][0].at(1));
std::vector<float> sizeRange(2, imageParams_[0].at(2)); std::vector<float> sizeRange(2, imageParams_[id][0].at(2));
std::vector<float> skewRange(2, imageParams_[0].at(3)); std::vector<float> skewRange(2, imageParams_[id][0].at(3));
for(unsigned int i=1; i<imageParams_.size(); ++i) 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[0] = imageParams_[id][i].at(0) < xRange[0] ? imageParams_[id][i].at(0) : xRange[0];
xRange[1] = imageParams_[i].at(0) > xRange[1] ? imageParams_[i].at(0) : xRange[1]; xRange[1] = imageParams_[id][i].at(0) > xRange[1] ? imageParams_[id][i].at(0) : xRange[1];
yRange[0] = imageParams_[i].at(1) < yRange[0] ? imageParams_[i].at(1) : yRange[0]; yRange[0] = imageParams_[id][i].at(1) < yRange[0] ? imageParams_[id][i].at(1) : yRange[0];
yRange[1] = imageParams_[i].at(1) > yRange[1] ? imageParams_[i].at(1) : yRange[1]; yRange[1] = imageParams_[id][i].at(1) > yRange[1] ? imageParams_[id][i].at(1) : yRange[1];
sizeRange[0] = imageParams_[i].at(2) < sizeRange[0] ? imageParams_[i].at(2) : sizeRange[0]; sizeRange[0] = imageParams_[id][i].at(2) < sizeRange[0] ? imageParams_[id][i].at(2) : sizeRange[0];
sizeRange[1] = imageParams_[i].at(2) > sizeRange[1] ? imageParams_[i].at(2) : sizeRange[1]; sizeRange[1] = imageParams_[id][i].at(2) > sizeRange[1] ? imageParams_[id][i].at(2) : sizeRange[1];
skewRange[0] = imageParams_[i].at(3) < skewRange[0] ? imageParams_[i].at(3) : skewRange[0]; skewRange[0] = imageParams_[id][i].at(3) < skewRange[0] ? imageParams_[id][i].at(3) : skewRange[0];
skewRange[1] = imageParams_[i].at(3) > skewRange[1] ? imageParams_[i].at(3) : skewRange[1]; skewRange[1] = imageParams_[id][i].at(3) > skewRange[1] ? imageParams_[id][i].at(3) : skewRange[1];
} }
UINFO("Stats:"); UINFO("Stats:");
UINFO(" Count = %d", (int)imagePoints_.size()); UINFO(" Count = %d", (int)imagePoints_[id].size());
UINFO(" x = [%f -> %f]", xRange[0], xRange[1]); UINFO(" x = [%f -> %f]", xRange[0], xRange[1]);
UINFO(" y = [%f -> %f]", yRange[0], yRange[1]); UINFO(" y = [%f -> %f]", yRange[0], yRange[1]);
UINFO(" size = [%f -> %f]", sizeRange[0], sizeRange[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 sizeGood = sizeRange[1] - sizeRange[0];
float skewGood = skewRange[1] - skewRange[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()); else
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)
{ {
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. if((int)imagePoints_[0].size() > ui_->progressBar_count->maximum())
cv::drawChessboardCorners( image, boardSize_, cv::Mat(pointBuf), found ); {
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()) if(calibrated_ && ui_->checkBox_rectified->isChecked())
{ {
cv::Mat temp = image.clone(); if(!stereo_)
cv::undistort(temp, image, cameraMatrix_, distCoeffs_); {
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 //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() void CalibrationDialog::restart()
{ {
// restart // restart
calibrated_ = false; calibrated_ = false;
imagePoints_.clear(); imagePoints_[0].clear();
imageParams_.clear(); imagePoints_[1].clear();
imageParams_[0].clear();
imageParams_[1].clear();
model_ = CameraModel();
stereoModel_ = StereoCameraModel();
cameraName_.clear();
ui_->pushButton_calibrate->setEnabled(false); ui_->pushButton_calibrate->setEnabled(false);
ui_->pushButton_save->setEnabled(false);
ui_->buttonBox->button(QDialogButtonBox::Ok)->setEnabled(false); ui_->buttonBox->button(QDialogButtonBox::Ok)->setEnabled(false);
ui_->checkBox_rectified->setEnabled(false); ui_->checkBox_rectified->setEnabled(false);
//ui_->pushButton_save->setEnabled(false);
ui_->progressBar_count->reset(); ui_->progressBar_count->reset();
ui_->progressBar_count->setMaximum(COUNT_MIN); ui_->progressBar_count->setMaximum(COUNT_MIN);
@@ -267,116 +412,282 @@ void CalibrationDialog::restart()
ui_->progressBar_size->reset(); ui_->progressBar_size->reset();
ui_->progressBar_skew->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_fx->setNum(0);
ui_->label_fy->setNum(0); ui_->label_fy->setNum(0);
ui_->label_cx->setNum(0); ui_->label_cx->setNum(0);
ui_->label_cy->setNum(0); ui_->label_cy->setNum(0);
ui_->label_baseline->setNum(0);
ui_->label_error->setNum(0); ui_->label_error->setNum(0);
ui_->lineEdit_K->clear(); ui_->lineEdit_K->clear();
ui_->lineEdit_D->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() void CalibrationDialog::calibrate()
{ {
//calibrate UINFO("Calibration...");
std::vector<cv::Mat> rvecs, tvecs; processingData_ = true;
std::vector<float> reprojErrs;
double totalAvgErr = 0;
std::vector<std::vector<cv::Point3f> > objectPoints(1); std::vector<std::vector<cv::Point3f> > objectPoints(1);
// compute board corner positions // 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 ) for( int j = 0; j < boardSize_.width; ++j )
objectPoints[0].push_back(cv::Point3f(float( j*squareSize_ ), float( i*squareSize_ ), 0)); 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 if(!stereo_)
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 )
{ {
cv::projectPoints( cv::Mat(objectPoints[i]), rvecs[i], tvecs[i], cameraMatrix_, //calibrate
distCoeffs_, imagePoints2); std::vector<cv::Mat> rvecs, tvecs;
err = cv::norm(cv::Mat(imagePoints_[i]), cv::Mat(imagePoints2), CV_L2); std::vector<float> reprojErrs;
cv::Mat K, D;
K = cv::Mat::eye(3,3,CV_64FC1);
int n = (int)objectPoints[i].size(); //Find intrinsic and extrinsic camera parameters
reprojErrs[i] = (float) std::sqrt(err*err/n); double rms = cv::calibrateCamera(objectPoints,
totalErr += err*err; imagePoints_[0],
totalPoints += n; 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); processingData_ = false;
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);
}
} }
void CalibrationDialog::save() void CalibrationDialog::save()
{ {
QString fileName = QFileDialog::getSaveFileName(this, tr("Export"), "calibration.yaml", "*.yaml"); processingData_ = true;
if(!stereo_)
if(!fileName.isEmpty())
{ {
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 if(!filePath.isEmpty())
fs << "camera_matrix" << "{"; {
fs << "rows" << cameraMatrix_.rows; if(model_.save(filePath.toStdString()))
fs << "cols" << cameraMatrix_.cols; {
fs << "data" << std::vector<double>((double*)cameraMatrix_.data, ((double*)cameraMatrix_.data)+(cameraMatrix_.rows*cameraMatrix_.cols)); QMessageBox::information(this, tr("Export"), tr("Calibration file saved to \"%1\".").arg(filePath));
fs << "}"; UINFO("Saved \"%s\"!", filePath.toStdString().c_str());
}
fs << "distortion_coefficients" << "{"; else
fs << "rows" << distCoeffs_.rows; {
fs << "cols" << distCoeffs_.cols; UERROR("Error saving \"%s\"", filePath.toStdString().c_str());
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());
} }
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) 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> <rect>
<x>0</x> <x>0</x>
<y>0</y> <y>0</y>
<width>1040</width> <width>875</width>
<height>745</height> <height>834</height>
</rect> </rect>
</property> </property>
<property name="windowTitle"> <property name="windowTitle">
@@ -15,11 +15,18 @@
</property> </property>
<layout class="QVBoxLayout" name="verticalLayout_3"> <layout class="QVBoxLayout" name="verticalLayout_3">
<item> <item>
<layout class="QHBoxLayout" name="horizontalLayout_3" stretch="1,0"> <layout class="QHBoxLayout" name="horizontalLayout_4" stretch="1,0">
<item> <item>
<layout class="QVBoxLayout" name="verticalLayout_2" stretch="1,0"> <layout class="QVBoxLayout" name="verticalLayout_2" stretch="1,0">
<item> <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>
<item> <item>
<layout class="QHBoxLayout" name="horizontalLayout_2"> <layout class="QHBoxLayout" name="horizontalLayout_2">
@@ -37,6 +44,13 @@
</property> </property>
</widget> </widget>
</item> </item>
<item>
<widget class="QCheckBox" name="checkBox_showHorizontalLines">
<property name="text">
<string>Show horizontal lines</string>
</property>
</widget>
</item>
<item> <item>
<spacer name="horizontalSpacer"> <spacer name="horizontalSpacer">
<property name="orientation"> <property name="orientation">
@@ -122,6 +136,23 @@
</property> </property>
</widget> </widget>
</item> </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> </layout>
</widget> </widget>
</item> </item>
@@ -130,11 +161,39 @@
<property name="title"> <property name="title">
<string>Progress</string> <string>Progress</string>
</property> </property>
<layout class="QFormLayout" name="formLayout"> <layout class="QGridLayout" name="gridLayout">
<item row="0" column="0"> <item row="1" column="0">
<widget class="QLabel" name="label_5"> <widget class="QLabel" name="label">
<property name="text"> <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> </property>
</widget> </widget>
</item> </item>
@@ -148,13 +207,6 @@
</property> </property>
</widget> </widget>
</item> </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"> <item row="1" column="1">
<widget class="QProgressBar" name="progressBar_x"> <widget class="QProgressBar" name="progressBar_x">
<property name="value"> <property name="value">
@@ -165,13 +217,6 @@
</property> </property>
</widget> </widget>
</item> </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"> <item row="2" column="1">
<widget class="QProgressBar" name="progressBar_y"> <widget class="QProgressBar" name="progressBar_y">
<property name="value"> <property name="value">
@@ -186,25 +231,57 @@
</property> </property>
</widget> </widget>
</item> </item>
<item row="3" column="1"> <item row="0" column="0">
<widget class="QProgressBar" name="progressBar_size"> <widget class="QLabel" name="label_5">
<property name="value">
<number>24</number>
</property>
</widget>
</item>
<item row="4" column="0">
<widget class="QLabel" name="label_4">
<property name="text"> <property name="text">
<string>Skew</string> <string>Count</string>
</property> </property>
</widget> </widget>
</item> </item>
<item row="4" column="1"> <item row="1" column="2">
<widget class="QProgressBar" name="progressBar_skew"> <widget class="QProgressBar" name="progressBar_x_2">
<property name="value"> <property name="value">
<number>24</number> <number>24</number>
</property> </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> </widget>
</item> </item>
</layout> </layout>
@@ -222,34 +299,7 @@
<property name="title"> <property name="title">
<string>Camera intrinsic parameters</string> <string>Camera intrinsic parameters</string>
</property> </property>
<layout class="QFormLayout" name="formLayout_2"> <layout class="QGridLayout" name="gridLayout_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>
<item row="1" column="1"> <item row="1" column="1">
<widget class="QLabel" name="label_fy"> <widget class="QLabel" name="label_fy">
<property name="text"> <property name="text">
@@ -260,10 +310,10 @@
</property> </property>
</widget> </widget>
</item> </item>
<item row="2" column="0"> <item row="5" column="1">
<widget class="QLabel" name="label_10"> <widget class="QLineEdit" name="lineEdit_K">
<property name="text"> <property name="readOnly">
<string>cx</string> <bool>true</bool>
</property> </property>
</widget> </widget>
</item> </item>
@@ -294,7 +344,7 @@
</property> </property>
</widget> </widget>
</item> </item>
<item row="4" column="0"> <item row="5" column="0">
<widget class="QLabel" name="label_6"> <widget class="QLabel" name="label_6">
<property name="toolTip"> <property name="toolTip">
<string>Camera matrix</string> <string>Camera matrix</string>
@@ -304,14 +354,62 @@
</property> </property>
</widget> </widget>
</item> </item>
<item row="4" column="1"> <item row="0" column="1">
<widget class="QLineEdit" name="lineEdit_K"> <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"> <property name="readOnly">
<bool>true</bool> <bool>true</bool>
</property> </property>
</widget> </widget>
</item> </item>
<item row="5" column="0"> <item row="6" column="0">
<widget class="QLabel" name="label_7"> <widget class="QLabel" name="label_7">
<property name="toolTip"> <property name="toolTip">
<string>Distorsion coefficients</string> <string>Distorsion coefficients</string>
@@ -321,14 +419,14 @@
</property> </property>
</widget> </widget>
</item> </item>
<item row="5" column="1"> <item row="6" column="1">
<widget class="QLineEdit" name="lineEdit_D"> <widget class="QLineEdit" name="lineEdit_D">
<property name="readOnly"> <property name="readOnly">
<bool>true</bool> <bool>true</bool>
</property> </property>
</widget> </widget>
</item> </item>
<item row="6" column="0"> <item row="9" column="0">
<widget class="QLabel" name="label_14"> <widget class="QLabel" name="label_14">
<property name="toolTip"> <property name="toolTip">
<string>Avg. reproduction error</string> <string>Avg. reproduction error</string>
@@ -338,17 +436,112 @@
</property> </property>
</widget> </widget>
</item> </item>
<item row="6" column="1"> <item row="9" column="1">
<widget class="QLabel" name="label_error"> <widget class="QLabel" name="label_error">
<property name="text"> <property name="text">
<string>0</string> <string>0</string>
</property> </property>
</widget> </widget>
</item> </item>
<item row="7" column="0" colspan="2"> <item row="0" column="2">
<widget class="QLabel" name="label_13"> <widget class="QLabel" name="label_fx_2">
<property name="text"> <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> </property>
</widget> </widget>
</item> </item>
guilib/src/utilite/UImageView.h
-5
View File
@@ -22,11 +22,6 @@ public:
public slots: public slots:
void setImage(const QImage & image) 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); pixmap_ = QPixmap::fromImage(image);
this->update(); this->update();
} }
tools/Calibration/main.cpp
+59 -23
View File
@@ -38,9 +38,17 @@ void showUsage()
printf("\nUsage:\n" printf("\nUsage:\n"
"rtabmap-calibration [options]\n" "rtabmap-calibration [options]\n"
"Options:\n" "Options:\n"
" --driver # Driver number to use: 0=USB camera, 1=OpenNI-PCL, 2=OpenNI2,\n" " --driver # Driver number to use:-1=USB camera\n"
" 3=Freenect, 4=OpenNI-CV, 5=OpenNI-CV-ASUS\n" " 0=OpenNI-PCL (Kinect)\n"
" --device # Device id\n\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); exit(1);
} }
@@ -49,8 +57,9 @@ int main(int argc, char * argv[])
ULogger::setType(ULogger::kTypeConsole); ULogger::setType(ULogger::kTypeConsole);
ULogger::setLevel(ULogger::kInfo); ULogger::setLevel(ULogger::kInfo);
int driver = 0; int driver = -1;
int device = 0; int device = 0;
bool stereo = false;
for(int i=1; i<argc; ++i) for(int i=1; i<argc; ++i)
{ {
if(strcmp(argv[i], "--driver") == 0) if(strcmp(argv[i], "--driver") == 0)
@@ -59,7 +68,7 @@ int main(int argc, char * argv[])
if(i < argc) if(i < argc)
{ {
driver = std::atoi(argv[i]); driver = std::atoi(argv[i]);
if(driver < 0) if(driver < -1)
{ {
showUsage(); showUsage();
} }
@@ -87,6 +96,16 @@ int main(int argc, char * argv[])
} }
continue; 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) if(strcmp(argv[i], "--help") == 0)
{ {
showUsage(); showUsage();
@@ -94,44 +113,52 @@ int main(int argc, char * argv[])
printf("Unrecognized option : %s\n", argv[i]); printf("Unrecognized option : %s\n", argv[i]);
showUsage(); showUsage();
} }
driver = atoi(argv[argc-1]); if(driver < -1 || driver > 6)
if(driver < 0 || driver > 5)
{ {
UERROR("driver should be between 0 and 5."); UERROR("driver should be between -1 and 6.");
showUsage(); showUsage();
} }
UINFO("Using driver %d", driver); UINFO("Using driver %d", driver);
UINFO("Using device %d", device); UINFO("Using device %d", device);
UINFO("Stereo: %s", stereo?"true":"false");
float imageRate = 1.0f;
rtabmap::Camera * cameraUsb = 0; rtabmap::Camera * cameraUsb = 0;
rtabmap::CameraRGBD * camera = 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) else if(driver == 1)
{
camera = new rtabmap::CameraOpenni(uNumber2Str(device), imageRate);
}
else if(driver == 2)
{ {
if(!rtabmap::CameraOpenNI2::available()) if(!rtabmap::CameraOpenNI2::available())
{ {
UERROR("Not built with OpenNI2 support..."); UERROR("Not built with OpenNI2 support...");
exit(-1); 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()) if(!rtabmap::CameraFreenect::available())
{ {
UERROR("Not built with Freenect support..."); UERROR("Not built with Freenect support...");
exit(-1); 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) else if(driver == 4)
{ {
@@ -140,16 +167,25 @@ int main(int argc, char * argv[])
UERROR("Not built with OpenNI from OpenCV support..."); UERROR("Not built with OpenNI from OpenCV support...");
exit(-1); exit(-1);
} }
camera = new rtabmap::CameraOpenNICV(false, imageRate); camera = new rtabmap::CameraOpenNICV(true);
} }
else if(driver == 5) 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); 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 else
{ {
@@ -180,7 +216,7 @@ int main(int argc, char * argv[])
} }
QApplication app(argc, argv); QApplication app(argc, argv);
rtabmap::CalibrationDialog dialog; rtabmap::CalibrationDialog dialog(stereo);
dialog.registerToEventsManager(); dialog.registerToEventsManager();
dialog.show(); dialog.show();