dense: detect and improve tower like structure reconstruction (#1017)

* add tower mesh * compute cameras focus point * export 3d_centerline * fix line export * add line * fix line * fix ply header and add bynary/ascii option * add camera focus debug * add focus point computation * add automatic tower check rules and tower modes command argument * fit line * finetune * finetune * try new optimization * add tower mode with autodetection and forced mode * lower radius avg startpoint add debug for why not tower * clean code * hide --------- Co-authored-by: Mihai <itacernea@yahoo.com>
cdcseacave · Jul 2, 2023 · 5fb4d9e · 5fb4d9e
1 parent cbe6c93
commit 5fb4d9e
Show file tree

Hide file tree

Showing 16 changed files with 967 additions and 135 deletions.
diff --git a/apps/DensifyPointCloud/DensifyPointCloud.cpp b/apps/DensifyPointCloud/DensifyPointCloud.cpp
@@ -61,6 +61,7 @@ float fSampleMesh;
 float fBorderROI;
 bool bCrop2ROI;
 int nEstimateROI;
+int	nTowerMode;
 int nFusionMode;
 float fEstimateScale;
 int thFilterPointCloud;
@@ -152,6 +153,7 @@ bool Initialize(size_t argc, LPCTSTR* argv)
         ("estimate-roi", boost::program_options::value(&OPT::nEstimateROI)->default_value(2), "estimate and set region-of-interest (0 - disabled, 1 - enabled, 2 - adaptive)")
         ("crop-to-roi", boost::program_options::value(&OPT::bCrop2ROI)->default_value(true), "crop scene using the region-of-interest")
         ("remove-dmaps", boost::program_options::value(&bRemoveDmaps)->default_value(false), "remove depth-maps after fusion")
+		("tower-mode", boost::program_options::value(&OPT::nTowerMode)->default_value(3), "add a cylinder of points in the center of ROI; scene assume to be Z-up oriented (0 - disabled, 1 - replace, 2 - append, 3 - select neighbors, <0 - force tower mode)")
         ;
 
 	// hidden options, allowed both on command line and
@@ -333,6 +335,8 @@ int main(int argc, LPCTSTR* argv)
 		Finalize();
 		return EXIT_SUCCESS;
 	}
+	if (OPT::nTowerMode!=0)
+		scene.InitTowerScene(OPT::nTowerMode);
 	if (!OPT::strMeshFileName.empty())
 		scene.mesh.Load(MAKE_PATH_SAFE(OPT::strMeshFileName));
 	if (!OPT::strViewNeighborsFileName.empty())

diff --git a/libs/Common/Common.h b/libs/Common/Common.h
@@ -188,6 +188,8 @@ typedef TOBB<float, 2> OBB2f;
 typedef TOBB<float, 3> OBB3f;
 typedef TRay<float, 2> Ray2f;
 typedef TRay<float, 3> Ray3f;
+typedef TLine<float, 2> Line2f;
+typedef TLine<float, 3> Line3f;
 typedef TTriangle<float, 2> Triangle2f;
 typedef TTriangle<float, 3> Triangle3f;
 typedef TPlane<float> Planef;
@@ -209,6 +211,8 @@ typedef TOBB<double, 2> OBB2d;
 typedef TOBB<double, 3> OBB3d;
 typedef TRay<double, 2> Ray2d;
 typedef TRay<double, 3> Ray3d;
+typedef TLine<double, 2> Line2d;
+typedef TLine<double, 3> Line3d;
 typedef TTriangle<double, 2> Triangle2d;
 typedef TTriangle<double, 3> Triangle3d;
 typedef TPlane<double> Planed;
@@ -230,6 +234,8 @@ typedef TOBB<REAL, 2> OBB2;
 typedef TOBB<REAL, 3> OBB3;
 typedef TRay<REAL, 2> Ray2;
 typedef TRay<REAL, 3> Ray3;
+typedef TLine<REAL, 2> Line2;
+typedef TLine<REAL, 3> Line3;
 typedef TTriangle<REAL, 2> Triangle2;
 typedef TTriangle<REAL, 3> Triangle3;
 typedef TPlane<REAL> Plane;

diff --git a/libs/Common/Line.h b/libs/Common/Line.h
@@ -0,0 +1,95 @@
+////////////////////////////////////////////////////////////////////
+// Line.h
+//
+// Copyright 2023 cDc@seacave
+// Distributed under the Boost Software License, Version 1.0
+// (See http://www.boost.org/LICENSE_1_0.txt)
+
+#ifndef __SEACAVE_LINE_H__
+#define __SEACAVE_LINE_H__
+
+
+// I N C L U D E S /////////////////////////////////////////////////
+
+
+// D E F I N E S ///////////////////////////////////////////////////
+
+
+namespace SEACAVE {
+
+// S T R U C T S ///////////////////////////////////////////////////
+
+// Generic line class represented as two points
+template <typename TYPE, int DIMS>
+class TLine
+{
+	STATIC_ASSERT(DIMS > 1 && DIMS <= 3);
+
+public:
+	typedef Eigen::Matrix<TYPE,DIMS,1> VECTOR;
+	typedef Eigen::Matrix<TYPE,DIMS,1> POINT;
+	typedef SEACAVE::TAABB<TYPE,DIMS> AABB;
+	typedef SEACAVE::TRay<TYPE,DIMS> RAY;
+	enum { numScalar = (2*DIMS) };
+	enum { numParams = numScalar-1 };
+
+	POINT	pt1, pt2;	// line description
+
+	//---------------------------------------
+
+	inline TLine() {}
+	inline TLine(const POINT& pt1, const POINT& pt2);
+	template <typename CTYPE>
+	inline TLine(const TLine<CTYPE,DIMS>&);
+
+	inline void Set(const POINT& pt1, const POINT& pt2);
+
+	int Optimize(const POINT*, size_t, int maxIters=100);
+	template <typename RobustNormFunctor>
+	int Optimize(const POINT*, size_t, const RobustNormFunctor& robust, int maxIters=100);
+
+	inline TYPE GetLength() const;
+	inline TYPE GetLengthSq() const;
+	inline POINT GetCenter() const;
+	inline VECTOR GetDir() const;
+	inline VECTOR GetNormDir() const;
+	inline RAY GetRay() const;
+
+	inline bool IsSame(const TLine&, TYPE th) const;
+
+	bool Intersects(const AABB& aabb) const;
+	bool Intersects(const AABB& aabb, TYPE& t) const;
+
+	inline TYPE DistanceSq(const POINT&) const;
+	inline TYPE Distance(const POINT&) const;
+
+	inline TYPE Classify(const POINT&) const;
+	inline POINT ProjectPoint(const POINT&) const;
+
+	inline TYPE& operator [] (BYTE i) { ASSERT(i<numScalar); return pt1.data()[i]; }
+	inline TYPE operator [] (BYTE i) const { ASSERT(i<numScalar); return pt1.data()[i]; }
+
+	#ifdef _USE_BOOST
+	// implement BOOST serialization
+	template<class Archive>
+	void serialize(Archive& ar, const unsigned int /*version*/) {
+		ar & pt1;
+		ar & pt2;
+	}
+	#endif
+}; // class TLine
+/*----------------------------------------------------------------*/
+
+template <typename TYPE, typename TYPEW=TYPE>
+struct FitLineOnline : FitPlaneOnline<TYPE,TYPEW,true> {
+	template <typename TYPEE> TPoint3<TYPEE> GetLine(TLine<TYPEE,3>& line) const;
+};
+/*----------------------------------------------------------------*/
+
+
+#include "Line.inl"
+/*----------------------------------------------------------------*/
+
+} // namespace SEACAVE
+
+#endif // __SEACAVE_LINE_H__
diff --git a/libs/Common/Line.inl b/libs/Common/Line.inl
@@ -0,0 +1,215 @@
+// D E F I N E S ///////////////////////////////////////////////////
+
+
+// S T R U C T S ///////////////////////////////////////////////////
+
+
+// C L A S S  //////////////////////////////////////////////////////
+
+template <typename TYPE, int DIMS>
+inline TLine<TYPE,DIMS>::TLine(const POINT& _pt1, const POINT& _pt2)
+	:
+	pt1(_pt1), pt2(_pt2)
+{
+	ASSERT(!ISZERO((_pt1-_pt2).norm()));
+} // constructor
+template <typename TYPE, int DIMS>
+template <typename CTYPE>
+inline TLine<TYPE,DIMS>::TLine(const TLine<CTYPE,DIMS>& rhs)
+	:
+	pt1(rhs.pt1.template cast<TYPE>()), pt2(rhs.pt2.template cast<TYPE>())
+{
+} // copy constructor
+/*----------------------------------------------------------------*/
+
+
+// set attributes
+template <typename TYPE, int DIMS>
+inline void TLine<TYPE,DIMS>::Set(const POINT& _pt1, const POINT& _pt2)
+{
+	ASSERT(!ISZERO((_pt1-_pt2).norm()));
+	pt1 = _pt1;
+	pt2 = _pt2;
+}
+/*----------------------------------------------------------------*/
+
+
+// least squares refinement of the line to the given 3D point set
+// (return the number of iterations)
+template <typename TYPE, int DIMS>
+template <typename RobustNormFunctor>
+int TLine<TYPE,DIMS>::Optimize(const POINT* points, size_t size, const RobustNormFunctor& robust, int maxIters)
+{
+	ASSERT(DIMS == 3);
+	ASSERT(size >= numParams);
+	struct OptimizationFunctor {
+		const POINT* points;
+		size_t size;
+		double scale;
+		const RobustNormFunctor& robust;
+		// construct with the data points
+		OptimizationFunctor(const POINT* _points, size_t _size, const RobustNormFunctor& _robust)
+			: points(_points), size(_size), robust(_robust) { ASSERT(size < std::numeric_limits<int>::max()); }
+		static void Residuals(const double* x, int nPoints, const void* pData, double* fvec, double* fjac, int* /*info*/) {
+			const OptimizationFunctor& data = *reinterpret_cast<const OptimizationFunctor*>(pData);
+			ASSERT((size_t)nPoints == data.size && fvec != NULL && fjac == NULL);
+			TLine<double,DIMS> line;
+			for (int j=0; j<DIMS; ++j)
+				line.pt1(j) = x[j];
+			DirScale2Vector(x+DIMS, &data.scale, line.pt2.data());
+			line.pt2 += line.pt1;
+			for (size_t i=0; i<data.size; ++i)
+				fvec[i] = data.robust(line.Distance(data.points[i].template cast<double>()));
+		}
+	} functor(points, size, robust);
+	double arrParams[numParams];
+	for (int j=0; j<DIMS; ++j)
+		arrParams[j] = (double)pt1(j);
+	POINT dir(pt2-pt1);
+	Vector2DirScale(dir.data(), arrParams+DIMS, &functor.scale);
+	lm_control_struct control = {1.e-6, 1.e-7, 1.e-8, 1.e-8, 100.0, maxIters}; // lm_control_float;
+	lm_status_struct status;
+	lmmin(numParams, arrParams, (int)size, &functor, OptimizationFunctor::Residuals, &control, &status);
+	switch (status.info) {
+	//case 4:
+	case 5:
+	case 6:
+	case 7:
+	case 8:
+	case 9:
+	case 10:
+	case 11:
+	case 12:
+		DEBUG_ULTIMATE("error: refine line: %s", lm_infmsg[status.info]);
+		return 0;
+	}
+	for (int j=0; j<DIMS; ++j)
+		pt1(j) = (TYPE)arrParams[j];
+	DirScale2Vector(arrParams+DIMS, &functor.scale, dir.data());
+	pt2 = pt1+dir;
+	return status.nfev;
+}
+template <typename TYPE, int DIMS>
+int TLine<TYPE,DIMS>::Optimize(const POINT* points, size_t size, int maxIters)
+{
+	const auto identity = [](double x) { return x; };
+	return Optimize(points, size, identity, maxIters);
+} // Optimize
+/*----------------------------------------------------------------*/
+
+
+// get attributes
+template <typename TYPE, int DIMS>
+inline TYPE TLine<TYPE,DIMS>::GetLength() const
+{
+	return (pt2 - pt1).norm();
+}
+template <typename TYPE, int DIMS>
+inline TYPE TLine<TYPE,DIMS>::GetLengthSq() const
+{
+	return (pt2 - pt1).squaredNorm();
+}
+template <typename TYPE, int DIMS>
+inline typename TLine<TYPE,DIMS>::POINT TLine<TYPE,DIMS>::GetCenter() const
+{
+	return (pt2 + pt1) / TYPE(2);
+}
+template <typename TYPE, int DIMS>
+inline typename TLine<TYPE,DIMS>::VECTOR TLine<TYPE,DIMS>::GetDir() const
+{
+	return (pt2 - pt1);
+}
+template <typename TYPE, int DIMS>
+inline typename TLine<TYPE,DIMS>::VECTOR TLine<TYPE,DIMS>::GetNormDir() const
+{
+	return (pt2 - pt1).normalized();
+}
+template <typename TYPE, int DIMS>
+inline typename TLine<TYPE,DIMS>::RAY TLine<TYPE,DIMS>::GetRay() const
+{
+	return RAY(pt1, GetNormDir());
+}
+/*----------------------------------------------------------------*/
+
+
+template <typename TYPE, int DIMS>
+inline bool TLine<TYPE,DIMS>::IsSame(const TLine& line, TYPE th) const
+{
+	const TYPE thSq(SQUARE(th));
+	const VECTOR l(pt2-pt1);
+	const TYPE invLenSq(INVERT(l.squaredNorm()));
+	const VECTOR r1(pt1-line.pt1);
+	const TYPE dSq1((l.cross(r1)).squaredNorm()*invLenSq);
+	if (dSq1 > thSq)
+		return false;
+	const VECTOR r2(pt1-line.pt2);
+	const TYPE dSq2((l.cross(r2)).squaredNorm()*invLenSq);
+	return dSq2 <= thSq;
+}
+/*----------------------------------------------------------------*/
+
+
+// test for intersection with aabb
+template <typename TYPE, int DIMS>
+bool TLine<TYPE,DIMS>::Intersects(const AABB &aabb) const
+{
+	return GetRay().Intersects(aabb);
+} // Intersects(AABB)
+template <typename TYPE, int DIMS>
+bool TLine<TYPE,DIMS>::Intersects(const AABB &aabb, TYPE& t) const
+{
+	return GetRay().Intersects(aabb, t);
+} // Intersects(AABB)
+/*----------------------------------------------------------------*/
+
+// Computes the distance between the line and a point.
+template <typename TYPE, int DIMS>
+inline TYPE TLine<TYPE,DIMS>::DistanceSq(const POINT& pt) const
+{
+	const VECTOR l(pt2-pt1), r(pt1-pt);
+	if (DIMS == 2)
+		return TYPE(SQUARE(l[0]*r[1]-r[0]*l[1])/(l[0]*l[0]+l[1]*l[1]));
+	ASSERT(DIMS == 3);
+	return TYPE((l.cross(r)).squaredNorm()/l.squaredNorm());
+} // DistanceSq(POINT)
+template <typename TYPE, int DIMS>
+inline TYPE TLine<TYPE,DIMS>::Distance(const POINT& pt) const
+{
+	return SQRT(DistanceSq(pt));
+} // Distance(POINT)
+/*----------------------------------------------------------------*/
+
+
+// Computes the position on the line segment of the point projection.
+// Returns 0 if it coincides with the first point, and 1 if it coincides with the second point.
+template <typename TYPE, int DIMS>
+inline TYPE TLine<TYPE,DIMS>::Classify(const POINT& p) const
+{
+	const VECTOR vL(pt2 - pt1);
+	ASSERT(!ISZERO(vL.squaredNorm()));
+	const VECTOR vP(p - pt1);
+	return vL.dot(vP) / vL.squaredNorm();
+} // Classify(POINT)
+// Calculate point's projection on this line (closest point to this line).
+template <typename TYPE, int DIMS>
+inline typename TLine<TYPE,DIMS>::POINT TLine<TYPE,DIMS>::ProjectPoint(const POINT& p) const
+{
+	const VECTOR vL(pt2 - pt1);
+	ASSERT(!ISZERO(vL.squaredNorm()));
+	const VECTOR vP(p - pt1);
+	return pt1 + vL * (vL.dot(vP) / vL.squaredNorm());
+} // ProjectPoint
+/*----------------------------------------------------------------*/
+
+
+template <typename TYPE, typename TYPEW>
+template <typename TYPEE>
+TPoint3<TYPEE> FitLineOnline<TYPE,TYPEW>::GetLine(TLine<TYPEE,3>& line) const
+{
+	TPoint3<TYPEW> avg, dir;
+	const TPoint3<TYPEW> quality(this->GetModel(avg, dir));
+	const TPoint3<TYPEW> pt2(avg+dir);
+	line.Set(TPoint3<TYPEE>(avg), TPoint3<TYPEE>(pt2));
+	return TPoint3<TYPEE>(quality);
+}
+/*----------------------------------------------------------------*/
diff --git a/libs/Common/Plane.h b/libs/Common/Plane.h
@@ -50,6 +50,10 @@ class TPlane
 	inline void Set(const POINT&, const POINT&, const POINT&);
 	inline void Set(const TYPE p[DIMS+1]);
 
+	int Optimize(const POINT*, size_t, int maxIters=100);
+	template <typename RobustNormFunctor>
+	int Optimize(const POINT*, size_t, const RobustNormFunctor& robust, int maxIters=100);
+
 	inline void Invalidate();
 	inline bool IsValid() const;
 
@@ -85,15 +89,15 @@ class TPlane
 }; // class TPlane
 /*----------------------------------------------------------------*/
 
-template <typename TYPE, typename TYPEW=TYPE>
+template <typename TYPE, typename TYPEW=TYPE, bool bFitLineMode=false>
 struct FitPlaneOnline {
 	TYPEW sumX, sumSqX, sumXY, sumXZ;
 	TYPEW sumY, sumSqY, sumYZ;
 	TYPEW sumZ, sumSqZ;
 	size_t size;
 	FitPlaneOnline();
 	void Update(const TPoint3<TYPE>& P);
-	TPoint3<TYPEW> GetPlane(TPoint3<TYPEW>& avg, TPoint3<TYPEW>& dir) const;
+	TPoint3<TYPEW> GetModel(TPoint3<TYPEW>& avg, TPoint3<TYPEW>& dir) const;
 	template <typename TYPEE> TPoint3<TYPEE> GetPlane(TPlane<TYPEE,3>& plane) const;
 };
 /*----------------------------------------------------------------*/