两个方向的夹角计算实现

两个方向的夹角计算实现：
  double v1_len = sqrt((v1.head(2).cwiseProduct(v1.head(2))).sum());
  double v2_len = sqrt((v2.head(2).cwiseProduct(v2.head(2))).sum());
  double cos_theta =
      (v1.head(2).cwiseProduct(v2.head(2))).sum() / (v1_len * v2_len);

公式来源：
https://baike.baidu.com/item/%E4%B8%89%E8%A7%92%E5%87%BD%E6%95%B0%E5%85%AC%E5%BC%8F/4374733?fr=aladdin

cos(a-b) = cos(a) * cos(b) + sin(a) * sin(b)
cos(a) = x1/v1_len;
cos(b) = x2/v2_len;

sin(a) = y1/v1_len;
sin(b) = y2/v2_len;

cos(a-b) = x1*x2/v1_len*v2_len  + y1*y2 / v1_len*v2_len
         = (x1*x2 + y1*y2)/v1_len*v2_len;