将代码生成伪代码，只保留括号格式和中文

函数 addGPSFactor(): 如果 (gpsQueue) 为空: 返回

如果 (cloudKeyPoses3D->points) 为空:
    返回
否则:
    如果 (pointDistance(cloudKeyPoses3D->front(), cloudKeyPoses3D->back()) < 5.0):
        返回

如果 (poseCovariance(3,3) < poseCovThreshold && poseCovariance(4,4) < poseCovThreshold):
    返回

静态变量 lastGPSPoint

当 (gpsQueue) 不为空:
    如果 (gpsQueue.front().header.stamp.toSec() < timeLaserInfoCur - 0.2):
        删除 (gpsQueue) 头部元素
    否则如果 (gpsQueue.front().header.stamp.toSec() > timeLaserInfoCur + 0.2):
        退出循环
    否则:
        nav_msgs::Odometry thisGPS = gpsQueue.front()
        删除 (gpsQueue) 头部元素

        noise_x = thisGPS.pose.covariance[0]
        noise_y = thisGPS.pose.covariance[7]
        noise_z = thisGPS.pose.covariance[14]
        如果 (noise_x > gpsCovThreshold || noise_y > gpsCovThreshold):
            继续循环

        gps_x = thisGPS.pose.pose.position.x
        gps_y = thisGPS.pose.pose.position.y
        gps_z = thisGPS.pose.pose.position.z
        如果 (!useGpsElevation):
            gps_z = transformTobeMapped[5]
            noise_z = 0.01

        如果 (abs(gps_x) < 1e-6 && abs(gps_y) < 1e-6):
            继续循环

        curGPSPoint.x = gps_x
        curGPSPoint.y = gps_y
        curGPSPoint.z = gps_z
        如果 (pointDistance(curGPSPoint, lastGPSPoint) < 5.0):
            继续循环
        否则:
            lastGPSPoint = curGPSPoint

        Vector3 = gtsam::Vector(3)
        Vector3 << max(noise_x, 1.0f), max(noise_y, 1.0f), max(noise_z, 1.0f)
        gps_noise = noiseModel::Diagonal::Variances(Vector3)
        gps_factor = gtsam::GPSFactor(cloudKeyPoses3D->size(), gtsam::Point3(gps_x, gps_y, gps_z), gps_noise)
        gtSAMgraph.add(gps_factor)

        aLoopIsClosed = true
        退出循环