Fixed odometry mesh with long edges, Optimizer/Robust is not set false because of some "solution not found" from g2o or gtsam

corelib/include/rtabmap/core/Parameters.h

@@ -329,7 +329,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Optimizer, Slam2D,            bool, false,     "If optimization is done only on x,y and theta (3DoF). Otherwise, it is done on full 6DoF poses.");
 	RTABMAP_PARAM(Optimizer, VarianceIgnored,   bool, false,     "Ignore constraints' variance. If checked, identity information matrix is used for each constraint. Otherwise, an information matrix is generated from the variance saved in the links.");
 	RTABMAP_PARAM(Optimizer, Epsilon,           double, 0.0001,  "Stop optimizing when the error improvement is less than this value.");
-	RTABMAP_PARAM(Optimizer, Robust,            bool, true,      "Robust graph optimization using Vertigo (only work for g2o and GTSAM optimization strategies).");
+	RTABMAP_PARAM(Optimizer, Robust,            bool, false,      "Robust graph optimization using Vertigo (only work for g2o and GTSAM optimization strategies).");
 
 	RTABMAP_PARAM(g2o, Solver,                  int, 0,          "0=csparse 1=pcg 2=cholmod");
 	RTABMAP_PARAM(g2o, Optimizer,               int, 0,          "0=Levenberg 1=GaussNewton");

corelib/include/rtabmap/core/util3d_surface.h

@@ -64,12 +64,14 @@ std::vector<pcl::Vertices> RTABMAP_EXP organizedFastMesh(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		double angleTolerance = M_PI/16,
 		bool quad=true,
-		int trianglePixelSize = 2);
+		int trianglePixelSize = 2,
+		const Eigen::Vector3f & viewpoint = Eigen::Vector3f(0,0,0));
 std::vector<pcl::Vertices> RTABMAP_EXP organizedFastMesh(
 		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
 		double angleTolerance = M_PI/16,
 		bool quad=true,
-		int trianglePixelSize = 2);
+		int trianglePixelSize = 2,
+		const Eigen::Vector3f & viewpoint = Eigen::Vector3f(0,0,0));
 
 void RTABMAP_EXP appendMesh(
 		pcl::PointCloud<pcl::PointXYZRGBNormal> & cloudA,

corelib/src/util3d_surface.cpp

@@ -95,7 +95,8 @@ std::vector<pcl::Vertices> organizedFastMesh(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		double angleTolerance,
 		bool quad,
-		int trianglePixelSize)
+		int trianglePixelSize,
+		const Eigen::Vector3f & viewpoint)
 {
 	UDEBUG("size=%d angle=%f quad=%d triangleSize=%d", (int)cloud->size(), angleTolerance, quad?1:0, trianglePixelSize);
 	UASSERT(cloud->is_dense == false);
@@ -106,6 +107,8 @@ std::vector<pcl::Vertices> organizedFastMesh(
 	ofm.setTriangulationType (quad?pcl::OrganizedFastMesh<pcl::PointXYZRGB>::QUAD_MESH:pcl::OrganizedFastMesh<pcl::PointXYZRGB>::TRIANGLE_RIGHT_CUT);
 	ofm.setInputCloud (cloud);
 	ofm.setAngleTolerance(angleTolerance);
+	ofm.setViewpoint(viewpoint);
+
 	std::vector<pcl::Vertices> vertices;
 	ofm.reconstruct (vertices);
 
@@ -130,7 +133,8 @@ std::vector<pcl::Vertices> organizedFastMesh(
 		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
 		double angleTolerance,
 		bool quad,
-		int trianglePixelSize)
+		int trianglePixelSize,
+		const Eigen::Vector3f & viewpoint)
 {
 	UDEBUG("size=%d angle=%f quad=%d triangleSize=%d", (int)cloud->size(), angleTolerance, quad?1:0, trianglePixelSize);
 	UASSERT(cloud->is_dense == false);
@@ -141,6 +145,8 @@ std::vector<pcl::Vertices> organizedFastMesh(
 	ofm.setTriangulationType (quad?pcl::OrganizedFastMesh<pcl::PointXYZRGBNormal>::QUAD_MESH:pcl::OrganizedFastMesh<pcl::PointXYZRGBNormal>::TRIANGLE_RIGHT_CUT);
 	ofm.setInputCloud (cloud);
 	ofm.setAngleTolerance(angleTolerance);
+	ofm.setViewpoint(viewpoint);
+
 	std::vector<pcl::Vertices> vertices;
 	ofm.reconstruct (vertices);
 

guilib/src/MainWindow.cpp

@@ -832,7 +832,8 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom)
 									output,
 									_preferencesDialog->getCloudMeshingAngle(),
 									_preferencesDialog->isCloudMeshingQuad(),
-									_preferencesDialog->getCloudMeshingTriangleSize());
+									_preferencesDialog->getCloudMeshingTriangleSize(),
+									Eigen::Vector3f(pose.x(), pose.y(), pose.z()));
 							if(polygons.size())
 							{
 								if(!_ui->widget_cloudViewer->addCloudMesh("cloudOdom", output, polygons, _odometryCorrection))
@@ -3720,6 +3721,23 @@ void MainWindow::postProcessing()
 								signatureFrom.setWords3(std::multimap<int, cv::Point3f>());
 								signatureTo.setWords(std::multimap<int, cv::KeyPoint>());
 								signatureTo.setWords3(std::multimap<int, cv::Point3f>());
+
+								if(signatureFrom.sensorData().imageRaw().empty())
+								{
+									signatureFrom.sensorData().uncompressData();
+									if(signatureFrom.sensorData().imageRaw().empty())
+									{
+										continue;
+									}
+								}
+								if(signatureTo.sensorData().imageRaw().empty())
+								{
+									signatureTo.sensorData().uncompressData();
+									if(signatureTo.sensorData().imageRaw().empty())
+									{
+										continue;
+									}
+								}
 							}
 
 							Transform transform;