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; }