Program Listing for File geo_handler.hpp

↰ Return to documentation for file (include/tuw_geometry/geo_handler.hpp)

#ifndef TUW_GEOMETRY__GEO_HANDLER_HPP
#define TUW_GEOMETRY__GEO_HANDLER_HPP

#include <GeographicLib/UTMUPS.hpp>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <opencv2/core/core.hpp>
#include <tuw_geometry/map_handler.hpp>
#include <tuw_geometry/pose2d.hpp>

namespace tuw
{

class WorldFile
{
public:
  WorldFile()
  : resolution_x(.0),
    rotation_y(.0),
    rotation_x(.0),
    coordinate_x(.0),
    coordinate_y(.0),
    coordinate_z(.0),
    origin_x(.0),
    origin_y(.0)
  {
  }

  double resolution_x;
  double rotation_y;
  double rotation_x;
  double resolution_y;
  double coordinate_x;
  double coordinate_y;
  double coordinate_z;
  double origin_x;
  double origin_y;

  bool write_jgw(const std::string & filename)
  {
    std::ofstream datei(filename);
    if (datei.is_open()) {
      // write six lines
      datei << std::fixed << std::setprecision(8) << resolution_x << std::endl;
      datei << std::fixed << std::setprecision(8) << rotation_y << std::endl;
      datei << std::fixed << std::setprecision(8) << rotation_x << std::endl;
      datei << std::fixed << std::setprecision(8) << resolution_y << std::endl;
      datei << std::fixed << std::setprecision(8) << coordinate_x << std::endl;
      datei << std::fixed << std::setprecision(8) << coordinate_y << std::endl;
      datei << std::fixed << std::setprecision(8) << coordinate_z << std::endl;
      datei << std::fixed << std::setprecision(8) << origin_x << std::endl;
      datei << std::fixed << std::setprecision(8) << origin_y << std::endl;
    }
  }
  bool read_jgw(const std::string & filename)
  {
    std::ifstream geo_info_file(filename.c_str());
    // Check if the file is open
    if (!geo_info_file.is_open()) {
      std::cerr << "Error opening the file!" << std::endl;
      return true;  // Return an error code
    }

    // Read six lines
    double num;
    if (geo_info_file >> num) {
      resolution_x = num;
    }
    if (geo_info_file >> num) {
      rotation_y = num;
    }
    if (geo_info_file >> num) {
      rotation_x = num;
    }
    if (geo_info_file >> num) {
      resolution_y = num;
    }
    if (geo_info_file >> num) {
      coordinate_x = num;
    }
    if (geo_info_file >> num) {
      coordinate_y = num;
    }
    try {
      if (geo_info_file >> num) {
        coordinate_z = num;
      }
      if (geo_info_file >> num) {
        origin_x = num;
      }
      if (geo_info_file >> num) {
        origin_y = num;
      }
    } catch (std::ifstream::failure & e) {
    }

    geo_info_file.close();
    return false;
  }
};

class GeoHdl : public MapHdl
{
  cv::Vec3d utm_;
  int zone_;
  bool northp_;

public:
  GeoHdl() {}

  std::string info_geo() const
  {
    char txt[0x1FF];
    cv::Vec3d lla = utm2lla(utm_);
    sprintf(
      txt,
      "GeoInfo: [%12.10f°, %12.10f°, %12.10fm] --> [%12.10fm, %12.10fm, %12.10fm] zone %d %s, "
      "origin [%5.2fm, %5.2fm]",
      lla[0], lla[1], lla[2], utm_[0], utm_[1], utm_[2], zone_, (northp_ ? "north" : "south"),
      origin_x(), origin_y());
    return txt;
  }
  void init(cv::Size canvas_size, double resolution, Origin origin, cv::Vec3d lla)
  {
    GeographicLib::UTMUPS::Forward(
      lla[0], lla[1], this->zone_, this->northp_, this->utm_[0], this->utm_[1]);
    this->utm_[2] = lla[3];
    MapHdl::init(canvas_size, resolution, origin);
  }

  void init(
    cv::Size canvas_size, double resolution, Origin origin, cv::Vec3d utm, int zone, bool northp)
  {
    this->utm_ = utm;
    this->zone_ = zone;
    this->northp_ = northp;
    MapHdl::init(canvas_size, resolution, origin);
  }

  cv::Vec3d & lla2utm(const cv::Vec3d & src, cv::Vec3d & des) const
  {
    int zone;
    double gamma, k;
    bool northp;
    GeographicLib::UTMUPS::Forward(
      src[0], src[1], zone, northp, des[0], des[1], gamma, k, this->zone_);
    des[2] = src[2];
    return des;
  }
  cv::Vec3d lla2utm(const cv::Vec3d & src) const
  {
    cv::Vec3d des;
    return lla2utm(src, des);
  }

  cv::Point & lla2map(const cv::Vec3d & src, cv::Point & des) const
  {
    cv::Vec3d utm;
    lla2utm(src, utm);
    cv::Vec3d world;
    utm2world(utm, world);
    return this->w2m(world[0], world[1]).to(des);
  }
  cv::Point lla2map(const cv::Vec3d & src) const
  {
    cv::Point des;
    return lla2map(src, des);
  }

  cv::Vec3d & utm2world(const cv::Vec3d & src, cv::Vec3d & des) const
  {
    des = src - utm_;
    return des;
  }
  cv::Vec3d utm2world(const cv::Vec3d & src) const
  {
    cv::Vec3d des;
    return utm2world(src, des);
  }
  cv::Vec3d & world2utm(const cv::Vec3d & src, cv::Vec3d & des) const
  {
    des = src + utm_;
    return des;
  }
  cv::Vec3d world2utm(const cv::Vec3d & src) const
  {
    cv::Vec3d des;
    return world2utm(src, des);
  }

  cv::Vec2d & world2map(const cv::Vec2d & src, cv::Vec2d & des) const
  {
    cv::Vec3d tmp = w2m(cv::Vec3d(src[0], src[1], 1.));
    des[0] = tmp[0], des[1] = tmp[1];
    return des;
  }
  cv::Vec2d world2map(const cv::Vec2d & src) const
  {
    cv::Vec2d des;
    return world2map(src, des);
  }

  cv::Vec3d & utm2lla(const cv::Vec3d & src, cv::Vec3d & des) const
  {
    GeographicLib::UTMUPS::Reverse(zone_, northp_, src[0], src[1], des[0], des[1]);
    des[2] = src[2];
    return des;
  }
  cv::Vec3d utm2lla(const cv::Vec3d & src) const
  {
    cv::Vec3d des;
    return utm2lla(src, des);
  }

  cv::Vec3d & map2lla(const cv::Point & src, cv::Vec3d & des) const
  {
    tuw::Point2D p_map(src.x, src.y);
    tuw::Point2D p_world;
    m2w(p_map, p_world);
    cv::Vec3d utm = world2utm(cv::Vec3d(p_world.x(), p_world.y(), 0));
    utm2lla(utm, des);
    return des;
  }
  cv::Vec3d map2lla(const cv::Point & src) const
  {
    cv::Vec3d des;
    return map2lla(src, des);
  }
  cv::Vec3d utm() {return utm_;}
  int zone() {return zone_;}

  bool is_north() {return northp_;}

  bool is_south() {return !northp_;}
};
}  // namespace tuw
#endif  // TUW_GEOMETRY__GEO_HANDLER_HPP