Added CameraDC1394, CameraModel and StereoCameraModel classes

corelib/include/rtabmap/core/CameraRGBD.h

@@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/SensorData.h"
 #include "rtabmap/utilite/UMutex.h"
 #include "rtabmap/utilite/USemaphore.h"
+#include "rtabmap/core/CameraModel.h"
 #include <set>
 #include <stack>
 #include <list>
@@ -112,6 +113,9 @@ protected:
 				float cx = 0.0f,
 				float cy = 0.0f);
 
+	/**
+	 * returned rgb and depth images should be already rectified
+	 */
 	virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy) = 0;
 
 private:
@@ -305,4 +309,37 @@ private:
 	libfreenect2::SyncMultiFrameListener * listener_;
 };
 
+/////////////////////////
+// CameraDC1394
+/////////////////////////
+class DC1394Device;
+
+class RTABMAP_EXP CameraDC1394 :
+	public CameraRGBD
+{
+public:
+	static bool available();
+
+public:
+	// default local transform z in, x right, y down));
+	CameraDC1394(const std::string & calibrationFolder,
+					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);
+	virtual ~CameraDC1394();
+
+	bool init();
+
+protected:
+	virtual void captureImage(cv::Mat & left, cv::Mat & right, float & fx, float & baseline, float & cx, float & cy);
+
+private:
+	std::string calibrationFolder_;
+	DC1394Device *device_;
+	StereoCameraModel stereoModel_;
+};
+
 } // namespace rtabmap

corelib/src/CMakeLists.txt

@@ -14,6 +14,7 @@ SET(SRC_FILES
     Camera.cpp
     CameraThread.cpp
     CameraRGBD.cpp
+    CameraModel.cpp
     
     EpipolarGeometry.cpp
 	VisualWord.cpp

corelib/src/CameraRGBD.cpp

@@ -36,8 +36,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UDirectory.h>
 #include <rtabmap/utilite/UTimer.h>
 
-#include <opencv2/imgproc/imgproc.hpp>
-
 #include <pcl/io/openni_grabber.h>
 
 #include <cmath>
@@ -56,6 +54,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <libfreenect2/frame_listener_impl.h>
 #endif
 
+#ifdef WITH_DC1394
+#include <dc1394/dc1394.h>
+#endif
+
 #ifdef WITH_OPENNI2
 #include <OniVersion.h>
 #include <OpenNI.h>
@@ -1087,4 +1089,363 @@ void CameraFreenect2::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, f
 #endif
 }
 
+//
+// CameraDC1394
+// Inspired from ROS camera1394stereo package
+//
+
+#ifdef WITH_DC1394
+class DC1394Device
+{
+public:
+	DC1394Device() :
+		camera_(0),
+		context_(0)
+	{
+
+	}
+	~DC1394Device()
+	{
+		if (camera_)
+		{
+			if (DC1394_SUCCESS != dc1394_video_set_transmission(camera_, DC1394_OFF) ||
+				DC1394_SUCCESS != dc1394_capture_stop(camera_))
+			{
+				UWARN("unable to stop camera");
+			}
+
+			// Free resources
+			dc1394_capture_stop(camera_);
+			dc1394_camera_free(camera_);
+			camera_ = NULL;
+		}
+		if(context_)
+		{
+			dc1394_free(context_);
+			context_ = NULL;
+		}
+	}
+
+	const std::string & guid() const {return guid_;}
+
+	bool init()
+	{
+		if(camera_)
+		{
+			// Free resources
+			dc1394_capture_stop(camera_);
+			dc1394_camera_free(camera_);
+			camera_ = NULL;
+		}
+
+		// look for a camera
+		int err;
+		if(context_ == NULL)
+		{
+			context_ = dc1394_new ();
+			if (context_ == NULL)
+			{
+				UERROR(    "Could not initialize dc1394_context.\n"
+				"Make sure /dev/raw1394 exists, you have access permission,\n"
+				"and libraw1394 development package is installed.");
+				return false;
+			}
+		}
+
+		dc1394camera_list_t *list;
+		err = dc1394_camera_enumerate(context_, &list);
+		if (err != DC1394_SUCCESS)
+		{
+			UERROR("Could not get camera list");
+			return false;
+		}
+
+		if (list->num == 0)
+		{
+			UERROR("No cameras found");
+			dc1394_camera_free_list (list);
+			return false;
+		}
+		uint64_t guid = list->ids[0].guid;
+		dc1394_camera_free_list (list);
+
+		// Create a camera
+		camera_ = dc1394_camera_new (context_, guid);
+		if (!camera_)
+		{
+			UERROR("Failed to initialize camera with GUID [%016lx]", guid);
+			return false;
+		}
+
+		uint32_t value[3];
+		value[0]= camera_->guid & 0xffffffff;
+		value[1]= (camera_->guid >>32) & 0x000000ff;
+		value[2]= (camera_->guid >>40) & 0xfffff;
+		guid_ = uFormat("%06x%02x%08x", value[2], value[1], value[0]);
+
+		UINFO("camera model: %s %s", camera_->vendor, camera_->model);
+
+		// initialize camera
+		// Enable IEEE1394b mode if the camera and bus support it
+		bool bmode = camera_->bmode_capable;
+		if (bmode
+			&& (DC1394_SUCCESS !=
+			dc1394_video_set_operation_mode(camera_,
+									DC1394_OPERATION_MODE_1394B)))
+		{
+			bmode = false;
+			UWARN("failed to set IEEE1394b mode");
+		}
+
+		// start with highest speed supported
+		dc1394speed_t request = DC1394_ISO_SPEED_3200;
+		int rate = 3200;
+		if (!bmode)
+		{
+			// not IEEE1394b capable: so 400Mb/s is the limit
+			request = DC1394_ISO_SPEED_400;
+			rate = 400;
+		}
+
+		// round requested speed down to next-lower defined value
+		while (rate > 400)
+		{
+			if (request <= DC1394_ISO_SPEED_MIN)
+			{
+				// get current ISO speed of the device
+				dc1394speed_t curSpeed;
+				if (DC1394_SUCCESS == dc1394_video_get_iso_speed(camera_, &curSpeed) && curSpeed <= DC1394_ISO_SPEED_MAX)
+				{
+					// Translate curSpeed back to an int for the parameter
+					// update, works as long as any new higher speeds keep
+					// doubling.
+					request = curSpeed;
+					rate = 100 << (curSpeed - DC1394_ISO_SPEED_MIN);
+				}
+				else
+				{
+					UWARN("Unable to get ISO speed; assuming 400Mb/s");
+					rate = 400;
+					request = DC1394_ISO_SPEED_400;
+				}
+				break;
+			}
+			// continue with next-lower possible value
+			request = (dc1394speed_t) ((int) request - 1);
+			rate = rate / 2;
+		}
+
+		// set the requested speed
+		if (DC1394_SUCCESS != dc1394_video_set_iso_speed(camera_, request))
+		{
+			UERROR("Failed to set iso speed");
+			return false;
+		}
+
+		// set video mode
+		dc1394video_modes_t vmodes;
+		err = dc1394_video_get_supported_modes(camera_, &vmodes);
+		if (err != DC1394_SUCCESS)
+		{
+			UERROR("unable to get supported video modes");
+			return (dc1394video_mode_t) 0;
+		}
+
+		// see if requested mode is available
+		bool found = false;
+		dc1394video_mode_t videoMode = DC1394_VIDEO_MODE_FORMAT7_3; // bumblebee
+		for (uint32_t i = 0; i < vmodes.num; ++i)
+		{
+			if (vmodes.modes[i] == videoMode)
+			{
+				found = true;
+			}
+		}
+		if(!found)
+		{
+			UERROR("unable to get video mode %d", videoMode);
+			return false;
+		}
+
+		if (DC1394_SUCCESS != dc1394_video_set_mode(camera_, videoMode))
+		{
+			UERROR("Failed to set video mode %d", videoMode);
+			return false;
+		}
+
+		// special handling for Format7 modes
+		if (dc1394_is_video_mode_scalable(videoMode) == DC1394_TRUE)
+		{
+			if (DC1394_SUCCESS != dc1394_format7_set_color_coding(camera_, videoMode, DC1394_COLOR_CODING_RAW16))
+			{
+				UERROR("Could not set color coding");
+				return false;
+			}
+			uint32_t packetSize;
+			if (DC1394_SUCCESS != dc1394_format7_get_recommended_packet_size(camera_, videoMode, &packetSize))
+			{
+				UERROR("Could not get default packet size");
+				return false;
+			}
+
+			if (DC1394_SUCCESS != dc1394_format7_set_packet_size(camera_, videoMode, packetSize))
+			{
+				UERROR("Could not set packet size");
+				return false;
+			}
+		}
+		else
+		{
+			UERROR("Video is not in mode scalable");
+		}
+
+		// start the device streaming data
+		// Set camera to use DMA, improves performance.
+		if (DC1394_SUCCESS != dc1394_capture_setup(camera_, 4, DC1394_CAPTURE_FLAGS_DEFAULT))
+		{
+			UERROR("Failed to open device!");
+			return false;
+		}
+
+		// Start transmitting camera data
+		if (DC1394_SUCCESS != dc1394_video_set_transmission(camera_, DC1394_ON))
+		{
+			UERROR("Failed to start device!");
+			return false;
+		}
+
+		return true;
+	}
+
+	bool getImages(cv::Mat & left, cv::Mat & right)
+	{
+		if(camera_)
+		{
+			dc1394video_frame_t * frame = NULL;
+			UDEBUG("[%016lx] waiting camera", camera_->guid);
+			dc1394_capture_dequeue (camera_, DC1394_CAPTURE_POLICY_WAIT, &frame);
+			if (!frame)
+			{
+				UERROR("Unable to capture frame");
+				return false;
+			}
+			dc1394video_frame_t frame1 = *frame;
+			// deinterlace frame into two images one on top the other
+			size_t frame1_size = frame->total_bytes;
+			frame1.image = (unsigned char *) malloc(frame1_size);
+			frame1.allocated_image_bytes = frame1_size;
+			frame1.color_coding = DC1394_COLOR_CODING_RAW8;
+			int err = dc1394_deinterlace_stereo_frames(frame, &frame1, DC1394_STEREO_METHOD_INTERLACED);
+			if (err != DC1394_SUCCESS)
+			{
+				free(frame1.image);
+				dc1394_capture_enqueue(camera_, frame);
+				UERROR("Could not extract stereo frames");
+				return false;
+			}
+
+			uint8_t* capture_buffer = reinterpret_cast<uint8_t *>(frame1.image);
+			UASSERT(capture_buffer);
+
+			cv::Mat image(frame->size[1], frame->size[0], CV_8UC3);
+			cv::Mat image2 = image.clone();
+
+			//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);
+
+			dc1394_capture_enqueue(camera_, frame);
+
+			free(frame1.image);
+
+			return true;
+		}
+		return false;
+	}
+
+private:
+	dc1394camera_t *camera_;
+	dc1394_t *context_;
+	std::string guid_;
+};
+#endif
+
+bool CameraDC1394::available()
+{
+#ifdef WITH_DC1394
+	return true;
+#else
+	return false;
+#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),
+		calibrationFolder_(calibrationFolder),
+		device_(0)
+{
+#ifdef WITH_DC1394
+	device_ = new DC1394Device();
+#endif
+}
+
+CameraDC1394::~CameraDC1394()
+{
+#ifdef WITH_DC1394
+	if(device_)
+	{
+		delete device_;
+	}
+#endif
+}
+
+bool CameraDC1394::init()
+{
+#ifdef WITH_DC1394
+	if(device_)
+	{
+		bool ok = device_->init();
+		if(ok)
+		{
+			// look for calibration files
+			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;
+	}
+#else
+	UERROR("CameraDC1394: RTAB-Map is not built with dc1394 support!");
+#endif
+	return false;
+}
+
+void CameraDC1394::captureImage(cv::Mat & left, cv::Mat & right, float & fx, float & baseline, float & cx, float & cy)
+{
+#ifdef WITH_DC1394
+	left = cv::Mat();
+	right = cv::Mat();
+	fx = 0.0f;
+	baseline = 0.0f;
+	cx = 0.0f;
+	cy = 0.0f;
+	if(device_)
+	{
+		device_->getImages(left, right);
+
+		// Rectification
+		left = stereoModel_.left().rectifyImage(left);
+		right = stereoModel_.right().rectifyImage(right);
+		fx = stereoModel_.left().fx();
+		cx = stereoModel_.left().cx();
+		cy = stereoModel_.left().cy();
+		baseline = stereoModel_.baseline();
+	}
+#else
+	UERROR("CameraDC1394: RTAB-Map is not built with dc1394 support!");
+#endif
+}
+
 } // namespace rtabmap

corelib/src/util3d.cpp

@@ -849,10 +849,18 @@ cv::Mat disparityFromStereoImages(
 		leftMono = leftImage;
 	}
 
-	cv::StereoBM stereo(cv::StereoBM::BASIC_PRESET, 0, 15);
+	cv::StereoBM stereo(cv::StereoBM::BASIC_PRESET);
+	stereo.state->SADWindowSize = 15;
+	stereo.state->minDisparity = 0;
+	stereo.state->numberOfDisparities = 64;
+	stereo.state->preFilterSize = 9;
+	stereo.state->preFilterCap = 31;
+	stereo.state->uniquenessRatio = 15;
+	stereo.state->textureThreshold = 10;
+	stereo.state->speckleWindowSize = 100;
+	stereo.state->speckleRange = 4;
 	cv::Mat disparity;
 	stereo(leftMono, rightImage, disparity, CV_16SC1);
-	cv::filterSpeckles(disparity, 0, 1000, 16);
 	return disparity;
 }
 

tools/CameraRGBD/main.cpp

@@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/util3d.h"
 #include "rtabmap/utilite/ULogger.h"
 #include <opencv2/highgui/highgui.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
 #include <pcl/visualization/cloud_viewer.h>
 #include <stdio.h>
 
@@ -36,12 +37,13 @@ void showUsage()
 {
 	printf("\nUsage:\n"
 			"rtabmap-rgbd_camera driver\n"
-			"  driver       Driver number to use: 0=OpenNI-PCL\n"
+			"  driver       Driver number to use: 0=OpenNI-PCL (Kinect)\n"
-			"                                     1=OpenNI2\n"
+			"                                     1=OpenNI2    (Kinect and Xtion PRO Live)\n"
-			"                                     2=Freenect\n"
+			"                                     2=Freenect   (Kinect)\n"
-			"                                     3=OpenNI-CV\n"
+			"                                     3=OpenNI-CV  (Kinect)\n"
-			"                                     4=OpenNI-CV-ASUS\n"
+			"                                     4=OpenNI-CV-ASUS (Xtion PRO Live)\n"
-			"                                     5=Freenect2\n\n");
+			"                                     5=Freenect2  (Kinect v2)\n"
+			"                                     6=DC1394     (Bumblebee2)\n\n");
 	exit(1);
 }
 
@@ -58,9 +60,9 @@ int main(int argc, char * argv[])
 	else
 	{
 		driver = atoi(argv[argc-1]);
-		if(driver < 0 || driver > 5)
+		if(driver < 0 || driver > 6)
 		{
-			UERROR("driver should be between 0 and 5.");
+			UERROR("driver should be between 0 and 6.");
 			showUsage();
 		}
 	}
@@ -116,6 +118,15 @@ int main(int argc, char * argv[])
 		}
 		camera = new rtabmap::CameraFreenect2();
 	}
+	else if(driver == 6)
+	{
+		if(!rtabmap::CameraDC1394::available())
+		{
+			UERROR("Not built with DC1394 support...");
+			exit(-1);
+		}
+		camera = new rtabmap::CameraDC1394("camera_info");
+	}
 	else
 	{
 		UFATAL("");
@@ -135,27 +146,52 @@ int main(int argc, char * argv[])
 		UWARN("RGB (%d/%d) and depth (%d/%d) frames are not the same size! The registered cloud cannot be shown.",
 				rgb.cols, rgb.rows, depth.cols, depth.rows);
 	}
-	cv::namedWindow("Video", CV_WINDOW_AUTOSIZE); // create window
+	if(!fx || !fy)
-	cv::namedWindow("Depth", CV_WINDOW_AUTOSIZE); // create window
+	{
+		UWARN("fx and/or fy are not set! The registered cloud cannot be shown.");
+	}
 	pcl::visualization::CloudViewer viewer("cloud");
-	rtabmap::Transform opticalTransform(0,0,1,0, -1,0,0,0, 0,-1,0,0);
+	rtabmap::Transform t(1, 0, 0, 0,
+						 0, -1, 0, 0,
+						 0, 0, -1, 0);
 	while(!rgb.empty() && !viewer.wasStopped())
 	{
-		if(depth.type() == CV_32FC1)
+		if(depth.type() == CV_16UC1 || depth.type() == CV_32FC1)
 		{
-			depth = rtabmap::util3d::cvtDepthFromFloat(depth);
+			// depth
+			if(depth.type() == CV_32FC1)
+			{
+				depth = rtabmap::util3d::cvtDepthFromFloat(depth);
+			}
+			cv::Mat tmp;
+			depth.convertTo(tmp, CV_8UC1, 255.0/2048.0);
+
+			cv::imshow("Video", rgb); // show frame
+			cv::imshow("Depth", tmp);
+
+			if(rgb.cols == depth.cols && rgb.rows == depth.rows && fx && fy)
+			{
+				pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = rtabmap::util3d::cloudFromDepthRGB(rgb, depth, cx, cy, fx, fy);
+				cloud = rtabmap::util3d::transformPointCloud<pcl::PointXYZRGB>(cloud, t);
+				viewer.showCloud(cloud, "cloud");
+			}
 		}
-		cv::Mat tmp;
+		else
-		depth.convertTo(tmp, CV_8UC1, 255.0/2048.0);
-
-		cv::imshow("Video", rgb); // show frame
-		cv::imshow("Depth", tmp);
-
-		if(rgb.cols == depth.cols && rgb.rows == depth.rows)
 		{
-			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = rtabmap::util3d::cloudFromDepthRGB(rgb, depth, cx, cy, fx, fy);
+			// stereo
-			cloud = rtabmap::util3d::transformPointCloud<pcl::PointXYZRGB>(cloud, opticalTransform);
+			cv::imshow("Left", rgb); // show frame
-			viewer.showCloud(cloud, "cloud");
+			cv::imshow("Right", depth);
+
+			if(rgb.cols == depth.cols && rgb.rows == depth.rows && fx && fy)
+			{
+				if(depth.channels() == 3)
+				{
+					cv::cvtColor(depth, depth, CV_BGR2GRAY);
+				}
+				pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = rtabmap::util3d::cloudFromStereoImages(rgb, depth, cx, cy, fx, fy);
+				cloud = rtabmap::util3d::transformPointCloud<pcl::PointXYZRGB>(cloud, t);
+				viewer.showCloud(cloud, "cloud");
+			}
 		}
 
 		int c = cv::waitKey(10); // wait 10 ms or for key stroke