/*
* This sample helps to evaluate odometry on TUM datasets and benchmark http://vision.in.tum.de/data/datasets/rgbd-dataset.
* At this link you can find instructions for evaluation. The sample runs some opencv odometry and saves a camera trajectory
* to file of format that the benchmark requires. Saved file can be used for online evaluation.
*/
int main(int argc, char** argv)
{
if (argc != 4)
{
cout << "Format: dataset_folder_path trajectory_file odometry_name [Rgbd or ICP or RgbdICP]" << endl;
return -1;
}
vector<string> timestamps;
vector<Mat> Rts;
const string dirname = argv[1];
tinydir_dir dir;
if (tinydir_open(&dir, dirname.c_str()) == -1)
{
perror("Error opening file");
return -1;
}
tinydir_close(&dir);
const string dirnameDepth = dirname + "/depth";
const string dirnameRgb = dirname + "/rgb";
tinydir_dir dirDepth, dirRgb;
tinydir_open(&dirDepth, dirnameDepth.c_str());
tinydir_open(&dirRgb, dirnameRgb.c_str());
const int timestampLength = 17;
const int rgbPathLehgth = 17+8;
const int depthPathLehgth = 17+10;
float fx = 525.0f, // default
fy = 525.0f,
cx = 319.5f,
cy = 239.5f;
if (dirname.find("freiburg1") != string::npos)
setCameraMatrixFreiburg1(fx, fy, cx, cy);
if (dirname.find("freiburg2") != string::npos)
setCameraMatrixFreiburg2(fx, fy, cx, cy);
Mat cameraMatrix = Mat::eye(3,3,CV_32FC1);
{
cameraMatrix.at<float>(0,0) = fx;
cameraMatrix.at<float>(1,1) = fy;
cameraMatrix.at<float>(0,2) = cx;
cameraMatrix.at<float>(1,2) = cy;
}
Ptr<OdometryFrame> frame_prev = Ptr<OdometryFrame>(new OdometryFrame()),
frame_curr = Ptr<OdometryFrame>(new OdometryFrame());
Ptr<Odometry> odometry = Odometry::create(string(argv[3]) + "Odometry");
if(odometry.empty())
{
cout << "Can not create Odometry algorithm. Check the passed odometry name." << endl;
return -1;
}
odometry->setCameraMatrix(cameraMatrix);
MyTickMeter gtm;
int count = 0;
while (dirDepth.has_next && dirRgb.has_next)
{
tinydir_file fileDepth;
tinydir_readfile(&dirDepth, &fileDepth);
tinydir_file fileRgb;
tinydir_readfile(&dirRgb, &fileRgb);
if (fileDepth.is_dir || fileRgb.is_dir)
{
tinydir_next(&dirDepth);
tinydir_next(&dirRgb);
continue;
}
string rgbFilename = fileRgb.path;
string timestap = fileRgb.name;
string depthFilename = fileDepth.path;
tinydir_next(&dirDepth);
tinydir_next(&dirRgb);
Mat image, depth;
// Read one pair (rgb and depth)
// example: 1305031453.359684 rgb/1305031453.359684.png 1305031453.374112 depth/1305031453.374112.png
#if BILATERAL_FILTER
MyTickMeter tm_bilateral_filter;
#endif
{
image = imread(rgbFilename);
depth = imread(depthFilename, -1);
CV_Assert(!image.empty());
CV_Assert(!depth.empty());
CV_Assert(depth.type() == CV_16UC1);
// scale depth
Mat depth_flt;
depth.convertTo(depth_flt, CV_32FC1, 1.f/5000.f);
#if !BILATERAL_FILTER
depth_flt.setTo(std::numeric_limits<float>::quiet_NaN(), depth == 0);
depth = depth_flt;
#else
tm_bilateral_filter.start();
depth = Mat(depth_flt.size(), CV_32FC1, Scalar(0));
const double depth_sigma = 0.03;
const double space_sigma = 4.5; // in pixels
Mat invalidDepthMask = depth_flt == 0.f;
depth_flt.setTo(-5*depth_sigma, invalidDepthMask);
bilateralFilter(depth_flt, depth, -1, depth_sigma, space_sigma);
depth.setTo(std::numeric_limits<float>::quiet_NaN(), invalidDepthMask);
tm_bilateral_filter.stop();
cout << "Time filter " << tm_bilateral_filter.getTimeSec() << endl;
#endif
timestamps.push_back( timestap );
}
{
Mat gray;
cvtColor(image, gray, COLOR_BGR2GRAY);
frame_curr->image = gray;
frame_curr->depth = depth;
imshow("gray", gray);
imshow("depth", depth);
char key = (char)waitKey(1);
Mat Rt;
if(!Rts.empty())
{
MyTickMeter tm;
tm.start();
gtm.start();
bool res = odometry->compute(frame_curr, frame_prev, Rt);
gtm.stop();
tm.stop();
count++;
cout << "Time " << tm.getTimeSec() << endl;
#if BILATERAL_FILTER
cout << "Time ratio " << tm_bilateral_filter.getTimeSec() / tm.getTimeSec() << endl;
#endif
if(!res)
Rt = Mat::eye(4,4,CV_64FC1);
}
if( Rts.empty() )
Rts.push_back(Mat::eye(4,4,CV_64FC1));
else
{
Mat& prevRt = *Rts.rbegin();
cout << "Rt " << Rt << endl;
Rts.push_back( prevRt * Rt );
}
if(!frame_prev.empty())
frame_prev->release();
std::swap(frame_prev, frame_curr);
}
}
std::cout << "Average time " << gtm.getTimeSec()/count << std::endl;
writeResults(argv[2], timestamps, Rts);
return 0;
}
/*
* This sample helps to evaluate odometry on TUM datasets and benchmark http://vision.in.tum.de/data/datasets/rgbd-dataset.
* At this link you can find instructions for evaluation. The sample runs some opencv odometry and saves a camera trajectory
* to file of format that the benchmark requires. Saved file can be used for online evaluation.
*/
int main(int argc, char** argv)
{
if(argc != 4)
{
cout << "Format: file_with_rgb_depth_pairs trajectory_file odometry_name [Rgbd or ICP or RgbdICP]" << endl;
return -1;
}
vector<string> timestamps;
vector<Mat> Rts;
const string filename = argv[1];
ifstream file( filename.c_str() );
if( !file.is_open() )
return -1;
char dlmrt = '/';
size_t pos = filename.rfind(dlmrt);
string dirname = pos == string::npos ? "" : filename.substr(0, pos) + dlmrt;
const int timestampLength = 17;
const int rgbPathLehgth = 17+8;
const int depthPathLehgth = 17+10;
float fx = 525.0f, // default
fy = 525.0f,
cx = 319.5f,
cy = 239.5f;
if(filename.find("freiburg1") != string::npos)
setCameraMatrixFreiburg1(fx, fy, cx, cy);
if(filename.find("freiburg2") != string::npos)
setCameraMatrixFreiburg2(fx, fy, cx, cy);
Mat cameraMatrix = Mat::eye(3,3,CV_32FC1);
{
cameraMatrix.at<float>(0,0) = fx;
cameraMatrix.at<float>(1,1) = fy;
cameraMatrix.at<float>(0,2) = cx;
cameraMatrix.at<float>(1,2) = cy;
}
Ptr<OdometryFrame> frame_prev = Ptr<OdometryFrame>(new OdometryFrame()),
frame_curr = Ptr<OdometryFrame>(new OdometryFrame());
Ptr<Odometry> odometry = Odometry::create("RGBD." + string(argv[3]) + "Odometry");
if(odometry.empty())
{
cout << "Can not create Odometry algorithm. Check the passed odometry name." << endl;
return -1;
}
odometry->setCameraMatrix(cameraMatrix);
MyTickMeter gtm;
int count = 0;
for(int i = 0; !file.eof(); i++)
{
string str;
std::getline(file, str);
if(str.empty()) break;
if(str.at(0) == '#') continue; /* comment */
Mat image, depth;
// Read one pair (rgb and depth)
// example: 1305031453.359684 rgb/1305031453.359684.png 1305031453.374112 depth/1305031453.374112.png
#if BILATERAL_FILTER
MyTickMeter tm_bilateral_filter;
#endif
{
string rgbFilename = str.substr(timestampLength + 1, rgbPathLehgth );
string timestap = str.substr(0, timestampLength);
string depthFilename = str.substr(2*timestampLength + rgbPathLehgth + 3, depthPathLehgth );
image = imread(dirname + rgbFilename);
depth = imread(dirname + depthFilename, -1);
CV_Assert(!image.empty());
CV_Assert(!depth.empty());
CV_Assert(depth.type() == CV_16UC1);
cout << i << " " << rgbFilename << " " << depthFilename << endl;
// scale depth
Mat depth_flt;
depth.convertTo(depth_flt, CV_32FC1, 1.f/5000.f);
#if !BILATERAL_FILTER
depth_flt.setTo(std::numeric_limits<float>::quiet_NaN(), depth == 0);
depth = depth_flt;
#else
tm_bilateral_filter.start();
depth = Mat(depth_flt.size(), CV_32FC1, Scalar(0));
const double depth_sigma = 0.03;
const double space_sigma = 4.5; // in pixels
Mat invalidDepthMask = depth_flt == 0.f;
depth_flt.setTo(-5*depth_sigma, invalidDepthMask);
bilateralFilter(depth_flt, depth, -1, depth_sigma, space_sigma);
depth.setTo(std::numeric_limits<float>::quiet_NaN(), invalidDepthMask);
tm_bilateral_filter.stop();
cout << "Time filter " << tm_bilateral_filter.getTimeSec() << endl;
#endif
timestamps.push_back( timestap );
}
{
Mat gray;
cvtColor(image, gray, COLOR_BGR2GRAY);
frame_curr->image = gray;
frame_curr->depth = depth;
Mat Rt;
if(!Rts.empty())
{
MyTickMeter tm;
tm.start();
gtm.start();
bool res = odometry->compute(frame_curr, frame_prev, Rt);
gtm.stop();
tm.stop();
count++;
cout << "Time " << tm.getTimeSec() << endl;
#if BILATERAL_FILTER
cout << "Time ratio " << tm_bilateral_filter.getTimeSec() / tm.getTimeSec() << endl;
#endif
if(!res)
Rt = Mat::eye(4,4,CV_64FC1);
}
if( Rts.empty() )
Rts.push_back(Mat::eye(4,4,CV_64FC1));
else
{
Mat& prevRt = *Rts.rbegin();
cout << "Rt " << Rt << endl;
Rts.push_back( prevRt * Rt );
}
if(!frame_prev.empty())
frame_prev->release();
std::swap(frame_prev, frame_curr);
}
}
std::cout << "Average time " << gtm.getTimeSec()/count << std::endl;
writeResults(argv[2], timestamps, Rts);
return 0;
}
