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") + "/"; //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 ) { 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; }