算法开发调试过程中的tips（持续更新）

什么都会一点儿的自动驾驶工程狮

已于 2023-09-07 11:28:55 修改

阅读量275

点赞数

分类专栏：应用类文章标签：算法

于 2020-07-13 11:11:06 首次发布

本文链接：https://blog.csdn.net/lzy6041/article/details/107302562

版权

应用类专栏收录该内容

6 篇文章 0 订阅

订阅专栏

tip_1.根据两位姿，计算线加速度和角速度的代码表达（from cartographer）

const transform::Rigid3d& newest_pose = newest_timed_pose.pose;
const transform::Rigid3d& oldest_pose = oldest_timed_pose.pose;
linear_velocity_from_poses_ =(newest_pose.translation() - oldest_pose.translation()) / queue_delta;//线速度
angular_velocity_from_poses_=transform::RotationQuaternionToAngleAxisVector(oldest_pose.rotation().inverse() * newest_pose.rotation()) /queue_delta;//角速度

这里需要注意的是旋转增量计算方法（相对旋转用四元数乘法）

template <typename T>
Eigen::Matrix<T, 3, 1> RotationQuaternionToAngleAxisVector(const Eigen::Quaternion<T>& quaternion) 
{
  Eigen::Quaternion<T> 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.) 
  {
    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 T angle =2. * atan2(normalized_quaternion.vec().norm(), normalized_quaternion.w());
  constexpr double kCutoffAngle = 1e-7;  // We linearize below this angle.
  const T scale = angle < kCutoffAngle ? T(2.) : angle / sin(angle / 2.);
  return Eigen::Matrix<T, 3, 1>(scale * normalized_quaternion.x(),
                                scale * normalized_quaternion.y(),
                                scale * normalized_quaternion.z());
}

tip_2:传感器坐标系下的一障碍物点（x,y)转换到map坐标系下。
理论：
已知障碍物点在传感器坐标系下坐标为 $laser_x,laser_y)$ ，传感器坐标系原点在车体坐标系下的坐标为 $x2,y2,theta_2)$ ,车体坐标原点在map坐标系下的坐标为 $x1,y1,theta_1)$
1）先计算传感器坐标系原点在map坐标系下的位置 $（ x, y)$
$x=x_1+x_2*cos(theta_1)-y_2*sin(theta_1)$
$y=y_1+x_2*sin(theta_1)+y_2*sin(theta_1)$
$theta=actan2((sin(theta_1+theta_2)/cos(theta_1+theta_2))$
2）传感器坐标系下的障碍物点在map坐标系下的位置 $x_0,y_0)$
$x_0=x+laser_x*cos(theta_1+theta_2)$
$y_0=y+laser_y*sin(theta_1+theta_2)$
代码：

Eigen::Vector2f offset_r2b;//rgbd2baselink
        Eigen::Vector2f offset_b2m;//baselink2map
        double yaw_r2b;
        double yaw_b2m;
        offset_r2b[0]=RGBD_TO_BASELINK.getOrigin().x();
        offset_r2b[1]=RGBD_TO_BASELINK.getOrigin().y();
        yaw_r2b=tf::getYaw(RGBD_TO_BASELINK.getRotation());

        offset_b2m[0]=robot_position[0];
        offset_b2m[1]=robot_position[1];
        yaw_b2m=robot_angle[2];
        for(int i=0;i<ori_obs_list.size();++i)
        {
            Eigen::Vector2f tmp_center;
            tmp_center[0]=(cos(yaw_b2m)*cos(yaw_r2b) - sin(yaw_b2m)*sin(yaw_r2b))*ori_obs_list[i].bounding_box_.pose.position.x
                        -(cos(yaw_b2m)*sin(yaw_r2b) + sin(yaw_b2m)*cos(yaw_r2b))*ori_obs_list[i].bounding_box_.pose.position.y
                        +cos(yaw_b2m)*offset_r2b[0] - sin(yaw_b2m)*offset_r2b[1] + offset_b2m[0];
            tmp_center[1]=(sin(yaw_b2m)*cos(yaw_r2b) + cos(yaw_b2m)*sin(yaw_r2b))*ori_obs_list[i].bounding_box_.pose.position.x
                        -(sin(yaw_b2m)*sin(yaw_r2b) - cos(yaw_b2m)*cos(yaw_r2b))*ori_obs_list[i].bounding_box_.pose.position.y
                        +sin(yaw_b2m)*offset_r2b[0] 
         }

tip_3:车体坐标系下的一个障碍物点，转换到地图坐标系下的两种实现方式：
方式1：
在这里插入图片描述
这里借用作者陈光在《无人驾驶干货铺》中一张坐标变换的图解。

所以，这里假设车体坐标系下一个障碍物点为(x1,y1)，车辆在地图坐标系下的位姿为(x’,y’,α)。那么障碍物点在地图坐标系下的坐标(x,y)为：
$x = x^{'} + x 1 cos α - y 1 s in α$
$y = y^{'} + x 1 s in α + y 1 cos α$
方式2：利用tf::transform

tf::Vector3 p_v(x1,y1,0);
p_v=transform(p_v);//transform为车体到map的坐标变换关系
pcl::PointXYZ p_inMap;
p_inMap.x=p_inMap.getX();
p_inMap.y=p_inMap.getY();
p_inMap.z=0;

tip_4:最近在看到一些代码对Eigen::ArrayXXd的乘法产生疑问，对此作出测试：

#include<vector>
#include<map>
#include<iostream>
#include<Eigen/Core>
using namespace std;
int main()
{
    size_t n_pts = 4;
    Eigen::ArrayXXd ranges (n_pts, 2);
    Eigen::ArrayXXd output (n_pts, 2);
    Eigen::ArrayXXd co_sine_ (n_pts, 2);

    for (size_t i = 0; i < n_pts; ++i)
    {
        ranges (i, 0) = i;
        ranges (i, 1) = i;
        co_sine_ (i, 0) = cos (M_PI/2);
        co_sine_ (i, 1) = sin (M_PI/2);
    }
    output = ranges * co_sine_;
    for(size_t i=0;i<n_pts;++i)
    {
        std::cout<<output(i,0)<<","<<output(i,1)<<std::endl;
    }
    return 0;
}

结果仍然是一个4行2列的数组

0,0
6.12323e-17,1
1.22465e-16,2
1.83697e-16,3

tip_5:最近在细读lego-loam源码，看到坐标转一段，作者有这么一段：
use pcl for point cloud transformation, results are not accurate.
前面测试过pcl的转换点云用法，好像没有问题，这里也记录一下lego-loam中转换方式吧，后面再对比一下。

    {
        ctRoll = cos(tIn->roll);
        stRoll = sin(tIn->roll);

        ctPitch = cos(tIn->pitch);
        stPitch = sin(tIn->pitch);

        ctYaw = cos(tIn->yaw);
        stYaw = sin(tIn->yaw);

        tInX = tIn->x;
        tInY = tIn->y;
        tInZ = tIn->z;
    }

    pcl::PointCloud<PointType>::Ptr transformPointCloud(pcl::PointCloud<PointType>::Ptr cloudIn)
    {
	    // !!! DO NOT use pcl for point cloud transformation, results are not accurate
        // Reason: unkown
        pcl::PointCloud<PointType>::Ptr cloudOut(new pcl::PointCloud<PointType>());

        PointType *pointFrom;
        PointType pointTo;

        int cloudSize = cloudIn->points.size();
        cloudOut->resize(cloudSize);

        for (int i = 0; i < cloudSize; ++i)
        {
            pointFrom = &cloudIn->points[i];//输入点云中的每一个点
            //先绕z轴旋转
            float x1 = ctYaw * pointFrom->x - stYaw * pointFrom->y;
            float y1 = stYaw * pointFrom->x + ctYaw* pointFrom->y;
            float z1 = pointFrom->z;
            //再绕x轴旋转
            float x2 = x1;
            float y2 = ctRoll * y1 - stRoll * z1;
            float z2 = stRoll * y1 + ctRoll* z1;
            //最后绕y轴旋转，再加上平移
            pointTo.x = ctPitch * x2 + stPitch * z2 + tInX;
            pointTo.y = y2 + tInY;
            pointTo.z = -stPitch * x2 + ctPitch * z2 + tInZ;

            pointTo.intensity = pointFrom->intensity;
            cloudOut->points[i] = pointTo;
        }
        return cloudOut;
    }

tip_6:imu四元数消息->四元数存入imu_orientation->rpy

double imu_roll, imu_pitch, imu_yaw;
tf::Quaternion imu_orientation;//imu旋转四元数
tf::quaternionMsgToTF(input->orientation, imu_orientation);//imu四元数消息转化为四元数存入imu_orientation
tf::Matrix3x3(imu_orientation).getRPY(imu_roll, imu_pitch, imu_yaw);//获取imu此时的rpy

tip_7：利用OpenCV计算正交阵的特征值与特征向量
这里以LeGo-LOAM中为例:

 cv::Mat matA1;
 cv::Mat matD1;
 cv::Mat matV1;
 matA1 = cv::Mat (3, 3, CV_32F, cv::Scalar::all(0));
 matD1 = cv::Mat (1, 3, CV_32F, cv::Scalar::all(0));
 matV1 = cv::Mat (3, 3, CV_32F, cv::Scalar::all(0));

 //求矩阵matA1=[ax,ay,az]，3*3的协方差矩阵
 matA1.at<float>(0, 0) = a11; 
 matA1.at<float>(0, 1) = a12; 
 matA1.at<float>(0, 2) = a13;

 matA1.at<float>(1, 0) = a12; 
 matA1.at<float>(1, 1) = a22; 
 matA1.at<float>(1, 2) = a23;

 matA1.at<float>(2, 0) = a13; 
 matA1.at<float>(2, 1) = a23; 
 matA1.at<float>(2, 2) = a33;
 cv::eigen(matA1, matD1, matV1);//求正交阵的特征值matD1，特征向量matV1

因为求取的特征值λ1>=λ2>=λ3是按照降序排列的，所以
1.如果这是一个边缘特征，则它的一个特征值远大于其余两个；
2.如果这是一个平面特征，那么其中一个特征值远小于其余两个特征值；
tip_8：变换矩阵转transform

/**
   * @brief convert a Eigen::Matrix to TransformedStamped
   * @param stamp           timestamp
   * @param pose            pose matrix
   * @param frame_id        frame_id
   * @param child_frame_id  child_frame_id
   * @return transform
   */
  geometry_msgs::TransformStamped matrix2transform(const ros::Time& stamp, const Eigen::Matrix4f& pose, const std::string& frame_id, const std::string& child_frame_id) 
  {
    Eigen::Quaternionf quat(pose.block<3, 3>(0, 0));
    quat.normalize();
    geometry_msgs::Quaternion odom_quat;
    odom_quat.w = quat.w();
    odom_quat.x = quat.x();
    odom_quat.y = quat.y();
    odom_quat.z = quat.z();

    geometry_msgs::TransformStamped odom_trans;
    odom_trans.header.stamp = stamp;
    odom_trans.header.frame_id = frame_id;
    odom_trans.child_frame_id = child_frame_id;

    odom_trans.transform.translation.x = pose(0, 3);
    odom_trans.transform.translation.y = pose(1, 3);
    odom_trans.transform.translation.z = pose(2, 3);
    odom_trans.transform.rotation = odom_quat;

    return odom_trans;
  }

tip_8：利用tf::Transform对一个geometry_msgs:Point消息的点坐标进行转换

geometry_msgs::Point TransformPoint(const geometry_msgs::Point &in_point,const tf::Transform &in_tf)
{
	tf::Point tf_point;
	tf::PointMsgToTF(in_point,tf_point);
	tf_point=in_tf*tf_point;
	geometry_msgs::Point out_point;
	tf::pointTFToMsg(tf_point,out_point);
	return out_point;
	
}

tip9：两帧传感器之间IMU姿态积分变换

 // Extract IMU data between the two frames
  std::vector<ImuMeas> imu_frame;

  for (const auto &i : this->imu_buffer)
  {

    // IMU data between two frames is when:
    //   current frame's timestamp minus imu timestamp is positive
    //   previous frame's timestamp minus imu timestamp is negative
    //从imuBuf中取 两帧之间的 imu观测数据 imu_frame
    double curr_frame_imu_dt = this->curr_frame_stamp - i.stamp;
    double prev_frame_imu_dt = this->prev_frame_stamp - i.stamp;

    if (curr_frame_imu_dt >= 0. && prev_frame_imu_dt <= 0.)
    {
      imu_frame.push_back(i);
    }
  }

  // Sort measurements by time 观测数据 imu_frame按时间进行排序
  std::sort(imu_frame.begin(), imu_frame.end(), this->comparatorImu);

  // Relative IMU integration of gyro and accelerometer
  double curr_imu_stamp = 0.;
  double prev_imu_stamp = 0.;
  double dt;

  Eigen::Quaternionf q = Eigen::Quaternionf::Identity();

  for (uint32_t i = 0; i < imu_frame.size(); ++i)//计算两帧之间的角度变化
  {

    if (prev_imu_stamp == 0.)
    {
      prev_imu_stamp = imu_frame[i].stamp;
      continue;
    }

    // Calculate difference in imu measurement times IN SECONDS
    curr_imu_stamp = imu_frame[i].stamp;
    dt = curr_imu_stamp - prev_imu_stamp;
    prev_imu_stamp = curr_imu_stamp;

    // Relative gyro propagation quaternion dynamics
    Eigen::Quaternionf qq = q;
    q.w() -= 0.5 * (qq.x() * imu_frame[i].ang_vel.x + qq.y() * imu_frame[i].ang_vel.y + qq.z() * imu_frame[i].ang_vel.z) * dt;
    q.x() += 0.5 * (qq.w() * imu_frame[i].ang_vel.x - qq.z() * imu_frame[i].ang_vel.y + qq.y() * imu_frame[i].ang_vel.z) * dt;
    q.y() += 0.5 * (qq.z() * imu_frame[i].ang_vel.x + qq.w() * imu_frame[i].ang_vel.y - qq.x() * imu_frame[i].ang_vel.z) * dt;
    q.z() += 0.5 * (qq.x() * imu_frame[i].ang_vel.y - qq.y() * imu_frame[i].ang_vel.x + qq.w() * imu_frame[i].ang_vel.z) * dt;
  }

  // Normalize quaternion
  double norm = sqrt(q.w() * q.w() + q.x() * q.x() + q.y() * q.y() + q.z() * q.z());
  q.w() /= norm;
  q.x() /= norm;
  q.y() /= norm;
  q.z() /= norm;

  // Store IMU guess角度归一化，位置给0，进行赋值
  this->imu_SE3 = Eigen::Matrix4f::Identity();
  this->imu_SE3.block(0, 0, 3, 3) = q.toRotationMatrix();

什么都会一点儿的自动驾驶工程狮

关注

0
点赞
踩
1

收藏

觉得还不错? 一键收藏
0
评论
算法开发调试过程中的tips（持续更新）

tip_1.根据两位姿，计算线加速度和角速度的代码表达（from cartographer）const transform::Rigid3d& newest_pose = newest_timed_pose.pose;const transform::Rigid3d& oldest_pose = oldest_timed_pose.pose;linear_velocity_from_poses_ =(newest_pose.translation() - oldest_pose.tra
复制链接

扫一扫