Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
Extract Pose from LSD_SLAM library
// 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
You can’t perform that action at this time.