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

ros-pkg: refactoring, updated rtabmap ROS msgs for easier processing (new NodeData.msg).

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/ros-pkg@1929 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-10-26 21:49:30 +00:00
parent 60c1e06059
commit b3933881ce
11 changed files with 322 additions and 656 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CMakeLists.txt
+1
View File
@@ -42,6 +42,7 @@ add_message_files(
InfoEx.msg InfoEx.msg
KeyPoint.msg KeyPoint.msg
MapData.msg MapData.msg
NodeData.msg
Bytes.msg Bytes.msg
) )
msg/Info.msg
-3
View File
@@ -10,7 +10,4 @@ int32 refId
int32 loopClosureId int32 loopClosureId
int32 localLoopClosureId int32 localLoopClosureId
geometry_msgs/Transform mapCorrection
geometry_msgs/Transform loopClosureTransform geometry_msgs/Transform loopClosureTransform
geometry_msgs/Pose currentPose
msg/InfoEx.msg
+1 -13
View File
@@ -9,13 +9,10 @@ int32 refId
int32 loopClosureId int32 loopClosureId
int32 localLoopClosureId int32 localLoopClosureId
geometry_msgs/Transform mapCorrection
geometry_msgs/Transform loopClosureTransform geometry_msgs/Transform loopClosureTransform
geometry_msgs/Pose currentPose
#### ####
# For statistics and visualization ... # For statistics...
#### ####
# std::map<int, float> posterior; # std::map<int, float> posterior;
int32[] posteriorKeys int32[] posteriorKeys
@@ -36,12 +33,3 @@ int32[] weightsValues
# std::map<std::string, float> stats # std::map<std::string, float> stats
string[] statsKeys string[] statsKeys
float32[] statsValues float32[] statsValues
#
# For features2d : std::multimap<int, cv::Keypoint> words
#
int32[] refWordsKeys
rtabmap/KeyPoint[] refWordsValues
int32[] loopWordsKeys
rtabmap/KeyPoint[] loopWordsValues
msg/MapData.msg
+16 -40
View File
@@ -1,47 +1,13 @@
Header header Header header
# Map ids std::map<nodeId, mapId> ##################
int32[] mapIDs # Graph stuff
int32[] maps ##################
# compressed images # /map to /odom transform
# use rtabmap::util3d::uncompressImage() from <rtabmap/core/util3d.h> # Always identity when the graph is optimized from the latest pose.
# std::map<int, uint8[]> images; geometry_msgs/Transform mapToOdom
int32[] imageIDs
rtabmap/Bytes[] images
# compressed depth images
# use rtabmap::util3d::uncompressImage() from <rtabmap/core/util3d.h>
# std::map<int, uint8[]> depths;
int32[] depthIDs
rtabmap/Bytes[] depths
# compressed 2D point clouds (laser scans)
# use rtabmap::util3d::uncompressData() from <rtabmap/core/util3d.h>
# std::map<int, uint8[]> depths2d;
int32[] depth2DIDs
rtabmap/Bytes[] depth2Ds
# depth fx
int32[] depthFxIDs
float32[] depthFxs
# depth fy
int32[] depthFyIDs
float32[] depthFys
# depth cx
int32[] depthCxIDs
float32[] depthCxs
# depth cy
int32[] depthCyIDs
float32[] depthCys
# local transforms
int32[] localTransformIDs
geometry_msgs/Transform[] localTransforms
# std::map<int, Pose> poses; # std::map<int, Pose> poses;
int32[] poseIDs int32[] poseIDs
@@ -53,3 +19,13 @@ int32[] constraintToIDs
int32[] constraintTypes int32[] constraintTypes
geometry_msgs/Transform[] constraints geometry_msgs/Transform[] constraints
# Map ids std::map<nodeId, mapId>
int32[] mapIDs
int32[] maps
##################
# Point cloud stuff
##################
NodeData[] nodes
msg/NodeData.msg
+27
View File
@@ -0,0 +1,27 @@
int32 id
# compressed image in /camera_link frame
# use rtabmap::util3d::uncompressImage() from <rtabmap/core/util3d.h>
rtabmap/Bytes image
# compressed depth image in /camera_link frame
# use rtabmap::util3d::uncompressImage() from <rtabmap/core/util3d.h>
rtabmap/Bytes depth
float32 fx
float32 fy
float32 cx
float32 cy
# compressed 2D point cloud (laser scan) in /base_link frame
# use rtabmap::util3d::uncompressData() from <rtabmap/core/util3d.h>
rtabmap/Bytes depth2D
# local transform (/base_link -> /camera_link)
geometry_msgs/Transform localTransform
# std::multimap<int, cv::Keypoint> words
int32[] wordsKeys
rtabmap/KeyPoint[] wordsValues
src/CoreWrapper.cpp
+106 -286
View File
@@ -928,14 +928,7 @@ bool CoreWrapper::getMapCallback(rtabmap::GetMap::Request& req, rtabmap::GetMap:
req.global?"true":"false", req.global?"true":"false",
req.optimized?"true":"false", req.optimized?"true":"false",
req.graphOnly?"true":"false"); req.graphOnly?"true":"false");
std::map<int, std::vector<unsigned char> > images; std::map<int, Signature> signatures;
std::map<int, std::vector<unsigned char> > depths;
std::map<int, std::vector<unsigned char> > depths2d;
std::map<int, float> depthFxs;
std::map<int, float> depthFys;
std::map<int, float> depthCxs;
std::map<int, float> depthCys;
std::map<int, Transform> localTransforms;
std::map<int, Transform> poses; std::map<int, Transform> poses;
std::multimap<int, Link> constraints; std::multimap<int, Link> constraints;
std::map<int, int> mapIds; std::map<int, int> mapIds;
@@ -952,14 +945,7 @@ bool CoreWrapper::getMapCallback(rtabmap::GetMap::Request& req, rtabmap::GetMap:
else else
{ {
rtabmap_.get3DMap( rtabmap_.get3DMap(
images, signatures,
depths,
depths2d,
depthFxs,
depthFys,
depthCxs,
depthCys,
localTransforms,
poses, poses,
constraints, constraints,
mapIds, mapIds,
@@ -980,84 +966,6 @@ bool CoreWrapper::getMapCallback(rtabmap::GetMap::Request& req, rtabmap::GetMap:
++i; ++i;
} }
rep.data.imageIDs.resize(images.size());
rep.data.images.resize(images.size());
i=0;
for(std::map<int, std::vector<unsigned char> >::iterator iter = images.begin(); iter!=images.end(); ++iter)
{
rep.data.imageIDs[i] = iter->first;
rep.data.images[i].bytes = iter->second;
++i;
}
rep.data.depthIDs.resize(depths.size());
rep.data.depths.resize(depths.size());
i=0;
for(std::map<int, std::vector<unsigned char> >::iterator iter = depths.begin(); iter!=depths.end(); ++iter)
{
rep.data.depthIDs[i] = iter->first;
rep.data.depths[i].bytes = iter->second;
++i;
}
rep.data.depth2DIDs.resize(depths2d.size());
rep.data.depth2Ds.resize(depths2d.size());
i=0;
for(std::map<int, std::vector<unsigned char> >::iterator iter = depths2d.begin(); iter!=depths2d.end(); ++iter)
{
rep.data.depth2DIDs[i] = iter->first;
rep.data.depth2Ds[i].bytes = iter->second;
++i;
}
// fx,fy,cx,cy parameters
rep.data.depthFxIDs.resize(depthFxs.size());
rep.data.depthFxs.resize(depthFxs.size());
i=0;
for(std::map<int, float>::iterator iter = depthFxs.begin(); iter!=depthFxs.end(); ++iter)
{
rep.data.depthFxIDs[i] = iter->first;
rep.data.depthFxs[i] = iter->second;
++i;
}
rep.data.depthFyIDs.resize(depthFys.size());
rep.data.depthFys.resize(depthFys.size());
i=0;
for(std::map<int, float>::iterator iter = depthFys.begin(); iter!=depthFys.end(); ++iter)
{
rep.data.depthFyIDs[i] = iter->first;
rep.data.depthFys[i] = iter->second;
++i;
}
rep.data.depthCxIDs.resize(depthCxs.size());
rep.data.depthCxs.resize(depthCxs.size());
i=0;
for(std::map<int, float>::iterator iter = depthCxs.begin(); iter!=depthCxs.end(); ++iter)
{
rep.data.depthCxIDs[i] = iter->first;
rep.data.depthCxs[i] = iter->second;
++i;
}
rep.data.depthCyIDs.resize(depthCys.size());
rep.data.depthCys.resize(depthCys.size());
i=0;
for(std::map<int, float>::iterator iter = depthCys.begin(); iter!=depthCys.end(); ++iter)
{
rep.data.depthCyIDs[i] = iter->first;
rep.data.depthCys[i] = iter->second;
++i;
}
rep.data.localTransformIDs.resize(localTransforms.size());
rep.data.localTransforms.resize(localTransforms.size());
i=0;
for(std::map<int, Transform>::iterator iter = localTransforms.begin(); iter!=localTransforms.end(); ++iter)
{
rep.data.localTransformIDs[i] = iter->first;
transformToGeometryMsg(iter->second, rep.data.localTransforms[i]);
++i;
}
rep.data.poseIDs.resize(poses.size()); rep.data.poseIDs.resize(poses.size());
rep.data.poses.resize(poses.size()); rep.data.poses.resize(poses.size());
i=0; i=0;
@@ -1082,6 +990,42 @@ bool CoreWrapper::getMapCallback(rtabmap::GetMap::Request& req, rtabmap::GetMap:
++i; ++i;
} }
// add data
rep.data.nodes.resize(signatures.size());
i=0;
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();
rep.data.nodes[i].fx = iter->second.getDepthFx();
rep.data.nodes[i].fy = iter->second.getDepthFy();
rep.data.nodes[i].cx = iter->second.getDepthCx();
rep.data.nodes[i].cy = iter->second.getDepthCy();
transformToGeometryMsg(iter->second.getLocalTransform(), rep.data.nodes[i].localTransform);
//Features stuff...
rep.data.nodes[i].wordsKeys = uKeys(iter->second.getWords());
rep.data.nodes[i].wordsValues.resize(iter->second.getWords().size());
int j = 0;
for(std::multimap<int, cv::KeyPoint>::const_iterator jter=iter->second.getWords().begin();
jter!=iter->second.getWords().end();
++jter)
{
rep.data.nodes[i].wordsValues.at(j).angle = jter->second.angle;
rep.data.nodes[i].wordsValues.at(j).response = jter->second.response;
rep.data.nodes[i].wordsValues.at(j).ptx = jter->second.pt.x;
rep.data.nodes[i].wordsValues.at(j).pty = jter->second.pt.y;
rep.data.nodes[i].wordsValues.at(j).size = jter->second.size;
rep.data.nodes[i].wordsValues.at(j).octave = jter->second.octave;
rep.data.nodes[i].wordsValues.at(j).class_id = jter->second.class_id;
++j;
}
++i;
}
rep.data.header.stamp = ros::Time::now(); rep.data.header.stamp = ros::Time::now();
rep.data.header.frame_id = mapFrameId_; rep.data.header.frame_id = mapFrameId_;
@@ -1094,14 +1038,7 @@ bool CoreWrapper::publishMapCallback(rtabmap::PublishMap::Request& req, rtabmap:
{ {
ROS_INFO("rtabmap: Publishing map..."); ROS_INFO("rtabmap: Publishing map...");
std::map<int, std::vector<unsigned char> > images; std::map<int, Signature> signatures;
std::map<int, std::vector<unsigned char> > depths;
std::map<int, std::vector<unsigned char> > depths2d;
std::map<int, float> depthFxs;
std::map<int, float> depthFys;
std::map<int, float> depthCxs;
std::map<int, float> depthCys;
std::map<int, Transform> localTransforms;
std::map<int, Transform> poses; std::map<int, Transform> poses;
std::multimap<int, Link> constraints; std::multimap<int, Link> constraints;
std::map<int, int> mapIds; std::map<int, int> mapIds;
@@ -1118,14 +1055,7 @@ bool CoreWrapper::publishMapCallback(rtabmap::PublishMap::Request& req, rtabmap:
else else
{ {
rtabmap_.get3DMap( rtabmap_.get3DMap(
images, signatures,
depths,
depths2d,
depthFxs,
depthFys,
depthCxs,
depthCys,
localTransforms,
poses, poses,
constraints, constraints,
mapIds, mapIds,
@@ -1140,93 +1070,8 @@ bool CoreWrapper::publishMapCallback(rtabmap::PublishMap::Request& req, rtabmap:
int i=0; int i=0;
msg->mapIDs.resize(mapIds.size()); msg->mapIDs = uKeys(mapIds);
msg->maps.resize(mapIds.size()); msg->maps = uValues(mapIds);
i=0;
for(std::map<int, int>::iterator iter = mapIds.begin(); iter!=mapIds.end(); ++iter)
{
msg->mapIDs[i] = iter->first;
msg->maps[i] = iter->second;
++i;
}
msg->imageIDs.resize(images.size());
msg->images.resize(images.size());
i=0;
for(std::map<int, std::vector<unsigned char> >::iterator iter = images.begin(); iter!=images.end(); ++iter)
{
msg->imageIDs[i] = iter->first;
msg->images[i].bytes = iter->second;
++i;
}
msg->depthIDs.resize(depths.size());
msg->depths.resize(depths.size());
i=0;
for(std::map<int, std::vector<unsigned char> >::iterator iter = depths.begin(); iter!=depths.end(); ++iter)
{
msg->depthIDs[i] = iter->first;
msg->depths[i].bytes = iter->second;
++i;
}
msg->depth2DIDs.resize(depths2d.size());
msg->depth2Ds.resize(depths2d.size());
i=0;
for(std::map<int, std::vector<unsigned char> >::iterator iter = depths2d.begin(); iter!=depths2d.end(); ++iter)
{
msg->depth2DIDs[i] = iter->first;
msg->depth2Ds[i].bytes = iter->second;
++i;
}
// fx,fy,cx,cy parameters
msg->depthFxIDs.resize(depthFxs.size());
msg->depthFxs.resize(depthFxs.size());
i=0;
for(std::map<int, float>::iterator iter = depthFxs.begin(); iter!=depthFxs.end(); ++iter)
{
msg->depthFxIDs[i] = iter->first;
msg->depthFxs[i] = iter->second;
++i;
}
msg->depthFyIDs.resize(depthFys.size());
msg->depthFys.resize(depthFys.size());
i=0;
for(std::map<int, float>::iterator iter = depthFys.begin(); iter!=depthFys.end(); ++iter)
{
msg->depthFyIDs[i] = iter->first;
msg->depthFys[i] = iter->second;
++i;
}
msg->depthCxIDs.resize(depthCxs.size());
msg->depthCxs.resize(depthCxs.size());
i=0;
for(std::map<int, float>::iterator iter = depthCxs.begin(); iter!=depthCxs.end(); ++iter)
{
msg->depthCxIDs[i] = iter->first;
msg->depthCxs[i] = iter->second;
++i;
}
msg->depthCyIDs.resize(depthCys.size());
msg->depthCys.resize(depthCys.size());
i=0;
for(std::map<int, float>::iterator iter = depthCys.begin(); iter!=depthCys.end(); ++iter)
{
msg->depthCyIDs[i] = iter->first;
msg->depthCys[i] = iter->second;
++i;
}
msg->localTransformIDs.resize(localTransforms.size());
msg->localTransforms.resize(localTransforms.size());
i=0;
for(std::map<int, Transform>::iterator iter = localTransforms.begin(); iter!=localTransforms.end(); ++iter)
{
msg->localTransformIDs[i] = iter->first;
transformToGeometryMsg(iter->second, msg->localTransforms[i]);
++i;
}
msg->poseIDs.resize(poses.size()); msg->poseIDs.resize(poses.size());
msg->poses.resize(poses.size()); msg->poses.resize(poses.size());
@@ -1252,6 +1097,42 @@ bool CoreWrapper::publishMapCallback(rtabmap::PublishMap::Request& req, rtabmap:
++i; ++i;
} }
// add data
msg->nodes.resize(signatures.size());
i=0;
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();
msg->nodes[i].fx = iter->second.getDepthFx();
msg->nodes[i].fy = iter->second.getDepthFy();
msg->nodes[i].cx = iter->second.getDepthCx();
msg->nodes[i].cy = iter->second.getDepthCy();
transformToGeometryMsg(iter->second.getLocalTransform(), msg->nodes[i].localTransform);
//Features stuff...
msg->nodes[i].wordsKeys = uKeys(iter->second.getWords());
msg->nodes[i].wordsValues.resize(iter->second.getWords().size());
int j = 0;
for(std::multimap<int, cv::KeyPoint>::const_iterator jter=iter->second.getWords().begin();
jter!=iter->second.getWords().end();
++jter)
{
msg->nodes[i].wordsValues.at(j).angle = jter->second.angle;
msg->nodes[i].wordsValues.at(j).response = jter->second.response;
msg->nodes[i].wordsValues.at(j).ptx = jter->second.pt.x;
msg->nodes[i].wordsValues.at(j).pty = jter->second.pt.y;
msg->nodes[i].wordsValues.at(j).size = jter->second.size;
msg->nodes[i].wordsValues.at(j).octave = jter->second.octave;
msg->nodes[i].wordsValues.at(j).class_id = jter->second.class_id;
++j;
}
++i;
}
mapData_.publish(msg); mapData_.publish(msg);
} }
return true; return true;
@@ -1271,9 +1152,7 @@ void CoreWrapper::publishStats(const Statistics & stats)
msg->loopClosureId = stats.loopClosureId(); msg->loopClosureId = stats.loopClosureId();
msg->localLoopClosureId = stats.localLoopClosureId(); msg->localLoopClosureId = stats.localLoopClosureId();
transformToGeometryMsg(stats.mapCorrection(), msg->mapCorrection);
transformToGeometryMsg(stats.loopClosureTransform(), msg->loopClosureTransform); transformToGeometryMsg(stats.loopClosureTransform(), msg->loopClosureTransform);
transformToPoseMsg(stats.currentPose(), msg->currentPose);
infoPub_.publish(msg); infoPub_.publish(msg);
} }
@@ -1289,9 +1168,7 @@ void CoreWrapper::publishStats(const Statistics & stats)
msg->loopClosureId = stats.loopClosureId(); msg->loopClosureId = stats.loopClosureId();
msg->localLoopClosureId = stats.localLoopClosureId(); msg->localLoopClosureId = stats.localLoopClosureId();
transformToGeometryMsg(stats.mapCorrection(), msg->mapCorrection);
transformToGeometryMsg(stats.loopClosureTransform(), msg->loopClosureTransform); transformToGeometryMsg(stats.loopClosureTransform(), msg->loopClosureTransform);
transformToPoseMsg(stats.currentPose(), msg->currentPose);
// Detailed info // Detailed info
if(stats.extended()) if(stats.extended())
@@ -1306,41 +1183,6 @@ void CoreWrapper::publishStats(const Statistics & stats)
msg->weightsKeys = uKeys(stats.weights()); msg->weightsKeys = uKeys(stats.weights());
msg->weightsValues = uValues(stats.weights()); msg->weightsValues = uValues(stats.weights());
//Features stuff...
msg->refWordsKeys = uKeys(stats.refWords());
msg->refWordsValues = std::vector<rtabmap::KeyPoint>(stats.refWords().size());
int index = 0;
for(std::multimap<int, cv::KeyPoint>::const_iterator i=stats.refWords().begin();
i!=stats.refWords().end();
++i)
{
msg->refWordsValues.at(index).angle = i->second.angle;
msg->refWordsValues.at(index).response = i->second.response;
msg->refWordsValues.at(index).ptx = i->second.pt.x;
msg->refWordsValues.at(index).pty = i->second.pt.y;
msg->refWordsValues.at(index).size = i->second.size;
msg->refWordsValues.at(index).octave = i->second.octave;
msg->refWordsValues.at(index).class_id = i->second.class_id;
++index;
}
msg->loopWordsKeys = uKeys(stats.loopWords());
msg->loopWordsValues = std::vector<rtabmap::KeyPoint>(stats.loopWords().size());
index = 0;
for(std::multimap<int, cv::KeyPoint>::const_iterator i=stats.loopWords().begin();
i!=stats.loopWords().end();
++i)
{
msg->loopWordsValues.at(index).angle = i->second.angle;
msg->loopWordsValues.at(index).response = i->second.response;
msg->loopWordsValues.at(index).ptx = i->second.pt.x;
msg->loopWordsValues.at(index).pty = i->second.pt.y;
msg->loopWordsValues.at(index).size = i->second.size;
msg->loopWordsValues.at(index).octave = i->second.octave;
msg->loopWordsValues.at(index).class_id = i->second.class_id;
++index;
}
// Statistics data // Statistics data
msg->statsKeys = uKeys(stats.data()); msg->statsKeys = uKeys(stats.data());
msg->statsValues = uValues(stats.data()); msg->statsValues = uValues(stats.data());
@@ -1355,6 +1197,8 @@ void CoreWrapper::publishStats(const Statistics & stats)
msg->header.stamp = timeNow; msg->header.stamp = timeNow;
msg->header.frame_id = mapFrameId_; msg->header.frame_id = mapFrameId_;
transformToGeometryMsg(stats.mapCorrection(), msg->mapToOdom);
msg->mapIDs = uKeys(stats.getMapIds()); msg->mapIDs = uKeys(stats.getMapIds());
msg->maps = uValues(stats.getMapIds()); msg->maps = uValues(stats.getMapIds());
@@ -1384,60 +1228,36 @@ void CoreWrapper::publishStats(const Statistics & stats)
++index; ++index;
} }
// // add data
// just add the last data for bandwidth efficiency msg->nodes.resize(1);
// msg->nodes[0].id = stats.getSignature().id();
if(uContains(stats.getImages(), stats.refImageId())) msg->nodes[0].image.bytes = stats.getSignature().getImage();
{ msg->nodes[0].depth.bytes = stats.getSignature().getDepth();
msg->imageIDs.push_back(stats.refImageId()); msg->nodes[0].depth2D.bytes = stats.getSignature().getDepth2D();
msg->images.resize(1); msg->nodes[0].fx = stats.getSignature().getDepthFx();
msg->images[0].bytes = stats.getImages().at(stats.refImageId()); msg->nodes[0].fy = stats.getSignature().getDepthFy();
} msg->nodes[0].cx = stats.getSignature().getDepthCx();
msg->nodes[0].cy = stats.getSignature().getDepthCy();
transformToGeometryMsg(stats.getSignature().getLocalTransform(), msg->nodes[0].localTransform);
if(uContains(stats.getDepths(), stats.refImageId())) //Features stuff...
msg->nodes[0].wordsKeys = uKeys(stats.getSignature().getWords());
msg->nodes[0].wordsValues.resize(stats.getSignature().getWords().size());
index = 0;
for(std::multimap<int, cv::KeyPoint>::const_iterator jter=stats.getSignature().getWords().begin();
jter!=stats.getSignature().getWords().end();
++jter)
{ {
msg->depthIDs.push_back(stats.refImageId()); msg->nodes[0].wordsValues.at(index).angle = jter->second.angle;
msg->depths.resize(1); msg->nodes[0].wordsValues.at(index).response = jter->second.response;
msg->depths[0].bytes = stats.getDepths().at(stats.refImageId()); msg->nodes[0].wordsValues.at(index).ptx = jter->second.pt.x;
msg->nodes[0].wordsValues.at(index).pty = jter->second.pt.y;
msg->nodes[0].wordsValues.at(index).size = jter->second.size;
msg->nodes[0].wordsValues.at(index).octave = jter->second.octave;
msg->nodes[0].wordsValues.at(index).class_id = jter->second.class_id;
++index;
} }
if(uContains(stats.getDepth2ds(), stats.refImageId()))
{
msg->depth2DIDs.push_back(stats.refImageId());
msg->depth2Ds.resize(1);
msg->depth2Ds[0].bytes = stats.getDepth2ds().at(stats.refImageId());
}
if(uContains(stats.getLocalTransforms(), stats.refImageId()))
{
msg->localTransformIDs.push_back(stats.refImageId());
msg->localTransforms.resize(1);
transformToGeometryMsg(stats.getLocalTransforms().at(stats.refImageId()), msg->localTransforms[0]);
}
// fx,fy,cx,cy parameters
if(uContains(stats.getDepthFxs(), stats.refImageId()))
{
msg->depthFxIDs.push_back(stats.refImageId());
msg->depthFxs.push_back(stats.getDepthFxs().at(stats.refImageId()));
}
if(uContains(stats.getDepthFys(), stats.refImageId()))
{
msg->depthFyIDs.push_back(stats.refImageId());
msg->depthFys.push_back(stats.getDepthFys().at(stats.refImageId()));
}
if(uContains(stats.getDepthCxs(), stats.refImageId()))
{
msg->depthCxIDs.push_back(stats.refImageId());
msg->depthCxs.push_back(stats.getDepthCxs().at(stats.refImageId()));
}
if(uContains(stats.getDepthCys(), stats.refImageId()))
{
msg->depthCyIDs.push_back(stats.refImageId());
msg->depthCys.push_back(stats.getDepthCys().at(stats.refImageId()));
}
mapData_.publish(msg); mapData_.publish(msg);
} }
} }
src/GridMapAssemblerNode.cpp
+4 -4
View File
@@ -69,12 +69,12 @@ public:
void mapDataReceivedCallback(const rtabmap::MapDataConstPtr & msg) void mapDataReceivedCallback(const rtabmap::MapDataConstPtr & msg)
{ {
for(unsigned int i=0; i<msg->depth2DIDs.size() && i<msg->depth2Ds.size(); ++i) for(unsigned int i=0; i<msg->nodes.size(); ++i)
{ {
if(!uContains(scans_, msg->depth2DIDs[i])) if(!uContains(scans_, msg->nodes[i].id) && msg->nodes[i].depth2D.bytes.size())
{ {
cv::Mat depth2d = util3d::uncompressData(msg->depth2Ds[i].bytes); cv::Mat depth2d = util3d::uncompressData(msg->nodes[i].depth2D.bytes);
scans_.insert(std::make_pair(msg->depth2DIDs[i], util3d::depth2DToPointCloud(depth2d))); scans_.insert(std::make_pair(msg->nodes[i].id, util3d::depth2DToPointCloud(depth2d)));
} }
} }
src/GuiWrapper.cpp
+71 -178
View File
@@ -167,32 +167,6 @@ void GuiWrapper::infoMapCallback(
} }
stat.setWeights(mapIntInt); stat.setWeights(mapIntInt);
//SURF stuff...
std::multimap<int, cv::KeyPoint> mapIntKeypoint;
for(unsigned int i=0; i<infoMsg->refWordsKeys.size() && i<infoMsg->refWordsValues.size(); ++i)
{
cv::KeyPoint pt;
pt.angle = infoMsg->refWordsValues.at(i).angle;
pt.response = infoMsg->refWordsValues.at(i).response;
pt.pt.x = infoMsg->refWordsValues.at(i).ptx;
pt.pt.y = infoMsg->refWordsValues.at(i).pty;
pt.size = infoMsg->refWordsValues.at(i).size;
mapIntKeypoint.insert(std::pair<int, cv::KeyPoint>(infoMsg->refWordsKeys.at(i), pt));
}
stat.setRefWords(mapIntKeypoint);
mapIntKeypoint.clear();
for(unsigned int i=0; i<infoMsg->loopWordsKeys.size() && i<infoMsg->loopWordsValues.size(); ++i)
{
cv::KeyPoint pt;
pt.angle = infoMsg->loopWordsValues.at(i).angle;
pt.response = infoMsg->loopWordsValues.at(i).response;
pt.pt.x = infoMsg->loopWordsValues.at(i).ptx;
pt.pt.y = infoMsg->loopWordsValues.at(i).pty;
pt.size = infoMsg->loopWordsValues.at(i).size;
mapIntKeypoint.insert(std::pair<int, cv::KeyPoint>(infoMsg->loopWordsKeys.at(i), pt));
}
stat.setLoopWords(mapIntKeypoint);
// Statistics data // Statistics data
for(unsigned int i=0; i<infoMsg->statsKeys.size() && i<infoMsg->statsValues.size(); i++) for(unsigned int i=0; i<infoMsg->statsKeys.size() && i<infoMsg->statsValues.size(); i++)
{ {
@@ -200,65 +174,8 @@ void GuiWrapper::infoMapCallback(
} }
//RGB-D SLAM data //RGB-D SLAM data
stat.setMapCorrection(transformFromGeometryMsg(infoMsg->mapCorrection)); stat.setMapCorrection(transformFromGeometryMsg(mapMsg->mapToOdom));
stat.setLoopClosureTransform(transformFromGeometryMsg(infoMsg->loopClosureTransform)); stat.setLoopClosureTransform(transformFromGeometryMsg(infoMsg->loopClosureTransform));
stat.setCurrentPose(transformFromPoseMsg(infoMsg->currentPose));
std::map<int, std::vector<unsigned char> > images;
for(unsigned int i=0; i<mapMsg->imageIDs.size() && i<mapMsg->images.size(); ++i)
{
images.insert(std::make_pair(mapMsg->imageIDs[i], mapMsg->images[i].bytes));
}
stat.setImages(images);
std::map<int, std::vector<unsigned char> > depths;
for(unsigned int i=0; i<mapMsg->depthIDs.size() && i<mapMsg->depths.size(); ++i)
{
depths.insert(std::make_pair(mapMsg->depthIDs[i], mapMsg->depths[i].bytes));
}
stat.setDepths(depths);
std::map<int, std::vector<unsigned char> > depth2ds;
for(unsigned int i=0; i<mapMsg->depth2DIDs.size() && i<mapMsg->depth2Ds.size(); ++i)
{
depth2ds.insert(std::make_pair(mapMsg->depth2DIDs[i], mapMsg->depth2Ds[i].bytes));
}
stat.setDepth2ds(depth2ds);
std::map<int, float> depthFxs;
for(unsigned int i=0; i<mapMsg->depthFxIDs.size() && i<mapMsg->depthFxs.size(); ++i)
{
depthFxs.insert(std::make_pair(mapMsg->depthFxIDs[i], mapMsg->depthFxs[i]));
}
stat.setDepthFxs(depthFxs);
std::map<int, float> depthFys;
for(unsigned int i=0; i<mapMsg->depthFyIDs.size() && i<mapMsg->depthFys.size(); ++i)
{
depthFys.insert(std::make_pair(mapMsg->depthFyIDs[i], mapMsg->depthFys[i]));
}
stat.setDepthFys(depthFys);
std::map<int, float> depthCxs;
for(unsigned int i=0; i<mapMsg->depthCxIDs.size() && i<mapMsg->depthCxs.size(); ++i)
{
depthCxs.insert(std::make_pair(mapMsg->depthCxIDs[i], mapMsg->depthCxs[i]));
}
stat.setDepthCxs(depthCxs);
std::map<int, float> depthCys;
for(unsigned int i=0; i<mapMsg->depthCyIDs.size() && i<mapMsg->depthCys.size(); ++i)
{
depthCys.insert(std::make_pair(mapMsg->depthCyIDs[i], mapMsg->depthCys[i]));
}
stat.setDepthCys(depthCys);
std::map<int, Transform> localTransforms;
for(unsigned int i=0; i<mapMsg->localTransformIDs.size() && i<mapMsg->localTransforms.size(); ++i)
{
localTransforms.insert(std::make_pair(mapMsg->localTransformIDs[i], transformFromGeometryMsg(mapMsg->localTransforms[i])));
}
stat.setLocalTransforms(localTransforms);
std::map<int, Transform> poses; std::map<int, Transform> poses;
for(unsigned int i=0; i<mapMsg->poseIDs.size() && i<mapMsg->poses.size(); ++i) for(unsigned int i=0; i<mapMsg->poseIDs.size() && i<mapMsg->poses.size(); ++i)
@@ -282,19 +199,48 @@ void GuiWrapper::infoMapCallback(
} }
stat.setMapIds(mapIds); stat.setMapIds(mapIds);
//data
if(mapMsg->nodes.size() == 1)
{
//Features stuff...
std::multimap<int, cv::KeyPoint> words;
for(unsigned int i=0; i<mapMsg->nodes[0].wordsKeys.size() && i<mapMsg->nodes[0].wordsValues.size(); ++i)
{
cv::KeyPoint pt;
pt.angle = mapMsg->nodes[0].wordsValues.at(i).angle;
pt.response = mapMsg->nodes[0].wordsValues.at(i).response;
pt.pt.x = mapMsg->nodes[0].wordsValues.at(i).ptx;
pt.pt.y = mapMsg->nodes[0].wordsValues.at(i).pty;
pt.size = mapMsg->nodes[0].wordsValues.at(i).size;
words.insert(std::pair<int, cv::KeyPoint>(mapMsg->nodes[0].wordsKeys.at(i), pt));
}
Signature signature(mapMsg->nodes[0].id,
-1, // not set, see maps above
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,
mapMsg->nodes[0].fx,
mapMsg->nodes[0].fy,
mapMsg->nodes[0].cx,
mapMsg->nodes[0].cy,
transformFromGeometryMsg(mapMsg->nodes[0].localTransform));
stat.setSignature(signature);
}
else if(mapMsg->nodes.size() > 1)
{
ROS_ERROR("rtabmapviz: nodes > 1 !?!?");
}
this->post(new RtabmapEvent(stat)); this->post(new RtabmapEvent(stat));
} }
void GuiWrapper::processRequestedMap(const rtabmap::MapData & map) void GuiWrapper::processRequestedMap(const rtabmap::MapData & map)
{ {
std::map<int, std::vector<unsigned char> > images; std::map<int, Signature> signatures;
std::map<int, std::vector<unsigned char> > depths;
std::map<int, std::vector<unsigned char> > depths2d;
std::map<int, float> depthFxs;
std::map<int, float> depthFys;
std::map<int, float> depthCxs;
std::map<int, float> depthCys;
std::map<int, Transform> localTransforms;
std::map<int, Transform> poses; std::map<int, Transform> poses;
std::multimap<int, Link> constraints; std::multimap<int, Link> constraints;
std::map<int, int> mapIds; std::map<int, int> mapIds;
@@ -305,45 +251,6 @@ void GuiWrapper::processRequestedMap(const rtabmap::MapData & map)
(int)map.maps.size(), (int)map.mapIDs.size()); (int)map.maps.size(), (int)map.mapIDs.size());
} }
if(map.imageIDs.size() != map.images.size())
{
ROS_WARN("rtabmapviz: receiving map... images and IDs are not the same size (%d vs %d)!",
(int)map.images.size(), (int)map.imageIDs.size());
}
if(map.depthIDs.size() != map.depths.size())
{
ROS_WARN("rtabmapviz: receiving map... depths and IDs are not the same size (%d vs %d)!",
(int)map.depths.size(), (int)map.depthIDs.size());
}
if(map.depth2DIDs.size() != map.depth2Ds.size())
{
ROS_WARN("rtabmapviz: receiving map... depths2D and IDs are not the same size (%d vs %d)!",
(int)map.depth2Ds.size(), (int)map.depth2DIDs.size());
}
if(map.depthFxIDs.size() != map.depthFxs.size())
{
ROS_WARN("rtabmapviz: receiving map... depthFxs and IDs are not the same size (%d vs %d)!",
(int)map.depthFxs.size(), (int)map.depthFxIDs.size());
}
if(map.depthFyIDs.size() != map.depthFys.size())
{
ROS_WARN("rtabmapviz: receiving map... depthFys and IDs are not the same size (%d vs %d)!",
(int)map.depthFys.size(), (int)map.depthFyIDs.size());
}
if(map.depthCxIDs.size() != map.depthCxs.size())
{
ROS_WARN("rtabmapviz: receiving map... depthCxs and IDs are not the same size (%d vs %d)!",
(int)map.depthCxs.size(), (int)map.depthCxIDs.size());
}
if(map.depthCyIDs.size() != map.depthCys.size())
{
ROS_WARN("rtabmapviz: receiving map... depthCys and IDs are not the same size (%d vs %d)!",
(int)map.depthCys.size(), (int)map.depthCyIDs.size());
}
if(map.poseIDs.size() != map.poses.size()) if(map.poseIDs.size() != map.poses.size())
{ {
ROS_WARN("rtabmapviz: receiving map... poses and IDs are not the same size (%d vs %d)!", ROS_WARN("rtabmapviz: receiving map... poses and IDs are not the same size (%d vs %d)!",
@@ -363,45 +270,6 @@ void GuiWrapper::processRequestedMap(const rtabmap::MapData & map)
mapIds.insert(std::make_pair(map.mapIDs[i], map.maps[i])); mapIds.insert(std::make_pair(map.mapIDs[i], map.maps[i]));
} }
for(unsigned int i=0; i<map.imageIDs.size() && i < map.images.size(); ++i)
{
images.insert(std::make_pair(map.imageIDs[i], map.images[i].bytes));
}
for(unsigned int i=0; i<map.depthIDs.size() && i < map.depths.size(); ++i)
{
depths.insert(std::make_pair(map.depthIDs[i], map.depths[i].bytes));
}
for(unsigned int i=0; i<map.depth2DIDs.size() && i < map.depth2Ds.size(); ++i)
{
depths2d.insert(std::make_pair(map.depth2DIDs[i], map.depth2Ds[i].bytes));
}
for(unsigned int i=0; i<map.depthFxIDs.size() && i < map.depthFxs.size(); ++i)
{
depthFxs.insert(std::make_pair(map.depthFxIDs[i], map.depthFxs[i]));
}
for(unsigned int i=0; i<map.depthFyIDs.size() && i < map.depthFys.size(); ++i)
{
depthFys.insert(std::make_pair(map.depthFyIDs[i], map.depthFys[i]));
}
for(unsigned int i=0; i<map.depthCxIDs.size() && i < map.depthCxs.size(); ++i)
{
depthCxs.insert(std::make_pair(map.depthCxIDs[i], map.depthCxs[i]));
}
for(unsigned int i=0; i<map.depthCyIDs.size() && i < map.depthCys.size(); ++i)
{
depthCys.insert(std::make_pair(map.depthCyIDs[i], map.depthCys[i]));
}
for(unsigned int i=0; i<map.localTransformIDs.size() && i < map.localTransforms.size(); ++i)
{
Transform t = transformFromGeometryMsg(map.localTransforms[i]);
localTransforms.insert(std::make_pair(map.localTransformIDs[i], t));
}
for(unsigned int i=0; i<map.poseIDs.size() && i < map.poses.size(); ++i) for(unsigned int i=0; i<map.poseIDs.size() && i < map.poses.size(); ++i)
{ {
Transform t = transformFromPoseMsg(map.poses[i]); Transform t = transformFromPoseMsg(map.poses[i]);
@@ -414,14 +282,39 @@ void GuiWrapper::processRequestedMap(const rtabmap::MapData & map)
constraints.insert(std::make_pair(map.constraintFromIDs[i], Link(map.constraintFromIDs[i], map.constraintToIDs[i], t, (Link::Type)map.constraintTypes[i]))); constraints.insert(std::make_pair(map.constraintFromIDs[i], Link(map.constraintFromIDs[i], map.constraintToIDs[i], t, (Link::Type)map.constraintTypes[i])));
} }
this->post(new RtabmapEvent3DMap(images, //data
depths, for(unsigned int i=0; i<map.nodes.size(); ++i)
depths2d, {
depthFxs, //Features stuff...
depthFys, std::multimap<int, cv::KeyPoint> words;
depthCxs, for(unsigned int j=0; j<map.nodes[i].wordsKeys.size() && j<map.nodes[0].wordsValues.size(); ++j)
depthCys, {
localTransforms, cv::KeyPoint pt;
pt.angle = map.nodes[i].wordsValues.at(j).angle;
pt.response = map.nodes[i].wordsValues.at(j).response;
pt.pt.x = map.nodes[i].wordsValues.at(j).ptx;
pt.pt.y = map.nodes[i].wordsValues.at(j).pty;
pt.size = map.nodes[i].wordsValues.at(j).size;
words.insert(std::pair<int, cv::KeyPoint>(map.nodes[i].wordsKeys.at(j), pt));
}
signatures.insert(std::make_pair(map.nodes[i].id,
Signature(map.nodes[i].id,
-1, // not set, see maps above
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,
map.nodes[i].fx,
map.nodes[i].fy,
map.nodes[i].cx,
map.nodes[i].cy,
transformFromGeometryMsg(map.nodes[i].localTransform))));
}
this->post(new RtabmapEvent3DMap(signatures,
poses, poses,
constraints, constraints,
mapIds)); mapIds));
src/MapAssemblerNode.cpp
+22 -65
View File
@@ -92,79 +92,40 @@ public:
void mapDataReceivedCallback(const rtabmap::MapDataConstPtr & msg) void mapDataReceivedCallback(const rtabmap::MapDataConstPtr & msg)
{ {
for(unsigned int i=0; i<msg->localTransformIDs.size() && i<msg->localTransforms.size(); ++i) for(unsigned int i=0; i<msg->nodes.size(); ++i)
{ {
int id = msg->localTransformIDs[i]; int id = msg->nodes[i].id;
if(!uContains(rgbClouds_, id)) if(!uContains(rgbClouds_, id))
{ {
rtabmap::Transform localTransform = transformFromGeometryMsg(msg->localTransforms[i]); rtabmap::Transform localTransform = transformFromGeometryMsg(msg->nodes[i].localTransform);
if(!localTransform.isNull()) if(!localTransform.isNull())
{ {
cv::Mat image, depth; cv::Mat image, depth;
float depthFx = 0.0f; float fx = msg->nodes[i].fx;
float depthFy = 0.0f; float fy = msg->nodes[i].fy;
float depthCx = 0.0f; float cx = msg->nodes[i].cx;
float depthCy = 0.0f; float cy = msg->nodes[i].cy;
for(unsigned int i=0; i<msg->imageIDs.size() && i<msg->images.size(); ++i) //uncompress data
{ util3d::CompressionThread ctImage(msg->nodes[i].image.bytes, true);
if(msg->imageIDs[i] == id) util3d::CompressionThread ctDepth(msg->nodes[i].depth.bytes, true);
{ ctImage.start();
image = util3d::uncompressImage(msg->images[i].bytes); ctDepth.start();
break; ctImage.join();
} ctDepth.join();
} image = ctImage.getUncompressedData();
for(unsigned int i=0; i<msg->depthIDs.size() && i<msg->depths.size(); ++i) depth = ctDepth.getUncompressedData();
{
if(msg->depthIDs[i] == id)
{
depth = util3d::uncompressImage(msg->depths[i].bytes);
break;
}
}
for(unsigned int i=0; i<msg->depthFxIDs.size() && i<msg->depthFxs.size(); ++i)
{
if(msg->depthFxIDs[i] == id)
{
depthFx = msg->depthFxs[i];
break;
}
}
for(unsigned int i=0; i<msg->depthFyIDs.size() && i<msg->depthFys.size(); ++i)
{
if(msg->depthFyIDs[i] == id)
{
depthFy = msg->depthFys[i];
break;
}
}
for(unsigned int i=0; i<msg->depthCxIDs.size() && i<msg->depthCxs.size(); ++i)
{
if(msg->depthCxIDs[i] == id)
{
depthCx = msg->depthCxs[i];
break;
}
}
for(unsigned int i=0; i<msg->depthCyIDs.size() && i<msg->depthCys.size(); ++i)
{
if(msg->depthCyIDs[i] == id)
{
depthCy = msg->depthCys[i];
break;
}
}
if(!image.empty() && !depth.empty() && depthFx > 0.0f && depthFy > 0.0f && depthCx >= 0.0f && depthCy >= 0.0f) if(!image.empty() && !depth.empty() && fx > 0.0f && fy > 0.0f && cx >= 0.0f && cy >= 0.0f)
{ {
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud; pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
if(depth.type() == CV_8UC1) if(depth.type() == CV_8UC1)
{ {
cloud = util3d::cloudFromStereoImages(image, depth, depthCx, depthCy, depthFx, depthFy, cloudDecimation_); cloud = util3d::cloudFromStereoImages(image, depth, cx, cy, fx, fy, cloudDecimation_);
} }
else else
{ {
cloud = util3d::cloudFromDepthRGB(image, depth, depthCx, depthCy, depthFx, depthFy, cloudDecimation_); cloud = util3d::cloudFromDepthRGB(image, depth, cx, cy, fx, fy, cloudDecimation_);
} }
if(cloudMaxDepth_ > 0) if(cloudMaxDepth_ > 0)
@@ -196,14 +157,10 @@ public:
} }
} }
} }
}
if(!uContains(scans_, id) && msg->nodes[i].depth2D.bytes.size())
for(unsigned int i=0; i<msg->depth2DIDs.size() && i<msg->depth2Ds.size(); ++i)
{
if(!uContains(scans_, msg->depth2DIDs[i]))
{ {
cv::Mat depth2d = util3d::uncompressData(msg->depth2Ds[i].bytes); cv::Mat depth2d = util3d::uncompressData(msg->nodes[i].depth2D.bytes);
if(!depth2d.empty()) if(!depth2d.empty())
{ {
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::depth2DToPointCloud(depth2d); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::depth2DToPointCloud(depth2d);
@@ -212,7 +169,7 @@ public:
cloud = util3d::voxelize<pcl::PointXYZ>(cloud, scanVoxelSize_); cloud = util3d::voxelize<pcl::PointXYZ>(cloud, scanVoxelSize_);
} }
scans_.insert(std::make_pair(msg->depth2DIDs[i], cloud)); scans_.insert(std::make_pair(id, cloud));
} }
} }
} }
src/MapOptimizerNode.cpp
+51 -7
View File
@@ -139,47 +139,82 @@ public:
// optimize only local map // optimize only local map
poses_.clear(); poses_.clear();
constraints_.clear(); constraints_.clear();
mapIds_.clear();
} }
// save new poses and constraints // save new poses and constraints
// Assuming that nodes/constraints are all linked together // Assuming that nodes/constraints are all linked together
UASSERT(msg->poseIDs.size() == msg->poses.size()); UASSERT(msg->poseIDs.size() == msg->poses.size());
UASSERT(msg->mapIDs.size() == msg->poseIDs.size());
UASSERT(msg->mapIDs.size() == msg->maps.size());
std::map<int, Transform> newPoses;
std::map<int, int> newMapIds;
for(unsigned int i=0; i<msg->poseIDs.size() && i<msg->poseIDs.size(); ++i) for(unsigned int i=0; i<msg->poseIDs.size() && i<msg->poseIDs.size(); ++i)
{ {
poses_.insert(std::make_pair(msg->poseIDs[i], transformFromPoseMsg(msg->poses[i]))); newPoses.insert(std::make_pair(msg->poseIDs[i], transformFromPoseMsg(msg->poses[i])));
newMapIds.insert(std::make_pair(msg->mapIDs[i], msg->maps[i]));
} }
UASSERT(msg->constraints.size() == msg->constraintFromIDs.size() && UASSERT(msg->constraints.size() == msg->constraintFromIDs.size() &&
msg->constraints.size() == msg->constraintToIDs.size() && msg->constraints.size() == msg->constraintToIDs.size() &&
msg->constraints.size() == msg->constraintTypes.size()); msg->constraints.size() == msg->constraintTypes.size());
std::multimap<int, Link> allNewConstraints;
std::multimap<int, Link> filteredNewConstraints;
bool constraintsChanged = false;
for(unsigned int i=0; i<msg->constraints.size() && i<msg->constraints.size(); ++i) for(unsigned int i=0; i<msg->constraints.size() && i<msg->constraints.size(); ++i)
{ {
Link link(msg->constraintFromIDs[i], msg->constraintToIDs[i], transformFromGeometryMsg(msg->constraints[i]), (Link::Type)msg->constraintTypes[i]);
allNewConstraints.insert(std::make_pair(link.from(), link));
bool edgeAlreadyAdded = false; bool edgeAlreadyAdded = false;
for(std::multimap<int, Link>::iterator iter = constraints_.lower_bound(msg->constraintFromIDs[i]); for(std::multimap<int, Link>::iterator iter = constraints_.lower_bound(link.from());
iter != constraints_.end() && iter->first == msg->constraintFromIDs[i]; iter != constraints_.end() && iter->first == link.from();
++iter) ++iter)
{ {
if(iter->second.to() == msg->constraintToIDs[i]) if(iter->second.to() == link.to())
{ {
edgeAlreadyAdded = true; edgeAlreadyAdded = true;
if(iter->second.transform() != link.transform())
{
constraintsChanged = true;
}
} }
} }
if(!edgeAlreadyAdded) if(!edgeAlreadyAdded)
{ {
constraints_.insert(std::make_pair(msg->constraintFromIDs[i], Link(msg->constraintFromIDs[i], msg->constraintToIDs[i], transformFromGeometryMsg(msg->constraints[i]), (Link::Type)msg->constraintTypes[i]))); filteredNewConstraints.insert(std::make_pair(link.from(), link));
} }
} }
//If a transform has changed, clear all.
if(constraintsChanged)
{
UWARN("Some received constraints have changed from the cached "
"constraints. RTAB-Map is restarted? If yes, ignore this "
"warning. Clearing all cached constraints and restart with "
"the new ones...");
poses_ = newPoses;
mapIds_ = newMapIds;
constraints_ = allNewConstraints;
}
else
{
poses_.insert(newPoses.begin(), newPoses.end());
mapIds_.insert(newMapIds.begin(), newMapIds.end());
constraints_.insert(filteredNewConstraints.begin(), filteredNewConstraints.end());
}
// Optimize only if there is a subscriber // Optimize only if there is a subscriber
if(mapDataPub_.getNumSubscribers()) if(mapDataPub_.getNumSubscribers())
{ {
std::map<int, Transform> optimizedPoses; std::map<int, Transform> optimizedPoses;
std::map<int, int> mapIds = mapIds_;
Transform mapCorrection = Transform::getIdentity();
if(poses_.size() > 1 && constraints_.size() > 0) if(poses_.size() > 1 && constraints_.size() > 0)
{ {
Transform mapCorrectionToro; Transform mapCorrectionToro;
util3d::optimizeTOROGraph(poses_, constraints_, optimizedPoses, mapCorrectionToro, iterations_, true); util3d::optimizeTOROGraph(poses_, constraints_, optimizedPoses, mapCorrectionToro, iterations_, true);
mapToOdomMutex_.lock(); mapToOdomMutex_.lock();
Transform mapCorrection = optimizedPoses.at(poses_.rbegin()->first) * poses_.rbegin()->second.inverse(); mapCorrection = optimizedPoses.at(poses_.rbegin()->first) * poses_.rbegin()->second.inverse();
rtabmap::transformToTF(mapCorrection, mapToOdom_); rtabmap::transformToTF(mapCorrection, mapToOdom_);
mapToOdomMutex_.unlock(); mapToOdomMutex_.unlock();
} }
@@ -192,16 +227,24 @@ public:
ROS_ERROR("map_optimizer: Poses=%zu and edges=%zu (poses must " ROS_ERROR("map_optimizer: Poses=%zu and edges=%zu (poses must "
"not be null if there are edges, and edges must be null if poses <= 1)", "not be null if there are edges, and edges must be null if poses <= 1)",
poses_.size(), constraints_.size()); poses_.size(), constraints_.size());
mapIds.clear();
} }
UASSERT(optimizedPoses.size() == mapIds.size());
rtabmap::MapData outputMsg = *msg; rtabmap::MapData outputMsg = *msg;
outputMsg.poseIDs.resize(optimizedPoses.size()); outputMsg.poseIDs.resize(optimizedPoses.size());
outputMsg.poses.resize(optimizedPoses.size()); outputMsg.poses.resize(optimizedPoses.size());
outputMsg.mapIDs.resize(mapIds.size());
outputMsg.maps.resize(mapIds.size());
rtabmap::transformToGeometryMsg(mapCorrection, outputMsg.mapToOdom);
int i=0; int i=0;
for(std::map<int, Transform>::iterator iter = optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter) std::map<int, int>::iterator jter = mapIds.begin();
for(std::map<int, Transform>::iterator iter = optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter, ++jter)
{ {
outputMsg.poseIDs[i] = iter->first; outputMsg.poseIDs[i] = iter->first;
transformToPoseMsg(iter->second, outputMsg.poses[i]); transformToPoseMsg(iter->second, outputMsg.poses[i]);
outputMsg.mapIDs[i] = jter->first;
outputMsg.maps[i] = jter->second;
++i; ++i;
} }
mapDataPub_.publish(outputMsg); mapDataPub_.publish(outputMsg);
@@ -225,6 +268,7 @@ private:
std::map<int, Transform> poses_; std::map<int, Transform> poses_;
std::multimap<int, Link> constraints_; std::multimap<int, Link> constraints_;
std::map<int, int> mapIds_;
tf::TransformBroadcaster tfBroadcaster_; tf::TransformBroadcaster tfBroadcaster_;
boost::thread* transformThread_; boost::thread* transformThread_;
src/rviz/MapCloudDisplay.cpp
+23 -60
View File
@@ -242,80 +242,41 @@ void MapCloudDisplay::processMessage( const rtabmap::MapDataConstPtr& msg )
void MapCloudDisplay::processMapData(const rtabmap::MapData& map) void MapCloudDisplay::processMapData(const rtabmap::MapData& map)
{ {
// Add new clouds... // Add new clouds...
for(unsigned int i=0; i<map.localTransformIDs.size() && i<map.localTransforms.size(); ++i) for(unsigned int i=0; i<map.nodes.size() && i<map.nodes.size(); ++i)
{ {
int id = map.localTransformIDs[i]; int id = map.nodes[i].id;
if(cloud_infos_.find(id) == cloud_infos_.end()) if(cloud_infos_.find(id) == cloud_infos_.end())
{ {
// Cloud not added to RVIZ, add it! // Cloud not added to RVIZ, add it!
rtabmap::Transform localTransform = transformFromGeometryMsg(map.localTransforms[i]); rtabmap::Transform localTransform = transformFromGeometryMsg(map.nodes[i].localTransform);
if(!localTransform.isNull()) if(!localTransform.isNull())
{ {
cv::Mat image, depth; cv::Mat image, depth;
float depthFx = 0.0f; float fx = map.nodes[i].fx;
float depthFy = 0.0f; float fy = map.nodes[i].fy;
float depthCx = 0.0f; float cx = map.nodes[i].cx;
float depthCy = 0.0f; float cy = map.nodes[i].cy;
for(unsigned int i=0; i<map.imageIDs.size() && i<map.images.size(); ++i) //uncompress data
{ util3d::CompressionThread ctImage(map.nodes[i].image.bytes, true);
if(map.imageIDs[i] == id) util3d::CompressionThread ctDepth(map.nodes[i].depth.bytes, true);
{ ctImage.start();
image = util3d::uncompressImage(map.images[i].bytes); ctDepth.start();
break; ctImage.join();
} ctDepth.join();
} image = ctImage.getUncompressedData();
for(unsigned int i=0; i<map.depthIDs.size() && i<map.depths.size(); ++i) depth = ctDepth.getUncompressedData();
{
if(map.depthIDs[i] == id)
{
depth = util3d::uncompressImage(map.depths[i].bytes);
break;
}
}
for(unsigned int i=0; i<map.depthFxIDs.size() && i<map.depthFxs.size(); ++i)
{
if(map.depthFxIDs[i] == id)
{
depthFx = map.depthFxs[i];
break;
}
}
for(unsigned int i=0; i<map.depthFyIDs.size() && i<map.depthFys.size(); ++i)
{
if(map.depthFyIDs[i] == id)
{
depthFy = map.depthFys[i];
break;
}
}
for(unsigned int i=0; i<map.depthCxIDs.size() && i<map.depthCxs.size(); ++i)
{
if(map.depthCxIDs[i] == id)
{
depthCx = map.depthCxs[i];
break;
}
}
for(unsigned int i=0; i<map.depthCyIDs.size() && i<map.depthCys.size(); ++i)
{
if(map.depthCyIDs[i] == id)
{
depthCy = map.depthCys[i];
break;
}
}
if(!image.empty() && !depth.empty() && depthFx > 0.0f && depthFy > 0.0f && depthCx >= 0.0f && depthCy >= 0.0f) if(!image.empty() && !depth.empty() && fx > 0.0f && fy > 0.0f && cx >= 0.0f && cy >= 0.0f)
{ {
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud; pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
if(depth.type() == CV_8UC1) if(depth.type() == CV_8UC1)
{ {
cloud = util3d::cloudFromStereoImages(image, depth, depthCx, depthCy, depthFx, depthFy, cloud_decimation_->getInt()); cloud = util3d::cloudFromStereoImages(image, depth, cx, cy, fx, fy, cloud_decimation_->getInt());
} }
else else
{ {
cloud = util3d::cloudFromDepthRGB(image, depth, depthCx, depthCy, depthFx, depthFy, cloud_decimation_->getInt()); cloud = util3d::cloudFromDepthRGB(image, depth, cx, cy, fx, fy, cloud_decimation_->getInt());
} }
if(cloud_max_depth_->getFloat() > 0.0f) if(cloud_max_depth_->getFloat() > 0.0f)
{ {
@@ -537,11 +498,13 @@ void MapCloudDisplay::downloadMap()
} }
else else
{ {
messageBox->setText(tr("Creating all clouds (%1 poses and %2 clouds downloaded)...").arg(getMapSrv.response.data.poses.size()).arg(getMapSrv.response.data.depths.size())); messageBox->setText(tr("Creating all clouds (%1 poses and %2 clouds downloaded)...")
.arg(getMapSrv.response.data.poses.size()).arg(getMapSrv.response.data.nodes.size()));
QApplication::processEvents(); QApplication::processEvents();
this->reset(); this->reset();
processMapData(getMapSrv.response.data); processMapData(getMapSrv.response.data);
messageBox->setText(tr("Creating all clouds (%1 poses and %2 clouds downloaded)... done!").arg(getMapSrv.response.data.poses.size()).arg(getMapSrv.response.data.depths.size())); messageBox->setText(tr("Creating all clouds (%1 poses and %2 clouds downloaded)... done!")
.arg(getMapSrv.response.data.poses.size()).arg(getMapSrv.response.data.nodes.size()));
QTimer::singleShot(1000, messageBox, SLOT(close())); QTimer::singleShot(1000, messageBox, SLOT(close()));
} }