三次元画像計測システムofxActiveScanをopenFrameworks0.8.4(Windows MSVC2012)で動かしてみる（その3）

前回、カメラを変更することで形状が正しくとれるようになりましたが出力結果が点だけなので、PLY形式に吐き出しても他のアプリケーションでちょっと使いにくい状況でした。なので、メッシュを張っておきましょう。変更するのは、ofxActiveScan.cpp内のtriangulate関数です。

ofMesh triangulate(Options& options, Map2f& hmap, Map2f& vmap, Map2f& mmap,
				   slib::CMatrix<3,3,double>& matKcam, double camDist,
				   slib::CMatrix<3,3,double>& matKpro, double proDist,
				   slib::CMatrix<3,4,double>& proRt, ofImage& cp, Map2i& indices)
{
	ofMesh mesh;
	indices.Initialize(hmap.size());
	indices.Clear(-1);
	
	slib::CVector<2,double>
		cod1=make_vector<double>((options.projector_width+1)/2.0,options.projector_height*options.projector_horizontal_center),
		cod2=(make_vector(1.0,1.0)+mmap.size())/2;
	
	// extrinsic matrices of projector and camera
	slib::CMatrix<3,4,double> camRt;
	camRt = make_diagonal_matrix(1,1,1).AppendCols(make_vector(0,0,0));//CMatrix<3,4,double>::GetIdentity(); // reconstruction is in camera coordinate frame
	
	// compute projection matrices of projector and camera
	std::vector<slib::CMatrix<3,4,double> > matrices(2);
	matrices[0] = matKcam * camRt; // camera
	matrices[1] = matKpro * proRt; // projector
	
	// fundamental matrix
	CMatrix<3,3,double> matR;
	CVector<3,double> vecT;
	matR.Initialize(proRt.ptr());
	vecT.Initialize(proRt.ptr()+9);
	CMatrix<3,3,double> matF = transpose_of(inverse_of(matKpro)) * GetSkewSymmetric(vecT) * matR * inverse_of(matKcam);
	
	// triangulate 3d points
	std::vector<CVector<3,double> > result;
	for (int y=0; y<hmap.size(1); y++)
	{
		if (y % (hmap.size(1)/10) == 0)
			TRACE("triangulation: %d%% done\n", 100*y/hmap.size(1));
		
		int nbehind=0;
		for (int x=0; x<hmap.size(0); x++)
		{
			if (mmap.cell(x,y))
			{
				// 2D correspondence
				std::vector<CVector<2,double> > p2d(2);
				
				// camra coordinate
				slib::fmatrix::CancelRadialDistortion(camDist,cod2,make_vector<double>(x,y),p2d[0]);
				
				// projector coordinate
				double proj_y;
				
				proj_y = vmap.cell(x,y);
				
				//CVector<3,double> epiline = matF * GetHomogeneousVector(p2d[0]);
				//proj_y = -(epiline[0] * hmap.cell(x,y) + epiline[2]) / epiline[1];
				
				slib::fmatrix::CancelRadialDistortion(proDist,cod1,make_vector<double>(hmap.cell(x,y),proj_y),p2d[1]);
				
				// triangulate
				CVector<3,double> p3d;
				SolveStereo(p2d, matrices, p3d);
				
				// save
				//result.push_back(p3d);
				if (p3d[2]<0) {
					nbehind++;
				} else {
					indices.cell(x, y) = mesh.getNumVertices();
					mesh.addVertex(ofVec3f(p3d[0], p3d[1], p3d[2]));
					if( cp.bAllocated() ) {
						mesh.addColor(cp.getColor(x, y));
					}
				}
			}
		}
		if (nbehind)
			TRACE("found %d points behind viewpoint.\n", nbehind);
	}

	// mesh
	float scale_factor = 1.0;
	float dist_thresh = 0.02 * scale_factor;
	for (int y=0; y<hmap.size(1)-1; y++) {
		for (int x=0; x<hmap.size(0); x++) {
			if(indices.cell(x,y)>=0){
				ofVec3f p0 = mesh.getVertex(indices.cell(x,y));
				if(x!=hmap.size(0)){
					if((indices.cell(x+1,y)>=0) && (indices.cell(x,y+1)>=0)){
						ofVec3f p1 = mesh.getVertex(indices.cell(x+1,y));
						ofVec3f p2 = mesh.getVertex(indices.cell(x,y+1));
						float dist_threshold = 0.02;
						if((p0.distance(p1) < dist_threshold) && (p1.distance(p2) < dist_threshold) && (p2.distance(p0) < dist_threshold)) {
							mesh.addIndex(indices.cell(x,y));
							mesh.addIndex(indices.cell(x+1,y));
							mesh.addIndex(indices.cell(x,y+1));
						}
					}
				}
				if(x!=0){
					if((indices.cell(x,y+1)>=0) && (indices.cell(x-1,y+1)>=0)){
						ofVec3f p1 = mesh.getVertex(indices.cell(x,y+1));
						ofVec3f p2 = mesh.getVertex(indices.cell(x-1,y+1));
						float dist_threshold = 0.02;
						if((p0.distance(p1) < dist_threshold) && (p1.distance(p2) < dist_threshold) && (p2.distance(p0) < dist_threshold)) {
							mesh.addIndex(indices.cell(x,y));
							mesh.addIndex(indices.cell(x,y+1));
							mesh.addIndex(indices.cell(x-1,y+1));
						}
					}
				}
			}
		}
	}

	return mesh;
}

白い部分が飛んでしまって計測に失敗していますが、メッシュはちゃんと張れていますね。
スケールをちゃんと合わせたいのですが、この計測法だとスケール合わせのためには一工夫必要なのが厄介ですね。メッシュのエッジの最大長とスケールをソースコード中に適当に書いてしまっているので、適宜工夫して調整してください。