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;