From b8afd0569427544bc5a2ff57d9072e37e8d2fa10 Mon Sep 17 00:00:00 2001 From: Paul Leroy Date: Fri, 22 Mar 2024 17:35:40 +0100 Subject: [PATCH] fitting ellopsoids in progress in progress: fitEllipsoidToGrain done: directFit decalared: explicitToImplicit, implicitToExplicit declared --- include/GrainsAsEllipsoids.h | 14 ++- src/ActionA.cpp | 3 +- src/GrainsAsEllipsoids.cpp | 182 ++++++++++++++++++++++++++++++++++- 3 files changed, 196 insertions(+), 3 deletions(-) diff --git a/include/GrainsAsEllipsoids.h b/include/GrainsAsEllipsoids.h index 555afcc..846a306 100644 --- a/include/GrainsAsEllipsoids.h +++ b/include/GrainsAsEllipsoids.h @@ -17,7 +17,9 @@ class ccPointCloud; class GrainsAsEllipsoids : public ccHObject { public: - GrainsAsEllipsoids(ccPointCloud *cloud, ccMainAppInterface* app); + typedef Eigen::Array Xb; + + GrainsAsEllipsoids(ccPointCloud *cloud, ccMainAppInterface* app, const std::vector >& stacks); //! Set the path for shaders void setShaderPath(const QString &path); @@ -49,7 +51,16 @@ public: // ELLIPSOID FITTING + enum Method{ + DIRECT = 0}; + bool explicitToImplicit(); + + bool implicitToExplicit(); + + Eigen::ArrayXf directFit(const Eigen::ArrayX3f &xyz); + + bool fitEllipsoidToGrain(int grainIndex, const Method& method=DIRECT); // DRAW @@ -73,6 +84,7 @@ public: ccPointCloud* m_cloud; ccMainAppInterface* m_app; Eigen::ArrayXi m_localMaximumIndexes; + std::vector> m_stacks; std::vector m_grainColors; std::vector m_ellipsoidInstance; diff --git a/src/ActionA.cpp b/src/ActionA.cpp index 60c3ac4..c5674a9 100644 --- a/src/ActionA.cpp +++ b/src/ActionA.cpp @@ -70,6 +70,7 @@ void G3PointAction::GetG3PointAction(ccPointCloud *cloud, ccMainAppInterface *ap } s_g3PointAction->showDlg(); s_g3PointAction->init(); + s_g3PointAction->segment(); } RGBAColorsTableType getRandomColors(int randomColorsNumber) @@ -1640,7 +1641,7 @@ void G3PointAction::segment() m_app->dispToConsole( "[G3Point] initial segmentation: " + QString::number(nLabels) + " labels", ccMainAppInterface::STD_CONSOLE_MESSAGE ); // plot display grains as ellipsoids - m_grainsAsEllipsoids = new GrainsAsEllipsoids(m_cloud, m_app); + m_grainsAsEllipsoids = new GrainsAsEllipsoids(m_cloud, m_app, m_stacks); m_grainsAsEllipsoids->setLocalMaximumIndexes(m_localMaximumIndexes); m_grainColors.reset(new RGBAColorsTableType(getRandomColors(m_localMaximumIndexes.size()))); m_grainsAsEllipsoids->setGrainColorsTable(*m_grainColors); diff --git a/src/GrainsAsEllipsoids.cpp b/src/GrainsAsEllipsoids.cpp index 45a1f85..e7702e1 100644 --- a/src/GrainsAsEllipsoids.cpp +++ b/src/GrainsAsEllipsoids.cpp @@ -7,9 +7,10 @@ #include #include -GrainsAsEllipsoids::GrainsAsEllipsoids(ccPointCloud *cloud, ccMainAppInterface *app) +GrainsAsEllipsoids::GrainsAsEllipsoids(ccPointCloud *cloud, ccMainAppInterface *app, const std::vector >& stacks) : m_cloud(cloud) , m_app(app) + , m_stacks(stacks) { setShaderPath("C:/dev/CloudCompare/plugins/private/qG3POINT/shaders"); } @@ -162,6 +163,184 @@ void GrainsAsEllipsoids::buildInterleavedVertices() // ELLIPSOID FITTING +bool GrainsAsEllipsoids::explicitToImplicit() +{ + return true; +} + +bool GrainsAsEllipsoids::implicitToExplicit() +{ + return true; +} + +Eigen::ArrayXf GrainsAsEllipsoids::directFit(const Eigen::ArrayX3f& xyz) +{ + std::cout << "[GrainsAsEllipsoids::directFit]" << std::endl; + + Eigen::MatrixXf d(xyz.rows(), 10); + + d << xyz(Eigen::all, 0).pow(2).matrix() + , xyz(Eigen::all, 1).pow(2).matrix() + , xyz(Eigen::all, 2).pow(2).matrix() + , (2 * xyz(Eigen::all, 1) * xyz(Eigen::all, 2)).matrix() + , (2 * xyz(Eigen::all, 0) * xyz(Eigen::all, 2)).matrix() + , (2 * xyz(Eigen::all, 0) * xyz(Eigen::all, 1)).matrix() + , (2 * xyz(Eigen::all, 0)).matrix() + , (2 * xyz(Eigen::all, 1)).matrix() + , (2 * xyz(Eigen::all, 2)).matrix() + , Eigen::MatrixXf::Ones(xyz.rows(), 1); + + Eigen::MatrixXf s = d.transpose() * d; + + int k = 4; + Eigen::Matrix3f c1; + Eigen::Matrix3f c2; + Eigen::MatrixXf c; + c = Eigen::MatrixXf::Zero(10, 10); + c1 << 0 , k , k + , k, 0, k + , k , k , 0; + c1 = c1.array() / 2 - 1; + c2 = -k * Eigen::Matrix3f::Identity(); + c.block(0, 0, 3, 3) = c1; + c.block(3, 3, 3, 3) = c2; + + Eigen::GeneralizedEigenSolver eigensolver(s, c); + if (eigensolver.info() != Eigen::Success) + { + abort(); + } + + Eigen::ArrayXf eigenValues(10); + eigenValues = eigensolver.eigenvalues().real(); + Xb condition = (eigenValues > 0) && (!eigenValues.isInf()); + + int flt = condition.count(); + std::cout << "flt " << flt << std::endl; + Eigen::ArrayXf finiteValues(flt); + finiteValues = Eigen::ArrayXf::Zero(flt); + for (int k = 0; k < flt; k++) + { + if (condition(k)) + { + finiteValues(k) = eigensolver.eigenvalues()(k).real(); + } + } + + std::cout << "eigenvalues eigenvectors" << std::endl; + std::cout << eigensolver.eigenvalues() << std::endl; + std::cout << eigensolver.eigenvectors() << std::endl; + + float eigenValue; + Eigen::MatrixXf v; + switch (flt) { + case 1: // regular case + eigenValue = finiteValues(0); // there is only one finite value + for (k = 0; k < 10; k++) + { + if (eigenValues(k) == eigenValue) + { + v = eigensolver.eigenvectors()(Eigen::all, k).real(); + break; + } + } + break; + case 0: // degenerate case + // # single positive eigenvalue becomes near-zero negative eigenvalue due to round-off error + eigenValue = finiteValues.abs().minCoeff(); + for (k = 0; k < 10; k++) + { + if (eigenValues(k) == eigenValue) + { + v = eigensolver.eigenvectors()(Eigen::all, k).real(); + break; + } + } + break; + default: // degenerate case + // several positive eigenvalues appear + eigenValue = finiteValues.abs().minCoeff(); + for (k = 0; k < 10; k++) + { + if (eigenValues(k) == eigenValue) + { + v = eigensolver.eigenvectors()(Eigen::all, k).real(); + break; + } + } + break; + } + + std::cout << eigenValue << " " << std::endl << v << std::endl; + + Eigen::ArrayXf p(10); + p << v(0), v(1), v(2) + , 2 * v(5), 2 * v(4), 2* v(3) + , 2 * v(6), 2 * v(7), 2 * v(8) + , v(9); + + std::cout << "p " << std::endl << p << std::endl; + + return p; +} + +bool GrainsAsEllipsoids::fitEllipsoidToGrain(int grainIndex, const Method& method) +{ + // Shift point cloud to have only positive coordinates + // (problem with quadfit if the point cloud is far from the coordinates of the origin (0,0,0)) + + static bool firstPass = true; + + if (firstPass) + { + // extract the point cloud related to the current index + CCCoreLib::ReferenceCloud referenceCloud(m_cloud); + for (int index : m_stacks[grainIndex]) + { + referenceCloud.addPointIndex(index); + } + + ccPointCloud* grainCloud = m_cloud->partialClone(&referenceCloud); + Eigen::Map> + grainPoints(static_cast(grainCloud->getPoint(0)->u), grainCloud->size(), 3); + + CCVector3 bbMin; + CCVector3 bbMax; + grainCloud->getBoundingBox(bbMin, bbMax); + CCVector3 bb(bbMax - bbMin); + Eigen::Vector3d scales(bb.x, bb.y, bb.z); + double scale = 1 / scales.maxCoeff(); + Eigen::RowVector3f means = grainPoints.colwise().mean(); + + std::cout << "[GrainsAsEllipsoids::fitEllipsoidToGrain] index " << grainIndex << std::endl; + std::cout << "scale " << scale << std::endl; + std::cout << "means" << means << std::endl; + + Eigen::ArrayXf p(10); + + switch (method) { + case DIRECT: + // Direct least squares fitting of ellipsoids under the constraint 4J - I**2 > 0. + // The constraint confines the class of ellipsoids to fit to those whose smallest radius is at least half of the + // largest radius. + + p = directFit(scale * (grainPoints.rowwise() - means)); // Ellipsoid fit + + if (!implicitToExplicit()) // Get the explicit parameters + { + + } + break; + default: + break; + } + } + + firstPass = false; + + return true; +} + // DRAW void GrainsAsEllipsoids::releaseShaders() @@ -399,6 +578,7 @@ void GrainsAsEllipsoids::drawGrains(CC_DRAW_CONTEXT& context) m_program->setUniformValue("modelViewMatrix", modelView); m_program->setUniformValue("normalMatrix", matrixNormal); drawSphere(context, 0); + fitEllipsoidToGrain(0); } m_program->release();