cvl-robot's diary

研究ノート メモメモ https://github.com/dotchang/

メモ:OpenGLの描画結果のデプスバッファから距離情報を復元する方法

OpenGLの描画結果のデプス値を単位を直して画像データとして取得するサンプル。詳細は[1]を参照のこと。
1. glReadPixelsでデプスバッファのデータを読み込み
2. nearClipとfarClipでデプス値を正規化
3. 上下反転されているので入れ替え

ofPixels pix;
pix.allocate(ofGetWidth(), ofGetHeight(), OF_IMAGE_COLOR);
double depth_scale = 1000.0;

std::vector<float> depth_image;
depth_image.resize(ofGetWidth() * ofGetHeight());

glReadPixels(0, 0, ofGetWidth(), ofGetHeight(), GL_DEPTH_COMPONENT, GL_FLOAT, depth_image.data());

double z_near = cam.getNearClip();
double z_far = cam.getFarClip();

for (int i = 0; i < ofGetHeight(); i++) {
	for (int j = 0; j < ofGetWidth(); j++) {
		double z_depth = 2.0 * z_near * z_far /
			(z_far + z_near - (2.0 * (double)depth_image[(ofGetHeight() - i - 1) * ofGetWidth() + j] - 1.0) *
			(z_far - z_near));
			uint16_t p = (uint16_t)std::min(std::round(depth_scale * z_depth), (double)INT16_MAX);

			pix.setColor(j, i, ofColor((float)p / 255, (float)p%255, 0));
	}
}

plyデータに復元

	std::vector<float> depth_image;
	depth_image.resize(ofGetWidth() * ofGetHeight());

	glFinish();
	glReadPixels(0, 0, ofGetWidth(), ofGetHeight(), GL_DEPTH_COMPONENT, GL_FLOAT, depth_image.data());

	double z_near = cam.getNearClip();
	double z_far = cam.getFarClip();

	ofMatrix4x4 mvp_matrix = cam.getProjectionMatrix();
	int width = ofGetWidth();
	int height = ofGetHeight();
	double depth_scale = 1000.0;

	depth_pointcloud.clear();
	for (int i = 0; i < height; i++) {
		for (int j = 0; j < width; j++) {
			double p = (double)depth_image[(height - i - 1) * width + j];
			double z_depth = 2.0 * z_near * z_far /
				(z_far + z_near - (2.0 * p - 1.0) *
				(z_far - z_near));
			uint16_t q = (uint16_t)std::min(std::round(depth_scale * z_depth), (double)INT16_MAX);

			ofVec3f u = Unproject(ofVec3f(j + 0.5, i + 0.5, p), mvp_matrix, width, height);
			if (u != ofVec3f()) {
				depth_pointcloud.addVertex(ofVec3f(-u.x, -u.y, u.z));
			}
			else {
				depth_pointcloud.addVertex(u);
			}
		}
	}

	// add triangle meshes
	for (int i = 1; i < height; i++) {
		for (int j = 1; j < width; j++) {
			ofVec3f a = depth_pointcloud.getVertex(i * width + j);
			ofVec3f b = depth_pointcloud.getVertex(i * width + (j - 1));
			ofVec3f c = depth_pointcloud.getVertex((i - 1) * width + (j - 1));
			ofVec3f d = depth_pointcloud.getVertex((i - 1) * width + j);
			if (a != ofVec3f() && b != ofVec3f() && c != ofVec3f()) {
				depth_pointcloud.addTriangle(i * width + j, (i - 1)*width + (j - 1), i * width + (j - 1));
			}
			if (a != ofVec3f() && c != ofVec3f() && d != ofVec3f()) {
				depth_pointcloud.addTriangle(i * width + j, (i - 1)*width + j, (i - 1) * width + (j - 1));
			}
		}
	}

	// remove verbose meshes
	std::map<int, int> idx;
	int cnt = 0;
	for (int i = 0; i < depth_pointcloud.getNumVertices(); i++) {
		if (depth_pointcloud.getVertex(i) == ofVec3f()) {
			idx[i] = -1;
		}
		else {
			idx[i] = cnt++;
		}
	}
	for (int i = depth_pointcloud.getNumVertices() - 1; i >= 0; i--) {
		if(idx[i] == -1) {
			depth_pointcloud.removeVertex(i);
		}
	}
	for (int i = 0; i < depth_pointcloud.getNumIndices(); i++) {
		depth_pointcloud.setIndex(i, idx[depth_pointcloud.getIndex(i)]);
	}

	// save to file
	depth_pointcloud.save("depth_test.ply");
ofVec3f Unproject(const ofVec3f &screen_point,
	const ofMatrix4x4 &mvp_matrix, const int width, const int height)
{
	ofMatrix4x4 inv_mvp_matrix_t = ofMatrix4x4::getTransposedOf(mvp_matrix.getInverse());
	ofVec4f point = inv_mvp_matrix_t *
		ofVec4f(screen_point.x / (double)width * 2.0 - 1.0,
			screen_point.y / (double)height * 2.0 - 1.0,
			screen_point.z * 2.0 - 1.0,
			1.0);
	if (point.w == 0.0) {
		return ofVec3f();
	}
	point /= point.w;
	return ofVec3f(point.x, point.y, point.z);
}

[1] Open3D Visualization
github.com

今日の座布団

安い椅子での長時間の座り仕事で、お尻が痛くなります。そんな時にはこれ。ちょっと暑いけど。