void BenchmarkNode::runFromFolder()
{
for(int img_id = 2; img_id < 188; ++img_id)
{
// load image
std::stringstream ss;
ss << svo::test_utils::getDatasetDir() << "/sin2_tex2_h1_v8_d/img/frame_"
<< std::setw( 6 ) << std::setfill( '0' ) << img_id << "_0.png";
if(img_id == 2)
std::cout << "reading image " << ss.str() << std::endl;
cv::Mat img(cv::imread(ss.str().c_str(), 0));
assert(!img.empty());
// process frame
vo_->addImage(img, 0.01*img_id);
// display tracking quality
if(vo_->lastFrame() != NULL)
{
std::cout << "Frame-Id: " << vo_->lastFrame()->id_ << " \t"
<< "#Features: " << vo_->lastNumObservations() << " \t"
<< "Proc. Time: " << vo_->lastProcessingTime()*1000 << "ms \n";
// access the pose of the camera via vo_->lastFrame()->T_f_w_.
}
}
}
void BenchmarkNode::runFromFolder(int start)
{
ofstream outfile;
outfile.open ("/home/worxli/Datasets/data/associate_unscaled.txt");
// outfile.open ("/home/worxli/data/test/associate_unscaled.txt");
for(int img_id = start;;++img_id)
{
// load image
std::stringstream ss;
ss << "/home/worxli/Datasets/data/img/color" << img_id << ".png";
// ss << "/home/worxli/data/test/img/color" << img_id << ".png";
std::cout << "reading image " << ss.str() << std::endl;
cv::Mat img(cv::imread(ss.str().c_str(), 0));
// end loop if no images left
if(img.empty())
break;
assert(!img.empty());
// process frame
vo_->addImage(img, img_id);
// display tracking quality
if(vo_->lastFrame() != NULL)
{
int id = vo_->lastFrame()->id_;
std::cout << "Frame-Id: " << id << " \t"
<< "#Features: " << vo_->lastNumObservations() << " \t"
<< "Proc. Time: " << vo_->lastProcessingTime()*1000 << "ms \n";
// access the pose of the camera via vo_->lastFrame()->T_f_w_.
//std::cout << vo_->lastFrame()->T_f_w_ << endl;
//std::count << vo_->lastFrame()->pos() << endl;
Quaterniond quat = vo_->lastFrame()->T_f_w_.unit_quaternion();
Vector3d trans = vo_->lastFrame()->T_f_w_.translation();
outfile << trans.x() << " "
<< trans.y() << " "
<< trans.z() << " "
<< quat.x() << " "
<< quat.y() << " "
<< quat.z() << " "
<< quat.w() << " "
<< "depth/mapped" << img_id << ".png "
<< "img/color" << img_id << ".png\n";
}
}
outfile.close();
}
void BenchmarkNode::runFromFolder()
{
cv::line(*plot_, cv::Point(250,0), cv::Point(250,500), cv::Scalar(255,0,255));
cv::line(*plot_, cv::Point(0,250), cv::Point(500,250), cv::Scalar(255, 0, 255));
cv::Point origin = cv::Point(250,250);
cv::Point prevPoint1 = origin, curPoint1 = origin;
cv::Point prevPoint2 = origin, curPoint2 = origin;
for(int img_id = 2; img_id < 188; ++img_id)
{
// load image
std::stringstream ss;
ss << svo::test_utils::getDatasetDir() << "/sin2_tex2_h1_v8_d/img/frame_"
<< std::setw( 6 ) << std::setfill( '0' ) << img_id << "_0.png";
if(img_id == 2)
std::cout << "reading image " << ss.str() << std::endl;
cv::Mat img(cv::imread(ss.str().c_str(), 0));
assert(!img.empty());
// process frame
vo_->addImage(img, 0.01*img_id);
// display tracking quality
if(vo_->lastFrame() != NULL)
{
std::cout << "Frame-Id: " << vo_->lastFrame()->id_ << " \t"
<< "#Features: " << vo_->lastNumObservations() << " \t"
<< "Proc. Time: " << vo_->lastProcessingTime()*1000 << "ms \n";
// access the pose of the camera via vo_->lastFrame()->T_f_w_.
Vector3d pos = vo_->lastFrame()->T_f_w_.translation();
std::cout << "\tvo_->lastFrame()->pos() XPos: " << vo_->lastFrame()->pos().x() << " \t"
<< "YPos: " << vo_->lastFrame()->pos().y() << " \t"
<< "ZPos: " << vo_->lastFrame()->pos().z() << "\n";
std::cout << "\tlastFrame()->T_f_w.transation() XPos: " << pos.x() << " \t"
<< "YPos: " << pos.y() << " \t"
<< "ZPos: " << pos.z() << "\n";
curPoint2 = origin + cv::Point(pos.x() * 13, pos.y() * 13);
curPoint1 = origin + cv::Point(vo_->lastFrame()->pos().x() * 13, vo_->lastFrame()->pos().y() * 13);
cv::circle(*plot_, prevPoint1, 2, cv::Scalar(0,255,0));
cv::circle(*plot_, curPoint1, 2, cv::Scalar(0,255,0));
cv::circle(*plot_, prevPoint2, 2, cv::Scalar(0,0,255));
cv::circle(*plot_, curPoint2, 2, cv::Scalar(0,0,255));
// cv::line(*plot_, prevPoint1, curPoint1, cv::Scalar(0,255,0));
// cv::line(*plot_, prevPoint2, curPoint2, cv::Scalar(0,0,255));
prevPoint1 = curPoint1;
prevPoint2 = curPoint2;
cv::imshow("plot_", *plot_);
cv::waitKey(0);
}
}
}