Refactored and integrated pull request https://github.com/introlab/rtabmap/pull/285

corelib/include/rtabmap/core/CameraStereo.h

@@ -226,6 +226,12 @@ public:
 			bool rectifyImages = false,
 			float imageRate = 0.0f,
 			const Transform & localTransform = Transform::getIdentity());
+	CameraStereoVideo(
+			int deviceLeft,
+			int deviceRight,
+			bool rectifyImages = false,
+			float imageRate = 0.0f,
+			const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraStereoVideo();
 
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
@@ -245,6 +251,7 @@ private:
 	std::string cameraName_;
 	CameraVideo::Source src_;
 	int usbDevice_;
+	int usbDevice2_;
 };
 
 } // namespace rtabmap

corelib/src/CameraStereo.cpp

@@ -1279,7 +1279,8 @@ CameraStereoVideo::CameraStereoVideo(
 		path_(path),
 		rectifyImages_(rectifyImages),
 		src_(CameraVideo::kVideoFile),
-		usbDevice_(0)
+		usbDevice_(0),
+		usbDevice2_(-1)
 {
 }
 
@@ -1294,7 +1295,8 @@ CameraStereoVideo::CameraStereoVideo(
 		path2_(pathRight),
 		rectifyImages_(rectifyImages),
 		src_(CameraVideo::kVideoFile),
-		usbDevice_(0)
+		usbDevice_(0),
+		usbDevice2_(-1)
 {
 }
 
@@ -1306,7 +1308,22 @@ CameraStereoVideo::CameraStereoVideo(
 	Camera(imageRate, localTransform),
 	rectifyImages_(rectifyImages),
 	src_(CameraVideo::kUsbDevice),
-	usbDevice_(device)
+	usbDevice_(device),
+	usbDevice2_(-1)
+{
+}
+
+CameraStereoVideo::CameraStereoVideo(
+	int deviceLeft,
+	int deviceRight,
+	bool rectifyImages,
+	float imageRate,
+	const Transform & localTransform) :
+	Camera(imageRate, localTransform),
+	rectifyImages_(rectifyImages),
+	src_(CameraVideo::kUsbDevice),
+	usbDevice_(deviceLeft),
+	usbDevice2_(deviceRight)
 {
 }
 
@@ -1330,20 +1347,27 @@ bool CameraStereoVideo::init(const std::string & calibrationFolder, const std::s
 
 	if (src_ == CameraVideo::kUsbDevice)
 	{
-		ULOGGER_DEBUG("CameraStereoVideo: Usb device initialization on device %d", usbDevice_);
 		capture_.open(usbDevice_);
+		if(usbDevice2_ < 0)
+		{
+			ULOGGER_DEBUG("CameraStereoVideo: Usb device initialization on device %d", usbDevice_);
+		}
+		else
+		{
+			ULOGGER_DEBUG("CameraStereoVideo: Usb device initialization on devices %d and %d", usbDevice_, usbDevice2_);
+			capture2_.open(usbDevice2_);
+		}
 	}
 	else if (src_ == CameraVideo::kVideoFile)
 	{
+		capture_.open(path_.c_str());
 		if(path2_.empty())
 		{
 			ULOGGER_DEBUG("CameraStereoVideo: filename=\"%s\"", path_.c_str());
-			capture_.open(path_.c_str());
 		}
 		else
 		{
 			ULOGGER_DEBUG("CameraStereoVideo: filenames=\"%s\" and \"%s\"", path_.c_str(), path2_.c_str());
-			capture_.open(path_.c_str());
 			capture2_.open(path2_.c_str());
 		}
 	}
@@ -1352,7 +1376,7 @@ bool CameraStereoVideo::init(const std::string & calibrationFolder, const std::s
 		ULOGGER_ERROR("CameraStereoVideo: Unknown source...");
 	}
 
-	if(!capture_.isOpened() || (!path2_.empty() && !capture2_.isOpened()))
+	if(!capture_.isOpened() || ((!path2_.empty() || usbDevice2_>=0) && !capture2_.isOpened()))
 	{
 		ULOGGER_ERROR("CameraStereoVideo: Failed to create a capture object!");
 		capture_.release();
@@ -1411,11 +1435,11 @@ SensorData CameraStereoVideo::captureImage(CameraInfo * info)
 	SensorData data;
 
 	cv::Mat img;
-	if(capture_.isOpened() && (path2_.empty() || capture2_.isOpened()))
+	if(capture_.isOpened() && ((path2_.empty() && usbDevice2_ < 0) || capture2_.isOpened()))
 	{
 		cv::Mat leftImage;
 		cv::Mat rightImage;
-		if(path2_.empty())
+		if(path2_.empty() && usbDevice2_ < 0)
 		{
 			if(!capture_.read(img))
 			{

tools/Calibration/main.cpp

@@ -50,7 +50,9 @@ void showUsage()
 			"                                       7=FlyCapture2 (Bumblebee2)\n"
 			"  --device #     Device id\n"
 			"  --debug        Debug log\n"
-			"  --stereo       Stereo\n\n");
+			"  --stereo       Stereo: assuming device provides \n"
+			"                 side-by-side stereo images, otherwise \n"
+			"                 add also \"--device_r #\" for the right device.\n\n");
 	exit(1);
 }
 
@@ -63,6 +65,7 @@ int main(int argc, char * argv[])
 
 	int driver = -1;
 	int device = 0;
+	int deviceRight = -1;
 	bool stereo = false;
 	for(int i=1; i<argc; ++i)
 	{
@@ -100,6 +103,23 @@ int main(int argc, char * argv[])
 			}
 			continue;
 		}
+		if(strcmp(argv[i], "--device_r") == 0)
+		{
+			++i;
+			if(i < argc)
+			{
+				deviceRight = std::atoi(argv[i]);
+				if(deviceRight < 0)
+				{
+					showUsage();
+				}
+			}
+			else
+			{
+				showUsage();
+			}
+			continue;
+		}
 		if(strcmp(argv[i], "--debug") == 0)
 		{
 			ULogger::setLevel(ULogger::kDebug);
@@ -128,6 +148,10 @@ int main(int argc, char * argv[])
 	UINFO("Using driver %d", driver);
 	UINFO("Using device %d", device);
 	UINFO("Stereo: %s", stereo?"true":"false");
+	if(stereo && deviceRight >= 0)
+	{
+		UINFO("Using right device %d", deviceRight);
+	}
 
 	QApplication app(argc, argv);
 	rtabmap::CalibrationDialog dialog(stereo, ".");
@@ -137,7 +161,16 @@ int main(int argc, char * argv[])
 	{
 		if(stereo)
 		{
-			camera = new rtabmap::CameraStereoVideo(device);
+			if(deviceRight>=0)
+			{
+				// left and right videos
+				camera = new rtabmap::CameraStereoVideo(device, deviceRight);
+			}
+			else
+			{
+				// side-by-side video
+				camera = new rtabmap::CameraStereoVideo(device);
+			}
 		}
 		else
 		{