1. 地理坐标简介

地理坐标又称工程坐标

使用平面来近似地球的球面，在地表选取一个点，沿着这个点做地球的切平面

2. 初始化

bool init(const Eigen::Vector3d& point) {const char* geoccs = "+proj=geocent +datum=WGS84";const char* latlon = "+proj=latlong +datum=WGS84 +units=m";if (!(pjGeoccs_ = pj_init_plus(geoccs))) {LOG(ERROR) << "initzero init pjGeoccs fail";return false;}if (!(pjLatlon_ = pj_init_plus(latlon))) {LOG(ERROR) << "initzero init pjLatlon fail";return false;}double X = point.x * DEG_TO_RAD;double Y = point.y * DEG_TO_RAD;double Z = point.z;int iRev = pj_transform(pjLatlon_, pjGeoccs_, 1, 1, &X, &Y, &Z);if (iRev != 0) {LOG(ERROR) << "initzero pj transform fail";return false;}Eigen::Matrix4d orgin_geo;orgin_geo << 1,  0,  0,  0,0,  1,  0,  0,0,  0,  1,  0,X,  Y,  Z,  1;Eigen::Matrix4d rotZ;double z = (point.x - 270.0) / 180.0 * M_PI;rotZ << cos(z), sin(z), 0, 0,-sin(z), cos(z), 0, 0,0, 0, 1, 0,0, 0, 0, 1;Eigen::Matrix4d rotX;double x = (90.0 - point.y) / 180.0 * M_PI;rotX << 1, 0, 0, 0,0, cos(x), sin(x), 0,0, -sin(x), cos(x), 0,0, 0, 0, 1;l2gMatrix_ = rotX * rotZ * orgin_geo;Eigen::Matrix4d I = Eigen::Matrix4d::Identity(4, 4);g2lMatrix_ = l2gMatrix_.colPivHouseholderQr().solve(I);return true;}

3. GPS转地理坐标

 void gps2geo(const Eigen::Vecotor3d& gps, Eigen::Vecotor3d& geo) {double* X = new double[1];double* Y = new double[1];double* Z = new double[1];X[0] = gps.x * DEG_TO_RAD;Y[0] = gps.y * DEG_TO_RAD;Z[0] = gps.z;int iRev = pj_transform(pjLatlon_, pjGeoccs_, 1, 1, X, Y, Z);if (iRev != 0) {LOG(ERROR) << "gps2geo pj transform fail";return false;}Eigen::RowVector4d P;P<< X[0], Y[0], Z[0], 1.0;P = P * g2lMatrix_;geo.x = P(0);geo.y = P(1);geop.z = P(2);delete[] X;delete[] Y;delete[] Z;return true;}

4. 地理坐标转GPS

  void geo2gps(const Eigen::Vector3d& geo, Eigen::Vector3d& gps) {double* X = new double[1];double* Y = new double[1];double* Z = new double[1];Eigen::RowVector4d P;P << geo.x, geo.y geo.z, 1.0;P = P * l2gMatrix_;X[0] = P(0);Y[0] = P(1);Z[0] = P(2);int iRev = pj_transform(pjGeoccs_, pjLatlon_, 1, 1, X, Y, Z);if (iRev != 0) {std::cout<<"gps2geo pj transform fail"<<std::endl;return false;}gps.x = X[0] * RAD_TO_DEG;gps.y = Y[0] * RAD_TO_DEG;gps.z = Z[0];delete[] X;delete[] Y;delete[] Z;gpspt->time = geopt.time;return true;}