Created
August 31, 2017 22:55
-
-
Save kevin-george/68246120099bdbfc4449745e13751b69 to your computer and use it in GitHub Desktop.
Extract Pose from LSD_SLAM library
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
// This file is part of SVO - Semi-direct Visual Odometry. | |
// | |
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch> | |
// (Robotics and Perception Group, University of Zurich, Switzerland). | |
// | |
// SVO is free software: you can redistribute it and/or modify it under the | |
// terms of the GNU General Public License as published by the Free Software | |
// Foundation, either version 3 of the License, or any later version. | |
// | |
// SVO is distributed in the hope that it will be useful, but WITHOUT ANY | |
// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS | |
// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. | |
// | |
// You should have received a copy of the GNU General Public License | |
// along with this program. If not, see <http://www.gnu.org/licenses/>. | |
#include <svo/config.h> | |
#include <svo/frame_handler_mono.h> | |
#include <svo/map.h> | |
#include <svo/frame.h> | |
#include <vector> | |
#include <string> | |
#include <vikit/math_utils.h> | |
#include <vikit/vision.h> | |
#include <vikit/abstract_camera.h> | |
#include <vikit/atan_camera.h> | |
#include <vikit/pinhole_camera.h> | |
#include <opencv2/opencv.hpp> | |
#include <sophus/se3.h> | |
#include <iostream> | |
#include "test_utils.h" | |
namespace svo { | |
class BenchmarkNode | |
{ | |
vk::AbstractCamera* cam_; | |
svo::FrameHandlerMono* vo_; | |
public: | |
BenchmarkNode(); | |
~BenchmarkNode(); | |
void runFromFolder(); | |
}; | |
BenchmarkNode::BenchmarkNode() | |
{ | |
cam_ = new vk::PinholeCamera(752, 480, 315.5, 315.5, 376.0, 240.0); | |
vo_ = new svo::FrameHandlerMono(cam_); | |
vo_->start(); | |
} | |
BenchmarkNode::~BenchmarkNode() | |
{ | |
delete vo_; | |
delete cam_; | |
} | |
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_. | |
SE3 T_world_from_cam(T_world_from_vision_*frame->T_f_w_.inverse()); | |
q = Quaterniond(T_world_from_cam.rotation_matrix()*T_world_from_vision_.rotation_matrix().transpose()); | |
p = T_world_from_cam.translation(); | |
Cov = T_world_from_cam.Adj()*frame->Cov_*T_world_from_cam.inverse().Adj(); | |
std::cout << "X: " << pose.pose.position.x = p[0] << "\n"; | |
std::cout << "Y: " << pose.pose.position.y = p[1] << "\n"; | |
std::cout << "Z: " << pose.pose.position.z = p[2] << "\n"; | |
std::cout << "Pose X: " << pose.pose.orientation.x = q.x() << "\n"; | |
std::cout << "Pose Y: " << pose.pose.orientation.y = q.y() << "\n"; | |
std::cout << "Pose Z: " << pose.pose.orientation.z = q.z() << "\n"; | |
std::cout << "Poze W: " << pose.pose.orientation.w = q.w() << "\n"; | |
} | |
} | |
} | |
} // namespace svo | |
int main(int argc, char** argv) | |
{ | |
{ | |
svo::BenchmarkNode benchmark; | |
benchmark.runFromFolder(); | |
} | |
printf("BenchmarkNode finished.\n"); | |
return 0; | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment