Skip to content

Commit

Permalink
viewer.cpp
Browse files Browse the repository at this point in the history
  • Loading branch information
@KYabuuchi
KYabuuchi committed Feb 28, 2019
1 parent 534c2f4 commit 7f879e2
Show file tree
Hide file tree
Showing 4 changed files with 118 additions and 90 deletions.
4 changes: 0 additions & 4 deletions include/map.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,16 +3,12 @@
#include <memory>
#include <opencv2/opencv.hpp>

namespace
{
const int PL = 0;
const int PR = 1;
const int CL = 2;
const int CR = 3;
const int P3 = 4;
const int C3 = 5;
} // namespace


class MapPoint
{
Expand Down
92 changes: 14 additions & 78 deletions include/viewer.hpp
View file
Original file line number Diff line number Diff line change
@@ -1,106 +1,42 @@
#pragma once
#include "map.hpp"
#include <array>
#include <condition_variable>
#include <memory>
#include <mutex>
#include <opencv2/opencv.hpp>
#include <thread>

//namespace
//{
//const int PL = 0;
//const int PR = 1;
//const int CL = 2;
//const int CR = 3;
//} // namespace

class Viewer
{
public:
Viewer()
: m_window_name("VIEW"),
m_thread(std::make_shared<std::thread>(&Viewer::drawLoop, this)),
m_reset_requested(false),
m_stop_requested(false),
m_updated(false) {}
Viewer();

~Viewer() { stop(); }
~Viewer();

void update(
const std::array<cv::Mat, 4>& images,
const std::vector<MapPointPtr>& mappoints)
{
std::lock_guard lock(m_mutex);
m_mappoints = mappoints;
m_images = images;
m_updated = true;
}
const std::vector<MapPointPtr>& mappoints);

void reset() { m_reset_requested = true; }
void stop() { m_stop_requested = true; }
void reset();
void stop();
int waitKeyEver();

private:
void drawLoop()
{
cv::namedWindow(m_window_name, cv::WINDOW_NORMAL);
cv::resizeWindow(m_window_name, 640, 480);

while (1) {
// 描画
if (m_updated) {
m_updated = false;

cv::Mat show, merge1, merge2;
cv::hconcat(m_images.at(PL), m_images.at(PR), merge1);
cv::hconcat(m_images.at(CL), m_images.at(CR), merge2);
cv::vconcat(merge1, merge2, show);

const cv::Size size = m_images.at(CL).size();
const cv::Point2f OFFSET_PL(0, 0);
const cv::Point2f OFFSET_PR(size.width, 0);
const cv::Point2f OFFSET_CL(0, size.height);
const cv::Point2f OFFSET_CR(size.width, size.height);

for (const MapPointPtr p : m_mappoints) {
if (p->enable(PL))
cv::circle(show, p->preLeft() + OFFSET_CL, 1, CV_RGB(255, 0, 0), 0, cv::LineTypes::LINE_AA);
if (p->enable(PR))
cv::circle(show, p->preRight() + OFFSET_PR, 1, CV_RGB(255, 0, 0), 0, cv::LineTypes::LINE_AA);
if (p->enable(CL))
cv::circle(show, p->curLeft() + OFFSET_CL, 1, CV_RGB(255, 0, 0), 0, cv::LineTypes::LINE_AA);
if (p->enable(CR))
cv::circle(show, p->curRight() + OFFSET_CL, 1, CV_RGB(255, 0, 0), 0, cv::LineTypes::LINE_AA);

if (p->triangulatable())
cv::line(show, p->curRight() + OFFSET_CL, p->curLeft() + OFFSET_CL, CV_RGB(255, 255, 0), 1, cv::LineTypes::LINE_AA);
if (p->motionEstimatable())
cv::line(show, p->preLeft() + OFFSET_CL, p->curLeft() + OFFSET_CL, CV_RGB(0, 255, 255), 1, cv::LineTypes::LINE_AA);
}
cv::imshow(m_window_name, show);
}

if (m_reset_requested) {
// TODO:
m_reset_requested = false;
}

if (m_stop_requested) {
break;
}

cv::waitKey(10);
}

std::cout << "viewer shut down " << std::endl;
}
void drawLoop();

std::array<cv::Mat, 4> m_images;
std::vector<MapPointPtr> m_mappoints;

const std::string m_window_name;

std::shared_ptr<std::thread> m_thread;
std::mutex m_mutex;
std::condition_variable m_cv;

bool m_reset_requested;
bool m_stop_requested;
bool m_updated;
bool m_update_called;

int m_last_key;
};
15 changes: 7 additions & 8 deletions main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,6 @@

int main()
{
// 初期化
Feature::init();
Viewer viewer;

Expand Down Expand Up @@ -118,17 +117,16 @@ int main()
}

// Epipolar Equation
cv::Mat T = calcPose(mappoints);
cv::Mat Tcw = calcPose(mappoints);

// 三角測量
int triangulatable_num = triangulate(mappoints);
std::cout << triangulatable_num << " points are used to triangulate" << std::endl;
triangulate(mappoints);

// スケールの計算
float scale = calcScale(mappoints, T.colRange(0, 3).rowRange(0, 3));
float scale = calcScale(mappoints, Tcw.colRange(0, 3).rowRange(0, 3));

std::cout << "\nTranslation " << scale << "\n"
<< T << "\n"
std::cout << "\nPose" << scale << "\n"
<< Tcw << "\n"
<< std::endl;

// 描画
Expand All @@ -142,7 +140,8 @@ int main()
pre_right_image = std::move(cur_right_image);

// wait
key = cv::waitKey(0);
key = viewer.waitKeyEver();
std::cout << "key: " << key << std::endl;
}

std::cout << "shut down" << std::endl;
Expand Down
97 changes: 97 additions & 0 deletions src/viewer.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,2 +1,99 @@
#include "viewer.hpp"
#include "params.hpp"
#include <chrono>

Viewer::Viewer()
: m_window_name("VIEW"),
m_thread(std::make_shared<std::thread>(&Viewer::drawLoop, this)),
m_reset_requested(false),
m_stop_requested(false),
m_update_called(false),
m_last_key(-1) {}

Viewer::~Viewer() { stop(); }

void Viewer::update(
const std::array<cv::Mat, 4>& images,
const std::vector<MapPointPtr>& mappoints)
{
std::lock_guard lock(m_mutex);
m_mappoints = mappoints;
m_images = images;
m_update_called = true;
}

void Viewer::reset() { m_reset_requested = true; }
void Viewer::stop() { m_stop_requested = true; }

int Viewer::waitKeyEver()
{
std::unique_lock lock(m_mutex);
m_cv.wait(lock, [this] { return (m_last_key != -1); });
return m_last_key;
}
void Viewer::drawLoop()
{
cv::namedWindow(m_window_name, cv::WINDOW_NORMAL);
cv::resizeWindow(m_window_name, 640, 480);

while (1) {

bool update = false;
{
std::lock_guard lock(m_mutex);
update = m_update_called;
m_update_called = false;
}

if (update) {
cv::Mat show, merge1, merge2;
cv::hconcat(m_images.at(PL), m_images.at(PR), merge1);
cv::hconcat(m_images.at(CL), m_images.at(CR), merge2);
cv::vconcat(merge1, merge2, show);

const cv::Size2f size = m_images.at(CL).size();
const cv::Point2f OFFSET_PL(0, 0);
const cv::Point2f OFFSET_PR(size.width, 0);
const cv::Point2f OFFSET_CL(0, size.height);
const cv::Point2f OFFSET_CR(size.width, size.height);

for (const MapPointPtr p : m_mappoints) {
if (p->enable(PL))
cv::circle(show, p->preLeft() + OFFSET_CL, 1, CV_RGB(255, 0, 0), 0, cv::LineTypes::LINE_AA);
if (p->enable(PR))
cv::circle(show, p->preRight() + OFFSET_PR, 1, CV_RGB(255, 0, 0), 0, cv::LineTypes::LINE_AA);
if (p->enable(CL))
cv::circle(show, p->curLeft() + OFFSET_CL, 1, CV_RGB(255, 0, 0), 0, cv::LineTypes::LINE_AA);
if (p->enable(CR))
cv::circle(show, p->curRight() + OFFSET_CL, 1, CV_RGB(255, 0, 0), 0, cv::LineTypes::LINE_AA);

if (p->triangulatable())
cv::line(show, p->curRight() + OFFSET_CL, p->curLeft() + OFFSET_CL, CV_RGB(255, 255, 0), 1, cv::LineTypes::LINE_AA);
if (p->motionEstimatable())
cv::line(show, p->preLeft() + OFFSET_CL, p->curLeft() + OFFSET_CL, CV_RGB(0, 255, 255), 1, cv::LineTypes::LINE_AA);
}
cv::imshow(m_window_name, show);
}

if (m_reset_requested) {
// TODO:
m_reset_requested = false;
}

if (m_stop_requested) {
break;
}

// key event
{
std::lock_guard lock(m_mutex);
m_last_key = cv::waitKey(5);
}

// wait for other thread action
m_cv.notify_all();
std::this_thread::sleep_for(std::chrono::milliseconds(10));
}

std::cout << "viewer shut down " << std::endl;
}

0 comments on commit 7f879e2

Please sign in to comment.