Undistorter* Undistorter::getUndistorterForFile(const char* configFilename) { std::string completeFileName = configFilename; printf("Reading Calibration from file %s",completeFileName.c_str()); std::ifstream f(completeFileName.c_str()); if (!f.good()) { f.close(); completeFileName = packagePath+"calib/"+configFilename; printf(" ... not found!\n Trying %s", completeFileName.c_str()); f.open(completeFileName.c_str()); if (!f.good()) { printf(" ... not found. Cannot operate without calibration, shutting down.\n"); f.close(); return 0; } } printf(" ... found!\n"); std::string l1; std::getline(f,l1); f.close(); float ic[10]; if(std::sscanf(l1.c_str(), "%f %f %f %f %f %f %f %f", &ic[0], &ic[1], &ic[2], &ic[3], &ic[4], &ic[5], &ic[6], &ic[7]) == 8) { printf("found OpenCV camera model, building rectifier.\n"); Undistorter* u = new UndistorterOpenCV(completeFileName.c_str()); if(!u->isValid()) return 0; return u; } else { printf("found ATAN camera model, building rectifier.\n"); Undistorter* u = new UndistorterPTAM(completeFileName.c_str()); if(!u->isValid()) return 0; return u; } }
int main(int argc, char** argv) { //ros::init(argc, argv, "LSD_SLAM"); //dynamic_reconfigure::Server<lsd_slam_core::LSDParamsConfig> srv(ros::NodeHandle("~")); //srv.setCallback(dynConfCb); //dynamic_reconfigure::Server<lsd_slam_core::LSDDebugParamsConfig> srvDebug(ros::NodeHandle("~Debug")); //srvDebug.setCallback(dynConfCbDebug); //packagePath = ros::package::getPath("lsd_slam_core") + "/"; //std::string source_name = "d:/tmp/data/Car.mp4"; std::string source_name = "r:/2016.02.12_Iphone_recordings/IMG_2531.MOV"; std::string calibFile = "calib.cfg"; //std::string source_name = "D:/Projects/3drec/LSD_machine/images/%05d.png"; //std::string calibFile = "d:/Projects/3drec/LSD_machine/cameraCalibration.cfg"; // get camera calibration in form of an undistorter object. // if no undistortion is required, the undistorter will just pass images through. Undistorter* undistorter = 0; //if (ros::param::get("~calib", calibFile)) //{ undistorter = Undistorter::getUndistorterForFile(calibFile.c_str()); // ros::param::del("~calib"); //} if (undistorter == 0) { printf("need camera calibration file! (set using _calib:=FILE)\n"); exit(0); } int w = undistorter->getOutputWidth(); int h = undistorter->getOutputHeight(); int w_inp = undistorter->getInputWidth(); int h_inp = undistorter->getInputHeight(); float fx = undistorter->getK().at<double>(0, 0); float fy = undistorter->getK().at<double>(1, 1); float cx = undistorter->getK().at<double>(2, 0); float cy = undistorter->getK().at<double>(2, 1); Sophus::Matrix3f K; K << fx, 0.0, cx, 0.0, fy, cy, 0.0, 0.0, 1.0; // make output wrapper. just set to zero if no output is required. Output3DWrapper* outputWrapper = nullptr;// new ROSOutput3DWrapper(w, h); //Output3DWrapper* outputWrapper = new ROSOutput3DWrapper(w, h); // make slam system SlamSystem* system = new SlamSystem(w, h, K, doSlam); system->setVisualization(outputWrapper); cv::VideoCapture capture(source_name); // open image files: first try to open as file. //std::string source = "files.txt"; //std::vector<std::string> files; //if (!ros::param::get("~files", source)) //{ // printf("need source files! (set using _files:=FOLDER)\n"); // exit(0); //} //ros::param::del("~files"); // get HZ double hz = 29; //if (!ros::param::get("~hz", hz)) // hz = 0; //ros::param::del("~hz"); cv::Mat image = cv::Mat(h, w, CV_8U); int runningIDX = 0; float fakeTimeStamp = 0; //ros::Rate r(hz); cv::Mat input_img; cv::Mat imageDist;// = cv::imread(files[i], CV_LOAD_IMAGE_GRAYSCALE); //capture.set(CV_CAP_PROP_POS_MSEC, 15000); while (capture.read(input_img)) { //cv::Mat o_i = input_img(cv::Rect(0, 200, 1280, 416)); //cv::cvtColor(o_i, imageDist, CV_RGB2GRAY); cv::cvtColor(input_img, imageDist, CV_RGB2GRAY); //imageDist = input_img; if (imageDist.rows != h_inp || imageDist.cols != w_inp) { //if (imageDist.rows * imageDist.cols == 0) // printf("failed to load image %s! skipping.\n", files[i].c_str()); //else // printf("image %s has wrong dimensions - expecting %d x %d, found %d x %d. Skipping.\n", // files[i].c_str(), // w, h, imageDist.cols, imageDist.rows); continue; } assert(imageDist.type() == CV_8U); undistorter->undistort(imageDist, image); assert(image.type() == CV_8U); if (runningIDX == 0) system->randomInit(image.data, fakeTimeStamp, runningIDX); else system->trackFrame(image.data, runningIDX, true, fakeTimeStamp); lsd_slam::Frame * f = system->getCurrentKeyframe(); system->getCurrentKeyframe(); //for (int i = 0; i < 100; ++i) //system->trackFrame(image.data, runningIDX, true, fakeTimeStamp); runningIDX++; fakeTimeStamp += 0.03; //if (hz != 0) //r.sleep(); if (fullResetRequested) { printf("FULL RESET!\n"); delete system; system = new SlamSystem(w, h, K, doSlam); system->setVisualization(outputWrapper); fullResetRequested = false; runningIDX = 0; } //cv::imshow("o_i", o_i); cv::imshow("o_i", input_img); int key = cv::waitKey(10); if (key == 27) { fullResetRequested = true; //break; } //ros::spinOnce(); //if (!ros::ok()) //break; } system->finalize(); delete system; delete undistorter; delete outputWrapper; return 0; }
int main( int argc, char** argv ) { auto worker = g3::LogWorker::createLogWorker(); auto handle = worker->addDefaultLogger(argv[0], "."); auto stderrHandle = worker->addSink(std::unique_ptr<ColorStderrSink>( new ColorStderrSink ), &ColorStderrSink::ReceiveLogMessage); g3::initializeLogging(worker.get()); std::future<std::string> log_file_name = handle->call(&g3::FileSink::fileName); std::cout << "*\n* Log file: [" << log_file_name.get() << "]\n\n" << std::endl; LOG(INFO) << "Starting log."; DataSource *dataSource = NULL; Undistorter* undistorter = NULL; Configuration conf; bool doGui = true; try { TCLAP::CmdLine cmd("LSD", ' ', "0.1"); TCLAP::ValueArg<std::string> calibFileArg("c", "calib", "Calibration file", false, "", "Calibration filename", cmd ); TCLAP::ValueArg<std::string> resolutionArg("r", "resolution", "", false, "hd1080", "{hd2k, hd1080, hd720, vga}", cmd ); TCLAP::ValueArg<std::string> logFileArg("","log-input","Name of logger file to read",false,"","Logger filename", cmd); TCLAP::SwitchArg noGuiSwitch("","no-gui","Do not run GUI", cmd, false); TCLAP::ValueArg<int> fpsArg("", "fps","FPS", false, 0, "", cmd ); TCLAP::UnlabeledMultiArg<std::string> imageFilesArg("input-files","Name of image files / directories to read", false, "Files or directories", cmd ); cmd.parse(argc, argv ); { std::vector< std::string > imageFiles = imageFilesArg.getValue(); if( logFileArg.isSet() ) { dataSource = new LoggerSource( logFileArg.getValue() ); } else if ( imageFiles.size() > 0 && fs::path(imageFiles[0]).extension().string() == ".log" ) { dataSource = new LoggerSource( imageFiles[0] ); } else { dataSource = new ImagesSource( imageFiles ); } if( fpsArg.isSet() ) dataSource->setFPS( fpsArg.getValue() ); if( !calibFileArg.isSet() ) { LOG(WARNING) << "Must specify camera calibration!"; exit(-1); } undistorter = Undistorter::getUndistorterForFile(calibFileArg.getValue()); CHECK(undistorter != NULL); } doGui = !noGuiSwitch.getValue(); } catch (TCLAP::ArgException &e) // catch any exceptions { LOG(WARNING) << "error: " << e.error() << " for arg " << e.argId(); exit(-1); } CHECK( undistorter != NULL ) << "Undistorter doesn't exist."; CHECK( dataSource != NULL ) << "Data source doesn't exist."; conf.inputImage = undistorter->inputImageSize(); conf.slamImage = undistorter->outputImageSize(); conf.camera = undistorter->getCamera(); LOG(INFO) << "Slam image: " << conf.slamImage.width << " x " << conf.slamImage.height; CHECK( (conf.camera.fx) > 0 && (conf.camera.fy > 0) ) << "Camera focal length is zero"; SlamSystem * system = new SlamSystem(conf); if( doGui ) { LOG(INFO) << "Starting GUI thread"; boost::thread guiThread(runGui, system ); guiReady.wait(); } LOG(INFO) << "Starting input thread."; boost::thread inputThread(runInput, system, dataSource, undistorter ); lsdReady.wait(); // Wait for all threads to be ready. LOG(INFO) << "Starting all threads."; startAll.notify(); while(true) { if( (lsdDone.getValue() || guiDone.getValue()) && !system->finalized) { LOG(INFO) << "Finalizing system."; system->finalize(); } sleep(1); } if( system ) delete system; if( undistorter ) delete undistorter; return 0; }
int mainLoopCodeForQtThread() { // get camera calibration in form of an undistorter object. // if no undistortion is required, the undistorter will just pass images through. Undistorter* undistorter = 0; undistorter = Undistorter::getUndistorterForFile(settings->value(CALIB_PATH_KEY, CAMERA_CALIB_PATH_DEFAULT).toString().toLatin1()); if(undistorter == 0) { printf("need camera calibration file! (set using _calib:=FILE)\n"); exit(0); } int w = undistorter->getOutputWidth(); int h = undistorter->getOutputHeight(); int w_inp = undistorter->getInputWidth(); int h_inp = undistorter->getInputHeight(); float fx = undistorter->getK().at<double>(0, 0); float fy = undistorter->getK().at<double>(1, 1); float cx = undistorter->getK().at<double>(2, 0); float cy = undistorter->getK().at<double>(2, 1); Sophus::Matrix3f K; K << fx, 0.0, cx, 0.0, fy, cy, 0.0, 0.0, 1.0; // make output wrapper. just set to zero if no output is required. //Output3DWrapper* outputWrapper = NULL;// = new ROSOutput3DWrapper(w,h); Output3DWrapper* outputWrapper = new ROSOutput3DWrapper(w,h); // Set pointcloudviewer pointer in OutputWrapper outputWrapper->setViewer(viewer); outputWrapper->setViews(display1, display2, display3, display4); // make slam system SlamSystem* system = new SlamSystem(w, h, K, doSlam); system->setVisualization(outputWrapper); // open image files: first try to open as file. std::string source; std::vector<std::string> files; cv::Mat image = cv::Mat(h,w,CV_8U); int runningIDX=0; float fakeTimeStamp = 0; //Initialize video source // Use camera if (videoSource == VideoSource::Camera || videoSource == VideoSource::Drone){ cv::VideoCapture webcam(settings->value(CAMERA_NUMBER_KEY, CAMERA_NUMBER_DEFAULT).toInt()); //cv::VideoCapture webcam(0); qDebug() << "camera opened: " << webcam.isOpened(); if(videoSource==VideoSource::Camera) { if(!webcam.isOpened()) { qDebug() << "camera could not be opened!!"; printf("Error: cannot open stream from webcam\n"); return -1; } webcam.set(CV_CAP_PROP_FRAME_WIDTH, w_inp); webcam.set(CV_CAP_PROP_FRAME_HEIGHT, h_inp); webcam.set(CV_CAP_PROP_FPS, 30.0); } while(true) { if (!startLSDSlam){ qDebug() << "checkpoint2 "; QThread::msleep(100); continue; } qDebug() << "frame capture"; cv::Mat frame; if(videoSource == VideoSource::Camera){ webcam >> frame; //cv::imshow("test", frame); //qDebug() << "ok: frame captured"; // } else { //@TODO add sleep or change method //#ifdef USE_DRONE // frame = droneWindow->getImage(); //non-blocking call //#endif } if (quit_signal) break; // exit cleanly on interrupt
int main( int argc, char** argv ) { // get camera calibration in form of an undistorter object. // if no undistortion is required, the undistorter will just pass images through. std::string calibFile; Undistorter* undistorter = 0; if(Parse::arg(argc, argv, "-c", calibFile) > 0) { undistorter = Undistorter::getUndistorterForFile(calibFile.c_str()); } if(undistorter == 0) { printf("need camera calibration file! (set using -c FILE)\n"); exit(0); } w = undistorter->getOutputWidth(); h = undistorter->getOutputHeight(); w_inp = undistorter->getInputWidth(); h_inp = undistorter->getInputHeight(); float fx = undistorter->getK().at<double>(0, 0); float fy = undistorter->getK().at<double>(1, 1); float cx = undistorter->getK().at<double>(2, 0); float cy = undistorter->getK().at<double>(2, 1); Sophus::Matrix3f K; K << fx, 0.0, cx, 0.0, fy, cy, 0.0, 0.0, 1.0; Resolution::getInstance(w, h); Intrinsics::getInstance(fx, fy, cx, cy); gui.initImages(); Output3DWrapper* outputWrapper = new PangolinOutput3DWrapper(w, h, gui); // make slam system SlamSystem * system = new SlamSystem(w, h, K, doSlam); system->setVisualization(outputWrapper); // open image files: first try to open as file. std::string source; if(!(Parse::arg(argc, argv, "-f", source) > 0)) { printf("need source files! (set using -f FOLDER or KLG)\n"); exit(0); } Bytef * decompressionBuffer = new Bytef[Resolution::getInstance().numPixels() * 2]; IplImage * deCompImage = 0; if(source.substr(source.find_last_of(".") + 1) == "klg") { logReader = new RawLogReader(decompressionBuffer, deCompImage, source); numFrames = logReader->getNumFrames(); } else { if(getdir(source, files) >= 0) { printf("found %d image files in folder %s!\n", (int)files.size(), source.c_str()); } else if(getFile(source, files) >= 0) { printf("found %d image files in file %s!\n", (int)files.size(), source.c_str()); } else { printf("could not load file list! wrong path / file?\n"); } numFrames = (int)files.size(); } boost::thread lsdThread(run, system, undistorter, outputWrapper, K); while(!pangolin::ShouldQuit()) { if(lsdDone.getValue() && !system->finalized) { system->finalize(); } gui.preCall(); gui.drawKeyframes(); gui.drawFrustum(); gui.drawImages(); gui.postCall(); } lsdDone.assignValue(true); lsdThread.join(); delete system; delete undistorter; delete outputWrapper; return 0; }
int main( int argc, char** argv ) { ros::init(argc, argv, "LSD_SLAM"); dynamic_reconfigure::Server<lsd_slam_core::LSDParamsConfig> srv(ros::NodeHandle("~")); srv.setCallback(dynConfCb); dynamic_reconfigure::Server<lsd_slam_core::LSDDebugParamsConfig> srvDebug(ros::NodeHandle("~Debug")); srvDebug.setCallback(dynConfCbDebug); packagePath = ros::package::getPath("lsd_slam_core")+"/"; // Load Camera Id [*Added*] int camId; ros::param::get("~camId",camId); std::cout<<"Camera Id = "<< camId << std::endl; // get camera calibration in form of an undistorter object. // if no undistortion is required, the undistorter will just pass images through. std::string calibFile; Undistorter* undistorter = 0; if(ros::param::get("~calib", calibFile)) { undistorter = Undistorter::getUndistorterForFile(calibFile.c_str()); ros::param::del("~calib"); } if(undistorter == 0) { printf("need camera calibration file! (set using _calib:=FILE)\n"); exit(0); } int w = undistorter->getOutputWidth(); int h = undistorter->getOutputHeight(); int w_inp = undistorter->getInputWidth(); int h_inp = undistorter->getInputHeight(); float fx = undistorter->getK().at<double>(0, 0); float fy = undistorter->getK().at<double>(1, 1); float cx = undistorter->getK().at<double>(2, 0); float cy = undistorter->getK().at<double>(2, 1); Sophus::Matrix3f K; K << fx, 0.0, cx, 0.0, fy, cy, 0.0, 0.0, 1.0; // make output wrapper. just set to zero if no output is required. Output3DWrapper* outputWrapper = new ROSOutput3DWrapper(w,h); // make slam system SlamSystem* system = new SlamSystem(w, h, K, doSlam); system->setVisualization(outputWrapper); // open image files: first try to open as file. std::string source; std::vector<std::string> files; if(!ros::param::get("~files", source)) { printf("need source files! (set using _files:=FOLDER)\n"); exit(0); } ros::param::del("~files"); if(getdir(source, files) >= 0) { printf("found %d image files in folder %s!\n", (int)files.size(), source.c_str()); } else if(getFile(source, files) >= 0) { printf("found %d image files in file %s!\n", (int)files.size(), source.c_str()); } else { printf("could not load file list! wrong path / file?\n"); } // get HZ double hz = 0; if(!ros::param::get("~hz", hz)) hz = 0; ros::param::del("~hz"); cv::Mat image = cv::Mat(h,w,CV_8U); int runningIDX=0; float fakeTimeStamp = 0; ros::Rate r(hz); for(unsigned int i=0;i<files.size();i++) { cv::Mat imageDist = cv::imread(files[i], CV_LOAD_IMAGE_GRAYSCALE); if(imageDist.rows != h_inp || imageDist.cols != w_inp) { if(imageDist.rows * imageDist.cols == 0) printf("failed to load image %s! skipping.\n", files[i].c_str()); else printf("image %s has wrong dimensions - expecting %d x %d, found %d x %d. Skipping.\n", files[i].c_str(), w,h,imageDist.cols, imageDist.rows); continue; } assert(imageDist.type() == CV_8U); undistorter->undistort(imageDist, image); assert(image.type() == CV_8U); if(runningIDX == 0) system->randomInit(image.data, fakeTimeStamp, runningIDX); else system->trackFrame(image.data, runningIDX ,hz == 0,fakeTimeStamp); runningIDX++; fakeTimeStamp+=0.03; if(hz != 0) r.sleep(); if(fullResetRequested) { printf("FULL RESET!\n"); delete system; system = new SlamSystem(w, h, K, doSlam); system->setVisualization(outputWrapper); fullResetRequested = false; runningIDX = 0; } ros::spinOnce(); if(!ros::ok()) break; } system->finalize(); delete system; delete undistorter; delete outputWrapper; return 0; }
int main( int argc, char** argv ) { ros::init(argc, argv, "LSD_SLAM"); dynamic_reconfigure::Server<lsd_slam_core::LSDParamsConfig> srv(ros::NodeHandle("~")); srv.setCallback(dynConfCb); dynamic_reconfigure::Server<lsd_slam_core::LSDDebugParamsConfig> srvDebug(ros::NodeHandle("~Debug")); srvDebug.setCallback(dynConfCbDebug); packagePath = ros::package::getPath("lsd_slam_core")+"/"; // get camera calibration in form of an undistorter object. // if no undistortion is required, the undistorter will just pass images through. std::string calibFile; Undistorter* undistorter = 0; if(ros::param::get("~calib", calibFile)) { undistorter = Undistorter::getUndistorterForFile(calibFile.c_str()); ros::param::del("~calib"); } if(undistorter == 0) { printf("need camera calibration file! (set using _calib:=FILE)\n"); exit(0); } int w = undistorter->getOutputWidth(); int h = undistorter->getOutputHeight(); int w_inp = undistorter->getInputWidth(); int h_inp = undistorter->getInputHeight(); float fx = undistorter->getK().at<double>(0, 0); float fy = undistorter->getK().at<double>(1, 1); float cx = undistorter->getK().at<double>(2, 0); float cy = undistorter->getK().at<double>(2, 1); Sophus::Matrix3f K; K << fx, 0.0, cx, 0.0, fy, cy, 0.0, 0.0, 1.0; // make output wrapper. just set to zero if no output is required. Output3DWrapper* outputWrapper = new ROSOutput3DWrapper(w,h); // make slam system SlamSystem* system = new SlamSystem(w, h, K, doSlam); system->setVisualization(outputWrapper); // open image files: first try to open as file. std::string source; std::vector<std::string> files; if(!ros::param::get("~files", source)) { printf("need source files! (set using _files:=FOLDER)\n"); exit(0); } ros::param::del("~files"); int save_K = 0; if(!ros::param::get("~save_K", save_K)) save_K = 0; ros::param::del("~save_K"); int only_K = 0; if(!ros::param::get("~only_K", only_K)) only_K = 0; ros::param::del("~only_K"); if (save_K == 1 || only_K == 1){ std::stringstream file_path; file_path << source.c_str() << "K.txt"; std::ofstream kfile; kfile.open(file_path.str(), std::ios::out | std::ios::trunc); if (kfile.is_open()){ std::cout << "Writing K to " << file_path.str() << std::endl; kfile << fx << "," << cx << "," << fy << "," << cy << "," << std::endl; } kfile.close(); if (only_K == 1){ delete system; delete undistorter; delete outputWrapper; return 0; } } if(getdir(source, files) >= 0) { printf("found %d image files in folder %s!\n", (int)files.size(), source.c_str()); } else if(getFile(source, files) >= 0) { printf("found %d image files in file %s!\n", (int)files.size(), source.c_str()); } else { printf("could not load file list! wrong path / file?\n"); } // get HZ double hz = 0; if(!ros::param::get("~hz", hz)) hz = 0; ros::param::del("~hz"); // get undistorted desired behaviour int save_undist = 0; if(!ros::param::get("~save_undistorted", save_undist)) save_undist = 0; ros::param::del("~save_undistorted"); if (save_undist == 1){ std::stringstream dir_path; dir_path << source.c_str() << "undistorted"; boost::filesystem::path dir(dir_path.str()); if(boost::filesystem::create_directory(dir)) { printf("Created undistorted directory successfully.\n"); } } // get color desired behaviour int save_ucolour = 0; if(!ros::param::get("~save_ucolour", save_ucolour)) save_ucolour = 0; ros::param::del("~save_ucolour"); if (save_ucolour == 1){ std::stringstream dir_path; dir_path << source.c_str() << "undist-colour"; boost::filesystem::path dir(dir_path.str()); if(boost::filesystem::create_directory(dir)) { printf("Created colour directory successfully.\n"); } } cv::Mat image = cv::Mat(h,w,CV_8U); cv::Mat c_image; int runningIDX=0; float fakeTimeStamp = 0; ros::Rate r(hz); for(unsigned int i=0;i<files.size();i++) { cv::Mat imageDist; imageDist = cv::imread(files[i], CV_LOAD_IMAGE_GRAYSCALE); if(imageDist.rows != h_inp || imageDist.cols != w_inp) { if(imageDist.rows * imageDist.cols == 0) printf("failed to load image %s! skipping.\n", files[i].c_str()); else printf("image %s has wrong dimensions - expecting %d x %d, found %d x %d. Skipping.\n", files[i].c_str(), w,h,imageDist.cols, imageDist.rows); continue; } assert(imageDist.type() == CV_8U); undistorter->undistort(imageDist, image); assert(image.type() == CV_8U); if (save_undist == 1){ std::stringstream file; file << source.c_str() << "undistorted/undist_" << i << ".png"; cv::imwrite(file.str(), image); } if (save_ucolour == 1){ cv::Mat colorMat = cv::imread(files[i], CV_LOAD_IMAGE_COLOR); undistorter->undistort(colorMat, c_image); std::stringstream file; file << source.c_str() << "undist-colour/ucolour_" << boost::format("%|06|")%i << ".png"; cv::imwrite(file.str(), c_image); } if(runningIDX == 0) system->randomInit(image.data, fakeTimeStamp, runningIDX); else system->trackFrame(image.data, runningIDX ,hz == 0,fakeTimeStamp); runningIDX++; fakeTimeStamp+=0.03; if(hz != 0) r.sleep(); if(fullResetRequested) { printf("FULL RESET!\n"); delete system; system = new SlamSystem(w, h, K, doSlam); system->setVisualization(outputWrapper); fullResetRequested = false; runningIDX = 0; } ros::spinOnce(); if(!ros::ok()) break; } system->finalize(); delete system; delete undistorter; delete outputWrapper; return 0; }