add adc_math module to decition folder

src/decition/decition_brain/decition/adc_math/coordinate_conversion.h

@@ -0,0 +1,122 @@
+/****************************************
+ * Functions:  Convert coordinates between GPS (longitude, latitude) system and UTM system.
+ * Purpose:    Vehicle localization
+ * Author:     Zhao Liang
+ * Last Updated:  2021/01/20
+ * Update note:  call third-party lib 'proj4' to do the conversionseee.
+*****************************************
+*
+* Convention: East is the X-axis, North is the Y-axis, Up (sky) is the Z-axis,
+* the pair (longitude, latitude) are in degrees: longitude ~ (-180, 180) and
+* latitude ~(-90, 90).
+*
+* Online tool: https://www.latlong.net/lat-long-utm.html
+*
+* Basically the convertion procedure can be splitted into three steps:
+* 1. Declare two `projPJ` objects for the source system and the target system.
+* 2. Put your projection params in two strings.
+* 3. Call `pj_transform` to do the conversion.
+*
+* String used by Apollo:
+*
+* "+proj=longlat +ellps=GRS80 +towgs84=0,0,0,0,0,0,0 +no_defs"
+*
+* 1. +proj=longlat means we are projecting from (longitude, latitude).
+* 2. +ellps=GRS80 means the ellipsoid model is GRS80.
+* 3. +towgs84=0,0,0,0,0,0,0 gives the 7 params for datum transformation.
+* 4. +no_defs means we don't want proj to read the default config file,
+* this option is obsolete since proj 6.x
+*/
+
+#ifndef COORDINATE_CONVERSION_H
+#define COORDINATE_CONVERSION_H
+
+#include <string>
+#include <proj_api.h>
+
+namespace iv {
+namespace math {
+
+/**
+ * @brief: Convert (lon, lat) coordinates to UTM coordinates.
+ * @param: longitude in degrees, range ~ (-180, 180).
+ * @param: latitude in degrees, range ~ (-90, 90).
+ * @param: x coordinate (East direction).
+ * @param: y coordinate (North direction).
+ * @return: Return true if the conversion between the two systems is valid.
+*/
+bool LatLonToUtmXY(double longitude, double latitude, double &x, double &y)
+{
+    projPJ pj_latlon;
+    projPJ pj_utm;
+    int zone = static_cast<int>((longitude + 180) / 6) + 1;
+    std::string latlon_src =
+        "+proj=longlat +ellps=GRS80 +towgs84=0,0,0,0,0,0,0 +no_defs";
+    std::string utm_dst =
+        "+proj=utm +zone=" + std::to_string(zone) + " +ellps=GRS80 +units=m +no_defs";
+
+    if (!(pj_latlon = pj_init_plus(latlon_src.c_str())))
+    {
+        return false;
+    }
+
+    if (!(pj_utm = pj_init_plus(utm_dst.c_str())))
+    {
+        return false;
+    }
+
+    // the pj_transform requires the (lon, lat) are in radians
+    double lon = longitude * DEG_TO_RAD;
+    double lat = latitude * DEG_TO_RAD;
+
+    // do the actual conversion
+    pj_transform(pj_latlon, pj_utm, 1, 1, &lon, &lat, nullptr);
+
+    x = lon;
+    y = lat;
+    pj_free(pj_latlon);
+    pj_free(pj_utm);
+    return true;
+}
+
+/**
+ * @brief: Convert UTM coordinates to (lon, lat) coordinates.
+ * @param: x coordinate (East direction).
+ * @param: y coordinate (North direction).
+ * @param: zone number.
+ * @param: longitude in degrees, range ~ (-180, 180).
+ * @param: latitude in degrees, range ~ (-90, 90).
+ * @return: Return true if the conversion between the two systems is valid.
+*/
+bool UtmXYToLatLon(double x, double y, int zone, double &longitude, double &latitude)
+{
+    projPJ pj_latlon;
+    projPJ pj_utm;
+    std::string latlon_src =
+        "+proj=longlat +ellps=GRS80 +towgs84=0,0,0,0,0,0,0 +no_defs";
+    std::string utm_dst =
+        "+proj=utm +zone=" + std::to_string(zone) + " +ellps=GRS80 +units=m +no_defs";
+
+    if (!(pj_latlon = pj_init_plus(latlon_src.c_str())))
+    {
+        return false;
+    }
+
+    if (!(pj_utm = pj_init_plus(utm_dst.c_str())))
+    {
+        return false;
+    }
+
+    // do the actual conversion
+    pj_transform(pj_utm, pj_latlon, 1, 1, &x, &y, nullptr);
+
+    // the result given by pj_transform are in radians
+    longitude = x * RAD_TO_DEG;
+    latitude = y * RAD_TO_DEG;
+    pj_free(pj_latlon);
+    pj_free(pj_utm);
+    return true;
+}
+}
+}
+#endif // COORDINATE_CONVERSION_H

src/decition/decition_brain/decition/adc_math/vec2.h

@@ -0,0 +1,289 @@
+/****************************************
+* Class:    2D vector class
+* Purpose:  A single header file handling 2D vector computations.
+* Author:   Zhao Liang
+* Last Updated:  2021/01/20
+*****************************************
+*
+* Note: Need to add error handling procedures in divisions.
+*
+*/
+
+#ifndef VEC2_H
+#define VEC2_H
+
+#pragma once
+
+#include <cmath>
+#include <iostream>
+
+namespace iv {
+namespace math {
+
+constexpr double _EPSILON = 1e-12;
+
+class Vec2
+{
+public:
+
+    // ----------------------------------------
+
+    /*
+     * Constructors.
+    */
+
+    Vec2() : x_(0), y_(0) {}
+
+    explicit Vec2(double a) : x_(a), y_(a) {}
+
+    Vec2(double a, double b) : x_(a), y_(b) {}
+
+    // ----------------------------------------
+
+    // Compare to another vector
+    bool operator == (const Vec2 &v) const
+    {
+        return (std::abs(x_ - v.x()) < _EPSILON &&
+                std::abs(y_ - v.y()) < _EPSILON);
+    }
+
+    // ----------------------------------------
+
+    /*
+     * Return the unit vector Vec2(cos(angle), sin(angle)),
+     * The param 'angle' should be in radians.
+    */
+    static Vec2 unitVec2(const double angle)
+    {
+        return Vec2(cos(angle), sin(angle));
+    }
+
+    // ----------------------------------------
+
+    /*
+     * Access x and y components
+    */
+    double x() const { return x_; }
+    double y() const { return y_; }
+
+    // ----------------------------------------
+
+    /*
+     * Set x and y components
+    */
+    void set_x(const double x) { x_ = x; }
+    void set_y(const double y) { y_ = y; }
+
+    // ----------------------------------------
+
+    /*
+     * Operations with other scalars,
+     * vec2 is the left operand, scalar is the right operand.
+    */
+
+    // ----------------------------------------
+
+    // add a scalar
+    Vec2 operator + (double c) const { return Vec2(x() + c, y() + c); }
+    // subtract a scalar
+    Vec2 operator - (double c) const { return Vec2(x() - c, y() - c); }
+    // multiply a scalar
+    Vec2 operator * (double c) const { return Vec2(x() * c, y() * c); }
+    // divide by a scalar
+    Vec2 operator / (double c) const { return Vec2(x() / c, y() / c); }
+    // in-place add a scalar
+    Vec2 operator += (double c) { x_ += c; y_ += c; return (*this); }
+    // in-place subtract a scalar
+    Vec2 operator -= (double c) { x_ -= c; y_ -= c; return (*this); }
+    // in-place multiply a scalar
+    Vec2 operator *= (double c) { x_ *= c; y_ *= c; return (*this); }
+    // in-place divide by a scalar
+    Vec2 operator /= (double c) { x_ /= c; y_ /= c; return (*this); }
+
+    // ----------------------------------------
+
+    /*
+     * Operations with other vec2
+    */
+
+    // ----------------------------------------
+
+    // add two vectors
+    Vec2 operator + (const Vec2 &v) const { return Vec2(x() + v.x(), y() + v.y()); }
+    // subtract another vector
+    Vec2 operator - (const Vec2 &v) const { return Vec2(x() - v.x(), y() - v.y()); }
+    // multiply two vectors as complex numbers
+    Vec2 operator * (const Vec2 &v) const
+    {
+        double x1 = x() * v.x() - y() * v.y();
+        double y1 = x() * v.y() + y() * v.x();
+        return Vec2(x1, y1);
+    }
+    // division as complex numbers
+    Vec2 operator / (const Vec2 &v) const
+    {
+        double snorm = v.squared_norm();
+        return Vec2(this->dot(v) / snorm, -this->cross(v) / snorm);
+    }
+    // in-place vector addition
+    Vec2 operator += (const Vec2 &v) { x_ += v.x(); y_ += v.y(); return (*this); }
+    // in-place vector subtraction
+    Vec2 operator -= (const Vec2 &v) { x_ -= v.x(); y_ -= v.y(); return (*this); }
+    // in-place vector multiplication as complex numbers
+    Vec2 operator *= (const Vec2 &v)
+    {
+        double x1 = x() * v.x() - y() * v.y();
+        double y1 = x() * v.y() + y() * v.x();
+        x_ = x1;
+        y_ = y1;
+        return (*this);
+    }
+    // in-place vector division as complex numbers
+    Vec2 operator /= (const Vec2 &v)
+    {
+        double snorm = v.squared_norm();
+        double x1 = this->dot(v) / snorm;
+        double y1 = -this->cross(v) / snorm;
+        x_ = x1;
+        y_ = y1;
+        return (*this);
+    }
+
+    // ----------------------------------------
+
+    /*
+     * Vector util functions
+    */
+
+    // ----------------------------------------
+
+    // Return squared norm
+    double squared_norm() const { return x() * x() + y() * y(); }
+
+    // Return the usual Euclidean norm
+    double norm() const { return sqrt(x() * x() + y() * y()); }
+
+    // Inner product of two vectors
+    double dot(const Vec2 &v) const { return x() * v.x() + y() * v.y(); }
+
+    // Angle with the positive x-axis in radians
+    double angle() const { return atan2(y(), x()); }
+
+    // Angle between two vectors
+    double angle(const Vec2 &v) const
+    {
+        return acos(this->dot(v) / (norm() * v.norm()));
+    }
+
+    // Distance between two vectors
+    double dist(const Vec2 &v) const { return (*this - v).norm(); }
+
+    // Squared distance between two vectors
+    double squared_dist(const Vec2 &v) const { return (*this - v).squared_norm(); }
+
+    // Cross product of two vectors
+    double cross(const Vec2 &v) const { return x() * v.y() - y() * v.x(); }
+
+    // Return a normalized version of this vector
+    Vec2 normalize() const
+    {
+        double s = norm();
+        return (*this) / s;
+    }
+
+    // Translate a vector
+    Vec2 translate(double tx, double ty) const { return Vec2(x() + tx, y() + ty); }
+
+    // Rotate a vector by angle
+    Vec2 rotate(const double theta) const
+    {
+        double ct = cos(theta), st = sin(theta);
+        double x1 = x() * ct - y() * st;
+        double y1 = x() * ct + y() * st;
+        return Vec2(x1, y1);
+    }
+
+    // return a perpendicular vector to this one
+    Vec2 perp() const { return Vec2(-y(), x()); }
+
+private:
+    double x_;
+    double y_;
+};
+
+// ----------------------------------------
+
+/*
+ * Vec2 operations as right operand
+*/
+
+// ----------------------------------------
+
+// print formatting
+std::ostream & operator << (std::ostream &out, const Vec2 &v)
+{
+    out << "Vec2(" << v.x() << ", " << v.y() << ")";
+    return out;
+}
+
+// add another scalar
+Vec2 operator + (double c, const Vec2 &v) { return Vec2(v.x() + c, v.y() + c); }
+// subtract another scalar
+Vec2 operator - (double c, const Vec2 &v) { return Vec2(c - v.x(), c - v.y()); }
+// multiply another scalar
+Vec2 operator * (double c, const Vec2 &v) { return Vec2(v.x() * c, v.y() * c); }
+// divide another scalar
+Vec2 operator / (double c, const Vec2 &v)
+{
+    double snorm = v.squared_norm();
+    return Vec2(v.x() * c / snorm, -v.y() * c / snorm);
+}
+
+// ----------------------------------------
+
+/*
+ * Vector util functions
+*/
+
+// ----------------------------------------
+
+// Inner product
+double vdot(const Vec2 &v1, const Vec2 &v2) { return v1.dot(v2); }
+
+// Cross
+double vcross(const Vec2 &v1, const Vec2 &v2) { return v1.cross(v2); }
+
+// Vector length
+double vlength(const Vec2 &v) { return v.norm(); }
+
+// Angle with positive x-axis
+double vangle(const Vec2 &v) { return v.angle(); }
+
+// Angle between two vectors
+double vangle(const Vec2 &v1, const Vec2 &v2) { return v1.angle(v2); }
+
+// distance between two vectors
+double vdist(const Vec2 &v1, const Vec2 &v2) { return v1.dist(v2); }
+
+// squared distance between two vectors
+double vsquared_dist(const Vec2 &v1, const Vec2 &v2) { return v1.squared_dist(v2); }
+
+// normalize a vector
+Vec2 vnormalize(const Vec2 &v) { return v.normalize(); }
+
+// return a perpendicular one
+Vec2 vperp(const Vec2 &v) { return v.perp(); }
+
+// rotate a vector
+Vec2 vrotate(const Vec2 &v, double theta) { return v.rotate(theta); }
+
+// return a unit vector of given direction
+Vec2 vdir(const double angle) { return Vec2::unitVec2(angle); }
+
+// return a linear combination of two vectors
+Vec2 vinterp(const Vec2 &v1, const Vec2 &v2, double t) { return v1 * (1 - t) + v2 * t; }
+
+}
+}
+
+#endif // VEC2_H