Vehicle-cpp/Nav/1220/geo_pos_conv.cpp

/*
 * Copyright 2015-2019 Autoware Foundation. All rights reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <gnss/geo_pos_conv.hpp>
#include <iostream>
#include <cmath>
#include <eigen3/Eigen/Dense>

using namespace Eigen;
using namespace std;

geo_pos_conv::geo_pos_conv()
    : m_x(0)
    , m_y(0)
    , m_z(0)
    , m_lat(0)
    , m_lon(0)
    , m_h(0)
    , m_PLato(0)
    , m_PLo(0)
{
}

double geo_pos_conv::x() const
{
  return m_x;
}

double geo_pos_conv::y() const
{
  return m_y;
}

double geo_pos_conv::z() const
{
  return m_z;
}

void geo_pos_conv::set_plane(double lat, double lon)
{
  m_PLato = lat;
  m_PLo = lon;
}

void geo_pos_conv::set_plane(int num)
{
  int lon_deg, lon_min, lat_deg, lat_min;  // longitude and latitude of origin of each plane in Japan
  if (num == 0)
  {
    lon_deg = 0;
    lon_min = 0;
    lat_deg = 0;
    lat_min = 0;
  }
  else if (num == 1)
  {
    lon_deg = 33;
    lon_min = 0;
    lat_deg = 129;
    lat_min = 30;
  }
  else if (num == 2)
  {
    lon_deg = 33;
    lon_min = 0;
    lat_deg = 131;
    lat_min = 0;
  }
  else if (num == 3)
  {
    lon_deg = 36;
    lon_min = 0;
    lat_deg = 132;
    lat_min = 10;
  }
  else if (num == 4)
  {
    lon_deg = 33;
    lon_min = 0;
    lat_deg = 133;
    lat_min = 30;
  }
  else if (num == 5)
  {
    lon_deg = 36;
    lon_min = 0;
    lat_deg = 134;
    lat_min = 20;
  }
  else if (num == 6)
  {
    lon_deg = 36;
    lon_min = 0;
    lat_deg = 136;
    lat_min = 0;
  }
  else if (num == 7)
  {
    lon_deg = 36;
    lon_min = 0;
    lat_deg = 137;
    lat_min = 10;
  }
  else if (num == 8)
  {
    lon_deg = 36;
    lon_min = 0;
    lat_deg = 138;
    lat_min = 30;
  }
  else if (num == 9)
  {
    lon_deg = 36;
    lon_min = 0;
    lat_deg = 139;
    lat_min = 50;
  }
  else if (num == 10)
  {
    lon_deg = 40;
    lon_min = 0;
    lat_deg = 140;
    lat_min = 50;
  }
  else if (num == 11)
  {
    lon_deg = 44;
    lon_min = 0;
    lat_deg = 140;
    lat_min = 15;
  }
  else if (num == 12)
  {
    lon_deg = 44;
    lon_min = 0;
    lat_deg = 142;
    lat_min = 15;
  }
  else if (num == 13)
  {
    lon_deg = 44;
    lon_min = 0;
    lat_deg = 144;
    lat_min = 15;
  }
  else if (num == 14)
  {
    lon_deg = 26;
    lon_min = 0;
    lat_deg = 142;
    lat_min = 0;
  }
  else if (num == 15)
  {
    lon_deg = 26;
    lon_min = 0;
    lat_deg = 127;
    lat_min = 30;
  }
  else if (num == 16)
  {
    lon_deg = 26;
    lon_min = 0;
    lat_deg = 124;
    lat_min = 0;
  }
  else if (num == 17)
  {
    lon_deg = 26;
    lon_min = 0;
    lat_deg = 131;
    lat_min = 0;
  }
  else if (num == 18)
  {
    lon_deg = 20;
    lon_min = 0;
    lat_deg = 136;
    lat_min = 0;
  }
  else if (num == 19)
  {
    lon_deg = 26;
    lon_min = 0;
    lat_deg = 154;
    lat_min = 0;
  }
  else if (num == 100)
  {
    // 根据组合导航信息填写
    lon_deg = 21;  // 请填写纬度的度
    lon_min = 33;   // 请填写纬度的分
    lat_deg = 108; // 请填写经度的度
    lat_min = 26;   // 请填写经度的分
  }

  // swap longitude and latitude
  m_PLo = M_PI * (static_cast<double>(lat_deg) + static_cast<double>(lat_min) / 60.0) / 180.0;
  m_PLato = M_PI * (static_cast<double>(lon_deg) + static_cast<double>(lon_min) / 60.0) / 180;
}

void geo_pos_conv::set_xyz(double cx, double cy, double cz)
{
  m_x = cx;
  m_y = cy;
  m_z = cz;
  conv_xyz2llh();
}

void geo_pos_conv::set_llh_nmea_degrees(double latd, double lond, double h)
{
  double lat, lad, lod, lon;
  // 1234.56 -> 12'34.56 -> 12+ 34.56/60

  if (latd > 0)
  {
    lad = floor(latd / 100.);
  }
  else
  {
    lad = ceil(latd / 100.);
  }
  lat = latd - lad * 100.;

  if (lond > 0)
  {
    lod = floor(lond / 100.);
  }
  else
  {
    lod = ceil(lond / 100.);
  }
  lon = lond - lod * 100.;

  // Changing Longitude and Latitude to Radians
  m_lat = (lad + lat / 60.0)  * M_PI / 180;
  m_lon = (lod + lon / 60.0) * M_PI / 180;
  m_h = h;

  conv_llh2xyz();
}

void geo_pos_conv::llh_to_xyz(double lat, double lon, double ele)
{
  m_lat = lat * M_PI / 180;
  m_lon = lon * M_PI / 180;
  m_h = ele;

  conv_llh2xyz();
}

void geo_pos_conv::conv_llh2xyz(void)
{
  double PS;   //
  double PSo;  //
  double PDL;  //
  double Pt;   //
  double PN;   //
  double PW;   //

  double PB1, PB2, PB3, PB4, PB5, PB6, PB7, PB8, PB9;
  double PA, PB, PC, PD, PE, PF, PG, PH, PI;
  double Pe;   //
  double Pet;  //
  double Pnn;  //
  double AW, FW, Pmo;

  Pmo = 0.9999;

  /*WGS84 Parameters*/
  AW = 6378137.0;            // Semimajor Axis
  FW = 1.0 / 298.257222101;  // 298.257223563 //Geometrical flattening

  Pe = static_cast<double>(std::sqrt(2.0 * FW - std::pow(FW, 2)));
  Pet = static_cast<double>(std::sqrt(std::pow(Pe, 2) / (1.0 - std::pow(Pe, 2))));

  PA = static_cast<double>(1.0 + 3.0 / 4.0 * std::pow(Pe, 2) + 45.0 / 64.0 * std::pow(Pe, 4) +
    175.0 / 256.0 * std::pow(Pe, 6) + 11025.0 / 16384.0 * std::pow(Pe, 8) + 43659.0 / 65536.0 *
    std::pow(Pe, 10) + 693693.0 / 1048576.0 * std::pow(Pe, 12) + 19324305.0 / 29360128.0 * std::pow(Pe, 14) +
    4927697775.0 / 7516192768.0 * std::pow(Pe, 16));

  PB = static_cast<double>(3.0 / 4.0 * std::pow(Pe, 2) + 15.0 / 16.0 * std::pow(Pe, 4) + 525.0 / 512.0 *
    std::pow(Pe, 6) + 2205.0 / 2048.0 * std::pow(Pe, 8) + 72765.0 / 65536.0 * std::pow(Pe, 10) + 297297.0 / 262144.0 *
    std::pow(Pe, 12) + 135270135.0 / 117440512.0 * std::pow(Pe, 14) + 547521975.0 / 469762048.0 * std::pow(Pe, 16));

  PC = static_cast<double>(15.0 / 64.0 * std::pow(Pe, 4) + 105.0 / 256.0 * std::pow(Pe, 6) + 2205.0 / 4096.0 *
    std::pow(Pe, 8) + 10395.0 / 16384.0 * std::pow(Pe, 10) + 1486485.0 / 2097152.0 * std::pow(Pe, 12) +
    45090045.0 / 58720256.0 * std::pow(Pe, 14) + 766530765.0 / 939524096.0 * std::pow(Pe, 16));

  PD = static_cast<double>(35.0 / 512.0 * std::pow(Pe, 6) + 315.0 / 2048.0 * std::pow(Pe, 8) + 31185.0 / 131072.0 *
    std::pow(Pe, 10) + 165165.0 / 524288.0 * std::pow(Pe, 12) + 45090045.0 / 117440512.0 * std::pow(Pe, 14) +
    209053845.0 / 469762048.0 * std::pow(Pe, 16));

  PE = static_cast<double>(315.0 / 16384.0 * std::pow(Pe, 8) + 3465.0 / 65536.0 * std::pow(Pe, 10) +
    99099.0 / 1048576.0 * std::pow(Pe, 12) + 4099095.0 / 29360128.0 * std::pow(Pe, 14) + 348423075.0 / 1879048192.0 *
    std::pow(Pe, 16));

  PF = static_cast<double>(693.0 / 131072.0 * std::pow(Pe, 10) + 9009.0 / 524288.0 * std::pow(Pe, 12) +
    4099095.0 / 117440512.0 * std::pow(Pe, 14) + 26801775.0 / 469762048.0 * std::pow(Pe, 16));

  PG = static_cast<double>(3003.0 / 2097152.0 * std::pow(Pe, 12) + 315315.0 / 58720256.0 * std::pow(Pe, 14) +
    11486475.0 / 939524096.0 * std::pow(Pe, 16));

  PH = static_cast<double>(45045.0 / 117440512.0 * std::pow(Pe, 14) + 765765.0 / 469762048.0 * std::pow(Pe, 16));

  PI = static_cast<double>(765765.0 / 7516192768.0 * std::pow(Pe, 16));

  PB1 = static_cast<double>(AW) * (1.0 - std::pow(Pe, 2)) * PA;
  PB2 = static_cast<double>(AW) * (1.0 - std::pow(Pe, 2)) * PB / -2.0;
  PB3 = static_cast<double>(AW) * (1.0 - std::pow(Pe, 2)) * PC / 4.0;
  PB4 = static_cast<double>(AW) * (1.0 - std::pow(Pe, 2)) * PD / -6.0;
  PB5 = static_cast<double>(AW) * (1.0 - std::pow(Pe, 2)) * PE / 8.0;
  PB6 = static_cast<double>(AW) * (1.0 - std::pow(Pe, 2)) * PF / -10.0;
  PB7 = static_cast<double>(AW) * (1.0 - std::pow(Pe, 2)) * PG / 12.0;
  PB8 = static_cast<double>(AW) * (1.0 - std::pow(Pe, 2)) * PH / -14.0;
  PB9 = static_cast<double>(AW) * (1.0 - std::pow(Pe, 2)) * PI / 16.0;

  PS = static_cast<double>(PB1) * m_lat + PB2 * std::sin(2.0 * m_lat) + PB3 * std::sin(4.0 * m_lat) + PB4 *
    std::sin(6.0 * m_lat) + PB5 * std::sin(8.0 * m_lat) + PB6 * std::sin(10.0 * m_lat) + PB7 * std::sin(12.0 * m_lat) +
    PB8 * std::sin(14.0 * m_lat) + PB9 * std::sin(16.0 * m_lat);

  PSo = static_cast<double>(PB1) * m_PLato + PB2 * std::sin(2.0 * m_PLato) + PB3 * std::sin(4.0 * m_PLato) + PB4 *
    std::sin(6.0 * m_PLato) + PB5 * std::sin(8.0 * m_PLato) + PB6 * std::sin(10.0 * m_PLato) + PB7 *
    std::sin(12.0 * m_PLato) + PB8 * std::sin(14.0 * m_PLato) + PB9 * std::sin(16.0 * m_PLato);

  PDL = static_cast<double>(m_lon) - m_PLo;
  Pt = static_cast<double>(std::tan(m_lat));
  PW = static_cast<double>(std::sqrt(1.0 - std::pow(Pe, 2) * std::pow(std::sin(m_lat), 2)));
  PN = static_cast<double>(AW) / PW;
  Pnn = static_cast<double>(std::sqrt(std::pow(Pet, 2) * std::pow(std::cos(m_lat), 2)));

  m_x = static_cast<double>(((PS - PSo) + (1.0 / 2.0) * PN * std::pow(std::cos(m_lat), 2.0) * Pt * std::pow(PDL, 2.0) +
    (1.0 / 24.0) * PN * std::pow(std::cos(m_lat), 4) * Pt *
    (5.0 - std::pow(Pt, 2) + 9.0 * std::pow(Pnn, 2) + 4.0 * std::pow(Pnn, 4)) * std::pow(PDL, 4) -
    (1.0 / 720.0) * PN * std::pow(std::cos(m_lat), 6) * Pt *
    (-61.0 + 58.0 * std::pow(Pt, 2) - std::pow(Pt, 4) - 270.0 * std::pow(Pnn, 2) + 330.0 *
      std::pow(Pt, 2) * std::pow(Pnn, 2)) *
    std::pow(PDL, 6) - (1.0 / 40320.0) * PN * std::pow(std::cos(m_lat), 8) * Pt *
    (-1385.0 + 3111 * std::pow(Pt, 2) - 543 * std::pow(Pt, 4) + std::pow(Pt, 6)) * std::pow(PDL, 8)) * Pmo);

  m_y = static_cast<double>((PN * std::cos(m_lat) * PDL -
    1.0 / 6.0 * PN * std::pow(std::cos(m_lat), 3) * (-1 + std::pow(Pt, 2) - std::pow(Pnn, 2)) * std::pow(PDL, 3) -
    1.0 / 120.0 * PN * std::pow(std::cos(m_lat), 5) *
    (-5.0 + 18.0 * std::pow(Pt, 2) - std::pow(Pt, 4) - 14.0 * std::pow(Pnn, 2) + 58.0 * std::pow(Pt, 2) *
      std::pow(Pnn, 2)) * std::pow(PDL, 5) -
    1.0 / 5040.0 * PN * std::pow(std::cos(m_lat), 7) *
    (-61.0 + 479.0 * std::pow(Pt, 2) - 179.0 * std::pow(Pt, 4) + std::pow(Pt, 6)) * std::pow(PDL, 7)) * Pmo);

  m_z = m_h;
  // float x=((m_x +0.05)*100);
  // m_x=(int)x;m_x/=100;
  // float y=((m_y+0.05)*100);
  // m_y=(int)y;m_y/=100;
  // float z=((m_z+0.05)*100);
  // m_z=(int)z;m_z/=100;
  
  Matrix3f R;
  RowVector3f T;
  RowVector3f m_3;
  RowVector3f m_4;
  
  m_3 << m_y, m_x, m_z;
  
  //R << -0.99968197,0.02245818,0.01147087,
  //-0.02351187, //-0.99451152,-0.10195115,
  //0.00911827,-0.10218843,0.99472327;
  // R << 1,0,0,0,1,0,0,0,1;
  // 1213
  R << 0.99951947,0.03044596,0.0058198,-0.0309692,0.98883653,0.14575054,-0.00131732,-0.14586074,0.98930425;
  // 1209
  // R << 0.99989083,0.01341525,0.00619421,-0.01375076,0.99823686,0.05774151,-0.00540867,-0.05782038,0.99831235;
  // R <<  0.99296001,0.11836079,0.00459846,-0.11840988,0.99288611,0.01250104,-0.00308611,-0.01295754,0.99991129;
  // R << 0.99286137,0.11918958,0.00448735,-0.11923041,0.99281298,0.01031932,-0.00322515,-0.01078068,0.99993669;
  // R << 0.99084307,0.13500673,0.00178633,-0.1348253,0.99004595,-0.04038763,-0.00722115,0.03977696,0.99918249;
  // R << 0.99078697,0.13415613,-0.01852888,-0.13532921,0.9860102,-0.09731338,0.00521448,0.09892433,0.9950813;
  // float x = 3.12909;
  // float y = 3.13609;
  // // float z = -1.365258;
  // float z = -1.365258+3.1415926;
  // //std::cout << eulerAngle /* (180 / 3.14) */ << std::endl;
  // Eigen::AngleAxisf rollAngle(AngleAxisf(x, Vector3f::UnitX()));
  // Eigen::AngleAxisf pitchAngle(AngleAxisf(y, Vector3f::UnitY()));
  // Eigen::AngleAxisf yawAngle(AngleAxisf(z, Vector3f::UnitZ()));
  // // Eigen::Matrix3d rotation_matrix; 
  // R =   rollAngle  * pitchAngle  * yawAngle;
  // std::cout << R << std::endl;
  // T << 493.85435101,1293.37698787,-1140.07271488;
  // T << 0,0,0;
  // 1213
  T << -732.43173303,-1210.60356777,191.86543091;
  // 1209
  // T << -705.80646672,-1238.00285777,89.81833518;
  // T << -834.82725677,-1157.98330612,28.22562205;
  // T << -835.96328914,-1157.19952746,25.56658815;
  // T << -857.30577788,-1143.1203693,-30.77452311;
  // T << -855.39785832,-1139.69844527,-115.14625126;

  m_4 = m_3 * R.transpose() + T;

  m_x = m_4[1];
  m_y = m_4[0];
  m_z = m_4[2];
}

void geo_pos_conv::conv_xyz2llh(void)
{
  // n/a
}