#include <localization_pose_buffer.h>

apollo::localization::ndt::LocalizationPoseBuffer 的协作图:

Public 成员函数
EIGEN_MAKE_ALIGNED_OPERATOR_NEW	LocalizationPoseBuffer ()

	~LocalizationPoseBuffer ()

void	UpdateLidarPose (double timestamp, const Eigen::Affine3d &locator_pose, const Eigen::Affine3d &novatel_pose)
	receive a pair of lidar pose and odometry pose which almost have the same timestame

Eigen::Affine3d	UpdateOdometryPose (double timestamp, const Eigen::Affine3d &novatel_pose)
	receive an odometry pose and estimate the output pose according to last lidar pose recorded.

unsigned int	GetUsedBufferSize ()
	Get the used size of buffer

unsigned int	GetHeadIndex ()
	Get the current head of the buffer

详细描述

在文件 localization_pose_buffer.h 第 35 行定义.

构造及析构函数说明

◆ LocalizationPoseBuffer()

apollo::localization::ndt::LocalizationPoseBuffer::LocalizationPoseBuffer ( )

在文件 localization_pose_buffer.cc 第 32 行定义.

                                               {
  lidar_poses_.resize(s_buffer_size_);
  used_buffer_size_ = 0;
  head_index_ = 0;
  has_initialized_ = false;
}

◆ ~LocalizationPoseBuffer()

apollo::localization::ndt::LocalizationPoseBuffer::~LocalizationPoseBuffer ( )

在文件 localization_pose_buffer.cc 第 39 行定义.

39{}

成员函数说明

◆ GetHeadIndex()

unsigned int apollo::localization::ndt::LocalizationPoseBuffer::GetHeadIndex ( )

inline

Get the current head of the buffer

在文件 localization_pose_buffer.h 第 53 行定义.

53{ return head_index_; }

◆ GetUsedBufferSize()

unsigned int apollo::localization::ndt::LocalizationPoseBuffer::GetUsedBufferSize ( )

inline

Get the used size of buffer

在文件 localization_pose_buffer.h 第 51 行定义.

51{ return used_buffer_size_; }

◆ UpdateLidarPose()

void apollo::localization::ndt::LocalizationPoseBuffer::UpdateLidarPose	(	double	timestamp,
		const Eigen::Affine3d &	locator_pose,
		const Eigen::Affine3d &	novatel_pose
	)

receive a pair of lidar pose and odometry pose which almost have the same timestame

在文件 localization_pose_buffer.cc 第 41 行定义.

                                       {
  if (!has_initialized_) {
    lidar_poses_[head_index_].locator_pose = locator_pose;
    lidar_poses_[head_index_].locator_pose.linear() = novatel_pose.linear();
    lidar_poses_[head_index_].novatel_pose = novatel_pose;
    lidar_poses_[head_index_].timestamp = timestamp;
    ++used_buffer_size_;
    has_initialized_ = true;
  } else {
    // add 10Hz pose
    unsigned int empty_position =
        (head_index_ + used_buffer_size_) % s_buffer_size_;
    lidar_poses_[empty_position].locator_pose = locator_pose;
    lidar_poses_[empty_position].novatel_pose = novatel_pose;
    lidar_poses_[empty_position].timestamp = timestamp;
 
    if (used_buffer_size_ == s_buffer_size_) {
      head_index_ = (head_index_ + 1) % s_buffer_size_;
    } else {
      ++used_buffer_size_;
    }
  }
}

◆ UpdateOdometryPose()

Eigen::Affine3d apollo::localization::ndt::LocalizationPoseBuffer::UpdateOdometryPose	(	double	timestamp,
		const Eigen::Affine3d &	novatel_pose
	)

receive an odometry pose and estimate the output pose according to last lidar pose recorded.

在文件 localization_pose_buffer.cc 第 67 行定义.

                                                         {
  Eigen::Affine3d pose = novatel_pose;
  if (used_buffer_size_ > 0) {
    pose.translation()[0] = 0;
    pose.translation()[1] = 0;
    pose.translation()[2] = 0;
    Eigen::Affine3d predict_pose;
    double weight = 0.0;
 
    Eigen::Quaterniond novatel_quat(novatel_pose.linear());
    novatel_quat.normalize();
    common::math::EulerAnglesZXYd novatel_ev(
        novatel_quat.w(), novatel_quat.x(), novatel_quat.y(), novatel_quat.z());
    Eigen::Vector3d pose_ev;
    pose_ev[0] = 0;
    pose_ev[1] = 0;
    pose_ev[2] = 0;
 
    for (unsigned int c = 0, i = head_index_; c < used_buffer_size_;
         ++c, i = (i + 1) % s_buffer_size_) {
      predict_pose.translation() = lidar_poses_[i].locator_pose.translation() -
                                   lidar_poses_[i].novatel_pose.translation() +
                                   novatel_pose.translation();
      pose.translation() += predict_pose.translation();
 
      Eigen::Quaterniond pair_locator_quat(
          lidar_poses_[i].locator_pose.linear());
      pair_locator_quat.normalize();
      Eigen::Quaterniond pair_novatel_quat(
          lidar_poses_[i].novatel_pose.linear());
      pair_novatel_quat.normalize();
      Eigen::Quaterniond predict_pose_quat =
          pair_locator_quat * pair_novatel_quat.inverse() * novatel_quat;
      predict_pose_quat.normalized();
 
      common::math::EulerAnglesZXYd predict_pose_ev(
          predict_pose_quat.w(), predict_pose_quat.x(), predict_pose_quat.y(),
          predict_pose_quat.z());
      double predict_yaw = predict_pose_ev.yaw();
      if (novatel_ev.yaw() > 0) {
        if (novatel_ev.yaw() - predict_pose_ev.yaw() > M_PI) {
          predict_yaw = predict_pose_ev.yaw() + M_PI * 2.0;
        }
      } else {
        if (predict_pose_ev.yaw() - novatel_ev.yaw() > M_PI) {
          predict_yaw = predict_pose_ev.yaw() - M_PI * 2.0;
        }
      }
      pose_ev[2] += predict_yaw;
      weight += 1;
    }
    pose.translation() *= (1.0 / weight);
    pose_ev[0] = novatel_ev.pitch();
    pose_ev[1] = novatel_ev.roll();
    pose_ev[2] *= (1.0 / weight);
    if (std::abs(pose_ev[2]) > M_PI) {
      if (pose_ev[2] > 0) {
        pose_ev[2] -= 2.0 * M_PI;
      } else {
        pose_ev[2] += 2.0 * M_PI;
      }
    }
    common::math::EulerAnglesZXYd pose_euler(pose_ev[1], pose_ev[0],
                                             pose_ev[2]);
    Eigen::Quaterniond tmp_qbn = pose_euler.ToQuaternion();
    tmp_qbn.normalize();
    pose.linear() = tmp_qbn.toRotationMatrix();
  }
  return pose;
}

该类的文档由以下文件生成:

modules/localization/ndt/localization_pose_buffer.h
modules/localization/ndt/localization_pose_buffer.cc

Public 成员函数