RGB to Depth映射

//相机内参
struct Intrinsic{
  int rows, cols;
  float cx, cy, fx, fy;
};

//相机外参R,T
struct Extrinsic{
  glm::mat3 rotation;
  glm::vec3 translation;
};

//图像帧
struct Frame{
  int rows, cols;
  int size;
  void *data;
};

//BGR像素
struct BGR888Pixel{
  uint8_t b, g, r;
};

//利用相机内参先计算出索引表，为后续点云计算提供预计算。
float* GenerateXyTable(const Intrinsic& intrinsic) {
	if (intrinsic.fx == 0 || intrinsic.fy == 0) {
		printf("camera intrinsic parameters error.");
		return nullptr;
	}

	float* xy_table = new float[intrinsic.rows * intrinsic.cols * 2];
	float fx_recip = 1.0f / intrinsic.fx;
	float fy_recip = 1.0f / intrinsic.fy;

	for (int h = 0; h < intrinsic.rows; h++){
		for (int w = 0; w < intrinsic.cols; w++) {
			int idx = h * intrinsic.cols + w;
			xy_table[2 * idx] = ((float)w - intrinsic.cx) * fx_recip;
			xy_table[2 * idx + 1] = ((float)h - intrinsic.cy) * fy_recip;
		}
	}
	return xy_table;
}

//彩色图映射到深度图空间，结果彩色图大小跟深度图大小一致
int AlignColorToDepthSpace(Frame& aligned_color,
	const Frame& depth_map,
	const Frame& color_img,
	const Intrinsic& rgb_intri,
	const glm::mat3& rotation,
	const glm::vec3& translation,
	float* xy_table){
	if (xy_table == nullptr || aligned_color.data == nullptr || depth_map.data == nullptr) 
	  return 3;
	memset(aligned_color.data, 0, aligned_color.size);
	
        //提供zbuf缓存，用于处理深度值冲突：取深度值小的像素为有效像素
	int* zbuf = new int[color_img.cols * color_img.rows];
	memset(zbuf, 0, color_img.cols * color_img.rows* sizeof(int));
	for (int idx = 0; idx <depth_map.cols * depth_map.rows; idx++) {
		float depth = static_cast<float>(((uint16_t*)depth_map.data)[idx]);
		if (depth == 0) continue;
		glm::vec3 point{ xy_table[2 * idx] * depth, xy_table[2 * idx + 1] * depth, depth };
		glm::vec3 point_world = rotation * point + translation;
		if (point_world.z == 0) continue;
		int u = static_cast<int>((point_world.x / point_world.z) * rgb_intri.fx + rgb_intri.cx);
		int v = static_cast<int>((point_world.y / point_world.z) * rgb_intri.fy + rgb_intri.cy);
		if (u < 0 || u >= color_img.cols || v < 0 || v >= color_img.rows)
			continue;

		int zbuf_idx = v * color_img.cols + u;
		if (zbuf[zbuf_idx] == 0) zbuf[zbuf_idx] = idx + 1;
		else if (depth < static_cast<float>(((uint16_t*)depth_map.data)[zbuf[zbuf_idx] - 1])) zbuf[zbuf_idx] = idx + 1;
	}

	BGR888Pixel* aligned_color_ptr = static_cast<BGR888Pixel*>(aligned_color.data);
	BGR888Pixel* color_ptr = static_cast<BGR888Pixel*>(color_img.data);
	for (int i = 0; i < color_img.cols * color_img.rows; i++) {
		if (zbuf[i] == 0) continue;
                //从索引取出对应的彩色像素用于填充原深度图尺寸的彩色图映射图
		aligned_color_ptr[zbuf[i] - 1] = color_ptr[i];
	}
	delete[] zbuf;
	return 0;
}