From b9b175443a81f54ac0105021271c531b80875161 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 11 Feb 2014 23:08:37 +0000 Subject: [PATCH] ROS package: Added link constraints to MapData.msg, moved GridMapAssembler::create2DMap() to util3d namespace git-svn-id: http://rtabmap.googlecode.com/svn/trunk/ros-pkg@1076 f169173b-cf89-36c8-b27e-44dbe73f0c83 --- rtabmap/msg/MapData.msg | 8 ++- rtabmap/src/CoreWrapper.cpp | 30 ++++++++ rtabmap/src/GridMapAssemblerNode.cpp | 103 +-------------------------- rtabmap/src/GuiWrapper.cpp | 32 ++++++++- 4 files changed, 69 insertions(+), 104 deletions(-) diff --git a/rtabmap/msg/MapData.msg b/rtabmap/msg/MapData.msg index 8c3748ef..cf535e64 100644 --- a/rtabmap/msg/MapData.msg +++ b/rtabmap/msg/MapData.msg @@ -31,7 +31,13 @@ float32[] depthConstants int32[] localTransformIDs geometry_msgs/Transform[] localTransforms -# std::map poses; +# std::map poses; int32[] poseIDs geometry_msgs/Pose[] poses +# std::multimap constraints; +int32[] constraintFromIDs +int32[] constraintToIDs +int32[] constraintTypes +geometry_msgs/Transform[] constraints + diff --git a/rtabmap/src/CoreWrapper.cpp b/rtabmap/src/CoreWrapper.cpp index 204dff0a..0f4b35a2 100644 --- a/rtabmap/src/CoreWrapper.cpp +++ b/rtabmap/src/CoreWrapper.cpp @@ -625,6 +625,7 @@ void CoreWrapper::publishMapData(bool full) std::map depthConstants; std::map localTransforms; std::map poses; + std::multimap 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::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::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); diff --git a/rtabmap/src/GridMapAssemblerNode.cpp b/rtabmap/src/GridMapAssemblerNode.cpp index 0dddd1bc..16df96e4 100644 --- a/rtabmap/src/GridMapAssemblerNode.cpp +++ b/rtabmap/src/GridMapAssemblerNode.cpp @@ -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 & poses, float delta, float & xMin, float & yMin) - { - std::map::Ptr > scans; - - pcl::PointCloud minMax; - for(std::map::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter) - { - if(uContains(scans_, iter->first)) - { - pcl::PointCloud::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::Ptr >::iterator iter = scans.begin(); iter!=scans.end(); ++iter) - { - for(unsigned int i=0; isecond->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(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 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(y, x) == -1) - { - grid.at(y, x) = 0; // free space - } - } - } - } - bool getMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res) { if(map_.data.size()) diff --git a/rtabmap/src/GuiWrapper.cpp b/rtabmap/src/GuiWrapper.cpp index b80846ac..9bd02e2c 100644 --- a/rtabmap/src/GuiWrapper.cpp +++ b/rtabmap/src/GuiWrapper.cpp @@ -219,6 +219,14 @@ void GuiWrapper::infoExReceivedCallback(const rtabmap::InfoExConstPtr & msg) } stat.setPoses(poses); + std::multimap constraints; + for(unsigned int i=0; idata.constraintFromIDs.size() && idata.constraintToIDs.size() && idata.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 mapIds; for(unsigned int i=0; idata.mapIDs.size() && idata.maps.size(); ++i) { @@ -237,6 +245,7 @@ void GuiWrapper::mapDataReceivedCallback(const rtabmap::MapDataConstPtr & msg) std::map depthConstants; std::map localTransforms; std::map poses; + std::multimap 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; iimageIDs.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; iconstraintFromIDs.size() && iconstraintToIDs.size() && iconstraintTypes.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)