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

ros: modified core to send only latest data in /mapData messages

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/ros-pkg@1545 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-07-11 21:45:50 +00:00
parent 9cb6d311c7
commit 0bd7f67bd2
1 changed files with 24 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CoreWrapper.cpp
+24 -38
View File
@@ -883,56 +883,42 @@ void CoreWrapper::publishStats(const Statistics & stats)
++index; ++index;
} }
msg->imageIDs.resize(stats.getImages().size()); //
msg->images.resize(stats.getImages().size()); // just add the last data for bandwidth efficiency
index = 0; //
for(std::map<int, std::vector<unsigned char> >::const_iterator i=stats.getImages().begin(); if(uContains(stats.getImages(), stats.refImageId()))
i!=stats.getImages().end();
++i)
{ {
msg->imageIDs[index] = i->first; msg->imageIDs.push_back(stats.refImageId());
msg->images[index].bytes = i->second; msg->images.resize(1);
++index; msg->images[0].bytes = stats.getImages().at(stats.refImageId());
} }
msg->depthIDs.resize(stats.getDepths().size()); if(uContains(stats.getDepths(), stats.refImageId()))
msg->depths.resize(stats.getDepths().size());
index = 0;
for(std::map<int, std::vector<unsigned char> >::const_iterator i=stats.getDepths().begin();
i!=stats.getDepths().end();
++i)
{ {
msg->depthIDs[index] = i->first; msg->depthIDs.push_back(stats.refImageId());
msg->depths[index].bytes = i->second; msg->depths.resize(1);
++index; msg->depths[0].bytes = stats.getDepths().at(stats.refImageId());
} }
msg->depth2DIDs.resize(stats.getDepth2ds().size()); if(uContains(stats.getDepth2ds(), stats.refImageId()))
msg->depth2Ds.resize(stats.getDepth2ds().size());
index = 0;
for(std::map<int, std::vector<unsigned char> >::const_iterator i=stats.getDepth2ds().begin();
i!=stats.getDepth2ds().end();
++i)
{ {
msg->depth2DIDs[index] = i->first; msg->depth2DIDs.push_back(stats.refImageId());
msg->depth2Ds[index].bytes = i->second; msg->depth2Ds.resize(1);
++index; msg->depth2Ds[0].bytes = stats.getDepth2ds().at(stats.refImageId());
} }
msg->localTransformIDs.resize(stats.getLocalTransforms().size()); if(uContains(stats.getLocalTransforms(), stats.refImageId()))
msg->localTransforms.resize(stats.getLocalTransforms().size());
index = 0;
for(std::map<int, Transform>::const_iterator i=stats.getLocalTransforms().begin();
i!=stats.getLocalTransforms().end();
++i)
{ {
msg->localTransformIDs[index] = i->first; msg->localTransformIDs.push_back(stats.refImageId());
transformToGeometryMsg(i->second, msg->localTransforms[index]); msg->localTransforms.resize(1);
++index; transformToGeometryMsg(stats.getLocalTransforms().at(stats.refImageId()), msg->localTransforms[0]);
} }
msg->depthConstantIDs = uKeys(stats.getDepthConstants()); if(uContains(stats.getDepthConstants(), stats.refImageId()))
msg->depthConstants = uValues(stats.getDepthConstants()); {
msg->depthConstantIDs.push_back(stats.refImageId());
msg->depthConstants.push_back(stats.getDepthConstants().at(stats.refImageId()));
}
mapData_.publish(msg); mapData_.publish(msg);
} }