Allow partial support for AliceVision v2.3.0 (see #564 for remaining issues)

CMakeLists.txt

@@ -519,9 +519,9 @@ IF(WITH_ALICE_VISION)
     IF(AliceVision_FOUND)
         SET(CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH};/usr/local/lib/cmake/modules")
         find_package(Geogram REQUIRED QUIET)
-        # Make sure the two following lines are also commented in AliceVision to avoid Eigen memory alignment error
+        add_definitions("-DRTABMAP_ALICE_VISION_MAJOR=${AliceVision_VERSION_MAJOR}")
-        #add_definitions("-DEIGEN_DONT_ALIGN_STATICALLY=1")
+        add_definitions("-DRTABMAP_ALICE_VISION_MINOR=${AliceVision_VERSION_MINOR}")
-        #add_definitions("-DEIGEN_DONT_VECTORIZE=1")
+        add_definitions("-DRTABMAP_ALICE_VISION_PATCH=${AliceVision_VERSION_PATCH}")
     ENDIF(AliceVision_FOUND)
 ENDIF(WITH_ALICE_VISION)
 
@@ -1214,7 +1214,7 @@ MESSAGE(STATUS "  With OpenChisel           = NO (open_chisel not found)")
 ENDIF()
 
 IF(AliceVision_FOUND)
-MESSAGE(STATUS "  With AliceVision          = YES (License: MPLv2)")
+MESSAGE(STATUS "  With AliceVision ${AliceVision_VERSION}    = YES (License: MPLv2)")
 ELSEIF(NOT WITH_ALICE_VISION)
 MESSAGE(STATUS "  With AliceVision          = NO (WITH_ALICE_VISION=OFF)")
 ELSE()

corelib/src/imufilter/MadgwickFilter.cpp

@@ -310,9 +310,11 @@ void MadgwickFilter::updateImpl(
 	  A[0] = ax;
 	  A[1] = ay;
 	  A[2] = az;
-	computeOrientation(A,orientation);
+	if(computeOrientation(A,orientation))
-	reset(orientation.x(), orientation.y(), orientation.z(), orientation.w());
+	{
-	initialized_ = true;
+		reset(orientation.x(), orientation.y(), orientation.z(), orientation.w());
+		initialized_ = true;
+	}
 	return;
   }
 

corelib/src/util3d_surface.cpp

@@ -2252,16 +2252,30 @@ bool multiBandTexturing(
 	UASSERT(vertexToPixels.size() == cloud2.size());
 	UINFO("Input mesh: %d points %d polygons", (int)cloud2.size(), (int)polygons.size());
 	mesh::Texturing texturing;
+#if RTABMAP_ALICE_VISION_MAJOR > 2 || (RTABMAP_ALICE_VISION_MAJOR==2 && RTABMAP_ALICE_VISION_MINOR>=3)
+	texturing.mesh = new mesh::Mesh();
+	texturing.mesh->pts.resize(cloud2.size());
+	texturing.mesh->pointsVisibilities.resize(cloud2.size());
+#else
 	texturing.me = new mesh::Mesh();
 	texturing.me->pts = new StaticVector<Point3d>(cloud2.size());
 	texturing.pointsVisibilities = new mesh::PointsVisibility();
 	texturing.pointsVisibilities->reserve(cloud2.size());
+#endif
 	texturing.texParams.textureSide = 8192;
 	texturing.texParams.downscale = 8192/textureSize;
 
 	for(size_t i=0;i<cloud2.size();++i)
 	{
 		pcl::PointXYZRGB pt = cloud2.at(i);
+#if RTABMAP_ALICE_VISION_MAJOR > 2 || (RTABMAP_ALICE_VISION_MAJOR==2 && RTABMAP_ALICE_VISION_MINOR>=3)
+		texturing.mesh->pointsVisibilities[i].reserve(vertexToPixels[i].size());
+		for(std::map<int, pcl::PointXY>::const_iterator iter=vertexToPixels[i].begin(); iter!=vertexToPixels[i].end();++iter)
+		{
+			texturing.mesh->pointsVisibilities[i].push_back(iter->first);
+		}
+		texturing.mesh->pts[i] = Point3d(pt.x, pt.y, pt.z);
+#else
 		mesh::PointVisibility* pointVisibility = new mesh::PointVisibility();
 		pointVisibility->reserve(vertexToPixels[i].size());
 		for(std::map<int, pcl::PointXY>::const_iterator iter=vertexToPixels[i].begin(); iter!=vertexToPixels[i].end();++iter)
@@ -2270,13 +2284,24 @@ bool multiBandTexturing(
 		}
 		texturing.pointsVisibilities->push_back(pointVisibility);
 		(*texturing.me->pts)[i] = Point3d(pt.x, pt.y, pt.z);
+#endif
 	}
 
+#if RTABMAP_ALICE_VISION_MAJOR > 2 || (RTABMAP_ALICE_VISION_MAJOR==2 && RTABMAP_ALICE_VISION_MINOR>=3)
+	texturing.mesh->tris.resize(polygons.size());
+	texturing.mesh->trisMtlIds().resize(polygons.size());
+#else
 	texturing.me->tris = new StaticVector<mesh::Mesh::triangle>(polygons.size());
+#endif
 	for(size_t i=0;i<polygons.size();++i)
 	{
 		UASSERT(polygons[i].vertices.size() == 3);
+#if RTABMAP_ALICE_VISION_MAJOR > 2 || (RTABMAP_ALICE_VISION_MAJOR==2 && RTABMAP_ALICE_VISION_MINOR>=3)
+		texturing.mesh->trisMtlIds()[i] = -1;
+		texturing.mesh->tris[i] = mesh::Mesh::triangle(
+#else
 		(*texturing.me->tris)[i] = mesh::Mesh::triangle(
+#endif
 				polygons[i].vertices[0],
 				polygons[i].vertices[1],
 				polygons[i].vertices[2]);
@@ -2405,21 +2430,22 @@ bool multiBandTexturing(
 			sfmData::CameraPose pose(geometry::Pose3(m), true);
 			sfmData.setAbsolutePose((IndexT)viewId, pose);
 
-			std::shared_ptr<camera::IntrinsicBase> camPtr(new camera::Pinhole(imageSize.width, imageSize.height, model.fx(), model.cx(), model.cy()));
+			std::shared_ptr<camera::IntrinsicBase> camPtr = std::make_shared<camera::Pinhole>(
+					imageSize.width, imageSize.height, model.fx(), model.cx(), model.cy());
 			sfmData.intrinsics.insert(std::make_pair((IndexT)viewId, camPtr));
 
 			std::string imagePath = tmpImageDirectory+uFormat("/%d.jpg", viewId);
 
 			cv::imwrite(imagePath, imageRoi);
 
-			sfmData.views.insert(std::make_pair((IndexT)viewId,
+			std::shared_ptr<sfmData::View> viewPtr = std::make_shared<sfmData::View>(
-					new sfmData::View(
+					imagePath,
-							imagePath,
+					(IndexT)viewId,
-							(IndexT)viewId,
+					(IndexT)viewId,
-							(IndexT)viewId,
+					(IndexT)viewId,
-							(IndexT)viewId,
+					imageSize.width,
-							imageSize.width,
+					imageSize.height);
-							imageSize.height)));
+			sfmData.views.insert(std::make_pair((IndexT)viewId, viewPtr));
 			++viewId;
 		}
 	}
@@ -2495,6 +2521,10 @@ bool multiBandTexturing(
 	fo.close();
 	UINFO("Rename/convert textures... done. %fs", timer.ticks());
 
+	sfmData.clear();
+
+	UINFO("dsadas");
+
 	return true;
 #else
 	UERROR("Cannot unwrap texture mesh. RTAB-Map is not built with Alice Vision support! Returning false.");