Apollo 10.0
自动驾驶开放平台
apollo::localization::msf::VoxelGridCovariance< PointT > 模板类 参考

#include <voxel_grid_covariance_hdmap.h>

类 apollo::localization::msf::VoxelGridCovariance< PointT > 继承关系图:
apollo::localization::msf::VoxelGridCovariance< PointT > 的协作图:

struct  Leaf
 

Public 类型

enum  LeafType { FEW , BAD , PLANE , LINE }
 
typedef boost::shared_ptr< pcl::VoxelGrid< PointT > > Ptr
 
typedef boost::shared_ptr< const pcl::VoxelGrid< PointT > > ConstPtr
 
typedef LeafLeafPtr
 
typedef const LeafLeafConstPtr
 

Public 成员函数

 VoxelGridCovariance ()
 
void SetMinPointPerVoxel (int min_points_per_voxel)
 
int GetMinPointPerVoxel ()
 
void SetCovEigValueInflationRatio (double min_covar_eigvalue_mult)
 
double GetCovEigValueInflationRatio ()
 
void Filter (PointCloudPtr output, bool searchable=false)
 
void Filter (bool searchable=false)
 
LeafConstPtr GetLeaf (int index)
 
LeafConstPtr GetLeaf (const PointT &p)
 
LeafConstPtr GetLeaf (const Eigen::Vector3f &p)
 
std::map< size_t, Leaf > & GetLeaves ()
 

Protected 类型

typedef pcl::traits::fieldList< PointT >::type FieldList
 
typedef pcl::PointCloud< PointT > PointCloud
 
typedef PointCloud::Ptr PointCloudPtr
 
typedef PointCloud::ConstPtr PointCloudConstPtr
 

详细描述

template<typename PointT>
class apollo::localization::msf::VoxelGridCovariance< PointT >

在文件 voxel_grid_covariance_hdmap.h73 行定义.

成员类型定义说明

◆ ConstPtr

template<typename PointT >
typedef boost::shared_ptr<const pcl::VoxelGrid<PointT> > apollo::localization::msf::VoxelGridCovariance< PointT >::ConstPtr

在文件 voxel_grid_covariance_hdmap.h104 行定义.

◆ FieldList

template<typename PointT >
typedef pcl::traits::fieldList<PointT>::type apollo::localization::msf::VoxelGridCovariance< PointT >::FieldList
protected

在文件 voxel_grid_covariance_hdmap.h97 行定义.

◆ LeafConstPtr

template<typename PointT >
typedef const Leaf* apollo::localization::msf::VoxelGridCovariance< PointT >::LeafConstPtr

在文件 voxel_grid_covariance_hdmap.h162 行定义.

◆ LeafPtr

template<typename PointT >
typedef Leaf* apollo::localization::msf::VoxelGridCovariance< PointT >::LeafPtr

在文件 voxel_grid_covariance_hdmap.h160 行定义.

◆ PointCloud

template<typename PointT >
typedef pcl::PointCloud<PointT> apollo::localization::msf::VoxelGridCovariance< PointT >::PointCloud
protected

在文件 voxel_grid_covariance_hdmap.h98 行定义.

◆ PointCloudConstPtr

template<typename PointT >
typedef PointCloud::ConstPtr apollo::localization::msf::VoxelGridCovariance< PointT >::PointCloudConstPtr
protected

在文件 voxel_grid_covariance_hdmap.h100 行定义.

◆ PointCloudPtr

template<typename PointT >
typedef PointCloud::Ptr apollo::localization::msf::VoxelGridCovariance< PointT >::PointCloudPtr
protected

在文件 voxel_grid_covariance_hdmap.h99 行定义.

◆ Ptr

template<typename PointT >
typedef boost::shared_ptr<pcl::VoxelGrid<PointT> > apollo::localization::msf::VoxelGridCovariance< PointT >::Ptr

在文件 voxel_grid_covariance_hdmap.h103 行定义.

成员枚举类型说明

◆ LeafType

构造及析构函数说明

◆ VoxelGridCovariance()

template<typename PointT >
apollo::localization::msf::VoxelGridCovariance< PointT >::VoxelGridCovariance ( )
inline

在文件 voxel_grid_covariance_hdmap.h165 行定义.

166 : searchable_(true),
167 min_points_per_voxel_(6),
168 min_covar_eigvalue_mult_(0.01),
169 leaves_(),
170 voxel_centroids_(),
171 voxel_centroidsleaf_indices_(),
172 kdtree_() {
173 downsample_all_data_ = false;
174 save_leaf_layout_ = false;
175 leaf_size_.setZero();
176 min_b_.setZero();
177 max_b_.setZero();
178 filter_name_ = "VoxelGridCovariance";
179 }

成员函数说明

◆ Filter() [1/2]

template<typename PointT >
void apollo::localization::msf::VoxelGridCovariance< PointT >::Filter ( bool  searchable = false)
inline

在文件 voxel_grid_covariance_hdmap.h215 行定义.

215 {
216 searchable_ = searchable;
217 voxel_centroids_ = PointCloudPtr(new PointCloud);
218 ApplyFilter(voxel_centroids_);
219 if (searchable_ && voxel_centroids_->size() > 0) {
220 kdtree_.setInputCloud(voxel_centroids_);
221 }
222 }

◆ Filter() [2/2]

template<typename PointT >
void apollo::localization::msf::VoxelGridCovariance< PointT >::Filter ( PointCloudPtr  output,
bool  searchable = false 
)
inline

在文件 voxel_grid_covariance_hdmap.h205 行定义.

205 {
206 searchable_ = searchable;
207 ApplyFilter(output);
208 voxel_centroids_ = PointCloudPtr(new PointCloud(*output));
209 if (searchable_ && voxel_centroids_->size() > 0) {
210 kdtree_.setInputCloud(voxel_centroids_);
211 }
212 }

◆ GetCovEigValueInflationRatio()

template<typename PointT >
double apollo::localization::msf::VoxelGridCovariance< PointT >::GetCovEigValueInflationRatio ( )
inline

在文件 voxel_grid_covariance_hdmap.h200 行定义.

200 {
201 return min_covar_eigvalue_mult_;
202 }

◆ GetLeaf() [1/3]

template<typename PointT >
LeafConstPtr apollo::localization::msf::VoxelGridCovariance< PointT >::GetLeaf ( const Eigen::Vector3f &  p)
inline

在文件 voxel_grid_covariance_hdmap.h257 行定义.

257 {
258 // Generate index associated with p
259 int ijk0 =
260 static_cast<int>(floor(p[0] * inverse_leaf_size_[0]) - min_b_[0]);
261 int ijk1 =
262 static_cast<int>(floor(p[1] * inverse_leaf_size_[1]) - min_b_[1]);
263 int ijk2 =
264 static_cast<int>(floor(p[2] * inverse_leaf_size_[2]) - min_b_[2]);
265
266 // Compute the centroid leaf index
267 int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2];
268
269 // Find leaf associated with index
270 typename std::map<size_t, Leaf>::iterator leaf_iter = leaves_.find(idx);
271 if (leaf_iter != leaves_.end()) {
272 // If such a leaf exists return the pointer to the leaf structure
273 LeafConstPtr ret(&(leaf_iter->second));
274 return ret;
275 } else {
276 return nullptr;
277 }
278 }

◆ GetLeaf() [2/3]

template<typename PointT >
LeafConstPtr apollo::localization::msf::VoxelGridCovariance< PointT >::GetLeaf ( const PointT &  p)
inline

在文件 voxel_grid_covariance_hdmap.h236 行定义.

236 {
237 // Generate index associated with p
238 int ijk0 = static_cast<int>(floor(p.x * inverse_leaf_size_[0]) - min_b_[0]);
239 int ijk1 = static_cast<int>(floor(p.y * inverse_leaf_size_[1]) - min_b_[1]);
240 int ijk2 = static_cast<int>(floor(p.z * inverse_leaf_size_[2]) - min_b_[2]);
241
242 // Compute the centroid leaf index
243 int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2];
244
245 // Find leaf associated with index
246 typename std::map<size_t, Leaf>::iterator leaf_iter = leaves_.find(idx);
247 if (leaf_iter != leaves_.end()) {
248 // If such a leaf exists return the pointer to the leaf structure
249 LeafConstPtr ret(&(leaf_iter->second));
250 return ret;
251 } else {
252 return nullptr;
253 }
254 }

◆ GetLeaf() [3/3]

template<typename PointT >
LeafConstPtr apollo::localization::msf::VoxelGridCovariance< PointT >::GetLeaf ( int  index)
inline

在文件 voxel_grid_covariance_hdmap.h225 行定义.

225 {
226 typename std::map<size_t, Leaf>::iterator leaf_iter = leaves_.find(index);
227 if (leaf_iter != leaves_.end()) {
228 LeafConstPtr ret(&(leaf_iter->second));
229 return ret;
230 } else {
231 return nullptr;
232 }
233 }

◆ GetLeaves()

template<typename PointT >
std::map< size_t, Leaf > & apollo::localization::msf::VoxelGridCovariance< PointT >::GetLeaves ( )
inline

在文件 voxel_grid_covariance_hdmap.h281 行定义.

281{ return leaves_; }

◆ GetMinPointPerVoxel()

template<typename PointT >
int apollo::localization::msf::VoxelGridCovariance< PointT >::GetMinPointPerVoxel ( )
inline

在文件 voxel_grid_covariance_hdmap.h193 行定义.

193{ return min_points_per_voxel_; }

◆ SetCovEigValueInflationRatio()

template<typename PointT >
void apollo::localization::msf::VoxelGridCovariance< PointT >::SetCovEigValueInflationRatio ( double  min_covar_eigvalue_mult)
inline

在文件 voxel_grid_covariance_hdmap.h196 行定义.

196 {
197 min_covar_eigvalue_mult_ = min_covar_eigvalue_mult;
198 }

◆ SetMinPointPerVoxel()

template<typename PointT >
void apollo::localization::msf::VoxelGridCovariance< PointT >::SetMinPointPerVoxel ( int  min_points_per_voxel)
inline

在文件 voxel_grid_covariance_hdmap.h182 行定义.

182 {
183 if (min_points_per_voxel > 2) {
184 min_points_per_voxel_ = min_points_per_voxel;
185 } else {
186 PCL_WARN("%s, Covariance need 3 pts, set min_pt_per_vexel to 3",
187 this->getClassName().c_str());
188 min_points_per_voxel_ = 3;
189 }
190 }

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