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

fixed demo_appearance_mapping.launch

Browse Source
This commit is contained in:
matlabbe
2015-08-14 13:21:49 -04:00
parent 5a953073a4
commit cc5203127f
3 changed files with 101 additions and 98 deletions
Download Patch File Download Diff File Expand all files Collapse all files
launch/demo/demo_appearance_mapping.launch
+2 -2
View File
@@ -23,8 +23,8 @@
<param name="Mem/IncrementalMemory" type="string" value="true"/> <!-- true = SLAM mode --> <param name="Mem/IncrementalMemory" type="string" value="true"/> <!-- true = SLAM mode -->
<param name="Mem/RehearsalIdUpdatedToNewOne" type="string" value="true"/> <!-- On merging, update to new ID--> <param name="Mem/RehearsalIdUpdatedToNewOne" type="string" value="true"/> <!-- On merging, update to new ID-->
<param name="Mem/BadSignaturesIgnored" type="string" value="true"/> <param name="Mem/BadSignaturesIgnored" type="string" value="true"/>
<param name="Kp/DetectorStrategy" type="string" value="2"/> <!-- use SURF --> <param name="Kp/DetectorStrategy" type="string" value="0"/> <!-- use SURF -->
<param name="Kp/NNStrategy" type="string" value="3"/> <!-- kdTree --> <param name="Kp/NNStrategy" type="string" value="1"/> <!-- kdTree -->
</node> </node>
<!-- visualization of the "infoEx" topic sent by rtabmap node --> <!-- visualization of the "infoEx" topic sent by rtabmap node -->
src/CameraNode.cpp
+2 -2
View File
@@ -193,12 +193,12 @@ public:
if(!path.empty() && UDirectory::exists(path)) if(!path.empty() && UDirectory::exists(path))
{ {
//images //images
camera_ = new rtabmap::CameraImages(path, 1, false, frameRate); camera_ = new rtabmap::CameraImages(path, 1, false, false, false, frameRate);
} }
else if(!path.empty() && UFile::exists(path)) else if(!path.empty() && UFile::exists(path))
{ {
//video //video
camera_ = new rtabmap::CameraVideo(path, frameRate); camera_ = new rtabmap::CameraVideo(path, false, frameRate);
} }
else else
{ {
src/GuiWrapper.cpp
+97 -94
View File
@@ -521,115 +521,118 @@ void GuiWrapper::commonDepthCallback(
return; return;
} }
int imageWidth = imageMsgs[0]->width;
int imageHeight = imageMsgs[0]->height;
int cameraCount = imageMsgs.size();
cv::Mat rgb; cv::Mat rgb;
cv::Mat depth; cv::Mat depth;
pcl::PointCloud<pcl::PointXYZ> scanCloud;
std::vector<CameraModel> cameraModels; std::vector<CameraModel> cameraModels;
for(unsigned int i=0; i<imageMsgs.size(); ++i) if(imageMsgs[0].get() && depthMsgs[0].get())
{ {
if(!(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 || int imageWidth = imageMsgs[0]->width;
imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 || int imageHeight = imageMsgs[0]->height;
imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 || int cameraCount = imageMsgs.size();
imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 || pcl::PointCloud<pcl::PointXYZ> scanCloud;
imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) || for(unsigned int i=0; i<imageMsgs.size(); ++i)
!(depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 ||
depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0 ||
depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0))
{ {
ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 and image_depth=32FC1,16UC1,mono16"); if(!(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
return; imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
} imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
UASSERT(imageMsgs[i]->width == imageWidth && imageMsgs[i]->height == imageHeight); imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
UASSERT(depthMsgs[i]->width == imageWidth && depthMsgs[i]->height == imageHeight); imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) ||
!(depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 ||
Transform localTransform = getTransform(frameId_, depthMsgs[i]->header.frame_id, depthMsgs[i]->header.stamp); depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0 ||
if(localTransform.isNull()) depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0))
{
return;
}
// sync with odometry stamp
if(odomHeader.stamp != depthMsgs[i]->header.stamp)
{
if(!odomT.isNull())
{ {
Transform sensorT = getTransform(odomHeader.frame_id, frameId_, depthMsgs[i]->header.stamp); ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 and image_depth=32FC1,16UC1,mono16");
if(sensorT.isNull()) return;
}
UASSERT(imageMsgs[i]->width == imageWidth && imageMsgs[i]->height == imageHeight);
UASSERT(depthMsgs[i]->width == imageWidth && depthMsgs[i]->height == imageHeight);
Transform localTransform = getTransform(frameId_, depthMsgs[i]->header.frame_id, depthMsgs[i]->header.stamp);
if(localTransform.isNull())
{
return;
}
// sync with odometry stamp
if(odomHeader.stamp != depthMsgs[i]->header.stamp)
{
if(!odomT.isNull())
{ {
return; Transform sensorT = getTransform(odomHeader.frame_id, frameId_, depthMsgs[i]->header.stamp);
if(sensorT.isNull())
{
return;
}
localTransform = odomT.inverse() * sensorT * localTransform;
} }
localTransform = odomT.inverse() * sensorT * localTransform;
} }
}
cv_bridge::CvImageConstPtr ptrImage; cv_bridge::CvImageConstPtr ptrImage;
if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0) if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0)
{
ptrImage = cv_bridge::toCvShare(imageMsgs[i]);
}
else if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
{
ptrImage = cv_bridge::toCvShare(imageMsgs[i], "mono8");
}
else
{
ptrImage = cv_bridge::toCvShare(imageMsgs[i], "bgr8");
}
cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsgs[i]);
cv::Mat subDepth = ptrDepth->image;
if(subDepth.type() == CV_32FC1)
{
subDepth = util3d::cvtDepthFromFloat(subDepth);
static bool shown = false;
if(!shown)
{ {
ROS_WARN("Use depth image with \"unsigned short\" type to " ptrImage = cv_bridge::toCvShare(imageMsgs[i]);
"avoid conversion. This message is only printed once..."); }
shown = true; else if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
{
ptrImage = cv_bridge::toCvShare(imageMsgs[i], "mono8");
}
else
{
ptrImage = cv_bridge::toCvShare(imageMsgs[i], "bgr8");
}
cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsgs[i]);
cv::Mat subDepth = ptrDepth->image;
if(subDepth.type() == CV_32FC1)
{
subDepth = util3d::cvtDepthFromFloat(subDepth);
static bool shown = false;
if(!shown)
{
ROS_WARN("Use depth image with \"unsigned short\" type to "
"avoid conversion. This message is only printed once...");
shown = true;
}
} }
}
// initialize // initialize
if(rgb.empty()) if(rgb.empty())
{ {
rgb = cv::Mat(imageHeight, imageWidth*cameraCount, ptrImage->image.type()); rgb = cv::Mat(imageHeight, imageWidth*cameraCount, ptrImage->image.type());
} }
if(depth.empty()) if(depth.empty())
{ {
depth = cv::Mat(imageHeight, imageWidth*cameraCount, subDepth.type()); depth = cv::Mat(imageHeight, imageWidth*cameraCount, subDepth.type());
} }
if(ptrImage->image.type() == rgb.type()) if(ptrImage->image.type() == rgb.type())
{ {
ptrImage->image.copyTo(cv::Mat(rgb, cv::Rect(i*imageWidth, 0, imageWidth, imageHeight))); ptrImage->image.copyTo(cv::Mat(rgb, cv::Rect(i*imageWidth, 0, imageWidth, imageHeight)));
} }
else else
{ {
ROS_ERROR("Some RGB images are not the same type!"); ROS_ERROR("Some RGB images are not the same type!");
return; return;
} }
if(subDepth.type() == depth.type()) if(subDepth.type() == depth.type())
{ {
subDepth.copyTo(cv::Mat(depth, cv::Rect(i*imageWidth, 0, imageWidth, imageHeight))); subDepth.copyTo(cv::Mat(depth, cv::Rect(i*imageWidth, 0, imageWidth, imageHeight)));
} }
else else
{ {
ROS_ERROR("Some Depth images are not the same type!"); ROS_ERROR("Some Depth images are not the same type!");
return; return;
} }
image_geometry::PinholeCameraModel model; image_geometry::PinholeCameraModel model;
model.fromCameraInfo(*cameraInfoMsgs[i]); model.fromCameraInfo(*cameraInfoMsgs[i]);
cameraModels.push_back(rtabmap::CameraModel( cameraModels.push_back(rtabmap::CameraModel(
model.fx(), model.fx(),
model.fy(), model.fy(),
model.cx(), model.cx(),
model.cy(), model.cy(),
localTransform)); localTransform));
}
} }
cv::Mat scan; cv::Mat scan;
@@ -1514,7 +1517,7 @@ void GuiWrapper::setupCallbacks(
ROS_INFO("\n%s subscribed to:\n %s", ROS_INFO("\n%s subscribed to:\n %s",
ros::this_node::getName().c_str(), ros::this_node::getName().c_str(),
odomSub_.getTopic().c_str()); defaultSub_.getTopic().c_str());
} }
} }