common: add plane Transform()

(cherry picked from commit 681037620cc9cf088f2bba6aa8bbf548c55f15e6)
cdcseacave · Sep 23, 2024 · b0b04bb · b0b04bb
1 parent aa86191
commit b0b04bb
Show file tree

Hide file tree

Showing 9 changed files with 82 additions and 20 deletions.
diff --git a/apps/InterfacePolycam/InterfacePolycam.cpp b/apps/InterfacePolycam/InterfacePolycam.cpp
@@ -137,8 +137,8 @@ bool Application::Initialize(size_t argc, LPCTSTR* argv)
 	LOG(_T("Command line: ") APPNAME _T("%s"), Util::CommandLineToString(argc, argv).c_str());
 
 	// validate input
-	Util::ensureValidFolderPath(OPT::strInputFileName);
 	const bool bInvalidCommand(OPT::strInputFileName.empty());
+	Util::ensureValidFolderPath(OPT::strInputFileName);
 	if (OPT::vm.count("help") || bInvalidCommand) {
 		boost::program_options::options_description visible("Available options");
 		visible.add(generic).add(config);
@@ -225,6 +225,7 @@ bool ParseImage(Scene& scene, const String& imagePath, const String& cameraPath,
 	const Point3d t = P.topRightCorner<3, 1>().eval();
 	pose.C = pose.R.t() * (-t);
 	imageData.camera = platform.GetCamera(imageData.cameraID, imageData.poseID);
+	++scene.nCalibratedImages;
 	// set image neighbors if available
 	nlohmann::json::const_iterator itNeighbors = data.find("neighbors");
 	if (itNeighbors != data.end()) {
@@ -285,6 +286,7 @@ bool ParseScene(Scene& scene, const String& scenePath)
 		VERBOSE("Invalid scene folder");
 		return false;
 	}
+	scene.nCalibratedImages = 0;
 	if (numCorrectedFolders == 2) {
 		// corrected data
 		CLISTDEFIDX(String, IIndex) imagePaths;

diff --git a/build/Templates/ConfigLocal.h.in b/build/Templates/ConfigLocal.h.in
@@ -6,9 +6,6 @@
 // OpenMVS compiled as static or dynamic libs
 #cmakedefine BUILD_SHARED_LIBS
 
-// Define to 1 if you have the <inttypes.h> header file
-#cmakedefine01 HAVE_INTTYPES_H
-
 // Define to 1 if exceptions are enabled
 #cmakedefine01 _HAS_EXCEPTIONS
 

diff --git a/build/Utils.cmake b/build/Utils.cmake
@@ -710,9 +710,6 @@ macro(optimize_default_compiler_settings)
 		string(REPLACE "/Zm1000" "" ${flags} "${${flags}}")
 	  endforeach()
 	endif()
-	if(CMAKE_C_COMPILER_ID)
-		CHECK_INCLUDE_FILE("inttypes.h" HAVE_INTTYPES_H)
-	endif()
 endmacro()
 
 

diff --git a/libs/Common/AABB.h b/libs/Common/AABB.h
@@ -29,6 +29,7 @@ class TAABB
 	typedef TYPE Type;
 	typedef Eigen::Matrix<TYPE,DIMS,1> POINT;
 	typedef Eigen::Matrix<TYPE,DIMS,DIMS,Eigen::RowMajor> MATRIX;
+	typedef Eigen::AlignedBox<TYPE,DIMS> ALIGNEDBOX;
 	enum { numChildren = (2<<(DIMS-1)) };
 	enum { numCorners = (DIMS==1 ? 2 : (DIMS==2 ? 4 : 8)) }; // 2^DIMS
 	enum { numScalar = (2*DIMS) };
@@ -39,18 +40,20 @@ class TAABB
 
 	inline TAABB() {}
 	inline TAABB(bool);
-	inline TAABB(const POINT& _pt);
+	inline TAABB(const POINT& pt);
 	inline TAABB(const POINT& _ptMin, const POINT& _ptMax);
 	inline TAABB(const POINT& center, const TYPE& radius);
+	inline TAABB(const ALIGNEDBOX& box);
 	template <typename TPoint>
 	inline TAABB(const TPoint* pts, size_t n);
 	template <typename CTYPE>
 	inline TAABB(const TAABB<CTYPE, DIMS>&);
 
 	inline void Reset();
-	inline void Set(const POINT& _pt);
+	inline void Set(const POINT& pt);
 	inline void Set(const POINT& _ptMin, const POINT& _ptMax);
 	inline void Set(const POINT& center, const TYPE& radius);
+	inline void Set(const ALIGNEDBOX& box);
 	template <typename TPoint>
 	inline void Set(const TPoint* pts, size_t n);
 
@@ -67,6 +70,8 @@ class TAABB
 	inline void Translate(const POINT&);
 	inline void Transform(const MATRIX&);
 
+	inline ALIGNEDBOX GetAlignedBox() const;
+
 	inline POINT GetCenter() const;
 	inline void GetCenter(POINT&) const;
 

diff --git a/libs/Common/AABB.inl b/libs/Common/AABB.inl
@@ -19,9 +19,9 @@ inline TAABB<TYPE,DIMS>::TAABB(bool)
 {
 }
 template <typename TYPE, int DIMS>
-inline TAABB<TYPE,DIMS>::TAABB(const POINT& _pt)
+inline TAABB<TYPE,DIMS>::TAABB(const POINT& pt)
 	:
-	ptMin(_pt), ptMax(_pt)
+	ptMin(pt), ptMax(pt)
 {
 }
 template <typename TYPE, int DIMS>
@@ -37,6 +37,12 @@ inline TAABB<TYPE,DIMS>::TAABB(const POINT& center, const TYPE& radius)
 {
 }
 template <typename TYPE, int DIMS>
+inline TAABB<TYPE,DIMS>::TAABB(const ALIGNEDBOX& box)
+	:
+	ptMin(box.min()), ptMax(box.max())
+{
+}
+template <typename TYPE, int DIMS>
 template <typename TPoint>
 inline TAABB<TYPE,DIMS>::TAABB(const TPoint* pts, size_t n)
 {
@@ -59,9 +65,9 @@ inline void TAABB<TYPE,DIMS>::Reset()
 	ptMax = POINT::Constant(std::numeric_limits<TYPE>::lowest());
 }
 template <typename TYPE, int DIMS>
-inline void TAABB<TYPE,DIMS>::Set(const POINT& _pt)
+inline void TAABB<TYPE,DIMS>::Set(const POINT& pt)
 {
-	ptMin = ptMax = _pt;
+	ptMin = ptMax = pt;
 }
 template <typename TYPE, int DIMS>
 inline void TAABB<TYPE,DIMS>::Set(const POINT& _ptMin, const POINT& _ptMax)
@@ -76,6 +82,12 @@ inline void TAABB<TYPE,DIMS>::Set(const POINT& center, const TYPE& radius)
 	ptMax = center+POINT::Constant(radius);
 }
 template <typename TYPE, int DIMS>
+inline void TAABB<TYPE,DIMS>::Set(const ALIGNEDBOX& box)
+{
+	ptMin = box.min();
+	ptMax = box.max();
+}
+template <typename TYPE, int DIMS>
 template <typename TPoint>
 inline void TAABB<TYPE,DIMS>::Set(const TPoint* pts, size_t n)
 {
@@ -116,6 +128,14 @@ inline TAABB<TYPE,DIMS>& TAABB<TYPE,DIMS>::EnlargePercent(TYPE x)
 /*----------------------------------------------------------------*/
 
 
+template <typename TYPE, int DIMS>
+inline typename TAABB<TYPE,DIMS>::ALIGNEDBOX TAABB<TYPE,DIMS>::GetAlignedBox() const
+{
+	return ALIGNEDBOX(ptMin, ptMax);
+} // GetAlignedBox
+/*----------------------------------------------------------------*/
+
+
 template <typename TYPE, int DIMS>
 inline typename TAABB<TYPE,DIMS>::POINT TAABB<TYPE,DIMS>::GetCenter() const
 {

diff --git a/libs/Common/Plane.h b/libs/Common/Plane.h
@@ -27,6 +27,7 @@ class TPlane
 	STATIC_ASSERT(DIMS > 0 && DIMS <= 3);
 
 public:
+	typedef Eigen::Matrix<TYPE,DIMS+1,DIMS+1,Eigen::RowMajor> MATRIX;
 	typedef Eigen::Matrix<TYPE,DIMS,1> VECTOR;
 	typedef Eigen::Matrix<TYPE,DIMS,1> POINT;
 	typedef SEACAVE::TAABB<TYPE,DIMS> AABB;
@@ -62,6 +63,9 @@ class TPlane
 	inline void Negate();
 	inline TPlane Negated() const;
 
+	inline TPlane Transformed(const MATRIX&) const;
+	inline TPlane& Transform(const MATRIX&);
+
 	inline TYPE Distance(const TPlane&) const;
 	inline TYPE Distance(const POINT&) const;
 	inline TYPE DistanceAbs(const POINT&) const;

diff --git a/libs/Common/Plane.inl b/libs/Common/Plane.inl
@@ -1,3 +1,4 @@
+#include "Plane.h"
 ////////////////////////////////////////////////////////////////////
 // Plane.inl
 //
@@ -175,6 +176,22 @@ inline TPlane<TYPE,DIMS> TPlane<TYPE,DIMS>::Negated() const
 /*----------------------------------------------------------------*/
 
 
+// transform plane from one coordinate system to another
+template <typename TYPE, int DIMS>
+inline TPlane<TYPE,DIMS> TPlane<TYPE,DIMS>::Transformed(const MATRIX& m) const
+{
+	const POINT p(m_vN * -m_fD);
+	const POINT pt((m * p.homogeneous()).hnormalized());
+	return TPlane(m.template topLeftCorner<DIMS,DIMS>() * m_vN, pt);
+} // Transformed
+template <typename TYPE, int DIMS>
+inline TPlane<TYPE,DIMS>& TPlane<TYPE,DIMS>::Transform(const MATRIX& m)
+{
+	return *this = Transformed(m);
+} // Transform
+/*----------------------------------------------------------------*/
+
+
 template <typename TYPE, int DIMS>
 inline TYPE TPlane<TYPE,DIMS>::Distance(const TPlane& p) const
 {
@@ -210,8 +227,8 @@ template <typename TYPE, int DIMS>
 inline GCLASS TPlane<TYPE,DIMS>::Classify(const POINT& p) const
 {
 	const TYPE f(Distance(p));
-	if (f >  ZEROTOLERANCE<TYPE,DIMS>()) return FRONT;
-	if (f < -ZEROTOLERANCE<TYPE,DIMS>()) return BACK;
+	if (f >  ZEROTOLERANCE<TYPE>()) return FRONT;
+	if (f < -ZEROTOLERANCE<TYPE>()) return BACK;
 	return PLANAR;
 }
 /*----------------------------------------------------------------*/
@@ -266,7 +283,7 @@ bool TPlane<TYPE,DIMS>::Intersects(const TPlane& plane, RAY& ray) const
 	// if crossproduct of normals 0 than planes parallel
 	const VECTOR vCross(m_vN.cross(plane.m_vN));
 	const TYPE fSqrLength(vCross.squaredNorm());
-	if (fSqrLength < ZEROTOLERANCE<TYPE,DIMS>()) 
+	if (fSqrLength < ZEROTOLERANCE<TYPE>()) 
 		return false;
 
 	// find line of intersection

diff --git a/libs/MVS/Camera.cpp b/libs/MVS/Camera.cpp
@@ -84,12 +84,32 @@ Camera Camera::GetScaled(const cv::Size& size, const cv::Size& newSize) const
 /*----------------------------------------------------------------*/
 
 
+Matrix4x4 Camera::GetP() const {
+	Matrix4x4 P4;
+	for (int i = 0; i < 3; ++i)
+		for (int j = 0; j < 4; ++j)
+			P4(i, j) = P(i, j);
+	P4(3, 0) = P4(3, 1) = P4(3, 2) = 0;
+	P4(3, 3) = 1;
+	return P4;
+} // GetP
+Matrix4x4 Camera::GetRC() const {
+	Matrix3x4 P3;
+	AssembleProjectionMatrix(R, C, P3);
+	Matrix4x4 RC4;
+	for (int i = 0; i < 3; ++i)
+		for (int j = 0; j < 4; ++j)
+			RC4(i, j) = P3(i, j);
+	RC4(3, 0) = RC4(3, 1) = RC4(3, 2) = 0;
+	RC4(3, 3) = 1;
+	return RC4;
+} // GetRC
+/*----------------------------------------------------------------*/
+
 void Camera::ComposeP_RC()
 {
 	AssembleProjectionMatrix(R, C, P);
 } // ComposeP_RC
-/*----------------------------------------------------------------*/
-
 void Camera::ComposeP()
 {
 	AssembleProjectionMatrix(K, R, C, P);
@@ -100,8 +120,6 @@ void Camera::DecomposeP_RC()
 {
 	DecomposeProjectionMatrix(P, R, C);
 } // DecomposeP_RC
-/*----------------------------------------------------------------*/
-
 void Camera::DecomposeP()
 {
 	DecomposeProjectionMatrix(P, K, R, C);

diff --git a/libs/MVS/Camera.h b/libs/MVS/Camera.h
@@ -275,6 +275,8 @@ class MVS_API Camera : public CameraIntern
 	Camera GetScaled(REAL s) const; // return a camera scaled by the given factor
 	Camera GetScaled(const cv::Size& size, const cv::Size& newSize) const; // return a camera scaled to the given resolution
 
+	Matrix4x4 GetP() const; // the composed projection matrix (4x4) assuming valid P
+	Matrix4x4 GetRC() const; // the composed transform matrix (4x4)
 	void ComposeP_RC(); // compose P from R and C only
 	void ComposeP(); // compose P from K, R and C
 	void DecomposeP_RC(); // decompose P in R and C, keep K unchanged