mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-09-16 00:00:20 +08:00
ros: added experimental homemade stereo odometry algorithm
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/ros-pkg@1840 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
@@ -142,6 +142,9 @@ target_link_libraries(rtabmap rtabmap_ros ${Libraries})
|
||||
add_executable(visual_odometry src/VisualOdometryNode.cpp)
|
||||
target_link_libraries(visual_odometry rtabmap_ros ${Libraries})
|
||||
|
||||
add_executable(stereo_odometry src/StereoOdometryNode.cpp)
|
||||
target_link_libraries(stereo_odometry rtabmap_ros ${Libraries})
|
||||
|
||||
add_executable(map_assembler src/MapAssemblerNode.cpp)
|
||||
target_link_libraries(map_assembler rtabmap_ros ${Libraries})
|
||||
|
||||
|
||||
@@ -27,7 +27,7 @@
|
||||
<node pkg="nodelet" type="nodelet" name="disparity2depth" args="load rtabmap/disparity_to_depth standalone_nodelet"/>
|
||||
</group>
|
||||
|
||||
<!-- Odometry: Choose between viso2_ros fovis_ros and homemade package -->
|
||||
<!-- Odometry: Choose between viso2_ros, fovis_ros and homemade packages -->
|
||||
<!--
|
||||
<node pkg="viso2_ros" type="stereo_odometer" name="stereo_odometer" output="screen">
|
||||
<remap from="stereo" to="/stereo_camera"/>
|
||||
@@ -51,7 +51,7 @@
|
||||
<remap from="rgb/camera_info" to="stereo_camera/left/camera_info"/>
|
||||
<remap from="odom" to="/stereo_odometer/odometry"/>
|
||||
|
||||
<param name="frame_id" type="string" value="base_link"/>
|
||||
<param name="frame_id" type="string" value="/base_link"/>
|
||||
|
||||
<param name="Odom/Type" type="string" value="6"/>
|
||||
<param name="Odom/NearestNeighbor" type="string" value="3"/>
|
||||
@@ -60,11 +60,42 @@
|
||||
<param name="Odom/NNDR" type="string" value="0.8"/>
|
||||
<param name="Odom/WordsRatio" type="string" value="0.5"/>
|
||||
<param name="Odom/LocalHistory" type="string" value="1000"/>
|
||||
<param name="Odom/InlierDistance" type="string" value="0.02"/>
|
||||
<param name="Odom/InlierDistance" type="string" value="0.01"/>
|
||||
<param name="GFTT/MaxCorners" type="string" value="400"/>
|
||||
<param name="BRIEF\Bytes" type="string" value="16"/>
|
||||
<param name="BRIEF/Bytes" type="string" value="16"/>
|
||||
</node>
|
||||
-->
|
||||
<!--
|
||||
<node pkg="rtabmap" type="stereo_odometry" name="stereo_odometry" output="screen">
|
||||
<remap from="left/image_rect" to="stereo_camera/left/image_rect_color"/>
|
||||
<remap from="right/image_rect" to="stereo_camera/right/image_rect_color"/>
|
||||
<remap from="left/camera_info" to="stereo_camera/left/camera_info"/>
|
||||
<remap from="right/camera_info" to="stereo_camera/right/camera_info"/>
|
||||
<remap from="odom" to="/stereo_odometer/odometry"/>
|
||||
|
||||
<param name="frame_id" type="string" value="/base_link"/>
|
||||
|
||||
<param name="min_disparity" type="int" value="0"/>
|
||||
<param name="max_disparity" type="int" value="128"/>
|
||||
<param name="k" type="int" value="10"/>
|
||||
|
||||
<param name="Odom/Type" type="string" value="6"/>
|
||||
<param name="Odom/NearestNeighbor" type="string" value="3"/>
|
||||
<param name="Odom/MinInliers" type="string" value="10"/>
|
||||
<param name="Odom/Iterations" type="string" value="200"/>
|
||||
<param name="Odom/MaxDepth" type="string" value="4"/>
|
||||
<param name="Odom/NNDR" type="string" value="0.8"/>
|
||||
<param name="Odom/WordsRatio" type="string" value="0.5"/>
|
||||
<param name="Odom/LocalHistory" type="string" value="200"/>
|
||||
<param name="Odom/InlierDistance" type="string" value="0.01"/>
|
||||
<param name="GFTT/MaxCorners" type="string" value="800"/>
|
||||
<param name="BRIEF/Bytes" type="string" value="16"/>
|
||||
<param name="BRISK/Octaves" type="string" value="0"/>
|
||||
<param name="BRISK/Thresh" type="string" value="10"/>
|
||||
</node>
|
||||
-->
|
||||
|
||||
|
||||
<group ns="rtabmap">
|
||||
|
||||
<!-- Visual SLAM (robot side) -->
|
||||
@@ -81,7 +112,7 @@
|
||||
<remap from="odom" to="/stereo_odometer/odometry"/>
|
||||
|
||||
<param name="frame_id" type="string" value="/base_link"/>
|
||||
<param name="queue_size" type="int" value="30"/>
|
||||
<param name="queue_size" type="int" value="50"/>
|
||||
|
||||
<param name="Rtabmap/TimeThr" type="string" value="700"/>
|
||||
<param name="Rtabmap/DetectionRate" type="string" value="1"/>
|
||||
@@ -122,7 +153,7 @@
|
||||
<remap from="rgb/camera_info_out" to="data_odom_sync/camera_info"/>
|
||||
<remap from="odom_out" to="odom_sync"/>
|
||||
|
||||
<param name="queue_size" type="int" value="30"/>
|
||||
<param name="queue_size" type="int" value="50"/>
|
||||
</node>
|
||||
<node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="load rtabmap/point_cloud_xyzrgb standalone_nodelet">
|
||||
<remap from="rgb/image" to="data_odom_sync/image"/>
|
||||
|
||||
+1
-8
@@ -69,14 +69,7 @@ int main(int argc, char** argv)
|
||||
// hide specific parameters
|
||||
for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end();)
|
||||
{
|
||||
if(uSplit(iter->first, '/').front().compare("Bayes") == 0 ||
|
||||
uSplit(iter->first, '/').front().compare("VhEp") == 0 ||
|
||||
uSplit(iter->first, '/').front().compare("Odom") == 0 ||
|
||||
uSplit(iter->first, '/').front().compare("FAST") == 0 ||
|
||||
uSplit(iter->first, '/').front().compare("BRIEF") == 0 ||
|
||||
uSplit(iter->first, '/').front().compare("ORB") == 0 ||
|
||||
uSplit(iter->first, '/').front().compare("FREAK") == 0 ||
|
||||
uSplit(iter->first, '/').front().compare("GFTT") == 0 ||
|
||||
if(uSplit(iter->first, '/').front().compare("Odom") == 0 ||
|
||||
uSplit(iter->first, '/').front().compare("OdomICP") == 0)
|
||||
{
|
||||
parameters.erase(iter++);
|
||||
|
||||
@@ -0,0 +1,632 @@
|
||||
/*
|
||||
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 <tf/tf.h>
|
||||
#include <tf/transform_broadcaster.h>
|
||||
#include <tf/transform_listener.h>
|
||||
|
||||
#include <std_srvs/Empty.h>
|
||||
|
||||
#include <message_filters/subscriber.h>
|
||||
#include <message_filters/time_synchronizer.h>
|
||||
#include <message_filters/sync_policies/approximate_time.h>
|
||||
|
||||
#include <image_transport/image_transport.h>
|
||||
#include <image_transport/subscriber_filter.h>
|
||||
|
||||
#include <sensor_msgs/Image.h>
|
||||
#include <sensor_msgs/image_encodings.h>
|
||||
#include <sensor_msgs/PointCloud2.h>
|
||||
#include <nav_msgs/Odometry.h>
|
||||
|
||||
#include <image_geometry/stereo_camera_model.h>
|
||||
|
||||
#include <pcl_conversions/pcl_conversions.h>
|
||||
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
|
||||
#include <rtabmap/core/Odometry.h>
|
||||
#include <rtabmap/core/util3d.h>
|
||||
#include <rtabmap/core/Memory.h>
|
||||
#include <rtabmap/core/Signature.h>
|
||||
#include "rtabmap/MsgConversion.h"
|
||||
#include "rtabmap/utilite/UConversion.h"
|
||||
#include "rtabmap/utilite/ULogger.h"
|
||||
#include "rtabmap/utilite/UStl.h"
|
||||
|
||||
using namespace rtabmap;
|
||||
|
||||
class StereoOdometry
|
||||
{
|
||||
public:
|
||||
StereoOdometry() :
|
||||
odometry_(0),
|
||||
feature2D_(0),
|
||||
frameId_("base_link"),
|
||||
odomFrameId_("odom"),
|
||||
publishTf_(true),
|
||||
minDisparity_(0.0),
|
||||
maxDisparity_(128.0),
|
||||
k_(100),
|
||||
sync_(0),
|
||||
paused_(false)
|
||||
{
|
||||
ros::NodeHandle nh;
|
||||
|
||||
odomPub_ = nh.advertise<nav_msgs::Odometry>("odom", 1);
|
||||
odomLocalMapPub_ = nh.advertise<sensor_msgs::PointCloud2>("odom_local_map", 1);
|
||||
odomLastFrame_ = nh.advertise<sensor_msgs::PointCloud2>("odom_last_frame", 1);
|
||||
//fundMatMapPub_ = nh.advertise<sensor_msgs::PointCloud2>("fund_mat_inliers", 1);
|
||||
//stereoMatchesPub_ = nh.advertise<sensor_msgs::PointCloud2>("stereo_matches", 1);
|
||||
|
||||
ros::NodeHandle pnh("~");
|
||||
|
||||
int queueSize = 5;
|
||||
pnh.param("frame_id", frameId_, frameId_);
|
||||
pnh.param("odom_frame_id", odomFrameId_, odomFrameId_);
|
||||
pnh.param("publish_tf", publishTf_, publishTf_);
|
||||
pnh.param("queue_size", queueSize, queueSize);
|
||||
pnh.param("min_disparity", minDisparity_, minDisparity_);
|
||||
pnh.param("max_disparity", maxDisparity_, maxDisparity_);
|
||||
pnh.param("k", k_, k_);
|
||||
|
||||
//parameters
|
||||
rtabmap::ParametersMap parameters = rtabmap::Parameters::getDefaultParameters();
|
||||
rtabmap::ParametersMap parametersOdom;
|
||||
for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
|
||||
{
|
||||
std::string group = uSplit(iter->first, '/').front();
|
||||
if(group.compare("Odom") == 0 ||
|
||||
group.compare("SURF") == 0 ||
|
||||
group.compare("SIFT") == 0 ||
|
||||
group.compare("ORB") == 0 ||
|
||||
group.compare("FAST") == 0 ||
|
||||
group.compare("FREAK") == 0 ||
|
||||
group.compare("BRIEF") == 0 ||
|
||||
group.compare("GFTT") == 0 ||
|
||||
group.compare("BRISK") == 0)
|
||||
{
|
||||
parametersOdom.insert(*iter);
|
||||
}
|
||||
}
|
||||
|
||||
for(rtabmap::ParametersMap::iterator iter=parametersOdom.begin(); iter!=parametersOdom.end(); ++iter)
|
||||
{
|
||||
std::string vStr;
|
||||
bool vBool;
|
||||
int vInt;
|
||||
double vDouble;
|
||||
if(pnh.getParam(iter->first, vStr))
|
||||
{
|
||||
ROS_INFO("Setting odometry parameter \"%s\"=\"%s\"", iter->first.c_str(), vStr.c_str());
|
||||
iter->second = vStr;
|
||||
}
|
||||
else if(pnh.getParam(iter->first, vBool))
|
||||
{
|
||||
ROS_INFO("Setting odometry parameter \"%s\"=\"%s\"", iter->first.c_str(), uBool2Str(vBool).c_str());
|
||||
iter->second = uBool2Str(vBool);
|
||||
}
|
||||
else if(pnh.getParam(iter->first, vInt))
|
||||
{
|
||||
ROS_INFO("Setting odometry parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vInt).c_str());
|
||||
iter->second = uNumber2Str(vInt);
|
||||
|
||||
if(iter->first.compare(Parameters::kOdomMinInliers()) == 0 && vInt < 8)
|
||||
{
|
||||
ROS_WARN("Parameter min_inliers must be >= 8, setting to 8...");
|
||||
iter->second = uNumber2Str(8);
|
||||
}
|
||||
}
|
||||
else if(pnh.getParam(iter->first, vDouble))
|
||||
{
|
||||
ROS_INFO("Setting odometry parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vDouble).c_str());
|
||||
iter->second = uNumber2Str(vDouble);
|
||||
}
|
||||
}
|
||||
|
||||
odometry_ = new rtabmap::OdometryBOW(parametersOdom);
|
||||
|
||||
//Keypoint detector
|
||||
ParametersMap::const_iterator iter;
|
||||
Feature2D::Type detectorStrategy = Feature2D::kFeatureUndef;
|
||||
if((iter=parametersOdom.find(Parameters::kOdomType())) != parametersOdom.end())
|
||||
{
|
||||
detectorStrategy = (Feature2D::Type)std::atoi((*iter).second.c_str());
|
||||
}
|
||||
switch(detectorStrategy)
|
||||
{
|
||||
case Feature2D::kFeatureSift:
|
||||
feature2D_ = new SIFT(parametersOdom);
|
||||
break;
|
||||
case Feature2D::kFeatureFastBrief:
|
||||
feature2D_ = new FAST_BRIEF(parametersOdom);
|
||||
break;
|
||||
case Feature2D::kFeatureFastFreak:
|
||||
feature2D_ = new FAST_FREAK(parametersOdom);
|
||||
break;
|
||||
case Feature2D::kFeatureOrb:
|
||||
feature2D_ = new ORB(parametersOdom);
|
||||
break;
|
||||
case Feature2D::kFeatureGfttFreak:
|
||||
feature2D_ = new GFTT_FREAK(parametersOdom);
|
||||
break;
|
||||
case Feature2D::kFeatureGfttBrief:
|
||||
feature2D_ = new GFTT_BRIEF(parametersOdom);
|
||||
break;
|
||||
case Feature2D::kFeatureBrisk:
|
||||
feature2D_ = new BRISK(parametersOdom);
|
||||
break;
|
||||
case Feature2D::kFeatureSurf:
|
||||
default:
|
||||
feature2D_ = new SURF(parametersOdom);
|
||||
break;
|
||||
}
|
||||
|
||||
ros::NodeHandle left_nh(nh, "left");
|
||||
ros::NodeHandle right_nh(nh, "right");
|
||||
ros::NodeHandle left_pnh(pnh, "left");
|
||||
ros::NodeHandle right_pnh(pnh, "right");
|
||||
image_transport::ImageTransport left_it(left_nh);
|
||||
image_transport::ImageTransport right_it(right_nh);
|
||||
image_transport::TransportHints hintsLeft("raw", ros::TransportHints(), left_pnh);
|
||||
image_transport::TransportHints hintsRight("raw", ros::TransportHints(), right_pnh);
|
||||
|
||||
imageRectLeft_.subscribe(left_it, left_nh.resolveName("image_rect"), 1, hintsLeft);
|
||||
imageRectRight_.subscribe(right_it, right_nh.resolveName("image_rect"), 1, hintsRight);
|
||||
cameraInfoLeft_.subscribe(left_nh, "camera_info", 1);
|
||||
cameraInfoRight_.subscribe(right_nh, "camera_info", 1);
|
||||
|
||||
sync_ = new message_filters::Synchronizer<MySyncPolicy>(MySyncPolicy(queueSize), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
|
||||
sync_->registerCallback(boost::bind(&StereoOdometry::callback, this, _1, _2, _3, _4));
|
||||
|
||||
resetSrv_ = nh.advertiseService("reset_odom", &StereoOdometry::reset, this);
|
||||
pauseSrv_ = nh.advertiseService("pause_odom", &StereoOdometry::pause, this);
|
||||
resumeSrv_ = nh.advertiseService("resume_odom", &StereoOdometry::resume, this);
|
||||
}
|
||||
|
||||
~StereoOdometry()
|
||||
{
|
||||
ros::NodeHandle pnh("~");
|
||||
ParametersMap parameters = Parameters::getDefaultParameters();
|
||||
for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
|
||||
{
|
||||
pnh.deleteParam(iter->first);
|
||||
}
|
||||
|
||||
delete sync_;
|
||||
delete odometry_;
|
||||
delete feature2D_;
|
||||
}
|
||||
|
||||
void callback(
|
||||
const sensor_msgs::ImageConstPtr& imageRectLeft,
|
||||
const sensor_msgs::ImageConstPtr& imageRectRight,
|
||||
const sensor_msgs::CameraInfoConstPtr& cameraInfoLeft,
|
||||
const sensor_msgs::CameraInfoConstPtr& cameraInfoRight)
|
||||
{
|
||||
if(!paused_)
|
||||
{
|
||||
if(!(imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
|
||||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
|
||||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
|
||||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) ||
|
||||
!(imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
|
||||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
|
||||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
|
||||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0))
|
||||
{
|
||||
ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 (mono8 recommended)");
|
||||
return;
|
||||
}
|
||||
|
||||
tf::StampedTransform localTransform;
|
||||
try
|
||||
{
|
||||
tfListener_.lookupTransform(frameId_, imageRectLeft->header.frame_id, imageRectLeft->header.stamp, localTransform);
|
||||
}
|
||||
catch(tf::TransformException & ex)
|
||||
{
|
||||
ROS_WARN("%s",ex.what());
|
||||
return;
|
||||
}
|
||||
|
||||
ros::WallTime time = ros::WallTime::now();
|
||||
|
||||
int quality = -1;
|
||||
if(imageRectLeft->data.size() && imageRectRight->data.size())
|
||||
{
|
||||
float depthFx = cameraInfoLeft->K[0];
|
||||
float depthFy = cameraInfoLeft->K[4];
|
||||
float depthCx = cameraInfoLeft->K[2];
|
||||
float depthCy = cameraInfoLeft->K[5];
|
||||
cv_bridge::CvImageConstPtr ptrImageLeft = cv_bridge::toCvShare(imageRectLeft, "mono8");
|
||||
cv_bridge::CvImageConstPtr ptrImageRight = cv_bridge::toCvShare(imageRectRight, "mono8");
|
||||
|
||||
//generate depth
|
||||
cv::Mat depth = cv::Mat::zeros(ptrImageLeft->image.rows, ptrImageLeft->image.cols, CV_32FC1);
|
||||
|
||||
std::vector<cv::KeyPoint> kptsLeft, kptsRight;
|
||||
cv::Mat descLeft, descRight;
|
||||
kptsLeft = feature2D_->generateKeypoints(ptrImageLeft->image);
|
||||
if(kptsLeft.size())
|
||||
{
|
||||
descLeft = feature2D_->generateDescriptors(ptrImageLeft->image, kptsLeft);
|
||||
|
||||
kptsRight = feature2D_->generateKeypoints(ptrImageRight->image);
|
||||
if(kptsRight.size())
|
||||
{
|
||||
descRight = feature2D_->generateDescriptors(ptrImageRight->image, kptsRight);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
if(kptsLeft.size() >= odometry_->getMinInliers() && kptsRight.size() >= odometry_->getMinInliers())
|
||||
{
|
||||
std::vector<std::vector<cv::DMatch> > matches;
|
||||
cv::BFMatcher matcher(descLeft.depth()==CV_8U?cv::NORM_HAMMING:cv::NORM_L2);
|
||||
matcher.knnMatch(descLeft, descRight, matches, k_);
|
||||
|
||||
if(matches.size())
|
||||
{
|
||||
image_geometry::StereoCameraModel model;
|
||||
model.fromCameraInfo(*cameraInfoLeft, *cameraInfoRight);
|
||||
|
||||
// Remove outliers using fundamental matrix RANSAC
|
||||
/*std::vector<uchar> status(matches.size(), 0);
|
||||
//Convert Keypoints to a structure that OpenCV understands
|
||||
//3 dimensions (Homogeneous vectors)
|
||||
cv::Mat points1(1, (int)matches.size(), CV_32FC2);
|
||||
cv::Mat points2(1, (int)matches.size(), CV_32FC2);
|
||||
|
||||
float * points1data = points1.ptr<float>(0);
|
||||
float * points2data = points2.ptr<float>(0);
|
||||
|
||||
// Fill the points here ...
|
||||
for(int i=0; i < matches.size(); ++i )
|
||||
{
|
||||
points1data[i*2] = kptsLeft[matches[i].queryIdx].pt.x;
|
||||
points1data[i*2+1] = kptsLeft[matches[i].queryIdx].pt.y;
|
||||
|
||||
points2data[i*2] = kptsRight[matches[i].trainIdx].pt.x;
|
||||
points2data[i*2+1] = kptsRight[matches[i].trainIdx].pt.y;
|
||||
}
|
||||
|
||||
// Find the fundamental matrix
|
||||
cv::Mat fundamentalMatrix = cv::findFundamentalMat(
|
||||
points1,
|
||||
points2,
|
||||
status,
|
||||
cv::FM_RANSAC,
|
||||
3.0,
|
||||
0.99);
|
||||
|
||||
int inliers = 0;
|
||||
if(!fundamentalMatrix.empty())
|
||||
{
|
||||
pcl::PointCloud<pcl::PointXYZ> cloud;
|
||||
for(int i = 0; i<matches.size(); ++i)
|
||||
{
|
||||
if(status[i])
|
||||
{
|
||||
float disparity = kptsLeft[matches[i].queryIdx].pt.x - kptsRight[matches[i].trainIdx].pt.x;
|
||||
cv::Point3d pt3d;
|
||||
model.projectDisparityTo3d(cv::Point2d(kptsLeft[matches[i].queryIdx].pt.x, kptsLeft[matches[i].queryIdx].pt.y), disparity, pt3d);
|
||||
cloud.push_back(pcl::PointXYZ(pt3d.x, pt3d.y, pt3d.z));
|
||||
inliers++;
|
||||
}
|
||||
}
|
||||
sensor_msgs::PointCloud2 cloudMsg;
|
||||
pcl::toROSMsg(cloud, cloudMsg);
|
||||
cloudMsg.header.stamp = imageRectLeft->header.stamp; // use corresponding time stamp to image
|
||||
cloudMsg.header.frame_id = odomFrameId_;
|
||||
fundMatMapPub_.publish(cloudMsg);
|
||||
}*/
|
||||
|
||||
|
||||
int added = 0;
|
||||
int addedFirst = 0;
|
||||
// pcl::PointCloud<pcl::PointXYZ> cloud;
|
||||
for(int i=0; i< matches.size(); ++i)
|
||||
{
|
||||
// add only those on same Y
|
||||
for(unsigned int j=0; j<k_; ++j)
|
||||
{
|
||||
float disparity = kptsLeft[matches[i].at(j).queryIdx].pt.x - kptsRight[matches[i].at(j).trainIdx].pt.x;
|
||||
|
||||
if((int)disparity >= minDisparity_ && (int)disparity <= maxDisparity_)
|
||||
{
|
||||
|
||||
float d = model.getZ(disparity);
|
||||
if(kptsLeft[matches[i].at(j).queryIdx].pt.x >= kptsRight[matches[i].at(j).trainIdx].pt.x+0.5f &&
|
||||
int(kptsLeft[matches[i].at(j).queryIdx].pt.y+0.5f) >= int(kptsRight[matches[i].at(j).trainIdx].pt.y+0.5f) - 3 &&
|
||||
int(kptsLeft[matches[i].at(j).queryIdx].pt.y+0.5f) <= int(kptsRight[matches[i].at(j).trainIdx].pt.y+0.5f) + 3)
|
||||
{
|
||||
|
||||
depth.at<float>(int(kptsLeft[matches[i].at(j).queryIdx].pt.y+0.5f), int(kptsLeft[matches[i].at(j).queryIdx].pt.x+0.5f)) = d;
|
||||
/*ROS_INFO("Add%d Left(%d, %d) Right(%d, %d) distance %d = %f disp=%f, depth=%f",
|
||||
j,
|
||||
int(kptsLeft[matches[i].at(j).queryIdx].pt.x+0.5f),
|
||||
int(kptsLeft[matches[i].at(j).queryIdx].pt.y+0.5f),
|
||||
int(kptsRight[matches[i].at(j).trainIdx].pt.x+0.5f),
|
||||
int(kptsRight[matches[i].at(j).trainIdx].pt.y+0.5f),
|
||||
i, matches[i].at(j).distance, disparity, d);*/
|
||||
//cv::Point3d pt3d;
|
||||
//model.projectDisparityTo3d(cv::Point2d(kptsLeft[matches[i].queryIdx].pt.x, kptsLeft[matches[i].queryIdx].pt.y), disparity, pt3d);
|
||||
//cloud.push_back(pcl::PointXYZ(pt3d.x, pt3d.y, pt3d.z));
|
||||
if(j == 0)
|
||||
{
|
||||
++addedFirst;
|
||||
}
|
||||
++added;
|
||||
break;
|
||||
}
|
||||
else
|
||||
{
|
||||
/*ROS_INFO("--- Left(%d, %d) Right(%d, %d) distance %d = %f disp=%f depth=%f",
|
||||
int(kptsLeft[matches[i].queryIdx].pt.x+0.5f),
|
||||
int(kptsLeft[matches[i].queryIdx].pt.y+0.5f),
|
||||
int(kptsRight[matches[i].trainIdx].pt.x+0.5f),
|
||||
int(kptsRight[matches[i].trainIdx].pt.y+0.5f),
|
||||
i, matches[i].distance, disparity, d);*/
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
/*sensor_msgs::PointCloud2 cloudMsg;
|
||||
pcl::toROSMsg(cloud, cloudMsg);
|
||||
cloudMsg.header.stamp = imageRectLeft->header.stamp; // use corresponding time stamp to image
|
||||
cloudMsg.header.frame_id = odomFrameId_;
|
||||
stereoMatchesPub_.publish(cloudMsg);*/
|
||||
//ROS_INFO("added = %d / %d inlier=%d", added, matches.size(), inliers);
|
||||
ROS_INFO("added = %d / %d (addedFirst=%d)", added, (int)matches.size(), addedFirst);
|
||||
|
||||
//
|
||||
rtabmap::SensorData data(ptrImageLeft->image,
|
||||
depth,
|
||||
depthFx,
|
||||
depthFy,
|
||||
depthCx,
|
||||
depthCy,
|
||||
rtabmap::Transform(),
|
||||
rtabmap::transformFromTF(localTransform));
|
||||
data.setFeatures(kptsLeft, descLeft);
|
||||
quality=0;
|
||||
rtabmap::Transform pose = odometry_->process(data, &quality);
|
||||
if(!pose.isNull())
|
||||
{
|
||||
//*********************
|
||||
// Update odometry
|
||||
//*********************
|
||||
tf::Transform poseTF;
|
||||
rtabmap::transformToTF(pose, poseTF);
|
||||
|
||||
if(publishTf_)
|
||||
{
|
||||
tfBroadcaster_.sendTransform( tf::StampedTransform (poseTF, imageRectLeft->header.stamp, odomFrameId_, frameId_));
|
||||
}
|
||||
|
||||
if(odomPub_.getNumSubscribers())
|
||||
{
|
||||
//next, we'll publish the odometry message over ROS
|
||||
nav_msgs::Odometry odom;
|
||||
odom.header.stamp = imageRectLeft->header.stamp; // use corresponding time stamp to image
|
||||
odom.header.frame_id = odomFrameId_;
|
||||
odom.child_frame_id = frameId_;
|
||||
|
||||
//set the position
|
||||
odom.pose.pose.position.x = poseTF.getOrigin().x();
|
||||
odom.pose.pose.position.y = poseTF.getOrigin().y();
|
||||
odom.pose.pose.position.z = poseTF.getOrigin().z();
|
||||
tf::quaternionTFToMsg(poseTF.getRotation().normalized(), odom.pose.pose.orientation);
|
||||
|
||||
//publish the message
|
||||
odomPub_.publish(odom);
|
||||
}
|
||||
|
||||
if(odomLocalMapPub_.getNumSubscribers())
|
||||
{
|
||||
const std::multimap<int, pcl::PointXYZ> & map = odometry_->getLocalMeansMap();
|
||||
pcl::PointCloud<pcl::PointXYZ> cloud;
|
||||
for(std::multimap<int, pcl::PointXYZ>::const_iterator iter=map.begin(); iter!=map.end(); ++iter)
|
||||
{
|
||||
cloud.push_back(iter->second);
|
||||
}
|
||||
sensor_msgs::PointCloud2 cloudMsg;
|
||||
pcl::toROSMsg(cloud, cloudMsg);
|
||||
cloudMsg.header.stamp = imageRectLeft->header.stamp; // use corresponding time stamp to image
|
||||
cloudMsg.header.frame_id = odomFrameId_;
|
||||
odomLocalMapPub_.publish(cloudMsg);
|
||||
}
|
||||
|
||||
if(odomLastFrame_.getNumSubscribers())
|
||||
{
|
||||
const rtabmap::Signature * s = odometry_->getMemory()->getLastWorkingSignature();
|
||||
if(s)
|
||||
{
|
||||
const std::multimap<int, pcl::PointXYZ> & words3 = s->getWords3();
|
||||
pcl::PointCloud<pcl::PointXYZ> cloud;
|
||||
rtabmap::Transform t = rtabmap::transformFromTF(localTransform);
|
||||
for(std::multimap<int, pcl::PointXYZ>::const_iterator iter=words3.begin(); iter!=words3.end(); ++iter)
|
||||
{
|
||||
// transform to odom frame
|
||||
pcl::PointXYZ pt = util3d::transformPoint(iter->second, pose);
|
||||
cloud.push_back(pt);
|
||||
}
|
||||
|
||||
sensor_msgs::PointCloud2 cloudMsg;
|
||||
pcl::toROSMsg(cloud, cloudMsg);
|
||||
cloudMsg.header.stamp = imageRectLeft->header.stamp; // use corresponding time stamp to image
|
||||
cloudMsg.header.frame_id = odomFrameId_;
|
||||
odomLastFrame_.publish(cloudMsg);
|
||||
ROS_INFO("cloud = %d", (int)cloud.size());
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
//ROS_WARN("Odometry lost!");
|
||||
|
||||
//send null pose to notify that odometry is lost
|
||||
nav_msgs::Odometry odom;
|
||||
odom.header.stamp = imageRectLeft->header.stamp; // use corresponding time stamp to image
|
||||
odom.header.frame_id = odomFrameId_;
|
||||
odom.child_frame_id = frameId_;
|
||||
|
||||
//publish the message
|
||||
odomPub_.publish(odom);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
ROS_INFO("Odom: quality=%d, update time=%fs", quality, (ros::WallTime::now()-time).toSec());
|
||||
}
|
||||
}
|
||||
|
||||
bool reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
|
||||
{
|
||||
ROS_INFO("visual_odometry: reset odom!");
|
||||
odometry_->reset();
|
||||
return true;
|
||||
}
|
||||
|
||||
bool pause(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
|
||||
{
|
||||
if(paused_)
|
||||
{
|
||||
ROS_WARN("visual_odometry: Already paused!");
|
||||
}
|
||||
else
|
||||
{
|
||||
paused_ = true;
|
||||
ROS_INFO("visual_odometry: paused!");
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool resume(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
|
||||
{
|
||||
if(!paused_)
|
||||
{
|
||||
ROS_WARN("visual_odometry: Already running!");
|
||||
}
|
||||
else
|
||||
{
|
||||
paused_ = false;
|
||||
ROS_INFO("visual_odometry: resumed!");
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
private:
|
||||
rtabmap::OdometryBOW * odometry_;
|
||||
rtabmap::Feature2D * feature2D_;
|
||||
|
||||
// parameters
|
||||
std::string frameId_;
|
||||
std::string odomFrameId_;
|
||||
bool publishTf_;
|
||||
int minDisparity_;
|
||||
int maxDisparity_;
|
||||
int k_;
|
||||
|
||||
ros::Publisher odomPub_;
|
||||
ros::Publisher odomLocalMapPub_;
|
||||
ros::Publisher odomLastFrame_;
|
||||
//ros::Publisher fundMatMapPub_;
|
||||
//ros::Publisher stereoMatchesPub_;
|
||||
ros::ServiceServer resetSrv_;
|
||||
ros::ServiceServer pauseSrv_;
|
||||
ros::ServiceServer resumeSrv_;
|
||||
tf::TransformBroadcaster tfBroadcaster_;
|
||||
tf::TransformListener tfListener_;
|
||||
|
||||
image_transport::SubscriberFilter imageRectLeft_;
|
||||
image_transport::SubscriberFilter imageRectRight_;
|
||||
message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoLeft_;
|
||||
message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoRight_;
|
||||
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> MySyncPolicy;
|
||||
message_filters::Synchronizer<MySyncPolicy> * sync_;
|
||||
|
||||
bool paused_;
|
||||
};
|
||||
|
||||
int main(int argc, char *argv[])
|
||||
{
|
||||
ULogger::setType(ULogger::kTypeConsole);
|
||||
ULogger::setLevel(ULogger::kInfo);
|
||||
ros::init(argc, argv, "visual_odometry");
|
||||
|
||||
for(int i=1;i<argc;++i)
|
||||
{
|
||||
if(strcmp(argv[i], "--params") == 0)
|
||||
{
|
||||
rtabmap::ParametersMap parameters = rtabmap::Parameters::getDefaultParameters();
|
||||
rtabmap::ParametersMap parametersOdom;
|
||||
if(strcmp(argv[i], "--params") == 0)
|
||||
{
|
||||
// show specific parameters
|
||||
for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
|
||||
{
|
||||
if(uSplit(iter->first, '/').front().compare("Odom") == 0 ||
|
||||
uSplit(iter->first, '/').front().compare("SURF") == 0 ||
|
||||
uSplit(iter->first, '/').front().compare("SIFT") == 0 ||
|
||||
uSplit(iter->first, '/').front().compare("ORB") == 0 ||
|
||||
uSplit(iter->first, '/').front().compare("FAST") == 0 ||
|
||||
uSplit(iter->first, '/').front().compare("FREAK") == 0 ||
|
||||
uSplit(iter->first, '/').front().compare("BRIEF") == 0 ||
|
||||
uSplit(iter->first, '/').front().compare("GFTT") == 0 ||
|
||||
uSplit(iter->first, '/').front().compare("BRISK") == 0)
|
||||
{
|
||||
parametersOdom.insert(*iter);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
for(rtabmap::ParametersMap::iterator iter=parametersOdom.begin(); iter!=parametersOdom.end(); ++iter)
|
||||
{
|
||||
std::string str = "Param: " + iter->first + " = \"" + iter->second + "\"";
|
||||
std::cout <<
|
||||
str <<
|
||||
std::setw(60 - str.size()) <<
|
||||
" [" <<
|
||||
rtabmap::Parameters::getDescription(iter->first).c_str() <<
|
||||
"]" <<
|
||||
std::endl;
|
||||
}
|
||||
ROS_WARN("Node will now exit after showing default odometry parameters because "
|
||||
"argument \"--params\" is detected!");
|
||||
exit(0);
|
||||
}
|
||||
}
|
||||
|
||||
StereoOdometry vOdom;
|
||||
ros::spin();
|
||||
return 0;
|
||||
}
|
||||
@@ -91,16 +91,15 @@ public:
|
||||
for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
|
||||
{
|
||||
std::string group = uSplit(iter->first, '/').front();
|
||||
if((group.compare("Odom") == 0 ||
|
||||
if(group.compare("Odom") == 0 ||
|
||||
group.compare("SURF") == 0 ||
|
||||
group.compare("SIFT") == 0 ||
|
||||
group.compare("ORB") == 0 ||
|
||||
group.compare("FAST") == 0 ||
|
||||
group.compare("FREAK") == 0 ||
|
||||
group.compare("BRIEF") == 0 ||
|
||||
group.compare("GFTT") == 0)
|
||||
&&
|
||||
group.compare("OdomICP") != 0)
|
||||
group.compare("GFTT") == 0 ||
|
||||
group.compare("BRISK") == 0)
|
||||
{
|
||||
parametersOdom.insert(*iter);
|
||||
}
|
||||
@@ -402,16 +401,15 @@ int main(int argc, char *argv[])
|
||||
// show specific parameters
|
||||
for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
|
||||
{
|
||||
if((uSplit(iter->first, '/').front().compare("Odom") == 0 ||
|
||||
if(uSplit(iter->first, '/').front().compare("Odom") == 0 ||
|
||||
uSplit(iter->first, '/').front().compare("SURF") == 0 ||
|
||||
uSplit(iter->first, '/').front().compare("SIFT") == 0 ||
|
||||
uSplit(iter->first, '/').front().compare("ORB") == 0 ||
|
||||
uSplit(iter->first, '/').front().compare("FAST") == 0 ||
|
||||
uSplit(iter->first, '/').front().compare("FREAK") == 0 ||
|
||||
uSplit(iter->first, '/').front().compare("BRIEF") == 0 ||
|
||||
uSplit(iter->first, '/').front().compare("GFTT") == 0)
|
||||
&&
|
||||
uSplit(iter->first, '/').front().compare("OdomICP") != 0)
|
||||
uSplit(iter->first, '/').front().compare("GFTT") == 0 ||
|
||||
uSplit(iter->first, '/').front().compare("BRISK") == 0)
|
||||
{
|
||||
parametersOdom.insert(*iter);
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user