From 7d732aae7c234e3b5a11bb4ba6829d2752a1d7ac Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 1 Oct 2015 11:37:40 -0400 Subject: [PATCH] StereoOdometryNode: now use default parameters Odom/MaxDepth=0 and Odom/EstimationType=1. Odometry ndoes exit directly when --params argument is detected. --- launch/rgbd_mapping.launch | 1 + launch/stereo_mapping.launch | 3 +- src/OdometryROS.cpp | 85 +++++++++++++++++------------------- src/OdometryROS.h | 8 ++-- src/RGBDOdometryNode.cpp | 3 ++ src/StereoOdometryNode.cpp | 5 ++- 6 files changed, 54 insertions(+), 51 deletions(-) diff --git a/launch/rgbd_mapping.launch b/launch/rgbd_mapping.launch index f78b6492..b86ef754 100644 --- a/launch/rgbd_mapping.launch +++ b/launch/rgbd_mapping.launch @@ -88,6 +88,7 @@ + diff --git a/launch/stereo_mapping.launch b/launch/stereo_mapping.launch index 2198b21b..4913e319 100644 --- a/launch/stereo_mapping.launch +++ b/launch/stereo_mapping.launch @@ -41,7 +41,7 @@ - + @@ -97,6 +97,7 @@ + diff --git a/src/OdometryROS.cpp b/src/OdometryROS.cpp index a0b29f99..5249f2e8 100644 --- a/src/OdometryROS.cpp +++ b/src/OdometryROS.cpp @@ -50,7 +50,7 @@ using namespace rtabmap; namespace rtabmap_ros { -OdometryROS::OdometryROS(int argc, char * argv[]) : +OdometryROS::OdometryROS(int argc, char * argv[], bool stereo) : odometry_(0), frameId_("base_link"), odomFrameId_("odom"), @@ -60,8 +60,6 @@ OdometryROS::OdometryROS(int argc, char * argv[]) : waitForTransformDuration_(0.1), // 100 ms paused_(false) { - this->processArguments(argc, argv); - ros::NodeHandle nh; odomPub_ = nh.advertise("odom", 1); @@ -117,25 +115,7 @@ OdometryROS::OdometryROS(int argc, char * argv[]) : //parameters - rtabmap::ParametersMap parameters = rtabmap::Parameters::getDefaultParameters(); - for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter) - { - std::string group = uSplit(iter->first, '/').front(); - if(uStrContains(group, "Odom") || - group.compare("Stereo") || - 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) - { - parameters_.insert(*iter); - } - } - + parameters_ = this->getDefaultOdometryParameters(stereo); for(rtabmap::ParametersMap::iterator iter=parameters_.begin(); iter!=parameters_.end(); ++iter) { std::string vStr; @@ -257,35 +237,48 @@ OdometryROS::~OdometryROS() delete odometry_; } -void OdometryROS::processArguments(int argc, char * argv[]) +rtabmap::ParametersMap OdometryROS::getDefaultOdometryParameters(bool stereo) +{ + rtabmap::ParametersMap odomParameters; + rtabmap::ParametersMap defaultParameters = rtabmap::Parameters::getDefaultParameters(); + for(rtabmap::ParametersMap::iterator iter=defaultParameters.begin(); iter!=defaultParameters.end(); ++iter) + { + std::string group = uSplit(iter->first, '/').front(); + if(uStrContains(group, "Odom") || + group.compare("Stereo") || + 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) + { + if(stereo) + { + if(iter->first.compare(Parameters::kOdomMaxDepth()) == 0) + { + iter->second = "0"; // infinity + } + else if(iter->first.compare(Parameters::kOdomEstimationType()) == 0) + { + iter->second = "1"; // 3D->2D (PNP) + } + } + odomParameters.insert(*iter); + } + } + return odomParameters; +} + +void OdometryROS::processArguments(int argc, char * argv[], bool stereo) { for(int i=1;ifirst.find("Odom") == 0 || - uSplit(iter->first, '/').front().compare("Stereo") == 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); - } - } - } - + rtabmap::ParametersMap parametersOdom = getDefaultOdometryParameters(stereo); for(rtabmap::ParametersMap::iterator iter=parametersOdom.begin(); iter!=parametersOdom.end(); ++iter) { std::string str = "Param: " + iter->first + " = \"" + iter->second + "\""; diff --git a/src/OdometryROS.h b/src/OdometryROS.h index af7aff72..5d37cc9b 100644 --- a/src/OdometryROS.h +++ b/src/OdometryROS.h @@ -49,10 +49,12 @@ namespace rtabmap_ros { class OdometryROS { public: - OdometryROS(int argc, char * argv[]); - ~OdometryROS(); + static rtabmap::ParametersMap getDefaultOdometryParameters(bool stereo = false); + static void processArguments(int argc, char * argv[], bool stereo = false); - void processArguments(int argc, char * argv[]); +public: + OdometryROS(int argc, char * argv[], bool stereo = false); + ~OdometryROS(); void processData(const rtabmap::SensorData & data, const ros::Time & stamp); bool reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&); diff --git a/src/RGBDOdometryNode.cpp b/src/RGBDOdometryNode.cpp index 5cb80ae0..80bd2a0d 100644 --- a/src/RGBDOdometryNode.cpp +++ b/src/RGBDOdometryNode.cpp @@ -382,6 +382,9 @@ int main(int argc, char *argv[]) ULogger::setLevel(ULogger::kWarning); ros::init(argc, argv, "rgbd_odometry"); + // process "--params" argument + rtabmap_ros::OdometryROS::processArguments(argc, argv); + RGBDOdometry odom(argc, argv); ros::spin(); return 0; diff --git a/src/StereoOdometryNode.cpp b/src/StereoOdometryNode.cpp index 3e1e05f0..6da21b11 100644 --- a/src/StereoOdometryNode.cpp +++ b/src/StereoOdometryNode.cpp @@ -52,7 +52,7 @@ class StereoOdometry : public rtabmap_ros::OdometryROS { public: StereoOdometry(int argc, char * argv[]) : - rtabmap_ros::OdometryROS(argc, argv), + rtabmap_ros::OdometryROS(argc, argv, true), approxSync_(0), exactSync_(0) { @@ -202,6 +202,9 @@ int main(int argc, char *argv[]) //pcl::console::setVerbosityLevel(pcl::console::L_DEBUG); ros::init(argc, argv, "stereo_odometry"); + // process "--params" argument + rtabmap_ros::OdometryROS::processArguments(argc, argv, true); + StereoOdometry odom(argc, argv); ros::spin(); return 0;