解决SLAM14讲第九节initial.ply不能显示的问题

meshlab只显示绿色的摄像机点，不显示正常3D点

解决思路：给ply中绿色的摄像机点和白色的普通点都加上坐标数值异常过滤
极大 / 极小的数值，这说明部分地图点的 3D 坐标是无效值（比如 NaN、inf 或者计算溢出的错误值），导致 MeshLab 显示出了异常的坐标刻度。

点击查看代码

void BALProblem::WriteToPLYFile(const std::string& filename) const
{
    //打开输出文件(C++风格）
    std::ofstream of(filename.c_str());
    if (!of.is_open())//关键：判断文件是否成功打开
    {
        std::cerr<<"错误：无法打开文件"<<filename<<
            ".无法写入PLY数据！"<<std::endl;
    }
    //写入PLY头文件
    of<<"ply"
    <<'\n'<<"format ascii 1.0"
    <<'\n'<<"element vertex "<<num_cameras_+num_points_// 顶点总数(相机+点)
    <<'\n'<<"property float x" //X坐标
    <<'\n'<<"property float y" //Y坐标
    <<'\n'<<"property float z"//Z坐标
    <<'\n'<<"property uchar red"//红色通道
    <<'\n'<<"property uchar green"//绿色通道
    <<'\n'<<"property uchar blue"//蓝色通道
    <<'\n'<<"end_header"<<std::endl;
    int validcount=0;
    //导出相机中心(绿色点)
    double angle_axis[3];//存储角轴旋转
    double center[3];//存储相机中心
    for (int i=0;i<num_cameras();++i)
    {
        //获取第i个相机的参数指针
        const double *camera=cameras()+camera_block_size()*i;
        //将相机参数转换为角轴旋转和相机中心
        CameraToAngelAxisAndCenter(camera,angle_axis,center);
        if (std::isnan(center[0]) || std::isnan(center[1]) || std::isnan(center[2])) continue;
        if (std::isinf(center[0]) || std::isinf(center[1]) || std::isinf(center[2])) continue;
        if (abs(center[0]) > 1000 || abs(center[1]) > 1000 || abs(center[2]) > 1000) continue; // 过滤过远点
        //写入相机中心坐标和绿色
        of<<center[0]<<' '<<center[1]<<' '<<center[2]
        <<" 0 255 0"<<'\n';//前面加一个空格，保持格式统一，避免连写
        validcount+=1;
    }
    //导出3D点（白色点：255，255，255）
    const double *points=parameters_+camera_block_size()*num_cameras_;
    for (int i=0;i<num_points();++i)
    {
        const double *point=points+i*point_block_size();
        if (fabs(point[0]>1e5)||fabs(point[1]>1e5)||fabs(point[2]>1e5))
        {
            continue;//跳过超大坐标点
        }
        if (std::isnan(point[0])||std::isinf(point[0])||
        std::isnan(point[1])||std::isinf(point[1])||
        std::isnan(point[2])||std::isinf(point[2])
        )
        {
            continue;//跳过无效值
        }
        validcount+=1;
        //写入3D点坐标

        for (int j=0;j<point_block_size();++j)
        {
            of<<point[j]<<' ';
        }
        //写入白色
        of<<" 255 255 255\n";
    }
    //关键：刷新缓冲区，确保所有数据写入磁盘
    of.flush();
    //关闭文件流
    of.close();
    //可选：添加写入成功提示
    std::cout<<"validcount:"<<validcount<<std::endl;
    std::cout<<"PLY文件写入成功: "<<filename<<std::endl;

}

然后按照输出的validcout值手动修改文件头element vertex的值 结果