diff --git a/apps/include/pcl/apps/impl/dominant_plane_segmentation.hpp b/apps/include/pcl/apps/impl/dominant_plane_segmentation.hpp index 549f893bcff..119c9f3b9d2 100644 --- a/apps/include/pcl/apps/impl/dominant_plane_segmentation.hpp +++ b/apps/include/pcl/apps/impl/dominant_plane_segmentation.hpp @@ -120,7 +120,6 @@ pcl::apps::DominantPlaneSegmentation::compute_table_plane() // Compute the plane coefficients Eigen::Vector4f model_coefficients; - EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix; model_coefficients[0] = table_coefficients_->values[0]; model_coefficients[1] = table_coefficients_->values[1]; @@ -251,7 +250,6 @@ pcl::apps::DominantPlaneSegmentation::compute_fast( // Compute the plane coefficients Eigen::Vector4f model_coefficients; - EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix; model_coefficients[0] = table_coefficients_->values[0]; model_coefficients[1] = table_coefficients_->values[1]; @@ -626,7 +624,6 @@ pcl::apps::DominantPlaneSegmentation::compute( // Compute the plane coefficients Eigen::Vector4f model_coefficients; - EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix; model_coefficients[0] = table_coefficients_->values[0]; model_coefficients[1] = table_coefficients_->values[1]; @@ -795,7 +792,6 @@ pcl::apps::DominantPlaneSegmentation::compute_full( // Compute the plane coefficients Eigen::Vector4f model_coefficients; - EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix; model_coefficients[0] = table_coefficients_->values[0]; model_coefficients[1] = table_coefficients_->values[1]; diff --git a/common/include/pcl/common/common.h b/common/include/pcl/common/common.h index af32634ca65..857cca893ac 100644 --- a/common/include/pcl/common/common.h +++ b/common/include/pcl/common/common.h @@ -180,7 +180,7 @@ namespace pcl getMaxDistance (const pcl::PointCloud &cloud, const Indices &indices, const Eigen::Vector4f &pivot_pt, Eigen::Vector4f &max_pt); - /** \brief Get the minimum and maximum values on each of the 3 (x-y-z) dimensions in a given pointcloud + /** \brief Get the minimum and maximum values on each of the 3 (x-y-z) dimensions in a given pointcloud. This is the axis aligned bounding box (AABB). * \param[in] cloud the point cloud data message * \param[out] min_pt the resultant minimum bounds * \param[out] max_pt the resultant maximum bounds @@ -189,7 +189,7 @@ namespace pcl template inline void getMinMax3D (const pcl::PointCloud &cloud, PointT &min_pt, PointT &max_pt); - /** \brief Get the minimum and maximum values on each of the 3 (x-y-z) dimensions in a given pointcloud + /** \brief Get the minimum and maximum values on each of the 3 (x-y-z) dimensions in a given pointcloud. This is the axis aligned bounding box (AABB). * \param[in] cloud the point cloud data message * \param[out] min_pt the resultant minimum bounds * \param[out] max_pt the resultant maximum bounds @@ -199,7 +199,7 @@ namespace pcl getMinMax3D (const pcl::PointCloud &cloud, Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt); - /** \brief Get the minimum and maximum values on each of the 3 (x-y-z) dimensions in a given pointcloud + /** \brief Get the minimum and maximum values on each of the 3 (x-y-z) dimensions in a given pointcloud. This is the axis aligned bounding box (AABB). * \param[in] cloud the point cloud data message * \param[in] indices the vector of point indices to use from \a cloud * \param[out] min_pt the resultant minimum bounds @@ -210,7 +210,7 @@ namespace pcl getMinMax3D (const pcl::PointCloud &cloud, const Indices &indices, Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt); - /** \brief Get the minimum and maximum values on each of the 3 (x-y-z) dimensions in a given pointcloud + /** \brief Get the minimum and maximum values on each of the 3 (x-y-z) dimensions in a given pointcloud. This is the axis aligned bounding box (AABB). * \param[in] cloud the point cloud data message * \param[in] indices the vector of point indices to use from \a cloud * \param[out] min_pt the resultant minimum bounds diff --git a/common/include/pcl/pcl_macros.h b/common/include/pcl/pcl_macros.h index 0e2740965ee..eebc14decd0 100644 --- a/common/include/pcl/pcl_macros.h +++ b/common/include/pcl/pcl_macros.h @@ -116,11 +116,13 @@ #define _PCL_DEPRECATED_HEADER_IMPL(Message) #endif +// NOLINTBEGIN(bugprone-macro-parentheses) /** * \brief A handy way to inform the user of the removal deadline */ #define _PCL_PREPARE_REMOVAL_MESSAGE(Major, Minor, Msg) \ - Msg " (It will be removed in PCL " BOOST_PP_STRINGIZE((Major).Minor) ")" + Msg " (It will be removed in PCL " BOOST_PP_STRINGIZE(Major.Minor) ")" +// NOLINTEND(bugprone-macro-parentheses) /** * \brief Tests for Minor < PCL_MINOR_VERSION diff --git a/features/include/pcl/features/impl/cppf.hpp b/features/include/pcl/features/impl/cppf.hpp index 975bb98cc81..94f9c3a61f3 100755 --- a/features/include/pcl/features/impl/cppf.hpp +++ b/features/include/pcl/features/impl/cppf.hpp @@ -42,6 +42,7 @@ #define PCL_FEATURES_IMPL_CPPF_H_ #include +#include // for KdTree ////////////////////////////////////////////////////////////////////////////////////////////// template diff --git a/features/include/pcl/features/impl/cvfh.hpp b/features/include/pcl/features/impl/cvfh.hpp index de4fbc17eb3..55173305456 100644 --- a/features/include/pcl/features/impl/cvfh.hpp +++ b/features/include/pcl/features/impl/cvfh.hpp @@ -44,6 +44,7 @@ #include #include #include +#include // for KdTree ////////////////////////////////////////////////////////////////////////////////////////////// template void diff --git a/features/include/pcl/features/impl/flare.hpp b/features/include/pcl/features/impl/flare.hpp index 2f8d3c9577d..e00f242f422 100644 --- a/features/include/pcl/features/impl/flare.hpp +++ b/features/include/pcl/features/impl/flare.hpp @@ -41,6 +41,8 @@ #include #include +#include // for KdTree +#include // for OrganizedNeighbor ////////////////////////////////////////////////////////////////////////////////////////////// template bool diff --git a/features/include/pcl/features/impl/integral_image_normal.hpp b/features/include/pcl/features/impl/integral_image_normal.hpp index f76765a1bcc..612a6df80a1 100644 --- a/features/include/pcl/features/impl/integral_image_normal.hpp +++ b/features/include/pcl/features/impl/integral_image_normal.hpp @@ -36,6 +36,7 @@ * */ #pragma once +#include // for eigen33 #include #include @@ -491,7 +492,6 @@ pcl::IntegralImageNormalEstimation::computePointNormalMirro Eigen::Vector3f center; typename IntegralImage2D::SecondOrderType so_elements; typename IntegralImage2D::ElementType tmp_center; - typename IntegralImage2D::SecondOrderType tmp_so_elements; center[0] = 0; center[1] = 0; diff --git a/features/include/pcl/features/impl/intensity_gradient.hpp b/features/include/pcl/features/impl/intensity_gradient.hpp index df54ae8f9e1..fcb8c85f4f2 100644 --- a/features/include/pcl/features/impl/intensity_gradient.hpp +++ b/features/include/pcl/features/impl/intensity_gradient.hpp @@ -43,6 +43,7 @@ #include #include // for pcl::isFinite +#include // for eigen33 ////////////////////////////////////////////////////////////////////////////////////////////// diff --git a/features/include/pcl/features/impl/our_cvfh.hpp b/features/include/pcl/features/impl/our_cvfh.hpp index b0f0bc01a2e..15fab154805 100644 --- a/features/include/pcl/features/impl/our_cvfh.hpp +++ b/features/include/pcl/features/impl/our_cvfh.hpp @@ -47,6 +47,7 @@ #include // for copyPointCloud #include // for getMaxDistance #include +#include // for KdTree ////////////////////////////////////////////////////////////////////////////////////////////// template void @@ -623,7 +624,6 @@ pcl::OURCVFHEstimation::computeFeature (PointCloud { pcl::PointIndices pi; - pcl::PointIndices pi_cvfh; pcl::PointIndices pi_filtered; clusters_.push_back (pi); diff --git a/features/include/pcl/features/impl/ppf.hpp b/features/include/pcl/features/impl/ppf.hpp index 8cdb914a975..7fb8818a645 100644 --- a/features/include/pcl/features/impl/ppf.hpp +++ b/features/include/pcl/features/impl/ppf.hpp @@ -43,6 +43,7 @@ #include #include #include // for computePairFeatures +#include // for KdTree ////////////////////////////////////////////////////////////////////////////////////////////// template diff --git a/features/include/pcl/features/impl/ppfrgb.hpp b/features/include/pcl/features/impl/ppfrgb.hpp index 87f74548a62..193d54d6f56 100644 --- a/features/include/pcl/features/impl/ppfrgb.hpp +++ b/features/include/pcl/features/impl/ppfrgb.hpp @@ -40,6 +40,7 @@ #include #include +#include // for KdTree ////////////////////////////////////////////////////////////////////////////////////////////// template diff --git a/features/include/pcl/features/impl/principal_curvatures.hpp b/features/include/pcl/features/impl/principal_curvatures.hpp index 0d2ddd34ebb..8c0561c41d5 100644 --- a/features/include/pcl/features/impl/principal_curvatures.hpp +++ b/features/include/pcl/features/impl/principal_curvatures.hpp @@ -43,6 +43,7 @@ #include #include // for pcl::isFinite +#include // for eigen33 /////////////////////////////////////////////////////////////////////////////////////////// template void diff --git a/filters/include/pcl/filters/convolution_3d.h b/filters/include/pcl/filters/convolution_3d.h index 31a5a8093e0..8cb21bdee3f 100644 --- a/filters/include/pcl/filters/convolution_3d.h +++ b/filters/include/pcl/filters/convolution_3d.h @@ -272,7 +272,7 @@ namespace pcl double search_radius_; /** \brief number of threads */ - unsigned int threads_; + unsigned int threads_{1}; /** \brief convlving kernel */ KernelT kernel_; diff --git a/filters/include/pcl/filters/covariance_sampling.h b/filters/include/pcl/filters/covariance_sampling.h index d44b2e66a09..831a623be33 100644 --- a/filters/include/pcl/filters/covariance_sampling.h +++ b/filters/include/pcl/filters/covariance_sampling.h @@ -56,6 +56,7 @@ namespace pcl * Based on the following publication: * * "Geometrically Stable Sampling for the ICP Algorithm" - N. Gelfand, L. Ikemoto, S. Rusinkiewicz, M. Levoy * + * \ingroup filters * \author Alexandru E. Ichim, alex.e.ichim@gmail.com */ template diff --git a/filters/include/pcl/filters/impl/random_sample.hpp b/filters/include/pcl/filters/impl/random_sample.hpp index 24a0c571a67..f4138afcf3c 100644 --- a/filters/include/pcl/filters/impl/random_sample.hpp +++ b/filters/include/pcl/filters/impl/random_sample.hpp @@ -63,7 +63,11 @@ pcl::RandomSample::applyFilter (Indices &indices) removed_indices_->resize (N - sample_size); // Set random seed so derived indices are the same each time the filter runs +#ifdef __OpenBSD__ + srand_deterministic (seed_); // OpenBSD only offers repeatable sequences with this function +#else std::srand (seed_); +#endif // __OpenBSD__ // Algorithm S std::size_t i = 0; diff --git a/filters/include/pcl/filters/model_outlier_removal.h b/filters/include/pcl/filters/model_outlier_removal.h index 8fbe3dce188..bc811290191 100644 --- a/filters/include/pcl/filters/model_outlier_removal.h +++ b/filters/include/pcl/filters/model_outlier_removal.h @@ -67,6 +67,7 @@ namespace pcl * filter.filter (*cloud_out); * \endcode + * \ingroup filters */ template class ModelOutlierRemoval : public FilterIndices diff --git a/filters/include/pcl/filters/radius_outlier_removal.h b/filters/include/pcl/filters/radius_outlier_removal.h index 23f1d48317d..b01d6619f8b 100644 --- a/filters/include/pcl/filters/radius_outlier_removal.h +++ b/filters/include/pcl/filters/radius_outlier_removal.h @@ -64,6 +64,7 @@ namespace pcl * indices_rem = rorfilter.getRemovedIndices (); * // The indices_rem array indexes all points of cloud_in that have 5 or more neighbors within the 0.1 search radius * \endcode + * \sa StatisticalOutlierRemoval * \author Radu Bogdan Rusu * \ingroup filters */ diff --git a/filters/include/pcl/filters/sampling_surface_normal.h b/filters/include/pcl/filters/sampling_surface_normal.h index 3c2e8397404..25253747a51 100644 --- a/filters/include/pcl/filters/sampling_surface_normal.h +++ b/filters/include/pcl/filters/sampling_surface_normal.h @@ -61,7 +61,7 @@ namespace pcl using PointCloudPtr = typename PointCloud::Ptr; using PointCloudConstPtr = typename PointCloud::ConstPtr; - using Vector = Eigen::Matrix; + using Vector = Eigen::Matrix; public: diff --git a/filters/include/pcl/filters/statistical_outlier_removal.h b/filters/include/pcl/filters/statistical_outlier_removal.h index 4abfd12316e..c752935b906 100644 --- a/filters/include/pcl/filters/statistical_outlier_removal.h +++ b/filters/include/pcl/filters/statistical_outlier_removal.h @@ -73,6 +73,7 @@ namespace pcl * indices_rem = sorfilter.getRemovedIndices (); * // The indices_rem array indexes all points of cloud_in that are outliers * \endcode + * \sa RadiusOutlierRemoval * \author Radu Bogdan Rusu * \ingroup filters */ diff --git a/filters/src/random_sample.cpp b/filters/src/random_sample.cpp index f271d8d4da2..97a75a6dbf1 100644 --- a/filters/src/random_sample.cpp +++ b/filters/src/random_sample.cpp @@ -117,7 +117,11 @@ pcl::RandomSample::applyFilter (Indices &indices) removed_indices_->resize (N - sample_size); // Set random seed so derived indices are the same each time the filter runs +#ifdef __OpenBSD__ + srand_deterministic (seed_); // OpenBSD only offers repeatable sequences with this function +#else std::srand (seed_); +#endif // __OpenBSD__ // Algorithm S std::size_t i = 0; diff --git a/io/src/ply_io.cpp b/io/src/ply_io.cpp index af58106a2cb..936e836fb0d 100644 --- a/io/src/ply_io.cpp +++ b/io/src/ply_io.cpp @@ -409,8 +409,16 @@ void pcl::PLYReader::vertexIntensityCallback (pcl::io::ply::uint8 intensity) { pcl::io::ply::float32 intensity_ (intensity); - cloud_->at(vertex_count_, vertex_offset_before_) = intensity_; - vertex_offset_before_ += static_cast (sizeof (pcl::io::ply::float32)); + try + { + cloud_->at(vertex_count_, vertex_offset_before_) = intensity_; + vertex_offset_before_ += static_cast (sizeof (pcl::io::ply::float32)); + } + catch(const std::out_of_range&) + { + PCL_WARN ("[pcl::PLYReader::vertexIntensityCallback] Incorrect data index specified (%lu)!\n", vertex_count_ * cloud_->point_step + vertex_offset_before_); + assert(false); + } } void diff --git a/outofcore/include/pcl/outofcore/impl/outofcore_depth_first_iterator.hpp b/outofcore/include/pcl/outofcore/impl/outofcore_depth_first_iterator.hpp index a7d15386619..b5744d52503 100644 --- a/outofcore/include/pcl/outofcore/impl/outofcore_depth_first_iterator.hpp +++ b/outofcore/include/pcl/outofcore/impl/outofcore_depth_first_iterator.hpp @@ -107,10 +107,10 @@ namespace pcl if (!stack_.empty ()) { std::pair*, unsigned char>& stackEntry = stack_.back (); - stack_.pop_back (); this->currentNode_ = stackEntry.first; currentChildIdx_ = stackEntry.second; + stack_.pop_back (); // stackEntry is a reference, so pop_back after accessing it! //don't do anything with the keys here... this->currentOctreeDepth_--; diff --git a/registration/include/pcl/registration/impl/pyramid_feature_matching.hpp b/registration/include/pcl/registration/impl/pyramid_feature_matching.hpp index ba7b4eebd0a..4ced722e45a 100644 --- a/registration/include/pcl/registration/impl/pyramid_feature_matching.hpp +++ b/registration/include/pcl/registration/impl/pyramid_feature_matching.hpp @@ -299,8 +299,8 @@ PyramidFeatureHistogram::compute() if (!initializeHistogram()) return; + std::vector feature_vector; // put here to reuse memory for (const auto& point : *input_) { - std::vector feature_vector; // NaN is converted to very high number that gives out of bound exception. if (!pcl::isFinite(point)) continue; diff --git a/registration/include/pcl/registration/impl/transformation_estimation_dq.hpp b/registration/include/pcl/registration/impl/transformation_estimation_dq.hpp deleted file mode 100644 index fce6079d3da..00000000000 --- a/registration/include/pcl/registration/impl/transformation_estimation_dq.hpp +++ /dev/null @@ -1,225 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Point Cloud Library (PCL) - www.pointclouds.org - * Copyright (c) 2010, Willow Garage, Inc. - * Copyright (c) 2012-, Open Perception, Inc. - * - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder(s) nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * - */ - -#ifndef PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_DQ_HPP_ -#define PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_DQ_HPP_ - -#include -PCL_DEPRECATED_HEADER(1, - 15, - "TransformationEstimationDQ has been renamed to " - "TransformationEstimationDualQuaternion."); - -namespace pcl { - -namespace registration { - -template -inline void -TransformationEstimationDQ:: - estimateRigidTransformation(const pcl::PointCloud& cloud_src, - const pcl::PointCloud& cloud_tgt, - Matrix4& transformation_matrix) const -{ - const auto nr_points = cloud_src.size(); - if (cloud_tgt.size() != nr_points) { - PCL_ERROR("[pcl::TransformationEstimationDQ::estimateRigidTransformation] Number " - "or points in source (%zu) differs than target (%zu)!\n", - static_cast(nr_points), - static_cast(cloud_tgt.size())); - return; - } - - ConstCloudIterator source_it(cloud_src); - ConstCloudIterator target_it(cloud_tgt); - estimateRigidTransformation(source_it, target_it, transformation_matrix); -} - -template -void -TransformationEstimationDQ:: - estimateRigidTransformation(const pcl::PointCloud& cloud_src, - const pcl::Indices& indices_src, - const pcl::PointCloud& cloud_tgt, - Matrix4& transformation_matrix) const -{ - if (indices_src.size() != cloud_tgt.size()) { - PCL_ERROR("[pcl::TransformationDQ::estimateRigidTransformation] Number or points " - "in source (%zu) differs than target (%zu)!\n", - indices_src.size(), - static_cast(cloud_tgt.size())); - return; - } - - ConstCloudIterator source_it(cloud_src, indices_src); - ConstCloudIterator target_it(cloud_tgt); - estimateRigidTransformation(source_it, target_it, transformation_matrix); -} - -template -inline void -TransformationEstimationDQ:: - estimateRigidTransformation(const pcl::PointCloud& cloud_src, - const pcl::Indices& indices_src, - const pcl::PointCloud& cloud_tgt, - const pcl::Indices& indices_tgt, - Matrix4& transformation_matrix) const -{ - if (indices_src.size() != indices_tgt.size()) { - PCL_ERROR("[pcl::TransformationEstimationDQ::estimateRigidTransformation] Number " - "or points in source (%zu) differs than target (%zu)!\n", - indices_src.size(), - indices_tgt.size()); - return; - } - - ConstCloudIterator source_it(cloud_src, indices_src); - ConstCloudIterator target_it(cloud_tgt, indices_tgt); - estimateRigidTransformation(source_it, target_it, transformation_matrix); -} - -template -void -TransformationEstimationDQ:: - estimateRigidTransformation(const pcl::PointCloud& cloud_src, - const pcl::PointCloud& cloud_tgt, - const pcl::Correspondences& correspondences, - Matrix4& transformation_matrix) const -{ - ConstCloudIterator source_it(cloud_src, correspondences, true); - ConstCloudIterator target_it(cloud_tgt, correspondences, false); - estimateRigidTransformation(source_it, target_it, transformation_matrix); -} - -template -inline void -TransformationEstimationDQ:: - estimateRigidTransformation(ConstCloudIterator& source_it, - ConstCloudIterator& target_it, - Matrix4& transformation_matrix) const -{ - const int npts = static_cast(source_it.size()); - - transformation_matrix.setIdentity(); - - // dual quaternion optimization - Eigen::Matrix C1 = Eigen::Matrix::Zero(); - Eigen::Matrix C2 = Eigen::Matrix::Zero(); - Scalar* c1 = C1.data(); - Scalar* c2 = C2.data(); - - for (int i = 0; i < npts; i++) { - const PointSource& a = *source_it; - const PointTarget& b = *target_it; - const Scalar axbx = a.x * b.x; - const Scalar ayby = a.y * b.y; - const Scalar azbz = a.z * b.z; - const Scalar axby = a.x * b.y; - const Scalar aybx = a.y * b.x; - const Scalar axbz = a.x * b.z; - const Scalar azbx = a.z * b.x; - const Scalar aybz = a.y * b.z; - const Scalar azby = a.z * b.y; - c1[0] += axbx - azbz - ayby; - c1[5] += ayby - azbz - axbx; - c1[10] += azbz - axbx - ayby; - c1[15] += axbx + ayby + azbz; - c1[1] += axby + aybx; - c1[2] += axbz + azbx; - c1[3] += aybz - azby; - c1[6] += azby + aybz; - c1[7] += azbx - axbz; - c1[11] += axby - aybx; - - c2[1] += a.z + b.z; - c2[2] -= a.y + b.y; - c2[3] += a.x - b.x; - c2[6] += a.x + b.x; - c2[7] += a.y - b.y; - c2[11] += a.z - b.z; - source_it++; - target_it++; - } - - c1[4] = c1[1]; - c1[8] = c1[2]; - c1[9] = c1[6]; - c1[12] = c1[3]; - c1[13] = c1[7]; - c1[14] = c1[11]; - c2[4] = -c2[1]; - c2[8] = -c2[2]; - c2[12] = -c2[3]; - c2[9] = -c2[6]; - c2[13] = -c2[7]; - c2[14] = -c2[11]; - - C1 *= -2.0f; - C2 *= 2.0f; - - const Eigen::Matrix A = - (0.25f / float(npts)) * C2.transpose() * C2 - C1; - - const Eigen::EigenSolver> es(A); - - ptrdiff_t i; - es.eigenvalues().real().maxCoeff(&i); - const Eigen::Matrix qmat = es.eigenvectors().col(i).real(); - const Eigen::Matrix smat = -(0.5f / float(npts)) * C2 * qmat; - - const Eigen::Quaternion q(qmat(3), qmat(0), qmat(1), qmat(2)); - const Eigen::Quaternion s(smat(3), smat(0), smat(1), smat(2)); - - const Eigen::Quaternion t = s * q.conjugate(); - - const Eigen::Matrix R(q.toRotationMatrix()); - - for (int i = 0; i < 3; ++i) - for (int j = 0; j < 3; ++j) - transformation_matrix(i, j) = R(i, j); - - transformation_matrix(0, 3) = -t.x(); - transformation_matrix(1, 3) = -t.y(); - transformation_matrix(2, 3) = -t.z(); -} - -} // namespace registration -} // namespace pcl - -#endif /* PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_DQ_HPP_ */ diff --git a/registration/include/pcl/registration/ndt.h b/registration/include/pcl/registration/ndt.h index e19c6d391ba..26115717bd7 100644 --- a/registration/include/pcl/registration/ndt.h +++ b/registration/include/pcl/registration/ndt.h @@ -126,7 +126,7 @@ class NormalDistributionsTransform // Prevents unnecessary voxel initiations if (resolution_ != resolution) { resolution_ = resolution; - if (input_) { + if (target_) { // init() needs target_ init(); } } diff --git a/registration/include/pcl/registration/transformation_estimation_dq.h b/registration/include/pcl/registration/transformation_estimation_dq.h deleted file mode 100644 index 5ad76cc6cc8..00000000000 --- a/registration/include/pcl/registration/transformation_estimation_dq.h +++ /dev/null @@ -1,138 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Point Cloud Library (PCL) - www.pointclouds.org - * Copyright (c) 2010-2011, Willow Garage, Inc. - * Copyright (c) 2012-, Open Perception, Inc. - * - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder(s) nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * - */ - -#pragma once - -#include -#include -PCL_DEPRECATED_HEADER(1, - 15, - "TransformationEstimationDQ has been renamed to " - "TransformationEstimationDualQuaternion."); - -namespace pcl { -namespace registration { -/** @b TransformationEstimationDQ implements dual quaternion based estimation of - * the transformation aligning the given correspondences. - * - * \note The class is templated on the source and target point types as well as on the - * output scalar of the transformation matrix (i.e., float or double). Default: float. - * \author Sergey Zagoruyko - * \ingroup registration - */ -template -class TransformationEstimationDQ -: public TransformationEstimation { -public: - using Ptr = shared_ptr>; - using ConstPtr = - shared_ptr>; - - using Matrix4 = - typename TransformationEstimation::Matrix4; - - TransformationEstimationDQ(){}; - - /** \brief Estimate a rigid rotation transformation between a source and a target - * point cloud using dual quaternion optimization \param[in] cloud_src the source - * point cloud dataset \param[in] cloud_tgt the target point cloud dataset \param[out] - * transformation_matrix the resultant transformation matrix - */ - inline void - estimateRigidTransformation(const pcl::PointCloud& cloud_src, - const pcl::PointCloud& cloud_tgt, - Matrix4& transformation_matrix) const; - - /** \brief Estimate a rigid rotation transformation between a source and a target - * point cloud using dual quaternion optimization \param[in] cloud_src the source - * point cloud dataset \param[in] indices_src the vector of indices describing the - * points of interest in \a cloud_src - * \param[in] cloud_tgt the target point cloud dataset - * \param[out] transformation_matrix the resultant transformation matrix - */ - inline void - estimateRigidTransformation(const pcl::PointCloud& cloud_src, - const pcl::Indices& indices_src, - const pcl::PointCloud& cloud_tgt, - Matrix4& transformation_matrix) const; - - /** \brief Estimate a rigid rotation transformation between a source and a target - * point cloud using dual quaternion optimization \param[in] cloud_src the source - * point cloud dataset \param[in] indices_src the vector of indices describing the - * points of interest in \a cloud_src - * \param[in] cloud_tgt the target point cloud dataset - * \param[in] indices_tgt the vector of indices describing the correspondences of the - * interest points from \a indices_src - * \param[out] transformation_matrix the resultant transformation matrix - */ - inline void - estimateRigidTransformation(const pcl::PointCloud& cloud_src, - const pcl::Indices& indices_src, - const pcl::PointCloud& cloud_tgt, - const pcl::Indices& indices_tgt, - Matrix4& transformation_matrix) const; - - /** \brief Estimate a rigid rotation transformation between a source and a target - * point cloud using dual quaternion optimization \param[in] cloud_src the source - * point cloud dataset \param[in] cloud_tgt the target point cloud dataset \param[in] - * correspondences the vector of correspondences between source and target point cloud - * \param[out] transformation_matrix the resultant transformation matrix - */ - void - estimateRigidTransformation(const pcl::PointCloud& cloud_src, - const pcl::PointCloud& cloud_tgt, - const pcl::Correspondences& correspondences, - Matrix4& transformation_matrix) const; - -protected: - /** \brief Estimate a rigid rotation transformation between a source and a target - * \param[in] source_it an iterator over the source point cloud dataset - * \param[in] target_it an iterator over the target point cloud dataset - * \param[out] transformation_matrix the resultant transformation matrix - */ - void - estimateRigidTransformation(ConstCloudIterator& source_it, - ConstCloudIterator& target_it, - Matrix4& transformation_matrix) const; -}; - -} // namespace registration -} // namespace pcl - -#include diff --git a/registration/src/transformation_estimation_dq.cpp b/registration/src/transformation_estimation_dq.cpp deleted file mode 100644 index a6d388fcf9e..00000000000 --- a/registration/src/transformation_estimation_dq.cpp +++ /dev/null @@ -1,39 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011, Alexandru-Eugen Ichim - * Willow Garage, Inc - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * $Id: transformation_estimation_svd.cpp 7153 2012-09-16 22:24:29Z aichim $ - * - */ - -#include diff --git a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_ellipse3d.hpp b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_ellipse3d.hpp index 33740e44904..abb0d32c9c4 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_ellipse3d.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_ellipse3d.hpp @@ -451,10 +451,9 @@ pcl::SampleConsensusModelEllipse3D::optimizeModelCoefficients ( OptimizationFunctor functor(this, inliers); Eigen::NumericalDiff num_diff(functor); Eigen::LevenbergMarquardt, double> lm(num_diff); - Eigen::VectorXd coeff; + Eigen::VectorXd coeff = model_coefficients.cast(); int info = lm.minimize(coeff); - for (Eigen::Index i = 0; i < coeff.size (); ++i) - optimized_coefficients[i] = static_cast (coeff[i]); + optimized_coefficients = coeff.cast(); // Compute the L2 norm of the residuals PCL_DEBUG ("[pcl::SampleConsensusModelEllipse3D::optimizeModelCoefficients] LM solver finished with exit code %i, having a residual norm of %g. \nInitial solution: %g %g %g %g %g %g %g %g %g %g %g\nFinal solution: %g %g %g %g %g %g %g %g %g %g %g\n", diff --git a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_torus.hpp b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_torus.hpp index dd9e7699c4d..81cd1070c1e 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_torus.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_torus.hpp @@ -86,7 +86,7 @@ pcl::SampleConsensusModelTorus::isSampleGood( ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// float -crossDot(Eigen::Vector3f v1, Eigen::Vector3f v2, Eigen::Vector3f v3) +crossDot(const Eigen::Vector3f& v1, const Eigen::Vector3f& v2, const Eigen::Vector3f& v3) { return v1.cross(v2).dot(v3); } diff --git a/sample_consensus/sample_consensus.doxy b/sample_consensus/sample_consensus.doxy index 914722bb28b..5ad7932867c 100644 --- a/sample_consensus/sample_consensus.doxy +++ b/sample_consensus/sample_consensus.doxy @@ -23,7 +23,7 @@
  • \link pcl::SampleConsensusModelSphere SACMODEL_SPHERE \endlink - used to determine sphere models. The four coefficients of the sphere are given by its 3D center and radius as: [center.x center.y center.z radius]
  • \link pcl::SampleConsensusModelCylinder SACMODEL_CYLINDER \endlink - used to determine cylinder models. The seven coefficients of the cylinder are given by a point on its axis, the axis direction, and a radius, as: [point_on_axis.x point_on_axis.y point_on_axis.z axis_direction.x axis_direction.y axis_direction.z radius]
  • \link pcl::SampleConsensusModelCone SACMODEL_CONE \endlink - used to determine cone models. The seven coefficients of the cone are given by a point of its apex, the axis direction and the opening angle, as: [apex.x, apex.y, apex.z, axis_direction.x, axis_direction.y, axis_direction.z, opening_angle]
    • -
  • SACMODEL_TORUS - not implemented yet
    • +
  • \link pcl::SampleConsensusModelTorus SACMODEL_TORUS \endlink - used to determine torus models. The eight coefficients of the torus are given by the inner and outer radius, the center point, and the torus axis.
  • \link pcl::SampleConsensusModelParallelLine SACMODEL_PARALLEL_LINE \endlink - a model for determining a line parallel with a given axis, within a maximum specified angular deviation. The line coefficients are similar to \link pcl::SampleConsensusModelLine SACMODEL_LINE \endlink.
  • \link pcl::SampleConsensusModelPerpendicularPlane SACMODEL_PERPENDICULAR_PLANE \endlink - a model for determining a plane perpendicular to a user-specified axis, within a maximum specified angular deviation. The plane coefficients are similar to \link pcl::SampleConsensusModelPlane SACMODEL_PLANE \endlink.
  • SACMODEL_PARALLEL_LINES - not implemented yet
    • diff --git a/surface/include/pcl/surface/impl/grid_projection.hpp b/surface/include/pcl/surface/impl/grid_projection.hpp index c5e232fa606..6147e51fea2 100644 --- a/surface/include/pcl/surface/impl/grid_projection.hpp +++ b/surface/include/pcl/surface/impl/grid_projection.hpp @@ -630,9 +630,7 @@ pcl::GridProjection::reconstructPolygons (std::vector &p for (pcl::index_t cp = 0; cp < static_cast (data_->size ()); ++cp) { // Check if the point is invalid - if (!std::isfinite ((*data_)[cp].x) || - !std::isfinite ((*data_)[cp].y) || - !std::isfinite ((*data_)[cp].z)) + if (!pcl::isXYZFinite((*data_)[cp])) continue; Eigen::Vector3i index_3d; diff --git a/visualization/include/pcl/visualization/impl/pcl_visualizer.hpp b/visualization/include/pcl/visualization/impl/pcl_visualizer.hpp index 08c81f08e23..f8060868d75 100644 --- a/visualization/include/pcl/visualization/impl/pcl_visualizer.hpp +++ b/visualization/include/pcl/visualization/impl/pcl_visualizer.hpp @@ -259,9 +259,7 @@ pcl::visualization::PCLVisualizer::convertPointCloudToVTKPolyData ( for (vtkIdType i = 0; i < nr_points; ++i) { // Check if the point is invalid - if (!std::isfinite ((*cloud)[i].x) || - !std::isfinite ((*cloud)[i].y) || - !std::isfinite ((*cloud)[i].z)) + if (!pcl::isXYZFinite((*cloud)[i])) continue; std::copy (&(*cloud)[i].x, &(*cloud)[i].x + 3, &data[ptr]); diff --git a/visualization/include/pcl/visualization/impl/point_cloud_color_handlers.hpp b/visualization/include/pcl/visualization/impl/point_cloud_color_handlers.hpp index 991991db08f..7441dd8a4a9 100644 --- a/visualization/include/pcl/visualization/impl/point_cloud_color_handlers.hpp +++ b/visualization/include/pcl/visualization/impl/point_cloud_color_handlers.hpp @@ -171,9 +171,7 @@ PointCloudColorHandlerRGBField::getColor () const for (vtkIdType cp = 0; cp < nr_points; ++cp) { // Copy the value at the specified field - if (!std::isfinite ((*cloud_)[cp].x) || - !std::isfinite ((*cloud_)[cp].y) || - !std::isfinite ((*cloud_)[cp].z)) + if (!pcl::isXYZFinite((*cloud_)[cp])) continue; memcpy (&rgb, (reinterpret_cast (&(*cloud_)[cp])) + rgba_offset, sizeof (pcl::RGB)); colors[j ] = rgb.r; @@ -256,9 +254,7 @@ PointCloudColorHandlerHSVField::getColor () const for (vtkIdType cp = 0; cp < nr_points; ++cp) { // Copy the value at the specified field - if (!std::isfinite ((*cloud_)[cp].x) || - !std::isfinite ((*cloud_)[cp].y) || - !std::isfinite ((*cloud_)[cp].z)) + if (!pcl::isXYZFinite((*cloud_)[cp])) continue; ///@todo do this with the point_types_conversion in common, first template it! @@ -409,7 +405,7 @@ PointCloudColorHandlerGenericField::getColor () const for (vtkIdType cp = 0; cp < nr_points; ++cp) { // Copy the value at the specified field - if (!std::isfinite ((*cloud_)[cp].x) || !std::isfinite ((*cloud_)[cp].y) || !std::isfinite ((*cloud_)[cp].z)) + if (!pcl::isXYZFinite((*cloud_)[cp])) continue; const std::uint8_t* pt_data = reinterpret_cast (&(*cloud_)[cp]); @@ -479,9 +475,7 @@ PointCloudColorHandlerRGBAField::getColor () const for (vtkIdType cp = 0; cp < nr_points; ++cp) { // Copy the value at the specified field - if (!std::isfinite ((*cloud_)[cp].x) || - !std::isfinite ((*cloud_)[cp].y) || - !std::isfinite ((*cloud_)[cp].z)) + if (!pcl::isXYZFinite((*cloud_)[cp])) continue; colors[j ] = (*cloud_)[cp].r; diff --git a/visualization/src/pcl_visualizer.cpp b/visualization/src/pcl_visualizer.cpp index 64df8564a1c..902ee201657 100644 --- a/visualization/src/pcl_visualizer.cpp +++ b/visualization/src/pcl_visualizer.cpp @@ -4358,7 +4358,7 @@ int pcl::visualization::PCLVisualizer::getGeometryHandlerIndex (const std::string &id) { auto am_it = style_->getCloudActorMap ()->find (id); - if (am_it != cloud_actor_map_->end ()) + if (am_it == cloud_actor_map_->end ()) return (-1); return (am_it->second.geometry_handler_index_); @@ -4593,7 +4593,7 @@ pcl::visualization::PCLVisualizer::getUniqueCameraFile (int argc, char **argv) boost::uuids::detail::sha1::digest_type digest; sha1.get_digest (digest); sstream << "."; - for (int i = 0; i < 5; ++i) { + for (int i = 0; i < static_cast(sizeof(digest)/sizeof(unsigned int)); ++i) { sstream << std::hex << *(reinterpret_cast(&digest[0]) + i); } sstream << ".cam";