cereas学习（5) vins-fusion vio融合gps全局优化

 

 

残差定义

 

 Factors.h

?

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125

/*******************************************************  * Copyright (C) 2019, Aerial Robotics Group, Hong Kong University of Science and Technology  *  * This file is part of VINS.  *  * Licensed under the GNU General Public License v3.0;  * you may not use this file except in compliance with the License.  *  * Author: Qin Tong (qintonguav@gmail.com)  *******************************************************/   #pragma once #include <ceres/ceres.h> #include <ceres/rotation.h>   template <typename T> inline void QuaternionInverse(const T q[4], T q_inverse[4]) {     q_inverse[0] = q[0];     q_inverse[1] = -q[1];     q_inverse[2] = -q[2];     q_inverse[3] = -q[3]; };     struct TError {     //输入观测量     TError(double t_x, double t_y, double t_z, double var)                   :t_x(t_x), t_y(t_y), t_z(t_z), var(var){}       template <typename T>     // 待优化输出的tj量     bool operator()(const T* tj, T* residuals) const     {          //残差= 待优化出来的量（未知）-观测量（实际看到的有误差） min(x'-x(已知))    y=(10-x)^2         residuals[0] = (tj[0] - T(t_x)) / T(var); //残差= (待优化tj-观测量t_x)/var gps的准确度方差         residuals[1] = (tj[1] - T(t_y)) / T(var);         residuals[2] = (tj[2] - T(t_z)) / T(var);           return true;     }       static ceres::CostFunction* Create(const double t_x, const double t_y, const double t_z, const double var)     {       return (new ceres::AutoDiffCostFunction<               TError, 3, 3>(                 new TError(t_x, t_y, t_z, var)));     }       double t_x, t_y, t_z, var;   };   struct RelativeRTError {     //输入观测值 i帧相对于j帧的平移和旋转     RelativeRTError(double t_x, double t_y, double t_z,                     double q_w, double q_x, double q_y, double q_z,                     double t_var, double q_var)                   :t_x(t_x), t_y(t_y), t_z(t_z),                    q_w(q_w), q_x(q_x), q_y(q_y), q_z(q_z),                    t_var(t_var), q_var(q_var){}       template <typename T>     //输出的优化参数 i帧世界旋转w_q_i i帧世界平移ti j帧世界旋转w_q_j j帧世界平移tj  残差residuals     bool operator()(const T* const w_q_i, const T* ti, const T* w_q_j, const T* tj, T* residuals) const     {         T t_w_ij[3];         t_w_ij[0] = tj[0] - ti[0]; //待优化值 j帧相对于i帧位移         t_w_ij[1] = tj[1] - ti[1]; //待优化值 j帧相对于i帧位移         t_w_ij[2] = tj[2] - ti[2]; //待优化值 j帧相对于i帧位移           T i_q_w[4];         QuaternionInverse(w_q_i, i_q_w);////待优化值 i帧世界旋转 四元数取逆           T t_i_ij[3];         //  i_q_w = 求逆（w_q_i）         //  t_w_ij 第j帧相对于第i帧的位移向量         //  t_i_ij=逆（w_q_i）* (tj[0] - ti[0])         ceres::QuaternionRotatePoint(i_q_w, t_w_ij, t_i_ij); // t_i_ij以i帧为原点 相对于j的平移  利用四元数旋转向量                   residuals[0] = (t_i_ij[0] - T(t_x)) / T(t_var); //残差 = （观测值 （全局计算i帧位姿计算出的相对于j帧的位移）- 预测值）/t_var(人为0.1)         residuals[1] = (t_i_ij[1] - T(t_y)) / T(t_var);         residuals[2] = (t_i_ij[2] - T(t_z)) / T(t_var);           T relative_q[4];         relative_q[0] = T(q_w); // 观测值 i帧相对j帧的旋转         relative_q[1] = T(q_x); // 观测值 i帧相对j帧的旋转         relative_q[2] = T(q_y); // 观测值 i帧相对j帧的旋转         relative_q[3] = T(q_z); // 观测值 i帧相对j帧的旋转           T q_i_j[4];         // q_i_j= 逆（w_q_i）* w_q_j    q_i_j待优化值计算出的i帧相对旋转j帧         ceres::QuaternionProduct(i_q_w, w_q_j, q_i_j);           T relative_q_inv[4];         //  relative_q 最新输入的数据 观测值相对旋转 relative_q求逆         QuaternionInverse(relative_q, relative_q_inv);           T error_q[4];         // error_q= relative_q_inv 观测值相对旋转的逆 * q_i_j待优化值计算出的i帧相对旋转j帧         ceres::QuaternionProduct(relative_q_inv, q_i_j, error_q);                     residuals[3] = T(2) * error_q[1] / T(q_var);//残差= error_q 观测值算出i相对于j的旋转逆*待优化值短处的相对旋转逆 *2/q_var(人为给0.01)         residuals[4] = T(2) * error_q[2] / T(q_var);         residuals[5] = T(2) * error_q[3] / T(q_var);           return true;     }       static ceres::CostFunction* Create(const double t_x, const double t_y, const double t_z,                                        const double q_w, const double q_x, const double q_y, const double q_z,                                        const double t_var, const double q_var)     {       return (new ceres::AutoDiffCostFunction<               RelativeRTError, 6, 4, 3, 4, 3>(                 new RelativeRTError(t_x, t_y, t_z, q_w, q_x, q_y, q_z, t_var, q_var)));     }       double t_x, t_y, t_z, t_norm;     double q_w, q_x, q_y, q_z;     double t_var, q_var;   };

　　

 

程序调用 

数据发布

 

 

globalOptNode.cpp

?

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175

/*******************************************************  * Copyright (C) 2019, Aerial Robotics Group, Hong Kong University of Science and Technology  *  * This file is part of VINS.  *  * Licensed under the GNU General Public License v3.0;  * you may not use this file except in compliance with the License.  *  * Author: Qin Tong (qintonguav@gmail.com)  *******************************************************/   #include "ros/ros.h" #include "globalOpt.h" #include <sensor_msgs/NavSatFix.h> #include <nav_msgs/Odometry.h> #include <nav_msgs/Path.h> #include <eigen3/Eigen/Dense> #include <eigen3/Eigen/Geometry> #include <iostream> #include <stdio.h> #include <visualization_msgs/Marker.h> #include <visualization_msgs/MarkerArray.h> #include <fstream> #include <queue> #include <mutex>   GlobalOptimization globalEstimator; ros::Publisher pub_global_odometry, pub_global_path, pub_car; nav_msgs::Path *global_path; double last_vio_t = -1; std::queue<sensor_msgs::NavSatFixConstPtr> gpsQueue; std::mutex m_buf;   void publish_car_model(double t, Eigen::Vector3d t_w_car, Eigen::Quaterniond q_w_car) {     visualization_msgs::MarkerArray markerArray_msg;     visualization_msgs::Marker car_mesh;     car_mesh.header.stamp = ros::Time(t);     car_mesh.header.frame_id = "world";     car_mesh.type = visualization_msgs::Marker::MESH_RESOURCE;     car_mesh.action = visualization_msgs::Marker::ADD;     car_mesh.id = 0;       car_mesh.mesh_resource = "package://global_fusion/models/car.dae";       Eigen::Matrix3d rot;     rot << 0, 0, -1, 0, -1, 0, -1, 0, 0;           Eigen::Quaterniond Q;     Q = q_w_car * rot;     car_mesh.pose.position.x    = t_w_car.x();     car_mesh.pose.position.y    = t_w_car.y();     car_mesh.pose.position.z    = t_w_car.z();     car_mesh.pose.orientation.w = Q.w();     car_mesh.pose.orientation.x = Q.x();     car_mesh.pose.orientation.y = Q.y();     car_mesh.pose.orientation.z = Q.z();       car_mesh.color.a = 1.0;     car_mesh.color.r = 1.0;     car_mesh.color.g = 0.0;     car_mesh.color.b = 0.0;       float major_scale = 2.0;       car_mesh.scale.x = major_scale;     car_mesh.scale.y = major_scale;     car_mesh.scale.z = major_scale;     markerArray_msg.markers.push_back(car_mesh);     pub_car.publish(markerArray_msg); }   void GPS_callback(const sensor_msgs::NavSatFixConstPtr &GPS_msg) {     //printf("gps_callback! \n");     m_buf.lock();     gpsQueue.push(GPS_msg);     m_buf.unlock(); }   void vio_callback(const nav_msgs::Odometry::ConstPtr &pose_msg) {     //printf("vio_callback! \n");     double t = pose_msg->header.stamp.toSec();     last_vio_t = t;     Eigen::Vector3d vio_t(pose_msg->pose.pose.position.x, pose_msg->pose.pose.position.y, pose_msg->pose.pose.position.z);     Eigen::Quaterniond vio_q;     vio_q.w() = pose_msg->pose.pose.orientation.w;     vio_q.x() = pose_msg->pose.pose.orientation.x;     vio_q.y() = pose_msg->pose.pose.orientation.y;     vio_q.z() = pose_msg->pose.pose.orientation.z;     globalEstimator.inputOdom(t, vio_t, vio_q);         m_buf.lock();     while(!gpsQueue.empty())     {         sensor_msgs::NavSatFixConstPtr GPS_msg = gpsQueue.front();         double gps_t = GPS_msg->header.stamp.toSec();         printf("vio t: %f, gps t: %f \n", t, gps_t);         // 10ms sync tolerance         if(gps_t >= t - 0.01 && gps_t <= t + 0.01)         {             //printf("receive GPS with timestamp %f\n", GPS_msg->header.stamp.toSec());             double latitude = GPS_msg->latitude;             double longitude = GPS_msg->longitude;             double altitude = GPS_msg->altitude;             //int numSats = GPS_msg->status.service;             double pos_accuracy = GPS_msg->position_covariance[0];             if(pos_accuracy <= 0)                 pos_accuracy = 1;             //printf("receive covariance %lf \n", pos_accuracy);             //if(GPS_msg->status.status > 8)                 globalEstimator.inputGPS(t, latitude, longitude, altitude, pos_accuracy);             gpsQueue.pop();             break;         }         else if(gps_t < t - 0.01)             gpsQueue.pop();         else if(gps_t > t + 0.01)             break;     }     m_buf.unlock();       Eigen::Vector3d global_t;     Eigen:: Quaterniond global_q;     globalEstimator.getGlobalOdom(global_t, global_q);       nav_msgs::Odometry odometry;     odometry.header = pose_msg->header;     odometry.header.frame_id = "world";     odometry.child_frame_id = "world";     odometry.pose.pose.position.x = global_t.x();     odometry.pose.pose.position.y = global_t.y();     odometry.pose.pose.position.z = global_t.z();     odometry.pose.pose.orientation.x = global_q.x();     odometry.pose.pose.orientation.y = global_q.y();     odometry.pose.pose.orientation.z = global_q.z();     odometry.pose.pose.orientation.w = global_q.w();     pub_global_odometry.publish(odometry);     pub_global_path.publish(*global_path);     publish_car_model(t, global_t, global_q);         // write result to file     std::ofstream foutC("/home/tony-ws1/output/vio_global.csv", ios::app);     foutC.setf(ios::fixed, ios::floatfield);     foutC.precision(0);     foutC << pose_msg->header.stamp.toSec() * 1e9 << ",";     foutC.precision(5);     foutC << global_t.x() << ","             << global_t.y() << ","             << global_t.z() << ","             << global_q.w() << ","             << global_q.x() << ","             << global_q.y() << ","             << global_q.z() << endl;     foutC.close(); }   int main(int argc, char **argv) {     ros::init(argc, argv, "globalEstimator");     ros::NodeHandle n("~");       global_path = &globalEstimator.global_path;       ros::Subscriber sub_GPS = n.subscribe("/gps", 100, GPS_callback);     ros::Subscriber sub_vio = n.subscribe("/vins_estimator/odometry", 100, vio_callback);     pub_global_path = n.advertise<nav_msgs::Path>("global_path", 100);     pub_global_odometry = n.advertise<nav_msgs::Odometry>("global_odometry", 100);     pub_car = n.advertise<visualization_msgs::MarkerArray>("car_model", 1000);     ros::spin();     return 0; }

　　

 

cere优化过程

globalOpt.cpp

?

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280

/*******************************************************  * Copyright (C) 2019, Aerial Robotics Group, Hong Kong University of Science and Technology  *  * This file is part of VINS.  *  * Licensed under the GNU General Public License v3.0;  * you may not use this file except in compliance with the License.  *  * Author: Qin Tong (qintonguav@gmail.com)  *******************************************************/   #include "globalOpt.h" #include "Factors.h"   GlobalOptimization::GlobalOptimization() {     initGPS = false;     newGPS = false;     WGPS_T_WVIO = Eigen::Matrix4d::Identity();     threadOpt = std::thread(&GlobalOptimization::optimize, this); }   GlobalOptimization::~GlobalOptimization() {     threadOpt.detach(); }   void GlobalOptimization::GPS2XYZ(double latitude, double longitude, double altitude, double* xyz) {     if(!initGPS)     {         geoConverter.Reset(latitude, longitude, altitude);         initGPS = true;     }     geoConverter.Forward(latitude, longitude, altitude, xyz[0], xyz[1], xyz[2]);     //printf("la: %f lo: %f al: %f\n", latitude, longitude, altitude);     //printf("gps x: %f y: %f z: %f\n", xyz[0], xyz[1], xyz[2]); }   void GlobalOptimization::inputOdom(double t, Eigen::Vector3d OdomP, Eigen::Quaterniond OdomQ) {     mPoseMap.lock();     vector<double> localPose{OdomP.x(), OdomP.y(), OdomP.z(),                              OdomQ.w(), OdomQ.x(), OdomQ.y(), OdomQ.z()};     localPoseMap[t] = localPose;       //localPose相对于上一帧的位移和旋转     Eigen::Quaterniond globalQ;     // WGPS_T_WVIO  把局部VIO位姿转化到GPS全局以第一帧GPS为原点的ENU平面直角坐标系     // WGPS_T_WVIO  GPS（ENU平面直角坐标系）到VIO（平年坐标系）转换 每次优化后要实时更新     globalQ = WGPS_T_WVIO.block<3, 3>(0, 0) * OdomQ;     Eigen::Vector3d globalP = WGPS_T_WVIO.block<3, 3>(0, 0) * OdomP + WGPS_T_WVIO.block<3, 1>(0, 3);     vector<double> globalPose{globalP.x(), globalP.y(), globalP.z(),                               globalQ.w(), globalQ.x(), globalQ.y(), globalQ.z()};     globalPoseMap[t] = globalPose;     lastP = globalP;     lastQ = globalQ;       geometry_msgs::PoseStamped pose_stamped;     pose_stamped.header.stamp = ros::Time(t);     pose_stamped.header.frame_id = "world";     pose_stamped.pose.position.x = lastP.x();     pose_stamped.pose.position.y = lastP.y();     pose_stamped.pose.position.z = lastP.z();     pose_stamped.pose.orientation.x = lastQ.x();     pose_stamped.pose.orientation.y = lastQ.y();     pose_stamped.pose.orientation.z = lastQ.z();     pose_stamped.pose.orientation.w = lastQ.w();     global_path.header = pose_stamped.header;     global_path.poses.push_back(pose_stamped);       mPoseMap.unlock(); }   void GlobalOptimization::getGlobalOdom(Eigen::Vector3d &odomP, Eigen::Quaterniond &odomQ) {     odomP = lastP;     odomQ = lastQ; }   void GlobalOptimization::inputGPS(double t, double latitude, double longitude, double altitude, double posAccuracy) {     double xyz[3];     // gps转化到以第一帧gps为原点的ENU平面直角坐标系     GPS2XYZ(latitude, longitude, altitude, xyz);     vector<double> tmp{xyz[0], xyz[1], xyz[2], posAccuracy};     //printf("new gps: t: %f x: %f y: %f z:%f \n", t, tmp[0], tmp[1], tmp[2]);     GPSPositionMap[t] = tmp;     newGPS = true;   }   /// @brief void GlobalOptimization::optimize() {     while(true)     {         if(newGPS)         {             newGPS = false;             printf("global optimization\n");             TicToc globalOptimizationTime;               ceres::Problem problem;             ceres::Solver::Options options;             options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;             //options.minimizer_progress_to_stdout = true;             //options.max_solver_time_in_seconds = SOLVER_TIME * 3;             options.max_num_iterations = 5;             ceres::Solver::Summary summary;             ceres::LossFunction *loss_function;             loss_function = new ceres::HuberLoss(1.0);             ceres::LocalParameterization* local_parameterization = new ceres::QuaternionParameterization();               //add param             mPoseMap.lock();             int length = localPoseMap.size();             // w^t_i   w^q_i             double t_array[length][3];             double q_array[length][4];             map<double, vector<double>>::iterator iter;             iter = globalPoseMap.begin();             for (int i = 0; i < length; i++, iter++)             {                 t_array[i][0] = iter->second[0];                 t_array[i][1] = iter->second[1];                 t_array[i][2] = iter->second[2];                 q_array[i][0] = iter->second[3];                 q_array[i][1] = iter->second[4];                 q_array[i][2] = iter->second[5];                 q_array[i][3] = iter->second[6];                 problem.AddParameterBlock(q_array[i], 4, local_parameterization);                 problem.AddParameterBlock(t_array[i], 3);             }               map<double, vector<double>>::iterator iterVIO, iterVIONext, iterGPS;             int i = 0;             for (iterVIO = localPoseMap.begin(); iterVIO != localPoseMap.end(); iterVIO++, i++)             {                 //vio factor                 iterVIONext = iterVIO;                 iterVIONext++;                 if(iterVIONext != localPoseMap.end())                 {                     Eigen::Matrix4d wTi = Eigen::Matrix4d::Identity();                     Eigen::Matrix4d wTj = Eigen::Matrix4d::Identity();                     wTi.block<3, 3>(0, 0) = Eigen::Quaterniond(iterVIO->second[3], iterVIO->second[4],                                                                iterVIO->second[5], iterVIO->second[6]).toRotationMatrix();                     wTi.block<3, 1>(0, 3) = Eigen::Vector3d(iterVIO->second[0], iterVIO->second[1], iterVIO->second[2]);                     wTj.block<3, 3>(0, 0) = Eigen::Quaterniond(iterVIONext->second[3], iterVIONext->second[4],                                                                iterVIONext->second[5], iterVIONext->second[6]).toRotationMatrix();                     wTj.block<3, 1>(0, 3) = Eigen::Vector3d(iterVIONext->second[0], iterVIONext->second[1], iterVIONext->second[2]);                     Eigen::Matrix4d   = wTi.inverse() * wTj;                     Eigen::Quaterniond iQj;                     iQj = iTj.block<3, 3>(0, 0);                     Eigen::Vector3d iPj = iTj.block<3, 1>(0, 3);                       ceres::CostFunction* vio_function = RelativeRTError::Create(iPj.x(), iPj.y(), iPj.z(),                                                                                 iQj.w(), iQj.x(), iQj.y(), iQj.z(),                                                                                 0.1, 0.01);                     //代价函数(cost function)、损失函数(loss function) 和 待优化状态量                     problem.AddResidualBlock(vio_function, NULL, q_array[i], t_array[i], q_array[i+1], t_array[i+1]);                       /*                     double **para = new double *[4];                     para[0] = q_array[i];                     para[1] = t_array[i];                     para[3] = q_array[i+1];                     para[4] = t_array[i+1];                       double *tmp_r = new double[6];                     double **jaco = new double *[4];                     jaco[0] = new double[6 * 4];                     jaco[1] = new double[6 * 3];                     jaco[2] = new double[6 * 4];                     jaco[3] = new double[6 * 3];                     vio_function->Evaluate(para, tmp_r, jaco);                       std::cout << Eigen::Map<Eigen::Matrix<double, 6, 1>>(tmp_r).transpose() << std::endl                         << std::endl;                     std::cout << Eigen::Map<Eigen::Matrix<double, 6, 4, Eigen::RowMajor>>(jaco[0]) << std::endl                         << std::endl;                     std::cout << Eigen::Map<Eigen::Matrix<double, 6, 3, Eigen::RowMajor>>(jaco[1]) << std::endl                         << std::endl;                     std::cout << Eigen::Map<Eigen::Matrix<double, 6, 4, Eigen::RowMajor>>(jaco[2]) << std::endl                         << std::endl;                     std::cout << Eigen::Map<Eigen::Matrix<double, 6, 3, Eigen::RowMajor>>(jaco[3]) << std::endl                         << std::endl;                     */                   }                 //gps factor                 double t = iterVIO->first;                 iterGPS = GPSPositionMap.find(t);                 //// GPSPositionMap 格式  map<double, vector<double>>  xyz[0], xyz[1], xyz[2], posAccuracy                 //double t_x, double t_y, double t_z, double var                 if (iterGPS != GPSPositionMap.end())                 {                       //double t_x, double t_y, double t_z, double var                     ceres::CostFunction* gps_function = TError::Create(iterGPS->second[0], iterGPS->second[1],                                                                        iterGPS->second[2], iterGPS->second[3]);                     //printf("inverse weight %f \n", iterGPS->second[3]);                     problem.AddResidualBlock(gps_function, loss_function, t_array[i]);                       /*                     double **para = new double *[1];                     para[0] = t_array[i];                       double *tmp_r = new double[3];                     double **jaco = new double *[1];                     jaco[0] = new double[3 * 3];                     gps_function->Evaluate(para, tmp_r, jaco);                       std::cout << Eigen::Map<Eigen::Matrix<double, 3, 1>>(tmp_r).transpose() << std::endl                         << std::endl;                     std::cout << Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor>>(jaco[0]) << std::endl                         << std::endl;                     */                 }               }             //mPoseMap.unlock();             ceres::Solve(options, &problem, &summary);             //std::cout << summary.BriefReport() << "\n";               // update global pose             //mPoseMap.lock();             iter = globalPoseMap.begin();             for (int i = 0; i < length; i++, iter++)             {   // enu平面坐标系                 vector<double> globalPose{t_array[i][0], t_array[i][1], t_array[i][2],                                           q_array[i][0], q_array[i][1], q_array[i][2], q_array[i][3]};                 iter->second = globalPose;                 if(i == length - 1)                 {                     Eigen::Matrix4d WVIO_T_body = Eigen::Matrix4d::Identity();                     Eigen::Matrix4d WGPS_T_body = Eigen::Matrix4d::Identity();                     double t = iter->first;                     //  VIO点的局部坐标系                     WVIO_T_body.block<3, 3>(0, 0) = Eigen::Quaterniond(localPoseMap[t][3], localPoseMap[t][4],                                                                        localPoseMap[t][5], localPoseMap[t][6]).toRotationMatrix();                     WVIO_T_body.block<3, 1>(0, 3) = Eigen::Vector3d(localPoseMap[t][0], localPoseMap[t][1], localPoseMap[t][2]);                     //  GPS的世界坐标系                     WGPS_T_body.block<3, 3>(0, 0) = Eigen::Quaterniond(globalPose[3], globalPose[4],                                                                         globalPose[5], globalPose[6]).toRotationMatrix();                     WGPS_T_body.block<3, 1>(0, 3) = Eigen::Vector3d(globalPose[0], globalPose[1], globalPose[2]);                     // 两个点的 变换矩阵                     WGPS_T_WVIO = WGPS_T_body * WVIO_T_body.inverse();                 }             }             updateGlobalPath();             //printf("global time %f \n", globalOptimizationTime.toc());             mPoseMap.unlock();         }         std::chrono::milliseconds dura(2000);         std::this_thread::sleep_for(dura);     }     return; }     void GlobalOptimization::updateGlobalPath() {     global_path.poses.clear();     map<double, vector<double>>::iterator iter;     for (iter = globalPoseMap.begin(); iter != globalPoseMap.end(); iter++)     {         geometry_msgs::PoseStamped pose_stamped;         pose_stamped.header.stamp = ros::Time(iter->first);         pose_stamped.header.frame_id = "world";         pose_stamped.pose.position.x = iter->second[0];         pose_stamped.pose.position.y = iter->second[1];         pose_stamped.pose.position.z = iter->second[2];         pose_stamped.pose.orientation.w = iter->second[3];         pose_stamped.pose.orientation.x = iter->second[4];         pose_stamped.pose.orientation.y = iter->second[5];         pose_stamped.pose.orientation.z = iter->second[6];         global_path.poses.push_back(pose_stamped);     } }

　　

一些涉及到gps的转换nuv关系

 

LocalCartesian.cpp

 

 

?

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62

/**  * \file LocalCartesian.cpp  * \brief Implementation for GeographicLib::LocalCartesian class  *  * Copyright (c) Charles Karney (2008-2015) <charles@karney.com> and licensed  * under the MIT/X11 License.  For more information, see  * https://geographiclib.sourceforge.io/  **********************************************************************/   #include "LocalCartesian.hpp"   namespace GeographicLib {     using namespace std;     void LocalCartesian::Reset(real lat0, real lon0, real h0) {     _lat0 = Math::LatFix(lat0);     _lon0 = Math::AngNormalize(lon0);     _h0 = h0;     _earth.Forward(_lat0, _lon0, _h0, _x0, _y0, _z0);     real sphi, cphi, slam, clam;     Math::sincosd(_lat0, sphi, cphi);     Math::sincosd(_lon0, slam, clam);     Geocentric::Rotation(sphi, cphi, slam, clam, _r);   }     void LocalCartesian::MatrixMultiply(real M[dim2_]) const {     // M = r' . M     real t[dim2_];     copy(M, M + dim2_, t);     for (size_t i = 0; i < dim2_; ++i) {       size_t row = i / dim_, col = i % dim_;       M[i] = _r[row] * t[col] + _r[row+3] * t[col+3] + _r[row+6] * t[col+6];     }   }     void LocalCartesian::IntForward(real lat, real lon, real h,                                   real& x, real& y, real& z,                                   real M[dim2_]) const {     real xc, yc, zc;     _earth.IntForward(lat, lon, h, xc, yc, zc, M);     xc -= _x0; yc -= _y0; zc -= _z0;     x = _r[0] * xc + _r[3] * yc + _r[6] * zc;     y = _r[1] * xc + _r[4] * yc + _r[7] * zc;     z = _r[2] * xc + _r[5] * yc + _r[8] * zc;     if (M)       MatrixMultiply(M);   }     void LocalCartesian::IntReverse(real x, real y, real z,                                   real& lat, real& lon, real& h,                                   real M[dim2_]) const {     real       xc = _x0 + _r[0] * x + _r[1] * y + _r[2] * z,       yc = _y0 + _r[3] * x + _r[4] * y + _r[5] * z,       zc = _z0 + _r[6] * x + _r[7] * y + _r[8] * z;     _earth.IntReverse(xc, yc, zc, lat, lon, h, M);     if (M)       MatrixMultiply(M);   }   } // namespace GeographicLib