void ParametersToolBox::updateParameter(const std::string & key, const std::string & value)
{
QString group = QString::fromStdString(key).split("/").first();
if(!ignoredGroups_.contains(group))
{
if(parameters_.find(key) == parameters_.end())
{
UWARN("key=\"%s\" doesn't exist", key.c_str());
}
else
{
parameters_.at(key) = value;
QWidget * widget = this->findChild<QWidget*>(key.c_str());
QString type = QString::fromStdString(Parameters::getType(key));
if(type.compare("string") == 0)
{
QString valueQt = QString::fromStdString(value);
if(valueQt.contains(';'))
{
// It's a list, just change the index
QStringList splitted = valueQt.split(':');
((QComboBox*)widget)->setCurrentIndex(splitted.first().toInt());
}
else
{
((QLineEdit*)widget)->setText(valueQt);
}
}
else if(type.compare("int") == 0)
{
((QSpinBox*)widget)->setValue(uStr2Int(value));
}
else if(type.compare("uint") == 0)
{
((QSpinBox*)widget)->setValue(uStr2Int(value));
}
else if(type.compare("double") == 0)
{
((QDoubleSpinBox*)widget)->setValue(uStr2Double(value));
}
else if(type.compare("float") == 0)
{
((QDoubleSpinBox*)widget)->setValue(uStr2Float(value));
}
else if(type.compare("bool") == 0)
{
((QCheckBox*)widget)->setChecked(uStr2Bool(value));
}
}
}
}
int main(int argc, char * argv[])
{
ULogger::setType(ULogger::kTypeConsole);
ULogger::setLevel(ULogger::kInfo);
int device = 0;
std::string path;
float rate = 0.0f;
std::string calibrationFile;
for(int i=1; i<argc; ++i)
{
if(strcmp(argv[i], "--rate") == 0)
{
++i;
if(i < argc)
{
rate = uStr2Float(argv[i]);
if(rate < 0)
{
showUsage();
}
}
else
{
showUsage();
}
continue;
}
if(strcmp(argv[i], "--device") == 0)
{
++i;
if(i < argc)
{
device = std::atoi(argv[i]);
if(device < 0)
{
showUsage();
}
}
else
{
showUsage();
}
continue;
}
if(strcmp(argv[i], "--path") == 0)
{
++i;
if(i < argc)
{
path = argv[i];
}
else
{
showUsage();
}
continue;
}
if(strcmp(argv[i], "--calibration") == 0)
{
++i;
if(i < argc)
{
calibrationFile = argv[i];
}
else
{
showUsage();
}
continue;
}
printf("Unrecognized option : %s\n", argv[i]);
showUsage();
}
if(path.empty())
{
UINFO("Using device %d", device);
}
else
{
UINFO("Using path %s", path.c_str());
}
rtabmap::Camera * camera = 0;
if(!path.empty())
{
if(UFile::exists(path))
{
if(UFile::getExtension(path).compare("db") == 0)
{
camera = new rtabmap::DBReader(path, rate);
}
else
{
camera = new rtabmap::CameraVideo(path, false, rate);
}
}
else if(UDirectory::exists(path))
{
camera = new rtabmap::CameraImages(path, rate);
}
else
{
UERROR("Path not valid! \"%s\"", path.c_str());
return -1;
}
}
else
{
camera = new rtabmap::CameraVideo(device, rate);
}
if(camera)
{
if(!calibrationFile.empty())
{
UINFO("Set calibration: %s", calibrationFile.c_str());
}
if(!camera->init(UDirectory::getDir(calibrationFile), UFile::getName(calibrationFile)))
{
delete camera;
UERROR("Cannot initialize the camera.");
return -1;
}
}
cv::Mat rgb;
rgb = camera->takeImage().imageRaw();
cv::namedWindow("Video", CV_WINDOW_AUTOSIZE); // create window
while(!rgb.empty())
{
cv::imshow("Video", rgb); // show frame
int c = cv::waitKey(10); // wait 10 ms or for key stroke
if(c == 27)
break; // if ESC, break and quit
rgb = camera->takeImage().imageRaw();
}
cv::destroyWindow("Video");
if(camera)
{
delete camera;
}
return 0;
}
int main(int argc, char * argv[])
{
ULogger::setType(ULogger::kTypeConsole);
ULogger::setLevel(ULogger::kInfo);
//ULogger::setPrintTime(false);
//ULogger::setPrintWhere(false);
int driver = 0;
std::string stereoSavePath;
float rate = 0.0f;
std::string fourcc = "MJPG";
if(argc < 2)
{
showUsage();
}
else
{
for(int i=1; i<argc; ++i)
{
if(strcmp(argv[i], "-rate") == 0)
{
++i;
if(i < argc)
{
rate = uStr2Float(argv[i]);
if(rate < 0.0f)
{
showUsage();
}
}
else
{
showUsage();
}
continue;
}
if(strcmp(argv[i], "-save_stereo") == 0)
{
++i;
if(i < argc)
{
stereoSavePath = argv[i];
}
else
{
showUsage();
}
continue;
}
if(strcmp(argv[i], "-fourcc") == 0)
{
++i;
if(i < argc)
{
fourcc = argv[i];
if(fourcc.size() != 4)
{
UERROR("fourcc should be 4 characters.");
showUsage();
}
}
else
{
showUsage();
}
continue;
}
if(strcmp(argv[i], "--help") == 0 || strcmp(argv[i], "-help") == 0)
{
showUsage();
}
else if(i< argc-1)
{
printf("Unrecognized option \"%s\"", argv[i]);
showUsage();
}
// last
driver = atoi(argv[i]);
if(driver < 0 || driver > 8)
{
UERROR("driver should be between 0 and 8.");
showUsage();
}
}
}
UINFO("Using driver %d", driver);
rtabmap::Camera * camera = 0;
if(driver < 6)
{
if(!stereoSavePath.empty())
{
UWARN("-save_stereo option cannot be used with RGB-D drivers.");
stereoSavePath.clear();
}
if(driver == 0)
{
camera = new rtabmap::CameraOpenni();
}
else if(driver == 1)
{
if(!rtabmap::CameraOpenNI2::available())
{
UERROR("Not built with OpenNI2 support...");
exit(-1);
}
camera = new rtabmap::CameraOpenNI2();
}
else if(driver == 2)
{
if(!rtabmap::CameraFreenect::available())
{
UERROR("Not built with Freenect support...");
exit(-1);
}
camera = new rtabmap::CameraFreenect();
}
else if(driver == 3)
{
if(!rtabmap::CameraOpenNICV::available())
{
UERROR("Not built with OpenNI from OpenCV support...");
exit(-1);
}
camera = new rtabmap::CameraOpenNICV(false);
}
else if(driver == 4)
{
if(!rtabmap::CameraOpenNICV::available())
{
UERROR("Not built with OpenNI from OpenCV support...");
exit(-1);
}
camera = new rtabmap::CameraOpenNICV(true);
}
else if(driver == 5)
{
if(!rtabmap::CameraFreenect2::available())
{
UERROR("Not built with Freenect2 support...");
exit(-1);
}
camera = new rtabmap::CameraFreenect2(0, rtabmap::CameraFreenect2::kTypeColor2DepthSD);
}
}
else if(driver == 6)
{
if(!rtabmap::CameraStereoDC1394::available())
{
UERROR("Not built with DC1394 support...");
exit(-1);
}
camera = new rtabmap::CameraStereoDC1394();
}
else if(driver == 7)
{
if(!rtabmap::CameraStereoFlyCapture2::available())
{
UERROR("Not built with FlyCapture2/Triclops support...");
exit(-1);
}
camera = new rtabmap::CameraStereoFlyCapture2();
}
else if(driver == 8)
{
if(!rtabmap::CameraStereoZed::available())
{
UERROR("Not built with ZED sdk support...");
exit(-1);
}
camera = new rtabmap::CameraStereoZed(0);
}
else
{
UFATAL("");
}
if(!camera->init())
{
printf("Camera init failed! Please select another driver (see \"--help\").\n");
delete camera;
exit(1);
}
rtabmap::SensorData data = camera->takeImage();
if(data.imageRaw().cols != data.depthOrRightRaw().cols || data.imageRaw().rows != data.depthOrRightRaw().rows)
{
UWARN("RGB (%d/%d) and depth (%d/%d) frames are not the same size! The registered cloud cannot be shown.",
data.imageRaw().cols, data.imageRaw().rows, data.depthOrRightRaw().cols, data.depthOrRightRaw().rows);
}
pcl::visualization::CloudViewer * viewer = 0;
if(!data.stereoCameraModel().isValidForProjection() && (data.cameraModels().size() == 0 || !data.cameraModels()[0].isValidForProjection()))
{
UWARN("Camera not calibrated! The registered cloud cannot be shown.");
}
else
{
viewer = new pcl::visualization::CloudViewer("cloud");
}
rtabmap::Transform t(1, 0, 0, 0,
0, -1, 0, 0,
0, 0, -1, 0);
cv::VideoWriter videoWriter;
UDirectory dir;
if(!stereoSavePath.empty() &&
!data.imageRaw().empty() &&
!data.rightRaw().empty())
{
if(UFile::getExtension(stereoSavePath).compare("avi") == 0)
{
if(data.imageRaw().size() == data.rightRaw().size())
{
if(rate <= 0)
{
UERROR("You should set the input rate when saving stereo images to a video file.");
showUsage();
}
cv::Size targetSize = data.imageRaw().size();
targetSize.width *= 2;
UASSERT(fourcc.size() == 4);
videoWriter.open(
stereoSavePath,
CV_FOURCC(fourcc.at(0), fourcc.at(1), fourcc.at(2), fourcc.at(3)),
rate,
targetSize,
data.imageRaw().channels() == 3);
}
else
{
UERROR("Images not the same size, cannot save stereo images to the video file.");
}
}
else if(UDirectory::exists(stereoSavePath))
{
UDirectory::makeDir(stereoSavePath+"/"+"left");
UDirectory::makeDir(stereoSavePath+"/"+"right");
}
else
{
UERROR("Directory \"%s\" doesn't exist.", stereoSavePath.c_str());
stereoSavePath.clear();
}
}
// to catch the ctrl-c
signal(SIGABRT, &sighandler);
signal(SIGTERM, &sighandler);
signal(SIGINT, &sighandler);
int id=1;
while(!data.imageRaw().empty() && (viewer==0 || !viewer->wasStopped()) && running)
{
cv::Mat rgb = data.imageRaw();
if(!data.depthRaw().empty() && (data.depthRaw().type() == CV_16UC1 || data.depthRaw().type() == CV_32FC1))
{
// depth
cv::Mat depth = data.depthRaw();
if(depth.type() == CV_32FC1)
{
depth = rtabmap::util2d::cvtDepthFromFloat(depth);
}
if(rgb.cols == depth.cols && rgb.rows == depth.rows &&
data.cameraModels().size() &&
data.cameraModels()[0].isValidForProjection())
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = rtabmap::util3d::cloudFromDepthRGB(
rgb, depth,
data.cameraModels()[0]);
cloud = rtabmap::util3d::transformPointCloud(cloud, t);
if(viewer)
viewer->showCloud(cloud, "cloud");
}
else if(!depth.empty() &&
data.cameraModels().size() &&
data.cameraModels()[0].isValidForProjection())
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = rtabmap::util3d::cloudFromDepth(
depth,
data.cameraModels()[0]);
cloud = rtabmap::util3d::transformPointCloud(cloud, t);
viewer->showCloud(cloud, "cloud");
}
cv::Mat tmp;
unsigned short min=0, max = 2048;
uMinMax((unsigned short*)depth.data, depth.rows*depth.cols, min, max);
depth.convertTo(tmp, CV_8UC1, 255.0/max);
cv::imshow("Video", rgb); // show frame
cv::imshow("Depth", tmp);
}
else if(!data.rightRaw().empty())
{
// stereo
cv::Mat right = data.rightRaw();
cv::imshow("Left", rgb); // show frame
cv::imshow("Right", right);
if(rgb.cols == right.cols && rgb.rows == right.rows && data.stereoCameraModel().isValidForProjection())
{
if(right.channels() == 3)
{
cv::cvtColor(right, right, CV_BGR2GRAY);
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = rtabmap::util3d::cloudFromStereoImages(
rgb, right,
data.stereoCameraModel());
cloud = rtabmap::util3d::transformPointCloud(cloud, t);
if(viewer)
viewer->showCloud(cloud, "cloud");
}
}
int c = cv::waitKey(10); // wait 10 ms or for key stroke
if(c == 27)
break; // if ESC, break and quit
if(videoWriter.isOpened())
{
cv::Mat left = data.imageRaw();
cv::Mat right = data.rightRaw();
if(left.size() == right.size())
{
cv::Size targetSize = left.size();
targetSize.width *= 2;
cv::Mat targetImage(targetSize, left.type());
if(right.type() != left.type())
{
cv::Mat tmp;
cv::cvtColor(right, tmp, left.channels()==3?CV_GRAY2BGR:CV_BGR2GRAY);
right = tmp;
}
UASSERT(left.type() == right.type());
cv::Mat roiA(targetImage, cv::Rect( 0, 0, left.size().width, left.size().height ));
left.copyTo(roiA);
cv::Mat roiB( targetImage, cvRect( left.size().width, 0, left.size().width, left.size().height ) );
right.copyTo(roiB);
videoWriter.write(targetImage);
printf("Saved frame %d to \"%s\"\n", id, stereoSavePath.c_str());
}
else
{
UERROR("Left and right images are not the same size!?");
}
}
else if(!stereoSavePath.empty())
{
cv::imwrite(stereoSavePath+"/"+"left/"+uNumber2Str(id) + ".jpg", data.imageRaw());
cv::imwrite(stereoSavePath+"/"+"right/"+uNumber2Str(id) + ".jpg", data.rightRaw());
printf("Saved frames %d to \"%s/left\" and \"%s/right\" directories\n", id, stereoSavePath.c_str(), stereoSavePath.c_str());
}
++id;
data = camera->takeImage();
}
printf("Closing...\n");
if(viewer)
{
delete viewer;
}
cv::destroyWindow("Video");
cv::destroyWindow("Depth");
delete camera;
return 0;
}
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;
}
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);
}
int main(int argc, char * argv[])
{
if(argc < 2)
{
showUsage();
}
ULogger::setType(ULogger::kTypeConsole);
ULogger::setLevel(ULogger::kDebug);
std::string dictionaryPath = argv[argc-1];
std::list<std::vector<float> > objectDescriptors;
//std::list<std::vector<float> > descriptors;
std::map<int, std::vector<float> > descriptors;
int dimension = 0;
UTimer timer;
int objectDescriptorsSize= 400;
std::ifstream file;
if(!dictionaryPath.empty())
{
file.open(dictionaryPath.c_str(), std::ifstream::in);
}
if(file.good())
{
UDEBUG("Loading the dictionary from \"%s\"", dictionaryPath.c_str());
// first line is the header
std::string str;
std::list<std::string> strList;
std::getline(file, str);
strList = uSplitNumChar(str);
for(std::list<std::string>::iterator iter = strList.begin(); iter != strList.end(); ++iter)
{
if(uIsDigit(iter->at(0)))
{
dimension = std::atoi(iter->c_str());
break;
}
}
if(dimension == 0 || dimension > 1000)
{
UERROR("Invalid dictionary file, visual word dimension (%d) is not valid, \"%s\"", dimension, dictionaryPath.c_str());
}
else
{
int descriptorsLoaded = 0;
// Process all words
while(file.good())
{
std::getline(file, str);
strList = uSplit(str);
if((int)strList.size() == dimension+1)
{
//first one is the visual word id
std::list<std::string>::iterator iter = strList.begin();
int id = atoi(iter->c_str());
++iter;
std::vector<float> descriptor(dimension);
int i=0;
//get descriptor
for(;i<dimension && iter != strList.end(); ++i, ++iter)
{
descriptor[i] = uStr2Float(*iter);
}
if(i != dimension)
{
UERROR("");
}
if(++descriptorsLoaded<=objectDescriptorsSize)
{
objectDescriptors.push_back(descriptor);
}
else
{
//descriptors.push_back(descriptor);
descriptors.insert(std::make_pair(id, descriptor));
}
}
else if(str.size())
{
UWARN("Cannot parse line \"%s\"", str.c_str());
}
}
}
UDEBUG("Time loading dictionary = %fs, dimension=%d", timer.ticks(), dimension);
}
else
{
UERROR("Cannot open dictionary file \"%s\"", dictionaryPath.c_str());
}
file.close();
if(descriptors.size() && objectDescriptors.size() && dimension)
{
cv::Mat dataTree;
cv::Mat queries;
UDEBUG("Creating data structures...");
// Create the data structure
dataTree = cv::Mat((int)descriptors.size(), dimension, CV_32F); // SURF descriptors are CV_32F
{//scope
//std::list<std::vector<float> >::const_iterator iter = descriptors.begin();
std::map<int, std::vector<float> >::const_iterator iter = descriptors.begin();
for(unsigned int i=0; i < descriptors.size(); ++i, ++iter)
{
UTimer tim;
//memcpy(dataTree.ptr<float>(i), iter->data(), dimension*sizeof(float));
memcpy(dataTree.ptr<float>(i), iter->second.data(), dimension*sizeof(float));
//if(i%100==0)
// UDEBUG("i=%d/%d tim=%fs", i, descriptors.size(), tim.ticks());
}
}
queries = cv::Mat((int)objectDescriptors.size(), dimension, CV_32F); // SURF descriptors are CV_32F
{//scope
std::list<std::vector<float> >::const_iterator iter = objectDescriptors.begin();
for(unsigned int i=0; i < objectDescriptors.size(); ++i, ++iter)
{
UTimer tim;
memcpy(queries.ptr<float>(i), iter->data(), dimension*sizeof(float));
//if(i%100==0)
// UDEBUG("i=%d/%d tim=%fs", i, objectDescriptors.size(), tim.ticks());
}
}
UDEBUG("descriptors.size()=%d, objectDescriptorsSize=%d, copying data = %f s",descriptors.size(), objectDescriptors.size(), timer.ticks());
UDEBUG("Creating indexes...");
cv::flann::Index * linearIndex = new cv::flann::Index(dataTree, cv::flann::LinearIndexParams());
UDEBUG("Time to create linearIndex = %f s", timer.ticks());
cv::flann::Index * kdTreeIndex1 = new cv::flann::Index(dataTree, cv::flann::KDTreeIndexParams(1));
UDEBUG("Time to create kdTreeIndex1 = %f s", timer.ticks());
cv::flann::Index * kdTreeIndex4 = new cv::flann::Index(dataTree, cv::flann::KDTreeIndexParams(4));
UDEBUG("Time to create kdTreeIndex4 = %f s", timer.ticks());
cv::flann::Index * kMeansIndex = new cv::flann::Index(dataTree, cv::flann::KMeansIndexParams());
UDEBUG("Time to create kMeansIndex = %f s", timer.ticks());
cv::flann::Index * compositeIndex = new cv::flann::Index(dataTree, cv::flann::CompositeIndexParams());
UDEBUG("Time to create compositeIndex = %f s", timer.ticks());
//cv::flann::Index * autoTunedIndex = new cv::flann::Index(dataTree, cv::flann::AutotunedIndexParams());
//UDEBUG("Time to create autoTunedIndex = %f s", timer.ticks());
UDEBUG("Search indexes...");
int k=2; // 2 nearest neighbors
cv::Mat results(queries.rows, k, CV_32SC1); // results index
cv::Mat dists(queries.rows, k, CV_32FC1); // Distance results are CV_32FC1
linearIndex->knnSearch(queries, results, dists, k);
//std::cout << results.t() << std::endl;
cv::Mat transposedLinear = dists.t();
UDEBUG("Time to search linearIndex = %f s", timer.ticks());
kdTreeIndex1->knnSearch(queries, results, dists, k);
//std::cout << results.t() << std::endl;
cv::Mat transposed = dists.t();
UDEBUG("Time to search kdTreeIndex1 = %f s (size=%d, dist error(k1,k2)=(%f,%f))",
timer.ticks(),
transposed.cols,
uMeanSquaredError( (float*)transposed.data,
transposed.cols,
(float*)transposedLinear.data,
transposedLinear.cols),
uMeanSquaredError( &transposed.at<float>(1,0),
transposed.cols,
&transposedLinear.at<float>(1,0),
transposedLinear.cols));
kdTreeIndex4->knnSearch(queries, results, dists, k);
//std::cout << results.t() << std::endl;
transposed = dists.t();
UDEBUG("Time to search kdTreeIndex4 = %f s (size=%d, dist error(k1,k2)=(%f,%f))",
timer.ticks(),
transposed.cols,
uMeanSquaredError( (float*)transposed.data,
transposed.cols,
(float*)transposedLinear.data,
transposedLinear.cols),
uMeanSquaredError( &transposed.at<float>(1,0),
transposed.cols,
&transposedLinear.at<float>(1,0),
transposedLinear.cols));
kMeansIndex->knnSearch(queries, results, dists, k);
//std::cout << results.t() << std::endl;
transposed = dists.t();
UDEBUG("Time to search kMeansIndex = %f s (size=%d, dist error(k1,k2)=(%f,%f))",
timer.ticks(),
transposed.cols,
uMeanSquaredError( (float*)transposed.data,
transposed.cols,
(float*)transposedLinear.data,
transposedLinear.cols),
uMeanSquaredError( &transposed.at<float>(1,0),
transposed.cols,
&transposedLinear.at<float>(1,0),
transposedLinear.cols));
compositeIndex->knnSearch(queries, results, dists, k);
//std::cout << results.t() << std::endl;
transposed = dists.t();
UDEBUG("Time to search compositeIndex = %f s (size=%d, dist error(k1,k2)=(%f,%f))",
timer.ticks(),
transposed.cols,
uMeanSquaredError( (float*)transposed.data,
transposed.cols,
(float*)transposedLinear.data,
transposedLinear.cols),
uMeanSquaredError( &transposed.at<float>(1,0),
transposed.cols,
&transposedLinear.at<float>(1,0),
transposedLinear.cols));
//autoTunedIndex->knnSearch(queries, results, dists, k);
//UDEBUG("Time to search autoTunedIndex = %f s", timer.ticks());
delete linearIndex;
delete kdTreeIndex1;
delete kdTreeIndex4;
delete kMeansIndex;
delete compositeIndex;
//delete autoTunedIndex;
}
return 0;
}
