fitting ellopsoids in progress

in progress: fitEllipsoidToGrain done: directFit decalared: explicitToImplicit, implicitToExplicit declared
2024-03-22 17:35:40 +01:00
parent 10371b1c5a
commit b8afd05694
3 changed files with 196 additions and 3 deletions
@@ -17,7 +17,9 @@ class ccPointCloud;
 class GrainsAsEllipsoids : public ccHObject
 {
 public:
-	GrainsAsEllipsoids(ccPointCloud *cloud, ccMainAppInterface* app);
+	typedef Eigen::Array<bool, Eigen::Dynamic, Eigen::Dynamic> Xb;
+
+	GrainsAsEllipsoids(ccPointCloud *cloud, ccMainAppInterface* app, const std::vector<std::vector<int> >& 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<std::vector<int>> m_stacks;
 	std::vector<CCVector3f> m_grainColors;

 	std::vector<CCVector3f> m_ellipsoidInstance;
@@ -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);
@@ -7,9 +7,10 @@
 #include <iostream>
 #include <random>

-GrainsAsEllipsoids::GrainsAsEllipsoids(ccPointCloud *cloud, ccMainAppInterface *app)
+GrainsAsEllipsoids::GrainsAsEllipsoids(ccPointCloud *cloud, ccMainAppInterface *app, const std::vector<std::vector<int> >& 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<Eigen::MatrixXf> 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<const Eigen::MatrixX3f, Eigen::Unaligned, Eigen::Stride<1, 3>>
+			grainPoints(static_cast<const float*>(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();