rtabmap::ParametersMap RTABMapApp::getRtabmapParameters()
{
rtabmap::ParametersMap parameters;
parameters.insert(mappingParameters_.begin(), mappingParameters_.end());
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kKpDetectorStrategy(), std::string("6"))); // GFTT/BRIEF
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kGFTTQualityLevel(), std::string("0.0001")));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemImagePreDecimation(), std::string(fullResolution_?"2":"1")));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kFASTThreshold(), std::string("1")));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kBRIEFBytes(), std::string("64")));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapTimeThr(), std::string("800")));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemBinDataKept(), uBool2Str(!trajectoryMode_)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemNotLinkedNodesKept(), std::string("false")));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerIterations(), graphOptimization_?"10":"0"));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemIncrementalMemory(), uBool2Str(!localizationMode_)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapMaxRetrieved(), uBool2Str(!localizationMode_)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kKpMaxDepth(), std::string("10"))); // to avoid extracting features in invalid depth (as we compute transformation directly from the words)
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDOptimizeFromGraphEnd(), std::string("true")));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kDbSqlite3InMemory(), std::string("true")));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisMinInliers(), std::string("15")));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisEstimationType(), std::string("0"))); // 0=3D-3D 1=PnP
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDOptimizeMaxError(), std::string("0.05")));
return parameters;
}
void RTABMapApp::setTrajectoryMode(bool enabled)
{
if(trajectoryMode_ != enabled)
{
main_scene_.SetCameraType(enabled?tango_gl::GestureCamera::kTopDown:tango_gl::GestureCamera::kThirdPersonFollow);
}
trajectoryMode_ = enabled;
this->post(new rtabmap::ParamEvent(rtabmap::Parameters::kMemBinDataKept(), uBool2Str(!trajectoryMode_)));
}
int main (int argc, char * argv[])
{
ULogger::setType(ULogger::kTypeConsole);
ULogger::setLevel(ULogger::kInfo);
ULogger::setPrintTime(false);
ULogger::setPrintWhere(false);
// parse arguments
float rate = 0.0;
std::string inputDatabase;
int driver = 0;
int odomType = rtabmap::Parameters::defaultOdomFeatureType();
bool icp = false;
bool flow = false;
bool mono = false;
int nnType = rtabmap::Parameters::defaultOdomBowNNType();
float nndr = rtabmap::Parameters::defaultOdomBowNNDR();
float distance = rtabmap::Parameters::defaultOdomInlierDistance();
int maxWords = rtabmap::Parameters::defaultOdomMaxFeatures();
int minInliers = rtabmap::Parameters::defaultOdomMinInliers();
float maxDepth = rtabmap::Parameters::defaultOdomMaxDepth();
int iterations = rtabmap::Parameters::defaultOdomIterations();
int resetCountdown = rtabmap::Parameters::defaultOdomResetCountdown();
int decimation = 4;
float voxel = 0.005;
int samples = 10000;
float ratio = 0.7f;
int maxClouds = 10;
int briefBytes = rtabmap::Parameters::defaultBRIEFBytes();
int fastThr = rtabmap::Parameters::defaultFASTThreshold();
float sec = 0.0f;
bool gpu = false;
int localHistory = rtabmap::Parameters::defaultOdomBowLocalHistorySize();
bool p2p = rtabmap::Parameters::defaultOdomPnPEstimation();
for(int i=1; i<argc; ++i)
{
if(strcmp(argv[i], "-driver") == 0)
{
++i;
if(i < argc)
{
driver = std::atoi(argv[i]);
if(driver < 0 || driver > 7)
{
showUsage();
}
}
else
{
showUsage();
}
continue;
}
if(strcmp(argv[i], "-o") == 0)
{
++i;
if(i < argc)
{
odomType = std::atoi(argv[i]);
if(odomType < 0 || odomType > 6)
{
showUsage();
}
}
else
{
showUsage();
}
continue;
}
if(strcmp(argv[i], "-nn") == 0)
{
++i;
if(i < argc)
{
nnType = std::atoi(argv[i]);
if(nnType < 0 || nnType > 4)
{
showUsage();
}
}
else
{
showUsage();
}
continue;
}
if(strcmp(argv[i], "-nndr") == 0)
{
++i;
if(i < argc)
{
nndr = uStr2Float(argv[i]);
if(nndr < 0.0f)
{
showUsage();
}
}
else
{
showUsage();
}
continue;
}
if(strcmp(argv[i], "-hz") == 0)
{
++i;
if(i < argc)
{
rate = uStr2Float(argv[i]);
if(rate < 0)
{
showUsage();
}
}
else
{
showUsage();
}
continue;
}
if(strcmp(argv[i], "-db") == 0)
{
++i;
if(i < argc)
{
inputDatabase = argv[i];
if(UFile::getExtension(inputDatabase).compare("db") != 0)
{
printf("Database path (%s) should end with \"db\" \n", inputDatabase.c_str());
showUsage();
}
}
else
{
showUsage();
}
continue;
}
if(strcmp(argv[i], "-clouds") == 0)
{
++i;
if(i < argc)
{
maxClouds = std::atoi(argv[i]);
if(maxClouds < 0)
{
showUsage();
}
}
else
{
showUsage();
}
continue;
}
if(strcmp(argv[i], "-sec") == 0)
{
++i;
if(i < argc)
{
sec = uStr2Float(argv[i]);
if(sec < 0.0f)
{
showUsage();
}
}
else
{
showUsage();
}
continue;
}
if(strcmp(argv[i], "-in") == 0)
{
++i;
if(i < argc)
{
distance = uStr2Float(argv[i]);
if(distance <= 0)
{
showUsage();
}
}
else
{
showUsage();
}
continue;
}
if(strcmp(argv[i], "-max") == 0)
{
++i;
if(i < argc)
{
maxWords = std::atoi(argv[i]);
if(maxWords < 0)
{
showUsage();
}
}
else
{
showUsage();
}
continue;
}
if(strcmp(argv[i], "-min") == 0)
{
++i;
if(i < argc)
{
minInliers = std::atoi(argv[i]);
if(minInliers < 0)
{
showUsage();
}
}
else
{
showUsage();
}
continue;
}
if(strcmp(argv[i], "-depth") == 0)
{
++i;
if(i < argc)
{
maxDepth = uStr2Float(argv[i]);
if(maxDepth < 0)
{
showUsage();
}
}
else
{
showUsage();
}
continue;
}
if(strcmp(argv[i], "-i") == 0)
{
++i;
if(i < argc)
{
iterations = std::atoi(argv[i]);
if(iterations <= 0)
{
showUsage();
}
}
else
{
showUsage();
}
continue;
}
if(strcmp(argv[i], "-reset") == 0)
{
++i;
if(i < argc)
{
resetCountdown = std::atoi(argv[i]);
if(resetCountdown < 0)
{
showUsage();
}
}
else
{
showUsage();
}
continue;
}
if(strcmp(argv[i], "-d") == 0)
{
++i;
if(i < argc)
{
decimation = std::atoi(argv[i]);
if(decimation < 1)
{
showUsage();
}
}
else
{
showUsage();
}
continue;
}
if(strcmp(argv[i], "-v") == 0)
{
++i;
if(i < argc)
{
voxel = uStr2Float(argv[i]);
if(voxel < 0)
{
showUsage();
}
}
else
{
showUsage();
}
continue;
}
if(strcmp(argv[i], "-s") == 0)
{
++i;
if(i < argc)
{
samples = std::atoi(argv[i]);
if(samples < 0)
{
showUsage();
}
}
else
{
showUsage();
}
continue;
}
if(strcmp(argv[i], "-cr") == 0)
{
++i;
if(i < argc)
{
ratio = uStr2Float(argv[i]);
if(ratio < 0.0f)
{
showUsage();
}
}
else
{
showUsage();
}
continue;
}
if(strcmp(argv[i], "-gpu") == 0)
{
gpu = true;
continue;
}
if(strcmp(argv[i], "-lh") == 0)
{
++i;
if(i < argc)
{
localHistory = std::atoi(argv[i]);
if(localHistory < 0)
{
showUsage();
}
}
else
{
showUsage();
}
continue;
}
if(strcmp(argv[i], "-brief_bytes") == 0)
{
++i;
if(i < argc)
{
briefBytes = std::atoi(argv[i]);
if(briefBytes < 1)
{
showUsage();
}
}
else
{
showUsage();
}
continue;
}
if(strcmp(argv[i], "-fast_thr") == 0)
{
++i;
if(i < argc)
{
fastThr = std::atoi(argv[i]);
if(fastThr < 1)
{
showUsage();
}
}
else
{
showUsage();
}
continue;
}
if(strcmp(argv[i], "-icp") == 0)
{
icp = true;
continue;
}
if(strcmp(argv[i], "-flow") == 0)
{
flow = true;
continue;
}
if(strcmp(argv[i], "-mono") == 0)
{
mono = true;
continue;
}
if(strcmp(argv[i], "-p2p") == 0)
{
p2p = true;
continue;
}
if(strcmp(argv[i], "-debug") == 0)
{
ULogger::setLevel(ULogger::kDebug);
ULogger::setPrintTime(true);
ULogger::setPrintWhere(true);
continue;
}
printf("Unrecognized option : %s\n", argv[i]);
showUsage();
}
if(odomType > 1 && nnType == rtabmap::VWDictionary::kNNFlannKdTree)
{
UERROR("You set \"-o %d\" (binary descriptor), you must use \"-nn 2\" (any \"-nn\" other than kNNFlannKdTree)", odomType);
showUsage();
}
else if(odomType <= 1 && nnType == rtabmap::VWDictionary::kNNFlannLSH)
{
UERROR("You set \"-o %d\" (float descriptor), you must use \"-nn 1\" (any \"-nn\" other than kNNFlannLSH)", odomType);
showUsage();
}
if(inputDatabase.size())
{
UINFO("Using database input \"%s\"", inputDatabase.c_str());
}
else
{
UINFO("Using OpenNI camera");
}
std::string odomName;
if(odomType == 0)
{
odomName = "SURF";
}
else if(odomType == 1)
{
odomName = "SIFT";
}
else if(odomType == 2)
{
odomName = "ORB";
}
else if(odomType == 3)
{
odomName = "FAST+FREAK";
}
else if(odomType == 4)
{
odomName = "FAST+BRIEF";
}
else if(odomType == 5)
{
odomName = "GFTT+FREAK";
}
else if(odomType == 6)
{
odomName = "GFTT+BRIEF";
}
else if(odomType == 7)
{
odomName = "BRISK";
}
if(icp)
{
odomName= "ICP";
}
if(flow)
{
odomName= "Optical Flow";
}
std::string nnName;
if(nnType == 0)
{
nnName = "kNNFlannLinear";
}
else if(nnType == 1)
{
nnName = "kNNFlannKdTree";
}
else if(nnType == 2)
{
nnName= "kNNFlannLSH";
}
else if(nnType == 3)
{
nnName= "kNNBruteForce";
}
else if(nnType == 4)
{
nnName= "kNNBruteForceGPU";
}
UINFO("Odometry used = %s", odomName.c_str());
UINFO("Camera rate = %f Hz", rate);
UINFO("Maximum clouds shown = %d", maxClouds);
UINFO("Delay = %f s", sec);
UINFO("Max depth = %f", maxDepth);
UINFO("Reset odometry coutdown = %d", resetCountdown);
QApplication app(argc, argv);
rtabmap::Odometry * odom = 0;
rtabmap::ParametersMap parameters;
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomMaxDepth(), uNumber2Str(maxDepth)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomResetCountdown(), uNumber2Str(resetCountdown)));
if(!icp)
{
UINFO("Min inliers = %d", minInliers);
UINFO("Inlier maximum correspondences distance = %f", distance);
UINFO("RANSAC iterations = %d", iterations);
UINFO("Max features = %d", maxWords);
UINFO("GPU = %s", gpu?"true":"false");
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomInlierDistance(), uNumber2Str(distance)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomMinInliers(), uNumber2Str(minInliers)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomIterations(), uNumber2Str(iterations)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomMaxFeatures(), uNumber2Str(maxWords)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomFeatureType(), uNumber2Str(odomType)));
if(odomType == 0)
{
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kSURFGpuVersion(), uBool2Str(gpu)));
}
if(odomType == 2)
{
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kORBGpu(), uBool2Str(gpu)));
}
if(odomType == 3 || odomType == 4)
{
UINFO("FAST threshold = %d", fastThr);
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kFASTThreshold(), uNumber2Str(fastThr)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kFASTGpu(), uBool2Str(gpu)));
}
if(odomType == 4 || odomType == 6)
{
UINFO("BRIEF bytes = %d", briefBytes);
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kBRIEFBytes(), uNumber2Str(briefBytes)));
}
if(flow)
{
// Optical Flow
odom = new rtabmap::OdometryOpticalFlow(parameters);
}
else
{
//BOW
UINFO("Nearest neighbor = %s", nnName.c_str());
UINFO("Nearest neighbor ratio = %f", nndr);
UINFO("Local history = %d", localHistory);
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomBowNNType(), uNumber2Str(nnType)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomBowNNDR(), uNumber2Str(nndr)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomBowLocalHistorySize(), uNumber2Str(localHistory)));
if(mono)
{
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomPnPFlags(), uNumber2Str(0))); //CV_ITERATIVE
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomPnPReprojError(), "4.0"));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomIterations(), "100"));
odom = new rtabmap::OdometryMono(parameters);
}
else
{
odom = new rtabmap::OdometryBOW(parameters);
}
}
}
else if(icp) // ICP
{
UINFO("ICP maximum correspondences distance = %f", distance);
UINFO("ICP iterations = %d", iterations);
UINFO("Cloud decimation = %d", decimation);
UINFO("Cloud voxel size = %f", voxel);
UINFO("Cloud samples = %d", samples);
UINFO("Cloud correspondence ratio = %f", ratio);
UINFO("Cloud point to plane = %s", p2p?"false":"true");
odom = new rtabmap::OdometryICP(decimation, voxel, samples, distance, iterations, ratio, !p2p);
}
rtabmap::OdometryThread odomThread(odom);
rtabmap::OdometryViewer odomViewer(maxClouds, 2, 0.0, 50);
UEventsManager::addHandler(&odomThread);
UEventsManager::addHandler(&odomViewer);
odomViewer.setWindowTitle("Odometry view");
odomViewer.resize(1280, 480+QPushButton().minimumHeight());
if(inputDatabase.size())
{
rtabmap::DBReader camera(inputDatabase, rate, true);
if(camera.init())
{
odomThread.start();
if(sec > 0)
{
uSleep(sec*1000);
}
camera.start();
app.exec();
camera.join(true);
odomThread.join(true);
}
}
else
{
rtabmap::CameraRGBD * camera = 0;
rtabmap::Transform t=rtabmap::Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0);
if(driver == 0)
{
camera = new rtabmap::CameraOpenni("", rate, t);
}
else if(driver == 1)
{
if(!rtabmap::CameraOpenNI2::available())
{
UERROR("Not built with OpenNI2 support...");
exit(-1);
}
camera = new rtabmap::CameraOpenNI2("", rate, t);
}
else if(driver == 2)
{
if(!rtabmap::CameraFreenect::available())
{
UERROR("Not built with Freenect support...");
exit(-1);
}
camera = new rtabmap::CameraFreenect(0, rate, t);
}
else if(driver == 3)
{
if(!rtabmap::CameraOpenNICV::available())
{
UERROR("Not built with OpenNI from OpenCV support...");
exit(-1);
}
camera = new rtabmap::CameraOpenNICV(false, rate, t);
}
else if(driver == 4)
{
if(!rtabmap::CameraOpenNICV::available())
{
UERROR("Not built with OpenNI from OpenCV support...");
exit(-1);
}
camera = new rtabmap::CameraOpenNICV(true, rate, t);
}
else if(driver == 5)
{
if(!rtabmap::CameraFreenect2::available())
{
UERROR("Not built with Freenect2 support...");
exit(-1);
}
camera = new rtabmap::CameraFreenect2(0, rtabmap::CameraFreenect2::kTypeRGBDepthSD, rate, t);
}
else if(driver == 6)
{
if(!rtabmap::CameraStereoDC1394::available())
{
UERROR("Not built with dc1394 support...");
exit(-1);
}
camera = new rtabmap::CameraStereoDC1394(rate, t);
}
else if(driver == 7)
{
if(!rtabmap::CameraStereoFlyCapture2::available())
{
UERROR("Not built with FlyCapture2/Triclops support...");
exit(-1);
}
camera = new rtabmap::CameraStereoFlyCapture2(rate, t);
}
else
{
UFATAL("Camera driver (%d) not found!", driver);
}
//pcl::console::setVerbosityLevel(pcl::console::L_DEBUG);
if(camera->init())
{
if(camera->isCalibrated())
{
rtabmap::CameraThread cameraThread(camera);
odomThread.start();
cameraThread.start();
odomViewer.exec();
cameraThread.join(true);
odomThread.join(true);
}
else
{
printf("The camera is not calibrated! You should calibrate the camera first.\n");
delete camera;
}
}
else
{
printf("Failed to initialize the camera! Please select another driver (see \"--help\").\n");
delete camera;
}
}
return 0;
}
void RTABMapApp::setLocalizationMode(bool enabled)
{
localizationMode_ = enabled;
this->post(new rtabmap::ParamEvent(rtabmap::Parameters::kMemIncrementalMemory(), uBool2Str(!localizationMode_)));
}
OdometryROS::OdometryROS(int argc, char * argv[], bool stereo) :
odometry_(0),
frameId_("base_link"),
odomFrameId_("odom"),
groundTruthFrameId_(""),
publishTf_(true),
waitForTransform_(true),
waitForTransformDuration_(0.1), // 100 ms
paused_(false)
{
ros::NodeHandle nh;
odomPub_ = nh.advertise<nav_msgs::Odometry>("odom", 1);
odomInfoPub_ = nh.advertise<rtabmap_ros::OdomInfo>("odom_info", 1);
odomLocalMap_ = nh.advertise<sensor_msgs::PointCloud2>("odom_local_map", 1);
odomLastFrame_ = nh.advertise<sensor_msgs::PointCloud2>("odom_last_frame", 1);
ros::NodeHandle pnh("~");
Transform initialPose = Transform::getIdentity();
std::string initialPoseStr;
std::string tfPrefix;
std::string configPath;
pnh.param("frame_id", frameId_, frameId_);
pnh.param("odom_frame_id", odomFrameId_, odomFrameId_);
pnh.param("publish_tf", publishTf_, publishTf_);
pnh.param("tf_prefix", tfPrefix, tfPrefix);
pnh.param("wait_for_transform", waitForTransform_, waitForTransform_);
pnh.param("wait_for_transform_duration", waitForTransformDuration_, waitForTransformDuration_);
pnh.param("initial_pose", initialPoseStr, initialPoseStr); // "x y z roll pitch yaw"
pnh.param("ground_truth_frame_id", groundTruthFrameId_, groundTruthFrameId_);
pnh.param("config_path", configPath, configPath);
configPath = uReplaceChar(configPath, '~', UDirectory::homeDir());
if(configPath.size() && configPath.at(0) != '/')
{
configPath = UDirectory::currentDir(true) + configPath;
}
if(!tfPrefix.empty())
{
if(!frameId_.empty())
{
frameId_ = tfPrefix + "/" + frameId_;
}
if(!odomFrameId_.empty())
{
odomFrameId_ = tfPrefix + "/" + odomFrameId_;
}
if(!groundTruthFrameId_.empty())
{
groundTruthFrameId_ = tfPrefix + "/" + groundTruthFrameId_;
}
}
if(initialPoseStr.size())
{
std::vector<std::string> values = uListToVector(uSplit(initialPoseStr, ' '));
if(values.size() == 6)
{
initialPose = Transform(
uStr2Float(values[0]), uStr2Float(values[1]), uStr2Float(values[2]),
uStr2Float(values[3]), uStr2Float(values[4]), uStr2Float(values[5]));
}
else
{
ROS_ERROR("Wrong initial_pose format: %s (should be \"x y z roll pitch yaw\" with angle in radians). "
"Identity will be used...", initialPoseStr.c_str());
}
}
//parameters
parameters_ = Parameters::getDefaultOdometryParameters(stereo);
if(!configPath.empty())
{
if(UFile::exists(configPath.c_str()))
{
ROS_INFO("Odometry: Loading parameters from %s", configPath.c_str());
rtabmap::ParametersMap allParameters;
Parameters::readINI(configPath.c_str(), allParameters);
// only update odometry parameters
for(ParametersMap::iterator iter=parameters_.begin(); iter!=parameters_.end(); ++iter)
{
ParametersMap::iterator jter = allParameters.find(iter->first);
if(jter!=allParameters.end())
{
iter->second = jter->second;
}
}
}
else
{
ROS_ERROR("Config file \"%s\" not found!", configPath.c_str());
}
}
for(rtabmap::ParametersMap::iterator iter=parameters_.begin(); iter!=parameters_.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, vDouble))
{
ROS_INFO("Setting odometry parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vDouble).c_str());
iter->second = uNumber2Str(vDouble);
}
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::kVisMinInliers()) == 0 && atoi(iter->second.c_str()) < 8)
{
ROS_WARN("Parameter min_inliers must be >= 8, setting to 8...");
iter->second = uNumber2Str(8);
}
}
rtabmap::ParametersMap parameters = rtabmap::Parameters::parseArguments(argc, argv);
for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
{
rtabmap::ParametersMap::iterator jter = parameters_.find(iter->first);
if(jter!=parameters_.end())
{
ROS_INFO("Update odometry parameter \"%s\"=\"%s\" from arguments", iter->first.c_str(), iter->second.c_str());
jter->second = iter->second;
}
}
// Backward compatibility
for(std::map<std::string, std::pair<bool, std::string> >::const_iterator iter=Parameters::getRemovedParameters().begin();
iter!=Parameters::getRemovedParameters().end();
++iter)
{
std::string vStr;
if(pnh.getParam(iter->first, vStr))
{
if(iter->second.first)
{
// can be migrated
parameters_.at(iter->second.second)= vStr;
ROS_WARN("Odometry: Parameter name changed: \"%s\" -> \"%s\". Please update your launch file accordingly. Value \"%s\" is still set to the new parameter name.",
iter->first.c_str(), iter->second.second.c_str(), vStr.c_str());
}
else
{
if(iter->second.second.empty())
{
ROS_ERROR("Odometry: Parameter \"%s\" doesn't exist anymore!",
iter->first.c_str());
}
else
{
ROS_ERROR("Odometry: Parameter \"%s\" doesn't exist anymore! You may look at this similar parameter: \"%s\"",
iter->first.c_str(), iter->second.second.c_str());
}
}
}
}
int odomStrategy = 0; // BOW
Parameters::parse(parameters_, Parameters::kOdomStrategy(), odomStrategy);
odometry_ = Odometry::create(parameters_);
if(!initialPose.isIdentity())
{
odometry_->reset(initialPose);
}
resetSrv_ = nh.advertiseService("reset_odom", &OdometryROS::reset, this);
resetToPoseSrv_ = nh.advertiseService("reset_odom_to_pose", &OdometryROS::resetToPose, this);
pauseSrv_ = nh.advertiseService("pause_odom", &OdometryROS::pause, this);
resumeSrv_ = nh.advertiseService("resume_odom", &OdometryROS::resume, this);
setLogDebugSrv_ = pnh.advertiseService("log_debug", &OdometryROS::setLogDebug, this);
setLogInfoSrv_ = pnh.advertiseService("log_info", &OdometryROS::setLogInfo, this);
setLogWarnSrv_ = pnh.advertiseService("log_warning", &OdometryROS::setLogWarn, this);
setLogErrorSrv_ = pnh.advertiseService("log_error", &OdometryROS::setLogError, this);
}
