fixed compilation with new multicam branch

CMakeLists.txt

@@ -122,6 +122,8 @@ add_message_files(
    GPS.msg
    Path.msg
    EnvSensor.msg
+   CameraModel.msg
+   CameraModels.msg
 )
 
 ## Generate services in the 'srv' folder

msg/CameraModel.msg

@@ -0,0 +1,4 @@
+
+sensor_msgs/CameraInfo camera_info
+geometry_msgs/Transform local_transform
+

msg/CameraModels.msg

@@ -0,0 +1,3 @@
+
+CameraModel[] models
+

msg/NodeData.msg

@@ -29,7 +29,7 @@ float32[] cx
 float32[] cy
 float32[] width
 float32[] height
-float32 baseline
+float32[] baseline
 # local transform (/base_link -> /camera_link)
 geometry_msgs/Transform[] localTransform
 

msg/OdomInfo.msg

@@ -32,8 +32,7 @@ float32 gravityPitchError
 int32[] localBundleIds
 
 # Local bundle camera models
-sensor_msgs/CameraInfo[] localBundleModels
+CameraModels[] localBundleModels
-geometry_msgs/Transform[] localBundleModelTransforms
 
 # Local bundle camera poses
 geometry_msgs/Pose[] localBundlePoses

src/CoreWrapper.cpp

@@ -521,16 +521,6 @@ void CoreWrapper::onInit()
 	bool subscribeRGBD = false;
 	pnh.param("rgbd_cameras", cameras, cameras);
 	pnh.param("subscribe_rgbd", subscribeRGBD, subscribeRGBD);
-	if(subscribeRGBD && cameras> 1 && estimationType>0)
-	{
-		NODELET_WARN("Setting \"%s\" parameter to 0 (%d is not supported "
-				"for multi-cameras) as \"subscribe_rgbd\" is "
-				"true and \"rgbd_cameras\">1. Set \"%s\" to 0 to suppress this warning.",
-				Parameters::kVisEstimationType().c_str(),
-				estimationType,
-				Parameters::kVisEstimationType().c_str());
-		uInsert(parameters_, ParametersPair(Parameters::kVisEstimationType(), "0"));
-	}
 
 	// modify default parameters with those in the database
 	if(!deleteDbOnStart)
@@ -1794,7 +1784,7 @@ void CoreWrapper::commonLaserScanCallback(
 			scan,
 			cv::Mat(),
 			cv::Mat(),
-			CameraModel(),
+			rtabmap::CameraModel(),
 			lastPoseIntermediate_?-1:!scan2dMsg.ranges.empty()?scan2dMsg.header.seq:scan3dMsg.header.seq,
 			rtabmap_ros::timestampFromROS(lastPoseStamp_),
 			userData);
@@ -1858,7 +1848,7 @@ void CoreWrapper::commonOdomCallback(
 	SensorData data(
 			cv::Mat(),
 			cv::Mat(),
-			CameraModel(),
+			rtabmap::CameraModel(),
 			lastPoseIntermediate_?-1:odomMsg->header.seq,
 			rtabmap_ros::timestampFromROS(lastPoseStamp_),
 			userData);
@@ -1943,7 +1933,7 @@ void CoreWrapper::process(
 						covariance.at<double>(4,4) = uIsFinite(covariance.at<double>(4,4)) && covariance.at<double>(4,4)!=0?covariance.at<double>(4,4):1;
 					}
 
-					SensorData interData(cv::Mat(), cv::Mat(), CameraModel(), -1, rtabmap_ros::timestampFromROS(iter->first.header.stamp));
+					SensorData interData(cv::Mat(), cv::Mat(), rtabmap::CameraModel(), -1, rtabmap_ros::timestampFromROS(iter->first.header.stamp));
 					Transform gt;
 					if(!groundTruthFrameId_.empty())
 					{

src/DbPlayerNode.cpp

@@ -293,30 +293,37 @@ int main(int argc, char** argv)
 		}
 		else if(!odom.data().rightRaw().empty() && odom.data().rightRaw().type() == CV_8U)
 		{
-			//stereo
+			if(odom.data().stereoCameraModels().size() > 1)
-			if(odom.data().stereoCameraModel().isValidForProjection())
 			{
-				camInfoA.D.resize(8,0);
+				ROS_WARN("Multi-cameras detected in database but this node cannot send multi-images yet...");
-
-				camInfoA.P[0] = odom.data().stereoCameraModel().left().fx();
-				camInfoA.K[0] = odom.data().stereoCameraModel().left().fx();
-				camInfoA.P[5] = odom.data().stereoCameraModel().left().fy();
-				camInfoA.K[4] = odom.data().stereoCameraModel().left().fy();
-				camInfoA.P[2] = odom.data().stereoCameraModel().left().cx();
-				camInfoA.K[2] = odom.data().stereoCameraModel().left().cx();
-				camInfoA.P[6] = odom.data().stereoCameraModel().left().cy();
-				camInfoA.K[5] = odom.data().stereoCameraModel().left().cy();
-
-				camInfoB = camInfoA;
-				camInfoB.P[3] = odom.data().stereoCameraModel().right().Tx(); // Right_Tx = -baseline*fx
 			}
+			else
+			{
+				//stereo
+				if(odom.data().stereoCameraModels()[0].isValidForProjection())
+				{
+					camInfoA.D.resize(8,0);
 
-			type=1;
+					camInfoA.P[0] = odom.data().stereoCameraModels()[0].left().fx();
+					camInfoA.K[0] = odom.data().stereoCameraModels()[0].left().fx();
+					camInfoA.P[5] = odom.data().stereoCameraModels()[0].left().fy();
+					camInfoA.K[4] = odom.data().stereoCameraModels()[0].left().fy();
+					camInfoA.P[2] = odom.data().stereoCameraModels()[0].left().cx();
+					camInfoA.K[2] = odom.data().stereoCameraModels()[0].left().cx();
+					camInfoA.P[6] = odom.data().stereoCameraModels()[0].left().cy();
+					camInfoA.K[5] = odom.data().stereoCameraModels()[0].left().cy();
 
-			if(leftPub.getTopic().empty()) leftPub = it.advertise("left/image", 1);
+					camInfoB = camInfoA;
-			if(rightPub.getTopic().empty()) rightPub = it.advertise("right/image", 1);
+					camInfoB.P[3] = odom.data().stereoCameraModels()[0].right().Tx(); // Right_Tx = -baseline*fx
-			if(leftCamInfoPub.getTopic().empty()) leftCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("left/camera_info", 1);
+				}
-			if(rightCamInfoPub.getTopic().empty()) rightCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("right/camera_info", 1);
+
+				type=1;
+
+				if(leftPub.getTopic().empty()) leftPub = it.advertise("left/image", 1);
+				if(rightPub.getTopic().empty()) rightPub = it.advertise("right/image", 1);
+				if(leftCamInfoPub.getTopic().empty()) leftCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("left/camera_info", 1);
+				if(rightCamInfoPub.getTopic().empty()) rightCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("right/camera_info", 1);
+			}
 
 		}
 		else
@@ -383,9 +390,9 @@ int main(int argc, char** argv)
 			{
 				localTransform = odom.data().cameraModels()[0].localTransform();
 			}
-			else if(odom.data().stereoCameraModel().isValidForProjection())
+			else if(odom.data().stereoCameraModels().size() == 1)
 			{
-				localTransform = odom.data().stereoCameraModel().left().localTransform();
+				localTransform = odom.data().stereoCameraModels()[0].left().localTransform();
 			}
 			if(!localTransform.isNull())
 			{

src/GuiWrapper.cpp

@@ -523,7 +523,7 @@ void GuiWrapper::commonDepthCallback(
 
 	cv::Mat rgb;
 	cv::Mat depth;
-	std::vector<CameraModel> cameraModels;
+	std::vector<rtabmap::CameraModel> cameraModels;
 	LaserScan scan;
 	rtabmap::OdometryInfo info;
 	bool ignoreData = false;
@@ -937,7 +937,7 @@ void GuiWrapper::commonLaserScanCallback(
 				scan,
 				cv::Mat(),
 				cv::Mat(),
-				CameraModel(),
+				rtabmap::CameraModel(),
 				odomHeader.seq,
 				rtabmap_ros::timestampFromROS(odomHeader.stamp)),
 		odomMsg.get()?rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose):odomT,
@@ -1009,7 +1009,7 @@ void GuiWrapper::commonOdomCallback(
 		rtabmap::SensorData(
 				cv::Mat(),
 				cv::Mat(),
-				CameraModel(),
+				rtabmap::CameraModel(),
 				odomHeader.seq,
 				rtabmap_ros::timestampFromROS(odomHeader.stamp)),
 		odomMsg.get()?rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose):odomT,

src/MsgConversion.cpp

@@ -225,14 +225,14 @@ void rgbdImageToROS(const rtabmap::SensorData & data, rtabmap_ros::RGBDImage & m
 		msg.rgb_camera_info.header = header;
 		localTransform = data.cameraModels().front().localTransform();
 	}
-	else
+	else if(data.stereoCameraModels().size() == 1)
 	{
 		//stereo
-		rtabmap_ros::cameraModelToROS(data.stereoCameraModel().left(), msg.rgb_camera_info);
+		rtabmap_ros::cameraModelToROS(data.stereoCameraModels()[0].left(), msg.rgb_camera_info);
-		rtabmap_ros::cameraModelToROS(data.stereoCameraModel().right(), msg.depth_camera_info);
+		rtabmap_ros::cameraModelToROS(data.stereoCameraModels()[0].right(), msg.depth_camera_info);
 		msg.rgb_camera_info.header = header;
 		msg.depth_camera_info.header = header;
-		localTransform = data.stereoCameraModel().localTransform();
+		localTransform = data.stereoCameraModels()[0].localTransform();
 	}
 
 	if(!data.imageRaw().empty())
@@ -1109,27 +1109,30 @@ rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg)
 		}
 	}
 
-	rtabmap::StereoCameraModel stereoModel;
+	std::vector<rtabmap::StereoCameraModel> stereoModels;
 	std::vector<rtabmap::CameraModel> models;
-	if(msg.baseline > 0.0f)
+	if(msg.baseline.size())
 	{
 		// stereo model
-		if(msg.fx.size() == 1 &&
+		if(msg.fx.size() == msg.baseline.size() &&
-		   msg.fy.size() == 1 &&
+		   msg.fy.size() == msg.baseline.size() &&
-		   msg.cx.size() == 1 &&
+		   msg.cx.size() == msg.baseline.size() &&
-		   msg.cy.size() == 1 &&
+		   msg.cy.size() == msg.baseline.size() &&
-		   msg.width.size() == 1 &&
+		   msg.width.size() == msg.baseline.size() &&
-		   msg.height.size() == 1 &&
+		   msg.height.size() == msg.baseline.size() &&
-		   msg.localTransform.size() == 1)
+		   msg.localTransform.size() == msg.baseline.size())
 		{
-			stereoModel = rtabmap::StereoCameraModel(
+			for(unsigned int i=0; i<msg.fx.size(); ++i)
-					msg.fx[0],
+			{
-					msg.fy[0],
+				stereoModels.push_back(rtabmap::StereoCameraModel(
-					msg.cx[0],
+						msg.fx[i],
-					msg.cy[0],
+						msg.fy[i],
-					msg.baseline,
+						msg.cx[i],
-					transformFromGeometryMsg(msg.localTransform[0]),
+						msg.cy[i],
-					cv::Size(msg.width[0], msg.height[0]));
+						msg.baseline[i],
+						transformFromGeometryMsg(msg.localTransform[i]),
+						cv::Size(msg.width[i], msg.height[i])));
+			}
 		}
 	}
 	else
@@ -1170,7 +1173,7 @@ rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg)
 			msg.label,
 			transformFromPoseMsg(msg.pose),
 			transformFromPoseMsg(msg.groundTruthPose),
-			stereoModel.isValidForProjection()?
+			stereoModels.size()?
 				rtabmap::SensorData(
 					rtabmap::LaserScan(compressedMatFromBytes(msg.laserScan),
 							msg.laserScanMaxPts,
@@ -1179,7 +1182,7 @@ rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg)
 							transformFromGeometryMsg(msg.laserScanLocalTransform)),
 					compressedMatFromBytes(msg.image),
 					compressedMatFromBytes(msg.depth),
-					stereoModel,
+					stereoModels,
 					msg.id,
 					msg.stamp,
 					compressedMatFromBytes(msg.userData)):
@@ -1236,7 +1239,6 @@ void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData &
 	msg.laserScanMaxRange = signature.sensorData().laserScanCompressed().rangeMax();
 	msg.laserScanFormat = signature.sensorData().laserScanCompressed().format();
 	transformToGeometryMsg(signature.sensorData().laserScanCompressed().localTransform(), msg.laserScanLocalTransform);
-	msg.baseline = 0;
 	if(signature.sensorData().cameraModels().size())
 	{
 		msg.fx.resize(signature.sensorData().cameraModels().size());
@@ -1257,17 +1259,27 @@ void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData &
 			transformToGeometryMsg(signature.sensorData().cameraModels()[i].localTransform(), msg.localTransform[i]);
 		}
 	}
-	else if(signature.sensorData().stereoCameraModel().isValidForProjection())
+	else if(signature.sensorData().stereoCameraModels().size())
 	{
-		msg.fx.push_back(signature.sensorData().stereoCameraModel().left().fx());
+		msg.fx.resize(signature.sensorData().stereoCameraModels().size());
-		msg.fy.push_back(signature.sensorData().stereoCameraModel().left().fy());
+		msg.fy.resize(signature.sensorData().stereoCameraModels().size());
-		msg.cx.push_back(signature.sensorData().stereoCameraModel().left().cx());
+		msg.cx.resize(signature.sensorData().stereoCameraModels().size());
-		msg.cy.push_back(signature.sensorData().stereoCameraModel().left().cy());
+		msg.cy.resize(signature.sensorData().stereoCameraModels().size());
-		msg.width.push_back(signature.sensorData().stereoCameraModel().left().imageWidth());
+		msg.width.resize(signature.sensorData().stereoCameraModels().size());
-		msg.height.push_back(signature.sensorData().stereoCameraModel().left().imageHeight());
+		msg.height.resize(signature.sensorData().stereoCameraModels().size());
-		msg.baseline = signature.sensorData().stereoCameraModel().baseline();
+		msg.baseline.resize(signature.sensorData().stereoCameraModels().size());
-		msg.localTransform.resize(1);
+		msg.localTransform.resize(signature.sensorData().stereoCameraModels().size());
-		transformToGeometryMsg(signature.sensorData().stereoCameraModel().left().localTransform(), msg.localTransform[0]);
+		for(unsigned int i=0; i<signature.sensorData().stereoCameraModels().size(); ++i)
+		{
+			msg.fx[i] = signature.sensorData().stereoCameraModels()[i].left().fx();
+			msg.fy[i] = signature.sensorData().stereoCameraModels()[i].left().fy();
+			msg.cx[i] = signature.sensorData().stereoCameraModels()[i].left().cx();
+			msg.cy[i] = signature.sensorData().stereoCameraModels()[i].left().cy();
+			msg.width[i] = signature.sensorData().stereoCameraModels()[i].left().imageWidth();
+			msg.height[i] = signature.sensorData().stereoCameraModels()[i].left().imageHeight();
+			msg.baseline[i] = signature.sensorData().stereoCameraModels()[i].baseline();
+			transformToGeometryMsg(signature.sensorData().stereoCameraModels()[i].left().localTransform(), msg.localTransform[i]);
+		}
 	}
 
 	//Features stuff...
@@ -1466,11 +1478,15 @@ rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg, bool ig
 	info.localBundleConstraints = msg.localBundleConstraints;
 	info.localBundleTime = msg.localBundleTime;
 	UASSERT(msg.localBundleModels.size() == msg.localBundleIds.size());
-	UASSERT(msg.localBundleModels.size() == msg.localBundleModelTransforms.size());
 	UASSERT(msg.localBundleModels.size() == msg.localBundlePoses.size());
 	for(size_t i=0; i<msg.localBundleIds.size(); ++i)
 	{
-		info.localBundleModels.insert(std::make_pair(msg.localBundleIds[i], cameraModelFromROS(msg.localBundleModels[i], transformFromGeometryMsg(msg.localBundleModelTransforms[i]))));
+		std::vector<rtabmap::CameraModel> models;
+		for(size_t j=0; j<msg.localBundleModels[i].models.size(); ++j)
+		{
+			models.push_back(cameraModelFromROS(msg.localBundleModels[i].models[j].camera_info, transformFromGeometryMsg(msg.localBundleModels[i].models[j].local_transform)));
+		}
+		info.localBundleModels.insert(std::make_pair(msg.localBundleIds[i], models));
 		info.localBundlePoses.insert(std::make_pair(msg.localBundleIds[i], transformFromPoseMsg(msg.localBundlePoses[i])));
 	}
 	info.keyFrameAdded = msg.keyFrameAdded;
@@ -1541,21 +1557,26 @@ void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::OdomInfo & m
 	msg.localBundleConstraints = info.localBundleConstraints;
 	msg.localBundleTime = info.localBundleTime;
 	UASSERT(info.localBundleModels.size() == info.localBundlePoses.size());
-	for(std::map<int, rtabmap::CameraModel>::const_iterator iter=info.localBundleModels.begin();
+	for(std::map<int, std::vector<rtabmap::CameraModel> >::const_iterator iter=info.localBundleModels.begin();
 		iter!=info.localBundleModels.end();
 		++iter)
 	{
 		msg.localBundleIds.push_back(iter->first);
-		sensor_msgs::CameraInfo camInfo;
+
-		cameraModelToROS(iter->second, camInfo);
-		msg.localBundleModels.push_back(camInfo);
-		geometry_msgs::Transform localT;
-		transformToGeometryMsg(iter->second.localTransform(), localT);
-		msg.localBundleModelTransforms.push_back(localT);
 		UASSERT(info.localBundlePoses.find(iter->first)!=info.localBundlePoses.end());
 		geometry_msgs::Pose pose;
 		transformToPoseMsg(info.localBundlePoses.at(iter->first), pose);
 		msg.localBundlePoses.push_back(pose);
+
+		rtabmap_ros::CameraModels models;
+		for(size_t i=0; i<iter->second.size(); ++i)
+		{
+			rtabmap_ros::CameraModel modelMsg;
+			cameraModelToROS(iter->second[i], modelMsg.camera_info);
+			transformToGeometryMsg(iter->second[i].localTransform(), modelMsg.local_transform);
+			models.models.push_back(modelMsg);
+		}
+		msg.localBundleModels.push_back(models);
 	}
 	msg.keyFrameAdded = info.keyFrameAdded;
 	msg.timeEstimation = info.timeEstimation;

src/StereoCameraNode.cpp

@@ -86,13 +86,20 @@ int main(int argc, char** argv)
 			cv::resize(data.rightRaw(), imageRight.image, cv::Size(0,0), scale, scale, CV_INTER_AREA);
 			imageRightPub.publish(imageRight.toImageMsg());
 
-			sensor_msgs::CameraInfo infoLeft, infoRight;
+			if(data.stereoCameraModels().size())
-			rtabmap_ros::cameraModelToROS(data.stereoCameraModel().left().scaled(scale), infoLeft);
+			{
-			rtabmap_ros::cameraModelToROS(data.stereoCameraModel().right().scaled(scale), infoRight);
+				sensor_msgs::CameraInfo infoLeft, infoRight;
-			infoLeft.header = imageLeft.header;
+				rtabmap_ros::cameraModelToROS(data.stereoCameraModels()[0].left().scaled(scale), infoLeft);
-			infoRight.header = imageLeft.header;
+				rtabmap_ros::cameraModelToROS(data.stereoCameraModels()[0].right().scaled(scale), infoRight);
-			infoLeftPub.publish(infoLeft);
+				infoLeft.header = imageLeft.header;
-			infoRightPub.publish(infoRight);
+				infoRight.header = imageLeft.header;
+				infoLeftPub.publish(infoLeft);
+				infoRightPub.publish(infoRight);
+			}
+			else
+			{
+				ROS_ERROR("No calibration loaded!");
+			}
 
 			ros::spinOnce();
 		}

src/nodelets/icp_odometry.cpp

@@ -478,7 +478,7 @@ private:
 						localScanTransform),
 				cv::Mat(),
 				cv::Mat(),
-				CameraModel(),
+				rtabmap::CameraModel(),
 				0,
 				rtabmap_ros::timestampFromROS(scanMsg->header.stamp));
 
@@ -724,7 +724,7 @@ private:
 				laserScan,
 				cv::Mat(),
 				cv::Mat(),
-				CameraModel(),
+				rtabmap::CameraModel(),
 				0,
 				rtabmap_ros::timestampFromROS(cloudMsg.header.stamp));
 

src/nodelets/rgbd_odometry.cpp

@@ -376,16 +376,6 @@ private:
 		bool subscribeRGBD = false;
 		pnh.param("subscribe_rgbd", subscribeRGBD, subscribeRGBD);
 		pnh.param("rgbd_cameras", rgbdCameras, rgbdCameras);
-		if(subscribeRGBD && rgbdCameras> 1 && estimationType>0)
-		{
-			NODELET_WARN("Setting \"%s\" parameter to 0 (%d is not supported "
-					"for multi-cameras) as \"subscribe_rgbd\" is "
-					"true and \"rgbd_cameras\">1. Set \"%s\" to 0 to suppress this warning.",
-					Parameters::kVisEstimationType().c_str(),
-					estimationType,
-					Parameters::kVisEstimationType().c_str());
-			uInsert(parameters, ParametersPair(Parameters::kVisEstimationType(), "0"));
-		}
 	}
 
 	void commonCallback(
@@ -408,7 +398,7 @@ private:
 		cv::Mat rgb;
 		cv::Mat depth;
 		pcl::PointCloud<pcl::PointXYZ> scanCloud;
-		std::vector<CameraModel> cameraModels;
+		std::vector<rtabmap::CameraModel> cameraModels;
 		double stampDiff = 0;
 		for(unsigned int i=0; i<rgbImages.size(); ++i)
 		{

src/rviz/MapCloudDisplay.cpp

@@ -292,7 +292,7 @@ void MapCloudDisplay::processMapData(const rtabmap_ros::MapData& map)
 		if((fromDepth &&
 			!s.sensorData().imageCompressed().empty() &&
 		    !s.sensorData().depthOrRightCompressed().empty() &&
-		    (s.sensorData().cameraModels().size() || s.sensorData().stereoCameraModel().isValidForProjection())) ||
+		    (s.sensorData().cameraModels().size() || s.sensorData().stereoCameraModels().size())) ||
 		   (!fromDepth && !s.sensorData().laserScanCompressed().isEmpty()))
 		{
 			cv::Mat image, depth;