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

Updated package with latest changes from lib 0.11.2. Updated demo_stereo_outdoor.launch

Browse Source
This commit is contained in:
matlabbe
2016-02-23 11:47:31 -05:00
parent d25c77ebba
commit 0bd147cfcf
3 changed files with 24 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files
launch/demo/demo_stereo_outdoor.launch
+13 -12
View File
@@ -49,12 +49,13 @@
<param name="odom_frame_id" type="string" value="odom"/> <param name="odom_frame_id" type="string" value="odom"/>
<param name="Odom/Strategy" type="string" value="0"/> <!-- 0=BOW, 1=OpticalFlow --> <param name="Odom/Strategy" type="string" value="0"/> <!-- 0=BOW, 1=OpticalFlow -->
<param name="Odom/EstimationType" type="string" value="1"/> <!-- 3D->2D (PnP) --> <param name="Vis/EstimationType" type="string" value="1"/> <!-- 3D->2D (PnP) -->
<param name="Odom/MinInliers" type="string" value="10"/> <param name="Vis/MinInliers" type="string" value="10"/>
<param name="Odom/RoiRatios" type="string" value="0.03 0.03 0.04 0.04"/> <param name="Vis/RoiRatios" type="string" value="0.03 0.03 0.04 0.04"/>
<param name="Odom/MaxDepth" type="string" value="10"/> <param name="Vis/MaxDepth" type="string" value="10"/>
<param name="OdomBow/NNDR" type="string" value="0.8"/> <param name="Vis/NNDR" type="string" value="0.8"/>
<param name="Odom/MaxFeatures" type="string" value="1000"/> <param name="Vis/MaxFeatures" type="string" value="1000"/>
<param name="Vis/CorGuessWinSize" type="string" value="31"/>
<param name="Odom/FillInfoData" type="string" value="$(arg rtabmapviz)"/> <param name="Odom/FillInfoData" type="string" value="$(arg rtabmapviz)"/>
<param name="GFTT/MinDistance" type="string" value="10"/> <param name="GFTT/MinDistance" type="string" value="10"/>
<param name="GFTT/QualityLevel" type="string" value="0.00001"/> <param name="GFTT/QualityLevel" type="string" value="0.00001"/>
@@ -80,19 +81,19 @@
<param name="Rtabmap/TimeThr" type="string" value="700"/> <param name="Rtabmap/TimeThr" type="string" value="700"/>
<param name="Rtabmap/DetectionRate" type="string" value="1"/> <param name="Rtabmap/DetectionRate" type="string" value="1"/>
<param name="Kp/WordsPerImage" type="string" value="200"/> <param name="Kp/MaxFeatures" type="string" value="200"/>
<param name="Kp/RoiRatios" type="string" value="0.03 0.03 0.04 0.04"/> <param name="Kp/RoiRatios" type="string" value="0.03 0.03 0.04 0.04"/>
<param name="Kp/DetectorStrategy" type="string" value="0"/> <!-- use SURF --> <param name="Kp/DetectorStrategy" type="string" value="0"/> <!-- use SURF -->
<param name="Kp/NNStrategy" type="string" value="1"/> <!-- kdTree --> <param name="Kp/NNStrategy" type="string" value="1"/> <!-- kdTree -->
<param name="SURF/HessianThreshold" type="string" value="1000"/> <param name="SURF/HessianThreshold" type="string" value="1000"/>
<param name="LccBow/MinInliers" type="string" value="10"/> <param name="Vis/MinInliers" type="string" value="10"/>
<param name="LccBow/EstimationType" type="string" value="1"/> <!-- 3D->2D (PnP) --> <param name="Vis/EstimationType" type="string" value="1"/> <!-- 3D->2D (PnP) -->
<param name="LccReextract/Activated" type="string" value="true"/> <param name="RGBD/LoopClosureReextractFeatures" type="string" value="true"/>
<param name="LccReextract/MaxWords" type="string" value="500"/> <param name="Vis/MaxFeatures" type="string" value="500"/>
<param name="LccReextract/MaxDepth" type="string" value="10"/> <param name="Vis/MaxDepth" type="string" value="10"/>
</node> </node>
<!-- Visualisation RTAB-Map --> <!-- Visualisation RTAB-Map -->
src/OdometryROS.cpp
+10 -10
View File
@@ -37,7 +37,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <cv_bridge/cv_bridge.h> #include <cv_bridge/cv_bridge.h>
#include <rtabmap/core/Rtabmap.h> #include <rtabmap/core/Rtabmap.h>
#include <rtabmap/core/OdometryLocalMap.h> #include <rtabmap/core/OdometryF2M.h>
#include <rtabmap/core/OdometryF2F.h> #include <rtabmap/core/OdometryF2F.h>
#include <rtabmap/core/util3d_transforms.h> #include <rtabmap/core/util3d_transforms.h>
#include <rtabmap/core/Memory.h> #include <rtabmap/core/Memory.h>
@@ -318,7 +318,8 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
// process data // process data
ros::WallTime time = ros::WallTime::now(); ros::WallTime time = ros::WallTime::now();
rtabmap::OdometryInfo info; rtabmap::OdometryInfo info;
rtabmap::Transform pose = odometry_->process(data, &info); SensorData dataCpy = data;
rtabmap::Transform pose = odometry_->process(dataCpy, &info);
if(!pose.isNull()) if(!pose.isNull())
{ {
//********************* //*********************
@@ -362,10 +363,10 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
} }
// local map / reference frame // local map / reference frame
if(odomLocalMap_.getNumSubscribers() && dynamic_cast<OdometryLocalMap*>(odometry_)) if(odomLocalMap_.getNumSubscribers() && dynamic_cast<OdometryF2M*>(odometry_))
{ {
pcl::PointCloud<pcl::PointXYZ> cloud; pcl::PointCloud<pcl::PointXYZ> cloud;
const std::multimap<int, cv::Point3f> & map = ((OdometryLocalMap*)odometry_)->getLocalMap(); const std::multimap<int, cv::Point3f> & map = ((OdometryF2M*)odometry_)->getMap().getWords3();
for(std::multimap<int, cv::Point3f>::const_iterator iter=map.begin(); iter!=map.end(); ++iter) for(std::multimap<int, cv::Point3f>::const_iterator iter=map.begin(); iter!=map.end(); ++iter)
{ {
cloud.push_back(pcl::PointXYZ(iter->second.x, iter->second.y, iter->second.z)); cloud.push_back(pcl::PointXYZ(iter->second.x, iter->second.y, iter->second.z));
@@ -379,12 +380,11 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
if(odomLastFrame_.getNumSubscribers()) if(odomLastFrame_.getNumSubscribers())
{ {
if(dynamic_cast<OdometryLocalMap*>(odometry_)) if(dynamic_cast<OdometryF2M*>(odometry_))
{ {
const rtabmap::Signature * s = ((OdometryLocalMap*)odometry_)->getMemory()->getLastWorkingSignature(); const std::multimap<int, cv::Point3f> & words3 = ((OdometryF2M*)odometry_)->getLastFrame().getWords3();
if(s) if(words3.size())
{ {
const std::multimap<int, cv::Point3f> & words3 = s->getWords3();
pcl::PointCloud<pcl::PointXYZ> cloud; pcl::PointCloud<pcl::PointXYZ> cloud;
for(std::multimap<int, cv::Point3f>::const_iterator iter=words3.begin(); iter!=words3.end(); ++iter) for(std::multimap<int, cv::Point3f>::const_iterator iter=words3.begin(); iter!=words3.end(); ++iter)
{ {
@@ -448,9 +448,9 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
ROS_INFO("Odom: quality=%d, std dev=%fm, update time=%fs", info.inliers, pose.isNull()?0.0f:std::sqrt(info.variance), (ros::WallTime::now()-time).toSec()); ROS_INFO("Odom: quality=%d, std dev=%fm, update time=%fs", info.inliers, pose.isNull()?0.0f:std::sqrt(info.variance), (ros::WallTime::now()-time).toSec());
} }
bool OdometryROS::isOdometryBOW() const bool OdometryROS::isOdometryF2M() const
{ {
return dynamic_cast<OdometryLocalMap*>(odometry_) != 0; return dynamic_cast<OdometryF2M*>(odometry_) != 0;
} }
bool OdometryROS::reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&) bool OdometryROS::reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
src/OdometryROS.h
+1 -1
View File
@@ -70,7 +70,7 @@ public:
const rtabmap::ParametersMap & parameters() const {return parameters_;} const rtabmap::ParametersMap & parameters() const {return parameters_;}
const tf::TransformListener & tfListener() const {return tfListener_;} const tf::TransformListener & tfListener() const {return tfListener_;}
bool isPaused() const {return paused_;} bool isPaused() const {return paused_;}
bool isOdometryBOW() const; bool isOdometryF2M() const;
rtabmap::Transform getTransform(const std::string & fromFrameId, const std::string & toFrameId, const ros::Time & stamp) const; rtabmap::Transform getTransform(const std::string & fromFrameId, const std::string & toFrameId, const ros::Time & stamp) const;
private: private: