Fixed issue #17

include/uvc_ros_driver.h

@@ -69,6 +69,7 @@
 #include <vector>
 #include <utility>  // std::pair
 #include <string>
+#include <algorithm>
 
 namespace uvc
 {
@@ -229,30 +230,34 @@ class uvcROSDriver
 	void setNumberOfCameras(int n_cameras)
 	{
 		n_cameras_ = n_cameras;
-        switch (n_cameras) {
-        case 8:
-            camera_config_ = 0x0F;
-            break;
-        case 6:
-            camera_config_ = 0x07;
-            break;
-        case 4:
-            camera_config_ = 0x03;
-            break;
-        case 2:
-        default:
-            camera_config_ = 0x01;
-            break;
-        }
+
+		switch (n_cameras) {
+		case 8:
+			camera_config_ = 0x0F;
+			break;
+
+		case 6:
+			camera_config_ = 0x07;
+			break;
+
+		case 4:
+			camera_config_ = 0x03;
+			break;
+
+		case 2:
+		default:
+			camera_config_ = 0x01;
+			break;
+		}
+	};
+	int getCameraConfig()
+	{
+		return camera_config_;
 	};
-    int getCameraConfig()
-    {
-        return camera_config_;
-    };
-//	void setCameraConfig(int camera_config)
-//	{
-//		camera_config_ = camera_config;
-//	};
+	//	void setCameraConfig(int camera_config)
+	//	{
+	//		camera_config_ = camera_config;
+	//	};
 	CameraParameters getCameraParams()
 	{
 		return camera_params_;

params/homography_mapping.yaml

@@ -1 +1,3 @@
+# place here the mapping between cameras from low to high numbers
+# ex: [[0,1],[2,3],[4,5]]
 homography_mapping: [[0,1]]

src/uvc_ros_driver.cpp

@@ -51,6 +51,12 @@ static void callback(uvc_frame *frame, void *arg)
 	obj->uvc_cb(frame);
 }
 
+static bool myPairMax(std::pair<int, int> p, std::pair<int, int> p1)
+{
+	// gratest value is supposed be on the second index
+	return p.second < p1.second;
+}
+
 uvcROSDriver::~uvcROSDriver()
 {
 	setParam("CAMERA_ENABLE", 0.0f);
@@ -342,10 +348,22 @@ void uvcROSDriver::setCalibration(CameraParameters camParams)
 	H_.resize(n_cameras_);
 	// pointer at camera info
 	sensor_msgs::CameraInfo *ci;
+	size_t homography_size;
 
-	// TODO: reimplment this part for multiple stereo base line based systems
 	if (set_calibration_) {
-		for (size_t i = 0; i < homography_mapping_.size(); i++) {
+		std::vector<std::pair<int, int> >::iterator it_homography =
+			std::max_element(homography_mapping_.begin(), homography_mapping_.end(),
+					 myPairMax);
+
+		if ((*it_homography).second > n_cameras_) {
+			homography_size = std::distance(homography_mapping_.begin(), it_homography);
+
+		} else {
+			homography_size = homography_mapping_.size();
+		}
+
+		// TODO: reimplment this part for multiple stereo base line based systems
+		for (size_t i = 0; i < homography_size; i++) {
 			// temp structures
 			Eigen::Matrix3d H0;
 			Eigen::Matrix3d H1;
@@ -621,7 +639,7 @@ void uvcROSDriver::uvc_cb(uvc_frame_t *frame)
 	unsigned frame_size = frame->height * frame->width * 2;
 
 	// read the IMU data
-	int16_t zero = 0;
+	// int16_t zero = 0;
 	uint16_t cam_id = 0;
 	static uint16_t count_prev;