i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-05 17:27:46 +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];