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: added map_optimizer node

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/ros-pkg@1897 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-10-18 00:00:42 +00:00
parent ae38cd2a54
commit 2d6c8faaa5
4 changed files with 268 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CMakeLists.txt
+3
View File
@@ -152,6 +152,9 @@ target_link_libraries(visual_odometry ${Libraries})
add_executable(stereo_odometry src/StereoOdometryNode.cpp)
target_link_libraries(stereo_odometry rtabmap_ros ${Libraries})
add_executable(map_optimizer src/MapOptimizerNode.cpp)
target_link_libraries(map_optimizer rtabmap_ros ${Libraries})
add_executable(map_assembler src/MapAssemblerNode.cpp)
target_link_libraries(map_assembler rtabmap_ros ${Libraries})
launch/tests/test_stereo_mapping_bag.launch
+13 -6
View File
@@ -67,11 +67,13 @@
<param name="frame_id" type="string" value="base_link"/>
<param name="queue_size" type="int" value="30"/>
<param name="RGBD/ToroIterations" type="string" value="0"/>
<param name="Rtabmap/TimeThr" type="string" value="700"/>
<param name="Rtabmap/DetectionRate" type="string" value="1"/>
<param name="Kp/DetectorStrategy" type="string" value="6"/>
<param name="NN/NNStrategy" type="string" value="3"/>
<param name="GFTT/MaxCorners" type="string" value="200"/>
<param name="Kp/DetectorStrategy" type="string" value="0"/>
<param name="Kp/WordsPerImage" type="string" value="200"/>
<param name="Kp/RoiRatios" type="string" value="0.03 0.03 0.04 0.04"/>
<param name="NN/NNStrategy" type="string" value="1"/>
<param name="LccBow/MinInliers" type="string" value="10"/>
<param name="LccBow/InlierDistance" type="string" value="0.02"/>
@@ -79,9 +81,9 @@
<param name="LccReextract/FeatureType" type="string" value="4"/>
<param name="LccReextract/Activated" type="string" value="true"/>
<param name="LccReextract/MaxDepth" type="string" value="3"/>
<param name="LccReextract/MaxDepth" type="string" value="10"/>
<param name="LccReextract/NNDR" type="string" value="0.8"/>
<param name="LccReextract/NNType" type="string" value="3"/>
<param name="LccReextract/NNType" type="string" value="3"/>
</node>
<!-- Visualisation (client side) -->
@@ -103,10 +105,15 @@
</node>
-->
<node pkg="rtabmap" type="map_optimizer" name="map_optimizer" output="screen"/>
<node pkg="rtabmap" type="map_assembler" name="map_assembler" output="screen">
<remap from="mapData" to="mapData_optimized"/>
</node>
</group>
<!-- RVIZ -->
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap)/launch/config/rgbd.rviz"/>
<node pkg="nodelet" type="nodelet" name="standalone_nodelet" args="manager" output="screen"/>
<node pkg="nodelet" type="nodelet" name="data_odom_sync" args="load rtabmap/data_odom_sync standalone_nodelet">
src/CoreWrapper.cpp
+12 -1
View File
@@ -67,6 +67,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
odomFrameId_(""),
configPath_(""),
mapToOdom_(tf::Transform::getIdentity()),
transformThread_(0),
rate_(Parameters::defaultRtabmapDetectionRate()),
time_(ros::Time::now())
{
@@ -244,7 +245,17 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
setupCallbacks(subscribeDepth, subscribeLaserScan, subscribeStereo, queueSize);
transformThread_ = new boost::thread(boost::bind(&CoreWrapper::publishLoop, this, tfDelay));
int toroIterations = 0;
Parameters::parse(parameters, Parameters::kRGBDToroIterations(), toroIterations);
if(toroIterations != 0)
{
transformThread_ = new boost::thread(boost::bind(&CoreWrapper::publishLoop, this, tfDelay));
}
else
{
UWARN("Graph optimization is disabled (%s=0), the tf between frame \"%s\" and odometry frame will not be published. You can safely ignore this warning if you are using map_optimizer node.",
Parameters::kRGBDToroIterations().c_str(), mapFrameId_.c_str());
}
}
CoreWrapper::~CoreWrapper()
src/MapOptimizerNode.cpp
+240
View File
@@ -0,0 +1,240 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <ros/ros.h>
#include "rtabmap/MapData.h"
#include "rtabmap/MsgConversion.h"
#include <rtabmap/core/util3d.h>
#include <rtabmap/core/Parameters.h>
#include <rtabmap/utilite/ULogger.h>
#include <ros/subscriber.h>
#include <ros/publisher.h>
#include <tf/tf.h>
#include <tf/transform_broadcaster.h>
#include <boost/thread.hpp>
using namespace rtabmap;
class MapOptimizer
{
public:
MapOptimizer() :
mapFrameId_("map"),
odomFrameId_("odom"),
iterations_(100),
globalOptimization_(true),
condChecked_(false),
mapToOdom_(tf::Transform::getIdentity()),
transformThread_(0)
{
ros::NodeHandle nh;
ros::NodeHandle pnh("~");
pnh.param("map_frame_id", mapFrameId_, mapFrameId_);
pnh.param("odom_frame_id", odomFrameId_, odomFrameId_);
pnh.param("iterations", iterations_, iterations_);
pnh.param("global_optimization", globalOptimization_, globalOptimization_);
UASSERT(iterations_ > 0);
double tfDelay = 0.05; // 20 Hz
pnh.param("tf_delay", tfDelay, tfDelay);
// Verify that rtabmap is not sending optimized poses!
std::string toroIterations;
if(nh.getParam(Parameters::kRGBDToroIterations(), toroIterations))
{
if(std::atoi(toroIterations.c_str()) != 0)
{
ROS_WARN("map_optimizer: Parameter \"%s\" of rtabmap node is not 0 (value=%s), it should be 0 (optimization desactivated).",
Parameters::kRGBDToroIterations().c_str(), toroIterations.c_str());
exit(-1);
}
}
mapDataTopic_ = nh.subscribe("mapData", 1, &MapOptimizer::mapDataReceivedCallback, this);
mapDataPub_ = nh.advertise<rtabmap::MapData>(nh.resolveName("mapData")+"_optimized", 1);
ROS_INFO("map_optimizer will publish tf between frames \"%s\" and \"%s\"", mapFrameId_.c_str(), odomFrameId_.c_str());
ROS_INFO("map_optimizer: map_frame_id = %s", mapFrameId_.c_str());
ROS_INFO("map_optimizer: odom_frame_id = %s", odomFrameId_.c_str());
ROS_INFO("map_optimizer: tf_delay = %f", tfDelay);
transformThread_ = new boost::thread(boost::bind(&MapOptimizer::publishLoop, this, tfDelay));
}
~MapOptimizer()
{
if(transformThread_)
{
transformThread_->join();
delete transformThread_;
}
}
void publishLoop(double tfDelay)
{
if(tfDelay == 0)
return;
ros::Rate r(1.0 / tfDelay);
while(ros::ok())
{
mapToOdomMutex_.lock();
ros::Time tfExpiration = ros::Time::now() + ros::Duration(tfDelay);
tfBroadcaster_.sendTransform( tf::StampedTransform (mapToOdom_, tfExpiration, mapFrameId_, odomFrameId_));
mapToOdomMutex_.unlock();
r.sleep();
}
}
void mapDataReceivedCallback(const rtabmap::MapDataConstPtr & msg)
{
if(globalOptimization_ && !condChecked_)
{
// Verify that rtabmap is not sending optimized poses!
ros::NodeHandle nh;
std::string toroIterations;
if(nh.getParam(Parameters::kRGBDToroIterations(), toroIterations))
{
if(std::atoi(toroIterations.c_str()) != 0)
{
ROS_ERROR("map_optimizer: Parameter \"%s\" of rtabmap node is not 0 (value=%s), it should be 0 (optimization desactivated).",
Parameters::kRGBDToroIterations().c_str(), toroIterations.c_str());
exit(-1);
}
}
else
{
ROS_ERROR("map_optimizer: Could not get parameter \"%s\" of rtabmap node, it should be 0 (optimization desactivated). Is rtabmap node started? and in the same namespace that map_assembler?", Parameters::kRGBDToroIterations().c_str());
exit(-1);
}
condChecked_ = true;
}
else if(!globalOptimization_)
{
// optimize only local map
poses_.clear();
constraints_.clear();
}
// save new poses and constraints
// Assuming that nodes/constraints are all linked together
UASSERT(msg->poseIDs.size() == msg->poses.size());
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])));
}
UASSERT(msg->constraints.size() == msg->constraintFromIDs.size() &&
msg->constraints.size() == msg->constraintToIDs.size() &&
msg->constraints.size() == msg->constraintTypes.size());
for(unsigned int i=0; i<msg->constraints.size() && i<msg->constraints.size(); ++i)
{
bool edgeAlreadyAdded = false;
for(std::multimap<int, Link>::iterator iter = constraints_.lower_bound(msg->constraintFromIDs[i]);
iter != constraints_.end() && iter->first == msg->constraintFromIDs[i];
++iter)
{
if(iter->second.to() == msg->constraintToIDs[i])
{
edgeAlreadyAdded = true;
}
}
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])));
}
}
// Optimize only if there is a subscriber
if(mapDataPub_.getNumSubscribers())
{
std::map<int, Transform> optimizedPoses;
if(poses_.size() > 1 && constraints_.size() > 0)
{
Transform mapCorrectionToro;
util3d::optimizeTOROGraph(poses_, constraints_, optimizedPoses, mapCorrectionToro, iterations_, true);
mapToOdomMutex_.lock();
Transform mapCorrection = optimizedPoses.at(poses_.rbegin()->first) * poses_.rbegin()->second.inverse();
rtabmap::transformToTF(mapCorrection, mapToOdom_);
mapToOdomMutex_.unlock();
}
else if(poses_.size() == 1 && constraints_.size() == 0)
{
optimizedPoses = poses_;
}
else if(poses_.size() || constraints_.size())
{
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)",
poses_.size(), constraints_.size());
}
rtabmap::MapData outputMsg = *msg;
outputMsg.poseIDs.resize(optimizedPoses.size());
outputMsg.poses.resize(optimizedPoses.size());
int i=0;
for(std::map<int, Transform>::iterator iter = optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter)
{
outputMsg.poseIDs[i] = iter->first;
transformToPoseMsg(iter->second, outputMsg.poses[i]);
++i;
}
mapDataPub_.publish(outputMsg);
}
}
private:
std::string mapFrameId_;
std::string odomFrameId_;
int iterations_;
bool globalOptimization_;
bool condChecked_;
tf::Transform mapToOdom_;
boost::mutex mapToOdomMutex_;
ros::Subscriber mapDataTopic_;
ros::Publisher mapDataPub_;
std::map<int, Transform> poses_;
std::multimap<int, Link> constraints_;
tf::TransformBroadcaster tfBroadcaster_;
boost::thread* transformThread_;
};
int main(int argc, char** argv)
{
ros::init(argc, argv, "map_optimizer");
MapOptimizer optimizer;
ros::spin();
return 0;
}