残差定义
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 (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
/*******************************************************
* 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
/*******************************************************
* 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
/**
* \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
浙公网安备 33010602011771号