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

updated for compressed cv::Mat data format

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/ros-pkg@1938 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-10-28 19:59:19 +00:00
parent 52c6bd60eb
commit f122090620
7 changed files with 63 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CoreWrapper.cpp
+9 -9
View File
@@ -1000,9 +1000,9 @@ bool CoreWrapper::getMapCallback(rtabmap::GetMap::Request& req, rtabmap::GetMap:
for(std::map<int, Signature>::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<int, Signature>::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();
src/GuiWrapper.cpp
+6 -6
View File
@@ -220,9 +220,9 @@ void GuiWrapper::infoMapCallback(
words,
std::multimap<int, pcl::PointXYZ>(),
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<int, pcl::PointXYZ>(),
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,
src/MapAssemblerNode.cpp
+2 -2
View File
@@ -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();
src/MsgConversion.cpp
+25
View File
@@ -97,4 +97,29 @@ rtabmap::Transform transformFromPoseMsg(const geometry_msgs::Pose & msg)
return transformFromTF(tfTransform);
}
void compressedMatToBytes(const cv::Mat & compressed, std::vector<unsigned char> & 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<unsigned char> & 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;
}
}
src/rviz/MapCloudDisplay.cpp
+2 -2
View File
@@ -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();