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

StereoOdometryNode: now use default parameters Odom/MaxDepth=0 and Odom/EstimationType=1. Odometry ndoes exit directly when --params argument is detected.

Browse Source
This commit is contained in:
matlabbe
2015-10-01 11:37:40 -04:00
parent 24a40ceee8
commit 7d732aae7c
6 changed files with 54 additions and 51 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/OdometryROS.cpp
+39 -46
View File
@@ -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<nav_msgs::Odometry>("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;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(iter->first.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 + "\"";
src/OdometryROS.h
+5 -3
View File
@@ -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&);
src/RGBDOdometryNode.cpp
+3
View File
@@ -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;
src/StereoOdometryNode.cpp
+4 -1
View File
@@ -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;