From f122090620ac2919e96cf1447ad1e98b9d242e9a Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 28 Oct 2014 19:59:19 +0000 Subject: [PATCH] updated for compressed cv::Mat data format git-svn-id: http://rtabmap.googlecode.com/svn/trunk/ros-pkg@1938 f169173b-cf89-36c8-b27e-44dbe73f0c83 --- include/rtabmap/MsgConversion.h | 5 +++++ launch/config/rgbd_gui.ini | 16 ++++++++++++++-- src/CoreWrapper.cpp | 18 +++++++++--------- src/GuiWrapper.cpp | 12 ++++++------ src/MapAssemblerNode.cpp | 4 ++-- src/MsgConversion.cpp | 25 +++++++++++++++++++++++++ src/rviz/MapCloudDisplay.cpp | 4 ++-- 7 files changed, 63 insertions(+), 21 deletions(-) diff --git a/include/rtabmap/MsgConversion.h b/include/rtabmap/MsgConversion.h index ad77ac49..037ace98 100644 --- a/include/rtabmap/MsgConversion.h +++ b/include/rtabmap/MsgConversion.h @@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include namespace rtabmap { @@ -44,6 +45,10 @@ rtabmap::Transform transformFromGeometryMsg(const geometry_msgs::Transform & msg void transformToPoseMsg(const rtabmap::Transform & transform, geometry_msgs::Pose & msg); rtabmap::Transform transformFromPoseMsg(const geometry_msgs::Pose & msg); +// copy data +void compressedMatToBytes(const cv::Mat & compressed, std::vector & bytes); +cv::Mat compressedMatFromBytes(const std::vector & bytes, bool copy = true); + } #endif /* MSGCONVERSION_H_ */ diff --git a/launch/config/rgbd_gui.ini b/launch/config/rgbd_gui.ini index 1466dffd..e6fcd222 100644 --- a/launch/config/rgbd_gui.ini +++ b/launch/config/rgbd_gui.ini @@ -13,8 +13,8 @@ General\beep=false General\keypointsOpacity=16 General\voxelSize=0 General\decimation=16 -MainWindow\state="@ByteArray(\0\0\0\xff\0\0\0\0\xfd\0\0\0\x3\0\0\0\0\0\0\0\xfa\0\0\x2\xba\xfc\x2\0\0\0\x1\xfb\0\0\0$\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0s\0t\0\x61\0t\0s\0V\0\x32\0\0\0\0(\0\0\x2\xba\0\0\0\xe0\0\xff\xff\xff\0\0\0\x1\0\0\x5\0\0\0\x2\x92\xfc\x2\0\0\0\x2\xfc\0\0\0(\0\0\x2\x92\0\0\0\xe1\0\xff\xff\xff\xfc\x1\0\0\0\x2\xfb\0\0\0(\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0i\0m\0\x61\0g\0\x65\0V\0i\0\x65\0w\x1\0\0\0\0\0\0\x1\\\0\0\0O\0\xff\xff\xff\xfc\0\0\x1\x62\0\0\x3\x9e\0\0\0\xe6\x1\0\0\x1d\xfa\0\0\0\x1\x2\0\0\0\x2\xfb\0\0\0,\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0g\0r\0\x61\0p\0h\0V\0i\0\x65\0w\0\x65\0r\x1\0\0\0\0\xff\xff\xff\xff\0\0\0g\0\xff\xff\xff\xfb\0\0\0,\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0\x63\0l\0o\0u\0\x64\0V\0i\0\x65\0w\0\x65\0r\x1\0\0\0(\0\0\x2\x92\0\0\0\xe1\0\xff\xff\xff\xfb\0\0\0\x38\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0l\0o\0o\0p\0\x43\0l\0o\0s\0u\0r\0\x65\0V\0i\0\x65\0w\0\x65\0r\0\0\0\x1I\0\0\0\xf7\0\0\0\xf7\0\xff\xff\xff\0\0\0\x3\0\0\x5\0\0\0\0\x9c\xfc\x1\0\0\0\x5\xfb\0\0\0(\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0p\0o\0s\0t\0\x65\0r\0i\0o\0r\0\0\0\0\0\0\0\x5\0\0\0\0\x87\0\xff\xff\xff\xfb\0\0\0*\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0l\0i\0k\0\x65\0l\0i\0h\0o\0o\0\x64\0\0\0\0\0\xff\xff\xff\xff\0\0\0\x87\0\xff\xff\xff\xfb\0\0\0$\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0\x63\0o\0n\0s\0o\0l\0\x65\0\0\0\x5u\0\0\0\xea\0\0\0g\0\xff\xff\xff\xfb\0\0\0\x30\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0r\0\x61\0w\0l\0i\0k\0\x65\0l\0i\0h\0o\0o\0\x64\0\0\0\0\0\xff\xff\xff\xff\0\0\0\x87\0\xff\xff\xff\xfb\0\0\0\x30\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0m\0\x61\0p\0V\0i\0s\0i\0\x62\0i\0l\0i\0t\0y\0\0\0\0\0\xff\xff\xff\xff\0\0\0`\0\xff\xff\xff\0\0\0\0\0\0\x2\x92\0\0\0\x4\0\0\0\x4\0\0\0\b\0\0\0\b\xfc\0\0\0\x1\0\0\0\x2\0\0\0\x1\0\0\0\xe\0t\0o\0o\0l\0\x42\0\x61\0r\x1\0\0\0\0\xff\xff\xff\xff\0\0\0\0\0\0\0\0)" -MainWindow\geometry=@ByteArray(\x1\xd9\xd0\xcb\0\x1\0\0\0\0\x1\x9c\0\0\0\xa1\0\0\x6\xab\0\0\x3\x94\0\0\x1\xa4\0\0\0\xbd\0\0\x6\xa3\0\0\x3\x8c\0\0\0\0\0\0) +MainWindow\state="@ByteArray(\0\0\0\xff\0\0\0\0\xfd\0\0\0\x3\0\0\0\0\0\0\x1\a\0\0\x2\x92\xfc\x2\0\0\0\x1\xfb\0\0\0$\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0s\0t\0\x61\0t\0s\0V\0\x32\x1\0\0\0(\0\0\x2\x92\0\0\x1\xe5\0\xff\xff\xff\0\0\0\x1\0\0\x3\xf3\0\0\x2\x92\xfc\x2\0\0\0\x2\xfc\0\0\0(\0\0\x2\x92\0\0\0\xe1\0\xff\xff\xff\xfc\x1\0\0\0\x2\xfb\0\0\0(\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0i\0m\0\x61\0g\0\x65\0V\0i\0\x65\0w\x1\0\0\x1\r\0\0\x1\x12\0\0\0q\0\xff\xff\xff\xfc\0\0\x2%\0\0\x2\xdb\0\0\0\xe6\x1\0\0\x1d\xfa\0\0\0\x1\x2\0\0\0\x2\xfb\0\0\0,\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0g\0r\0\x61\0p\0h\0V\0i\0\x65\0w\0\x65\0r\x1\0\0\0\0\xff\xff\xff\xff\0\0\0g\0\xff\xff\xff\xfb\0\0\0,\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0\x63\0l\0o\0u\0\x64\0V\0i\0\x65\0w\0\x65\0r\x1\0\0\0(\0\0\x2\x92\0\0\0\xe1\0\xff\xff\xff\xfb\0\0\0\x38\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0l\0o\0o\0p\0\x43\0l\0o\0s\0u\0r\0\x65\0V\0i\0\x65\0w\0\x65\0r\0\0\0\x1I\0\0\0\xf7\0\0\0\xf7\0\xff\xff\xff\0\0\0\x3\0\0\x5\0\0\0\0\x9a\xfc\x1\0\0\0\x5\xfb\0\0\0(\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0p\0o\0s\0t\0\x65\0r\0i\0o\0r\0\0\0\0\0\0\0\x5\0\0\0\0\x87\0\xff\xff\xff\xfb\0\0\0*\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0l\0i\0k\0\x65\0l\0i\0h\0o\0o\0\x64\0\0\0\0\0\xff\xff\xff\xff\0\0\0\x87\0\xff\xff\xff\xfb\0\0\0$\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0\x63\0o\0n\0s\0o\0l\0\x65\0\0\0\0\0\0\0\x5\0\0\0\0g\0\xff\xff\xff\xfb\0\0\0\x30\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0r\0\x61\0w\0l\0i\0k\0\x65\0l\0i\0h\0o\0o\0\x64\0\0\0\0\0\xff\xff\xff\xff\0\0\0\x87\0\xff\xff\xff\xfb\0\0\0\x30\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0m\0\x61\0p\0V\0i\0s\0i\0\x62\0i\0l\0i\0t\0y\0\0\0\0\0\xff\xff\xff\xff\0\0\0`\0\xff\xff\xff\0\0\0\0\0\0\x2\x92\0\0\0\x4\0\0\0\x4\0\0\0\b\0\0\0\b\xfc\0\0\0\x1\0\0\0\x2\0\0\0\x1\0\0\0\xe\0t\0o\0o\0l\0\x42\0\x61\0r\x1\0\0\0\0\xff\xff\xff\xff\0\0\0\0\0\0\0\0)" +MainWindow\geometry=@ByteArray(\x1\xd9\xd0\xcb\0\x1\0\0\0\0\x1\x64\0\0\0\xa1\0\0\x6s\0\0\x3\x94\0\0\x1l\0\0\0\xbd\0\0\x6k\0\0\x3\x8c\0\0\0\0\0\0) General\showClouds0=true General\voxelSize0=0 General\decimation0=4 @@ -50,3 +50,15 @@ General\meshNormalKSearch1=20 General\meshGP3Radius1=0.04 General\meshSmoothing1=true General\meshSmoothingRadius1=0.04 +General\odomQualityThr=50 +General\meshing=false +General\meshGP3Radius=0.04 +General\meshNormalKSearch=20 +General\meshSmoothing=false +General\meshSmoothingRadius=0.04 +General\gridMapShown=false +General\gridMapResolution=0.05 +General\gridMapFillEmptySpace=true +General\gridMapOccupancyFrom3DCloud=false +General\gridMapFillEmptyRadius=0 +General\gridMapOpacity=0.75 diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp index 3ed8580b..c0c5c029 100644 --- a/src/CoreWrapper.cpp +++ b/src/CoreWrapper.cpp @@ -1000,9 +1000,9 @@ bool CoreWrapper::getMapCallback(rtabmap::GetMap::Request& req, rtabmap::GetMap: for(std::map::iterator iter = signatures.begin(); iter!=signatures.end(); ++iter) { rep.data.nodes[i].id = iter->second.id(); - rep.data.nodes[i].image.bytes = iter->second.getImage(); - rep.data.nodes[i].depth.bytes = iter->second.getDepth(); - rep.data.nodes[i].depth2D.bytes = iter->second.getDepth2D(); + compressedMatToBytes(iter->second.getImageCompressed(), rep.data.nodes[i].image.bytes); + compressedMatToBytes(iter->second.getDepthCompressed(), rep.data.nodes[i].depth.bytes); + compressedMatToBytes(iter->second.getDepth2DCompressed(), rep.data.nodes[i].depth2D.bytes); rep.data.nodes[i].fx = iter->second.getDepthFx(); rep.data.nodes[i].fy = iter->second.getDepthFy(); rep.data.nodes[i].cx = iter->second.getDepthCx(); @@ -1107,9 +1107,9 @@ bool CoreWrapper::publishMapCallback(rtabmap::PublishMap::Request& req, rtabmap: for(std::map::iterator iter = signatures.begin(); iter!=signatures.end(); ++iter) { msg->nodes[i].id = iter->second.id(); - msg->nodes[i].image.bytes = iter->second.getImage(); - msg->nodes[i].depth.bytes = iter->second.getDepth(); - msg->nodes[i].depth2D.bytes = iter->second.getDepth2D(); + compressedMatToBytes(iter->second.getImageCompressed(), msg->nodes[i].image.bytes); + compressedMatToBytes(iter->second.getDepthCompressed(), msg->nodes[i].depth.bytes); + compressedMatToBytes(iter->second.getDepth2DCompressed(), msg->nodes[i].depth2D.bytes); msg->nodes[i].fx = iter->second.getDepthFx(); msg->nodes[i].fy = iter->second.getDepthFy(); msg->nodes[i].cx = iter->second.getDepthCx(); @@ -1235,9 +1235,9 @@ void CoreWrapper::publishStats(const Statistics & stats) // add data msg->nodes.resize(1); msg->nodes[0].id = stats.getSignature().id(); - msg->nodes[0].image.bytes = stats.getSignature().getImage(); - msg->nodes[0].depth.bytes = stats.getSignature().getDepth(); - msg->nodes[0].depth2D.bytes = stats.getSignature().getDepth2D(); + compressedMatToBytes(stats.getSignature().getImageCompressed(), msg->nodes[0].image.bytes); + compressedMatToBytes(stats.getSignature().getDepthCompressed(), msg->nodes[0].depth.bytes); + compressedMatToBytes(stats.getSignature().getDepth2DCompressed(), msg->nodes[0].depth2D.bytes); msg->nodes[0].fx = stats.getSignature().getDepthFx(); msg->nodes[0].fy = stats.getSignature().getDepthFy(); msg->nodes[0].cx = stats.getSignature().getDepthCx(); diff --git a/src/GuiWrapper.cpp b/src/GuiWrapper.cpp index e288c2a9..03f69220 100644 --- a/src/GuiWrapper.cpp +++ b/src/GuiWrapper.cpp @@ -220,9 +220,9 @@ void GuiWrapper::infoMapCallback( words, std::multimap(), Transform(), // not set, see poses above - mapMsg->nodes[0].depth2D.bytes, - mapMsg->nodes[0].image.bytes, - mapMsg->nodes[0].depth.bytes, + compressedMatFromBytes(mapMsg->nodes[0].depth2D.bytes), + compressedMatFromBytes(mapMsg->nodes[0].image.bytes), + compressedMatFromBytes(mapMsg->nodes[0].depth.bytes), mapMsg->nodes[0].fx, mapMsg->nodes[0].fy, mapMsg->nodes[0].cx, @@ -304,9 +304,9 @@ void GuiWrapper::processRequestedMap(const rtabmap::MapData & map) words, std::multimap(), Transform(), // not set, see poses above - map.nodes[i].depth2D.bytes, - map.nodes[i].image.bytes, - map.nodes[i].depth.bytes, + compressedMatFromBytes(map.nodes[i].depth2D.bytes), + compressedMatFromBytes(map.nodes[i].image.bytes), + compressedMatFromBytes(map.nodes[i].depth.bytes), map.nodes[i].fx, map.nodes[i].fy, map.nodes[i].cx, diff --git a/src/MapAssemblerNode.cpp b/src/MapAssemblerNode.cpp index 5480c29f..7d8870f3 100644 --- a/src/MapAssemblerNode.cpp +++ b/src/MapAssemblerNode.cpp @@ -107,8 +107,8 @@ public: float cy = msg->nodes[i].cy; //uncompress data - util3d::CompressionThread ctImage(&msg->nodes[i].image.bytes, true); - util3d::CompressionThread ctDepth(&msg->nodes[i].depth.bytes, true); + util3d::CompressionThread ctImage(compressedMatFromBytes(msg->nodes[i].image.bytes, false), true); + util3d::CompressionThread ctDepth(compressedMatFromBytes(msg->nodes[i].depth.bytes, false), true); ctImage.start(); ctDepth.start(); ctImage.join(); diff --git a/src/MsgConversion.cpp b/src/MsgConversion.cpp index ce160bab..14d77a4c 100644 --- a/src/MsgConversion.cpp +++ b/src/MsgConversion.cpp @@ -97,4 +97,29 @@ rtabmap::Transform transformFromPoseMsg(const geometry_msgs::Pose & msg) return transformFromTF(tfTransform); } +void compressedMatToBytes(const cv::Mat & compressed, std::vector & bytes) +{ + UASSERT(compressed.empty() || compressed.type() == CV_8UC1); + bytes.clear(); + if(!compressed.empty()) + { + bytes.resize(compressed.cols * compressed.rows); + memcpy(bytes.data(), compressed.data, bytes.size()); + } +} + +cv::Mat compressedMatFromBytes(const std::vector & bytes, bool copy) +{ + cv::Mat out; + if(bytes.size()) + { + out = cv::Mat(1, bytes.size(), CV_8UC1, (void*)bytes.data()); + if(copy) + { + out = out.clone(); + } + } + return out; +} + } diff --git a/src/rviz/MapCloudDisplay.cpp b/src/rviz/MapCloudDisplay.cpp index 045ed3f7..2aa5d227 100644 --- a/src/rviz/MapCloudDisplay.cpp +++ b/src/rviz/MapCloudDisplay.cpp @@ -258,8 +258,8 @@ void MapCloudDisplay::processMapData(const rtabmap::MapData& map) float cy = map.nodes[i].cy; //uncompress data - util3d::CompressionThread ctImage(&map.nodes[i].image.bytes, true); - util3d::CompressionThread ctDepth(&map.nodes[i].depth.bytes, true); + util3d::CompressionThread ctImage(compressedMatFromBytes(map.nodes[i].image.bytes, false), true); + util3d::CompressionThread ctDepth(compressedMatFromBytes(map.nodes[i].depth.bytes, false), true); ctImage.start(); ctDepth.start(); ctImage.join();