From c33abea940c94ae4a493b4a5b153e04a0a7dc3ae Mon Sep 17 00:00:00 2001 From: Daniel Girardeau-Montaut Date: Sun, 10 Nov 2024 00:19:03 +0100 Subject: [PATCH] Scalar fields now rely on an internal offset to properly manage double values (as long as the min and max values are not too far from each other!) --- CMakeLists.txt | 16 ---- include/CCConst.h | 2 +- include/CCTypes.h | 6 -- include/GenericDistribution.h | 42 +++++++-- include/PointCloudTpl.h | 38 ++++++-- include/ScalarField.h | 148 +++++++++++++++++++++++++------- src/AutoSegmentationTools.cpp | 35 ++++---- src/DgmOctreeReferenceCloud.cpp | 6 +- src/NormalDistribution.cpp | 6 +- src/ReferenceCloud.cpp | 8 +- src/RegistrationTools.cpp | 63 ++++++++++++-- src/ScalarField.cpp | 65 ++++++++++---- src/WeibullDistribution.cpp | 31 +++++-- 13 files changed, 340 insertions(+), 126 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 0554314..7833242 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -29,10 +29,6 @@ option( CCCORELIB_SHARED "Compile CCCoreLib as a shared library" ON ) -option( CCCORELIB_SCALAR_DOUBLE - "Define ScalarType as double (instead of float)" - OFF -) set(CMAKE_POSITION_INDEPENDENT_CODE ON) @@ -119,18 +115,6 @@ target_compile_definitions( CCCoreLib "$<$:CC_DEBUG>" ) -if ( CCCORELIB_SCALAR_DOUBLE ) - target_compile_definitions( CCCoreLib - PUBLIC - CC_CORE_LIB_USES_DOUBLE - ) -else() - target_compile_definitions( CCCoreLib - PUBLIC - CC_CORE_LIB_USES_FLOAT - ) -endif() - # Nanoflann option(NANOFLANN_BUILD_BENCHMARKS "" OFF) option(NANOFLANN_BUILD_EXAMPLES "" OFF) diff --git a/include/CCConst.h b/include/CCConst.h index f5e8f4a..5a86917 100644 --- a/include/CCConst.h +++ b/include/CCConst.h @@ -76,7 +76,7 @@ namespace CCCoreLib }; //! Min number of points to compute local models (see CC_LOCAL_MODEL_TYPES) - constexpr unsigned CC_LOCAL_MODEL_MIN_SIZE[] = { + constexpr unsigned CC_LOCAL_MODEL_MIN_SIZE[] { 1, //!< for single point model (i.e. no model ;) 3, //!< for least Square best fitting plane 3, //!< for Delaunay triangulation (2.5D) diff --git a/include/CCTypes.h b/include/CCTypes.h index eafa140..7f131b0 100644 --- a/include/CCTypes.h +++ b/include/CCTypes.h @@ -9,10 +9,4 @@ using PointCoordinateType = float; //! Type of a single scalar field value -#if defined CC_CORE_LIB_USES_DOUBLE using ScalarType = double; -#elif defined CC_CORE_LIB_USES_FLOAT -using ScalarType = float; -#else -static_assert(false, "type for ScalarType has not been declared"); -#endif diff --git a/include/GenericDistribution.h b/include/GenericDistribution.h index f5b9d8c..716e52e 100644 --- a/include/GenericDistribution.h +++ b/include/GenericDistribution.h @@ -4,11 +4,7 @@ #pragma once //Local -#include "CCCoreLib.h" -#include "CCTypes.h" - -//system -#include +#include "ScalarField.h" namespace CCCoreLib { @@ -39,8 +35,40 @@ namespace CCCoreLib **/ virtual bool isValid() const { return m_isValid; } - //! Scalar values container - using ScalarContainer = std::vector; + //! Scalar values container interface + struct ScalarContainer + { + virtual size_t size() const = 0; + virtual ScalarType getValue(size_t index) const = 0; + }; + + //! Wrapper of a CCCoreLib's scalar field as a Scalar values container + struct SFAsScalarContainer : ScalarContainer + { + SFAsScalarContainer(const ScalarField& sf) + : ScalarContainer() + , m_sf(sf) + {} + + inline size_t size() const override { return m_sf.size(); } + inline ScalarType getValue(size_t index) const override { return m_sf.getValue(index); } + + const CCCoreLib::ScalarField& m_sf; + }; + + //! Wrapper of a std::vector as a Scalar values container + struct VectorAsScalarContainer : ScalarContainer + { + VectorAsScalarContainer(const std::vector& vector) + : ScalarContainer() + , m_vector(vector) + {} + + inline size_t size() const override { return m_vector.size(); } + inline ScalarType getValue(size_t index) const override { return m_vector[index]; } + + const std::vector& m_vector; + }; //! Computes the distribution parameters from a set of values /** \param values a set of scalar values diff --git a/include/PointCloudTpl.h b/include/PointCloudTpl.h index 6e4d7e7..c7af351 100644 --- a/include/PointCloudTpl.h +++ b/include/PointCloudTpl.h @@ -92,9 +92,27 @@ namespace CCCoreLib } unsigned n = size(); - for (unsigned i = 0; i < n; ++i) + + if (0 != n) { - action(m_points[i], (*currentOutScalarFieldArray)[i]); + double previousOffset = currentOutScalarFieldArray->getOffset(); + if (n == currentOutScalarFieldArray->size()) + { + // if we are going to change ALL the values, we can also apply the functor on the offset + double firstValue = currentOutScalarFieldArray->getValue(0); + action(m_points.front(), firstValue); + if (ScalarField::ValidValue(firstValue)) + { + currentOutScalarFieldArray->setOffset(firstValue); + } + } + + for (unsigned i = 0; i < n; ++i) + { + ScalarType value = previousOffset + currentOutScalarFieldArray->getLocalValue(i); // warning, the offset has been changed, we can't use getValue anymore + action(m_points[i], value); + currentOutScalarFieldArray->setValue(i, value); + } } } @@ -244,7 +262,7 @@ namespace CCCoreLib //if something fails, we restore the previous size for already processed SFs! for (std::size_t j = 0; j < i; ++j) { - m_scalarFields[j]->resize(oldCount); + m_scalarFields[j]->resizeSafe(oldCount); m_scalarFields[j]->computeMinAndMax(); } //we can assume that newNumberOfPoints > oldCount, so it should always be ok @@ -348,23 +366,25 @@ namespace CCCoreLib /** \param index a scalar field index \return a pointer to a string structure (null-terminated array of characters), or 0 if the index is invalid. **/ - const char* getScalarFieldName(int index) const + const std::string getScalarFieldName(int index) const { - return (index >= 0 && index < static_cast(m_scalarFields.size()) ? m_scalarFields[index]->getName() : 0); + return (index >= 0 && index < static_cast(m_scalarFields.size()) ? m_scalarFields[index]->getName() : std::string{}); } //! Returns the index of a scalar field represented by its name /** \param name a scalar field name \return an index (-1 if the scalar field couldn't be found) **/ - int getScalarFieldIndexByName(const char* name) const + int getScalarFieldIndexByName(const std::string& name) const { std::size_t sfCount = m_scalarFields.size(); for (std::size_t i = 0; i < sfCount; ++i) { //we don't accept two SF with the same name! - if (strcmp(m_scalarFields[i]->getName(), name) == 0) + if (0 == m_scalarFields[i]->getName().compare(name)) + { return static_cast(i); + } } return -1; @@ -414,7 +434,7 @@ namespace CCCoreLib \param uniqueName scalar field name (must be unique) \return index of this new scalar field (or -1 if an error occurred) **/ - virtual int addScalarField(const char* uniqueName) + virtual int addScalarField(const std::string& uniqueName) { //we don't accept two SF with the same name! if (getScalarFieldIndexByName(uniqueName) >= 0) @@ -455,7 +475,7 @@ namespace CCCoreLib \param newName new name \return success **/ - bool renameScalarField(int index, const char* newName) + bool renameScalarField(int index, const std::string& newName) { if (getScalarFieldIndexByName(newName) < 0) { diff --git a/include/ScalarField.h b/include/ScalarField.h index dd31fe6..08ea392 100644 --- a/include/ScalarField.h +++ b/include/ScalarField.h @@ -9,6 +9,7 @@ //System #include +#include namespace CCCoreLib { @@ -16,17 +17,31 @@ namespace CCCoreLib /** A monodimensional array of scalar values. It has also specific parameters for display purposes. + It is now using a base offset value of type double, and internally + stores the values as float to limit the memory consumption. + Invalid values can be represented by CCCoreLib::NAN_VALUE. **/ - class ScalarField : public std::vector, public CCShareable + class ScalarField : protected std::vector, public CCShareable { public: + //! Shortcut to the (protected) std::vector::size() method + using std::vector::size; + //! Shortcut to the (protected) std::vector::capacity() method + using std::vector::capacity; + //! Shortcut to the (protected) std::vector::reserve() method + using std::vector::reserve; + //! Shortcut to the (protected) std::vector::shrink_to_fit() method + using std::vector::shrink_to_fit; + //! Shortcut to the (protected) std::vector::empty() method + using std::vector::empty; + //! Default constructor /** [SHAREABLE] Call 'link' when associating this structure to an object. \param name scalar field name **/ - CC_CORE_LIB_API explicit ScalarField(const char* name = nullptr); + CC_CORE_LIB_API explicit ScalarField(const std::string& name = std::string()); //! Copy constructor /** \param sf scalar field to copy @@ -35,19 +50,39 @@ namespace CCCoreLib CC_CORE_LIB_API ScalarField(const ScalarField& sf); //! Sets scalar field name - CC_CORE_LIB_API void setName(const char* name); + CC_CORE_LIB_API void setName(const std::string& name); //! Returns scalar field name - inline const char* getName() const { return m_name; } + inline const std::string& getName() const { return m_name; } //! Returns the specific NaN value static inline ScalarType NaN() { return NAN_VALUE; } + //! Returns the offset + inline double getOffset() const { return m_offset; } + + //! Sets the offset + /** \warning Dangerous. All values inside the vector are relative to the offset! + \param offset the new offset value + **/ + inline void setOffset(double offset) + { + m_offsetHasBeenSet = true; + m_offset = offset; + } + + //! Clears the scalar field + inline void clear() + { + std::vector::clear(); + m_offsetHasBeenSet = false; + } + //! Computes the mean value (and optionally the variance value) of the scalar field /** \param mean a field to store the mean value \param variance if not void, the variance will be computed and stored here **/ - CC_CORE_LIB_API void computeMeanAndVariance(ScalarType &mean, ScalarType* variance = nullptr) const; + CC_CORE_LIB_API void computeMeanAndVariance(ScalarType& mean, ScalarType* variance = nullptr) const; //! Determines the min and max values CC_CORE_LIB_API virtual void computeMinAndMax(); @@ -56,18 +91,38 @@ namespace CCCoreLib static inline bool ValidValue(ScalarType value) { return std::isfinite(value); } //! Sets the value as 'invalid' (i.e. CCCoreLib::NAN_VALUE) - inline void flagValueAsInvalid(std::size_t index) { at(index) = NaN(); } + inline void flagValueAsInvalid(std::size_t index) { (*this)[index] = std::numeric_limits::quiet_NaN(); } //! Returns the number of valid values in this scalar field CC_CORE_LIB_API std::size_t countValidValues() const; //! Returns the minimum value - inline ScalarType getMin() const { return m_minVal; } + inline ScalarType getMin() const { return m_offset + m_localMinVal; } //! Returns the maximum value - inline ScalarType getMax() const { return m_maxVal; } + inline ScalarType getMax() const { return m_offset + m_localMaxVal; } //! Fills the array with a particular value - inline void fill(ScalarType fillValue = 0) { if (empty()) resize(capacity(), fillValue); else std::fill(begin(), end(), fillValue); } + inline void fill(ScalarType fillValue = 0) + { + float fillValueF = 0.0f; + if (m_offsetHasBeenSet) + { + fillValueF = static_cast(fillValue - m_offset); + } + else + { + setOffset(fillValue); + } + + if (empty()) + { + resize(capacity(), fillValueF); + } + else + { + std::fill(begin(), end(), fillValueF); + } + } //! Reserves memory (no exception thrown) CC_CORE_LIB_API bool reserveSafe(std::size_t count); @@ -75,11 +130,41 @@ namespace CCCoreLib CC_CORE_LIB_API bool resizeSafe(std::size_t count, bool initNewElements = false, ScalarType valueForNewElements = 0); //Shortcuts (for backward compatibility) - inline ScalarType& getValue(std::size_t index) { return at(index); } - inline const ScalarType& getValue(std::size_t index) const { return at(index); } - inline void setValue(std::size_t index, ScalarType value) { at(index) = value; } - inline void addElement(ScalarType value) { push_back(value); } + inline ScalarType getValue(std::size_t index) const { return m_offset + (*this)[index]; } + + inline float getLocalValue(std::size_t index) const { return (*this)[index]; } + inline void setLocalValue(std::size_t index, float value) { (*this)[index] = value; } + inline const float* getLocalValues() const { return data(); } + + inline void setValue(std::size_t index, ScalarType value) + { + if (m_offsetHasBeenSet) + { + (*this)[index] = static_cast(value - m_offset); + } + else + { + setOffset(value); + (*this)[index] = 0.0f; + } + } + + inline void addElement(ScalarType value) + { + if (m_offsetHasBeenSet) + { + push_back(static_cast(value - m_offset)); + } + else + { + // if the offset has not been set yet, we use the first value by default + setOffset(value); + push_back(0.0f); + } + } + inline unsigned currentSize() const { return static_cast(size()); } + inline void swap(std::size_t i1, std::size_t i2) { std::swap(at(i1), at(i2)); } protected: //methods @@ -92,36 +177,41 @@ namespace CCCoreLib protected: //members //! Scalar field name - char m_name[256]; + std::string m_name; private: - //! Minimum value - ScalarType m_minVal; - //! Maximum value - ScalarType m_maxVal; + //! Offset value (local to global) + double m_offset; + //! Whether the offset has been set or not + bool m_offsetHasBeenSet; + //! Minimum value (local) + float m_localMinVal; + //! Maximum value (local) + float m_localMaxVal; }; inline void ScalarField::computeMinAndMax() { - ScalarType minVal, maxVal; + float localMinVal = 0.0f; + float localMaxVal = 0.0f; bool minMaxInitialized = false; for (std::size_t i = 0; i < size(); ++i) { - const ScalarType& val = at(i); - if (ValidValue(val)) + float val = at(i); + if (std::isfinite(val)) { if (minMaxInitialized) { - if (val < minVal) - minVal = val; - else if (val > maxVal) - maxVal = val; + if (val < localMinVal) + localMinVal = val; + else if (val > localMaxVal) + localMaxVal = val; } else { //first valid value is used to init min and max - minVal = maxVal = val; + localMinVal = localMaxVal = val; minMaxInitialized = true; } } @@ -129,12 +219,12 @@ namespace CCCoreLib if (minMaxInitialized) { - m_minVal = minVal; - m_maxVal = maxVal; + m_localMinVal = localMinVal; + m_localMaxVal = localMaxVal; } else //particular case: zero valid values { - m_minVal = m_maxVal = 0; + m_localMinVal = m_localMaxVal = 0.0; } } } diff --git a/src/AutoSegmentationTools.cpp b/src/AutoSegmentationTools.cpp index 8ded5af..bed1283 100644 --- a/src/AutoSegmentationTools.cpp +++ b/src/AutoSegmentationTools.cpp @@ -12,6 +12,7 @@ //System #include +#include using namespace CCCoreLib; @@ -151,7 +152,7 @@ bool AutoSegmentationTools::frontPropagationBasedSegmentation( GenericIndexedClo } } - //on calcule le gradient (va ecraser le champ des distances) + //we compute the gradient (may overwrite the distances SF) if (ScalarFieldTools::computeScalarFieldGradient(theCloud, radius, true, true, progressCb, theOctree) < 0) { if (nullptr == inputOctree) @@ -161,7 +162,7 @@ bool AutoSegmentationTools::frontPropagationBasedSegmentation( GenericIndexedClo return false; } - //et on lisse le resultat + //we optionally smooth the result if (applyGaussianFilter) { ScalarFieldTools::applyScalarFieldGaussianFilter(radius / 3, theCloud, -1, progressCb, theOctree); @@ -170,8 +171,8 @@ bool AutoSegmentationTools::frontPropagationBasedSegmentation( GenericIndexedClo unsigned seedPoints = 0; unsigned numberOfSegmentedLists = 0; - //on va faire la propagation avec le FastMarching(); - FastMarchingForPropagation* fm = new FastMarchingForPropagation(); + //start the FastMarching front propagation + std::unique_ptr fm(new FastMarchingForPropagation()); { fm->setJumpCoef(50.0); fm->setDetectionThreshold(alpha); @@ -183,7 +184,6 @@ bool AutoSegmentationTools::frontPropagationBasedSegmentation( GenericIndexedClo { delete theOctree; } - delete fm; return false; } } @@ -212,7 +212,6 @@ bool AutoSegmentationTools::frontPropagationBasedSegmentation( GenericIndexedClo delete theOctree; } theDists->release(); - delete fm; return false; } } @@ -226,20 +225,24 @@ bool AutoSegmentationTools::frontPropagationBasedSegmentation( GenericIndexedClo ScalarType maxDist = NAN_VALUE; //on cherche la premiere distance superieure ou egale a "minSeedDist" - while (begingetPoint(begin); - const ScalarType& theDistance = theDists->at(begin); + const CCVector3* thePoint = theCloud->getPoint(begin); + const ScalarType theDistance = theDists->getValue(begin); ++begin; - //FIXME DGM: what happens if SF is negative?! - if (theCloud->getPointScalarValue(begin) >= 0 && theDistance >= minSeedDist) + if ( (theCloud->getPointScalarValue(begin) >= 0) + && (theDistance >= minSeedDist) ) { maxDist = theDistance; startPoint = *thePoint; maxDistIndex = begin; break; } + else + { + //FIXME DGM: what happens if SF is negative?! + } } //il n'y a plus de point avec des distances suffisamment grandes ! @@ -252,9 +255,10 @@ bool AutoSegmentationTools::frontPropagationBasedSegmentation( GenericIndexedClo for (unsigned i = begin; i < numberOfPoints; ++i) { const CCVector3 *thePoint = theCloud->getPoint(i); - const ScalarType& theDistance = theDists->at(i); + const ScalarType theDistance = theDists->getValue(i); - if ((theCloud->getPointScalarValue(i) >= 0.0) && (theDistance > maxDist)) + if ( (theCloud->getPointScalarValue(i) >= 0.0) + && (theDistance > maxDist) ) { maxDist = theDistance; startPoint = *thePoint; @@ -317,15 +321,12 @@ bool AutoSegmentationTools::frontPropagationBasedSegmentation( GenericIndexedClo for (unsigned i = 0; i < numberOfPoints; ++i) { - theCloud->setPointScalarValue(i, theDists->at(i)); + theCloud->setPointScalarValue(i, theDists->getValue(i)); } theDists->release(); theDists = nullptr; - delete fm; - fm = nullptr; - if (nullptr == inputOctree) { delete theOctree; diff --git a/src/DgmOctreeReferenceCloud.cpp b/src/DgmOctreeReferenceCloud.cpp index 284f49d..d02deb5 100644 --- a/src/DgmOctreeReferenceCloud.cpp +++ b/src/DgmOctreeReferenceCloud.cpp @@ -55,7 +55,9 @@ void DgmOctreeReferenceCloud::computeBB() void DgmOctreeReferenceCloud::getBoundingBox(CCVector3& bbMin, CCVector3& bbMax) { if (!m_validBB) + { computeBB(); + } bbMin = m_bbMin; bbMax = m_bbMax; @@ -64,11 +66,11 @@ void DgmOctreeReferenceCloud::getBoundingBox(CCVector3& bbMin, CCVector3& bbMax) void DgmOctreeReferenceCloud::forEach(genericPointAction action) { unsigned count = size(); - for (unsigned i=0; i(m_set->at(i).squareDistd); - action(*m_set->at(i).point,sqDist); + action(*m_set->at(i).point, sqDist); m_set->at(i).squareDistd = static_cast(sqDist); } } diff --git a/src/NormalDistribution.cpp b/src/NormalDistribution.cpp index c2b32d1..29c397e 100644 --- a/src/NormalDistribution.cpp +++ b/src/NormalDistribution.cpp @@ -122,8 +122,9 @@ bool NormalDistribution::computeParameters(const ScalarContainer& values) double stddev2 = 0.0; unsigned counter = 0; - for (ScalarType v : values) + for (size_t i = 0; i< values.size(); ++i) { + ScalarType v = values.getValue(i); if (ScalarField::ValidValue(v)) { mean += v; @@ -155,8 +156,9 @@ bool NormalDistribution::computeRobustParameters(const ScalarContainer& values, double mean = 0.0; double stddev2 = 0.0; - for (ScalarType v : values) + for (size_t i = 0; i < values.size(); ++i) { + ScalarType v = values.getValue(i); if (static_cast(std::abs(v - m_mu)) < maxStddev) { mean += v; diff --git a/src/ReferenceCloud.cpp b/src/ReferenceCloud.cpp index b2436b4..37137a0 100644 --- a/src/ReferenceCloud.cpp +++ b/src/ReferenceCloud.cpp @@ -159,12 +159,10 @@ void ReferenceCloud::forEach(genericPointAction action) unsigned count = size(); for (unsigned i = 0; i < count; ++i) { - const unsigned& index = m_theIndexes[i]; + unsigned index = m_theIndexes[i]; ScalarType d = m_theAssociatedCloud->getPointScalarValue(index); - ScalarType d2 = d; - action(*m_theAssociatedCloud->getPointPersistentPtr(index), d2); - if (d != d2) - m_theAssociatedCloud->setPointScalarValue(index, d2); + action(*m_theAssociatedCloud->getPointPersistentPtr(index), d); + m_theAssociatedCloud->setPointScalarValue(index, d); } } diff --git a/src/RegistrationTools.cpp b/src/RegistrationTools.cpp index cc8f8c8..91f26b8 100644 --- a/src/RegistrationTools.cpp +++ b/src/RegistrationTools.cpp @@ -468,31 +468,61 @@ ICPRegistrationTools::RESULT_TYPE ICPRegistrationTools::Register( GenericIndexed { filteredData.cloud->addPointIndex(data.cloud->getPointGlobalIndex(i)); if (filteredData.CPSetRef) + { filteredData.CPSetRef->addPointIndex(data.CPSetRef->getPointGlobalIndex(i)); + } else if (filteredData.CPSetPlain) + { filteredData.CPSetPlain->addPoint(*(data.CPSetPlain->getPoint(i))); + } if (filteredData.weights) + { filteredData.weights->addElement(data.weights->getValue(i)); + } } } //resize should be ok as we have called reserve first filteredData.cloud->resize(filteredData.cloud->size()); //should always be ok as current size < pointCount if (filteredData.CPSetRef) - filteredData.CPSetRef->resize(filteredData.CPSetRef->size()); + { + if (!filteredData.CPSetRef->resize(filteredData.CPSetRef->size())) + { + result = ICP_ERROR_NOT_ENOUGH_MEMORY; + break; + } + } else if (filteredData.CPSetPlain) - filteredData.CPSetPlain->resize(filteredData.CPSetPlain->size()); + { + if (!filteredData.CPSetPlain->resize(filteredData.CPSetPlain->size())) + { + result = ICP_ERROR_NOT_ENOUGH_MEMORY; + break; + } + } if (filteredData.weights) - filteredData.weights->resize(filteredData.weights->currentSize()); + { + if (!filteredData.weights->resizeSafe(filteredData.weights->currentSize())) + { + result = ICP_ERROR_NOT_ENOUGH_MEMORY; + break; + } + } //replace old structures by new ones cloudGarbage.destroy(data.cloud); if (data.CPSetRef) + { cloudGarbage.destroy(data.CPSetRef); + } else if (data.CPSetPlain) + { cloudGarbage.destroy(data.CPSetPlain); + } if (data.weights) + { sfGarbage.destroy(data.weights); + } data = filteredData; pointOrderHasBeenChanged = true; @@ -502,7 +532,8 @@ ICPRegistrationTools::RESULT_TYPE ICPRegistrationTools::Register( GenericIndexed //shall we ignore/remove some points based on their distance? DataCloud trueData; unsigned pointCount = data.cloud->size(); - if (maxOverlapCount != 0 && pointCount > maxOverlapCount) + if ( (0 != maxOverlapCount) + && (pointCount > maxOverlapCount) ) { assert(overlapDistances.size() >= pointCount); for (unsigned i = 0; i < pointCount; ++i) @@ -573,11 +604,29 @@ ICPRegistrationTools::RESULT_TYPE ICPRegistrationTools::Register( GenericIndexed //resize should be ok as we have called reserve first filteredData.cloud->resize(filteredData.cloud->size()); //should always be ok as current size < pointCount if (filteredData.CPSetRef) - filteredData.CPSetRef->resize(filteredData.CPSetRef->size()); + { + if (!filteredData.CPSetRef->resize(filteredData.CPSetRef->size())) + { + result = ICP_ERROR_NOT_ENOUGH_MEMORY; + break; + } + } else if (filteredData.CPSetPlain) - filteredData.CPSetPlain->resize(filteredData.CPSetPlain->size()); + { + if (!filteredData.CPSetPlain->resize(filteredData.CPSetPlain->size())) + { + result = ICP_ERROR_NOT_ENOUGH_MEMORY; + break; + } + } if (filteredData.weights) - filteredData.weights->resize(filteredData.weights->currentSize()); + { + if (!filteredData.weights->resizeSafe(filteredData.weights->currentSize())) + { + result = ICP_ERROR_NOT_ENOUGH_MEMORY; + break; + } + } //(temporarily) replace old structures by new ones trueData = data; diff --git a/src/ScalarField.cpp b/src/ScalarField.cpp index bcf7923..0d5e0a7 100644 --- a/src/ScalarField.cpp +++ b/src/ScalarField.cpp @@ -9,31 +9,46 @@ using namespace CCCoreLib; -ScalarField::ScalarField(const char* name/*=nullptr*/) - : m_minVal{ 0 } - , m_maxVal{ 0 } +ScalarField::ScalarField(const std::string& name/*=std::string()*/) + : m_name{ } + , m_offset{ 0.0 } + , m_offsetHasBeenSet{ false } + , m_localMinVal{ 0.0f } + , m_localMaxVal{ 0.0f } { setName(name); } ScalarField::ScalarField(const ScalarField& sf) - : std::vector(sf) - , m_minVal{ 0 } - , m_maxVal{ 0 } + : std::vector(sf) + , m_name{ sf.m_name } + , m_offset{ sf.m_offset } + , m_offsetHasBeenSet{ sf.m_offsetHasBeenSet } + , m_localMinVal{ sf.m_localMinVal } + , m_localMaxVal{ sf.m_localMaxVal } { - setName(sf.m_name); } -void ScalarField::setName(const char* name) +void ScalarField::setName(const std::string& name) { - if (name) - strncpy(m_name, name, 255); + if (name.empty()) + { + m_name = "Undefined"; + } else - strcpy(m_name, "Undefined"); + { + m_name = name; + } } std::size_t ScalarField::countValidValues() const { + if (false == std::isfinite(m_offset)) + { + // special case: if the offset is invalid, all values become invalid! + return size(); + } + std::size_t count = 0; for (std::size_t i = 0; i < size(); ++i) @@ -48,7 +63,7 @@ std::size_t ScalarField::countValidValues() const return count; } -void ScalarField::computeMeanAndVariance(ScalarType &mean, ScalarType* variance) const +void ScalarField::computeMeanAndVariance(ScalarType& mean, ScalarType* variance) const { double _mean = 0.0; double _std2 = 0.0; @@ -56,8 +71,8 @@ void ScalarField::computeMeanAndVariance(ScalarType &mean, ScalarType* variance) for (std::size_t i = 0; i < size(); ++i) { - const ScalarType& val = at(i); - if (ValidValue(val)) + float val = at(i); + if (std::isfinite(val)) { _mean += val; _std2 += static_cast(val) * val; @@ -68,13 +83,16 @@ void ScalarField::computeMeanAndVariance(ScalarType &mean, ScalarType* variance) if (count) { _mean /= count; - mean = static_cast(_mean); + mean = _mean; if (variance) { _std2 = std::abs(_std2 / count - _mean*_mean); *variance = static_cast(_std2); } + + mean += m_offset; // only after the standard deviation has been calculated! + } else { @@ -105,9 +123,24 @@ bool ScalarField::resizeSafe(std::size_t count, bool initNewElements/*=false*/, try { if (initNewElements) - resize(count, valueForNewElements); + { + float fillValueF = 0.0f; + if (m_offsetHasBeenSet) + { + fillValueF = static_cast(valueForNewElements - m_offset); + } + else + { + // if the offset has not been set yet, we use the first value by default + setOffset(valueForNewElements); + } + + resize(count, fillValueF); + } else + { resize(count); + } } catch (const std::bad_alloc&) { diff --git a/src/WeibullDistribution.cpp b/src/WeibullDistribution.cpp index be45b5a..709ad9e 100644 --- a/src/WeibullDistribution.cpp +++ b/src/WeibullDistribution.cpp @@ -166,22 +166,23 @@ bool WeibullDistribution::computeParameters(const ScalarContainer& values) ScalarType minValue = 0; ScalarType maxValue = 0; bool firstValue = true; - for (ScalarType s : values) + for (size_t i = 0; i < values.size(); ++i) { - if (!ScalarField::ValidValue(s)) + ScalarType v = values.getValue(i); + if (!ScalarField::ValidValue(v)) continue; if (firstValue) { - minValue = maxValue = s; + minValue = maxValue = v; firstValue = false; } else { - if (s < minValue) - minValue = s; - else if (s > maxValue) - maxValue = s; + if (v < minValue) + minValue = v; + else if (v > maxValue) + maxValue = v; } } @@ -199,14 +200,16 @@ bool WeibullDistribution::computeParameters(const ScalarContainer& values) double a = FindGRoot(values, minValue, valueRange); if (a < 0.0) + { return false; + } //we can compute b double b = 0; unsigned counter = 0; for (size_t i = 0; i < n; ++i) { - ScalarType v = values[i]; + ScalarType v = values.getValue(i); if (ScalarField::ValidValue(v)) //we ignore NaN values { if (v >= minValue) @@ -217,7 +220,9 @@ bool WeibullDistribution::computeParameters(const ScalarContainer& values) } } if (counter == 0) + { return false; + } return setParameters( static_cast(a), static_cast(valueRange * pow(b / counter, 1.0 / a)), @@ -228,7 +233,9 @@ double WeibullDistribution::computeP(ScalarType _x) const { double x = static_cast(_x - m_valueShift) / m_b; if (x < 0) + { return 0; + } double xp = pow(x, m_a - 1.0); return (static_cast(m_a) / m_b) * xp * exp(-xp*x); @@ -242,9 +249,13 @@ double WeibullDistribution::computePfromZero(ScalarType x) const double WeibullDistribution::computeP(ScalarType x1, ScalarType x2) const { if (x1 < m_valueShift) + { x1 = m_valueShift; + } if (x2 < m_valueShift) + { return 0; + } //pi = computeP(minV+(ScalarType(k)+0.5)*step)*step; //...instead we take the sampling into account and then integrate return exp(-pow(static_cast(x1 - m_valueShift) / m_b, static_cast(m_a))) - exp(-pow(static_cast(x2 - m_valueShift) / m_b, static_cast(m_a))); @@ -256,7 +267,9 @@ double WeibullDistribution::ComputeG(const ScalarContainer& values, double r, Sc //a & n should be strictly positive! if (r <= 0.0 || n == 0) + { return 1.0; //a positive value means that ComputeG failed + } double p = 0; double q = 0; @@ -266,7 +279,7 @@ double WeibullDistribution::ComputeG(const ScalarContainer& values, double r, Sc for (unsigned i = 0; i < n; ++i) { - ScalarType v = values[i]; + ScalarType v = values.getValue(i); if (ScalarField::ValidValue(v)) //we ignore NaN values { double v0 = static_cast(v) - valueShift;