util.hpp 4.88 KB
Newer Older
wangdawei's avatar
wangdawei committed
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141
#pragma once

#include <dirent.h>
#include <Eigen/Dense>

namespace juefx {

inline std::vector<std::string> split_string(const std::string &str, char tag)
{
    std::vector<std::string> list;
    std::string subStr = "";

    for (size_t i = 0; i < str.length(); i++)
    {
        if (tag == str[i])
        {
            if (tag != ' ' || !subStr.empty())
            {
                list.push_back(subStr);
                subStr.clear();
            }
        }
        else
        {
            subStr.push_back(str[i]);
        }
    }
    if (!subStr.empty())
    {
        list.push_back(subStr);
    }
    return list;
}

inline void get_filenames(std::string path, std::vector<std::string> &filenames, std::string tail = "")
{
    DIR *pDir;
    struct dirent *ptr;
    if (!(pDir = opendir(path.c_str())))
    {
        printf("path %s doesn't exist!\n", path.c_str());
        return;
    }
    while ((ptr = readdir(pDir)) != 0)
    {
        if (strcmp(ptr->d_name, ".") != 0 && strcmp(ptr->d_name, "..") != 0)
        {
            bool match = true;
            int name_len = strlen(ptr->d_name);
            int length = tail.length();
            for (int i = 0; i < length; i++) {
                if (!tail.empty() && ptr->d_name[name_len - length + i] != tail[i])
                {
                    match = false;
                    break;
                }
            }
            if (match)
                filenames.push_back(path + "/" + ptr->d_name);
        }
    }
    std::sort(filenames.begin(), filenames.end());
    closedir(pDir);
}

inline Eigen::Vector3d matrix_2_euler_rpy(const Eigen::Matrix3d mat)
{
    double temp = mat(2, 1) * mat(2, 1) + mat(2, 2) * mat(2, 2);
    double roll = atan2(mat(2, 1), mat(2, 2));
    double pitch = atan2(-mat(2, 0), sqrt(temp));
    double yaw = atan2(mat(1, 0), mat(0, 0));
    return Eigen::Vector3d(roll, pitch, yaw);
}

inline Eigen::Matrix3d euler_rpy_2_matrix(Eigen::Vector3d rpy)
{
    Eigen::AngleAxisd angle_axis(Eigen::AngleAxisd(rpy[2], Eigen::Vector3d::UnitZ()) *
                                 Eigen::AngleAxisd(rpy[1], Eigen::Vector3d::UnitY()) *
                                 Eigen::AngleAxisd(rpy[0], Eigen::Vector3d::UnitX()));
    return angle_axis.toRotationMatrix();
}

inline Eigen::Vector3d quaternion_2_angle_axis(const Eigen::Quaterniond &quaternion)
{
    Eigen::Quaterniond normalized_quaternion = quaternion.normalized();
    // We choose the quaternion with positive 'w', i.e., the one with a smaller
    // angle that represents this orientation.
    if (normalized_quaternion.w() < 0.)
    {
        // Multiply by -1. http://eigen.tuxfamily.org/bz/show_bug.cgi?id=560
        normalized_quaternion.w() = -1. * normalized_quaternion.w();
        normalized_quaternion.x() = -1. * normalized_quaternion.x();
        normalized_quaternion.y() = -1. * normalized_quaternion.y();
        normalized_quaternion.z() = -1. * normalized_quaternion.z();
    }
    // We convert the normalized_quaternion into a vector along the rotation axis
    // with length of the rotation angle.
    const double angle =
        2. * atan2(normalized_quaternion.vec().norm(), normalized_quaternion.w());
    constexpr double kCutoffAngle = 1e-7; // We linearize below this angle.
    const double scale = angle < kCutoffAngle ? 2. : angle / sin(angle / 2.);
    return Eigen::Vector3d(scale * normalized_quaternion.x(),
                           scale * normalized_quaternion.y(),
                           scale * normalized_quaternion.z());
}

inline Eigen::Quaterniond angle_axis_2_quaternion(const Eigen::Vector3d &angle_axis)
{
    double scale = 0.5;
    double w = 1.;
    constexpr double kCutoffAngle = 1e-8; // We linearize below this angle.
    if (angle_axis.squaredNorm() > kCutoffAngle)
    {
        const double norm = angle_axis.norm();
        scale = sin(norm / 2.) / norm;
        w = cos(norm / 2.);
    }
    const Eigen::Vector3d quaternion_xyz = scale * angle_axis;
    return Eigen::Quaterniond(w, quaternion_xyz.x(), quaternion_xyz.y(),
                              quaternion_xyz.z());
}

// 函数是求 目标坐标轴to 到 当前坐标轴from 的旋转
// 这个旋转等价于 将当前坐标系中的点from 变换到 目标坐标系的点to
inline Eigen::AngleAxisd get_cross_vector_rotation(const Eigen::Vector3d to, const Eigen::Vector3d from)
{
    Eigen::Vector3d normal = to.cross(from);
    double angle = atan2(normal.norm(), to.transpose() * from);
    /*    if (to == Eigen::Vector3d::UnitZ())
    {
        if ((from.x() > 0 && from.y() > 0 && !(normal.x() > 0 && normal.y() < 0)) 
             || (from.x() > 0 && from.y() < 0 && !(normal.x() < 0 && normal.y() < 0)) 
             || (from.x() < 0 && from.y() < 0 && !(normal.x() < 0 && normal.y() > 0)) 
             || (from.x() < 0 && from.y() > 0 && !(normal.x() > 0 && normal.y() > 0)))
            normal = -normal;
    }*/
    normal.normalize();
    return Eigen::AngleAxisd(angle, normal);
}

}