123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791 |
- #include "ndt_cpu/VoxelGrid.h"
- #include "ndt_cpu/debug.h"
- #include <math.h>
- #include <limits>
- #include <inttypes.h>
- #include <vector>
- #include <cmath>
- #include <stdio.h>
- #include <sys/time.h>
- #include "ndt_cpu/SymmetricEigenSolver.h"
- namespace cpu {
- template <typename PointSourceType>
- VoxelGrid<PointSourceType>::VoxelGrid():
- voxel_num_(0),
- max_x_(FLT_MIN),
- max_y_(FLT_MIN),
- max_z_(FLT_MIN),
- min_x_(FLT_MAX),
- min_y_(FLT_MAX),
- min_z_(FLT_MAX),
- voxel_x_(0),
- voxel_y_(0),
- voxel_z_(0),
- max_b_x_(0),
- max_b_y_(0),
- max_b_z_(0),
- min_b_x_(0),
- min_b_y_(0),
- min_b_z_(0),
- vgrid_x_(0),
- vgrid_y_(0),
- vgrid_z_(0),
- min_points_per_voxel_(6),
- real_max_bx_(INT_MIN),
- real_max_by_(INT_MIN),
- real_max_bz_(INT_MIN),
- real_min_bx_(INT_MAX),
- real_min_by_(INT_MAX),
- real_min_bz_(INT_MAX)
- {
- centroid_.reset();
- icovariance_.reset();
- points_id_.reset();
- points_per_voxel_.reset();
- tmp_centroid_.reset();
- tmp_cov_.reset();
- };
- template <typename PointSourceType>
- int VoxelGrid<PointSourceType>::roundUp(int input, int factor)
- {
- return (input < 0) ? -((-input) / factor) * factor : ((input + factor - 1) / factor) * factor;
- }
- template <typename PointSourceType>
- int VoxelGrid<PointSourceType>::roundDown(int input, int factor)
- {
- return (input < 0) ? -((-input + factor - 1) / factor) * factor : (input / factor) * factor;
- }
- template <typename PointSourceType>
- int VoxelGrid<PointSourceType>::div(int input, int divisor)
- {
- return (input < 0) ? -((-input + divisor - 1) / divisor) : input / divisor;
- }
- template <typename PointSourceType>
- void VoxelGrid<PointSourceType>::initialize()
- {
- centroid_.reset();
- centroid_ = boost::make_shared<std::vector<Eigen::Vector3d> >(voxel_num_);
- icovariance_.reset();
- icovariance_ = boost::make_shared<std::vector<Eigen::Matrix3d> >(voxel_num_);
- points_id_.reset();
- points_id_ = boost::make_shared<std::vector<std::vector<int> > >(voxel_num_);
- points_per_voxel_.reset();
- points_per_voxel_ = boost::make_shared<std::vector<int> >(voxel_num_, 0);
- tmp_centroid_.reset();
- tmp_centroid_ = boost::make_shared<std::vector<Eigen::Vector3d> >(voxel_num_);
- tmp_cov_.reset();
- tmp_cov_ = boost::make_shared<std::vector<Eigen::Matrix3d> >(voxel_num_);
- }
- template <typename PointSourceType>
- int VoxelGrid<PointSourceType>::getVoxelNum() const
- {
- return voxel_num_;
- }
- template <typename PointSourceType>
- float VoxelGrid<PointSourceType>::getMaxX() const
- {
- return max_x_;
- }
- template <typename PointSourceType>
- float VoxelGrid<PointSourceType>::getMaxY() const
- {
- return max_y_;
- }
- template <typename PointSourceType>
- float VoxelGrid<PointSourceType>::getMaxZ() const
- {
- return max_z_;
- }
- template <typename PointSourceType>
- float VoxelGrid<PointSourceType>::getMinX() const
- {
- return min_x_;
- }
- template <typename PointSourceType>
- float VoxelGrid<PointSourceType>::getMinY() const
- {
- return min_y_;
- }
- template <typename PointSourceType>
- float VoxelGrid<PointSourceType>::getMinZ() const
- {
- return min_z_;
- }
- template <typename PointSourceType>
- float VoxelGrid<PointSourceType>::getVoxelX() const
- {
- return voxel_x_;
- }
- template <typename PointSourceType>
- float VoxelGrid<PointSourceType>::getVoxelY() const
- {
- return voxel_y_;
- }
- template <typename PointSourceType>
- float VoxelGrid<PointSourceType>::getVoxelZ() const
- {
- return voxel_z_;
- }
- template <typename PointSourceType>
- int VoxelGrid<PointSourceType>::getMaxBX() const
- {
- return max_b_x_;
- }
- template <typename PointSourceType>
- int VoxelGrid<PointSourceType>::getMaxBY() const
- {
- return max_b_y_;
- }
- template <typename PointSourceType>
- int VoxelGrid<PointSourceType>::getMaxBZ() const
- {
- return max_b_z_;
- }
- template <typename PointSourceType>
- int VoxelGrid<PointSourceType>::getMinBX() const
- {
- return min_b_x_;
- }
- template <typename PointSourceType>
- int VoxelGrid<PointSourceType>::getMinBY() const
- {
- return min_b_y_;
- }
- template <typename PointSourceType>
- int VoxelGrid<PointSourceType>::getMinBZ() const
- {
- return min_b_z_;
- }
- template <typename PointSourceType>
- int VoxelGrid<PointSourceType>::getVgridX() const
- {
- return vgrid_x_;
- }
- template <typename PointSourceType>
- int VoxelGrid<PointSourceType>::getVgridY() const
- {
- return vgrid_y_;
- }
- template <typename PointSourceType>
- int VoxelGrid<PointSourceType>::getVgridZ() const
- {
- return vgrid_z_;
- }
- template <typename PointSourceType>
- Eigen::Vector3d VoxelGrid<PointSourceType>::getCentroid(int voxel_id) const
- {
- return (*centroid_)[voxel_id];
- }
- template <typename PointSourceType>
- Eigen::Matrix3d VoxelGrid<PointSourceType>::getInverseCovariance(int voxel_id) const
- {
- return (*icovariance_)[voxel_id];
- }
- template <typename PointSourceType>
- void VoxelGrid<PointSourceType>::setLeafSize(float voxel_x, float voxel_y, float voxel_z)
- {
- voxel_x_ = voxel_x;
- voxel_y_ = voxel_y;
- voxel_z_ = voxel_z;
- }
- template <typename PointSourceType>
- int VoxelGrid<PointSourceType>::voxelId(PointSourceType p)
- {
- int idx = static_cast<int>(floor(p.x / voxel_x_)) - min_b_x_;
- int idy = static_cast<int>(floor(p.y / voxel_y_)) - min_b_y_;
- int idz = static_cast<int>(floor(p.z / voxel_z_)) - min_b_z_;
- return (idx + idy * vgrid_x_ + idz * vgrid_x_ * vgrid_y_);
- }
- template <typename PointSourceType>
- int VoxelGrid<PointSourceType>::voxelId(PointSourceType p,
- float voxel_x, float voxel_y, float voxel_z,
- int min_b_x, int min_b_y, int min_b_z,
- int vgrid_x, int vgrid_y, int vgrid_z)
- {
- int idx = static_cast<int>(floor(p.x / voxel_x)) - min_b_x;
- int idy = static_cast<int>(floor(p.y / voxel_y)) - min_b_y;
- int idz = static_cast<int>(floor(p.z / voxel_z)) - min_b_z;
- return (idx + idy * vgrid_x + idz * vgrid_x * vgrid_y);
- }
- template <typename PointSourceType>
- int VoxelGrid<PointSourceType>::voxelId(int idx, int idy, int idz,
- int min_b_x, int min_b_y, int min_b_z,
- int size_x, int size_y, int size_z)
- {
- return (idx - min_b_x) + (idy - min_b_y) * size_x + (idz - min_b_z) * size_x * size_y;
- }
- template <typename PointSourceType>
- void VoxelGrid<PointSourceType>::computeCentroidAndCovariance()
- {
- for (int idx = real_min_bx_; idx <= real_max_bx_; idx++)
- for (int idy = real_min_by_; idy <= real_max_by_; idy++)
- for (int idz = real_min_bz_; idz <= real_max_bz_; idz++) {
- int i = voxelId(idx, idy, idz, min_b_x_, min_b_y_, min_b_z_, vgrid_x_, vgrid_y_, vgrid_z_);
- int ipoint_num = (*points_id_)[i].size();
- double point_num = static_cast<double>(ipoint_num);
- Eigen::Vector3d pt_sum = (*tmp_centroid_)[i];
- if (ipoint_num > 0) {
- (*centroid_)[i] = pt_sum / point_num;
- }
- Eigen::Matrix3d covariance;
- if (ipoint_num >= min_points_per_voxel_) {
- covariance = ((*tmp_cov_)[i] - 2.0 * (pt_sum * (*centroid_)[i].transpose())) / point_num + (*centroid_)[i] * (*centroid_)[i].transpose();
- covariance *= (point_num - 1.0) / point_num;
- SymmetricEigensolver3x3 sv(covariance);
- sv.compute();
- Eigen::Matrix3d evecs = sv.eigenvectors();
- Eigen::Matrix3d evals = sv.eigenvalues().asDiagonal();
- if (evals(0, 0) < 0 || evals(1, 1) < 0 || evals(2, 2) <= 0) {
- (*points_per_voxel_)[i] = -1;
- continue;
- }
- double min_cov_eigvalue = evals(2, 2) * 0.01;
- if (evals(0, 0) < min_cov_eigvalue) {
- evals(0, 0) = min_cov_eigvalue;
- if (evals(1, 1) < min_cov_eigvalue) {
- evals(1, 1) = min_cov_eigvalue;
- }
- covariance = evecs * evals * evecs.inverse();
- }
- (*icovariance_)[i] = covariance.inverse();
- }
- }
- }
- //Input are supposed to be in device memory
- template <typename PointSourceType>
- void VoxelGrid<PointSourceType>::setInput(typename pcl::PointCloud<PointSourceType>::Ptr input_cloud)
- {
- if (input_cloud->points.size() > 0) {
- /* If no voxel grid was created, then
- * build the initial voxel grid and octree
- */
- source_cloud_ = input_cloud;
- findBoundaries();
- std::vector<Eigen::Vector3i> voxel_ids(input_cloud->points.size());
- for (int i = 0; i < input_cloud->points.size(); i++) {
- Eigen::Vector3i &vid = voxel_ids[i];
- PointSourceType p = input_cloud->points[i];
- vid(0) = static_cast<int>(floor(p.x / voxel_x_));
- vid(1) = static_cast<int>(floor(p.y / voxel_y_));
- vid(2) = static_cast<int>(floor(p.z / voxel_z_));
- }
- octree_.setInput(voxel_ids, input_cloud);
- voxel_ids.clear();
- initialize();
- scatterPointsToVoxelGrid();
- computeCentroidAndCovariance();
- }
- }
- template <typename PointSourceType>
- void VoxelGrid<PointSourceType>::findBoundaries()
- {
- findBoundaries(source_cloud_, max_x_, max_y_, max_z_, min_x_, min_y_, min_z_);
- real_max_bx_ = max_b_x_ = static_cast<int> (floor(max_x_ / voxel_x_));
- real_max_by_ = max_b_y_ = static_cast<int> (floor(max_y_ / voxel_y_));
- real_max_bz_ = max_b_z_ = static_cast<int> (floor(max_z_ / voxel_z_));
- real_min_bx_ = min_b_x_ = static_cast<int> (floor(min_x_ / voxel_x_));
- real_min_by_ = min_b_y_ = static_cast<int> (floor(min_y_ / voxel_y_));
- real_min_bz_ = min_b_z_ = static_cast<int> (floor(min_z_ / voxel_z_));
- /* Allocate a poll of memory that is larger than the requested memory so
- * we do not have to reallocate buffers when the target cloud is set
- */
- /* Max bounds round toward plus infinity */
- max_b_x_ = roundUp(max_b_x_, MAX_BX_);
- max_b_y_ = roundUp(max_b_y_, MAX_BY_);
- max_b_z_ = roundUp(max_b_z_, MAX_BZ_);
- /* Min bounds round toward minus infinity */
- min_b_x_ = roundDown(min_b_x_, MAX_BX_);
- min_b_y_ = roundDown(min_b_y_, MAX_BY_);
- min_b_z_ = roundDown(min_b_z_, MAX_BZ_);
- vgrid_x_ = max_b_x_ - min_b_x_ + 1;
- vgrid_y_ = max_b_y_ - min_b_y_ + 1;
- vgrid_z_ = max_b_z_ - min_b_z_ + 1;
- if (vgrid_x_ > 0 && vgrid_y_ > 0 && vgrid_z_ > 0) {
- voxel_num_ = vgrid_x_ * vgrid_y_ * vgrid_z_;
- } else {
- voxel_num_ = 0;
- }
- }
- template <typename PointSourceType>
- void VoxelGrid<PointSourceType>::findBoundaries(typename pcl::PointCloud<PointSourceType>::Ptr input_cloud,
- float &max_x, float &max_y, float &max_z,
- float &min_x, float &min_y, float &min_z)
- {
- max_x = max_y = max_z = -FLT_MAX;
- min_x = min_y = min_z = FLT_MAX;
- for (int i = 0; i < input_cloud->points.size(); i++) {
- float x = input_cloud->points[i].x;
- float y = input_cloud->points[i].y;
- float z = input_cloud->points[i].z;
- max_x = (max_x > x) ? max_x : x;
- max_y = (max_y > y) ? max_y : y;
- max_z = (max_z > z) ? max_z : z;
- min_x = (min_x < x) ? min_x : x;
- min_y = (min_y < y) ? min_y : y;
- min_z = (min_z < z) ? min_z : z;
- }
- }
- template <typename PointSourceType>
- void VoxelGrid<PointSourceType>::radiusSearch(PointSourceType p, float radius, std::vector<int> &voxel_ids, int max_nn)
- {
- float t_x = p.x;
- float t_y = p.y;
- float t_z = p.z;
- int max_id_x = static_cast<int>(floor((t_x + radius) / voxel_x_));
- int max_id_y = static_cast<int>(floor((t_y + radius) / voxel_y_));
- int max_id_z = static_cast<int>(floor((t_z + radius) / voxel_z_));
- int min_id_x = static_cast<int>(floor((t_x - radius) / voxel_x_));
- int min_id_y = static_cast<int>(floor((t_y - radius) / voxel_y_));
- int min_id_z = static_cast<int>(floor((t_z - radius) / voxel_z_));
- /* Find intersection of the cube containing
- * the NN sphere of the point and the voxel grid
- */
- max_id_x = (max_id_x > real_max_bx_) ? real_max_bx_ : max_id_x;
- max_id_y = (max_id_y > real_max_by_) ? real_max_by_ : max_id_y;
- max_id_z = (max_id_z > real_max_bz_) ? real_max_bz_ : max_id_z;
- min_id_x = (min_id_x < real_min_bx_) ? real_min_bx_ : min_id_x;
- min_id_y = (min_id_y < real_min_by_) ? real_min_by_ : min_id_y;
- min_id_z = (min_id_z < real_min_bz_) ? real_min_bz_ : min_id_z;
- int nn = 0;
- for (int idx = min_id_x; idx <= max_id_x && nn < max_nn; idx++) {
- for (int idy = min_id_y; idy <= max_id_y && nn < max_nn; idy++) {
- for (int idz = min_id_z; idz <= max_id_z && nn < max_nn; idz++) {
- int vid = voxelId(idx, idy, idz,
- min_b_x_, min_b_y_, min_b_z_,
- vgrid_x_, vgrid_y_, vgrid_z_);
- if ((*points_per_voxel_)[vid] >= min_points_per_voxel_) {
- double cx = (*centroid_)[vid](0) - static_cast<double>(t_x);
- double cy = (*centroid_)[vid](1) - static_cast<double>(t_y);
- double cz = (*centroid_)[vid](2) - static_cast<double>(t_z);
- double distance = sqrt(cx * cx + cy * cy + cz * cz);
- if (distance < radius) {
- nn++;
- voxel_ids.push_back(vid);
- }
- }
- }
- }
- }
- }
- template <typename PointSourceType>
- void VoxelGrid<PointSourceType>::scatterPointsToVoxelGrid()
- {
- for (int pid = 0; pid < source_cloud_->points.size(); pid++) {
- int vid = voxelId(source_cloud_->points[pid]);
- PointSourceType p = source_cloud_->points[pid];
- Eigen::Vector3d p3d(p.x, p.y, p.z);
- if ((*points_id_)[vid].size() == 0) {
- (*centroid_)[vid].setZero();
- (*points_per_voxel_)[vid] = 0;
- (*tmp_centroid_)[vid].setZero();
- (*tmp_cov_)[vid].setIdentity();
- }
- (*tmp_centroid_)[vid] += p3d;
- (*tmp_cov_)[vid] += p3d * p3d.transpose();
- (*points_id_)[vid].push_back(pid);
- (*points_per_voxel_)[vid]++;
- }
- }
- template <typename PointSourceType>
- int VoxelGrid<PointSourceType>::nearestVoxel(PointSourceType query_point, Eigen::Matrix<float, 6, 1> boundaries, float max_range)
- {
- // Index of the origin of the circle (query point)
- float qx = query_point.x;
- float qy = query_point.y;
- float qz = query_point.z;
- int lower_x = static_cast<int>(floor(boundaries(0) / voxel_x_));
- int lower_y = static_cast<int>(floor(boundaries(1) / voxel_y_));
- int lower_z = static_cast<int>(floor(boundaries(2) / voxel_z_));
- int upper_x = static_cast<int>(floor(boundaries(3) / voxel_x_));
- int upper_y = static_cast<int>(floor(boundaries(4) / voxel_y_));
- int upper_z = static_cast<int>(floor(boundaries(5) / voxel_z_));
- double min_dist = DBL_MAX;
- int nn_vid = -1;
- for (int i = lower_x; i <= upper_x; i++) {
- for (int j = lower_y; j <= upper_y; j++) {
- for (int k = lower_z; k <= upper_z; k++) {
- int vid = voxelId(i, j, k, min_b_x_, min_b_y_, min_b_z_, vgrid_x_, vgrid_y_, vgrid_z_);
- Eigen::Vector3d c = (*centroid_)[vid];
- if ((*points_id_)[vid].size() > 0) {
- double cur_dist = sqrt((qx - c(0)) * (qx - c(0)) + (qy - c(1)) * (qy - c(1)) + (qz - c(2)) * (qz - c(2)));
- if (cur_dist < min_dist) {
- min_dist = cur_dist;
- nn_vid = vid;
- }
- }
- }
- }
- }
- return nn_vid;
- }
- template <typename PointSourceType>
- double VoxelGrid<PointSourceType>::nearestNeighborDistance(PointSourceType q, float max_range)
- {
- Eigen::Matrix<float, 6, 1> nn_node_bounds;
- nn_node_bounds = octree_.nearestOctreeNode(q);
- int nn_vid = nearestVoxel(q, nn_node_bounds, max_range);
- Eigen::Vector3d c = (*centroid_)[nn_vid];
- double min_dist = sqrt((q.x - c(0)) * (q.x - c(0)) + (q.y - c(1)) * (q.y - c(1)) + (q.z - c(2)) * (q.z - c(2)));
- if (min_dist >= max_range) {
- return DBL_MAX;
- }
- return min_dist;
- }
- template <typename PointSourceType>
- void VoxelGrid<PointSourceType>::updateBoundaries(float max_x, float max_y, float max_z,
- float min_x, float min_y, float min_z)
- {
- float new_max_x, new_max_y, new_max_z;
- float new_min_x, new_min_y, new_min_z;
- new_max_x = (max_x_ >= max_x) ? max_x_ : max_x;
- new_max_y = (max_y_ >= max_y) ? max_y_ : max_y;
- new_max_z = (max_z_ >= max_z) ? max_z_ : max_z;
- new_min_x = (min_x_ <= min_x) ? min_x_ : min_x;
- new_min_y = (min_y_ <= min_y) ? min_y_ : min_y;
- new_min_z = (min_z_ <= min_z) ? min_z_ : min_z;
- /* If the boundaries change, then we need to extend the list of voxels */
- if (new_max_x > max_x_ || new_max_y > max_y_ || new_max_z > max_z_ ||
- new_min_x < min_x_ || new_min_y < min_y_ || new_min_z < min_z_) {
- int max_b_x = static_cast<int> (floor(new_max_x / voxel_x_));
- int max_b_y = static_cast<int> (floor(new_max_y / voxel_y_));
- int max_b_z = static_cast<int> (floor(new_max_z / voxel_z_));
- int min_b_x = static_cast<int> (floor(new_min_x / voxel_x_));
- int min_b_y = static_cast<int> (floor(new_min_y / voxel_y_));
- int min_b_z = static_cast<int> (floor(new_min_z / voxel_z_));
- int real_max_bx = max_b_x;
- int real_max_by = max_b_y;
- int real_max_bz = max_b_z;
- int real_min_bx = min_b_x;
- int real_min_by = min_b_y;
- int real_min_bz = min_b_z;
- /* Max bounds round toward plus infinity */
- max_b_x = roundUp(max_b_x, MAX_BX_);
- max_b_y = roundUp(max_b_y, MAX_BY_);
- max_b_z = roundUp(max_b_z, MAX_BZ_);
- /* Min bounds round toward minus infinity */
- min_b_x = roundDown(min_b_x, MAX_BX_);
- min_b_y = roundDown(min_b_y, MAX_BY_);
- min_b_z = roundDown(min_b_z, MAX_BZ_);
- if (max_b_x > max_b_x_ || max_b_y > max_b_y_ || max_b_z > max_b_z_ ||
- min_b_x < min_b_x_ || min_b_y < min_b_y_ || min_b_z < min_b_z_) {
- int vgrid_x = max_b_x - min_b_x + 1;
- int vgrid_y = max_b_y - min_b_y + 1;
- int vgrid_z = max_b_z - min_b_z + 1;
- int voxel_num = vgrid_x * vgrid_y * vgrid_z;
- boost::shared_ptr<std::vector<Eigen::Vector3d> > old_centroid = centroid_;
- boost::shared_ptr<std::vector<Eigen::Matrix3d> > old_icovariance = icovariance_;
- boost::shared_ptr<std::vector<std::vector<int> > > old_points_id = points_id_;
- boost::shared_ptr<std::vector<int> > old_points_per_voxel = points_per_voxel_;
- boost::shared_ptr<std::vector<Eigen::Vector3d> > old_tmp_centroid = tmp_centroid_;
- boost::shared_ptr<std::vector<Eigen::Matrix3d> > old_tmp_cov = tmp_cov_;
- centroid_ = boost::make_shared<std::vector<Eigen::Vector3d> >(voxel_num);
- icovariance_ = boost::make_shared<std::vector<Eigen::Matrix3d> >(voxel_num);
- points_id_ = boost::make_shared<std::vector<std::vector<int> > >(voxel_num);
- points_per_voxel_ = boost::make_shared<std::vector<int> >(voxel_num, 0);
- tmp_centroid_ = boost::make_shared<std::vector<Eigen::Vector3d> >(voxel_num);
- tmp_cov_ = boost::make_shared<std::vector<Eigen::Matrix3d> >(voxel_num);
- // Move the old non-empty voxels to the new list of voxels
- int idx, idy, idz;
- int old_id, new_id;
- for (int idx = real_min_bx_; idx <= real_max_bx_; idx++) {
- for (int idy = real_min_by_; idy <= real_max_by_; idy++) {
- for (int idz = real_min_bz_; idz <= real_max_bz_; idz++) {
- old_id = voxelId(idx, idy, idz,
- min_b_x_, min_b_y_, min_b_z_,
- vgrid_x_, vgrid_y_, vgrid_z_);
- new_id = voxelId(idx, idy, idz,
- min_b_x, min_b_y, min_b_z,
- vgrid_x, vgrid_y, vgrid_z);
- if ((*old_points_id)[old_id].size() > 0) {
- (*points_per_voxel_)[new_id] = (*old_points_per_voxel)[old_id];
- (*centroid_)[new_id] = (*old_centroid)[old_id];
- (*icovariance_)[new_id] = (*old_icovariance)[old_id];
- (*points_id_)[new_id] = (*old_points_id)[old_id];
- (*tmp_centroid_)[new_id] = (*old_tmp_centroid)[old_id];
- (*tmp_cov_)[new_id] = (*old_tmp_cov)[old_id];
- }
- }
- }
- }
- // Update boundaries of voxels
- max_b_x_ = max_b_x;
- max_b_y_ = max_b_y;
- max_b_z_ = max_b_z;
- min_b_x_ = min_b_x;
- min_b_y_ = min_b_y;
- min_b_z_ = min_b_z;
- vgrid_x_ = vgrid_x;
- vgrid_y_ = vgrid_y;
- vgrid_z_ = vgrid_z;
- voxel_num_ = voxel_num;
- }
- // Update actual voxel boundaries
- real_min_bx_ = real_min_bx;
- real_min_by_ = real_min_by;
- real_min_bz_ = real_min_bz;
- real_max_bx_ = real_max_bx;
- real_max_by_ = real_max_by;
- real_max_bz_ = real_max_bz;
- //Update boundaries of points
- max_x_ = new_max_x;
- max_y_ = new_max_y;
- max_z_ = new_max_z;
- min_x_ = new_min_x;
- min_y_ = new_min_y;
- min_z_ = new_min_z;
- }
- }
- template <typename PointSourceType>
- void VoxelGrid<PointSourceType>::update(typename pcl::PointCloud<PointSourceType>::Ptr new_cloud)
- {
- if (new_cloud->points.size() <= 0) {
- return;
- }
- float new_max_x, new_max_y, new_max_z;
- float new_min_x, new_min_y, new_min_z;
- // Find boundaries of the new point cloud
- findBoundaries(new_cloud, new_max_x, new_max_y, new_max_z, new_min_x, new_min_y, new_min_z);
- /* Update current boundaries of the voxel grid
- * Also allocate buffer for new voxel grid and
- * octree and move the current voxel grid and
- * octree to the new buffer if necessary
- */
- updateBoundaries(new_max_x, new_max_y, new_max_z, new_min_x, new_min_y, new_min_z);
- /* Update changed voxels (voxels that contains new points).
- * Update centroids of voxels and their covariance matrixes
- * as well as inverse covariance matrixes */
- updateVoxelContent(new_cloud);
- /* Update octree */
- std::vector<Eigen::Vector3i> new_voxel_id(new_cloud->points.size());
- for (int i = 0; i < new_cloud->points.size(); i++) {
- Eigen::Vector3i &vid = new_voxel_id[i];
- PointSourceType p = new_cloud->points[i];
- vid(0) = static_cast<int>(floor(p.x / voxel_x_));
- vid(1) = static_cast<int>(floor(p.y / voxel_y_));
- vid(2) = static_cast<int>(floor(p.z / voxel_z_));
- }
- octree_.update(new_voxel_id, new_cloud);
- *source_cloud_ += *new_cloud;
- }
- template <typename PointSourceType>
- void VoxelGrid<PointSourceType>::updateVoxelContent(typename pcl::PointCloud<PointSourceType>::Ptr new_cloud)
- {
- int total_points_num = source_cloud_->points.size();
- for (int i = 0; i < new_cloud->points.size(); i++) {
- PointSourceType p = new_cloud->points[i];
- Eigen::Vector3d p3d(p.x, p.y, p.z);
- int vx = static_cast<int>(floor(p.x / voxel_x_));
- int vy = static_cast<int>(floor(p.y / voxel_y_));
- int vz = static_cast<int>(floor(p.z / voxel_z_));
- int vid = voxelId(vx, vy, vz, min_b_x_, min_b_y_, min_b_z_, vgrid_x_, vgrid_y_, vgrid_z_);
- std::vector<int> &tmp_pid = (*points_id_)[vid];
- if (tmp_pid.size() == 0) {
- (*centroid_)[vid].setZero();
- (*tmp_cov_)[vid].setIdentity();
- (*tmp_centroid_)[vid].setZero();
- }
- (*tmp_centroid_)[vid] += p3d;
- (*tmp_cov_)[vid] += p3d * p3d.transpose();
- (*points_id_)[vid].push_back(total_points_num + i);
- // Update centroids
- int ipoint_num = (*points_id_)[vid].size();
- (*centroid_)[vid] = (*tmp_centroid_)[vid] / static_cast<double>(ipoint_num);
- (*points_per_voxel_)[vid] = ipoint_num;
- // Update covariance
- double point_num = static_cast<double>(ipoint_num);
- Eigen::Vector3d pt_sum = (*tmp_centroid_)[vid];
- // Update centroids
- (*centroid_)[vid] = (*tmp_centroid_)[vid] / point_num;
- if (ipoint_num >= min_points_per_voxel_) {
- Eigen::Matrix3d covariance;
- covariance = ((*tmp_cov_)[vid] - 2.0 * (pt_sum * (*centroid_)[vid].transpose())) / point_num + (*centroid_)[vid] * (*centroid_)[vid].transpose();
- covariance *= (point_num - 1.0) / point_num;
- SymmetricEigensolver3x3 sv(covariance);
- sv.compute();
- Eigen::Matrix3d evecs = sv.eigenvectors();
- Eigen::Matrix3d evals = sv.eigenvalues().asDiagonal();
- if (evals(0, 0) < 0 || evals(1, 1) < 0 || evals(2, 2) <= 0) {
- (*points_per_voxel_)[vid] = -1;
- continue;
- }
- double min_cov_eigvalue = evals(2, 2) * 0.01;
- if (evals(0, 0) < min_cov_eigvalue) {
- evals(0, 0) = min_cov_eigvalue;
- if (evals(1, 1) < min_cov_eigvalue) {
- evals(1, 1) = min_cov_eigvalue;
- }
- covariance = evecs * evals * evecs.inverse();
- }
- (*icovariance_)[vid] = covariance.inverse();
- }
- }
- }
- template class VoxelGrid<pcl::PointXYZI>;
- template class VoxelGrid<pcl::PointXYZ>;
- }
|