2画像間の3次元復元

アバター
kazuoni
記事: 17
登録日時: 14年前
住所: 愛知
連絡を取る:
kazuoni に連絡する

2画像間の3次元復元

投稿記事 by kazuoni » 14年前

先日SolidFromPhoto様にメールを送ったのですが、返事がないので結局自分で実装となりました・・・。
2日かけてやっと半分終わりました。
今は山田健人らの2画像からの3次元復元最新アルゴリズム
ttp://www.suri.cs.okayama-u.ac.jp/~kanatani/p ... 2views.pdf
をすべて実装しました。
これだけでも3次元復元は行えるのですが、全周復元には向いていません。
っというのも、一つの特徴点が複数枚の画像に取れるとき、
それぞれの画像間で独立的に3次元復元を行うので3次元座標がすべて独立してしまいます。
これを複数の画像で調節するために「バンドル調節」を行います。
今からはこの実装を行おうと思います。

ちょっとここで今回作成したコードがこの日記でどの様に掲載されるか気になったんでちょっとテストがてらに載せてみます。
長いからどうなるんだろう・・・。

3DRec.h

CODE: 

#ifndef INCLUDE_3DREC_H
#define INCLUDE_3DREC_H

#include 
#include 

namespace _3DRec{
	// 基礎行列の計算
	math::Matrix CalcFundamentalMatrixIni(const math::Matrix& p1, const math::Matrix& p2, double f0);

	// 自由焦点距離計算
	bool CalcFreeFocusDistance(const math::Matrix& F, double f0, double &f, double &fd);

	// 平均焦点距離計算
	bool CalcAverageFocusDistance(const math::Matrix& F, double f0, double &f, double &fd);

	// 固定焦点距離計算
	bool CalcFixedFocusDistance(const math::Matrix& F, double f0, double &f, double &fd);

	// カメラパラメータの計算
	void CalcCameraParam(math::Matrix p1, math::Matrix p2, const math::Matrix& F, double f, double fd, double f0, math::Vector& t, math::Matrix& R);

	// 再投影誤差の計算
	double CalcReprojectionError(const math::Matrix& p1, const math::Matrix& p2, const math::Matrix& F, math::Matrix& ph1, math::Matrix& ph2, double f0);

	// 3次元位置の計算
	void Calc3DPoint(const math::Matrix& ph1, const math::Matrix& ph2, double f, double fd, double f0, const math::Vector& t, const math::Matrix& R, math::Matrix& p3d);
	
};

#endif
3DRec.cpp

CODE: 

#include 
#include 
#include 
#include 
#include 
#include 
#include "3DRec.h"
#include "Common.h"

using namespace std;

#define E_ini 1.0e+9
#define epsilon 1.0e-6
#define E_epsilon 1.0e-4

enum ANS_FORMAT{ NON_CALC , ONE_CALC , CALC };

double FunctionK(math::Vector& a, double xi);
ANS_FORMAT CalcTrieEuation(double a, double b, double c, double d, math::Vector& xi);
int MySgn(double x);

namespace _3DRec{
	math::Matrix CalcFundamentalMatrixIni(const math::Matrix& p1, const math::Matrix& p2, double f0){
		math::Matrix F(3,3);
		F.Clear();

		if(p1.Row()!=p2.Row() || p1.Column()!=p2.Column()){
			cerr 0) && (1+eta)>0){
				f = f0/sqrt(1+xi);
				fd = f0/sqrt(1+eta);
				flag = true;
			}	
		}
		return flag;
	}

	bool CalcAverageFocusDistance(const math::Matrix& F, double f0, double &f, double &fd){
		math::Vector k(3), e(3), ed(3), fk(3), ftk(3), ek(3), edk(3), lambda(3);
		math::Matrix Ft = math::Transpose(F), H(2,2), fft(3,3), ftf(3,3), m_lambda(3,3);
		double kfk, xi, eta;
		bool flag = false;
		k[0] = k[1] = 0.0; k[2] = 1.0;

		fk = F*k;
		ftk = Ft*k;
		fft = F*Ft;
		ftf = Ft*F;
		fft.BoostEigen(lambda,m_lambda);
		e = math::Normalize(m_lambda.ColumnVector(0));
		ed = math::Normalize(m_lambda.ColumnVector(1));
		ek = e%k;
		edk = ed%k;
		kfk = (k,fk);

		if(!(fabs(kfk)0) && (1+eta)>0){
				f = f0/sqrt(1+xi);
				fd = f0/sqrt(1+eta);
				flag = true;
			}
		}
		return flag;
	}

	bool CalcFixedFocusDistance(const math::Matrix& F, double f0, double &f, double &fd){
		math::Vector k(3), e(3), ed(3), fk(3), ftk(3), ek(3), edk(3), a(5), fftk(3), ftfk(3), lambda(3), xi(3);
		math::Matrix Ft = math::Transpose(F), fft(3,3), ftf(3,3), m_lambda(3,3);
		double kfk, K[3], f_xi;
		ANS_FORMAT format;

		k[0] = k[1] = 0.0; k[2] = 1.0;
		fk = F*k;
		ftk = Ft*k;
		fft = F*Ft;
		ftf = Ft*F;
		fft.BoostEigen(lambda,m_lambda);
		e = math::Normalize(m_lambda.ColumnVector(0));
		ed = math::Normalize(m_lambda.ColumnVector(1));
		ek = e%k;
		edk = ed%k;
		kfk = (k,fk);
		fftk = fft*k;
		ftfk = Ft*F*k;

		// (1)
		a[0] = pow(kfk,4.0)/2.0;
		a[1] = pow(kfk,2.0)*(ftk.NormSQR()+fk.NormSQR());
		a[2] = (pow(ftk.NormSQR()-fk.NormSQR(),2.0))/2.0 + kfk*(4.0*(k,F*Ft*F*k)-kfk*F.NormSQR());
		a[3] = 2.0*(fftk.NormSQR()+ftfk.NormSQR()) - (ftk.NormSQR()+fk.NormSQR())*F.NormSQR();
		a[4] = fft.NormSQR() - pow(F.NormSQR(),2.0)/2.0;

		// (2), (3), (4), (5)
		if(fabs(kfk)0.0){
			f = fd = f0/sqrt(1.0+f_xi);
			return true;
		}
		else
			return false;
	}

	void CalcCameraParam(math::Matrix p1, math::Matrix p2, const math::Matrix& F, double f, double fd, double f0, math::Vector& t, math::Matrix& R){
		math::Matrix E(3,3), diag1(3,3), diag2(3,3), m_lambda(3,3), eet(3,3), te_ex(3,3), U(3,3), diag3(3,3), Vt(3,3), diag4(3,3), UVt(3,3);
		math::Vector lambda(3);
		int corr_point_num = p1.Column();
		double sum_4 = 0.0;

		if(p1.Row()!=p2.Row() || p1.Column()!=p2.Column()){
			cerr = 0 ) u = pow( d_u, 1.0/3.0 );
			else           u = -pow( -d_u, 1.0/3.0 );
			if( d_v >= 0 ) v = pow( d_v, 1.0/3.0 );
			else           v = -pow( -d_v, 1.0/3.0 );
			y1 = u+v;
			y2 = y3 = -y1/2;
			Im = sqrt(3.0)/2*(u-v);
			flag = ONE_CALC;
		}
		xi[0] = y1 - b/(3*a);
		xi[1] = y2 - b/(3*a);
		xi[2] = y3 - b/(3*a);
	}

    return flag;
}

int MySgn(double x){
	if(x>0.0)
		return 1;
	else if(x<0.0)
		return -1;
	else
		return 0;
}
ページトップ
コメントはまだありません。