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 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 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
void runInput(SlamSystem * system, DataSource *dataSource, Undistorter* undistorter )
{
// get HZ
float fps = dataSource->fps();
long int dt_us = (fps > 0) ? (1e6/fps) : 0;
long int dt_wiggle = 0;
const bool doDepth( system->conf().doDepth && dataSource->hasDepth() );
lsdReady.notify();
startAll.wait();
int numFrames = dataSource->numFrames();
LOG_IF( INFO, numFrames > 0 ) << "Running for " << numFrames << " frames";
cv::Mat image = cv::Mat(system->conf().slamImage.cvSize(), CV_8U);
int runningIdx=0;
float fakeTimeStamp = 0;
for(unsigned int i = 0; (numFrames < 0) || (i < numFrames); ++i)
{
if(lsdDone.getValue()) break;
std::chrono::time_point<std::chrono::steady_clock> start(std::chrono::steady_clock::now());
cv::Mat imageDist = cv::Mat( system->conf().inputImage.cvSize(), CV_8U);
if( dataSource->grab() ) {
dataSource->getImage( imageDist );
undistorter->undistort(imageDist, image);
CHECK(image.type() == CV_8U);
std::shared_ptr<Frame> frame( new Frame( runningIdx, system->conf(), fakeTimeStamp, image.data ));
if( doDepth ) {
cv::Mat depthOrig, depth;
dataSource->getDepth( depthOrig );
// Depth needs to be scaled as well...
undistorter->undistortDepth( depthOrig, depth );
CHECK(depth.type() == CV_32F );
CHECK( (depth.rows == image.rows) && (depth.cols == image.cols) );
frame->setDepthFromGroundTruth( depth.ptr<float>() );
}
if(runningIdx == 0)
{
if( doDepth )
system->gtDepthInit( frame );
else
system->randomInit( frame );
}
else
{
system->trackFrame( frame, fps == 0 );
}
if( gui ){
gui->pose.assignValue(system->getCurrentPoseEstimateScale());
gui->updateFrameNumber( runningIdx );
gui->updateLiveImage( image.data );
}
runningIdx++;
fakeTimeStamp += (fps > 0) ? (1.0/fps) : 0.03;
if(fullResetRequested)
{
SlamSystem *newSystem = new SlamSystem( system->conf() );
newSystem->set3DOutputWrapper( system->outputWrapper() );
LOG(WARNING) << "FULL RESET!";
delete system;
system = newSystem;
fullResetRequested = false;
runningIdx = 0;
}
} else {
if( system->conf().stopOnFailedRead ) break;
}
if( dt_us > 0 ) std::this_thread::sleep_until( start + std::chrono::microseconds( dt_us + dt_wiggle ) );
}
LOG(INFO) << "Have processed all input frames.";
lsdDone.assignValue(true);
}
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;
}
