i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-09-16 00:00:20 +08:00
Code Issues Packages Projects Releases Wiki Activity

ros: added mono16 supported image input

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/ros-pkg@1586 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-07-22 19:00:06 +00:00
parent 2fbd147305
commit 5ac64bd223
3 changed files with 86 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CoreWrapper.cpp
+81 -4
View File
@@ -290,7 +290,26 @@ void CoreWrapper::defaultCallback(const sensor_msgs::ImageConstPtr & imageMsg)
}
time_ = ros::Time::now();
cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(imageMsg, "bgr8");
if(!(imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
imageMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
imageMsg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0))
{
ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8");
return;
}
cv_bridge::CvImageConstPtr ptrImage;
if(imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
{
ptrImage = cv_bridge::toCvShare(imageMsg, "mono8");
}
else
{
ptrImage = cv_bridge::toCvShare(imageMsg, "bgr8");
}
process(ptrImage->header.seq,
ptrImage->image);
}
@@ -313,6 +332,17 @@ void CoreWrapper::depthCallback(
}
time_ = ros::Time::now();
if(!(imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
imageMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
imageMsg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) &&
(depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)!=0 ||
depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)!=0))
{
ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 and image_depth=32FC1,16UC1");
return;
}
// TF ready?
Transform localTransform;
try
@@ -329,7 +359,16 @@ void CoreWrapper::depthCallback(
Transform odom = transformFromPoseMsg(odomMsg->pose.pose);
cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(imageMsg, "bgr8");
cv_bridge::CvImageConstPtr ptrImage;
if(imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
{
ptrImage = cv_bridge::toCvShare(imageMsg, "mono8");
}
else
{
ptrImage = cv_bridge::toCvShare(imageMsg, "bgr8");
}
cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsg);
float depthConstant = 1.0f/cameraInfoMsg->K[4];
@@ -361,6 +400,15 @@ void CoreWrapper::scanCallback(
}
time_ = ros::Time::now();
if(!(imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
imageMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
imageMsg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0))
{
ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8");
return;
}
// TF ready?
try
{
@@ -383,7 +431,16 @@ void CoreWrapper::scanCallback(
Transform odom = transformFromPoseMsg(odomMsg->pose.pose);
cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(imageMsg, "bgr8");
cv_bridge::CvImageConstPtr ptrImage;
if(imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
{
ptrImage = cv_bridge::toCvShare(imageMsg, "mono8");
}
else
{
ptrImage = cv_bridge::toCvShare(imageMsg, "bgr8");
}
process(ptrImage->header.seq,
ptrImage->image,
@@ -414,6 +471,17 @@ void CoreWrapper::depthScanCallback(
}
time_ = ros::Time::now();
if(!(imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
imageMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
imageMsg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) &&
(depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)!=0 ||
depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)!=0))
{
ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 and image_depth=32FC1,16UC1");
return;
}
// TF ready?
Transform localTransform;
try
@@ -439,7 +507,16 @@ void CoreWrapper::depthScanCallback(
Transform odom = transformFromPoseMsg(odomMsg->pose.pose);
cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(imageMsg, "bgr8");
cv_bridge::CvImageConstPtr ptrImage;
if(imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
{
ptrImage = cv_bridge::toCvShare(imageMsg, "mono8");
}
else
{
ptrImage = cv_bridge::toCvShare(imageMsg, "bgr8");
}
cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsg);
float depthConstant = 1.0f/cameraInfoMsg->K[4];
src/VisualOdometryNode.cpp
+3 -2
View File
@@ -153,12 +153,13 @@ public:
if(!paused_)
{
if(!(image->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
image->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
image->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
image->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) ||
!(depth->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)==0 ||
depth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)==0))
{
ROS_ERROR("Input type must be image=mono8,rgb8,bgr8 (mono8 recommended) and image_depth=16UC1");
ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 (mono8 recommended) and image_depth=16UC1");
return;
}
else if(depth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)==0)
@@ -188,7 +189,7 @@ public:
if(image->data.size() && depth->data.size() && cameraInfo->K[4] != 0)
{
float depthConstant = 1.0f/cameraInfo->K[4];
cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(image);
cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(image, "mono8");
cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depth);
rtabmap::Image data(ptrImage->image,
src/nodelets/point_cloud_xyzrgb.cpp
+2 -1
View File
@@ -79,12 +79,13 @@ private:
const sensor_msgs::CameraInfoConstPtr& cameraInfo)
{
if(!(image->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
image->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
image->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
image->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) &&
(imageDepth->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)!=0 ||
imageDepth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)!=0))
{
ROS_ERROR("Input type must be image=mono8,rgb8,bgr8 and image_depth=32FC1,16UC1");
ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 and image_depth=32FC1,16UC1");
return;
}