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 )
{
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;
}
void run(SlamSystem * system, Undistorter* undistorter, Output3DWrapper* outputWrapper, Sophus::Matrix3f K)
{
// get HZ
double hz = 30;
cv::Mat image = cv::Mat(h, w, CV_8U);
int runningIDX=0;
float fakeTimeStamp = 0;
for(unsigned int i = 0; i < numFrames; i++)
{
if(lsdDone.getValue())
break;
cv::Mat imageDist = cv::Mat(h, w, CV_8U);
if(logReader)
{
logReader->getNext();
cv::Mat3b img(h, w, (cv::Vec3b *)logReader->rgb);
cv::cvtColor(img, imageDist, CV_RGB2GRAY);
}
else
{
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);
}
gui.pose.assignValue(system->getCurrentPoseEstimateScale());
runningIDX++;
fakeTimeStamp+=0.03;
if(fullResetRequested)
{
printf("FULL RESET!\n");
delete system;
system = new SlamSystem(w, h, K, doSlam);
system->setVisualization(outputWrapper);
fullResetRequested = false;
runningIDX = 0;
}
}
lsdDone.assignValue(true);
}