diff --git a/.github/workflows/build-test-linux-vcpkg.yml b/.github/workflows/build-test-linux-vcpkg.yml index 02193273ec42..a87d0ead4178 100644 --- a/.github/workflows/build-test-linux-vcpkg.yml +++ b/.github/workflows/build-test-linux-vcpkg.yml @@ -50,15 +50,6 @@ jobs: compiler: [Clang 19, GCC 11] full_config_build: - ${{ fromJSON( inputs.full_config_build ) }} - exclude: - # Do not run Debug Clang 19 build on every commit (but only once a day) - - full_config_build: false - compiler: Clang 19 - config: Debug - # Do not run Release GCC 11 build on every commit (but only once a day) - - full_config_build: false - compiler: GCC 11 - config: Release include: - compiler: Clang 19 cxx-compiler: /usr/bin/clang++-19 @@ -176,7 +167,9 @@ jobs: run: xvfb-run -a ./build/${{ matrix.config }}/bin/MeshViewer -hidden -noEventLoop -unloadPluginsAtEnd - name: Unit Tests - run: ./build/${{ matrix.config }}/bin/MRTest + run: | + objdump -T ./build/${{ matrix.config }}/bin/libMRMesh.so | c++filt + ./build/${{ matrix.config }}/bin/MRTest - name: C Unit Tests (old bindings) run: ./build/${{ matrix.config }}/bin/MRTestC diff --git a/.github/workflows/build-test-macos.yml b/.github/workflows/build-test-macos.yml index ab6861a1571d..676312ecc0b1 100644 --- a/.github/workflows/build-test-macos.yml +++ b/.github/workflows/build-test-macos.yml @@ -168,7 +168,10 @@ jobs: run: ./build/${{ matrix.config }}/bin/MeshViewer -tryHidden -noEventLoop -unloadPluginsAtEnd - name: Unit Tests - run: ./build/${{ matrix.config }}/bin/MRTest + run: | + export PATH="$(brew --prefix llvm@18)/bin:$PATH" + nm -gU ./build/${{ matrix.config }}/bin/libMRMesh.dylib | llvm-cxxfilt + ./build/${{ matrix.config }}/bin/MRTest - name: C Unit Tests (old bindings) run: ./build/${{ matrix.config }}/bin/MRTestC diff --git a/source/MRMesh/MRICP.cpp b/source/MRMesh/MRICP.cpp index 723b179651d2..93c955a8e90f 100644 --- a/source/MRMesh/MRICP.cpp +++ b/source/MRMesh/MRICP.cpp @@ -7,7 +7,16 @@ #include "MRQuaternion.h" #include "MRBestFit.h" #include "MRBitSetParallelFor.h" +#include "MRStreamOperators.h" +#include "MRPch/MRSpdlog.h" #include +#include "MRSystem.h" +#include +#include "MRRingIterator.h" +#include +#ifdef __linux__ +#include +#endif namespace MR { @@ -68,16 +77,16 @@ AffineXf3f ICP::autoSelectFloatXf() PointAccumulator refAcc; ref_.obj.accumulate( refAcc ); - const auto refBasisXfs = refAcc.get4BasicXfs(); + const auto refBasisXfs = refAcc.get4BasicXfs3f(); PointAccumulator floatAcc; flt_.obj.accumulate( floatAcc ); - const auto floatBasisXf = floatAcc.getBasicXf(); + const auto floatBasisXf = floatAcc.getBasicXf3f(); // TODO: perform computations in parallel by calling free functions to measure the distance for ( const auto & refBasisXf : refBasisXfs ) { - AffineXf3f fltXf( AffineXf3d( ref_.xf ) * refBasisXf * floatBasisXf.inverse() ); + auto fltXf = ref_.xf * refBasisXf * floatBasisXf.inverse(); setFloatXf( fltXf ); updatePointPairs(); const float dist = getMeanSqDistToPoint(); @@ -114,9 +123,13 @@ void ICP::sampleRefPoints( float samplingVoxelSize ) void ICP::updatePointPairs() { MR_TIMER; + spdlog::info( "updatePointPairs flt->ref" ); MR::updatePointPairs( flt2refPairs_, flt_, ref_, prop_.cosThreshold, prop_.distThresholdSq, prop_.mutualClosest ); + spdlog::info( "updatePointPairs ref->flt" ); MR::updatePointPairs( ref2fltPairs_, ref_, flt_, prop_.cosThreshold, prop_.distThresholdSq, prop_.mutualClosest ); deactivatefarDistPairs_(); + spdlog::info( "flt2refPairs_.active = {}", MR::getNumActivePairs( flt2refPairs_ ) ); + spdlog::info( "ref2fltPairs_.active = {}", MR::getNumActivePairs( ref2fltPairs_ ) ); } std::string getICPStatusInfo( int iterations, ICPExitType exitType ) @@ -144,13 +157,50 @@ std::string getICPStatusInfo( int iterations, ICPExitType exitType ) return result; } +void logPointPairs( const PointPairs& pairs ) +{ + for ( int i = 0; i < pairs.vec.size(); ++i ) + { + const auto& pair = pairs.vec[i]; + spdlog::info( "#{}: active={}, srcVertId={}, tgtCloseVert={}, srcPoint=({} {} {}), srcNorm=({} {} {}), tgtPoint=({} {} {}), tgtNorm=({} {} {}), distSq={}, weight={}, normalsAngleCos={}, tgtOnBd={}", + i, pairs.active.test( i ), (int)pair.srcVertId, (int)pair.tgtCloseVert, + pair.srcPoint.x, pair.srcPoint.y, pair.srcPoint.z, + pair.srcNorm.x, pair.srcNorm.y, pair.srcNorm.z, + pair.tgtPoint.x, pair.tgtPoint.y, pair.tgtPoint.z, + pair.tgtNorm.x, pair.tgtNorm.y, pair.tgtNorm.z, + pair.distSq, pair.weight, pair.normalsAngleCos, pair.tgtOnBd ); + } +} + +static void logXf( const char* var, const AffineXf3f& xf ) +{ + spdlog::info( "{} x: {} {} {} {}", var, xf.A.x.x, xf.A.x.y, xf.A.x.z, xf.b.x ); + spdlog::info( "{} y: {} {} {} {}", var, xf.A.y.x, xf.A.y.y, xf.A.y.z, xf.b.y ); + spdlog::info( "{} z: {} {} {} {}", var, xf.A.z.x, xf.A.z.y, xf.A.z.z, xf.b.z ); +} + +static void logXfHex( const char* var, const AffineXf3f& xf ) +{ + auto d = [] ( float f ) + { + return std::bit_cast< std::uint32_t >( f ); + }; + spdlog::info( "{} x: {:08x} {:08x} {:08x} {:08x}", var, d( xf.A.x.x ), d( xf.A.x.y ), d( xf.A.x.z ), d( xf.b.x ) ); + spdlog::info( "{} y: {:08x} {:08x} {:08x} {:08x}", var, d( xf.A.y.x ), d( xf.A.y.y ), d( xf.A.y.z ), d( xf.b.y ) ); + spdlog::info( "{} z: {:08x} {:08x} {:08x} {:08x}", var, d( xf.A.z.x ), d( xf.A.z.y ), d( xf.A.z.z ), d( xf.b.z ) ); +} + void updatePointPairs( PointPairs & pairs, const MeshOrPointsXf& src, const MeshOrPointsXf& tgt, float cosThreshold, float distThresholdSq, bool mutualClosest ) { MR_TIMER; - const AffineXf3f src2tgtXf( AffineXf3d( tgt.xf ).inverse() * AffineXf3d( src.xf ) ); - const AffineXf3f tgt2srcXf( AffineXf3d( src.xf ).inverse() * AffineXf3d( tgt.xf ) ); + const auto src2tgtXf = tgt.xf.inverse() * src.xf; + const auto tgt2srcXf = src.xf.inverse() * tgt.xf; + logXf( "src2tgtXf", src2tgtXf ); + logXfHex( "src2tgtXf", src2tgtXf ); + logXf( "tgt2srcXf", tgt2srcXf ); + logXfHex( "tgt2srcXf", tgt2srcXf ); const VertCoords& srcPoints = src.obj.points(); const VertCoords& tgtPoints = tgt.obj.points(); @@ -166,7 +216,8 @@ void updatePointPairs( PointPairs & pairs, pairs.active.resize( pairs.vec.size(), true ); // calculate pairs - BitSetParallelForAll( pairs.active, [&] ( size_t idx ) + //BitSetParallelForAll( pairs.active, [&] ( size_t idx ) + for( size_t idx : pairs.active ) { auto & res = pairs.vec[idx]; const auto p0 = srcPoints[res.srcVertId]; @@ -191,7 +242,7 @@ void updatePointPairs( PointPairs & pairs, { // no target point found within distance threshold pairs.active.reset( idx ); - return; + continue; } const auto p1 = prj.point; @@ -210,7 +261,7 @@ void updatePointPairs( PointPairs & pairs, if ( prj.isBd || vp.normalsAngleCos < cosThreshold || vp.distSq > distThresholdSq ) { pairs.active.reset( idx ); - return; + continue; } if ( mutualClosest ) { @@ -220,7 +271,7 @@ void updatePointPairs( PointPairs & pairs, if ( prj.closestVert != res.srcVertId ) pairs.active.reset( idx ); } - } ); + }// ); } void ICP::deactivatefarDistPairs_() @@ -231,6 +282,7 @@ void ICP::deactivatefarDistPairs_() { const auto avgDist = getMeanSqDistToPoint(); const auto maxDistSq = sqr( prop_.farDistFactor * avgDist ); + spdlog::info( "avgDist = {}, maxDistSq = {}", avgDist, maxDistSq ); if ( maxDistSq >= prop_.distThresholdSq ) break; @@ -284,13 +336,13 @@ bool ICP::p2ptIter_() return true; } -AffineXf3d getAligningXf( const PointToPlaneAligningTransform & p2pl, +AffineXf3f getAligningXf( const PointToPlaneAligningTransform & p2pl, ICPMode mode, float angleLimit, float scaleLimit, const Vector3f & fixedRotationAxis ) { - AffineXf3d res; + AffineXf3f res; if( mode == ICPMode::TranslationOnly ) { - res = AffineXf3d( Matrix3d(), p2pl.findBestTranslation() ); + res = AffineXf3f( Matrix3f(), Vector3f( p2pl.findBestTranslation() ) ); } else { @@ -327,11 +379,21 @@ AffineXf3d getAligningXf( const PointToPlaneAligningTransform & p2pl, // recompute translation part am.b = p2pl.findBestTranslation( am.a, am.s ); } - res = am.rigidScaleXf(); + res = AffineXf3f( am.rigidScaleXf() ); } return res; } +struct __attribute__((visibility("default"))) A { + void foo() {} + void bar(); +}; + +auto pfoo = &A::foo; +auto pbar = &A::bar; + +void A::bar() {} + bool ICP::p2plIter_() { MR_TIMER; @@ -354,6 +416,7 @@ bool ICP::p2plIter_() if ( activeCount <= 0 ) return false; centroidRef /= float(activeCount * 2); + AffineXf3f centroidRefXf = AffineXf3f(Matrix3f(), centroidRef); PointToPlaneAligningTransform p2pl; for ( size_t idx : flt2refPairs_.active ) @@ -368,41 +431,175 @@ bool ICP::p2plIter_() } p2pl.prepare(); - const AffineXf3d res = getAligningXf( p2pl, prop_.icpMode, prop_.p2plAngleLimit, prop_.p2plScaleLimit, prop_.fixedRotationAxis ); + AffineXf3f res = getAligningXf( p2pl, prop_.icpMode, prop_.p2plAngleLimit, prop_.p2plScaleLimit, prop_.fixedRotationAxis ); if (std::isnan(res.b.x)) //nan check return false; - - const AffineXf3d subCentroid{ Matrix3d(), Vector3d( -centroidRef ) }; - const AffineXf3d addCentroid{ Matrix3d(), Vector3d( centroidRef ) }; - setFloatXf( AffineXf3f( addCentroid * res * subCentroid * AffineXf3d( flt_.xf ) ) ); + setFloatXf( centroidRefXf * res * centroidRefXf.inverse() * flt_.xf ); return true; } -void ICP::calcGen_( float (ICP::*dist)() const, bool (ICP::*iter)() ) +inline float myLength( const Vector3f& a ) +{ + return std::sqrt( a.x * a.x + a.y * a.y + a.z * a.z ); +} + +inline float angleLog( const Vector3f& a, const Vector3f& b ) +{ + const auto c = cross( a, b ); + spdlog::info( "a=({} {} {}), b=({} {} {}), c=({} {} {})", + a.x, a.y, a.z, + b.x, b.y, b.z, + c.x, c.y, c.z ); + auto x = myLength( c ); + auto y = dot( a, b ); + auto r = std::atan2( x, y ); + spdlog::info( "std::atan2( {}, {} ) = {}", x, y, r ); + return r; +} + +static Vector3f pseudonormalLog( const MeshTopology& topology, const VertCoords& points, VertId v, const FaceBitSet* region = nullptr ) { + Vector3f sum; + for ( EdgeId e : orgRing( topology, v ) ) + { + const auto l = topology.left( e ); + if ( l && ( !region || region->test( l ) ) ) + { + auto d0 = edgeVector( topology, points, e ); + auto d1 = edgeVector( topology, points, topology.next( e ) ); + auto angle = MR::angleLog( d0, d1 ); + auto n = cross( d0, d1 ); + sum += angle * n.normalized(); + } + } + + return sum.normalized(); +} + +AffineXf3f ICP::calculateTransformation() +{ + MR::setupLoggerByDefault(); + spdlog::info( "fegetround() = {}", fegetround() ); +#ifndef __EMSCRIPTEN__ + spdlog::info( "stacktrace:\n{}", getCurrentStacktrace() ); +#endif + auto* refMeshPart = ref_.obj.asMeshPart(); + auto* fltMeshPart = flt_.obj.asMeshPart(); + spdlog::info( "ref mesh = {}, flt mesh = {}", refMeshPart != nullptr, fltMeshPart != nullptr ); + if ( refMeshPart && fltMeshPart ) + { + spdlog::info( "ref region = {}, flt region = {}", (void*)refMeshPart->region, (void*)fltMeshPart->region ); + spdlog::info( "same meshes = {}", refMeshPart->mesh == fltMeshPart->mesh ); + +#ifdef __linux__ + using A = std::array; + void* symbol = std::bit_cast(&Vector3f::lengthSq)[0]; + Dl_info info; + if ( !dladdr( symbol, &info ) ) + spdlog::error( "Failed to get library path" ); + else + spdlog::info( "Vector3f::lengthSq: fname={}, sname={}", info.dli_fname, info.dli_sname ); +#endif + + Vector3f c( -0.02146666f, 0.0014069901f, 0.0014069926f ); + spdlog::info( "({} {} {}).lengthSq() = {}, length = {}", c.x, c.y, c.z, c.lengthSq(), c.length() ); + + const auto& refPoints = refMeshPart->mesh.points; + const auto& refTopology = refMeshPart->mesh.topology; + //const auto& fltTopology = fltMeshPart->mesh.topology; +/* for ( auto v = 0_v; v < refTopology.vertSize(); ++v ) + { + auto refN = refMeshPart->mesh.pseudonormal( v ); + auto fltN = fltMeshPart->mesh.pseudonormal( v ); + auto fltNN = refN.normalized(); + spdlog::info( "{}_v: refN=({} {} {}) fltN=({} {} {}) fltNN=({} {} {})", (int)v, + refN.x, refN.y, refN.z, + fltN.x, fltN.y, fltN.z, + fltNN.x, fltNN.y, fltNN.z ); + }*/ + auto n0 = pseudonormalLog( refTopology, refPoints, 0_v ); + spdlog::info( "pseudonormal 0: {} {} {}", n0.x, n0.y, n0.z ); + spdlog::info( "point 0: {} {} {}", refPoints[0_v].x, refPoints[0_v].y, refPoints[0_v].z ); + { + std::ostringstream s; + s << "0_v nei verts: "; + for ( auto e : orgRing( refTopology, 0_v ) ) + { + auto d = refTopology.dest( e ); + spdlog::info( "point {}: {} {} {}", (int)d, refPoints[d].x, refPoints[d].y, refPoints[d].z ); + s << (int)d << ' '; + } + spdlog::info( "{}", s.str() ); + } + { + std::ostringstream s; + s << "0_v nei faces: "; + for ( auto e : orgRing( refTopology, 0_v ) ) + { + s << ( int )refTopology.left( e ) << ' '; + } + spdlog::info( "{}", s.str() ); + } + } + logXf( "ref_.xf", ref_.xf ); + logXfHex( "ref_.xf", ref_.xf ); + logXf( "flt_.xf", flt_.xf ); + logXfHex( "flt_.xf", flt_.xf ); + + spdlog::info( "method = {}", ( int )prop_.method ); + spdlog::info( "p2plAngleLimit = {}", prop_.p2plAngleLimit ); + spdlog::info( "p2plScaleLimit = {}", prop_.p2plScaleLimit ); + spdlog::info( "cosThreshold = {}", prop_.cosThreshold ); + spdlog::info( "distThresholdSq = {}", prop_.distThresholdSq ); + spdlog::info( "farDistFactor = {}", prop_.farDistFactor ); + spdlog::info( "icpMode = {}", (int)prop_.icpMode ); + spdlog::info( "fixedRotationAxis = {} {} {}", prop_.fixedRotationAxis.x, prop_.fixedRotationAxis.y, prop_.fixedRotationAxis.z ); + spdlog::info( "iterLimit = {}", prop_.iterLimit ); + spdlog::info( "badIterStopCount = {}", prop_.badIterStopCount ); + spdlog::info( "exitVal = {}", prop_.exitVal ); + spdlog::info( "mutualClosest = {}", prop_.mutualClosest ); + + bool pt2pt = prop_.method == ICPMethod::Combined || prop_.method == ICPMethod::PointToPoint; updatePointPairs(); - float minDist = (this->*dist)(); + //spdlog::info( "flt2refPairs" ); + //logPointPairs( flt2refPairs_ ); + //spdlog::info( "ref2fltPairs" ); + //logPointPairs( ref2fltPairs_ ); + + spdlog::info( "getMeanSqDistToPoint = {}", getMeanSqDistToPoint() ); + spdlog::info( "getMeanSqDistToPlane = {}", getMeanSqDistToPlane() ); + float minDist = pt2pt ? getMeanSqDistToPoint() : getMeanSqDistToPlane(); + spdlog::info( "minDist0 = {}", minDist ); int badIterCount = 0; resultType_ = ICPExitType::MaxIterations; AffineXf3f resXf = flt_.xf; + for ( iter_ = 1; iter_ <= prop_.iterLimit; ++iter_ ) { - if ( !(this->*iter)() ) + spdlog::info( "Iter #{}", iter_ ); + logXf( "ref_.xf", ref_.xf ); + logXf( "flt_.xf", flt_.xf ); + pt2pt = ( prop_.method == ICPMethod::Combined && iter_ < 3 ) + || prop_.method == ICPMethod::PointToPoint; + spdlog::info( "pt2pt = {}", pt2pt ); + + if ( !( pt2pt ? p2ptIter_() : p2plIter_() ) ) { resultType_ = ICPExitType::NotFoundSolution; break; } - // without this call, getMeanSqDistToPoint()/getMeanSqDistToPlane() will ignore xf changed in p2ptIter_()/p2plIter_() updatePointPairs(); - const float curDist = (this->*dist)(); + const float curDist = pt2pt ? getMeanSqDistToPoint() : getMeanSqDistToPlane(); + spdlog::info( "curDist = {}", curDist ); // exit if several(3) iterations didn't decrease minimization parameter - if ( curDist < minDist ) + if (curDist < minDist) { resXf = flt_.xf; minDist = curDist; + spdlog::info( "minDist = {}", minDist ); badIterCount = 0; if ( prop_.exitVal > curDist ) @@ -421,54 +618,10 @@ void ICP::calcGen_( float (ICP::*dist)() const, bool (ICP::*iter)() ) badIterCount++; } } + spdlog::info( "minDist1 = {}", minDist ); + logXf( "resXf", resXf ); flt_.xf = resXf; -} - -void ICP::calcP2Pt_() -{ - MR_TIMER; - calcGen_( &ICP::getMeanSqDistToPoint, &ICP::p2ptIter_ ); -} - -void ICP::calcP2Pl_() -{ - MR_TIMER; - calcGen_( &ICP::getMeanSqDistToPlane, &ICP::p2plIter_ ); -} - -void ICP::calcCombined_() -{ - MR_TIMER; - assert( prop_.iterLimit > 2 ); - const auto safeLimit = prop_.iterLimit; - - prop_.iterLimit = 2; - calcP2Pt_(); - - prop_.iterLimit = safeLimit - 2; - calcP2Pl_(); - - prop_.iterLimit = safeLimit; -} - -AffineXf3f ICP::calculateTransformation() -{ - switch ( prop_.method ) - { - case ICPMethod::Combined: - calcCombined_(); - break; - case ICPMethod::PointToPoint: - calcP2Pt_(); - break; - case ICPMethod::PointToPlane: - calcP2Pl_(); - break; - default: - assert( false ); - break; - } - return flt_.xf; + return resXf; } size_t getNumActivePairs( const IPointPairs& pairs ) diff --git a/source/MRMesh/MRICP.h b/source/MRMesh/MRICP.h index 74a850492e2d..90cec3c74ea5 100644 --- a/source/MRMesh/MRICP.h +++ b/source/MRMesh/MRICP.h @@ -114,7 +114,7 @@ struct NumSum [[nodiscard]] MRMESH_API std::string getICPStatusInfo( int iterations, ICPExitType exitType ); /// given prepared (p2pl) object, finds the best transformation from it of given type with given limitations on rotation angle and global scale -[[nodiscard]] MRMESH_API AffineXf3d getAligningXf( const PointToPlaneAligningTransform & p2pl, +[[nodiscard]] MRMESH_API AffineXf3f getAligningXf( const PointToPlaneAligningTransform & p2pl, ICPMode mode, float angleLimit, float scaleLimit, const Vector3f & fixedRotationAxis ); @@ -270,11 +270,6 @@ class [[nodiscard]] ICP int iter_ = 0; bool p2ptIter_(); bool p2plIter_(); - - void calcGen_( float (ICP::*dist)() const, bool (ICP::*iter)() ); - void calcP2Pt_(); - void calcP2Pl_(); - void calcCombined_(); }; } //namespace MR diff --git a/source/MRMesh/MRMultiwayICP.cpp b/source/MRMesh/MRMultiwayICP.cpp index a96a45ac51ca..4403f69ceeb3 100644 --- a/source/MRMesh/MRMultiwayICP.cpp +++ b/source/MRMesh/MRMultiwayICP.cpp @@ -892,6 +892,7 @@ bool MultiwayICP::p2plIter_() return; } centroidRef /= float( activeCount * 2 ); + AffineXf3f centroidRefXf = AffineXf3f( Matrix3f(), centroidRef ); PointToPlaneAligningTransform p2pl; for ( ICPElementId j( 0 ); j < objs_.size(); ++j ) @@ -911,15 +912,13 @@ bool MultiwayICP::p2plIter_() } p2pl.prepare(); - AffineXf3d res = getAligningXf( p2pl, prop_.icpMode, prop_.p2plAngleLimit, prop_.p2plScaleLimit, prop_.fixedRotationAxis ); + AffineXf3f res = getAligningXf( p2pl, prop_.icpMode, prop_.p2plAngleLimit, prop_.p2plScaleLimit, prop_.fixedRotationAxis ); if ( std::isnan( res.b.x ) ) //nan check { valid[objId] = FullSizeBool( false ); return; } - const AffineXf3d subCentroid{ Matrix3d(), Vector3d( -centroidRef ) }; - const AffineXf3d addCentroid{ Matrix3d(), Vector3d( centroidRef ) }; - objs_[objId].xf = AffineXf3f( addCentroid * res * subCentroid * AffineXf3d( objs_[objId].xf ) ); + objs_[objId].xf = centroidRefXf * res * centroidRefXf.inverse() * objs_[objId].xf; valid[objId] = FullSizeBool( true ); } ); return std::all_of( valid.vec_.begin(), valid.vec_.end(), [] ( auto v ) { return bool( v ); } );