Added stereo option for the CalibrationDialog class

2026-09-01 17:10:26 +08:00 · 2015-04-05 16:42:41 -04:00
parent 6a0e48a6fa
commit fee32c4e25
14 changed files with 1075 additions and 318 deletions
--- a/corelib/include/rtabmap/core/CameraEvent.h
+++ b/corelib/include/rtabmap/core/CameraEvent.h
@@ -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
--- a/corelib/include/rtabmap/core/CameraModel.h
+++ b/corelib/include/rtabmap/core/CameraModel.h
@@ -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 */
--- a/corelib/include/rtabmap/core/CameraRGBD.h
+++ b/corelib/include/rtabmap/core/CameraRGBD.h
@@ -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_;
--- a/corelib/src/CameraModel.cpp
+++ b/corelib/src/CameraModel.cpp
@@ -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
--- a/corelib/src/CameraRGBD.cpp
+++ b/corelib/src/CameraRGBD.cpp
@@ -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
--- a/corelib/src/CameraThread.cpp
+++ b/corelib/src/CameraThread.cpp
@@ -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
 		{
--- a/corelib/src/Memory.cpp
+++ b/corelib/src/Memory.cpp
@@ -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);
--- a/corelib/src/Odometry.cpp
+++ b/corelib/src/Odometry.cpp
@@ -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);

--- a/corelib/src/SensorData.cpp
+++ b/corelib/src/SensorData.cpp
@@ -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)");
 }