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