projection bug fix, add vcglib test

CMakeLists.txt

@@ -43,12 +43,22 @@ set(LIB_PATH ${CMAKE_SOURCE_DIR}/lib)
 set(DEPEND_PATH ${CMAKE_SOURCE_DIR}/utils)
 
 option(OPTION_BUILD_DEMO "build demo" ON)
-#option(OPTION_USE_VCGLIB "use vcglib" OFF)
+option(OPTION_USE_VCGLIB "use vcglib" OFF)
 option(OPTION_BUILD_MT "use mt build" ON)
 option(OPTION_USE_BATOOLS_AS_LIB "use libBATools as static lib" OFF)
 
+if(OPTION_USE_VCGLIB)
+set (VCGLIB_INCLUDE_DIR_HINTS ${DEPEND_PATH}/vcglib)
+set (VCGLIB_INCLUDE_DIR ${VCGLIB_INCLUDE_DIR_HINTS} CACHE PATH "vcglib directory")
+find_package(vcglib REQUIRED)
+if (VCGLIB_FOUND)
+  set(USE_VCGLIB 1)
+  message(STATUS "vcglib found in: ${VCGLIB_INCLUDE_DIRS}")
+endif (VCGLIB_FOUND)
+endif()
+
 if(OPTION_USE_BATOOLS_AS_LIB)
-set(USE_BATOOLS_AS_LIB)
+set(USE_BATOOLS_AS_LIB 1)
 endif()
 
 # set MD to MT

README.md

@@ -6,7 +6,7 @@ tools to parse bundle adjustment results
 
 The code is based on the following prerequisites:
 - Eigen (tested on 3.3.4)
-
+- vcglib (optional) [get vcglib](https://github.com/StOriJimmy/vcglib/tree/devel)
 
 ## Compilation
 prerequisites: cmake version >= 3.0

demo/CMakelists.txt

@@ -15,4 +15,10 @@ target_include_directories(${PROJECT_NAME} PUBLIC
 
 if (USE_BATOOLS_AS_LIB)
 	set_property( TARGET ${PROJECT_NAME} APPEND PROPERTY COMPILE_DEFINITIONS USE_BATOOLS_AS_LIB)
-endif()
+endif()
+
+if (USE_VCGLIB)
+add_definitions(-DUSE_VCGLIB)
+target_include_directories(${PROJECT_NAME} PUBLIC 
+	${VCGLIB_INCLUDE_DIR})
+endif ()

demo/test_libbatools.cpp

@@ -2,11 +2,36 @@
 #include <tchar.h>
 #include <iostream>
 
+#ifdef USE_VCGLIB
+#include "vcg/space/intersection3.h"
+#endif // USE_VCGLIB
+
 bool testProjection(const libba::BAImageInfo & image, const libba::baPoint3d & pt_world, libba::baPoint2d & pt_img)
 {
 	return image.isInImage(pt_world, &pt_img);
 }
 
+#ifdef USE_VCGLIB
+bool testBackProjection(const libba::BAImageInfo & image, const libba::baPoint2d & pt_img, const vcg::Plane3d & plane, libba::baPoint3d & pt_world)
+{
+	vcg::Line3d line;
+	libba::baPoint3d cam_center = image.getViewPoint();
+	line.SetOrigin(vcg::Point3d(*cam_center));
+
+	libba::baPoint3d pt_img_world = image.convertImageToWorldCoordinates(pt_img);
+	libba::baPoint3d pt_dir = pt_img_world - cam_center;
+	line.SetDirection(vcg::Point3d(*pt_dir));
+	vcg::Point3d pt;
+	if (!vcg::IntersectionPlaneLine(plane, line, pt))
+		return false;
+
+	pt_world[0] = pt[0];
+	pt_world[1] = pt[1];
+	pt_world[2] = pt[2];
+	return true;
+}
+#endif // USE_VCGLIB
+
 int _tmain(int argc, _TCHAR* argv[])
 {
 	if (argc < 3) {
@@ -22,10 +47,21 @@ int _tmain(int argc, _TCHAR* argv[])
 		return EXIT_FAILURE;
 	}
 
+	std::cout << image_data.size() << " images loaded!" << std::endl;
+
 	libba::baPoint2d image_point;
-	if (!testProjection(image_data[0], libba::baPoint3d(0, 0, 0), image_point)) {
+	if (!testProjection(image_data[0], libba::baPoint3d(316384.156250, 234354.078125,55.636), image_point)) {
 		std::cout << "point out of range" << std::endl;
 	}
+
+#ifdef USE_VCGLIB
+	vcg::Plane3d plane;
+	plane.Set(vcg::Point3d(0, 0, 1), 55);
+	libba::baPoint3d pt_world;
+	if (!testBackProjection(image_data[0], libba::baPoint2d(4996, 2958), plane, pt_world)) {
+		std::cout << "intersection failed" << std::endl;
+	}
+#endif // USE_VCGLIB
 
-	return 0;
+	return EXIT_SUCCESS;
 }

lib/libBATools.cpp

@@ -80,9 +80,9 @@ baPoint3d BAImageInfo::convertWolrdToCameraCoordinates(const baPoint3d & pt_3d)
 	baPoint3d p = pt_3d - getViewPoint();
 	auto rot = Extrinsics().rot;
 	baPoint3d cp;
-	cp[0] = rot[0] * p[0];
-	cp[1] = rot[5] * p[1];
-	cp[2] = rot[10] * p[2];
+	cp[0] = rot[0] * p[0] + rot[1] * p[1] + rot[2] * p[2];
+	cp[1] = rot[4] * p[0] + rot[5] * p[1] + rot[6] * p[2];
+	cp[2] = rot[8] * p[0] + rot[9] * p[1] + rot[10] * p[2];
 
 	cp[2] = -cp[2];
 	return cp;
@@ -95,14 +95,15 @@ baPoint3d BAImageInfo::convertCameraToWolrdCoordinates(const baPoint3d & pt_3d)
 	auto rot = Extrinsics().rot;
 
 	Eigen::Matrix4d mm, mmi;
+	mm.setIdentity();
 	for (int i = 0; i < 4; i++)
 		for (int j = 0; j < 4; j++)
-			mm(i, j) = rot[i << 2 + j];
+			mm(i, j) = rot[i + 4 * j];
 	mmi = mm.inverse();
 	baPoint3d p;
-	p[0] = mmi(0, 0) * cp[0];
-	p[1] = mmi(1, 1) * cp[1];
-	p[2] = mmi(2, 2) * cp[2];
+	p[0] = mmi(0, 0) * cp[0] + mmi(1, 0) * cp[1] + mmi(2, 0) * cp[2];
+	p[1] = mmi(0, 1) * cp[0] + mmi(1, 1) * cp[1] + mmi(2, 1) * cp[2];
+	p[2] = mmi(0, 2) * cp[0] + mmi(1, 2) * cp[1] + mmi(2, 2) * cp[2];
 	p += getViewPoint();
 	return p;
 }
@@ -183,7 +184,7 @@ baPoint2d BAImageInfo::convertWorldToImageCoordinates(const baPoint3d & pt_3d) c
 	baPoint2d uv = Intrinsics().localToViewportPx(p_f);
 
 	///< left bottom to left top
-	uv[1] = Intrinsics().viewportPx[1] - uv[1];
+	uv[1] = height() - uv[1];
 	return uv;
 }
 
@@ -464,6 +465,7 @@ LIBBA_API bool loadBundleOutFile(const std::string & file_path, std::vector<BAIm
 		//cmr_intri.viewportPx = baPoint2i(_img.width, _img.height);
 		cmr_intri.centerPx[0] = (int)((double)cmr_intri.viewportPx[0] / 2.0f);
 		cmr_intri.centerPx[1] = (int)((double)cmr_intri.viewportPx[1] / 2.0f);
+		cmr_intri.pixelSizeMm = baPoint2d(1.f, 1.f);
 
 		auto & rot = _img.Extrinsics().rot;
 		ifstr >> rot[0] >> rot[1] >> rot[2]