Apollo2.0自動駕駛之apollo/modules/prediction/common/geometry_util.cc

/****************Apollo源碼分析****************************
 Copyright 2018 The File Authors & zouyu. All Rights Reserved.
 Contact with: [email protected] 181663309504

 源碼主要是c++實現的，也有少量python，git下載幾百兆，其實代碼不太多，主要是地圖和數據了大量空間，主要程序
在apollo/modules目錄中，
 我們把它分成以下幾部分（具體說明見各目錄下的modules）：
 感知：感知當前位置，速度，障礙物等等
 Apollo/modules/perception
 預測：對場景下一步的變化做出預測
 Apollo/modules/prediction
 規劃：
 (1) 全局路徑規劃：通過起點終點計算行駛路徑
 Apollo/modules/routing
 (2) 規劃當前軌道：通過感知，預測，路徑規劃等信息計算軌道
 Apollo/modules/planning
 (3) 規劃轉換成命令：將軌道轉換成控制汽車的命令（加速，制動，轉向等）
 Apollo/modules/control
 其它
 (1) 輸入輸出
 i. Apollo/modules/drivers 設備驅動
 ii. Apollo/modules/localization 位置信息
 iii. Apollo/modules/monitor 監控模塊
 iv. Apollo/modules/canbus 與汽車硬件交互
 v. Apollo/modules/map 地圖數據
 vi. Apollo/modules/third_party_perception 三方感知器支持
 (2) 交互
 i. Apollo/modules/dreamview 可視化模塊
 ii. Apollo/modules/hmi 把汽車當前狀態顯示給用戶
 (3) 工具
 i. Apollo/modules/calibration 標註工具
 ii. Apollo/modules/common 支持其它模塊的公共工具
 iii. Apollo/modules/data 數據工具
 iv. Apollo/modules/tools 一些Python工具
 (4) 其它
 i. Apollo/modules/elo 高精度定位系統，無源碼，但有文檔
 ii. Apollo/modules/e2e 收集傳感器數據給PX2,ROS

  自動駕駛系統先通過起點終點規劃出整體路徑（routing）；然後在行駛過程中感知（perception）當前環境
（識別車輛行人路況標誌等），並預測下一步發展；然後把已知信息都傳入規劃模塊（planning），規劃出之後的軌道；
控制模塊（control）將軌道數據轉換成對車輛的控制信號，通過汽車交互模塊（canbus）控制汽車．
我覺得這裏面算法技術含量最高的是感知perception和規劃planning，具體請見本博客中各模塊的分析代碼。
/****************************************************************************************

/******************************************************************************
 * Copyright 2017 The Apollo Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/

#include "modules/perception/common/geometry_util.h"

#include "modules/common/math/math_utils.h"

namespace apollo {
namespace perception {

using pcl_util::Point;
using pcl_util::PointCloud;
using pcl_util::PointCloudPtr;

/*
 * Transform point cloud methods
 */
void TransformPointCloud(pcl_util::PointCloudPtr cloud,
                         const std::vector<int>& indices,
                         pcl_util::PointDCloud* trans_cloud) {
  if (trans_cloud->size() != indices.size()) {
    trans_cloud->resize(indices.size());
  }
  for (size_t i = 0; i < indices.size(); ++i) {
    const Point& p = cloud->at(indices[i]);
    Eigen::Vector3d v(p.x, p.y, p.z);
    pcl_util::PointD& tp = trans_cloud->at(i);
    tp.x = v.x();
    tp.y = v.y();
    tp.z = v.z();
    tp.intensity = p.intensity;
  }
}

void TransformPointCloud(pcl_util::PointCloudPtr cloud,
                         const Eigen::Matrix4d& pose_velodyne,
                         pcl_util::PointDCloudPtr trans_cloud) {
  Eigen::Matrix4d pose = pose_velodyne;

  if (trans_cloud->size() != cloud->size()) {
    trans_cloud->resize(cloud->size());
  }
  for (size_t i = 0; i < cloud->points.size(); ++i) {
    const Point& p = cloud->at(i);
    Eigen::Vector4d v(p.x, p.y, p.z, 1);
    v = pose * v;
    pcl_util::PointD& tp = trans_cloud->at(i);
    tp.x = v.x();
    tp.y = v.y();
    tp.z = v.z();
    tp.intensity = p.intensity;
  }
}

/*
 * Vector & Matrix related methods
 */
void TransAffineToMatrix4(const Eigen::Vector3d& translation,
                          const Eigen::Vector4d& rotation,
                          Eigen::Matrix4d* trans_matrix) {
  const double t_x = translation(0);
  const double t_y = translation(1);
  const double t_z = translation(2);

  const double qx = rotation(0);
  const double qy = rotation(1);
  const double qz = rotation(2);
  const double qw = rotation(3);

  (*trans_matrix) << 1 - 2 * (qy * qy + qz * qz), 2 * (qx * qy - qz * qw),
      2 * (qx * qz + qy * qw), t_x, 2 * (qx * qy + qz * qw),
      1 - 2 * (qx * qx + qz * qz), 2 * (qy * qz - qx * qw), t_y,
      2 * (qx * qz - qy * qw), 2 * (qy * qz + qx * qw),
      1 - 2 * (qx * qx + qy * qy), t_z, 0, 0, 0, 1;
}

void ComputeMostConsistentBboxDirection(const Eigen::Vector3f& previous_dir,
                                        Eigen::Vector3f* current_dir) {
  float dot_val_00 =
      previous_dir(0) * (*current_dir)(0) + previous_dir(1) * (*current_dir)(1);
  float dot_val_01 =
      previous_dir(0) * (*current_dir)(1) - previous_dir(1) * (*current_dir)(0);
  if (fabs(dot_val_00) >= fabs(dot_val_01)) {
    if (dot_val_00 < 0) {
      (*current_dir) = -(*current_dir);
    }
  } else {
    if (dot_val_01 < 0) {
      (*current_dir) =
          Eigen::Vector3f((*current_dir)(1), -(*current_dir)(0), 0);
    } else {
      (*current_dir) =
          Eigen::Vector3f(-(*current_dir)(1), (*current_dir)(0), 0);
    }
  }
}

double VectorCosTheta2dXy(const Eigen::Vector3f& v1,
                          const Eigen::Vector3f& v2) {
  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);
  return cos_theta;
}

double VectorTheta2dXy(const Eigen::Vector3f& v1, const Eigen::Vector3f& v2) {
  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);
  double sin_theta = (v1(0) * v2(1) - v1(1) * v2(0)) / (v1_len * v2_len);

  cos_theta = common::math::Clamp(cos_theta, 1.0, -1.0);

  double theta = acos(cos_theta);
  if (sin_theta < 0) {
    theta = -theta;
  }
  return theta;
}

}  // namespace perception
}  // namespace apollo

這篇博客講解了在自動駕駛感知模塊中的幾何構建。

/******************************************************************************
 * Copyright 2017 The Apollo Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/

#include "modules/perception/common/geometry_util.h"

#include "modules/common/math/math_utils.h"

namespace apollo {
namespace perception {

using pcl_util::Point;
using pcl_util::PointCloud;
using pcl_util::PointCloudPtr;

/*
 * Transform point cloud methods
 */
void TransformPointCloud(pcl_util::PointCloudPtr cloud,
 const std::vector<int>& indices,
 pcl_util::PointDCloud* trans_cloud) {
 if (trans_cloud->size() != indices.size()) {
 trans_cloud->resize(indices.size());
 }
 for (size_t i = 0; i < indices.size(); ++i) {
 const Point& p = cloud->at(indices[i]);
 Eigen::Vector3d v(p.x, p.y, p.z);
 pcl_util::PointD& tp = trans_cloud->at(i);
 tp.x = v.x();
 tp.y = v.y();
 tp.z = v.z();
 tp.intensity = p.intensity;
 }
}

void TransformPointCloud(pcl_util::PointCloudPtr cloud,
 const Eigen::Matrix4d& pose_velodyne,
 pcl_util::PointDCloudPtr trans_cloud) {
 Eigen::Matrix4d pose = pose_velodyne;

 if (trans_cloud->size() != cloud->size()) {
 trans_cloud->resize(cloud->size());
 }
 for (size_t i = 0; i < cloud->points.size(); ++i) {
 const Point& p = cloud->at(i);
 Eigen::Vector4d v(p.x, p.y, p.z, 1);
 v = pose * v;
 pcl_util::PointD& tp = trans_cloud->at(i);
 tp.x = v.x();
 tp.y = v.y();
 tp.z = v.z();
 tp.intensity = p.intensity;
 }
}

/*
 * Vector & Matrix related methods
 */
void TransAffineToMatrix4(const Eigen::Vector3d& translation,
 const Eigen::Vector4d& rotation,
 Eigen::Matrix4d* trans_matrix) {
 const double t_x = translation(0);
 const double t_y = translation(1);
 const double t_z = translation(2);

 const double qx = rotation(0);
 const double qy = rotation(1);
 const double qz = rotation(2);
 const double qw = rotation(3);

 (*trans_matrix) << 1 - 2 * (qy * qy + qz * qz), 2 * (qx * qy - qz * qw),
 2 * (qx * qz + qy * qw), t_x, 2 * (qx * qy + qz * qw),
 1 - 2 * (qx * qx + qz * qz), 2 * (qy * qz - qx * qw), t_y,
 2 * (qx * qz - qy * qw), 2 * (qy * qz + qx * qw),
 1 - 2 * (qx * qx + qy * qy), t_z, 0, 0, 0, 1;
}

void ComputeMostConsistentBboxDirection(const Eigen::Vector3f& previous_dir,
 Eigen::Vector3f* current_dir) {
 float dot_val_00 =
 previous_dir(0) * (*current_dir)(0) + previous_dir(1) * (*current_dir)(1);
 float dot_val_01 =
 previous_dir(0) * (*current_dir)(1) - previous_dir(1) * (*current_dir)(0);
 if (fabs(dot_val_00) >= fabs(dot_val_01)) {
 if (dot_val_00 < 0) {
 (*current_dir) = -(*current_dir);
 }
 } else {
 if (dot_val_01 < 0) {
 (*current_dir) =
 Eigen::Vector3f((*current_dir)(1), -(*current_dir)(0), 0);
 } else {
 (*current_dir) =
 Eigen::Vector3f(-(*current_dir)(1), (*current_dir)(0), 0);
 }
 }
}

double VectorCosTheta2dXy(const Eigen::Vector3f& v1,
 const Eigen::Vector3f& v2) {
 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);
 return cos_theta;
}

double VectorTheta2dXy(const Eigen::Vector3f& v1, const Eigen::Vector3f& v2) {
 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);
 double sin_theta = (v1(0) * v2(1) - v1(1) * v2(0)) / (v1_len * v2_len);

 cos_theta = common::math::Clamp(cos_theta, 1.0, -1.0);

 double theta = acos(cos_theta);
 if (sin_theta < 0) {
 theta = -theta;
 }
 return theta;
}

} // namespace perception
} // namespace apollo

Apollo2.0自動駕駛之apollo/modules/prediction/common/geometry_util.cc

/Apollo源碼分析************

Apollo2.0自動駕駛之apollo/modules/prediction/common/geometry_util.cc

Apollo2.0自動駕駛之apollo/modules//prediction/prediction.h

Apollo2.0自動駕駛之apollo/modules/common/vehicle_state/vehicle_state_provider_test.cc

Thingsboard開源物聯網平臺在阿里雲Ubuntu上的搭建

Apollo2.0自動駕駛之apollo/modules/common/vehicle_state/vehicle_state_provider.cc

Mac下配置sublime實現LaTeX

https://yachay.unat.edu.pe/blog/index.php?comment_area=format_blog&comment_component=blog&comment_co

linux以太網驅動總結