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

ROS package: Added link constraints to MapData.msg, moved GridMapAssembler::create2DMap() to util3d namespace

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/ros-pkg@1076 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-02-11 23:08:37 +00:00
parent df1ee86df7
commit b9b175443a
4 changed files with 69 additions and 104 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap/msg/MapData.msg
+7 -1
View File
@@ -31,7 +31,13 @@ float32[] depthConstants
int32[] localTransformIDs
geometry_msgs/Transform[] localTransforms
# std::map<int, Poses> poses;
# std::map<int, Pose> poses;
int32[] poseIDs
geometry_msgs/Pose[] poses
# std::multimap<int, Link> constraints;
int32[] constraintFromIDs
int32[] constraintToIDs
int32[] constraintTypes
geometry_msgs/Transform[] constraints
rtabmap/src/CoreWrapper.cpp
+30
View File
@@ -625,6 +625,7 @@ void CoreWrapper::publishMapData(bool full)
std::map<int, float> depthConstants;
std::map<int, Transform> localTransforms;
std::map<int, Transform> poses;
std::multimap<int, Link> constraints;
if(mapData_.getNumSubscribers())
{
@@ -637,6 +638,7 @@ void CoreWrapper::publishMapData(bool full)
depthConstants,
localTransforms,
poses,
constraints,
true,
full);
@@ -702,6 +704,20 @@ void CoreWrapper::publishMapData(bool full)
++i;
}
msg->constraintFromIDs.resize(constraints.size());
msg->constraintToIDs.resize(constraints.size());
msg->constraintTypes.resize(constraints.size());
msg->constraints.resize(constraints.size());
i=0;
for(std::multimap<int, Link>::iterator iter = constraints.begin(); iter!=constraints.end(); ++iter)
{
msg->constraintFromIDs[i] = iter->first;
msg->constraintToIDs[i] = iter->second.to();
msg->constraintTypes[i] = iter->second.type();
transformToGeometryMsg(iter->second.transform(), msg->constraints[i]);
++i;
}
mapData_.publish(msg);
}
}
@@ -763,6 +779,20 @@ void CoreWrapper::publishStats(const Statistics & stats)
++i;
}
msg->data.constraintFromIDs.resize(stats.constraints().size());
msg->data.constraintToIDs.resize(stats.constraints().size());
msg->data.constraintTypes.resize(stats.constraints().size());
msg->data.constraints.resize(stats.constraints().size());
i=0;
for(std::multimap<int, Link>::const_iterator iter = stats.constraints().begin(); iter!=stats.constraints().end(); ++iter)
{
msg->data.constraintFromIDs[i] = iter->first;
msg->data.constraintToIDs[i] = iter->second.to();
msg->data.constraintTypes[i] = iter->second.type();
transformToGeometryMsg(iter->second.transform(), msg->data.constraints[i]);
++i;
}
transformToGeometryMsg(stats.mapCorrection(), msg->mapCorrection);
transformToGeometryMsg(stats.loopClosureTransform(), msg->loopClosureTransform);
transformToPoseMsg(stats.currentPose(), msg->currentPose);
rtabmap/src/GridMapAssemblerNode.cpp
+1 -102
View File
@@ -83,7 +83,7 @@ public:
// create the map
float xMin=0.0f, yMin=0.0f;
cv::Mat pixels = create2DMap(poses, delta, xMin, yMin);
cv::Mat pixels = util3d::create2DMap(poses, scans_, delta, xMin, yMin);
if(!pixels.empty())
{
@@ -134,107 +134,6 @@ public:
}
}
cv::Mat create2DMap(const std::map<int, Transform> & poses, float delta, float & xMin, float & yMin)
{
std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > scans;
pcl::PointCloud<pcl::PointXYZ> minMax;
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
if(uContains(scans_, iter->first))
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::transformPointCloud(scans_.at(iter->first), iter->second);
pcl::PointXYZ min, max;
pcl::getMinMax3D(*cloud, min, max);
minMax.push_back(min);
minMax.push_back(max);
minMax.push_back(pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z()));
scans.insert(std::make_pair(iter->first, cloud));
}
}
cv::Mat map;
if(minMax.size())
{
//Get map size
pcl::PointXYZ min, max;
pcl::getMinMax3D(minMax, min, max);
xMin = min.x-1.0f;
yMin = min.y-1.0f;
float xMax = max.x+1.0f;
float yMax = max.y+1.0f;
map = cv::Mat::ones((yMax - yMin) / delta, (xMax - xMin) / delta, CV_8S)*-1;
for(std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::iterator iter = scans.begin(); iter!=scans.end(); ++iter)
{
for(unsigned int i=0; i<iter->second->size(); ++i)
{
const Transform & pose = poses.at(iter->first);
cv::Point2i start((pose.x()-xMin)/delta + 0.5f, (pose.y()-yMin)/delta + 0.5f);
cv::Point2i end((iter->second->points[i].x-xMin)/delta + 0.5f, (iter->second->points[i].y-yMin)/delta + 0.5f);
rayTrace(start, end, map); // trace free space
map.at<char>(end.y, end.x) = 100; // obstacle
}
}
}
return map;
}
void rayTrace(const cv::Point2i & start, const cv::Point2i & end, cv::Mat & grid)
{
UASSERT_MSG(start.x >= 0 && start.x < grid.cols, uFormat("start.x=%d grid.cols=%d", start.x, grid.cols).c_str());
UASSERT_MSG(start.y >= 0 && start.y < grid.rows, uFormat("start.y=%d grid.rows=%d", start.y, grid.rows).c_str());
UASSERT_MSG(end.x >= 0 && end.x < grid.cols, uFormat("end.x=%d grid.cols=%d", end.x, grid.cols).c_str());
UASSERT_MSG(end.y >= 0 && end.y < grid.rows, uFormat("end.x=%d grid.cols=%d", end.y, grid.rows).c_str());
cv::Point2i ptA, ptB;
if(start.x > end.x)
{
ptA = end;
ptB = start;
}
else
{
ptA = start;
ptB = end;
}
float slope = float(ptB.y - ptA.y)/float(ptB.x - ptA.x);
float b = ptA.y - slope*ptA.x;
//ROS_WARN("start=%d,%d end=%d,%d", ptA.x, ptA.y, ptB.x, ptB.y);
//ROS_WARN("y = %f*x + %f", slope, b);
for(int x=ptA.x; x<ptB.x; ++x)
{
float lowerbound = float(x)*slope + b;
float upperbound = float(x+1)*slope + b;
if(lowerbound > upperbound)
{
float tmp = lowerbound;
lowerbound = upperbound;
upperbound = tmp;
}
//ROS_WARN("lowerbound=%f upperbound=%f", lowerbound, upperbound);
UASSERT_MSG(lowerbound >= 0 && lowerbound < grid.rows, uFormat("lowerbound=%f grid.cols=%d x=%d slope=%f b=%f", lowerbound, grid.cols, x, slope, b).c_str());
UASSERT_MSG(upperbound >= 0 && upperbound < grid.rows, uFormat("upperbound=%f grid.cols=%d x+1=%d slope=%f b=%f", upperbound, grid.cols, x+1, slope, b).c_str());
for(int y = lowerbound; y<=(int)upperbound; ++y)
{
//if(grid.at<char>(y, x) == -1)
{
grid.at<char>(y, x) = 0; // free space
}
}
}
}
bool getMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res)
{
if(map_.data.size())
rtabmap/src/GuiWrapper.cpp
+31 -1
View File
@@ -219,6 +219,14 @@ void GuiWrapper::infoExReceivedCallback(const rtabmap::InfoExConstPtr & msg)
}
stat.setPoses(poses);
std::multimap<int, Link> constraints;
for(unsigned int i=0; i<msg->data.constraintFromIDs.size() && i<msg->data.constraintToIDs.size() && i<msg->data.constraintTypes.size() && i < msg->data.constraints.size(); ++i)
{
Transform t = transformFromGeometryMsg(msg->data.constraints[i]);
constraints.insert(std::make_pair(msg->data.constraintFromIDs[i], Link(msg->data.constraintFromIDs[i], msg->data.constraintToIDs[i], t, (Link::Type)msg->data.constraintTypes[i])));
}
stat.setConstraints(constraints);
std::map<int, int> mapIds;
for(unsigned int i=0; i<msg->data.mapIDs.size() && i<msg->data.maps.size(); ++i)
{
@@ -237,6 +245,7 @@ void GuiWrapper::mapDataReceivedCallback(const rtabmap::MapDataConstPtr & msg)
std::map<int, float> depthConstants;
std::map<int, Transform> localTransforms;
std::map<int, Transform> poses;
std::multimap<int, Link> constraints;
if(msg->imageIDs.size() != msg->images.size())
{
@@ -262,6 +271,20 @@ void GuiWrapper::mapDataReceivedCallback(const rtabmap::MapDataConstPtr & msg)
(int)msg->depthConstants.size(), (int)msg->depthConstantIDs.size());
}
if(msg->poseIDs.size() != msg->poses.size())
{
ROS_WARN("rtabmapviz: receiving map... poses and IDs are not the same size (%d vs %d)!",
(int)msg->poses.size(), (int)msg->poseIDs.size());
}
if(msg->constraintFromIDs.size() != msg->constraints.size() ||
msg->constraintToIDs.size() != msg->constraints.size() ||
msg->constraintTypes.size() != msg->constraints.size())
{
ROS_WARN("rtabmapviz: receiving map... constraints and IDs are not the same size (%d vs %d vs %d vs %d)!",
(int)msg->constraints.size(), (int)msg->constraintFromIDs.size(), (int)msg->constraintToIDs.size(), (int)msg->constraintTypes.size());
}
for(unsigned int i=0; i<msg->imageIDs.size() && i < msg->images.size(); ++i)
{
images.insert(std::make_pair(msg->imageIDs[i], msg->images[i].bytes));
@@ -294,12 +317,19 @@ void GuiWrapper::mapDataReceivedCallback(const rtabmap::MapDataConstPtr & msg)
poses.insert(std::make_pair(msg->poseIDs[i], t));
}
for(unsigned int i=0; i<msg->constraintFromIDs.size() && i<msg->constraintToIDs.size() && i<msg->constraintTypes.size() && i < msg->constraints.size(); ++i)
{
Transform t = transformFromGeometryMsg(msg->constraints[i]);
constraints.insert(std::make_pair(msg->constraintFromIDs[i], Link(msg->constraintFromIDs[i], msg->constraintToIDs[i], t, (Link::Type)msg->constraintTypes[i])));
}
this->post(new RtabmapEvent3DMap(images,
depths,
depths2d,
depthConstants,
localTransforms,
poses));
poses,
constraints));
}
void GuiWrapper::handleEvent(UEvent * anEvent)