util.hpp 4.89 KB
#pragma once

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

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;
            size_t name_len = strlen(ptr->d_name);
            size_t length = tail.length();
            for (size_t 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);
}