残差定义
Factors.h
/******************************************************* * 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 ([email protected]) *******************************************************/ #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
/******************************************************* * 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 ([email protected]) *******************************************************/ #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
/******************************************************* * 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 ([email protected]) *******************************************************/ #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
/** * \file LocalCartesian.cpp * \brief Implementation for GeographicLib::LocalCartesian class * * Copyright (c) Charles Karney (2008-2015) <[email protected]> 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
标签：global,Eigen,second,cereas,pose,vio,fusion,double,var From： https://www.cnblogs.com/gooutlook/p/17026445.html