void write_mesh_ply(Point3f * pCameraCoordinates, Point2f * pColorCoordinatesOfDepth, int deepth_width, int deepth_height, int rgb_width, int rgb_height, int step = 1)
{
if(NULL == pCameraCoordinates)
{
return;
}
mesh_face_typedef ply_face;
vector<mesh_face_typedef> v_ply_face;
Point3f p1;
Point3f p2;
Point3f p3;
Point3f p4;
Point3f* ptr = NULL;
Point2f uv1;
Point2f uv2;
Point2f uv3;
Point2f uv4;
Point2f* ptr2 = NULL;
float f_width = (float)deepth_width;
float f_height = (float)deepth_height;
float z_th = 0.5;
int tri_count_flag = 0;
/*
这里解释为什么取20
http://stackoverflow.com/questions/11902593/get-depth-at-color-position-kinect-sdk
*/
for(int i = 20; i < deepth_height - step - 20; i += step)
{
ptr = pCameraCoordinates + i * deepth_width;
ptr2 = pColorCoordinatesOfDepth + i * deepth_width;
for(int j = 20; j < deepth_width - step - 20; j += step)
{
p1 = ptr[j];
p2 = ptr[j + step];
p3 = ptr[deepth_width * step + j];
p4 = ptr[deepth_width * step + step + j];
uv1 = ptr2[j];
uv2 = ptr2[j + step];
uv3 = ptr2[deepth_width * step + j];
uv4 = ptr2[deepth_width * step + step + j];
tri_count_flag = 0;
/* 取1 2 3 点画三角形 */
if((abs(p1.Z - p2.Z) < z_th) && (abs(p2.Z - p3.Z) < z_th) && (abs(p1.Z - p3.Z) < z_th))
{
ply_face.p1 = p1;
ply_face.p2 = p2;
ply_face.p3 = p3;
ply_face.uv1.X = uv1.X / (float)rgb_width;
ply_face.uv1.Y = uv1.Y / (float)rgb_height;
ply_face.uv2.X = uv2.X / (float)rgb_width;
ply_face.uv2.Y = uv2.Y / (float)rgb_height;
ply_face.uv3.X = uv3.X / (float)rgb_width;
ply_face.uv3.Y = uv3.Y / (float)rgb_height;
v_ply_face.push_back(ply_face);
tri_count_flag++;
}
/* 取2 3 4点画三角形 */
if((abs(p2.Z - p3.Z) < z_th) && (abs(p2.Z - p4.Z) < z_th) && (abs(p3.Z - p4.Z) < z_th))
{
ply_face.p1 = p2;
ply_face.p2 = p4;
ply_face.p3 = p3;
ply_face.uv1.X = uv2.X / (float)rgb_width;
ply_face.uv1.Y = uv2.Y / (float)rgb_height;
ply_face.uv2.X = uv4.X / (float)rgb_width;
ply_face.uv2.Y = uv4.Y / (float)rgb_height;
ply_face.uv3.X = uv3.X / (float)rgb_width;
ply_face.uv3.Y = uv3.Y / (float)rgb_height;
v_ply_face.push_back(ply_face);
tri_count_flag++;
}
if(!tri_count_flag)
{
/* 取1 2 4 点画三角形 */
if((abs(p1.Z - p2.Z) < z_th) && (abs(p2.Z - p4.Z) < z_th) && (abs(p1.Z - p4.Z) < z_th))
{
ply_face.p1 = p1;
ply_face.p2 = p2;
ply_face.p3 = p4;
ply_face.uv1.X = uv1.X / (float)rgb_width;
ply_face.uv1.Y = uv1.Y / (float)rgb_height;
ply_face.uv2.X = uv2.X / (float)rgb_width;
ply_face.uv2.Y = uv2.Y / (float)rgb_height;
ply_face.uv3.X = uv4.X / (float)rgb_width;
ply_face.uv3.Y = uv4.Y / (float)rgb_height;
v_ply_face.push_back(ply_face);
}
/* 取1 3 4点画三角形 */
if((abs(p1.Z - p3.Z) < z_th) && (abs(p1.Z - p4.Z) < z_th) && (abs(p3.Z - p4.Z) < z_th))
{
ply_face.p1 = p1;
ply_face.p2 = p4;
ply_face.p3 = p3;
ply_face.uv1.X = uv1.X / (float)rgb_width;
ply_face.uv1.Y = uv1.Y / (float)rgb_height;
ply_face.uv2.X = uv4.X / (float)rgb_width;
ply_face.uv2.Y = uv4.Y / (float)rgb_height;
ply_face.uv3.X = uv3.X / (float)rgb_width;
ply_face.uv3.Y = uv3.Y / (float)rgb_height;
v_ply_face.push_back(ply_face);
}
}
}
}
FILE *f_ply;
fopen_s(&f_ply, "output_ply.ply", "w");
//write ply header
fprintf(f_ply, "ply\n");
fprintf(f_ply, "format ascii 1.0\n");
fprintf(f_ply, "comment TextureFile rgb.png\n");
fprintf(f_ply, "element vertex %d\n", v_ply_face.size() * 3);
fprintf(f_ply, "property float x\n");
fprintf(f_ply, "property float y\n");
fprintf(f_ply, "property float z\n");
fprintf(f_ply, "element face %d\n", v_ply_face.size());
fprintf(f_ply, "property list uchar int vertex_indices\n");
fprintf(f_ply, "property list uchar float texcoord\n");
fprintf(f_ply, "end_header\n");
for(auto iter = v_ply_face.begin(); iter != v_ply_face.end(); iter++)
{
fprintf(f_ply, "%f %f %f\n", iter->p1.X, iter->p1.Y, iter->p1.Z);
fprintf(f_ply, "%f %f %f\n", iter->p2.X, iter->p2.Y, iter->p2.Z);
fprintf(f_ply, "%f %f %f\n", iter->p3.X, iter->p3.Y, iter->p3.Z);
}
int index = 0;
for(auto iter = v_ply_face.begin(); iter != v_ply_face.end(); iter++)
{
fprintf(f_ply, "3 %d %d %d ", index, index + 1, index + 2);
//print triangle uv
fprintf(f_ply, "6 %f %f %f %f %f %f\n",
iter->uv1.X, iter->uv1.Y,
iter->uv2.X, iter->uv2.Y,
iter->uv3.X, iter->uv3.Y);
index += 3;
}
fclose(f_ply);
}
写 带mesh的ply格式文件
最新推荐文章于 2023-11-28 23:23:33 发布