OpenCV 3.1のsfmをVS2015とopenFrameworksで動かしてみる(その1) -vizを使わないビューアの作成

(編集中)
OpenCV3.1でSfm(ストラクチャーフロムモーション)がモジュールとして採用されたそうです。
いつものようにWindows環境で適当に動くところまでやってみようと思い立ったところ、素晴らしい先行事例の記事がありました。VisualStudio2013で動いたそうです。とても心強いですね。この記事では、VisualStudio2015でやろうとした時の差分をまとめていこうと思います。
qiita.com

Ceres Solverのビルド

最初にgoogleの非線形最適化ライブラリCeres Solverをビルドします。最初に必要なソースコードを集めます。ダウンロード後、解凍してフォルダ名を修正してください。
1.Ceres SolverのVisual Studio用プロジェクト。
GitHub - tbennun/ceres-windows: An easy to use ceres-solver port for Visual Studio on Windows, using Eigen for sparse linear algebra and Google glog for logging.
フォルダ名: ceres-windows
2.Ceres Solverのコア部分のソースコード。
http://ceres-solver.org/ceres-solver-1.11.0.tar.gz
フォルダ名: ceres-solver
3.GLogの最新版。
Google Code Archive - Long-term storage for Google Code Project Hosting.
フォルダ名: glog
4.gflagsの最新版。
https://github.com/gflags/gflags
※単体でビルドしておいた方が楽。
5.Eigen3の最新版。
Eigen
フォルダ名: Eigen

1のフォルダの下に2,3,4,5を移動します。
ceres-windows
-ceres-solver
-Eigen
-glog
-gflags

GLog

slnファイルを開いて、プロジェクトのアップデートをダイアログ通りに掛けて進めます。

次に、logging.ccとlogging.hのソースコードの修正をします。
logging.ccの冒頭部分に一行追加する。

#include <algorithm>

ソースコード半ばにあるminの定義を書き換える。

      // Store shortened fatal message for other logs and GWQ status
      //const int copy = min<int>(data_->num_chars_to_log_,
      //                          sizeof(fatal_message)-1);
      const int copy = std::min<int>(data_->num_chars_to_log_,
	  sizeof(fatal_message) - 1);

まだ先行記事には紹介されていないエラーが出ます。GLog+Visual Studioのキーワードで検索すると、解決記事が見つかります。
c++ - glog on visual studio 2015 - Stack Overflow

logging.hのclass GOOGLE_GLOG_DLL_DECL LogMessageの中に2行追加します。

  public:
    LogStream(char *buf, int len, int ctr)
        : std::ostream(NULL),
          streambuf_(buf, len),
          ctr_(ctr),
          self_(this) {
      rdbuf(&streambuf_);
    }

    LogStream(const LogStream&) = delete;  // 追加
    LogStream& operator=(const LogStream&) = delete; // 追加

ceres-windows

ceres-2012.slnを開いてバッチビルドで全ての条件にチェックを入れてビルドします。
x64のDebugでエラーがでてコンパイルが最後まで通りませんが、とりあえず置いておきます。

CeresはNew BSD licenseのようです。

OpenCV3.1 sfmモジュール

モジュールの解説はこちらにあります。
OpenCV: Structure From Motion
OpenCV3.1のチュートリアルに解説があります。OpenCV: Structure From Motion
opencv_contribのソースコードはこちらからダウンロードします。
github.com

（編集中、先行事例に倣ってごにょごにょしてると、libが揃います。）

vizが使いにくいとのことなので、この部分をopenFrameworksに移植します。

ofApp.h

#pragma once

#include "ofMain.h"
#include "opencv2/core/affine.hpp"

class ofApp : public ofBaseApp{

	public:
		void setup();
		void update();
		void draw();

		void keyPressed(int key);
		void keyReleased(int key);
		void mouseMoved(int x, int y );
		void mouseDragged(int x, int y, int button);
		void mousePressed(int x, int y, int button);
		void mouseReleased(int x, int y, int button);
		void mouseEntered(int x, int y);
		void mouseExited(int x, int y);
		void windowResized(int w, int h);
		void dragEvent(ofDragInfo dragInfo);
		void gotMessage(ofMessage msg);
		
	public:
		void drawFrustum(float _f, float _cx, float _cy, float clip_near, float clip_far);
		string tracks_filename;
		double f, cx, cy;

	protected:
		bool camera_pov;
		bool is_projective;
		std::vector<cv::Mat> Rs_est, ts_est, points3d_estimated;
		std::vector<cv::Vec3f> point_cloud_est;
		std::vector<cv::Affine3d> path_est;
		int idx, forw, n;

		ofEasyCam cam;
		ofVec3f last_cam_pos, last_cam_dir, last_cam_up;
		float last_fov;
};

ofApp.cpp

#include "ofApp.h"
#include "opencv_lib.hpp"

#define CERES_FOUND 1

#include <opencv2/core.hpp>
#include <opencv2/sfm.hpp>
//#include <opencv2/viz.hpp>
#include <opencv2/core/affine.hpp>

#include <iostream>
#include <fstream>
#include <string>

#ifdef _DEBUG
#pragma comment(lib, "ceres_d.lib")
#pragma comment(lib, "libglog_d.lib")
#pragma comment(lib, "gflags_d.lib")
#pragma comment(lib, "correspondence_d.lib")
#pragma comment(lib, "multiview_d.lib")
#pragma comment(lib, "numeric_d.lib")
#pragma comment(lib, "simple_pipeline_d.lib")
#else
#pragma comment(lib, "ceres.lib")
#pragma comment(lib, "libglog.lib")
#pragma comment(lib, "gflags.lib")
#pragma comment(lib, "correspondence.lib")
#pragma comment(lib, "multiview.lib")
#pragma comment(lib, "numeric.lib")
#pragma comment(lib, "simple_pipeline.lib")
#endif

static void help() {
	cout
		<< "\n------------------------------------------------------------------\n"
		<< " This program shows the camera trajectory reconstruction capabilities\n"
		<< " in the OpenCV Structure From Motion (SFM) module.\n"
		<< " \n"
		<< " Usage:\n"
		<< "        example_sfm_trajectory_reconstruction <path_to_tracks_file> <f> <cx> <cy>\n"
		<< " where: is the tracks file absolute path into your system. \n"
		<< " \n"
		<< "        The file must have the following format: \n"
		<< "        row1 : x1 y1 x2 y2 ... x36 y36 for track 1\n"
		<< "        row2 : x1 y1 x2 y2 ... x36 y36 for track 2\n"
		<< "        etc\n"
		<< " \n"
		<< "        i.e. a row gives the 2D measured position of a point as it is tracked\n"
		<< "        through frames 1 to 36.  If there is no match found in a view then x\n"
		<< "        and y are -1.\n"
		<< " \n"
		<< "        Each row corresponds to a different point.\n"
		<< " \n"
		<< "        f  is the focal lenght in pixels. \n"
		<< "        cx is the image principal point x coordinates in pixels. \n"
		<< "        cy is the image principal point y coordinates in pixels. \n"
		<< "------------------------------------------------------------------\n\n"
		<< endl;
}

/* Build the following structure data
*
*            frame1           frame2           frameN
*  track1 | (x11,y11) | -> | (x12,y12) | -> | (x1N,y1N) |
*  track2 | (x21,y11) | -> | (x22,y22) | -> | (x2N,y2N) |
*  trackN | (xN1,yN1) | -> | (xN2,yN2) | -> | (xNN,yNN) |
*
*
*  In case a marker (x,y) does not appear in a frame its
*  values will be (-1,-1).
*/


void
parser_2D_tracks(const string &_filename, std::vector<cv::Mat> &points2d)
{
	ifstream myfile(_filename.c_str());

	if (!myfile.is_open())
	{
		cout << "Unable to read file: " << _filename << endl;
		exit(0);

	}
	else {

		double x, y;
		string line_str;
		int n_frames = 0, n_tracks = 0;

		// extract data from text file

		vector<vector<cv::Vec2d> > tracks;
		for (; getline(myfile, line_str); ++n_tracks)
		{
			istringstream line(line_str);

			vector<cv::Vec2d> track;
			for (n_frames = 0; line >> x >> y; ++n_frames)
			{
				if (x > 0 && y > 0)
					track.push_back(cv::Vec2d(x, y));
				else
					track.push_back(cv::Vec2d(-1));
			}
			tracks.push_back(track);
		}

		// embed data in reconstruction api format

		for (int i = 0; i < n_frames; ++i)
		{
			cv::Mat_<double> frame(2, n_tracks);

			for (int j = 0; j < n_tracks; ++j)
			{
				frame(0, j) = tracks[j][i][0];
				frame(1, j) = tracks[j][i][1];
			}
			points2d.push_back(cv::Mat(frame));
		}

		myfile.close();
	}

}


//--------------------------------------------------------------
void ofApp::setup(){
	camera_pov = false;

	// Read input parameters
	
	// Read 2D points from text file
	tracks_filename = "data/desktop_tracks.txt";
	f = 1914.0;
	cx = 640;
	cy = 360;
	std::vector<cv::Mat> points2d;
	parser_2D_tracks(tracks_filename, points2d);

	// Set the camera calibration matrix
	cv::Matx33d K = cv::Matx33d(f, 0, cx,
								0, f, cy,
								0, 0, 1);

	/// Reconstruct the scene using the 2d correspondences

	is_projective = true;
	cv::sfm::reconstruct(points2d, Rs_est, ts_est, K, points3d_estimated, is_projective);

	// Print output

	cout << "\n----------------------------\n" << endl;
	cout << "Reconstruction: " << endl;
	cout << "============================" << endl;
	cout << "Estimated 3D points: " << points3d_estimated.size() << endl;
	cout << "Estimated cameras: " << Rs_est.size() << endl;
	cout << "Refined intrinsics: " << endl << K << endl << endl;

	cout << "3D Visualization: " << endl;
	cout << "============================" << endl;

	/// Create 3D windows
	ofSetWindowTitle("Estimation Coordinate Frame");
	ofSetBackgroundColor(ofColor::black);

	// Create the pointcloud
	cout << "Recovering points  ... ";

	// recover estimated points3d
	for (int i = 0; i < points3d_estimated.size(); ++i)
		point_cloud_est.push_back(cv::Vec3f(points3d_estimated[i]));

	cout << "[DONE]" << endl;

	/// Recovering cameras
	cout << "Recovering cameras ... ";

	for (size_t i = 0; i < Rs_est.size(); ++i)
		path_est.push_back(cv::Affine3d(Rs_est[i], ts_est[i]));

	cout << "[DONE]" << endl;

	/// Add cameras
	cout << "Rendering Trajectory  ... ";

	/// Wait for key 'q' to close the window
	cout << endl << "Press:                       " << endl;
	cout << " 's' to switch the camera pov" << endl;
	cout << " 'q' to close the windows    " << endl;

	if (path_est.size() > 0)
	{
		// animated trajectory
		idx = 0;
		forw = -1;
		n = static_cast<int>(path_est.size());
	}

	cam.setNearClip(0.001f);
	cam.setDistance(10.0f);
}

//--------------------------------------------------------------
void ofApp::update(){

}

void ofApp::drawFrustum(float _f, float _cx, float _cy, float clip_near, float clip_far)
{
	float aw = 2 * atan2(_cx, 2 * _f);
	float ah = 2 * atan2(_cy, 2 * _f);
	
	float px0 = clip_near * tan(aw);
	float px1 = clip_far * tan(aw);
	float py0 = clip_near * tan(ah);
	float py1 = clip_far * tan(ah);

	ofDrawLine(px0, py0, clip_near, px1, py1, clip_far);
	ofDrawLine(-px0, py0, clip_near,  -px1, py1, clip_far);
	ofDrawLine(-px0, -py0, clip_near, -px1, -py1, clip_far);
	ofDrawLine(px0, -py0, clip_near, px1, -py1, clip_far);

	ofDrawLine(px0, py0, clip_near, -px0, py0, clip_near);
	ofDrawLine(-px0, py0, clip_near, -px0, -py0, clip_near);
	ofDrawLine(-px0, -py0, clip_near, px0, -py0, clip_near);
	ofDrawLine(px0, -py0, clip_near, px0, py0, clip_near);

	ofDrawLine(px1, py1, clip_far, -px1, py1, clip_far);
	ofDrawLine(-px1, py1, clip_far, -px1, -py1, clip_far);
	ofDrawLine(-px1, -py1, clip_far, px1, -py1, clip_far);
	ofDrawLine(px1, -py1, clip_far, px1, py1, clip_far);
}

//--------------------------------------------------------------
void ofApp::draw(){
	if (path_est.size() > 0)
	{
		cam.begin();
		ofPushStyle();
		/// Render points as 3D cubes
		for (size_t i = 0; i < point_cloud_est.size(); ++i)
		{
			cv::Vec3d point = point_cloud_est[i];
			cv::Affine3d point_pose(cv::Mat::eye(3, 3, CV_64F), point);
			
			char buffer[50];
			sprintf(buffer, "%d", static_cast<int>(i));

			ofBoxPrimitive box;
			ofSetLineWidth(2.0f);
			ofSetColor(ofColor::blue);
			box.set(0.1, 0.1, -0.1);
			box.setPosition(point[0], point[1], point[2]);
			box.drawWireframe();	
		}
		ofPopStyle();
		cam.end();

		cv::Affine3d cam_pose = path_est[idx];
		
                cv::Matx44d mat44 = cam_pose.matrix;
		ofMatrix4x4 m44(mat44(0, 0), mat44(1, 0), mat44(2, 0), mat44(3, 0),
				mat44(0, 1), mat44(1, 1), mat44(2, 1), mat44(3, 1),
				mat44(0, 2), mat44(1, 2), mat44(2, 2), mat44(3, 2),
				mat44(0, 3), mat44(1, 3), mat44(2, 3), mat44(3, 3));

		if (camera_pov) {			
			cam.setPosition(m44.getTranslation());
			cam.lookAt(ofVec3f(0,0,1)*m44, ofVec3f(mat44(1,0),mat44(1,1), mat44(1,2)));
		}
		else
		{
			std::vector<ofPoint> path;
			for (int i = 0; i < path_est.size()-1; i++) {
				cv::Vec3d point = path_est[i].translation();
				path.push_back(ofPoint(point[0], point[1], point[2]));
			}
			ofPolyline trajectory(path);
			
			// render complete trajectory
			cam.begin();
			ofSetColor(ofColor::green);
			trajectory.draw();
			ofSetColor(ofColor::yellow);
			ofPushMatrix();
			ofMultMatrix(m44);
			ofDrawAxis(0.25);
			drawFrustum(f, cx, cy, 0.1, 0.4);
			ofPopMatrix();
			cam.end();
		}

		// update trajectory index (spring effect)
		forw *= (idx == n-1 || idx == 0) ? -1 : 1; idx += forw;
	}
}

//--------------------------------------------------------------
void ofApp::keyPressed(int key){
	if (key == 's') {
		if (!camera_pov) {
			last_cam_pos = cam.getPosition();
			last_cam_dir = cam.getLookAtDir();
			last_cam_up = cam.getUpDir();
			last_fov = cam.getFov();
                        cam.setFov(2 * atan2(2 * cx, 2 * f) * 180.0f/PI);
		}
		else {
			cam.setPosition(last_cam_pos);
			cam.lookAt(last_cam_dir, last_cam_up);
			cam.setFov(last_fov);
		}
		camera_pov = !camera_pov;
	}
	else if (key == 'h') {
		help();
	}
	else if (key == 'q') {
		ofExit();
	}
}

//--------------------------------------------------------------
void ofApp::keyReleased(int key){

}

//--------------------------------------------------------------
void ofApp::mouseMoved(int x, int y ){

}

//--------------------------------------------------------------
void ofApp::mouseDragged(int x, int y, int button){

}

//--------------------------------------------------------------
void ofApp::mousePressed(int x, int y, int button){

}

//--------------------------------------------------------------
void ofApp::mouseReleased(int x, int y, int button){

}

//--------------------------------------------------------------
void ofApp::mouseEntered(int x, int y){

}

//--------------------------------------------------------------
void ofApp::mouseExited(int x, int y){

}

//--------------------------------------------------------------
void ofApp::windowResized(int w, int h){

}

//--------------------------------------------------------------
void ofApp::gotMessage(ofMessage msg){

}

//--------------------------------------------------------------
void ofApp::dragEvent(ofDragInfo dragInfo){ 

}

あとでGitにあげます。

vizの方を試していないので、どの程度操作感が違うかわかりませんが、自由に視点を切り替えることはできます。

openCVのsfmのサンプルは、入力がトラッキング済みの対応点群のデータなのであんまりおもしろくありません。カメラを繋いでオプティカルフローで入力を作れるようにしたいと思います。

今日の漫画

野崎ふみこ先生のごはん漫画は、ドキュメンタリー感30～40％ぐらい含まれる感じが良いです。白山ベーグルは文京区にお店が実在したそうです。

たまこ定食 注文のいらないお店 ： 1 (ジュールコミックス)

ベーグル食べない？?幸せカフェごはん? (A.L.C. DX)

ベーグル食べない?~幸せカフェごはん~ 2 (秋田レディースコミックスデラックス)

ベーグル食べない?~幸せカフェごはん~ 3 (秋田レディースコミックスデラックス)