算法开发调试过程中的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.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坐标系下。
理论：
已知障碍物点在传感器坐标系下坐标为$(lase{r}_x,lase{r}_y)$，传感器坐标系原点在车体坐标系下的坐标为$(x2,y2,thet{a}_2)$,车体坐标原点在map坐标系下的坐标为$(x1,y1,thet{a}_1)$
1）先计算传感器坐标系原点在map坐标系下的位置$（x,y)$
$x=x_1+x_2\ast cos(thet{a}_1)-y_2\ast sin(thet{a}_1)$
$y=y_1+x_2\ast sin(thet{a}_1)+y_2\ast sin(thet{a}_1)$
$theta=actan2((sin(thet{a}_1+thet{a}_2)/cos(thet{a}_1+thet{a}_2))$
2）传感器坐标系下的障碍物点在map坐标系下的位置$(x_0,y_0)$
$x_0=x+lase{r}_x\ast cos(thet{a}_1+thet{a}_2)$
$y_0=y+lase{r}_y\ast sin(thet{a}_1+thet{a}_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^{\prime }+x1cos\alpha -y1sin\alpha$
$y=y^{\prime }+x1sin\alpha +y1cos\alpha$
方式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
#include
#include
#include
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