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