apollo::localization::msf::pyramid_map::NdtMapSingleCell类 参考

The data structure of a single ndt map cell. 更多...

#include <ndt_map_matrix.h>

apollo::localization::msf::pyramid_map::NdtMapSingleCell 的协作图:

Public 成员函数
	NdtMapSingleCell ()
	The default constructor.
void	Reset ()
	Reset to default value.
size_t	LoadBinary (const unsigned char *buf)
	Load the map cell from a binary chunk.
size_t	CreateBinary (unsigned char *buf, size_t buf_size) const
	Create the binary.
size_t	GetBinarySize () const
	Get the binary size of the object.
NdtMapSingleCell &	operator= (const NdtMapSingleCell &ref)
	Overloading the assign operator.
void	AddSample (const float intensity, const float altitude, const Eigen::Vector3f &centroid, bool is_road=false)
	Add an sample to the single 3d map cell.
void	MergeCell (const float intensity, const float intensity_var, const unsigned int road_pt_count, const unsigned int count, const Eigen::Vector3f &centroid, const Eigen::Matrix3f &centroid_cov)
	Merge two cells.
void	MergeCell (const NdtMapSingleCell &cell_new)
void	CentroidEigenSolver (const Eigen::Matrix3f &centroid_cov)

静态 Public 成员函数
static void	Reduce (NdtMapSingleCell *cell, const NdtMapSingleCell &cell_new)
	Combine two NdtMapSingleCell instances (Reduce).

Public 属性
float	intensity_ = 0
	The average intensity value.
float	intensity_var_ = 0
	The variance intensity value.
unsigned int	road_pt_count_ = 0
	The number of samples belonging to road surface.
unsigned int	count_ = 0
	The number of samples in the cell.
Eigen::Vector3f	centroid_
	the centroid of the cell.
Eigen::Matrix3f	centroid_average_cov_
	the pose covariance of the cell.
Eigen::Matrix3f	centroid_icov_
	the pose inverse covariance of the cell.
unsigned char	is_icov_available_ = 0
	the inverse covariance available flag.
const unsigned int	minimum_points_threshold_ = 6
	minimum number of points needed.

详细描述

The data structure of a single ndt map cell.

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

构造及析构函数说明

NdtMapSingleCell()

apollo::localization::msf::pyramid_map::NdtMapSingleCell::NdtMapSingleCell

(

)

The default constructor.

在文件 ndt_map_matrix.cc 第 26 行定义.

                                   {
  intensity_ = 0.0;
  intensity_var_ = 0.0;
  road_pt_count_ = 0;
  count_ = 0;
  centroid_ = Eigen::Vector3f::Zero();
  centroid_average_cov_ = Eigen::Matrix3f::Zero();
  centroid_icov_ = Eigen::Matrix3f::Identity();
  is_icov_available_ = 0;
}

成员函数说明

AddSample()

void apollo::localization::msf::pyramid_map::NdtMapSingleCell::AddSample	(	const float	intensity,
		const float	altitude,
		const Eigen::Vector3f &	centroid,
		bool	is_road = false
	)

Add an sample to the single 3d map cell.

在文件 ndt_map_matrix.cc 第 159 行定义.

                                               {
  ++count_;
  float v1 = intensity - intensity_;
  intensity_ += v1 / static_cast<float>(count_);
  float v2 = intensity - intensity_;
  intensity_var_ = (static_cast<float>(count_ - 1) * intensity_var_ + v1 * v2) /
                   static_cast<float>(count_);
 
  if (is_road) {
    ++road_pt_count_;
  }
 
  Eigen::Vector3f v3 = centroid - centroid_;
  centroid_ += v3 / static_cast<float>(count_);
  Eigen::Matrix3f v4 = centroid * centroid.transpose() - centroid_average_cov_;
  centroid_average_cov_ += v4 / static_cast<float>(count_);
  CentroidEigenSolver(centroid_average_cov_);
}

CentroidEigenSolver()

void apollo::localization::msf::pyramid_map::NdtMapSingleCell::CentroidEigenSolver

(

const Eigen::Matrix3f &

centroid_cov

)

在文件 ndt_map_matrix.cc 第 209 行定义.

                                       {
  // Contain more than five points, we calculate the eigen vector/value of cov.
  // [Magnusson 2009]
  if (count_ >= minimum_points_threshold_) {
    Eigen::Matrix3f cov = centroid_cov - centroid_ * centroid_.transpose();
    cov *= static_cast<float>((count_ - 1.0) / count_);
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigen_solver;
    eigen_solver.compute(cov);
    Eigen::Matrix3f eigen_val = eigen_solver.eigenvalues().asDiagonal();
    Eigen::Matrix3f centroid_evecs = eigen_solver.eigenvectors();
    if (eigen_val(0, 0) < 0 || eigen_val(1, 1) < 0 || eigen_val(2, 2) <= 0) {
      is_icov_available_ = 0;
      return;
    }
    // Avoid matrices near singularities (eq 6.11) [Magnusson 2009].
    float min_covar_eigvalue = 0.01f * eigen_val(2, 2);
    if (eigen_val(0, 0) < min_covar_eigvalue) {
      eigen_val(0, 0) = min_covar_eigvalue;
      if (eigen_val(1, 1) < min_covar_eigvalue) {
        eigen_val(1, 1) = min_covar_eigvalue;
      }
    }
    // Calculate inverse covariance
    centroid_icov_ =
        (centroid_evecs * eigen_val * centroid_evecs.inverse()).inverse();
    if (centroid_icov_.maxCoeff() == std::numeric_limits<float>::infinity() ||
        centroid_icov_.minCoeff() == -std::numeric_limits<float>::infinity()) {
      is_icov_available_ = 0;
      return;
    }
    // Set icov available
    is_icov_available_ = 1;
  }
}

CreateBinary()

size_t apollo::localization::msf::pyramid_map::NdtMapSingleCell::CreateBinary	(	unsigned char *	buf,
		size_t	buf_size
	)		const

Create the binary.

Serialization of the object.

参数

<buf,buf_size>	The buffer and its size.
<return>	The required or the used size of is returned.

在文件 ndt_map_matrix.cc 第 90 行定义.

                                                             {
  size_t target_size = GetBinarySize();
  if (buf_size >= target_size) {
    float* p = reinterpret_cast<float*>(buf);
    *p = intensity_;
    ++p;
    *p = intensity_var_;
    ++p;
    unsigned int* pp =
        reinterpret_cast<unsigned int*>(reinterpret_cast<void*>(p));
    *pp = road_pt_count_;
    ++pp;
    *pp = count_;
    ++pp;
    float* ppp = reinterpret_cast<float*>(reinterpret_cast<void*>(pp));
    *ppp = centroid_[0];
    ++ppp;
    *ppp = centroid_[1];
    ++ppp;
    *ppp = centroid_[2];
 
    for (unsigned int i = 0; i < 3; ++i) {
      for (unsigned int j = 0; j < 3; ++j) {
        ++ppp;
        *ppp = centroid_average_cov_(i, j);
      }
    }
    if (count_ >= minimum_points_threshold_) {
      for (unsigned int i = 0; i < 3; ++i) {
        for (unsigned int j = 0; j < 3; ++j) {
          ++ppp;
          *ppp = centroid_icov_(i, j);
        }
      }
      ++ppp;
      unsigned char* pppp = reinterpret_cast<unsigned char*>(ppp);
      *pppp = is_icov_available_;
    }
  }
  return target_size;
}

GetBinarySize()

size_t apollo::localization::msf::pyramid_map::NdtMapSingleCell::GetBinarySize

(

)

const

Get the binary size of the object.

在文件 ndt_map_matrix.cc 第 133 行定义.

                                             {
  size_t sz = sizeof(float) * 2 + sizeof(unsigned int) * 2 + sizeof(float) * 3 +
              sizeof(float) * 9;
  if (count_ >= minimum_points_threshold_) {
    sz += sizeof(float) * 9 + sizeof(unsigned char) * 1;
  }
  return sz;
}

LoadBinary()

size_t apollo::localization::msf::pyramid_map::NdtMapSingleCell::LoadBinary

(

const unsigned char *

buf

)

Load the map cell from a binary chunk.

参数

<return>

The size read (the real size of object).

在文件 ndt_map_matrix.cc 第 48 行定义.

                                                            {
  const float* f_buf = reinterpret_cast<const float*>(buf);
  intensity_ = *f_buf;
  ++f_buf;
  intensity_var_ = *f_buf;
  ++f_buf;
  const unsigned int* ui_buf = reinterpret_cast<const unsigned int*>(
      reinterpret_cast<const void*>(f_buf));
  road_pt_count_ = *ui_buf;
  ++ui_buf;
  count_ = *ui_buf;
  ++ui_buf;
  f_buf = reinterpret_cast<const float*>(reinterpret_cast<const void*>(ui_buf));
  centroid_[0] = *f_buf;
  ++f_buf;
  centroid_[1] = *f_buf;
  ++f_buf;
  centroid_[2] = *f_buf;
 
  for (unsigned int i = 0; i < 3; ++i) {
    for (unsigned int j = 0; j < 3; ++j) {
      ++f_buf;
      centroid_average_cov_(i, j) = *f_buf;
    }
  }
  if (count_ >= minimum_points_threshold_) {
    for (unsigned int i = 0; i < 3; ++i) {
      for (unsigned int j = 0; j < 3; ++j) {
        ++f_buf;
        centroid_icov_(i, j) = *f_buf;
      }
    }
    ++f_buf;
    const unsigned char* uc_buf = reinterpret_cast<const unsigned char*>(f_buf);
    is_icov_available_ = *uc_buf;
  } else {
    centroid_icov_ = Eigen::Matrix3f::Identity();
    is_icov_available_ = 0;
  }
  return GetBinarySize();
}

MergeCell() [1/2]

void apollo::localization::msf::pyramid_map::NdtMapSingleCell::MergeCell	(	const float	intensity,
		const float	intensity_var,
		const unsigned int	road_pt_count,
		const unsigned int	count,
		const Eigen::Vector3f &	centroid,
		const Eigen::Matrix3f &	centroid_cov
	)

Merge two cells.

在文件 ndt_map_matrix.cc 第 180 行定义.

                                                                    {
  unsigned int new_count = count_ + count;
  float p0 = static_cast<float>(count_) / static_cast<float>(new_count);
  float p1 = static_cast<float>(count) / static_cast<float>(new_count);
  float intensity_diff = intensity_ - intensity;
 
  intensity_ = intensity_ * p0 + intensity * p1;
  intensity_var_ = intensity_var_ * p0 + intensity_var * p1 +
                   intensity_diff * intensity_diff * p0 * p1;
 
  centroid_[0] = centroid_[0] * p0 + centroid[0] * p1;
  centroid_[1] = centroid_[1] * p0 + centroid[1] * p1;
  centroid_[2] = centroid_[2] * p0 + centroid[2] * p1;
 
  count_ = new_count;
  road_pt_count_ += road_pt_count;
}

MergeCell() [2/2]

void apollo::localization::msf::pyramid_map::NdtMapSingleCell::MergeCell

(

const NdtMapSingleCell &

cell_new

)

在文件 ndt_map_matrix.cc 第 203 行定义.

                                                                 {
  MergeCell(cell_new.intensity_, cell_new.intensity_var_,
            cell_new.road_pt_count_, cell_new.count_, cell_new.centroid_,
            cell_new.centroid_average_cov_);
}

operator=()

NdtMapSingleCell & apollo::localization::msf::pyramid_map::NdtMapSingleCell::operator=

(

const NdtMapSingleCell &

ref

)

Overloading the assign operator.

在文件 ndt_map_matrix.cc 第 142 行定义.

                                                                         {
  count_ = ref.count_;
  intensity_ = ref.intensity_;
  intensity_var_ = ref.intensity_var_;
  road_pt_count_ = ref.road_pt_count_;
  centroid_ = ref.centroid_;
  centroid_average_cov_ = ref.centroid_average_cov_;
  centroid_icov_ = ref.centroid_icov_;
  is_icov_available_ = ref.is_icov_available_;
  return *this;
}

Reduce()

void apollo::localization::msf::pyramid_map::NdtMapSingleCell::Reduce ( NdtMapSingleCell *  cell, const NdtMapSingleCell &  cell_new  )

static

Combine two NdtMapSingleCell instances (Reduce).

在文件 ndt_map_matrix.cc 第 154 行定义.

                                                                {
  cell->MergeCell(cell_new);
}

Reset()

void apollo::localization::msf::pyramid_map::NdtMapSingleCell::Reset

(

)

Reset to default value.

在文件 ndt_map_matrix.cc 第 37 行定义.

                             {
  intensity_ = 0.0;
  intensity_var_ = 0.0;
  road_pt_count_ = 0;
  count_ = 0;
  centroid_ = Eigen::Vector3f::Zero();
  centroid_average_cov_ = Eigen::Matrix3f::Zero();
  centroid_icov_ = Eigen::Matrix3f::Identity();
  is_icov_available_ = false;
}

类成员变量说明

centroid_

Eigen::Vector3f apollo::localization::msf::pyramid_map::NdtMapSingleCell::centroid_

the centroid of the cell.

在文件 ndt_map_matrix.h 第 85 行定义.

centroid_average_cov_

Eigen::Matrix3f apollo::localization::msf::pyramid_map::NdtMapSingleCell::centroid_average_cov_

the pose covariance of the cell.

在文件 ndt_map_matrix.h 第 87 行定义.

centroid_icov_

Eigen::Matrix3f apollo::localization::msf::pyramid_map::NdtMapSingleCell::centroid_icov_

the pose inverse covariance of the cell.

在文件 ndt_map_matrix.h 第 89 行定义.

count_

unsigned int apollo::localization::msf::pyramid_map::NdtMapSingleCell::count_ = 0

The number of samples in the cell.

在文件 ndt_map_matrix.h 第 82 行定义.

intensity_

float apollo::localization::msf::pyramid_map::NdtMapSingleCell::intensity_ = 0

The average intensity value.

在文件 ndt_map_matrix.h 第 76 行定义.

intensity_var_

float apollo::localization::msf::pyramid_map::NdtMapSingleCell::intensity_var_ = 0

The variance intensity value.

在文件 ndt_map_matrix.h 第 78 行定义.

is_icov_available_

unsigned char apollo::localization::msf::pyramid_map::NdtMapSingleCell::is_icov_available_ = 0

the inverse covariance available flag.

在文件 ndt_map_matrix.h 第 91 行定义.

minimum_points_threshold_

const unsigned int apollo::localization::msf::pyramid_map::NdtMapSingleCell::minimum_points_threshold_ = 6

minimum number of points needed.

在文件 ndt_map_matrix.h 第 93 行定义.

road_pt_count_

unsigned int apollo::localization::msf::pyramid_map::NdtMapSingleCell::road_pt_count_ = 0

The number of samples belonging to road surface.

在文件 ndt_map_matrix.h 第 80 行定义.

---

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

• modules/localization/msf/local_pyramid_map/ndt_map/ndt_map_matrix.h
• modules/localization/msf/local_pyramid_map/ndt_map/ndt_map_matrix.cc