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/Strategy" type="string" value="0"/> <!-- 0=BOW, 1=OpticalFlow -->
<param name="Odom/EstimationType" type="string" value="1"/> <!-- 3D->2D (PnP) -->
<param name="Odom/MinInliers" type="string" value="10"/>
<param name="Odom/RoiRatios" type="string" value="0.03 0.03 0.04 0.04"/>
<param name="Odom/MaxDepth" type="string" value="10"/>
<param name="OdomBow/NNDR" type="string" value="0.8"/>
<param name="Odom/MaxFeatures" type="string" value="1000"/>
<param name="Vis/EstimationType" type="string" value="1"/> <!-- 3D->2D (PnP) -->
<param name="Vis/MinInliers" type="string" value="10"/>
<param name="Vis/RoiRatios" type="string" value="0.03 0.03 0.04 0.04"/>
<param name="Vis/MaxDepth" type="string" value="10"/>
<param name="Vis/NNDR" type="string" value="0.8"/>
<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="GFTT/MinDistance" type="string" value="10"/>
<param name="GFTT/QualityLevel" type="string" value="0.00001"/>
@@ -80,19 +81,19 @@
<param name="Rtabmap/TimeThr" type="string" value="700"/>
<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/DetectorStrategy" type="string" value="0"/> <!-- use SURF -->
<param name="Kp/NNStrategy" type="string" value="1"/> <!-- kdTree -->
<param name="SURF/HessianThreshold" type="string" value="1000"/>
<param name="LccBow/MinInliers" type="string" value="10"/>
<param name="LccBow/EstimationType" type="string" value="1"/> <!-- 3D->2D (PnP) -->
<param name="Vis/MinInliers" type="string" value="10"/>
<param name="Vis/EstimationType" type="string" value="1"/> <!-- 3D->2D (PnP) -->
<param name="LccReextract/Activated" type="string" value="true"/>
<param name="LccReextract/MaxWords" type="string" value="500"/>
<param name="LccReextract/MaxDepth" type="string" value="10"/>
<param name="RGBD/LoopClosureReextractFeatures" type="string" value="true"/>
<param name="Vis/MaxFeatures" type="string" value="500"/>
<param name="Vis/MaxDepth" type="string" value="10"/>
</node>
<!-- 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 <rtabmap/core/Rtabmap.h>
#include <rtabmap/core/OdometryLocalMap.h>
#include <rtabmap/core/OdometryF2M.h>
#include <rtabmap/core/OdometryF2F.h>
#include <rtabmap/core/util3d_transforms.h>
#include <rtabmap/core/Memory.h>
@@ -318,7 +318,8 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
// process data
ros::WallTime time = ros::WallTime::now();
rtabmap::OdometryInfo info;
rtabmap::Transform pose = odometry_->process(data, &info);
SensorData dataCpy = data;
rtabmap::Transform pose = odometry_->process(dataCpy, &info);
if(!pose.isNull())
{
//*********************
@@ -362,10 +363,10 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
}
// local map / reference frame
if(odomLocalMap_.getNumSubscribers() && dynamic_cast<OdometryLocalMap*>(odometry_))
if(odomLocalMap_.getNumSubscribers() && dynamic_cast<OdometryF2M*>(odometry_))
{
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)
{
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(dynamic_cast<OdometryLocalMap*>(odometry_))
if(dynamic_cast<OdometryF2M*>(odometry_))
{
const rtabmap::Signature * s = ((OdometryLocalMap*)odometry_)->getMemory()->getLastWorkingSignature();
if(s)
const std::multimap<int, cv::Point3f> & words3 = ((OdometryF2M*)odometry_)->getLastFrame().getWords3();
if(words3.size())
{
const std::multimap<int, cv::Point3f> & words3 = s->getWords3();
pcl::PointCloud<pcl::PointXYZ> cloud;
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());
}
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&)
src/OdometryROS.h
+1 -1
View File
@@ -70,7 +70,7 @@ public:
const rtabmap::ParametersMap & parameters() const {return parameters_;}
const tf::TransformListener & tfListener() const {return tfListener_;}
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;
private: